/*
 * @Description: lidar calib tools
 * @Author: yct
 * @Date: 2021-09-02 11:02:00
 * @LastEditTime: 2022-01-25 12:15:07
 * @LastEditors: yct
 */

#include "../tool/point_tool.h"
#include <Eigen/Core>
#include <Eigen/Geometry>

/**
 * @description: used to find vlp16 intrinsic
 * @param  {*}
 * @return {*}
 * @param {Quaterniond} ground_quat  quaternion to align lidar coords with ground plane
 * @param {double} angle_z_axis  rotation angle along z axis to align lidar coords with PLC coords
 */
void find_intrinsic(Eigen::Vector3d ground_norm, Eigen::Vector3d y_axis)
{
    Eigen::Quaterniond ground_quat = Eigen::Quaterniond::FromTwoVectors(ground_norm, Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond xy_plane_quat = Eigen::Quaterniond::FromTwoVectors(y_axis, Eigen::Vector3d::UnitY());
    // std::cout << "y euler: " << xy_plane_quat.toRotationMatrix().eulerAngles(2, 1, 0).transpose() << std::endl;
    Eigen::Matrix3d combined_rotation = ground_quat.toRotationMatrix() * xy_plane_quat.toRotationMatrix();
    Eigen::Vector3d euler_angles = combined_rotation.eulerAngles(2, 1, 0);
    for (size_t i = 0; i < euler_angles.rows(); i++)
    {
        if(fabs(euler_angles[i]) > M_PI_2)
        {
            if(euler_angles[i] > 0)
            {
                euler_angles[i] = euler_angles[i] - M_PI;
            }else{
                euler_angles[i] = M_PI + euler_angles[i];
            }
        }
    }
    if(euler_angles[0]<0)
    {
        euler_angles[0] = euler_angles[0] + M_PI;
    }
    std::cout << "lidar intrinsic y p r: " << euler_angles.transpose()*180.0/M_PI << std::endl;
}

/**
 * @description: fit ground norm from two poles, then find y axis direction from front to back
 * @return {*}
 * @param {Ptr} front_barrier
 * @param {Ptr} back_barrier
 */
void find_rotation_from_points(pcl::PointCloud<pcl::PointXYZ>::Ptr front_barrier, pcl::PointCloud<pcl::PointXYZ>::Ptr back_barrier,
                                Eigen::Vector3d &front_direction, Eigen::Vector3d &back_direction, Eigen::Vector3d &y_direction)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr direction_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

    Eigen::Vector3d point_front, point_back;
    // 根据点云拟合直线
    //创建一个模型参数对象，用于记录拟合结果
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//inliers通过点云序号来记录模型内点
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	//创建一个分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	//Optional,设置结果直线展示的点是分割掉的点还是分割剩下的点
	seg.setOptimizeCoefficients(true);//flase 展示的是分割剩下的点
	//Mandatory-设置目标几何形状
	seg.setModelType(pcl::SACMODEL_LINE);
	//分割方法：随机采样法
	seg.setMethodType(pcl::SAC_RANSAC);
	//设置误差容忍范围，也就是阈值
	seg.setDistanceThreshold(0.01);
	//输入点云
	seg.setInputCloud(front_barrier);
	//分割点云
	seg.segment(*inliers, *coefficients);

    if(coefficients->values.size() < 6)
    {
        std::cout << "fatal, front line coeff size not enough." << std::endl;
        return;
    }
    else
    {
        point_front << coefficients->values[0], coefficients->values[1], coefficients->values[2];
        front_direction << coefficients->values[3], coefficients->values[4], coefficients->values[5];
        std::cout << "front direction: " << front_direction.transpose() << std::endl;
    }

    // 保存前杆方向点云，调试用
    for (size_t i = 0; i < 200; i++)
    {
        Eigen::Vector3d t_point = point_front + front_direction * 0.01 * i;
        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
    }

    seg.setInputCloud(back_barrier);
    seg.segment(*inliers, *coefficients);
    if(coefficients->values.size() < 6)
    {
        std::cout << "fatal, back line coeff size not enough." << std::endl;
        return;
    }
    else
    {
        point_back << coefficients->values[0], coefficients->values[1], coefficients->values[2];
        back_direction << coefficients->values[3], coefficients->values[4], coefficients->values[5];
        std::cout << "back direction: " << back_direction.transpose() << std::endl;
    }

    // 保存后杆方向点云，调试用
    for (size_t i = 0; i < 200; i++)
    {
        Eigen::Vector3d t_point = point_back + back_direction * 0.01 * i;
        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
    }

    std::cout << "point front to back : " << point_front.transpose() << " -----> " << point_back.transpose() << std::endl;

    // 注意y方向需要清除z分量
    y_direction = point_front - point_back;
    y_direction.z() = 0.0;
    std::cout << "y direction: " << y_direction.transpose() << std::endl;

    // 保存y方向点云，调试用
    for (size_t i = 0; i < 200; i++)
    {
        Eigen::Vector3d t_point = point_back + y_direction * 0.01 * i;
        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
    }
    save_cloud_txt(direction_cloud, "./direction_cloud.txt");
}

// passthrough and fit plane 
#include "pcl/filters/passthrough.h"
void find_ground(pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, Eigen::Vector4d &params, double avg_z=0.03)
{
    if(ground_cloud->size()<=0)
    {
        std::cout<<"empty ground cloud"<<std::endl;
        return;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    // 直通滤波
    double min_x = -2.6, max_x = 2.3, min_y = -3.0, max_y = 3.0, max_z = 0.1;
    for (size_t i = 0; i < ground_cloud->size(); i++)
    {
        pcl::PointXYZ pt = ground_cloud->points[i];
        if (pt.x > min_x && pt.x < max_x && pt.y > min_y && pt.y < max_y && pt.z < max_z)
        {
            t_cloud->push_back(pt);
        }
    }
    // 平面拟合
    if(t_cloud->size()<1)
    {
        std::cout<<"ground cloud not enough"<<std::endl;
        return;
    }
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(avg_z);
    seg.setInputCloud(t_cloud);
    seg.segment(*inliers, *coefficients);

    if(coefficients->values.size()<4)
    {
        std::cout<<"fit plane coeff size not enough"<<std::endl;
        return;
    }
    params << coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3];
}

void matrix_trans()
{
    // 921 A left
    Eigen::Matrix3d rot_matrix0;
    rot_matrix0.setIdentity();
    // rot_matrix0 << 0.3967, -0.9023, -0.1686,
    //     -0.8619, -0.4293, 0.2698,
    //     -0.3159, 0.03827, -0.948;
    // 671 B left
    // rot_matrix0 << 0.095394842327, -0.995411992073, 0.007405274082,
    //     -0.901792705059, -0.083268046379, 0.42407116293,
    //     -0.421508878469, -0.047132223845, -0.9055985808;
    // 211 B out left
    // rot_matrix0 << 0.083884611726, 0.990452945232, -0.109390966594,
    //     0.894117712975, -0.123275801539, -0.430530607700,
    //     -0.439905554056, -0.061693508178, -0.89592242240;
    // 541
    // rot_matrix0 << 0.127279058099, -0.989588797092, -0.067186594009,
    //     -0.888444364071, -0.143861815333, 0.435855954885,
    //     -0.440983742476, 0.004216216505, -0.897505164146;
    //621
    rot_matrix0 << -0.108142159879, -0.990688860416, 0.082709200680, 3.672443628311,
                    0.993864834309, -0.109678961337, -0.014255203307, -0.117867439985,
                    0.023193931207, 0.080660179257, 0.996471762657, 0.054298080504,
                    0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000;
    Eigen::Matrix3d rot_matrix1;
    rot_matrix1.setIdentity();
    // rot_matrix1 << 0.9993, 0.03618, 0.0,
    //     -0.03618, 0.9993, 0.0,
    //     0.0, 0.0, 1.0;

    Eigen::Vector3d euler = (rot_matrix1 * rot_matrix0).eulerAngles(2, 1, 0);
    std::cout << "mat to euler left (z y x): " << euler[0]*180.0/M_PI << ", " << euler[1]*180.0/M_PI << ", " << euler[2]*180.0/M_PI << std::endl;

    // 541 A right
    Eigen::Matrix3d rot_matrix2;
    rot_matrix2.setIdentity();
    // rot_matrix2 << 0.0812, 0.997, 0.0185,
    //     0.8787, -0.0628, -0.4731,
    //     -0.4703, 0.0547, -0.8808;
    // 091 B right
    // rot_matrix2 << -0.089172042906, -0.994127452374, 0.06131035462,
    //     -0.912935018539, 0.106191061437, 0.394047111273,
    //     -0.398243665695, -0.020834382623, -0.9170430302;
    // 051 B out right
    // rot_matrix2 << -0.097560293972, 0.995089590549, 0.016694894060,
    //     0.888557076454, 0.094646930695, -0.448896795511,
    //     -0.448272645473, -0.028960136697, -0.89342767000;
    // 001
    // rot_matrix2 << -0.077520310879, -0.990943193436, 0.109645560384,
    //     -0.916525423527, 0.114115700126, 0.383352011442,
    //     -0.392392337322, -0.070775382221, -0.91707092523;
    //131
    rot_matrix2 << 0.144158020616, 0.987975895405, -0.055875182152, 
            0.896719515324, -0.154303416610, -0.414830744267,
            -0.418464511633, 0.009696813300, -0.908181369305;

    Eigen::Matrix3d rot_matrix3;
    rot_matrix3.setIdentity();
    // rot_matrix3 << 0.97486, 0.2228, 0.0,
    //     -0.2228, 0.97486, 0.0,
    //     0.0, 0.0, 1.0;

    euler = (rot_matrix3 * rot_matrix2).eulerAngles(2, 1, 0);
    std::cout << "mat to euler right (z y x): " << euler[0]*180.0/M_PI << ", " << euler[1]*180.0/M_PI << ", " << euler[2]*180.0/M_PI << std::endl;

    //541: 98.1528, 153.833, -0.269157
    //001: 85.1654, 156.897, 4.41308

    // 921----- -2.31 161.58 -64 2.512
    // 541----- 176.45 28.05 71.85
    euler << -2.31, 161.58, -64;
    Eigen::Matrix3d euler2mat = Eigen::AngleAxisd(euler[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()).toRotationMatrix() * 
                                        Eigen::AngleAxisd(euler[1]*M_PI/180.0, Eigen::Vector3d::UnitY()).toRotationMatrix() *
                                        Eigen::AngleAxisd(euler[0]*M_PI/180.0, Eigen::Vector3d::UnitX()).toRotationMatrix();
    std::cout << "921 euler mat: \n" << euler2mat << std::endl;
    // -0.415912  0.892481  0.174631
    // 0.852745  0.449462 -0.266101
    // -0.31598 0.0382411 -0.947995

    euler << 176.45, 28.05, 71.85;
    euler2mat = Eigen::AngleAxisd(euler[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()).toRotationMatrix() * 
                                        Eigen::AngleAxisd(euler[1]*M_PI/180.0, Eigen::Vector3d::UnitY()).toRotationMatrix() *
                                        Eigen::AngleAxisd(euler[0]*M_PI/180.0, Eigen::Vector3d::UnitX()).toRotationMatrix();

    std::cout << "541 euler mat: \n" << euler2mat << std::endl;
    //   0.274916   0.957491 -0.0873633
    // 0.838626   -0.28324  -0.465276
    // -0.470242  0.0546463  -0.880844
}

int main()
{
    matrix_trans();
    return 0;

    // calib process:
    // 1. cc cut and fit ground, get normal and z
    // 2. get front and back pole txt
    // 3. run this calib func to get rpy and z
    std::cout << "------------------------ start calib lidar 0 ----------------------" << std::endl;
    // lidar 0 normal [09:22:54] 	- normal: (-0.000392,-0.011935,0.999929) z -0.07697
    // 90.6538 0.68354 0.0302616

    // lidar 1 normal 0.000297,-0.001893,0.999998 z -0.0686 
    // 91.3879 0.108841 -0.0143847

    // lidar 2 normal [10:43:07] 	- normal: (0.001131,-0.024040,0.999710) z -0.0461051
    // 86.3796  1.37069 -0.15165

    // lidar 3 normal [11:18:44] 	- normal: (0.000833,0.010742,0.999942) z -0.071
    // 91.4077  -0.614125 -0.0628335
    // pitch 1.1 ---- 0.485875

    pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
    t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    if (!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/3front_pole.txt", t_front))
    {
        std::cout << "read front failed." << std::endl;
        return -1;
    }
    if(!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/3back_pole.txt", t_back))
    {
        std::cout << "read back failed." << std::endl;
        return -1;
    }

    Eigen::Vector3d front_direction, back_direction, y_direction;
    find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);
    // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
    // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
    // find_intrinsic(Eigen::Vector3d(-0.000392,-0.011935,0.999929), y_direction);
    // find_intrinsic(Eigen::Vector3d(0.001131,-0.024040,0.999710), y_direction);
    find_intrinsic(Eigen::Vector3d(0.000833,0.010742,0.999942), y_direction);
    return 0;

    std::cout << "------------------------ start calib lidar 1 ----------------------" << std::endl;
    // cloud 1
    if (!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/front_pole2.txt", t_front))
    {
        std::cout << "read front failed." << std::endl;
        return -1;
    }
    if(!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/back_pole2.txt", t_back))
    {
        std::cout << "read back failed." << std::endl;
        return -1;
    }
    find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);

    // -0.00566995 0-0.0233422 0000.999712 000.0949598
    find_intrinsic(Eigen::Vector3d(-0.00566995,-0.0233422,0.999712), y_direction);

    return 0;
}