//
// 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"
/**
 * 万集区域检测算法类
 * */
class Region_detector
{
public:
    // 有参构造函数
    Region_detector(int id, wj::Region region);
    // 析构函数
    ~Region_detector();
    // 检测传入点云是否为合法四轮
    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
    // 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width, bool print=true);
    //通过ceres检测中心,旋转,轴距,宽度,前轮旋转
    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
            double &wheelbase, double &width, bool print=true);
    // 获得区域id
    int get_region_id();

private:
    // 预处理，直通滤波限制点云范围
    Error_manager preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
    // 点云聚类，寻找四类点云并检验是否近似矩形
    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);

private:
    wj::Region m_region_param;      // 区域范围参数
    std::atomic<int> m_region_id;   // 区域id

    detect_wheel_ceres              m_detector_ceres;   //ceres优化对象

};

#endif //REGION_DETECT_H