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

#include "detect_ceres3d.h"
#include "interpolated_grid.hpp"
#include <pcl/common/transforms.h>

class CostFunctor3d
{
 private:
    const InterpolatedProbabilityGrid m_interpolated_grid;
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //左前点云

 public:
    CostFunctor3d(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                  const HybridGrid& interpolated_grid)
                  :m_interpolated_grid(interpolated_grid),m_cloud(cloud)
    {
    }

    template<typename T>
    bool operator()(const T *const variable, T *residual) const
    {
        T cx = variable[0];
        T cy = variable[1];
        T theta = variable[2];

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

        //
        int right_rear_num = m_cloud->size();
        for (int i = 0; i < m_cloud->size(); ++i)
        {
            //先平移后旋转
            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x)+cx),
                                               (T(m_cloud->points[i].y)+cy));
            //绕原点旋转
            const Eigen::Matrix<T, 2, 1> rotate_point = rotation_matrix * point;

            residual[i] = T(1.0) -
                    m_interpolated_grid.GetInterpolatedValue(rotate_point[0],rotate_point[1],
                                                                  T(m_cloud->points[i].z));
        }
        return true;
    }

};


detect_ceres3d::detect_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr model_grid_cloud)
{



    float del_x=0.05;
    float del_y=0.03;
    float del_z=0.05;
    Eigen::Matrix3f delta=Eigen::MatrixXf::Identity(3,3);
    delta(0,0)=del_x*del_x;
    delta(1,1)=del_y*del_y;
    delta(2,2)=del_z*del_z;
    Eigen::Matrix3f delta_inv=delta.inverse();


    Eigen::Vector3f d(0,0,0.02);
    Eigen::MatrixXf ce=-0.5*((d).transpose()*delta_inv*(d));
    float cut_p=exp(ce(0,0));

    float resolution=0.02;
    mp_model_grid=new HybridGrid(resolution);


    for(int i=0;i<model_grid_cloud->size();++i)
    {
        pcl::PointXYZ pt=model_grid_cloud->points[i];
        Eigen::Vector3f u(pt.x,pt.y,pt.z);

        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_model_grid->resolution())
        {
            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_model_grid->resolution())
            {
                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_model_grid->resolution())
                {
                    Eigen::Vector3f p(x,y,z);
                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
                    float prob=exp(B(0,0));
                    if(prob>cut_p)
                        prob=cut_p;
                    Eigen::Array3i index=mp_model_grid->GetCellIndex(p);
                    const Eigen::Array3i shifted_index = index+ (mp_model_grid->grid_size() >> 1);
                    if ((shifted_index.cast<unsigned int>() >= mp_model_grid->grid_size()).any())
                    {
                    	std::cout<<index.transpose()<<","<<u.transpose()<<std::endl;
                    }
                    if(mp_model_grid->GetProbability(index)<prob)
                        mp_model_grid->SetProbability(index,prob);

                }
            }
        }

    }

    save_model("./");

}

#include "point_tool.h"
void detect_ceres3d::save_model(std::string dir)
{

    InterpolatedProbabilityGrid inter_grid(*mp_model_grid);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
    for(int x=0;x<mp_model_grid->grid_size();++x)
    {
        for(int y=0;y<mp_model_grid->grid_size();++y)
        {
            for(int z=0;z<mp_model_grid->grid_size();++z)
            {
                Eigen::Array3i index(x-mp_model_grid->grid_size()/2,
                        y-mp_model_grid->grid_size()/2,z-mp_model_grid->grid_size()/2);
                Eigen::Vector3f pt=mp_model_grid->GetCenterOfCell(index);

                float prob=inter_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
                if(prob>0.01)
                {
                    Eigen::Vector3f pt=mp_model_grid->GetCenterOfCell(index);
                    int rgb=int((prob-0.01)*255);
                    pcl::PointXYZRGB point;
                    point.x=pt[0];
                    point.y=pt[1];
                    point.z=pt[2];
                    point.r=rgb;
                    point.g=0;
                    point.b=255-rgb;
                    cloud_grid->push_back(point);
                }
            }
        }
    }


    std::cout<<"  save model :"<<dir<<std::endl;
    save_cloud_txt(cloud_grid,dir+"/model.txt");

}

detect_ceres3d::~detect_ceres3d()
{
    delete mp_model_grid;
    mp_model_grid= nullptr;
}



bool detect_ceres3d::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
        Detect_result &detect_result, std::string &error_info)
{


    error_info = "";

    double cx=detect_result.cx;
    double cy=detect_result.cy;
    double init_theta=detect_result.theta;


    double variable[] = {cx, cy, init_theta};
    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    CostFunctor3d *cost_func = new CostFunctor3d(cloud,*mp_model_grid);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction* cost_function =new
            ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 3>(
            cost_func,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.logging_type = ceres::LoggingType::SILENT;
    options.max_num_iterations=500;
    options.num_threads=2;
    options.minimizer_progress_to_stdout = false;//输出到cout
    ceres::Solver::Summary summary;//优化信息
    ceres::Solve(options, &problem, &summary);//求解!!!

    double loss=summary.final_cost/(cloud->size());

    if(loss>0.05)
    {
        char buf[64]={0};
        sprintf(buf," loss : %.5f  > 0.05",loss);
        error_info=buf;
        return false;
    }

    detect_result.cx=variable[0];
    detect_result.cy=variable[1];
    detect_result.theta=variable[2];

    return true;

}

bool detect_ceres3d::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Detect_result &detect_result,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,std::string &error_info)
{

    if(detect(cloud,detect_result,error_info))
    {

        Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
        pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud0(new pcl::PointCloud<pcl::PointXYZ>());
        transform_2.translation() << detect_result.cx, detect_result.cy, 0;
        pcl::transformPointCloud(*cloud, *transform_cloud0, transform_2);

        transform_2=Eigen::Affine3f::Identity();
        transform_2.rotate(Eigen::AngleAxisf(detect_result.theta, Eigen::Vector3f::UnitZ()));
        pcl::transformPointCloud(*transform_cloud0, *transformed_cloud, transform_2);


        return true;
    }
    std::cout << "error:    " << error_info << std::endl;
    return false;
}