#ifndef FUSION_HPP
#define FUSION_HPP
#include "fusion_probabilities.h"
#include "fusionobjectarray.pb.h"

namespace iv {
namespace fusion {

// 先将毫米波和图像进行融合,将融合后的数据放入fusion中
//void RadarCameraFusion(iv::vision::cameraobjectarray& camera_object_array, iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& Ra_Ca_fusion_object)
//{
//    for(int i = 0; i<camera_object_array.obj_size(); i++)
//    {
//        std::vector<int>  obj_index;

//        for(int j = 0; j<radar_object_array.obj_size(); j++)
//        {
//            if(radar_object_array.obj(j).bvalid() == false) continue;
//            if(!(FusionProbabilities::ComputeRadarCameraFusionProb(radar_object_array.obj(j),camera_object_array.obj(i)))) continue;
////            else if () {

////            }
//            else {
//                obj_index.push_back(j);
//            }

//         }
//        if(obj_index.size()==0)
//        {
//            iv::fusion::fusionobject fusion_object;
//            fusion_object.set_id(camera_object_array.obj(i).id());
//            fusion_object.set_type(camera_object_array.obj(i).type());
//            fusion_object.set_prob(camera_object_array.obj(i).con());

//            iv::fusion::Dimension dimension;
//            iv::fusion::Dimension *obj_dimension;
//            dimension.set_x(camera_object_array.obj(i).w());
//            dimension.set_z(camera_object_array.obj(i).h());
//            obj_dimension = fusion_object.mutable_dimensions();
//            obj_dimension->CopyFrom(dimension);

//            iv::fusion::fusionobject *po = Ra_Ca_fusion_object.add_obj();
//            po->CopyFrom(fusion_object);

//        }else if (obj_index.size()==1)
//        {
//            iv::fusion::fusionobject fusion_object;
//            fusion_object.set_id(camera_object_array.obj(i).id());
//            fusion_object.set_type(camera_object_array.obj(i).type());
//            fusion_object.set_prob(camera_object_array.obj(i).con());

//            iv::fusion::PointXYZ centroid;
//            iv::fusion::PointXYZ *centerpoint;
//            centroid.set_x(radar_object_array.obj(obj_index.at(0)).x());
//            centroid.set_y(radar_object_array.obj(obj_index.at(0)).y());
//            centerpoint=fusion_object.mutable_centroid();
//            centerpoint->CopyFrom(centroid);

//            iv::fusion::Dimension dimension;
//            iv::fusion::Dimension *obj_dimension;
//            dimension.set_x(camera_object_array.obj(i).w());
//            dimension.set_z(camera_object_array.obj(i).h());
//            obj_dimension = fusion_object.mutable_dimensions();
//            obj_dimension->CopyFrom(dimension);

//            iv::fusion::VelXY vel_relative;
//            iv::fusion::VelXY *velrelative;
//            vel_relative.set_x(radar_object_array.obj(obj_index.at(0)).vx());
//            vel_relative.set_y(radar_object_array.obj(obj_index.at(0)).vy());

//            velrelative = fusion_object.mutable_vel_relative();
//            velrelative->CopyFrom(vel_relative);

//            iv::fusion::fusionobject *po = Ra_Ca_fusion_object.add_obj();
//            po->CopyFrom(fusion_object);
//        }else
//        {


//        }
//        obj_index.clear();

//    }
//}

float ComputeDis(cv::Point2f po1, cv::Point2f po2)
{
    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
}


void RLfusion( iv::lidar::objectarray& lidar_object_array,  iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
{
    lidar_radar_fusion_object_array.Clear();
    std::cout<<"~~~~~~~~~~~~"<<lidar_object_array.obj_size()<<std::endl;

    for(int i = 0; i<lidar_object_array.obj_size(); i++)
    {
        std::vector<int> fuindex;
        for(int j =0; j<radar_object_array.obj_size(); j++)
        {
            if(radar_object_array.obj(j).bvalid() == false) continue;
            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_array.obj(i))))
            {
                fuindex.push_back(j);
            }
        }
        if(fuindex.size() == 0)
        {
            iv::fusion::fusionobject fusion_object;
            fusion_object.set_id(lidar_object_array.obj(i).id());
            fusion_object.set_type_name(lidar_object_array.obj(i).type_name());
//            fusion_object.set_prob(lidar_object_array.obj(i).type_probs());
            fusion_object.set_yaw(lidar_object_array.obj(i).tyaw());

            Vel_Struct vels;
            iv::fusion::VelXY vel;
            iv::fusion::VelXY *vel_;
            vels = iv::fusion::Transformation::lidarVelTra(lidar_object_array.obj(i).tyaw(),lidar_object_array.obj(i).velocity_linear_x());
            vel.set_x(vels.vx);
            vel.set_y(vels.vy);
            vel_ = fusion_object.mutable_vel_relative();
            vel_->CopyFrom(vel);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(lidar_object_array.obj(i).centroid().x());
            centroid.set_y(lidar_object_array.obj(i).centroid().y());
            centroid.set_z(lidar_object_array.obj(i).centroid().z());
            centroid_ = fusion_object.mutable_centroid();
            centroid_->CopyFrom(centroid);



            iv::fusion::Dimension dimension;
            iv::fusion::Dimension *dimension_;
            dimension.set_x(lidar_object_array.obj(i).dimensions().x());
            dimension.set_y(lidar_object_array.obj(i).dimensions().y());
            dimension.set_z(lidar_object_array.obj(i).dimensions().z());
            dimension_ = fusion_object.mutable_dimensions();
            dimension_->CopyFrom(dimension);


            for(int k =0 ;k<lidar_object_array.obj(i).cloud().size(); k++)
            {
                iv::fusion::PointXYZI *pointcloud_ = fusion_object.add_cloud();
                pointcloud_->set_x(lidar_object_array.obj(i).cloud(k).x());
                pointcloud_->set_y(lidar_object_array.obj(i).cloud(k).y());
                pointcloud_->set_z(lidar_object_array.obj(i).cloud(k).z());
                pointcloud_->set_i(lidar_object_array.obj(i).cloud(k).i());
            }

                iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
                pobj->CopyFrom(fusion_object);
            }else
            {
                std::vector<float> dis;
                cv::Point2f po1;
                po1.x = lidar_object_array.obj(i).centroid().x();
                po1.y = lidar_object_array.obj(i).centroid().y();
                for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
                {
                    cv::Point2f po2;
                    po2.x = radar_object_array.obj(*d).x();
                    po2.y = radar_object_array.obj(*d).y();
                    dis.push_back(ComputeDis(po1,po2));
                }
                auto smallest = std::min_element(std::begin(dis), std::end(dis));
                int index = std::distance(std::begin(dis), smallest);
                dis.clear();

                iv::fusion::fusionobject fusion_object;
                fusion_object.set_id(lidar_object_array.obj(i).id());
                fusion_object.set_type_name(lidar_object_array.obj(i).type_name());
//                fusion_object.set_prob(lidar_object_array.obj(i).type_probs());
                fusion_object.set_yaw(lidar_object_array.obj(i).tyaw());

                iv::fusion::VelXY vel;
                iv::fusion::VelXY *vel_;
                vel.set_x(radar_object_array.obj(fuindex[index]).vx());
                vel.set_y(radar_object_array.obj(fuindex[index]).vy());
                vel_ = fusion_object.mutable_vel_relative();
                vel_->CopyFrom(vel);

                iv::fusion::PointXYZ centroid;
                iv::fusion::PointXYZ *centroid_;
                centroid.set_x(radar_object_array.obj(fuindex[index]).x());
                centroid.set_y(radar_object_array.obj(fuindex[index]).y());
                centroid.set_z(lidar_object_array.obj(i).centroid().z());
                centroid_ = fusion_object.mutable_centroid();
                centroid_->CopyFrom(centroid);

                iv::fusion::Dimension dimension;
                iv::fusion::Dimension *dimension_;
                dimension.set_x(lidar_object_array.obj(i).dimensions().x());
                dimension.set_y(lidar_object_array.obj(i).dimensions().y());
                dimension.set_z(lidar_object_array.obj(i).dimensions().z());
                dimension_ = fusion_object.mutable_dimensions();
                dimension_->CopyFrom(dimension);

                for(int k =0 ;k<lidar_object_array.obj(i).cloud().size(); k++)
                {
                    iv::fusion::PointXYZI *pointcloud_ = fusion_object.add_cloud();
                    pointcloud_->set_x(lidar_object_array.obj(i).cloud(k).x());
                    pointcloud_->set_y(lidar_object_array.obj(i).cloud(k).y());
                    pointcloud_->set_z(lidar_object_array.obj(i).cloud(k).z());
                    pointcloud_->set_i(lidar_object_array.obj(i).cloud(k).i());
                }

                iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
                pobj->CopyFrom(fusion_object);
            }
            fuindex.clear();
           }
        }



void RLVfusion( iv::fusion::fusionobjectarray& li_ra_fusion_array, iv::vision::ObstacleInfo& vision_object_array,  iv::fusion::fusionobjectarray& li_ra_ca_fusion_array)
{
    li_ra_ca_fusion_array.Clear();
    for(int i = 0; i<li_ra_fusion_array.obj_size();i++)
    {
        std::vector<int> findexs;
        std::vector<int> flags;
        for(int j = 0; j<vision_object_array.bbox_3d_size(); j++)
        {
            int result = iv::fusion::FusionProbabilities::ComputeLidarCameraFusionProb(li_ra_fusion_array.obj(i), vision_object_array.bbox_3d(j));
            if(result <20) continue;
            else {
                findexs.push_back(j);
                flags.push_back(result);
            }
        }

        if(findexs.size() == 0)
        {
            iv::fusion::fusionobject fusionobject;
            fusionobject.set_id(li_ra_fusion_array.obj(i).id());
            fusionobject.set_yaw(li_ra_fusion_array.obj(i).yaw());
            fusionobject.set_type_name(li_ra_fusion_array.obj(i).type_name());
            fusionobject.set_prob(li_ra_fusion_array.obj(i).prob());

            iv::fusion::VelXY vel;
            iv::fusion::VelXY *vel_;
            vel.set_x(li_ra_fusion_array.obj(i).vel_relative().x());
            vel.set_y(li_ra_fusion_array.obj(i).vel_relative().y());
            vel_ = fusionobject.mutable_vel_relative();
            vel_->CopyFrom(vel);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(li_ra_fusion_array.obj(i).centroid().x());
            centroid.set_y(li_ra_fusion_array.obj(i).centroid().y());
            centroid.set_z(li_ra_fusion_array.obj(i).centroid().z());
            centroid_ = fusionobject.mutable_centroid();
            centroid_->CopyFrom(centroid);

            iv::fusion::Dimension dimension;
            iv::fusion::Dimension *dimension_;
            dimension.set_x(li_ra_fusion_array.obj(i).dimensions().x());
            dimension.set_y(li_ra_fusion_array.obj(i).dimensions().y());
            dimension.set_z(li_ra_fusion_array.obj(i).dimensions().z());
            dimension_ = fusionobject.mutable_dimensions();
            dimension_->CopyFrom(dimension);

            for(int m =0 ;m<li_ra_fusion_array.obj(i).cloud().size(); m++)
            {
                iv::fusion::PointXYZI pointcloud;
                iv::fusion::PointXYZI *pointcloud_;
                pointcloud.set_x(li_ra_fusion_array.obj(i).cloud(m).x());
                pointcloud.set_y(li_ra_fusion_array.obj(i).cloud(m).y());
                pointcloud.set_z(li_ra_fusion_array.obj(i).cloud(m).z());
                pointcloud.set_i(li_ra_fusion_array.obj(i).cloud(m).i());
                pointcloud_= fusionobject.add_cloud();
                pointcloud_->CopyFrom(pointcloud);
           }

           iv::fusion::fusionobject *pbj = li_ra_ca_fusion_array.add_obj();
           pbj->CopyFrom(fusionobject);
       }else
        {
            auto bigest = std::min_element(std::begin(flags), std::end(flags));
            int index = std::distance(std::begin(flags), bigest);

            iv::fusion::fusionobject fusionobject;
            fusionobject.set_id(li_ra_fusion_array.obj(i).id());
            fusionobject.set_yaw(li_ra_fusion_array.obj(i).yaw());
//            fusionobject.set_prob(vision_object_array.bbox_3d(findexs[index]).);
            fusionobject.set_type_name(vision_object_array.bbox_3d(findexs[index]).category());

            iv::fusion::VelXY vel;
            iv::fusion::VelXY *vel_;
            vel.set_x(li_ra_fusion_array.obj(i).vel_relative().x());
            vel.set_y(li_ra_fusion_array.obj(i).vel_relative().y());
            vel_ = fusionobject.mutable_vel_relative();
            vel_->CopyFrom(vel);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(li_ra_fusion_array.obj(i).centroid().x());
            centroid.set_y(li_ra_fusion_array.obj(i).centroid().y());
            centroid.set_z(li_ra_fusion_array.obj(i).centroid().z());
            centroid_ = fusionobject.mutable_centroid();
            centroid_->CopyFrom(centroid);

            iv::fusion::Dimension dimension;
            iv::fusion::Dimension *dimension_;
            dimension.set_x(li_ra_fusion_array.obj(i).dimensions().x());
            dimension.set_y(li_ra_fusion_array.obj(i).dimensions().y());
            dimension.set_z(li_ra_fusion_array.obj(i).dimensions().z());
            dimension_ = fusionobject.mutable_dimensions();
            dimension_->CopyFrom(dimension);

            for(int n =0 ;n<li_ra_ca_fusion_array.obj(i).cloud().size(); n++)
            {
                iv::fusion::PointXYZI pointcloud;
                iv::fusion::PointXYZI *pointcloud_;
                pointcloud.set_x(li_ra_fusion_array.obj(i).cloud(n).x());
                pointcloud.set_y(li_ra_fusion_array.obj(i).cloud(n).y());
                pointcloud.set_z(li_ra_fusion_array.obj(i).cloud(n).z());
                pointcloud.set_i(li_ra_fusion_array.obj(i).cloud(n).i());
                pointcloud_= fusionobject.add_cloud();
                pointcloud_->CopyFrom(pointcloud);
            }

            iv::fusion::fusionobject *pobj = li_ra_ca_fusion_array.add_obj();
            pobj->CopyFrom(fusionobject);
            }

        findexs.clear();
        flags.clear();

    }

}


}
}


#endif // FUSION_HPP