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

#ifndef LIDARMEASURE__GROUD_REGION_HPP_
#define LIDARMEASURE__GROUD_REGION_HPP_

#include <thread>

#include "../tool/thread_condition.h"
#include "../tool/common_data.h"
#include "../error_code/error_code.h"
#include "match3d/detect_wheel_ceres3d.h"
#include "velodyne_config.pb.h"
#include "../message/measure_message.pb.h"

#include <opencv2/opencv.hpp>
#include <glog/logging.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

class Ground_region
{
public:

	enum Ground_region_status
	{
		E_UNKNOWN = 0, //未知
		E_READY = 1,   //准备，待机
		E_BUSY = 2,	   //工作正忙

		E_FAULT = 10, //故障
	};
   #define GROUND_REGION_DETECT_CYCLE_MS 200

   Ground_region();
   ~Ground_region();
   // 区域类初始化
   Error_manager init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model);
   // 公有检测函数,根据初始化设置的区域进行裁剪，并识别获得结果
   Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &result);
   // 公有点云更新函数，传入最新点云获得结果
   Error_manager update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
   //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
	Error_manager get_current_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
												std::chrono::system_clock::time_point command_time);
   //外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待
	Error_manager get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
											 std::chrono::system_clock::time_point command_time);

   // 获取区域终端号, 注意！未初始化调用将返回-1
   int get_terminal_id()
   {
      if(m_region_status == E_UNKNOWN)
         return -1;
      else
         return m_region.region_id();
   }
   // 获取区域状态
   Ground_region_status get_status() { return m_region_status; }
   // 获取更新时间
   std::chrono::system_clock::time_point get_detect_update_time() { return m_detect_update_time; }
   // 获取配置参数
   velodyne::Region get_param() { return m_region; }

private:
   // 点云分类，z切，3d优化
   bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                              detect_wheel_ceres3d::Detect_result &result);
   // 自动检测线程函数,频率由外部更新点云频率控制
   void thread_measure_func();

   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:
   velodyne::Region m_region; // 区域配置
   detect_wheel_ceres3d *m_detector; // 检测器
   Ground_region_status m_region_status; // 区域状态

   // 自动检测线程
   std::thread *m_measure_thread;
   Thread_condition m_measure_condition;

   std::mutex	 								      m_cloud_collection_mutex;       // 点云更新互斥锁, 内存由上级管理
	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由上级管理
   std::chrono::system_clock::time_point     m_cloud_collection_time; // 点云更新时间

   Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
	std::chrono::system_clock::time_point		m_detect_update_time;			//定位结果的更新时间.
};

#endif //LIDARMEASURE__GROUD_REGION_HPP_