//
// Created by zx on 2019/12/30.
//

#include "locater.h"
#include <pcl/filters//voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <glog/logging.h>
Locater::Locater():mp_yolo_detector(0),mp_point_sift(0),mp_cnn3d(0)
{
}
Locater::~Locater()
{}

Error_manager Locater::get_error()
{
    return SUCCESS;
}

Error_manager Locater::init(Measure::LocateParameter parameter)
{
    Error_manager  code;
    {
        int point_size = parameter.seg_parameter().point_size();
        int cls_num = parameter.seg_parameter().cls_num();
        double freq = parameter.seg_parameter().freq();
        std::string graph = parameter.seg_parameter().graph();
        std::string cpkt = parameter.seg_parameter().cpkt();
        Measure::Area3d area = parameter.seg_parameter().area();
        pcl::PointXYZ minp(area.x_min(), area.y_min(), area.z_min());
        pcl::PointXYZ maxp(area.x_max(), area.y_max(), area.z_max());

        LOG(INFO)<<"Creating pointSift net ...";
        mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,minp,maxp);

        std::string	graph_file =  graph;
        std::string	cpkt_file =  cpkt;
        LOG(INFO)<<"init  pointSift net graph:"<<graph;
        code=mp_point_sift->init(graph_file,cpkt_file);
        if(code!=SUCCESS)
        {
            LOG(ERROR)<<code.to_string();
            return code;
        }
    }

    {
        LOG(INFO)<<"Creating darknet ...";
        float min_x = parameter.yolo_parameter().min_x();
        float max_x = parameter.yolo_parameter().max_x();
        float min_y = parameter.yolo_parameter().min_y();
        float max_y = parameter.yolo_parameter().max_y();
        float freq = parameter.yolo_parameter().freq();
        std::string cfg = parameter.yolo_parameter().cfg();
        std::string weights = parameter.yolo_parameter().weights();
        mp_yolo_detector = new YoloDetector(cfg, weights, min_x, max_x, min_y, max_y, freq);

    }

    int l = parameter.net_3dcnn_parameter().length();
    int w = parameter.net_3dcnn_parameter().width();
    int h = parameter.net_3dcnn_parameter().height();
    int nClass = parameter.net_3dcnn_parameter().nclass();
    int freq = parameter.net_3dcnn_parameter().freq();

    mp_cnn3d = new Cnn3d_segmentation(l, w, h, freq, nClass);

    LOG(INFO)<<"Init 3dcnn ...";
    std::string	weights = parameter.net_3dcnn_parameter().weights_file();
    code=mp_cnn3d->init(weights);
    if(code!=SUCCESS)
    {
        return code;
    }
    return SUCCESS;

}

Error_manager Locater::execute_task(Task_Base* task,double time_out)
{
    LOG(INFO)<<"NEW LOCATE TASK ==============================================================================";
    Locate_information locate_information;
    locate_information.locate_correct=false;

    //判断task是否合法
    if(task==0) return LOCATER_TASK_ERROR;
    if(task->get_task_type()!=LOCATE_TASK)
    {
        task->update_statu(TASK_OVER,"locate task type error");
        return  Error_manager(LOCATER_TASK_ERROR,NORMAL,"locate Task type error");
    }
    Locate_task* locate_task=(Locate_task*)task;
    locate_task->update_statu(TASK_SIGNED);
    Error_manager code;
    ///第一步,获取task中input点云,文件保存路径
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_input=locate_task->get_locate_cloud();
    std::string save_path=locate_task->get_save_path();
    if(t_cloud_input.get()==0)
    {
        locate_task->update_statu(TASK_OVER,"Task set cloud is uninit");
        return Error_manager(LOCATER_INPUT_CLOUD_UNINIT,NORMAL,"Task set cloud is uninit");
    }
    //第二步,加锁,更新task状态,调用locate,进入测量中
    std::lock_guard<std::mutex> t_lock(m_mutex_lock);
    locate_task->update_statu(TASK_WORKING);
    code=locate(t_cloud_input,locate_information,save_path);
    locate_task->set_locate_cloud(t_cloud_input);
    locate_task->set_locate_information(locate_information);
    if(code!=SUCCESS)
    {
        locate_task->update_statu(TASK_OVER,"Locate Failed");
        return code;
    }
    locate_task->update_statu(TASK_OVER,"Locate OK");
    return code;

}

Error_manager Locater::locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
                       std::vector<YoloBox>& boxes, std::string work_dir)
{

    if(cloud_in.get()==0)
    {
        return Error_manager(LOCATER_YOLO_INPUT_CLOUD_UNINIT,NORMAL,"yolo input cloud uninit");
    }
    if(cloud_in->size()==0)
    {
        return Error_manager(LOCATER_INPUT_YOLO_CLOUD_EMPTY,NORMAL,"locate_yolo input cloud empty");
    }
    if(mp_yolo_detector==0)
    {
        return Error_manager(LOCATER_YOLO_UNINIT,NORMAL,"locate_yolo yolo is not init");
    }
    //1,根据点云区域,调整yolo边界参数,边界参数必须保证长宽 1:1
    pcl::PointXYZ min_point,max_point;
    pcl::getMinMax3D(*cloud_in,min_point,max_point);
    float yolo_minx=min_point.x;
    float yolo_maxx=max_point.x;
    float yolo_miny=min_point.y;
    float yolo_maxy=max_point.y;
    float length=yolo_maxx-yolo_minx;
    float width=yolo_maxy-yolo_miny;
    if(length>2*width) yolo_maxy=(length/2.0)+yolo_miny;
    else if(length<2*width) yolo_maxx=(2*width)+yolo_minx;

    Error_manager code=mp_yolo_detector->set_region(yolo_minx,yolo_maxx,yolo_miny,
        yolo_maxy,m_locate_parameter.yolo_parameter().freq());
    if(code!=SUCCESS)
    {
        return code;
    }
    //2,识别车辆位置
    boxes.clear();
    return mp_yolo_detector->detect(cloud_in,boxes,work_dir);

}

Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox> boxes,
                                   pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
                                   std::string work_dir)
{
    if(cloud_in.get()==0)
    {
        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,NORMAL,"sift input cloud uninit");
    }
    if(cloud_in->size()==0)
    {
        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,NORMAL,"locate_sift input cloud empty");
    }
    if(mp_point_sift==0)
    {
        return Error_manager(LOCATER_POINTSIFT_UNINIT,NORMAL,"Point Sift unInit");
    }
    //第一步,根据点云边界调整PointSIft 参数
    pcl::PointXYZ min_point,max_point;
    pcl::getMinMax3D(*cloud_in,min_point,max_point);
    Error_manager code ;
    code=mp_point_sift->set_region(min_point,max_point);
    if(code!=SUCCESS)
    {
        return code;
    }
    //第二步,点云抽稀,抽稀后的点云大约在8192左右,
    // 如果有多辆车(多个yolo框),取第一个框
    YoloBox yolo_box=boxes[0];
    pcl::PointXYZ minp, maxp;
    minp.x = yolo_box.x;
    minp.y = yolo_box.y;
    maxp.x = minp.x + yolo_box.w;
    maxp.y = minp.y + yolo_box.h;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input_sift(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsample(new pcl::PointCloud<pcl::PointXYZ>());
    ///	///体素网格 下采样
    pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
    vgfilter.setInputCloud(cloud_in);
    vgfilter.setLeafSize(60.f, 60.f, 50.f);
    vgfilter.filter(*cloud_downsample);
    //限制 x y
    pcl::PassThrough<pcl::PointXYZ> passX;
    pcl::PassThrough<pcl::PointXYZ> passY;
    pcl::PassThrough<pcl::PointXYZ> passZ;
    passX.setInputCloud(cloud_downsample);
    passX.setFilterFieldName("x");
    passX.setFilterLimits(minp.x, maxp.x);
    passX.filter(*cloud_in);

    passY.setInputCloud(cloud_in);
    passY.setFilterFieldName("y");
    passY.setFilterLimits(minp.y, maxp.y);
    passY.filter(*cloud_in);

    passY.setInputCloud(cloud_in);
    passY.setFilterFieldName("z");
    passY.setFilterLimits(40, 2000);
    passY.filter(*cloud_in);

    //第三步,分割车辆点云
    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
    code=mp_point_sift->seg(cloud_in, segmentation_clouds, work_dir);
    if(code!=SUCCESS)
    {
        return code;
    }
    if(segmentation_clouds[0]->size()==0||segmentation_clouds[2]->size()==0)
    {
        return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"sift predict no car or wheel");
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
    //第0类即是轮胎点云,第二类为车身点云
    pcl::copyPointCloud(*segmentation_clouds[0], *t_cloud_wheel);
    pcl::copyPointCloud(*segmentation_clouds[2], *t_cloud_car);
    cloud_wheel=t_cloud_wheel;
    cloud_car=t_cloud_car;
    return SUCCESS;
}

Error_manager Locater::locate_wheel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                                float& center_x,float& center_y,
                                                float& wheel_base,float& width,float& angle,
                                    std::string work_dir)
{
    if(cloud.get()==0)
    {
        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,NORMAL,"3dcnn input cloud uninit");
    }
    if(cloud->size()==0)
    {
        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,NORMAL,"locate_3dcnn input cloud empty");
    }
    if(mp_cnn3d==0)
    {
        return Error_manager(LOCATER_3DCNN_UNINIT,NORMAL,"locate_wheel 3dcnn is not init");
    }
    //识别车轮
    Error_manager code;

    code=mp_cnn3d->predict(cloud,center_x,center_y,wheel_base,width,angle,work_dir);
    if(code!=SUCCESS)
    {
        return code;
    }
    //根据plc限制判断合法性
    if(center_y>=930 || center_y<=200)
    {
        char description[255]={0};
        sprintf(description,"Y is out of range by plc (200  , 900)  Y:%.2f",center_y);
        return Error_manager(LOCATER_Y_OUT_RANGE_BY_PLC,NORMAL,description);
    }
    return SUCCESS;


}

Error_manager Locater::measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height)
{
    if(cloud_car.get()==0)
    {
        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,NORMAL,"measure height input cloud uninit");
    }
    if(cloud_car->size()==0)
    {
        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,NORMAL,"measure height input cloud is empty");
    }
    pcl::PointXYZ min_point,max_point;
    pcl::getMinMax3D(*cloud_car,min_point,max_point);
    //限制车高的范围,检验结果
    height=max_point.z;
    // 20220111 changed by yct, height 1950
    if(height>=1950 || height<=1000)
    {
        char description[255]={0};
        sprintf(description,"height is out of range, retry :%.2f",height);
        return Error_manager(LOCATER_MEASURE_HEIGHT_OUT_RANGE,NORMAL,description);
    }

    return SUCCESS;
}

void Locater::save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string save_file)
{
    FILE* pfile=fopen(save_file.c_str(),"w");
    for(int i=0;i<cloud->size();++i)
    {
        fprintf(pfile,"%.3f %.3f %.3f\n",cloud->points[i].x,cloud->points[i].y,cloud->points[i].z);
    }
    fclose(pfile);
}

Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
                  Locate_information& locate_information, std::string work_dir)
{
    if(cloud_src.get()==0)
    {
        return Error_manager(LOCATER_INPUT_CLOUD_UNINIT,NORMAL,"locate input cloud is not init");
    }
    if(cloud_src->size()==0) {
        return Error_manager(LOCATER_INPUT_CLOUD_EMPTY,NORMAL,"input cloud is empty");
    }
    //10mm网格过滤
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    ///	///体素网格 下采样
    pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
    vgfilter.setInputCloud(cloud_src);
    vgfilter.setLeafSize(30.f, 30.f, 30.f);
    vgfilter.filter(*cloud_in);
    //保存原始点云 src.txt
    std::string src_cloud_file=work_dir+"/src.txt";
    save_cloud_txt(cloud_in,src_cloud_file);
    //置初始结果为错误
    locate_information.locate_correct=false;
    Error_manager code;
    //第一步 yolo定位车辆外接矩形
    std::vector<YoloBox> t_yolo_boxes;
    code=locate_yolo(cloud_in,t_yolo_boxes,work_dir);
    if(code!=SUCCESS)
    {
        return code;
    }

    //第二步 ,根据yolo框结合PointSift识别车身点云,第0类即轮胎,第二类为车身
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
    code=locate_sift(cloud_in,t_yolo_boxes,cloud_wheel,cloud_car,work_dir);
    // added by yct, 传出车轮点云，放入任务中替换完整点云
    cloud_src = cloud_wheel;
    if(code!=SUCCESS)
    {
        return code;
    }

    //第四步,识别轮胎,并根据轮胎数量计算旋转矩形
    float center_x;
    float center_y;
    float wheel_base;
    float width;
    float angle;
    code=locate_wheel(cloud_wheel,center_x,center_y,wheel_base,width,angle,work_dir);
    if(code!=SUCCESS)
    {
        return code;
    }

    //第三步,计算车辆高度
    // changed by yct, 传入完整点云而非仅车身点云
    // 20211207 changed by yct, 点云除去栏杆附近点计算高度
    float height_car=0;
    float height_car_filtered=0;
    // 计算x范围
    std::vector<Eigen::Vector2d> t_wheel_centers;
    float height_calc_offsetx = 100;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car_height(new pcl::PointCloud<pcl::PointXYZ>);
    t_wheel_centers.push_back(Eigen::Vector2d(-(width+height_calc_offsetx)/2.0, wheel_base/2.0));
    t_wheel_centers.push_back(Eigen::Vector2d((width+height_calc_offsetx)/2.0, wheel_base/2.0));
    t_wheel_centers.push_back(Eigen::Vector2d(-(width+height_calc_offsetx)/2.0, -wheel_base/2.0));
    t_wheel_centers.push_back(Eigen::Vector2d((width+height_calc_offsetx)/2.0, -wheel_base/2.0));
    Eigen::Rotation2Dd t_rot((90.0-angle)*M_PI/180.0);
    double min_x=INT_MAX, max_x = 0.0;
    for (int i = 0; i < t_wheel_centers.size(); ++i) {
        t_wheel_centers[i] = t_rot.toRotationMatrix() * t_wheel_centers[i];
        if(min_x>t_wheel_centers[i].x())
        {
            min_x = t_wheel_centers[i].x();
        }
        if(max_x<t_wheel_centers[i].x())
        {
            max_x = t_wheel_centers[i].x();
        }
    }
    if(min_x<-width || max_x>width)
    {
        LOG(WARNING) << "min max x calc for height filter error, minx maxx: "<<min_x<<", "<<max_x;
        return Error_manager(LOCATER_MEASURE_HEIGHT_OUT_RANGE,NORMAL,"min max x calc error");
    }
    LOG(WARNING) << "min max x calc, minx maxx: "<<min_x+center_x<<", "<<max_x+center_x<<",,width: "<<width;
    for (int j = 0; j < cloud_in->size(); ++j) {
        if(cloud_in->points[j].x > (min_x+center_x) && cloud_in->points[j].x < (max_x+center_x))
        {
            cloud_car_height->push_back(cloud_in->points[j]);
        }
    }

    // 采用切除栏杆后点云计算车高
    code=measure_height(cloud_car_height,height_car_filtered);
    if(code!=SUCCESS)
    {
        return code;
    }
    locate_information.locate_hight=height_car_filtered;
    measure_height(cloud_in,height_car);
    LOG(WARNING)<< "height compare, origin---filtered: "<<height_car<<", "<<height_car_filtered<<"..cloud size:"<<cloud_car_height->size();

    //第六步,结果赋值
    //角度以度为单位
    // 20211223 added by yct, car height filtered become valid.
    locate_information.locate_x=center_x;
    locate_information.locate_y=center_y;
    locate_information.locate_theta = angle;
    locate_information.locate_length=wheel_base;
    locate_information.locate_width=width;
    locate_information.locate_hight=height_car_filtered;
    locate_information.locate_wheel_base=wheel_base;
    locate_information.locate_correct=true;
    LOG(INFO) << "Locate SUCCESS :" << " center:" << "[" << locate_information.locate_x
                << "," << locate_information.locate_y<< "] size= "
              << locate_information.locate_length << ", "
              << locate_information.locate_width << "," << locate_information.locate_hight
              << "  angle=" << locate_information.locate_theta ;

    return SUCCESS;
}