/*
 * @Description: match scan with grid map
 * @Author: yct
 * @Date: 2020-04-03 11:50:01
 * @LastEditTime: 2020-05-17 23:05:08
 * @LastEditors: yct
 */
#ifndef SCAN_MATCH_HH
#define SCAN_MATCH_HH

#include <algorithm>
#include <string>
#include <iostream>
#include <fstream>
#include <atomic>
#include <mutex>
#include <cmath>
#include <chrono>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"

class prob_array
{
public:
    prob_array():rows(0),cols(0){
        m_probability_vector.clear();
    }
    void operator=(prob_array& other)
    {
        this->rows = other.rows;
        this->cols = other.cols;
        this->m_probability_vector.clear();
        for (size_t i = 0; i < other.m_probability_vector.size(); i++)
        {
            this->m_probability_vector.push_back(other.m_probability_vector[i]);
        }
    }
    // 列数
    int rows;
    // 行数
    int cols;
    // 像素值数组，用于计算加速
    std::vector<double> m_probability_vector;
};

class Scan_match
{
public:

    Scan_match(std::string filename);
    ~Scan_match();
    bool read_map(std::string conf_filename);

    bool Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat global_map);
    bool Solve();

private:
    void scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
    void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg);

private:
    // ros句柄
    ros::NodeHandle m_nh;
    ros::Subscriber m_scan_subscriber;
    ros::Subscriber m_initial_pose_subscriber;
    ros::Publisher m_cloud_publisher;
    tf::TransformBroadcaster tf_broadcaster_;
    std::atomic_bool m_initial_pose_updated;
    tf::Pose m_initial_pose;
    std::mutex m_mutex;

    // cv全局地图
    cv::Mat m_global_map;
    // 图像分辨率
    double m_resolution;
    // 全局地图像素值数组，用于加速
    prob_array m_probability_array;
    // 当前待匹配点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
    // 待求变换
    Eigen::Vector3d m_trans;
    // 初始化状态
    std::atomic_bool m_initialized;
    // 地图路径
    std::string m_map_path;
    // 地图中心点
    Eigen::Vector3d m_map_origin;
};

class BicubicInterpolationCost
{
public:
    BicubicInterpolationCost(const prob_array& map, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const double resolution, const Eigen::Vector3d map_origin, const double scale)
    :m_resolution(resolution),
    m_scale(scale),
    m_map_origin(map_origin),
    m_prob_array(map)
    {
        // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
        // m_prob_array = map;
        // m_resolution = resolution;
        // m_scale = scale;
        // m_map_origin = map_origin;
        m_points.reserve(cloud->size());
        for (size_t i = 0; i < cloud->size(); i++)
        {
            m_points.emplace_back(Eigen::Vector2d(cloud->at(i).x, cloud->at(i).y));
        }
        
        // std::cout<<"*************************************************************************"<<std::endl;
        // m_grid.reset(new ceres::Grid2D<double, 1>(
        //     m_grid_values.data(), 0, m_map.rows*3, 0, m_map.cols*3));
        // m_interpolator.reset(new ceres::BiCubicInterpolator<ceres::Grid2D<double, 1> >(*m_grid));

        // generate_map_img();
        // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
        // int duration = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
        // std::cout << "bicubic init time: " << duration << std::endl;
    }

    void generate_map_img()
    {
        cv::Mat interpolate_map = cv::Mat::ones(m_prob_array.rows, m_prob_array.cols, CV_8UC1);
        const GridArrayAdapter adapter(m_prob_array);
        ceres::BiCubicInterpolator<GridArrayAdapter> t_interpolator(adapter);
        // interpolate_map *= 50.0;
        std::cout<<"*************************************************************************"<<std::endl;
        for (size_t i = 0; i < m_prob_array.rows; i++)
        {
            for (size_t j = 0; j < m_prob_array.cols; j++)
            {
                double f;
                t_interpolator.Evaluate(double(i+kPadding), double(j+kPadding), &f);
                // std::cout<<i<<", "<<j<<"--- map value: "<< f <<std::endl;
                interpolate_map.at<uchar>(i, j) = static_cast<uchar>(f*255.0);
            }
        }

        // 将点云映射到图像中
        Eigen::Matrix<double, 2, 1> translation(0.0, 0.0);
        Eigen::Rotation2D<double> rotation(0.0);
        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
        Eigen::Matrix<double, 3, 3> transform;
        transform << rotation_matrix, translation, double(0.), double(0.), double(1.);
        for (size_t i = 0; i < m_points.size(); ++i)
        {
            // Note that this is a 2D point. The third component is a scaling factor.
            const Eigen::Matrix<double, 3, 1> point(
                double(m_points[i].x()),
                double(m_points[i].y()),
                double(1.)
                );
            const Eigen::Matrix<double, 3, 1> world = transform * point;
            int row = (int)(m_prob_array.rows - ( - m_map_origin.y() + world[1] ) / m_resolution + 0.5);
            int col = (int)(( - m_map_origin.x() + world[0] ) / m_resolution + 0.5);
            interpolate_map.at<uchar>(row, col) = static_cast<uchar>(0);
        }
        
        // cv::imwrite("./src/map_scan_match/interpolate.jpg", interpolate_map);
        cv::namedWindow("map", cv::WINDOW_FREERATIO);
        cv::imshow("map", interpolate_map);
        cv::waitKey();
    }

    template <typename T>
    bool operator()(const T *const pose, T *residual) const
    {
        Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
        // Eigen::Matrix<T, 2, 1> translation(pose[0] - m_map_origin[0], pose[1] - m_map_origin[1]);
        Eigen::Rotation2D<T> rotation(pose[2]);
        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
        Eigen::Matrix<T, 3, 3> transform;
        transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
        
        const GridArrayAdapter adapter(m_prob_array);
        ceres::BiCubicInterpolator<GridArrayAdapter> t_interpolator(adapter);
        // const MapLimits &limits = grid_.limits();
        // std::cout<<"****************************"<<std::endl;
        // std::cout<<"+++++++ (row, col): "<<m_map.rows<<" "<<m_map.cols<<std::endl;
        // char res[2048];
        // memset(res, 0, 2048);
        // sprintf(res, "residual values:\n");
        for (size_t i = 0; i < m_points.size(); ++i)
        {
            // Note that this is a 2D point. The third component is a scaling factor.
            const Eigen::Matrix<T, 3, 1> point(
                T(m_points[i].x()),
                T(m_points[i].y()),
                T(1.)
                );    
            const Eigen::Matrix<T, 3, 1> world = transform * point;
            t_interpolator.Evaluate(
                (T)m_prob_array.rows - ( - m_map_origin.y() + world[1] ) / m_resolution + 0.5 + T(kPadding),
                ( - m_map_origin.x() + world[0] ) / m_resolution + 0.5 + T(kPadding),
                &residual[i]);
            // t_interpolator.Evaluate(
            //     (T(m_map.rows/2.0) - world[0] / m_resolution) - 0.5 + T(kPadding),
            //     (T(m_map.cols/2.0) - world[1] / m_resolution) - 0.5 + T(kPadding),
            //     &residual[i]);
            residual[i] *= m_scale;
            // if (i % 10 == 0)
            // {
            //     sprintf(res + strlen(res), "\n");
            // }
            // sprintf(res + strlen(res), "%.3f ", residual[i]);
            // std::cout<<i<<"th point: "<<(-world[1]) / m_resolution - 0.5 + T(m_map.rows) - 60.0<<" "<<(width+world[0]) / m_resolution - 0.5 + T(m_map.cols)+75.0<<std::endl;
            // std::cout<<i<<"th residual: "<<residual[i]<<std::endl;
            // if(i%40==0)
            // {
            //     // std::cout<<"0 point: "<<m_points[i]<<std::endl;
            //     // std::cout<<world[0]<<", "<<world[1]<<", "<< m_resolution << "." <<std::endl;
            //     std::cout<<(T(m_map.rows) - (world[1]) / m_resolution) - 0.5<<",, "<<(world[0]) / m_resolution - 0.5<<",,"<<residual[i]<<std::endl;
            // }
        }
        // std::cout<<res<<std::endl;
        return true;
    }

    class GridArrayAdapter
    {
    public:
        enum { DATA_DIMENSION = 1 };
        explicit GridArrayAdapter(const prob_array& map) : m_grid_map(map) {
        }

        void GetValue(const int row, const int column, double *const value) const
        {
            if (m_grid_map.m_probability_vector.empty() || row < kPadding || row >= NumRows() - kPadding || column < kPadding || column >= NumCols() - kPadding)
            {
                *value = 0.9;
                return;
            }
            *value = m_grid_map.m_probability_vector[(row - kPadding) * m_grid_map.cols + column - kPadding];
            // std::cout<<"rc value"<<*value<<std::endl;
        }

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

        int NumCols() const
        {
            return m_grid_map.cols + 2 * kPadding;
        }

    private:
        const prob_array& m_grid_map;
    };
private:
    double m_scale;
    // 边界
    static constexpr int kPadding = INT_MAX / 4;
    // 概率地图结构体
    const prob_array& m_prob_array;
    // 点坐标数组
    std::vector<Eigen::Vector2d> m_points;
    // 地图原始分辨率
    double m_resolution;
    // 地图原点
    Eigen::Vector3d m_map_origin;
    // // ceres二维地图类
    // std::unique_ptr<ceres::Grid2D<double, 1> > m_grid;
    // // ceres插值器
    // std::unique_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<double, 1> > > m_interpolator;
};

/**
 * @description: 平移代价函数类
 */
class TranslationCost
{
public:
    TranslationCost(Eigen::Vector2d translation, double translation_weight)
    {
        m_translation = translation;
        m_translation_weight = translation_weight;
    }
    ~TranslationCost()
    {
        
    }

    template <typename T>
    bool operator()(const T *const pose, T *residual) const
    {
        residual[0] = m_translation_weight * (pose[0] - m_translation[0]);
        residual[1] = m_translation_weight * (pose[1] - m_translation[1]);
        return true;
    }

private:
    // 原始平移
    Eigen::Vector2d m_translation;
    double m_translation_weight;
};

/**
 * @description: 旋转代价函数类
 */
class RotationCost
{
public:
    RotationCost(double angle, double rotation_weight)
    {
        m_angle = angle;
        m_rotation_weight = rotation_weight;
    }
    ~RotationCost()
    {

    }

    template <typename T>
    bool operator()(const T *const pose, T *residual) const
    {
        residual[0] = m_rotation_weight * (pose[2] - m_angle);
        return true;
    }
private:
    // 原始旋转
    double m_angle;
    double m_rotation_weight;
};


#endif // !SCAN_MATCH_HH