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

#ifndef REGION_DETECT_H
#define REGION_DETECT_H

#include <iostream>
#include <fstream>
#include <string>
#include <mutex>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
#include <vector>
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.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 <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 "EleFence.pb.h"
#include "StdCondition.h"
#include "opencv2/opencv.hpp"

class Region_detector
{
public:
    Region_detector(int id, EleFence::Region region);
    ~Region_detector();
    // given region id (start from 0) and cloud, find four wheels
    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double &x, double &y, double &c, double &wheelbase, double &width);

private:
    bool preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
    bool clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
    bool findWheelbase();

private:
    std::mutex              m_seg_mutex;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_cloud_segs;
    // 剪切后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cut_cloud;

public:
    EleFence::Region m_region_param;
    int m_region_id;
};

#endif //REGION_DETECT_H
