//
// Created by zx on 2019/12/6.
//
#ifndef REGION_DETECT_H
#define REGION_DETECT_H

#include <iostream>
#include <fstream>
#include <string>
#include <mutex>
#include <vector>
#include <atomic>
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"

#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>

#include <boost/thread.hpp>

#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

#include "glog/logging.h"
#include "../error_code/error_code.h"
#include "wj_lidar_conf.pb.h"

//#include "../tool/StdCondition.h"
#include "opencv2/opencv.hpp"
#include "detect_wheel_ceres.h"
#include "../tool/point2D_tool.h"
#include "../tool/common_data.h"

/**
 * 万集区域检测算法类
 * */
class Region_detector
{
public:
    // 有参构造函数
    Region_detector();
    // 析构函数
    ~Region_detector();

	Error_manager init(Point2D_tool::Point2D_box point2D_box);

    // 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
    Error_manager detect(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
    Common_data::Car_wheel_information& car_wheel_information, bool print=true);
    //通过ceres检测中心,旋转,轴距,宽度,  新算法, 可以测量前轮旋转
    Error_manager detect_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
    Common_data::Car_wheel_information& car_wheel_information, bool print=false);

protected:
    // 预处理，直通滤波限制点云范围
    Error_manager preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud);
    // 点云聚类，寻找四类点云并检验是否近似矩形
    Error_manager clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points,
            std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print=false);
    //仅仅聚类
    Error_manager clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
                             std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print=false);
    // 判断是否足够近似矩形
    Error_manager isRect(std::vector<cv::Point2f>& points, bool print=false);

    double distance(cv::Point2f p1, cv::Point2f p2);

protected:
	Point2D_tool::Point2D_box		m_region_box;		// 区域范围参数

    detect_wheel_ceres              m_detector_ceres;   //ceres优化对象

};

#endif //REGION_DETECT_H