/*
 * @Description: 
 * @Author: yct
 * @Date: 2021-09-16 15:18:34
 * @LastEditTime: 2021-10-12 17:38:39
 * @LastEditors: yct
 */

#include "car_pose_detector.h"

// 变换点云, 反向平移后旋转
void Car_pose_detector::inv_trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta)
{
    for (int i = 0; i < cloud_ptr->size(); i++)
    {
        Eigen::Matrix<double, 2, 1> t_point(double(cloud_ptr->points[i].x) - x, double(cloud_ptr->points[i].y) - y);
        Eigen::Rotation2D<double> rotation(theta);
        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
        Eigen::Matrix<double, 2, 1> trans_point = rotation_matrix * t_point;
        cloud_ptr->points[i].x = trans_point.x();
        cloud_ptr->points[i].y = trans_point.y();
    }
}

// 变换点云,旋转后平移
void Car_pose_detector::trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta)
{
    for (int i = 0; i < cloud_ptr->size(); i++)
    {
        Eigen::Matrix<double, 2, 1> t_point(double(cloud_ptr->points[i].x), double(cloud_ptr->points[i].y));
        Eigen::Rotation2D<double> rotation(theta);
        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
        Eigen::Matrix<double, 2, 1> trans_point = rotation_matrix * t_point;
        cloud_ptr->points[i].x = trans_point.x()+x;
        cloud_ptr->points[i].y = trans_point.y()+y;
    }
}

// 创建两轴具体曲线
void Car_pose_detector::create_curve_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double width)
{
    for (double x = -2.5; x < 2.5; x += 0.02)
    {
        double left_value = 1.0 / (1.0 + exp(30 * (x + width / 2.0)));
        double right_value = 1.0 / (1.0 + exp(30 * (-x + width / 2.0)));
        double front_value = 1.0 / (1.0 + exp(15 * (x + 2.2)));
        double back_value = 1.0 / (1.0 + exp(15 * (-x + 2.2)));
        cloud_ptr->push_back(pcl::PointXYZ(x, std::max(left_value, right_value) - 3.0, 0.0));
        cloud_ptr->push_back(pcl::PointXYZ(std::max(front_value, back_value) - 2.0, x, 0.0));
    }
}

// 检测底盘z方向值，原始点云去中心，并切除底盘z以上部分
bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, double &width, double &z_value, bool debug_cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    t_cloud->operator+=(*cloud_ptr);
    double t_vars[4] = {0, 0, 0, 1.9};
    // 去中心化
    pcl::PointXYZ t_center(0, 0, 0);
    for (size_t i = 0; i < t_cloud->size(); i++)
    {
        // t_cloud->points[i].x /= 1000.0;
        // t_cloud->points[i].y /= 1000.0;
        // t_cloud->points[i].z /= 1000.0;
        t_center.x += t_cloud->points[i].x;
        t_center.y += t_cloud->points[i].y;
        // t_center.z += t_cloud->points[i].z;
    }
    t_center.x /= t_cloud->size();
    t_center.y /= t_cloud->size();
    // t_center.z /= t_cloud->size();
    for (size_t i = 0; i < t_cloud->size(); i++)
    {
        t_cloud->points[i].x -= t_center.x;
        t_cloud->points[i].y -= t_center.y;
        // t_cloud->points[i].z -= t_center.z;
    }
    // // write_pointcloud(t_cloud, std::to_string(count)+"_ori_");

    // 构建最小二乘问题
    ceres::Problem problem;
    problem.AddResidualBlock( // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
        new ceres::AutoDiffCostFunction<Car_pose_cost, ceres::DYNAMIC, 3>(
            new Car_pose_cost(t_cloud), t_cloud->size()),
        nullptr, // 核函数，这里不使用，为空
        t_vars   // 待估计参数
    );

    // 配置求解器
    ceres::Solver::Options options;                            // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
    options.minimizer_progress_to_stdout = false;              // 输出到cout
    options.max_num_iterations = 100;

    ceres::Solver::Summary summary; // 优化信息
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary); // 开始优化
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    // std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;

    // 输出结果
    // std::cout << summary.BriefReport() << std::endl;
    // t_vars[3] -= 0.4;
    x = t_vars[0];
    y = t_vars[1];
    theta = t_vars[2];
    if(fabs(theta)>30*M_PI/180.0)
    {
        //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
        return false;
    }

    inv_trans_cloud(t_cloud, x, y, theta);
    // viewer.showCloud(cloud_ptr);
    // write_pointcloud(cloud_ptr, "uniform_");

    // //离群点过滤
    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    // sor.setInputCloud(t_cloud);
    // sor.setMeanK(15);            //K近邻搜索点个数
    // sor.setStddevMulThresh(3.0); //标准差倍数
    // sor.setNegative(false);      //保留未滤波点（内点）
    // sor.filter(*t_cloud);      //保存滤波结果到cloud_filter

    // 判断x方向边界，若不关于中心对称则错误
    pcl::PointXYZ total_min_p, total_max_p;
    pcl::getMinMax3D(*t_cloud, total_min_p, total_max_p);
    double x_diff = fabs(total_max_p.x + total_min_p.x) / 2.0;
    if (x_diff > 2.0)
    {
        // std::cout << "left, right not mirroring----"<<x_diff << std::endl;
    }
    else
    {
        // std::cout << "x diff " << x_diff << std::endl;
        width = total_max_p.x - total_min_p.x;
        // 最大轮宽0.28再加上前轮极限旋转角35度
        double wheel_width = 0.28 * (1 + sin(35 * M_PI / 180.0));
        // std::cout << "car width: " << width << std::endl;
        // 切出底盘点，找最低即为z值
        pcl::PointCloud<pcl::PointXYZ>::Ptr inside_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(t_cloud);
        pass.setFilterFieldName("x");
        pass.setFilterLimits(-width / 2.0 + wheel_width, width / 2.0 - wheel_width);
        pass.setFilterLimitsNegative(false);
        pass.filter(*inside_cloud);
        // 找最低为z值
        pcl::PointXYZ min_p, max_p;
        pcl::getMinMax3D(*inside_cloud, min_p, max_p);
        z_value = min_p.z-0.01;
        // 根据z值切原始点云
        pass.setInputCloud(t_cloud);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(total_min_p.z, z_value);
        pass.setFilterLimitsNegative(false);
        pass.filter(*t_cloud);
        // std::cout << "\n--------------------------- chassis z0: " << min_p.z << std::endl;

        if (debug_cloud)
        {
            create_curve_cloud(t_cloud, t_vars[3]);

            for (size_t i = 0; i < 60; i++)
            {
                t_cloud->push_back(pcl::PointXYZ(-t_vars[3] / 2.0, -3.0 + i * 0.1, 0.0));
                t_cloud->push_back(pcl::PointXYZ(t_vars[3] / 2.0, -3.0 + i * 0.1, 0.0));
                t_cloud->push_back(pcl::PointXYZ(-width / 2.0, -3.0 + i * 0.1, 0.0));
                t_cloud->push_back(pcl::PointXYZ(width / 2.0, -3.0 + i * 0.1, 0.0));
                t_cloud->push_back(pcl::PointXYZ(-width / 2.0 + wheel_width, -3.0 + i * 0.1, 0.0));
                t_cloud->push_back(pcl::PointXYZ(width / 2.0 - wheel_width, -3.0 + i * 0.1, 0.0));
            }
            out_cloud_ptr->clear();
            out_cloud_ptr->operator+=(*t_cloud);
        }else
        {
            out_cloud_ptr->clear();
            out_cloud_ptr->operator+=(*t_cloud);
        }
    }
    x += t_center.x;
    y += t_center.y;
    // std::cout << "estimated x,y,theta = " << x << ", " << y << ", " << theta*180.0/M_PI << std::endl;
    // std::cout << "----------------------------------" << std::endl;
    return true;
}