//
// 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(10); //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;
}

double distance(cv::Point2f p1, cv::Point2f p2)
{
    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}
bool Region_detector::isRect(std::vector<cv::Point2f>& points)
{
    if (points.size() == 4)
    {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        int max_index = 0;
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i) {
            if (L[i] > max_l) {
                max_index = i;
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }
        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.13) {
            std::cout << " angle cos >0.13 =" << cosa << "  i=" << max_index;
            std::cout << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
            return false;
        }

        float width=std::min(l1,l2);
        float length=std::max(l1,l2);
        if(width<1.400 || width >2.000 || length >3.300 ||length < 2.200)
        {
            std::cout<<"\t width<1400 || width >2100 || length >3300 ||length < 2100 "
                      <<"  length:"<<length<<"  width:"<<width;
            return false;
        }

        double d = distance(pc, points[3]);
        cv::Point2f center1 = (ps + pt) * 0.5;
        cv::Point2f center2 = (pc + points[3]) * 0.5;
        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
            std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2
                    << "  center distance=" << distance(center1, center2);;
            return false;
        }
        std::cout << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
                  << "  center distance=" << distance(center1, center2);
        return true;
    }
    else if(points.size()==3)
    {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        int max_index = 0;
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i) {
            if (L[i] > max_l) {
                max_index = i;
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }
        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.12) {
            std::cout << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
            std::cout << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
            return false;
        }

        double l=std::max(l1,l2);
        double w=std::min(l1,l2);
        if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
        {
            //生成第四个点
            cv::Point2f vec1=ps-pc;
            cv::Point2f vec2=pt-pc;
            cv::Point2f point4=(vec1+vec2)+pc;
            points.push_back(point4);
            std::cout << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << l
                      << "  w=" << w;
            return true;
        }
        else
        {
            std::cout<< "3 wheels rectangle verify Failed  cos angle=" << cosa << "  L=" << l
                      << "  w=" << w;
            return false;
        }

    }

}

bool Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
    if (cloud_in->size() == 0) return false;
    /////聚类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud(cloud_in);

    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(cloud_in);
    std::vector<pcl::PointIndices> clusters;
    clustering.extract(clusters);
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    for (int i = 0; i < clusters.size(); ++i) {
        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    }

    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(cloud_in->points[*pit]);
            cloud_cluster->width = cloud_cluster->points.size();
            cloud_cluster->height = 1;
            cloud_cluster->is_dense = true;
        }
        seg_clouds[j] = cloud_cluster;
        j++;
    }

    std::cout << " cluster class:" << clusters.size() << std::endl;

    std::vector<cv::Point2f> cvPoints;
    for(int i=0;i<clusters.size();++i)
    {
        if(seg_clouds[i]->size()==0)
            continue;
        float sumX=0,sumY=0;
        for(int j=0;j<seg_clouds[i]->size();++j)
        {
            sumX+=seg_clouds[i]->points[j].x;
            sumY+=seg_clouds[i]->points[j].y;
        }

        float center_x=sumX/float(seg_clouds[i]->size());
        float center_y=sumY/float(seg_clouds[i]->size());
        cvPoints.push_back(cv::Point2f(center_x,center_y));
    }
    return isRect(cvPoints);

}

bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    return false;
    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);



    if(result)
    {
        // convert all points after preprocessing into 2d
        std::vector<cv::Point2f> all_points;
        // car center
        for (int j = 0; j < cloud_filterd->size(); ++j) {
            all_points.push_back(cv::Point2f(cloud_filterd->points[j].x, cloud_filterd->points[j].y));
        }
         std::cout<<"--------detector find center------"<<std::endl;
        cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
        x=wheel_box.center.x;
        y=wheel_box.center.y;
         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;
}