//
// Created by zx on 2021/8/27.
//

#include "rotate_center_caliber.h"
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>

// extern pcl::visualization::PCLVisualizer viewer;
// extern std::mutex mut;

void rotate_center_caliber::rotate_center_caliber_init()
{
}

void rotate_center_caliber::rotate_center_caliber_uninit()
{
    delete mp_matcher;
    mp_matcher = nullptr;
}

void rotate_center_caliber::set_first_frame(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model)
{
    m_model_cloud=cloud_model;
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud_model, centroid);
    m_center.x=centroid[0];
    m_center.y=centroid[1];
    m_center.z=centroid[2];

    //平移点云
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    transform_2.translation() << -centroid[0],-centroid[1],-centroid[2];
    pcl::transformPointCloud(*cloud_model, *transformed_cloud, transform_2);

    printf("model center: %.5f,%.5f,%.5f\n",centroid[0],centroid[1],centroid[2]);


    mp_matcher=new detect_ceres3d(transformed_cloud);
}

bool rotate_center_caliber::push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{

    //平移点云
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    pcl::PointCloud<pcl::PointXYZ>::Ptr offset_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    transform_2.translation() << -m_center.x,-m_center.y,-m_center.z;
    pcl::transformPointCloud(*cloud, *offset_cloud, transform_2);

    // mut.lock();
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); // green
    // viewer.removePointCloud("cloud_rotation");
    // viewer.addPointCloud(cloud, single_color,"cloud_rotation");
    // mut.unlock();


    if(mp_matcher== nullptr)
        return false;

    static detect_ceres3d::Detect_result last_result;
    std::string error;
    pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    detect_ceres3d::Detect_result result=last_result;
    if(mp_matcher->detect(offset_cloud,result,transform_cloud,error))
    {
        if(fabs(result.theta-last_result.theta)>1.0/180.*M_PI)
        {
            printf(" detect success  r : %.3f   t:%.5f,%.5f\n", result.theta * 180. / M_PI, result.cx, result.cy);

            Eigen::Affine3f transform_affine = Eigen::Affine3f::Identity();
            pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZ>());
            transform_affine.translation() << m_center.x, m_center.y, m_center.z;
            pcl::transformPointCloud(*transform_cloud, *new_cloud, transform_affine);

            // mut.lock();
            // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 0, 0); // green
            // viewer.removePointCloud("ndt_cloud");
            // viewer.addPointCloud(new_cloud, single_color, "ndt_cloud");
            // mut.unlock();

            match_transform solve_data;
            solve_data.theta = result.theta;
            solve_data.dx = result.cx;
            solve_data.dy = result.cy;

            m_match_datas.push_back(solve_data);
            last_result = result;
        }
        else
        {
            // std::cout << " small dtheta: " << (result.theta - last_result.theta) * 180.0 / M_PI << std::endl;
        }
    }else{
        std::cout << "?????" << std::endl;
    }

    /*Eigen::Matrix<float,6,1>  temp;
    temp.setZero();
    static Eigen::Matrix<float, 6,1> last_transform=temp;
    printf("init r:%.3f,%.3f,%.3f, t :%.5f,%.5f,%.5f\n",
            last_transform(2,0)*180.0/M_PI,last_transform(1,0)*180.0/M_PI,last_transform(0,0)*180.0/M_PI,
           last_transform(3,0),last_transform(4,0),last_transform(5,0));
    Eigen::Matrix4f matrix=match(m_model_cloud,cloud,last_transform);

    //std::cout<<"  match :"<<matrix<<std::endl;

    Eigen::Matrix3f roation=matrix.topLeftCorner(3,3);
    Eigen::Vector3f euler_angles = roation.eulerAngles(0, 1, 2);

    //std::cout<<euler_angles<<std::endl;
    printf("r:%.3f,%.3f,%.3f  t:%.5f,%.5f\n",euler_angles[2]*180.0/M_PI,euler_angles[1]*180.0/M_PI,
           euler_angles[0]*180.0/M_PI,matrix(0,3),matrix(1,3));

    match_transform solve_data;
    solve_data.theta=euler_angles[2];
    solve_data.dx=matrix(0,3);
    solve_data.dy=matrix(1,3);

    m_match_datas.push_back(solve_data);

    last_transform[0]=euler_angles[0];
    last_transform[1]=euler_angles[1];
    last_transform[2]=euler_angles[2];

    last_transform[3]=matrix(0,3);
    last_transform[4]=matrix(1,3);
    last_transform[5]=matrix(2,3);
*/

    if(solve())
    {
        /*printf(" solve center = %.5f,%.5f\n",
                m_rotation_center_x,m_rotation_center_y);*/

        // updata_points();
    }

}


Eigen::Matrix4f rotate_center_caliber::match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Eigen::Matrix<float, 6,1> init_transform)
{
    //初始化正态分布变换（NDT）
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    //设置依赖尺度NDT参数
    //为终止条件设置最小转换差异
    ndt.setTransformationEpsilon(0.0001);
    //为More-Thuente线搜索设置最大步长
    ndt.setStepSize(0.1);
    //设置NDT网格结构的分辨率（VoxelGridCovariance）
    ndt.setResolution(0.3);
    //设置匹配迭代的最大次数
    ndt.setMaximumIterations(100);
    // 设置要配准的点云
    ndt.setInputSource(cloud);
    //设置点云配准目标
    ndt.setInputTarget(cloud_center);
    //设置使用机器人测距法得到的初始对准估计结果

    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(init_transform(0), Eigen::Vector3f::UnitX()));
    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(init_transform(1), Eigen::Vector3f::UnitY()));
    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(init_transform(2), Eigen::Vector3f::UnitZ()));
    Eigen::AngleAxisf rotation ;
    rotation = yawAngle*pitchAngle*rollAngle;
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

    transform_2.translation() << init_transform(3),init_transform(4),init_transform(5);	// 三个数分别对应X轴、Y轴、Z轴方向上的平移
    transform_2.rotate(rotation);	//同理，UnitX(),绕X轴；UnitY(

    Eigen::Matrix4f init_guess = transform_2.matrix();
    //计算需要的刚体变换以便将输入的点云匹配到目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, init_guess);

    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
              << " score: " << ndt.getFitnessScore() << std::endl;



    //使用创建的变换对未过滤的输入点云进行变换
    return ndt.getFinalTransformation();
}

bool rotate_center_caliber::solve()
{
    int size=m_match_datas.size();

    if(size<=1)
    {
        LOG(INFO) <<"match_data size:"<< m_match_datas.size();
        return false;
    }

    Eigen::MatrixXf A;
    A.resize(2*size,2);
    Eigen::MatrixXf B;
    B.resize(2*size,1);

    for(int i=0;i<size;i++)
    {
        match_transform data=m_match_datas[i];
        float da=-data.theta;
        float dx=-data.dx;
        float dy=-data.dy;

        float RA=1-cos(da);
        float RB=sin(da);
        float RC=-sin(da);
        float RD=1-cos(da);

        A(i*2+0,0)=RA;
        A(i*2+0,1)=RB;
        A(i*2+1,0)=RC;
        A(i*2+1,1)=RD;

        B(i*2+0,0)=dx;
        B(i*2+1,0)=dy;

    }
    //std::cout<<"A:"<<A<<std::endl;
    Eigen::MatrixXf result= (A.transpose()*A).inverse()*(A.transpose())*B;
    //std::cout<<"result M:"<<result.transpose()<<std::endl;

    m_rotation_center_x=result(0,0)+m_center.x;
    m_rotation_center_y=result(1,0)+m_center.y;
    return true;

}
void rotate_center_caliber::updata_points()
{
    if(m_match_datas.size()<1)
        return ;
    static int id = 0;
    if(id==0)
    {
        // viewer.addLine(pcl::PointXYZ(m_center.x,m_center.y,0),
        //         pcl::PointXYZ(m_rotation_center_x,m_rotation_center_y,0),0,1,1,"org");
    }
    double max_d=-1,min_d=1e9;
    for (int i = 0; i < m_match_datas.size(); ++i)
    {

        float x = -m_match_datas[i].dx + m_center.x;
        float y = -m_match_datas[i].dy + m_center.y;

        char buf[32] = {0};
        sprintf(buf, "line_%d", i);
        // mut.lock();
        // if (i < id)
        // {
        //     viewer.removeShape(buf);
        // }

        // viewer.addLine(pcl::PointXYZ(x, y, 0),
        //                pcl::PointXYZ(m_rotation_center_x, m_rotation_center_y, 0),
        //                0,
        //                0,
        //                1,
        //                buf);

        // mut.unlock();

        double distance=sqrt(pow(m_rotation_center_x-x,2)+pow(m_rotation_center_y-y,2));
        if(distance<min_d)
            min_d=distance;
        if(distance>max_d)
            max_d=distance;

    }
    id = m_match_datas.size();
    if((max_d-min_d)>0.01)
    {
        LOG(ERROR)<<" distances verify failed ";
        return ;
    }
    LOG(INFO)<<" distances verify :"<<max_d-min_d;
}