#ifndef SLAM_SCAN_MATCH_H_
#define SLAM_SCAN_MATCH_H_

// C++ 标准库头文件
#include <iostream>
#include <memory>
#include <new>
#include <queue>
#include <thread>
#include <mutex>

// C++ 第三方库头文件
#include <glog/logging.h>

// ROS 相关头文件
#include <ros/ros.h>
#include <ros/time.h>
#include <ros/duration.h>
#include <pcl_ros/point_cloud.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/Time.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <laser_geometry/laser_geometry.h>

// PCL 库头文件
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/approximate_voxel_grid.h>

// fast_gicp 相关头文件
#include <fast_gicp/gicp/fast_gicp.hpp>

// 项目内部头文件
#include "odom_laser/registrations.hpp"
#include "odom_laser/ScanMatchingStatus.h"
#include "odom_laser/ros_utils.hpp"

#undef new

typedef pcl::PointXYZI PointT;

struct MatchPCL
{
  ros::Time time;
  pcl::PointCloud<PointT> cloud;
};

namespace laser_odom_ns
{
  class Laser_Odom
  {
  private:
    static Laser_Odom *laser_odom;

    ros::NodeHandle nh;

    ros::Publisher odom_pub; // 里程计发布
    ros::Publisher path_pub;
    ros::Publisher transformed_cloud_pub;

    sensor_msgs::PointCloud2 cloud2;            // 点云数据类型转换的中间变量
    pcl::PointCloud<PointT>::Ptr cloud;         // 点云数据
    pcl::PointCloud<PointT>::Ptr trans_cloud;   // 转换后的点云数据
    pcl::PointCloud<PointT>::ConstPtr keyframe; // 关键帧
    pcl::Filter<PointT>::Ptr downsample_filter;
    fast_gicp::FastGICP<PointT, PointT>::Ptr registration;

    ros::Time prev_time;
    ros::Time keyframe_stamp;      // 关键帧的时间戳
    Eigen::Matrix4f prev_trans;    // 地图起点到当前帧的仿射变换
    Eigen::Matrix4f keyframe_pose; // 地图起点到当前关键帧的仿射变换
    Eigen::Matrix4f odom;

    double downsample_resolution; 
    double keyframe_delta_trans;  
    double keyframe_delta_angle;
    double keyframe_delta_time;

    laser_geometry::LaserProjection projector;

    std::mutex mutex_;
    std::queue<MatchPCL> clouds_;

    void procThread();
    void point_cloud_callback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg);
    void publish_odometry(const ros::Time &stamp, const std::string &base_frame_id, const Eigen::Matrix4f &pose);
    void publish_keyfrrame(const ros::Time &stamp, const std::string &base_frame_id, const Eigen::Matrix4f &pose); // 发布里程计数据
    pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr &cloud);
    Eigen::Matrix4f matching(const ros::Time &stamp, const pcl::PointCloud<PointT>::ConstPtr &cloud);
    geometry_msgs::TransformStamped matrix2transform(const ros::Time &stamp, const Eigen::Matrix4f &pose, const std::string &frame_id, const std::string &child_frame_id);

  public:
    Laser_Odom();
    ~Laser_Odom();
    void init();
  };
}; // namespace laser_odom_ns

#endif // SLAM_SCAN_MATCH_H_