#include "laser_odom.hpp"
#include "hdl_graph_slam/registrations.hpp"

#include <memory>
#include <iostream>

#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 <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <hdl_graph_slam/ros_utils.hpp>
#include <hdl_graph_slam/registrations.hpp>
#include <hdl_graph_slam/ScanMatchingStatus.h>

#include <laser_geometry/laser_geometry.h>
#include <nav_msgs/Path.h>

#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char* argv[]) {
  // 执行 ros 节点初始化
  ros::init(argc, argv, "laser_odom");

  Laser_Odom laser_odom;
  laser_odom.init();
  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息

  return 0;
}

Laser_Odom::Laser_Odom() {
  laser_odom = this;

  cloud.reset(new pcl::PointCloud<PointT>());
  registration.reset(new fast_gicp::FastGICP<PointT, PointT>());
}

Laser_Odom::~Laser_Odom() {}

void Laser_Odom::init() {
  auto voxelgrid = new pcl::VoxelGrid<PointT>();
  voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
  downsample_filter.reset(voxelgrid);

  // 二维数据里程计求解参数
  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
  // registration->setNumThreads(5);     // 求解的核心数
  // registration->setTransformationEpsilon(1e-5);   // 配准算法的收敛容差，即当连续迭代的位姿变换小于该值时，算法将认为已经收敛并停止迭代
  // registration->setMaximumIterations(6400);   // 设置配准算法的最大迭代次数
  // registration->setMaxCorrespondenceDistance(2);  // 设置配准过程中两点之间的最大对应距离。超出此距离的对应关系将被忽略
  // registration->setCorrespondenceRandomness(5); // 指定计算对应点时使用的随机性，通常用于减少计算量或增强配准的鲁棒性。较小的值会限制对应点的选择范围，较大的值可以增加算法对较少数据的适应性

  // 自己数据的参数
  registration->setNumThreads(8);                   // 求解的核心数
  registration->setTransformationEpsilon(1e-5);     // 配准算法的收敛容差，即当连续迭代的位姿变换小于该值时，算法将认为已经收敛并停止迭代
  registration->setMaximumIterations(6400);         // 设置配准算法的最大迭代次数
  registration->setMaxCorrespondenceDistance(0.2);  // 设置配准过程中两点之间的最大对应距离。超出此距离的对应关系将被忽略
  registration->setCorrespondenceRandomness(5);  // 指定计算对应点时使用的随机性，通常用于减少计算量或增强配准的鲁棒性。较小的值会限制对应点的选择范围，较大的值可以增加算法对较少数据的适应性

  // 应该是这里的参数没调好?

  // 三维数据里程计求解参数
  // registration->setNumThreads(5);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
  // registration->setTransformationEpsilon(0.01);
  // registration->setMaximumIterations(64);
  // registration->setMaxCorrespondenceDistance(2.5);
  // registration->setCorrespondenceRandomness(20);

  // registration->setRANSACOutlierRejectionThreshold();
  trans_cloud.reset(new pcl::PointCloud<PointT>());

  odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息

  // keyframe_pub = nh.advertise<pcl::PointCloud<PointT>>("/keyframe", 32);
  // read_until_pub = nh.advertise<std_msgs::Header>("/keyframe", 32);
  aligned_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);    // 对齐后的点云
  keyframe_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/keyframe_points", 32);  // 对齐后的点云
  map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/map_points", 1, true);

  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Laser_Odom::cloud_callback, this);
  ros::Subscriber sub2 = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &Laser_Odom::cloud_callback2, this);
  // ros::Subscriber opt_odom_sub = nh.subscribe<nav_msgs::Odometry>("/opt_odom", 1, &Laser_Odom::opt_odom_callback, this);

  ros::Subscriber opt_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>("opt_odom", 10, &Laser_Odom::opt_odom_callback, this);

  std::thread proc_thread(&Laser_Odom::procThread, this);
  proc_thread.detach();

  ros::spin();
}

void Laser_Odom::procThread() {
  while(ros::ok()) {
    if(clouds_.empty()) {
      usleep(10);
      continue;
    }

    MatchPCL cloud = clouds_.front();
    clouds_.pop();

    Eigen::Matrix4f pose = laser_odom->matching(cloud.time, cloud.cloud.makeShared());

    publish_odometry(cloud.time, "odom", pose);
  }
}

Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
// 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
// 这里就不改了吧,后端不能这么写,因为后端要多线程
// 多线程实现中有一个跟这个很类似的,是 懒汉式（线程不安全） 实现, 它是在给 laser_odom 赋值前先判断是不是等于 nullptr , 如果是才赋值,否则直接返回laser_odom
// 但多线程很有可能同时满足 laser_odom == nullptr, 然后创建多个实例
// 但其实我的cpp文件只跑一次main而已, 多线程问题在这里不会出现呀,
void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
  if(!ros::ok()) {
    return;
  }
  // 消息类型转换

  sensor_msgs::LaserScan scan = *laser_scan_msg;
  for(int i = 0; i < scan.ranges.size(); i++) {
    // if(scan.intensities[i] < 100)
    // {
    //     scan.ranges[i] = 0;
    //     scan.intensities[i] = 0;
    // }
    scan.intensities.push_back(200);
  }

  pcl::PointCloud<PointT> cloud;
  laser_odom->projector.projectLaser(scan, laser_odom->cloud2);
  pcl::fromROSMsg(laser_odom->cloud2, cloud);

  MatchPCL mpc;
  mpc.cloud = cloud;
  mpc.time = laser_scan_msg->header.stamp;
  clouds_.push(mpc);
  return;
}
void Laser_Odom::cloud_callback2(const sensor_msgs::PointCloud2::ConstPtr& laser_scan_msg) {
  if(!ros::ok()) {
    return;
  }
  // 消息类型转换
  // LOG(INFO) << "cloud callback 2 ";
  pcl::PointCloud<PointT> cloud;
  // laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
  pcl::fromROSMsg(*laser_scan_msg, cloud);

  MatchPCL mpc;
  mpc.cloud = cloud;
  mpc.time = laser_scan_msg->header.stamp;
  clouds_.push(mpc);
  return;
}

Eigen::Matrix4f matrix;
void Laser_Odom::opt_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
  // 创建一个 4x4 的 Eigen::Matrix4f 矩阵
  // matrix = Eigen::Matrix4f::Identity();

  // // 提取平移部分
  // matrix(0, 3) = msg->pose.position.x;
  // matrix(1, 3) = msg->pose.position.y;
  // matrix(2, 3) = msg->pose.position.z;

  // // 提取旋转部分（四元数转换为旋转矩阵）
  // Eigen::Quaternionf quaternion(
  //     msg->pose.orientation.w,
  //     msg->pose.orientation.x,
  //     msg->pose.orientation.y,
  //     msg->pose.orientation.z
  // );
  // Eigen::Matrix3f rotation_matrix = quaternion.toRotationMatrix();

  // // 将旋转部分赋值给 4x4 矩阵的左上 3x3 子矩阵
  // matrix.block<3, 3>(0, 0) = rotation_matrix;

  // 输出或使用这个转换后的矩阵
  // ROS_INFO_STREAM("Received 4x4 Transformation Matrix:\n" << matrix);
  // odom = matrix;
}

/**
 * @brief estimate the relative pose between an input cloud and a keyframe cloud
 * @param stamp  the timestamp of the input cloud
 * @param cloud  the input cloud
 * @return the relative pose between the input cloud and the keyframe cloud
 */
Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置

  if(!keyframe) {                            // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
    prev_time = ros::Time();                 //
    prev_trans.setIdentity();                // 设置为单位矩阵，表示：与上一次的位资没有发生变化
    keyframe_pose.setIdentity();             // 关键帧的初始位资
    keyframe_stamp = stamp;                  // 关键帧的时间戳
    keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
    registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧
    return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵

    // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
  }
  auto filtered = downsample(cloud);       // 下采样
  registration->setInputSource(filtered);  // 把点云数据给到 registration

  std::string msf_source;                                       // 记录初始位资估计的来源（imu 或者 odometry)
  Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();  // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
  // 缩放变换和旋转变换称为线性变换（linear transform）   线性变换和平移变换统称为仿射变换（affine transfrom）

  pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
  registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?

  // LOG(INFO) << "matrix = \n" << matrix;
  // LOG(INFO) << "prev_trans * msf_delta.matrix() = \n" << prev_trans * msf_delta.matrix();
  // LOG(INFO) << "keyframe_pose = \n" << keyframe_pose;

  // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新

  if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
    ROS_ERROR("scan matching has not converged!!");
    return keyframe_pose * prev_trans;
  }

  Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
  odom = keyframe_pose * trans;                                    // 算出来 odom

  prev_time = stamp;   // 当前帧的时间戳
  prev_trans = trans;  // 当前帧的仿射变换

  auto keyframe_trans = matrix2transform(stamp, keyframe_pose, "odom", "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
  keyframe_broadcaster.sendTransform(keyframe_trans);                                // 发布关键帧的变换(这个是发送到哪里?  )

  double delta_trans = trans.block<3, 1>(0, 3).norm();                              // 计算 当前帧 与 关键帧 变换的 平移距离
  double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());  // 计算 当前帧 与 关键帧 变换的 旋转角度
  double delta_time = (stamp - keyframe_stamp).toSec();                             // 计算 当前帧 与 关键帧 变换的 时间差

  // LOG(INFO)<<"delta_angle:"<<delta_angle<<" delta_trans:"<<delta_trans;

  // if(delta_trans > 1 || delta_angle > 1 || delta_time > 0.5) {
  if(delta_trans > 1 || delta_angle > 0.2) {
    // 如果有一个超过阈值,更新关键帧
    keyframe = filtered;
    registration->setInputTarget(keyframe);

    keyframe_pose = odom;

    keyframe_stamp = stamp;
    prev_time = stamp;
    prev_trans.setIdentity();

    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
    cloud_msg->header.frame_id = "map";
    pcl::toROSMsg(*keyframe, *cloud_msg);

    map_points_pub.publish(*cloud_msg);

    trans_cloud->points.clear();
  }
  // else
  // {
  pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
  pcl::transformPointCloud(*filtered, *aligned_trans, odom);
  *trans_cloud = *aligned_trans;

  // 发布
  sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
  pcl::toROSMsg(*trans_cloud, *cloud2_msg);
  cloud2_msg->header.frame_id = "map";
  keyframe_points_pub.publish(*cloud2_msg);  // 话题名:
  // }
  //
  // Eigen::Matrix4f odom;
  // std::cout << "The matrix odom is: \n" << odom << std::endl;
  return odom;  // 返回里程计
}

/**
 * @brief downsample a point cloud
 * @param cloud  input cloud
 * @return downsampled point cloud
 */
pcl::PointCloud<PointT>::ConstPtr Laser_Odom::downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
  if(!downsample_filter) {
    return cloud;
  }
  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
  downsample_filter->setInputCloud(cloud);
  downsample_filter->filter(*filtered);
  return filtered;
}

/**
 * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
 * @param stamp            timestamp
 * @param pose             Eigen::Matrix to be converted
 * @param frame_id         tf frame_id
 * @param child_frame_id   tf child frame_id
 * @return converted TransformStamped
 */
geometry_msgs::TransformStamped Laser_Odom::matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
  Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
  quat.normalize();
  geometry_msgs::Quaternion odom_quat;
  odom_quat.w = quat.w();
  odom_quat.x = quat.x();
  odom_quat.y = quat.y();
  odom_quat.z = quat.z();

  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = stamp;
  odom_trans.header.frame_id = frame_id;
  odom_trans.child_frame_id = child_frame_id;

  odom_trans.transform.translation.x = pose(0, 3);
  odom_trans.transform.translation.y = pose(1, 3);
  odom_trans.transform.translation.z = pose(2, 3);
  odom_trans.transform.rotation = odom_quat;

  return odom_trans;
}

/**
 * @brief publish odometry
 * @param stamp  timestamp
 * @param pose   odometry pose to be published
 */
void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
  // publish transform stamped for IMU integration
  // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
  geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
  // trans_pub.publish(odom_trans);

  // // broadcast the transform over tf
  // odom_broadcaster.sendTransform(odom_trans);

  // publish the transform
  nav_msgs::Odometry odom;
  odom.header.stamp = stamp;
  odom.header.frame_id = "odom";
  odom.child_frame_id = "base_link";

  odom.pose.pose.position.x = pose(0, 3);
  odom.pose.pose.position.y = pose(1, 3);
  odom.pose.pose.position.z = pose(2, 3);
  odom.pose.pose.orientation = odom_trans.transform.rotation;

  odom.child_frame_id = base_frame_id;
  odom.twist.twist.linear.x = 0.0;
  odom.twist.twist.linear.y = 0.0;
  odom.twist.twist.angular.z = 0.0;

  odom_pub.publish(odom);
}
