//
// Created by zx on 2021/5/11.
//

#include "point_tool.h"
/*
void rotation_matrix_to_rpy()
{
    //2#  92.0084   112.664  179.263
    //3#  85.2597  73.8993  -1.70915
    //4#  91.93   112.146   -179.65
    //5#  32.9435  67.8446  -52.9897
    //6#  124.098  119.42  -150.147
    //8#  37.3443  111.887  130.948


//    mat_r00:0.0229183
//    mat_r01:-0.9985044
//    mat_r02:0.0496393
//    mat_r03:5287.829
//    mat_r10:0.2763783
//    mat_r11:0.0540452
//    mat_r12:0.9595283
//    mat_r13:-2399.535
//    mat_r20:-0.9607757
//    mat_r21:-0.0082715
//    mat_r22:0.2772034
//    mat_r23:6674.29500
    Eigen::Matrix3d mat;
    mat<<0.0229183,
            -0.9985044,
    0.0496393,
            0.2763783,
    0.0540452,
    0.9595283,
            -0.9607757,
                    -0.0082715,
    0.2772034;
    Eigen::Vector3d euler_angles = mat.eulerAngles(2, 1, 0);
    std::cout<<euler_angles*180.0/M_PI<<std::endl;
}

void rpy_to_rotation_matrix()
{
    //
    float ea[3]={-172,105.42,99.38};
    Eigen::Matrix3d rotation_matrix3;
    rotation_matrix3 = Eigen::AngleAxisd(ea[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(ea[1]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(ea[0]*M_PI/180.0, Eigen::Vector3d::UnitX());
    std::cout << "rotation matrix3 =\n" << rotation_matrix3 << std::endl;
}

void reverse_test(){
    Eigen::Matrix3d mat;
    mat<<-0.2963634,0.9547403,-0.0252988,-0.2261306,-0.0958803,-0.9693665,-0.9279189,-0.2815638,0.2443113;
    Eigen::Vector3d euler_angles = mat.eulerAngles(2, 1, 0);
    std::cout<<euler_angles*180.0/M_PI<<std::endl;
    Eigen::Matrix3d rotation_matrix3;
    rotation_matrix3 = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitX());
    std::cout << "origin matrix3 =\n" << mat << std::endl;
    std::cout << "rotation matrix3 =\n" << rotation_matrix3 << std::endl;
}

bool string2point(std::string str,pcl::PointXYZ& point)
{
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3]={0};
    for(int i=0;i<3;++i)
    {
        if(iss>>value)
        {
            data[i]=value;
        }
        else
        {
            return false;
        }
    }
    point.x=data[0];
    point.y=data[1];
    point.z=data[2];
    return true;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
{
    std::ifstream fin(file.c_str());
    const int line_length=64;
    char str[line_length]={0};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    while(fin.getline(str,line_length))
    {
        pcl::PointXYZ point;
        if(string2point(str,point))
        {
            cloud->push_back(point);
        }
    }
    return cloud;
}


void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string file)
{
    FILE* pf=fopen(file.c_str(),"w");
    for(int i=0;i<cloud->size();++i)
    {
        pcl::PointXYZ point=cloud->points[i];
        fprintf(pf,"%.3f %.3f %.3f\n",point.x,point.y,point.z);
    }
    fclose(pf);
}

void save_cloud_txt(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string file)
{
    FILE* pf=fopen(file.c_str(),"w");
    for(int i=0;i<cloud->size();++i)
    {
        pcl::PointXYZRGB point=cloud->points[i];
        fprintf(pf,"%.3f %.3f %.3f %d %d %d\n",point.x,point.y,point.z,point.r,point.g,point.b);
    }
    fclose(pf);
}

//欧式聚类*******************************************************
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
{
    std::vector<pcl::PointIndices> ece_inlier;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
    ece.setInputCloud(sor_cloud);
    ece.setClusterTolerance(0.2);
    ece.setMinClusterSize(20);
    ece.setMaxClusterSize(20000);
    ece.setSearchMethod(tree);
    ece.extract(ece_inlier);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>  segmentation_clouds;
    for (int i = 0; i < ece_inlier.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
        segmentation_clouds.push_back(cloud_copy);
    }
    return segmentation_clouds;
}

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 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];
        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_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];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh=20.0*M_PI/180.0;
        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
        float angle=atan2(vct.y,vct.x);
        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
        {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
//            char description[255]={0};
//            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
//            std::cout<<description<<std::endl;
            return false;
        }

        float width=std::min(l1,l2);
        float length=std::max(l1,l2);
        if(width<1.400 || width >1.900 || length >3.300 ||length < 2.200)
        {
//            char description[255]={0};
//            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
//            std::cout<<description<<std::endl;
            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<<std::endl;
//            char description[255]={0};
//            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
//            std::cout<<description<<std::endl;
            return false;
        }
        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;


        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];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh=20.0*M_PI/180.0;
        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
        float angle=atan2(vct.y,vct.x);
        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
        {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
//            char description[255]={0};
//            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
//            std::cout<<description<<std::endl;
            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);

//            char description[255]={0};
//            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
//            std::cout<<description<<std::endl;
            return true;
        }
        else
        {

//            char description[255]={0};
//            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
//            std::cout<<description<<std::endl;
            return false;
        }

    }
    //std::cout<<" default false"<<std::endl;
    return false;

}

bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
    pointcloud->clear();
    std::ifstream in(filename);
    if (!in.is_open())
    {
        std::cout << "failed to open file " << filename << std::endl;
        return false;
    }
    while (!in.eof())
    {
        std::string t_linestr;
        if (getline(in, t_linestr))
        {
            std::vector<std::string> str_vector;
            std::stringstream ss(t_linestr);
            std::string num_str;
            while (getline(ss, num_str, ' '))
            {
                str_vector.push_back(num_str);
            }
            if (str_vector.size() != 3)
            {
                std::cout << "unsupported point cloud / cannot find point x y z value: " << filename << std::endl;
                return false;
            }
            pcl::PointXYZ t_cloud;
            t_cloud.x = stod(str_vector[0]);
            t_cloud.y = stod(str_vector[1]);
            t_cloud.z = stod(str_vector[2]);
            pointcloud->push_back(t_cloud);
        }
    }
    in.close();
    std::cout << "file read finished with points " << pointcloud->size() << std::endl;
    return true;
}
*/