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

#ifndef LOCATER_H
#define LOCATER_H
#include "locate_task.h"
#include "locater_parameter.pb.h"
#include "../error_code/error_code.h"
#include "point_sift_segmentation.h"
#include "YoloDetector.h"
#include "cnn3d_segmentation.h"
#include <mutex>


class Locater
{
public:
    Locater();
    ~Locater();
    //初始化测量算法参数
    Error_manager init(Measure::LocateParameter parameter);
    //获取测量模块状态(错误码);返回是否能执行任务,以及不能执行任务的原因
    Error_manager get_error();
    //执行任务,
    //task:测量任务
    //time_out:超时时间,单位秒
    Error_manager execute_task(Task_Base* task,double time_out=5);
protected:
    //测量算法函数
    //cloud_in:输入点云
    //locate_information:测量结果
    //work_dir:中间文件保存路径
    Error_manager locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in,
        Locate_information& locate_information, std::string work_dir);
    //yolo定位车辆外接矩形
    //cloud_in:输入点云,定位算法内部不会修改点云内容
    //boxes:定位到的外接box框
    //work_dir:中间文件输出路径
    Error_manager locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
        std::vector<YoloBox>& boxes, std::string work_dir);
    //根据yolo识别结果, 利用PointSift从场景中分割车辆点云
    //cloud:输入点云
    //boxes:yolo识别结果
    //cloud_target:车辆点云
    //work_dir:中间文件保存路径
    Error_manager locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<YoloBox> boxes,
                           pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
                           std::string work_dir);
    //输入汽车点云,识别轮胎,并计算轴长,宽,中心,旋转角
    Error_manager 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);
    //根据汽车点云计算车高
    //cloud_car:车辆点云
    Error_manager measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height);
    /*
     * 保存点云成txt到文件
     */
    static void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string save_file);
private:
    Measure::LocateParameter           m_locate_parameter;
    Point_sift_segmentation*                mp_point_sift;
    YoloDetector*                           mp_yolo_detector;
    Cnn3d_segmentation*                     mp_cnn3d;

    std::mutex						        m_mutex_lock;
};


#endif //LOCATER_H