#include "clamp_safety_detect.h"

bool clamp_safety_detect::fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Line &line) {
    if (cloud->size() < 20)
        return false;

    Eigen::MatrixXd A, B;
    A.resize(cloud->size(), 2);
    B.resize(cloud->size(), 1);
    for (int i = 0; i < cloud->size(); ++i) {
        A(i, 0) = cloud->points[i].x;
        A(i, 1) = 1.0;
        B(i, 0) = cloud->points[i].y;
    }
    Eigen::MatrixXd P = (A.transpose() * A).inverse() * (A.transpose()) * B;

    float a = P(0, 0);
    float b = -1.0;
    float c = P(1, 0);
    float sum = 0;
    float sumx = 0, sumy = 0, w = 0, sumw = 0;

    for (int i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ pt = cloud->points[i];
        w = float(1 / pow(cos(atan(fabs(pt.x / pt.y))), 2));
        sumw += w;
        sumx += w * pt.x;
        sumy += pt.y;
        sum += std::pow(a * pt.x + b * pt.y + c, 2) / (a * a + b * b);
    }

    line.cov = sqrt(sum) / float(cloud->size());
    line.cx = sumx / sumw;
    line.cy = sumy / float(cloud->size());

    line.a = a;
    line.b = b;
    line.c = c;
    line.theta_by_x = acos(abs(b / sqrt(a * a + b * b))) * 180.0 / M_PI;
    line.cloud = cloud;

    //printf(" abc(%f,%f,%f) line.vonv  cov:%f,%f,%f,%f,%f\n",a,b,c,line.theta_by_x,line.cov);
    return true;

}

bool compare_r(const Line &line1, const Line &line2) {
    return line1.theta_by_x < line2.theta_by_x;
}

bool pca(pcl::PointCloud<pcl::PointXY>::Ptr neighber, float &theta) {

    if (neighber->size() < 10) {
        return false;
    }
    float sumx = 0, sumy = 0;
    for (int i = 0; i < neighber->size(); ++i) {

        sumx += neighber->points[i].x;
        sumy += neighber->points[i].y;

    }
    float mean_x = sumx / float(neighber->size());
    float mean_y = sumy / float(neighber->size());
    Eigen::MatrixXd A(2, neighber->size());
    for (int i = 0; i < neighber->size(); ++i) {
        pcl::PointXY pt = neighber->points[i];
        A(0, i) = pt.x - mean_x;
        A(1, i) = pt.y - mean_y;

    }
    Eigen::MatrixXd cov = A * A.transpose();
    Eigen::EigenSolver<Eigen::MatrixXd> es(cov);

    Eigen::VectorXd values = es.eigenvalues().real().normalized();
    Eigen::MatrixXd vectors = es.eigenvectors().real();
    //printf("pca values: %.5f  %.5f ",values[0],values[1]);
    float max_value = values[0];
    Eigen::Vector2d normal = vectors.col(0);
    if (values[1] > values[0]) {
        max_value = values[1];
        normal = vectors.col(1);
    }

    if (isnanf(max_value)) {
        return false;
    }
    if (max_value < 0.90) {
        return false;
    }


    float r = 90 * 0.005 / (2. * M_PI);
    theta = acos(abs(normal[0])) * r;

    return true;
}

void pca_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr features, int length) {
    if (cloud->size() < length) {
        printf("ERROR  pca points size < %d\n", length);
        return;
    }
    features->clear();
    int fail_pca = 0;
    for (int i = length / 2; i < cloud->size() - length / 2; ++i) {
        pcl::PointCloud<pcl::PointXY>::Ptr neighber(new pcl::PointCloud<pcl::PointXY>);
        for (int j = i - length / 2; j < i + length / 2; ++j) {
            pcl::PointXYZ pt = cloud->points[j];
            if (pt.y < 0.05 || pt.y > 0.5 || fabs(pt.x) > 0.5)
                continue;
            pcl::PointXY p_xy;
            p_xy.x = cloud->points[j].x;
            p_xy.y = cloud->points[j].y;
            if (isnan(p_xy.x) || isnan(p_xy.y))
                continue;
            neighber->push_back(p_xy);
        }
        float theta = 0;
        if (pca(neighber, theta)) {
            pcl::PointXYZ pt = cloud->points[i];
            pt.z = theta;
            features->push_back(pt);

        } else {
            fail_pca++;
        }
    }

}

void pca_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr features, float radius) {
    if (cloud->size() < 200)
        return;

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    //pcl::KdTreeFLANN<pcl::PointXYZ> tree;
    tree->setInputCloud(cloud);
    features->clear();
    for (int i = 0; i < cloud->size(); ++i) {
        std::vector<float> distances;    //distance Vector
        std::vector<int> neighber_ids;
        tree->radiusSearch(cloud->points[i], radius, neighber_ids, distances); //KD tree

        pcl::PointCloud<pcl::PointXY>::Ptr neighber(new pcl::PointCloud<pcl::PointXY>);
        for (int neighber_id : neighber_ids) {
            pcl::PointXY p_xy{};
            p_xy.x = cloud->points[neighber_id].x;
            p_xy.y = cloud->points[neighber_id].y;
            neighber->push_back(p_xy);
        }
        float theta = 0;
        if (pca(neighber, theta)) {
            pcl::PointXYZ pt = cloud->points[i];
            pt.z = theta;
            features->push_back(pt);
        }
    }

}


clamp_safety_detect::clamp_safety_detect() = default;

clamp_safety_detect::~clamp_safety_detect() = default;

#if __VIEW__PCL

void clamp_safety_detect::set_viewer(pcl::visualization::PCLVisualizer* viewer,int port)
{
    m_viewer=viewer;
    m_port=port;
}

void clamp_safety_detect::view_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string name,int r,int g,int b)
{
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud,r,g,b);
    m_viewer->addPointCloud(cloud, single_color2, name,m_port);
    m_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, name,m_port);
}

#endif


std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clamp_safety_detect::classify_cloud(
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_cloud;
    //printf("  cloud size():%d\n",cloud->size());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_r(new pcl::PointCloud<pcl::PointXYZ>);

    pca_cloud(cloud, cloud_r, 20);

    if (cloud_r->size() == 0) {
        return cluster_cloud;
    }
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

    tree->setInputCloud(cloud_r);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.1);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(1000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_r);
    ec.extract(cluster_indices);


    cluster_cloud.resize(cluster_indices.size());

    for (int i = 0; i < cluster_indices.size(); ++i) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr clust(new pcl::PointCloud<pcl::PointXYZ>);
        for (int j = 0; j < cluster_indices[i].indices.size(); ++j) {
            pcl::PointXYZ pt;
            pcl::PointXYZ ptr = cloud_r->points[cluster_indices[i].indices[j]];
            pt.x = ptr.x;
            pt.y = ptr.y;
            pt.z = ptr.z; //z表示角度
            clust->push_back(pt);
        }
        cluster_cloud[i] = clust;

    }

    return cluster_cloud;
}

bool clamp_safety_detect::check_wheel_line(const Line &line, Safety_status &safety) {
    if (line.cloud->size() < 20) {
        printf("line cloud size <20\n");
        return false;
    }
    ///判断轮胎
    pcl::PointXYZ minp, maxp;
    pcl::getMinMax3D(*line.cloud, minp, maxp);
    float length = maxp.x - minp.x;
    if (line.cov < 0.03 && fabs(line.theta_by_x) < 10.
        && (length > 0.1 && length < 0.5)) {
        //前方直线是轮胎,判断中心是否居中、直线长度是否在范围内

        safety.cx = line.cx;
        // printf("safety.cx = %.3f\n", safety.cx);
        //雷达距离轮胎的距离   间隙
        safety.gap = fabs(line.cy);
        //中心偏差5cm内，可夹，plc决定
        safety.wheel_exist = true;
    }
#if __VIEW__PCL
    char name[64]={0};
    //显示直线，正确为绿，偏移为黄，错误为红，
    char buf[64]={0};
    sprintf(buf,"cx:%.4f,gap:%.4f,Θ:%.4f cov:%.4f",safety.cx,fabs(line.cy),line.theta_by_x,line.cov);
    sprintf(name,"wheel_%d",m_port);
    m_viewer->addText3D(buf,pcl::PointXYZ(line.cx-0.2,line.cy,0),0.01,0,1,0,"wheel",m_port);

    float x1=line.cx-0.2;
    float x2=line.cx+0.2;
    float y1=-(x1*line.a/line.b+line.c/line.b);
    float y2=-(x2*line.a/line.b+line.c/line.b);

    sprintf(name,"line_%d",m_port);
    if (safety.wheel_exist)
    {
        m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 0, 255, 0, name,m_port);
    }
    else if (fabs(safety.cx) < 0.5)
    {
        m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 255, 0, name,m_port);
    }
    else
    {
        m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 0, 0, name,m_port);
    }
#endif
    return safety.wheel_exist;

}

bool clamp_safety_detect::check_clamp_lines(const Line &line1, const Line &line2, Safety_status &safety) {
    if (fabs(line1.theta_by_x - line2.theta_by_x) > 10 || line1.cov > 0.03 || line2.cov > 0.03) {

    }

    if (fabs(line1.theta_by_x - 90.) < 15. && fabs(line2.theta_by_x - 90.) < 15.)
        safety.clamp_completed = true;

#if __VIEW__PCL
    char name[64]={0};
    float x1=line1.cx-0.2;
    float x2=line1.cx+0.2;
    float y1=-(x1*line1.a/line1.b+line1.c/line1.b);
    float y2=-(x2*line1.a/line1.b+line1.c/line1.b);
    sprintf(name,"line1_%d",m_port);
    m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 0, 0, name,m_port);
    char buf[32]={0};
    sprintf(buf,"cx:%.3f,Θ:%.1f cov:%.3f",line1.cx,line1.theta_by_x,line1.cov);
    sprintf(name,"line1_txt_%d",m_port);
    m_viewer->addText3D(buf,pcl::PointXYZ(line1.cx-0.15,line1.cy,0),0.01,0,1,0,name,m_port);

    x1=line2.cx-0.2;
    x2=line2.cx+0.2;
    y1=-(x1*line2.a/line2.b+line2.c/line2.b);
    y2=-(x2*line2.a/line2.b+line2.c/line2.b);
    sprintf(name,"line2_%d",m_port);
    m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 0, 0, name,m_port);
    memset(buf,0,32);
    sprintf(buf,"cx:%.3f,Θ:%.3f cov:%.3f",line2.cx,line2.theta_by_x,line2.cov);
    sprintf(name,"line2_txt%d",m_port);
    m_viewer->addText3D(buf,pcl::PointXYZ(line2.cx-0.15,line2.cy,0),0.01,0,1,0,name,m_port);
#endif
    return true;
}

bool clamp_safety_detect::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Safety_status &safety) {
#if __VIEW__PCL
    m_viewer->removeAllPointClouds(m_port);
    m_viewer->removeAllShapes(m_port);
    char buf[32]={0};
    sprintf(buf,"cloud_%d",m_port);
    view_cloud(cloud,buf,255,255,255);
#endif
    //找直线，记录内点率，直线与x轴夹角，斜率b/a
    Line line;
    if (fit_line(cloud, line) && check_wheel_line(line, safety)) {
        return true;
    }
    return false;
}