#include <ceres/ceres.h>
#include <ceres/cubic_interpolation.h>
#include "car_pose_detector.h"

struct DiscreteCostFunction {
    std::vector<double> line_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
    explicit DiscreteCostFunction(std::vector<double> &line,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
        line_ = line;
        cloud_=cloud;
    }

    template<typename JetT>
    bool operator()(const JetT* const x, JetT* residual) const {

        ceres::Grid1D<double> grid(line_.data(), 0, line_.size());
        ceres::CubicInterpolator<ceres::Grid1D<double>> cubic(grid);
        JetT tx=x[0];
        JetT theta=x[1];
        for(int i=0;i<cloud_->size();++i){
            pcl::PointXYZ pt=cloud_->points[i];
            JetT cx= JetT(pt.x) * ceres::cos(theta) - JetT(pt.y) * ceres::sin(theta) + tx + 1.5;
            cubic.Evaluate(cx*100.,&residual[i]);
            residual[i]=JetT(1.0)-residual[i];
        }
        return true;
    }
};

Car_pose_detector::Car_pose_detector(){
    float width = 1.8;
    float sigma=25.;
    float solution = 0.01;
    for (int i=-130;i<130;++i) {
        float x=i*solution;
        double y = 1.0 / (1.0 + ceres::exp(-sigma * (x+width/2.))) -
                1.0 / (1.0 + ceres::exp(-sigma * (x-width/2.)));
        line_.emplace_back(y);
//            printf("---Debug %s %d : [%d %f %f]. \n", __func__, __LINE__, i, x, y);
    }
}
// 检测底盘z方向值，去中心，使用mat加速
bool Car_pose_detector::detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr,
                                        pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud, float &x,
                                        float &theta) {
    //离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_ptr);
    sor.setMeanK(50);            //K近邻搜索点个数
    sor.setStddevMulThresh(0.7); //标准差倍数
    sor.setNegative(false);      //保留未滤波点（内点）
    sor.filter(*cloud_ptr); //保存滤波结果到cloud_filter

    double t_vars[2] = {0, 0};
    // 去中心化

    // 构建最小二乘问题
    ceres::Problem problem;
    // tp_trans_mat_cost->debug_img(t_cloud, m_model);

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

//    problem.SetParameterLowerBound(t_vars, 0, -0.1);
//    problem.SetParameterUpperBound(t_vars, 0, 0.1);
    problem.SetParameterLowerBound(t_vars, 1, (-20) * M_PI / 180.0f);
    problem.SetParameterUpperBound(t_vars, 1, (20) * M_PI / 180.0f);

    // 配置求解器
    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;

    x = t_vars[0];
    theta = t_vars[1] * 180 / M_PI;

    for (auto &point: cloud_ptr->points) {
        pcl::PointXYZ result;
        result.x = - point.y * std::sin(t_vars[1]) + point.x * std::cos(t_vars[1]) + t_vars[0];
        result.y = point.y * std::cos(t_vars[1]) + point.x * std::sin(t_vars[1]);
        result.z = point.z;
        out_cloud->emplace_back(result);
    }

    return true;
}