/** ****************************************************************************************
*  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
*  Adapted: Updated to work with refactored pure-C++ CLaserOdometry2D algorithm class.
******************************************************************************************** */

#include "rf2o_laser_odometry/CLaserOdometry2DNode.hpp"
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> // ✅ 确保包含此头文件以支持 tf2::convert

using namespace rf2o;

CLaserOdometry2DNode::CLaserOdometry2DNode(): Node("rf2o_laser_odometry_node")
{
  RCLCPP_INFO(get_logger(), "Initializing RF2O node...");

  this->declare_parameter<std::string>("laser_scan_topic", "/scan");
  this->get_parameter("laser_scan_topic", laser_scan_topic);
  this->declare_parameter<std::string>("odom_topic", "/odom_rf2o");
  this->get_parameter("odom_topic", odom_topic);
  this->declare_parameter<std::string>("base_frame_id", "base_link");
  this->get_parameter("base_frame_id", base_frame_id);
  this->declare_parameter<std::string>("odom_frame_id", "odom");
  this->get_parameter("odom_frame_id", odom_frame_id);
  this->declare_parameter<bool>("publish_tf", true);
  this->get_parameter("publish_tf", publish_tf);
  this->declare_parameter<std::string>("init_pose_from_topic", "/base_pose_ground_truth");
  this->get_parameter("init_pose_from_topic", init_pose_from_topic);
  this->declare_parameter<double>("freq", 10.0);
  this->get_parameter("freq", freq);

  buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
  odom_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
  odom_pub  = this->create_publisher<nav_msgs::msg::Odometry>(odom_topic, 5);
  laser_sub = this->create_subscription<sensor_msgs::msg::LaserScan>(
      laser_scan_topic,
      rclcpp::QoS(rclcpp::KeepLast(1)).best_effort().durability_volatile(),
      std::bind(&CLaserOdometry2DNode::LaserCallBack, this, std::placeholders::_1));
  
  if (init_pose_from_topic != "") {
    initPose_sub = this->create_subscription<nav_msgs::msg::Odometry>(
        init_pose_from_topic,
        rclcpp::QoS(rclcpp::KeepLast(1)).best_effort().durability_volatile(),
        std::bind(&CLaserOdometry2DNode::initPoseCallBack, this, std::placeholders::_1));
    GT_pose_initialized = false;
  } else {
    GT_pose_initialized = true;
    initial_robot_pose.pose.pose.position.x = 0;
    initial_robot_pose.pose.pose.position.y = 0;
    initial_robot_pose.pose.pose.position.z = 0;
    initial_robot_pose.pose.pose.orientation.w = 1; // ✅ 修正：初始四元数 w 应为 1
  }

  rf2o_ref.module_initialized = false;
  rf2o_ref.first_laser_scan   = true;
}

void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::msg::LaserScan::SharedPtr new_scan)
{
  if (GT_pose_initialized) {
    last_scan = *new_scan;
    
    // ✅ 关键修改：将 ROS 时间戳转换为 double 秒
    rf2o_ref.current_scan_time_sec = 
        static_cast<double>(last_scan.header.stamp.sec) + 
        static_cast<double>(last_scan.header.stamp.nanosec) / 1e9;
    
    if (!rf2o_ref.first_laser_scan) {
      for (unsigned int i = 0; i < rf2o_ref.width; i++)
        rf2o_ref.range_wf(i) = new_scan->ranges[i];
      new_scan_available = true;
    } else {
      setLaserPoseFromTf();
      rf2o_ref.init(last_scan, initial_robot_pose.pose.pose);
      rf2o_ref.first_laser_scan = false;
    }
  }
}

bool CLaserOdometry2DNode::setLaserPoseFromTf()
{  
  bool retrieved = false;  
  geometry_msgs::msg::TransformStamped tf_laser;

  try {
    tf_laser = buffer_->lookupTransform(base_frame_id, last_scan.header.frame_id, tf2::TimePointZero);
    retrieved = true;
  } catch (const tf2::TransformException &ex) {
    RCLCPP_ERROR(get_logger(), "%s", ex.what());
  }

  if (retrieved) {
    tf2::Transform transform;
    tf2::convert(tf_laser.transform, transform);
    const tf2::Matrix3x3 &basis = transform.getBasis();
    Eigen::Matrix3d R;
    for(int r = 0; r < 3; r++)
      for(int c = 0; c < 3; c++)
        R(r,c) = basis[r][c];

    Pose3d laser_tf(R);
    const tf2::Vector3 &t = transform.getOrigin();
    laser_tf.translation()(0) = t[0];
    laser_tf.translation()(1) = t[1];
    laser_tf.translation()(2) = t[2];
    rf2o_ref.setLaserPose(laser_tf);
  }
  return retrieved;
}

bool CLaserOdometry2DNode::scan_available() {
  return new_scan_available;
}

void CLaserOdometry2DNode::process()
{
  if (rf2o_ref.is_initialized() && scan_available()) {
    // ✅ 关键修改：传入当前时间戳
    double now_sec = this->now().seconds();
    rf2o_ref.odometryCalculation(last_scan, now_sec);
    publish();
    new_scan_available = false;
  } else {
    RCLCPP_WARN(get_logger(), "Waiting for laser_scans....");
  }
}

void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::msg::Odometry::SharedPtr new_initPose)
{
  if (!GT_pose_initialized) {
    initial_robot_pose = *new_initPose;
    GT_pose_initialized = true;
  }
}

void CLaserOdometry2DNode::publish()
{
  tf2::Quaternion tf_quaternion;
  tf_quaternion.setRPY(0.0, 0.0, rf2o::getYaw(rf2o_ref.robot_pose_.rotation()));
  geometry_msgs::msg::Quaternion quaternion = tf2::toMsg(tf_quaternion);
  
  nav_msgs::msg::Odometry odom;
  
  // ✅ 关键修改：将 double 秒转换回 ROS 时间戳
  odom.header.stamp.sec = static_cast<int32_t>(rf2o_ref.last_odom_time_sec);
  odom.header.stamp.nanosec = static_cast<uint32_t>(
      (rf2o_ref.last_odom_time_sec - odom.header.stamp.sec) * 1e9);
      
  odom.header.frame_id = odom_frame_id;
  odom.pose.pose.position.x = rf2o_ref.robot_pose_.translation()(0);
  odom.pose.pose.position.y = rf2o_ref.robot_pose_.translation()(1);
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = quaternion;
  odom.child_frame_id = base_frame_id;
  odom.twist.twist.linear.x = rf2o_ref.lin_speed;
  odom.twist.twist.linear.y = 0.0;
  odom.twist.twist.angular.z = rf2o_ref.ang_speed;
  
  odom_pub->publish(odom);

  if (publish_tf) {
    geometry_msgs::msg::TransformStamped odom_trans;
    odom_trans.header.stamp = odom.header.stamp; // ✅ 复用已转换的时间戳
    odom_trans.header.frame_id = odom_frame_id;
    odom_trans.child_frame_id = base_frame_id;
    odom_trans.transform.translation.x = rf2o_ref.robot_pose_.translation()(0);
    odom_trans.transform.translation.y = rf2o_ref.robot_pose_.translation()(1);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = quaternion;
    odom_broadcaster->sendTransform(odom_trans);
  }
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto myLaserOdomNode = std::make_shared<rf2o::CLaserOdometry2DNode>();
  rclcpp::Rate rate(myLaserOdomNode->freq);

  while (rclcpp::ok()) { 
    rclcpp::spin_some(myLaserOdomNode);
    myLaserOdomNode->process();
    rate.sleep();
  }
  return 0;
}