#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< dis.size(); index++)
            {
                cv::Point2f po2;
                po2.x = radar_object_array.obj(dis[index]).x();
                po2.y = radar_object_array.obj(dis[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 = 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_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
        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;
            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).position().x());
            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().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);
        }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_ = 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);
        }

        iv::fusion::PointXYZ min_point;
        iv::fusion::PointXYZ *min_point_;
        min_point.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x()
                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/2.0);
        min_point.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y()
                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/2.0);
        min_point_ = fusion_object.mutable_min_point();
        min_point_->CopyFrom(min_point);

        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);

        if((lidar_object_arr.obj(match_idx[i].nlidar).centroid().x()>0)&&
                (lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()>0))
        {
            int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
            if(xp == 0)xp=1;
            int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).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 = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
                    float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
                    nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
                    nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t);
                    nomal_centroid_ = fusion_object.add_nomal_centroid();
                    nomal_centroid_->CopyFrom(nomal_centroid);
                }
            }
        }
        for(int k = 0; k<10; k++)
        {
            //            std::cout<<"fusion    begin"<<std::endl;
            iv::fusion::PointXYZ point_forcaste;
            iv::fusion::PointXYZ *point_forcaste_;
            float forcast_x = lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() +
                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
            float forcast_y = lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() +
                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
            point_forcaste.set_x(forcast_x);
            point_forcaste.set_y(forcast_y);
            point_forcaste.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
            point_forcaste_ = fusion_object.add_point_forecast();
            point_forcaste_->CopyFrom(point_forcaste);

            iv::fusion::NomalForecast forcast_normal;
            iv::fusion::NomalForecast *forcast_normal_;
            forcast_normal.set_forecast_index(i);
            //            std::cout<<"fusion     end"<<std::endl;
            if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
                    (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
            {
                int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
                if(xp == 0)xp=1;
                int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).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_forcast;
                        iv::fusion::NomalXYZ *nomal_forcast_;
                        float nomal_x = ix*0.2 - xp*0.2;
                        float nomal_y = iy*0.2 - yp*0.2;
                        float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
                        float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
                                nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
                        float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
                                nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
                        nomal_forcast.set_nomal_x(forcast_x + s);
                        nomal_forcast.set_nomal_y(forcast_y + t);
                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
                        nomal_forcast_->CopyFrom(nomal_forcast);
                    }
                }
            }
            forcast_normal_=fusion_object.add_forecast_nomals();
            forcast_normal_->CopyFrom(forcast_normal);
        }

        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
        pobj->CopyFrom(fusion_object);
    }
    for(int j = 0; j< radar_idx.size() ; j++){
        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);

        int xp = (int)((0.5/0.2)/2.0);
        if(xp == 0)xp=1;
        int yp = (int)((0.5/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(radar_object_array.obj(radar_idx[j]).angle())
                        - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
                float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
                        + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
                nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx[j]).x() + s);
                nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx[j]).y() + t);
                nomal_centroid_ = fusion_obj.add_nomal_centroid();
                nomal_centroid_->CopyFrom(nomal_centroid);
            }
        }

        for(int k = 0; k<10; k++)
        {
            //                std::cout<<"fusion    begin"<<std::endl;
            iv::fusion::PointXYZ point_forcaste;
            iv::fusion::PointXYZ *point_forcaste_;
            float forcast_x = radar_object_array.obj(radar_idx[j]).x()
                    + radar_object_array.obj(radar_idx[j]).vx()*time_step*(k+1);
            float forcast_y = radar_object_array.obj(radar_idx[j]).y()
                    + radar_object_array.obj(radar_idx[j]).vy()*time_step*(k+1);
            point_forcaste.set_x(forcast_x);
            point_forcaste.set_y(forcast_y);
            point_forcaste.set_z(1.0);
            point_forcaste_ = fusion_obj.add_point_forecast();
            point_forcaste_->CopyFrom(point_forcaste);

            iv::fusion::NomalForecast forcast_normal;
            iv::fusion::NomalForecast *forcast_normal_;
            forcast_normal.set_forecast_index(k);

            int xp = (int)((0.5/0.2)/2.0);
            if(xp == 0)xp=1;
            int yp = (int)((0.5/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_forcast;
                    iv::fusion::NomalXYZ *nomal_forcast_;
                    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(radar_object_array.obj(radar_idx[j]).angle())
                            - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
                    float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
                            + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
                    nomal_forcast.set_nomal_x(forcast_x + s);
                    nomal_forcast.set_nomal_y(forcast_y + t);
                    nomal_forcast_ = forcast_normal.add_forecast_nomal();
                    nomal_forcast_->CopyFrom(nomal_forcast);
                }
            }
            forcast_normal_=fusion_obj.add_forecast_nomals();
            forcast_normal_->CopyFrom(forcast_normal);
        }

        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_yaw(xobs_info.xobj(j).obsang());
        iv::fusion::VelXY vel_relative;
        iv::fusion::VelXY *vel_relative_;
        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);

        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);

        int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
        if(xp == 0)xp=1;
        int yp = (int)((2.0/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(0)
                        - nomal_y*sin(0);
                float t = nomal_x*sin(0)
                        + nomal_y*cos(0);
                nomal_centroid.set_nomal_x(-xobs_info.xobj(j).pos_y() + s);
                nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
                nomal_centroid_ = fusion_obj.add_nomal_centroid();
                nomal_centroid_->CopyFrom(nomal_centroid);
            }
        }

        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
        obj->CopyFrom(fusion_obj);
    }

}

}
}

#endif // FUSION_HPP