//
// Created by zx on 2021/5/20.
//

#ifndef LIDARMEASURE__GROUD_REGION_HPP_
#define LIDARMEASURE__GROUD_REGION_HPP_

#include "error_code/error_code.h"
#include <opencv2/opencv.hpp>
#include "match3d/detect_wheel_ceres3d.h"
#include "configure.pb.h"
#include "vlp16.hpp"
#include <glog/logging.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include <google/protobuf/message.h>
#include "tool/thread_condition.h"
using google::protobuf::Message;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::ZeroCopyOutputStream;

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include "message/ground_measure_msg.pb.h"

class Ground_region
{
 public:
    Ground_region();
    ~Ground_region();
    Error_manager init(std::string configure_file);
    Error_manager init(ground_region::Configure configure);

    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const ground_region::Box& box,
                         detect_wheel_ceres3d::Detect_result& result);

 private:
    bool read_proto_param(std::string file, ::google::protobuf::Message& parameter);
    Error_manager sync_capture(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud,double timeout_second=1.0);
    pcl::PointCloud<pcl::PointXYZ>::Ptr scans2cloud(const velodyne::Frame& frame,const ground_region::Calib_parameter& calib);

    //预处理点云， 并并判断雷达状态
    Error_manager prehandle_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,
            message::Ground_measure_statu_msg& msg);
    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
                               detect_wheel_ceres3d::Detect_result& result);

    static void thread_measure_func(Ground_region* p);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
    double distance(cv::Point2f p1, cv::Point2f p2);
    bool isRect(std::vector<cv::Point2f>& points);
 private:

    std::vector<velodyne::VLP16Capture*>     m_lidars;
 public:
    ground_region::Configure                m_configure;
    detect_wheel_ceres3d*                      m_detector;
 private:
    std::thread*                            m_running_thread;
    Thread_condition                        m_exit_condition;



};


#endif //LIDARMEASURE__GROUD_REGION_HPP_