//
// Created by zx on 2020/7/1.
//

#include "detect_wheel_ceres.h"
#include <ceres/cubic_interpolation.h>
#include <pcl/common/transforms.h>


constexpr float kMinProbability = 0.0f;
constexpr float kMaxProbability = 1.f - kMinProbability;
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
constexpr int kPadding = INT_MAX / 4;
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<double >(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_;
};

class CostFunctor {
private:
    cv::Mat  m_map;
    double m_scale;
    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云

public:
    CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front,pcl::PointCloud<pcl::PointXYZ> right_front,
                pcl::PointCloud<pcl::PointXYZ> left_rear,pcl::PointCloud<pcl::PointXYZ> right_rear,
                cv::Mat& map,double scale)
    {
        m_map=map;
        m_scale=scale;
        m_left_front_cloud=left_front;
        m_right_front_cloud=right_front;
        m_left_rear_cloud=left_rear;
        m_right_rear_cloud=right_rear;
    }
    template <typename T>
    bool operator()(const T* const variable, T* residual) const {
        T cx = variable[0];
        T cy = variable[1];
        T theta = variable[2];
        T length = variable[3];
        T width = variable[4];
        T theta_front = variable[5];

        //整车旋转
        Eigen::Rotation2D<T> rotation(theta);
        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();

        //左前偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
        //右前偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
        //左后偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
        //右后偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);

        //前轮旋转
        Eigen::Rotation2D<T> rotation_front(theta_front);
        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();


        const GridArrayAdapter adapter(m_map);
        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);

        double rows = m_map.rows;
        double cols = m_map.cols;
        //左前轮
        int left_front_num = m_left_front_cloud.size();
        for (int i = 0; i < m_left_front_cloud.size(); ++i) {
            const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
                                               (T(m_left_front_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
            //旋转
            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[i]);

        }
        //右前轮
        int right_front_num = m_right_front_cloud.size();
        for (int i = 0; i < m_right_front_cloud.size(); ++i) {
            const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
                                               (T(m_right_front_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
            //旋转
            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[left_front_num + i]);

        }
        //左后轮
        int left_rear_num = m_left_rear_cloud.size();

        for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
            const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
                                               (T(m_left_rear_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;

            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[left_front_num + right_front_num + i]);

        }

        //右后轮

        for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
            const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
                                               (T(m_right_rear_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;

            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[left_front_num + right_front_num + left_rear_num + i]);

        }

        return true;
    }

private:

};

detect_wheel_ceres::detect_wheel_ceres()
{
    /////创建地图
    int cols=800;
    int rows=200;
    int L=std::min(rows,cols);
    cv::Mat map=cv::Mat::ones(L,L,CV_64F);
    //map=map*255;
    cv::Point center(L/2,L/2);


    float K=L*0.08;
    for(int n=0;n<L;n+=2)
    {
        cv::Size size(n+2,n+2);
        cv::Rect rect(center-cv::Point(n/2,n/2),size);
        if(n<K*2)
            cv::rectangle(map,rect,3.0*float(K*2-n)/float(L));
        else
            cv::rectangle(map,rect,float(n-K*2)/float(L));

    }
    cv::resize(map,map,cv::Size(cols,rows));
    cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
}
detect_wheel_ceres::~detect_wheel_ceres(){}



bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
                                double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta)
{
    //清理点云
    m_left_front_cloud.clear();
    m_right_front_cloud.clear();
    m_left_rear_cloud.clear();
    m_right_rear_cloud.clear();
    //重新计算点云,按方位分割
    //第一步,计算整体中心,主轴方向
    pcl::PointCloud<pcl::PointXYZ> cloud_all;
    for(int i=0;i<cloud_vec.size();++i)
    {
        cloud_all+=cloud_vec[i];
    }

    if(cloud_all.size()<20)
        return false;

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(cloud_all, centroid);
    double center_x=centroid[0];
    double center_y=centroid[1];
    //计算外接旋转矩形
    std::vector<cv::Point2f> points_cv;
    for(int i=0;i<cloud_all.size();++i)
    {
        points_cv.push_back(cv::Point2f(cloud_all[i].x,cloud_all[i].y));
    }
    cv::RotatedRect rotate_rect=cv::minAreaRect(points_cv);
    //计算旋转矩形与X轴的夹角
    cv::Point2f vec;
    cv::Point2f vertice[4];
    rotate_rect.points(vertice);

    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
    // 寻找长边，倾角为长边与x轴夹角
    if (len1 > len2)
    {
        vec.x = vertice[0].x - vertice[1].x;
        vec.y = vertice[0].y - vertice[1].y;
    }
    else
    {
        vec.x = vertice[1].x - vertice[2].x;
        vec.y = vertice[1].y - vertice[2].y;
    }
    float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
    //printf("rect theta: %.3f\n",angle_x);
    //第二步, 将没分点云旋转回去,计算点云重心所在象限
    for(int i=0;i<cloud_vec.size();++i)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud=cloud_vec[i];
        Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
        // 平移
        traslation.translation() << -center_x, -center_y, 0.0;
        pcl::PointCloud<pcl::PointXYZ> translate_cloud;
        pcl::transformPointCloud(cloud, translate_cloud, traslation);

        // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
        Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
        rotation.rotate(Eigen::AngleAxisf(-angle_x*M_PI/180.0, Eigen::Vector3f::UnitZ()));

        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
        pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);

        //计算重心
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(transformed_cloud, centroid);
        double x=centroid[0];
        double y=centroid[1];
        //计算象限
        if(x>0&&y>0)
        {
            //第一象限
            m_left_front_cloud=cloud;
        }
        if(x>0 && y<0)
        {
            //第四象限
            m_right_front_cloud=cloud;
        }
        if(x<0 && y>0)
        {
            //第二象限
            m_left_rear_cloud=cloud;
        }
        if(x<0 && y<0)
        {
            //第三象限
            m_right_rear_cloud=cloud;

        }

    }
    cx=center_x;
    cy=center_y;
    theta=-angle_x*M_PI/180.0;
    return Solve(cx,cy,theta,wheel_base,width,front_theta);
}

bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta)
{
    double SCALE=300.0;
    double cx=x;
    double cy=y;
    double init_theta=theta;
    double init_wheel_base=2.7;
    double init_width=1.55;
    double init_theta_front=0*M_PI/180.0;

    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction* cost_function =new
            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
                    new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。


    //第三部分： 配置并运行求解器
    ceres::Solver::Options options;
    options.use_nonmonotonic_steps=false;
    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
    options.max_num_iterations=20;
    options.num_threads=1;
    options.minimizer_progress_to_stdout = false;//输出到cout
    ceres::Solver::Summary summary;//优化信息
    ceres::Solve(options, &problem, &summary);//求解!!!

    //std::cout << summary.BriefReport() << "\n";//输出优化的简要信息

    /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
           x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/

    double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());


    x=variable[0];
    y=variable[1];
    theta=(-variable[2])*180.0/M_PI;
    wheel_base=variable[3];
    width=variable[4];
    front_theta=-(variable[5]*180.0/M_PI);

    if(theta>180.0)
        theta=theta-180.0;
    if(theta<0)
        theta+=180.0;

    //检验
    if(loss>0.03)
        return false;
    if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
    {
        return false;
    }

    width+=0.15;//车宽+10cm
    //printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
    return true;

}