#ifndef FUSION_HPP
#define FUSION_HPP
#include "fusion_probabilities.h"
#include "fusionobjectarray.pb.h"
#include <iostream>
#include "opencv2/opencv.hpp"
#include "objectarray.pb.h"
#include "mobileye.pb.h"

#include "Eigen/Eigen"

namespace iv {
namespace fusion {

//std::vector<Match_index> match_idxs;

static float time_step = 0.3;
static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};

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

void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array,
                        std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
{
//    std::cout<<" get mat  begin  "<<std::endl;
    int nlidar = lidar_object_arry.obj_size();
    int nradar = radar_object_array.obj_size();
    match_idx.clear();
    radar_idx.clear();
    for(int i = 0; i<nlidar; i++)
    {
        match_index match;
        match.nlidar = i;
        std::vector<int> fuindex;
        for(int j =0; j<nradar; j++)
        {
            if(radar_object_array.obj(j).bvalid() == false) continue;
            if(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_arry.obj(i)))
            {
                fuindex.push_back(j);
            }
        }
        if(fuindex.size() >0)
        {
            std::vector<float> dis;
            cv::Point2f po1;
            po1.x = lidar_object_arry.obj(i).centroid().x();
            po1.y = lidar_object_arry.obj(i).centroid().y();
            for(int index =0; index< fuindex.size(); index++)
            {
                cv::Point2f po2;
                po2.x = radar_object_array.obj(fuindex[index]).x();
                po2.y = radar_object_array.obj(fuindex[index]).y();
                dis.push_back(ComputeDis(po1,po2));
            }
//            std::cout<<"  x    y     :   "<<po1.x<<"   "<<po1.y<<std::endl;
            auto smallest = std::min_element(std::begin(dis), std::end(dis));
            int index_ = std::distance(std::begin(dis), smallest);
            dis.clear();
            match.nradar = fuindex[index_];
        }else {
            match.nradar = -1000;
        }
        match_idx.push_back(match);

    }
//std::cout<<"  match_size   :   "<<match_idx.size()<<"    "<<match_idx[0].nradar<<std::endl;

    for(int k = 0; k<radar_object_array.obj_size(); k++)
    {
        std::vector<int> indexs;
        for(int radar_idx =0; radar_idx<match_idx.size(); radar_idx++)
        {
            indexs.push_back(match_idx[radar_idx].nradar);
        }
        if(radar_object_array.obj(k).bvalid() == false) continue;
        if(abs(radar_object_array.obj(k).x())< 2 && radar_object_array.obj(k).y()< 100 ){
            if(!(std::find(indexs.begin(),indexs.end(),k) !=indexs.end()))
            {
                radar_idx.push_back(k);
                //            std::cout<<"    x    y   "<<radar_object_array.obj(k).x()<<"   "<<radar_object_array.obj(k).y()<<std::endl;
            }
        }}
//            std::cout<<"   radar_size   :   "<<"    "<<radar_idx.size()<<std::endl;
//    std::cout<<"   get  mat end  "<<std::endl;
}

void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
{
//    std::cout<< "    get   fusion  begin   "<<std::endl;
    lidar_radar_fusion_object_array.Clear();
    std::vector<match_index> match_idx;
    std::vector<int> radar_idx;
    Get_AssociationMat(lidar_object_arr,radar_object_array,match_idx,radar_idx);
    for(int i =0; i< match_idx.size(); i++)
    {
        iv::fusion::fusionobject fusion_object;
        fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
        fusion_object.set_sensor_type(0);
        fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
        fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());

        if(match_idx.at(i).nradar == -1000)
        {
//            std::cout<<"   fusion    is    ok  "<<std::endl;
            if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
                   lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
            {
                fusion_object.set_yaw(1.57);
            } else {
                fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
            }

            iv::fusion::VelXY vel_relative;
            iv::fusion::VelXY *vel_relative_;
            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
            vel_relative_ = fusion_object.mutable_vel_relative();
            vel_relative_->CopyFrom(vel_relative);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x());
            centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y());
            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
            centroid_ = fusion_object.mutable_centroid();
            centroid_->CopyFrom(centroid);
            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;

        }else {
//             std::cout<<"   fusion    is    ok  "<<std::endl;
            fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
            fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());

            iv::fusion::VelXY vel_relative;
            iv::fusion::VelXY *vel_relative_;
//            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
//            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().x());
            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).vel_relative().y());
            vel_relative_ = fusion_object.mutable_vel_relative();
            vel_relative_->CopyFrom(vel_relative);

            iv::fusion::VelXY vel_abs;
            iv::fusion::VelXY *vel_abs_;
            vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
            vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
            vel_abs_ = fusion_object.mutable_vel_abs();
            vel_abs_->CopyFrom(vel_abs);

            iv::fusion::PointXYZ centroid;
            iv::fusion::PointXYZ *centroid_;
            centroid.set_x(radar_object_array.obj(match_idx[i].nradar).x());
            centroid.set_y(radar_object_array.obj(match_idx[i].nradar).y());
            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
            centroid_ = fusion_object.mutable_centroid();
            centroid_->CopyFrom(centroid);
#ifdef DEBUG_SHOW
            std::cout<<"radar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
#endif
        }

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

        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;



        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
        pobj->CopyFrom(fusion_object);
    }
    for(int j = 0; j< radar_idx.size() ; j++){
        break;
        iv::fusion::fusionobject fusion_obj;
        fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
        iv::fusion::VelXY vel_relative;
        iv::fusion::VelXY *vel_relative_;
        vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx());
        vel_relative.set_y(radar_object_array.obj(radar_idx[j]).vy());
        vel_relative_ = fusion_obj.mutable_vel_relative();
        vel_relative_->CopyFrom(vel_relative);

        iv::fusion::PointXYZ centroid;
        iv::fusion::PointXYZ *centroid_;
        centroid.set_x(radar_object_array.obj(radar_idx[j]).x());
        centroid.set_y(radar_object_array.obj(radar_idx[j]).y());
        centroid.set_z(1.0);
        centroid_ = fusion_obj.mutable_centroid();
        centroid_->CopyFrom(centroid);

        iv::fusion::Dimension dimension;
        iv::fusion::Dimension *dimension_;
        dimension.set_x(0.5);
        dimension.set_y(0.5);
        dimension.set_z(1.0);
        dimension_ = fusion_obj.mutable_dimensions();
        dimension_->CopyFrom(dimension);

        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
        obj->CopyFrom(fusion_obj);
    }
//    std::cout<<"   fusion   end    "<<std::endl;
}


void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
{
    int time_step = 0.3;
//    lidar_radar_fusion_object_array.Clear();
    for(int j = 0; j< xobs_info.xobj_size() ; j++){
        iv::fusion::fusionobject fusion_obj;
        fusion_obj.set_sensor_type(1);
        fusion_obj.set_yaw(0);
        iv::fusion::VelXY vel_relative;
        iv::fusion::VelXY *vel_relative_;
        if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
        {
            fusion_obj.set_type(2);
        }else{
            fusion_obj.set_type(1);
            vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
            vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
        }
        vel_relative_ = fusion_obj.mutable_vel_relative();
        vel_relative_->CopyFrom(vel_relative);

        iv::fusion::PointXYZ centroid;
        iv::fusion::PointXYZ *centroid_;
        centroid.set_x(-(xobs_info.xobj(j).pos_y()));
        centroid.set_y(xobs_info.xobj(j).pos_x());
        centroid.set_z(1.0);
        centroid_ = fusion_obj.mutable_centroid();
        centroid_->CopyFrom(centroid);
#ifdef DEBUG_SHOW
        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
#endif

        iv::fusion::Dimension dimension;
        iv::fusion::Dimension *dimension_;
        dimension.set_x(xobs_info.xobj(j).obswidth());
        dimension.set_y(2.0);
        dimension.set_z(1.0);
        dimension_ = fusion_obj.mutable_dimensions();
        dimension_->CopyFrom(dimension);
        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
        obj->CopyFrom(fusion_obj);
    }

}

void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
{
    for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
    {
        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;

        if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
                (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
        {
            int xp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().x()/0.2)/2.0);
            if(xp == 0)xp=1;
            int yp = (int)((lidar_radar_fusion_object_array.obj(i).dimensions().y()/0.2)/2.0);
            if(yp == 0)yp=1;
            int ix,iy;
            for(ix = 0; ix<(xp*2); ix++)
            {
                for(iy = 0; iy<(yp*2); iy++)
                {
                    iv::fusion::NomalXYZ nomal_centroid;
                    iv::fusion::NomalXYZ *nomal_centroid_;
                    float nomal_x = ix*0.2 - xp*0.2;
                    float nomal_y = iy*0.2 - yp*0.2;
                    float nomal_z = 1.0;
                    float s = nomal_x*cos(lidar_radar_fusion_object_array.obj(i).yaw())
                            - nomal_y*sin(lidar_radar_fusion_object_array.obj(i).yaw());
                    float t = nomal_x*sin(lidar_radar_fusion_object_array.obj(i).yaw())
                            + nomal_y*cos(lidar_radar_fusion_object_array.obj(i).yaw());
                    nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                    nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                    if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
                    else{
                        nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                        nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                        iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
                        nomal_centroid_ = fusion_obj.add_nomal_centroid();
                        nomal_centroid_->CopyFrom(nomal_centroid);
                    }
                }
            }

    }
}}


}
}

#endif // FUSION_HPP