#include "fusion_probabilities.h"


int iv::fusion::FusionProbabilities::ComputePointInBox(cv::Point2f PointInCamera, Point top_left, Point bot_right)
{
    if(!((PointInCamera.x>=top_left.x)&&(PointInCamera.x<=bot_right.x)&&(PointInCamera.y>=bot_right.y)&&(PointInCamera.y<=top_left.y)))
    {
        return 0;
    } else {
        return 1;
    }
}

int iv::fusion::FusionProbabilities::ComputeLidarCameraFusionProb(const iv::fusion::fusionobject& li_ra_fusion_object, const iv::vision::Bbox3D& vision_object )
{
    std::vector<cv::Point2f> lidarIncamera;
    int flag = 0;
    Point top_left, bot_right;
    top_left.x = vision_object.top_left_x();
    top_left.y = vision_object.top_left_y();
    bot_right.x = vision_object.bottom_right_x();
    bot_right.y = vision_object.bottom_right_y();

    iv::fusion::Transformation::PointInCamera( li_ra_fusion_object,lidarIncamera);
    for(std::vector<cv::Point2f>::iterator point2D = lidarIncamera.begin(); point2D != lidarIncamera.end(); point2D++)
    {

        flag = flag + ComputePointInBox(*point2D, top_left,bot_right);
    }
    lidarIncamera.clear();

    return flag;
}

//int iv::fusion::FusionProbabilities::ComputeRadarInBox(Point radarincamera, Point top_left, Point bot_right)
//{
//    if(!((radarincamera.x>=top_left.x)&&(radarincamera.x<=bot_right.x)&&(radarincamera.y>=bot_right.y)&&(radarincamera.y<=top_left.y)))
//    {
//        return 0;
//    } else {
//        return 1;
//    }
//}


//int iv::fusion::FusionProbabilities::ComputeRadarCameraFusionProb(const iv::radar::radarobject &radar_object, const iv::vision::cameraobject &camera_object)
//{
//    double z = 0.3;
//    int flag = 0;
//    Point top_left, bot_right;
//    top_left.x = camera_object.x() - int(camera_object.w()/2);
//    top_left.y = camera_object.y() + int(camera_object.h()/2);
//    bot_right.x = camera_object.x() + int(camera_object.w()/2);
//    bot_right.y = camera_object.y() - int(camera_object.h()/2);


//    for(int i = 0; i<5; i++)
//    {
//        Point radar_in_world, radar_in_camera;
//        radar_in_world.x = radar_object.x();
//        radar_in_world.y = radar_object.y();
//        radar_in_world.z = z*(i+1);

//        radar_in_camera = iv::fusion::Transformation::RadarToCamera(radar_in_world);

//        flag = flag + iv::fusion::FusionProbabilities::ComputeRadarInBox(radar_in_camera, top_left, bot_right);

//    }

//    if(flag>=3)
//    {
//        return 1;
//    }else {
//        return 0;
//    }
//}





//毫米波雷达object点是否和激光雷达object的俯视box匹配
int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::object& lidarobject)
{
    if(!((radarPoint.x()>=lidarobject.max_point().x())&&(radarPoint.x()<= lidarobject.min_point().x())&&
         ((radarPoint.y() - 2)>=lidarobject.max_point().y())&&((radarPoint.y() - 2)<=lidarobject.min_point().y())))
    {
        return 0;
    } else {
        return 1;
    }
}