/*
 * @Description: 
 * @Author: yct
 * @Date: 2021-09-16 15:18:34
 * @LastEditTime: 2021-11-17 15:37:25
 * @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));
    }
}

// 代价函数的计算模型
struct Car_pose_cost
{
    Car_pose_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr) : m_cloud_ptr(cloud_ptr) {}

    // 残差的计算
    template <typename T>
    bool operator()(
        const T *const vars, // 模型参数，x y theta w
        T *residual) const
    {
        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
        {
            std::cout << "error occured" << std::endl;
            return false;
        }
        const T norm_scale = T(0.002);
        //residual[0] = T(0);
        // residual[1] = T(0);
        // residual[m_cloud_ptr->size()] = T(0.0);
        // 点云loss
        const T width = T(2.0); //vars[3];
        
        char buf[40960]={0};
        // sprintf(buf, "----");
        for (int i = 0; i < m_cloud_ptr->size(); i++)
        {
            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
            Eigen::Rotation2D<T> rotation(vars[2]);
            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;

            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.2))));
            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.2))));
            residual[i] = left_loss + right_loss + front_loss + back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
            // if(left_loss > T(0.01))
            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
            // sprintf(buf + strlen(buf), "%.4f ", residual[i]);
            // if(i%20==8)
            // {
            //     sprintf(buf + strlen(buf), "\n");
            // }
        }
        // printf(buf);

        // // 参数L2正则化loss
        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
        return true;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
};

// 公式转图片优化
#include <ceres/cubic_interpolation.h>

constexpr float kMinProbability = 0.01f;
constexpr float kMaxProbability = 1.f - kMinProbability;
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
constexpr int kPadding = INT_MAX / 4;
constexpr float resolutionx = 0.01f;
constexpr float resolutiony = 0.01f;
constexpr float min_x = -2.0f;
constexpr float min_y = -3.0f;
constexpr float max_x = 2.0f;
constexpr float max_y = 3.0f;

cv::Mat Car_pose_detector::m_model = Car_pose_detector::create_mat(min_x, max_x, min_y, max_y, resolutionx, resolutiony);

class GridArrayAdapter
{
public:
    enum { DATA_DIMENSION = 1 };

    explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}

    void GetValue(const int row, const int column, double* const value) const {
        if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
            column >= NumCols() - kPadding) {
            *value = kMaxCorrespondenceCost;
        } else {
            *value = static_cast<double>(grid_.at<float>(row - kPadding, column - kPadding));
            // if (row - kPadding > 100 && row - kPadding < 300 && column - kPadding > 50 && column - kPadding < 150)
            //     printf("row:%d, col:%d, val%.3f\n", row-kPadding, column-kPadding, grid_.at<float>(row - kPadding, column - kPadding));
        }
    }

    int NumRows() const {
        return grid_.rows + 2 * kPadding;
    }

    int NumCols() const {
        return grid_.cols + 2 * kPadding;
    }
private:
    const cv::Mat& grid_;
};

// 代价函数的计算模型
struct Trans_mat_cost
{
    Trans_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat &mat) : m_cloud_ptr(cloud_ptr), m_mat(mat) {
        // cv::namedWindow("img", CV_WINDOW_FREERATIO);
        // cv::imshow("img", m_mat);
        // cv::waitKey();
        // debug_img(cloud_ptr, mat);
    }

    void debug_img(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat &mat)
    {
        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
        {
            std::cout << "error occured" << std::endl;
            return;
        }
        cv::Mat img_show = mat.clone();//cv::Mat::zeros(mat.rows * 1, mat.cols * 1, CV_32FC1);
        const GridArrayAdapter adapter(mat);
        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
        
        // 点云loss
        // char buf[150960]={0};
        // sprintf(buf, "----");
        for (int i = 0; i < m_cloud_ptr->size(); i++)
        {
            Eigen::Matrix<double, 2, 1> t_point(double(m_cloud_ptr->points[i].x), double(m_cloud_ptr->points[i].y));
            Eigen::Rotation2D<double> rotation(0);
            Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
            Eigen::Matrix<double, 2, 1> t_trans_point = rotation_matrix * t_point;

            int col_index = int((t_trans_point.x() - min_x) / resolutionx);
            int row_index = int((t_trans_point.y() - min_y) / resolutiony);
            double val=0.0;
            interpolator.Evaluate(row_index +0.5+ kPadding, col_index +0.5+ kPadding, &val);
            cv::circle(img_show, cv::Point(col_index, row_index), 1, cv::Scalar(0.25), 1);
            // sprintf(buf + strlen(buf), "(%.2f,%.2f)---%.3f ", t_trans_point.x(), t_trans_point.y(), val);
            // if(i%15==8)
            // {
            //     sprintf(buf + strlen(buf), "\n");
            // }
        }
        // printf(buf);
        // for (size_t i = -mat.rows; i < mat.rows*2; i++)
        // {
        //     for (size_t j = -mat.cols; j < mat.cols*2; j++)
        //     {
        //         double val;
        //         // adapter.GetValue(i + kPadding, j + kPadding, &val);
        //         interpolator.Evaluate(i+kPadding, j+kPadding, &val);
        //         img_show.at<float>(i, j) = val;
        //     }
        // }
        // printf("r:%d c:%d\n", img_show.rows, img_show.cols);
        cv::namedWindow("img", cv::WINDOW_FREERATIO);
        // cv::imshow("origin", mat);
        cv::imshow("img", img_show);
        cv::waitKey();
    }

    // 残差的计算
    template <typename T>
    bool operator()(
        const T *const vars, // 模型参数，x y theta w
        T *residual) const
    {
        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
        {
            std::cout << "error occured" << std::endl;
            return false;
        }
        const GridArrayAdapter adapter(m_mat);
        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);

        // 点云loss
        Eigen::Rotation2D<T> rotation(vars[2]);
        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
        for (int i = 0; i < m_cloud_ptr->size(); i++)
        {
            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;

            T col_index = (t_trans_point.x() - T(min_x)) / T(resolutionx) + T(0.5+kPadding);
            T row_index = (t_trans_point.y() - T(min_y)) / T(resolutiony) + T(0.5+kPadding);
            interpolator.Evaluate(row_index, col_index, &residual[i]);
        }

        // // 参数L2正则化loss
        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
        return true;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
    cv::Mat m_mat;
};

// 生成优化图像，降低优化耗时
cv::Mat Car_pose_detector::create_mat(double min_x, double max_x, double min_y, double max_y, double resolution_x, double resolution_y)
{
    if (max_x < min_x || max_y < min_y || resolution_x <= 0 || resolution_y <= 0)
    {
        return cv::Mat();
    }
    int cols = (max_x - min_x) / resolution_x + 1;
    int rows = (max_y - min_y) / resolution_y + 1;
    if(rows <=1 || cols <=1)
    {
        return cv::Mat();
    }
    cv::Mat t_mat(rows, cols, CV_32FC1);

    for (size_t i = 0; i < rows; i++)
    {
        for (size_t j = 0; j < cols; j++)
        {
            double x = j * resolution_x + min_x;
            double y = i * resolution_y + min_y;
            double left_value = 1.0 / (1.0 + exp(30 * (x + 1.0)));
            double right_value = 1.0 / (1.0 + exp(30 * (-x + 1.0)));
            double front_value = 1.0 / (1.0 + exp(15 * (y + 2.2)));
            double back_value = 1.0 / (1.0 + exp(15 * (-y + 2.2)));
            t_mat.at<float>(i, j) = float((left_value + right_value + front_value + back_value) / 1.0f);
        }
    }
    // std::cout << "r,c " << t_mat.rows << ", " << t_mat.cols << std::endl;
    // cv::imshow("img", t_mat);
    // cv::waitKey();

    return t_mat;
}

// 检测底盘z方向值，去中心，使用mat加速
bool Car_pose_detector::detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, 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_center.x += t_cloud->points[i].x;
        t_center.y += t_cloud->points[i].y;
    }
    t_center.x /= t_cloud->size();
    t_center.y /= 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;
    }
    // // write_pointcloud(t_cloud, std::to_string(count)+"_ori_");

    // 构建最小二乘问题
    ceres::Problem problem;
    Trans_mat_cost *tp_trans_mat_cost = new Trans_mat_cost(t_cloud, m_model);
    // tp_trans_mat_cost->debug_img(t_cloud, m_model);
    problem.AddResidualBlock( // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
        new ceres::AutoDiffCostFunction<Trans_mat_cost, ceres::DYNAMIC, 3>(
            tp_trans_mat_cost, t_cloud->size()),
        new ceres::HuberLoss(1.0), // 核函数，这里不使用，为空
        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] + t_center.x;
    y = t_vars[1] + t_center.y;
    theta = t_vars[2];
    // printf("x:%.3f y:%.3f th:%.3f\n", x, y, theta);
    if (fabs(theta) > 10 * M_PI / 180.0)
    {
        //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
        return false;
    }
    return true;
}

// 检测底盘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()),
        new ceres::HuberLoss(1.0), // 核函数，这里不使用，为空
        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] + t_center.x;
    y = t_vars[1] + t_center.y;
    theta = t_vars[2];
    // printf("x:%.3f y:%.3f th:%.3f\n", x, y, theta);
    if (fabs(theta) > 10 * 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);
        }
    }
    // std::cout << "estimated x,y,theta = " << x << ", " << y << ", " << theta*180.0/M_PI << std::endl;
    // std::cout << "----------------------------------" << std::endl;
    return true;
}