//
// Created by zx on 2019/12/6.
//
#include "region_detect.h"

Region_detector::Region_detector(int id, EleFence::Region region):m_region_id(-1)
{
    m_region_param.CopyFrom(region);
    m_region_id = id;
    for (int i = 0; i < 4; ++i) {
        m_cloud_segs.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    }
    cut_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
}

Region_detector::~Region_detector()
{

}

bool Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out){
    if(!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny())
        return false;
//    if(cloud_in->size() <= 0)
//        return false;
    cloud_out->clear();
    pcl::copyPointCloud(*cloud_in, *cloud_out);
    if(cloud_out->size() <= 0)
        return false;
    // 直通滤波, 筛选xy
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_out);
    pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx());//将x轴的0到1范围内
    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter(*cloud_out);//输出到结果指针

    pass.setInputCloud(cloud_out);
    pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
    pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy());//将x轴的0到1范围内
    pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter(*cloud_out);//输出到结果指针

    if(cloud_out->size() <= 0)
        return false;
    // 离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_out);
    sor.setMeanK(4); //K近邻搜索点个数
    sor.setStddevMulThresh(4.0); //标准差倍数
    sor.setNegative(false); //保留未滤波点（内点）
    sor.filter(*cloud_out);  //保存滤波结果到cloud_filter

    if(cloud_out->size() <= 0)
        return false;
    else
        return true;
}

bool Region_detector::findWheelbase()
{
    if (cut_cloud->size() <= 0)
        return false;
    
    return true;
}

bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in){
    /////聚类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud(cut_cloud);

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
    // 设置聚类的最小值 2cm (small values may cause objects to be divided
    // in several clusters, whereas big values may join objects in a same cluster).
    clustering.setClusterTolerance(0.2);
    // 设置聚类的小点数和最大点云数
    clustering.setMinClusterSize(10);
    clustering.setMaxClusterSize(500);
    clustering.setSearchMethod(kdtree);
    clustering.setInputCloud(cut_cloud);
    std::vector<pcl::PointIndices> clusters;
    clustering.extract(clusters);

    for (int i = 0; i < 4; ++i) {
        m_cloud_segs[i]->clear();
    }
    if(clusters.size()==4)
    {
        int j = 0;
        for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
            //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
            for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
            {
                cloud_cluster->points.push_back(cut_cloud->points[*pit]);
                cloud_cluster->width = cloud_cluster->points.size();
                cloud_cluster->height = 1;
                cloud_cluster->is_dense = true;
            }
            m_cloud_segs[j]=cloud_cluster;
            j++;
        }
        // std::cout<<" cluster class:"<<clusters.size()<<std::endl;
        return true;
    }
    return false;
}

bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    bool result = false;
    preprocess(cloud_in, cut_cloud);

    if (cut_cloud->size() <= 0)
        return false;
    m_seg_mutex.lock();
    result = clustering(cut_cloud);
    m_seg_mutex.unlock();
    return result;
}

bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double &x, double &y, double &c, double &wheelbase, double &width)
{
    bool result = false;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filterd(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    preprocess(cloud_in, cloud_filterd);
    // std::cout<<"--------detector after preprocess------"<<std::endl;

    if (cloud_filterd->size() <= 0)
        return false;

    m_seg_mutex.lock();
    result = clustering(cloud_filterd);
    // std::cout<<"--------detector after clustering------"<<std::endl;
    if(result){
        // 获得四类，计算中心点角度等
        std::vector<Eigen::Vector4f> centers;
        std::vector<cv::Point2f> center_2d_points;
        Eigen::Vector4f car_center;
        for (size_t i = 0; i < 4; i++)
        {
            // 创建存储点云重心的对象
            Eigen::Vector4f centroid;
            if(0 == pcl::compute3DCentroid(*m_cloud_segs[i], centroid))
                return false;
            centers.push_back(centroid);
            car_center += centroid;
            center_2d_points.push_back(cv::Point2f(centroid[0], centroid[1]));
        }
        // std::cout<<"--------detector find center------"<<std::endl;
        car_center /= 4.0;
        x = car_center[0];
        y = car_center[1];

        cv::RotatedRect wheel_box = cv::minAreaRect(center_2d_points);
        // std::cout<<"--------detector find rect -----"<<std::endl;
        cv::Point2f vec;
        cv::Point2f vertice[4];
        wheel_box.points(vertice);
        float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
        float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
        if (len1 > len2)
        {
            vec.x = vertice[0].x - vertice[1].x;
            vec.y = vertice[0].y - vertice[1].y;
        }
        else
        {
            vec.x = vertice[1].x - vertice[2].x;
            vec.y = vertice[1].y - vertice[2].y;
        }
        float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
        c = angle_x;
        wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
        width = std::min(wheel_box.size.width, wheel_box.size.height);
        std::cout<<"--------detector find all------"<<std::endl;
    }

    m_seg_mutex.unlock();
    return result;
}