#include "origincar_base/origincar_base.h"
#include "origincar_base/command_frame.hpp"
#include "rclcpp/rclcpp.hpp"
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
#include "origincar_msg/msg/data.hpp"
#include "robot_localization/srv/set_pose.hpp"
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <algorithm>
#include <chrono>
#include <cmath>

using std::placeholders::_1;
using namespace std;
void sigintHandler(int sig);
sensor_msgs::msg::Imu Mpu6050;
rclcpp::Node::SharedPtr node_handle = nullptr;
int Init_imu_num = 0;
float gyro_z_sum = 0;
namespace
{
  constexpr float kDefaultGyroZLowPassAlpha = 0.6f;
  constexpr float kStationaryLinearSpeedThreshold = 0.01f;

  float median3(float a, float b, float c)
  {
    if (a > b)
    {
      std::swap(a, b);
    }
    if (b > c)
    {
      std::swap(b, c);
    }
    if (a > b)
    {
      std::swap(a, b);
    }
    return b;
  }

  bool shouldForceZeroYawRate(float vx, float vy)
  {
    return std::fabs(vx) < kStationaryLinearSpeedThreshold &&
           std::fabs(vy) < kStationaryLinearSpeedThreshold;
  }

  std::vector<::origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride)
  {
    std::vector<::origincar_wall::WallPoint> points;
    stride = std::max(1, stride);
    points.reserve(msg.ranges.size() / static_cast<std::size_t>(stride) + 1);
    for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast<std::size_t>(stride))
    {
      const auto distance = static_cast<double>(msg.ranges[index]);
      if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max)
      {
        continue;
      }
      const auto angle = static_cast<double>(msg.angle_min) + static_cast<double>(index) * msg.angle_increment;
      points.push_back({distance * std::cos(angle), distance * std::sin(angle)});
    }
    return points;
  }
}

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  signal(SIGINT, sigintHandler);
  origincar_base Robot_Control;
  Robot_Control.Control();
  rclcpp::shutdown();
  return 0;
}

short origincar_base::IMU_Trans(uint8_t Data_High, uint8_t Data_Low)
{
  short transition_16;
  transition_16 = 0;
  transition_16 |= Data_High << 8;
  transition_16 |= Data_Low;
  return transition_16;
}

float origincar_base::Odom_Trans(uint8_t Data_High, uint8_t Data_Low)
{
  float data_return;
  short transition_16;
  transition_16 = 0;
  transition_16 |= Data_High << 8;
  transition_16 |= Data_Low;
  data_return = (transition_16 / 1000) + (transition_16 % 1000) * 0.001;
  return data_return;
}

void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
{
  origincar_base_core::Command command;
  command.linear_x = akm_ctl->drive.speed;
  command.steering_angle = akm_ctl->drive.steering_angle;
  command.ackermann = true;
  command_gate_->update(command, std::chrono::steady_clock::now());
}

void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
{
  origincar_base_core::Command command;
  command.linear_x = twist_aux->linear.x;
  command.linear_y = twist_aux->linear.y;
  command.angular_z = twist_aux->angular.z;
  command_gate_->update(command, std::chrono::steady_clock::now());
}

void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
{
  (void)sign_switch;
  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  if (sign_switch->data == -1)
  {
    memset(&Robot_Pos, 0, sizeof(Robot_Pos));
    Mpu6050 = sensor_msgs::msg::Imu();
    Robot_Pos.X = 0.54;
    Robot_Pos.Y = 0.2;
    Robot_Pos.Z = 0;
    Robot_Vel.X = 0;
    Robot_Vel.Y = 0;
    Robot_Vel.Z = 0;
    memset(&Robot_Vel, 0, sizeof(Robot_Vel));
    reset_filter();
  }
  else if (sign_switch->data == -2)
  {
    memset(&Robot_Pos, 0, sizeof(Robot_Pos));
    Robot_Pos.X = 2.54;
    Robot_Pos.Y = 2.5;
    Robot_Pos.Z = -1.5708;
    Robot_Vel.X = 0;
    Robot_Vel.Y = 0;
    Robot_Vel.Z = 0;
    memset(&Robot_Vel, 0, sizeof(Robot_Vel));
    reset_filter();
  }
  /*
  else if (sign_switch->data == 6) {
      memset(&Robot_Pos, 0, sizeof(Robot_Pos));
      Robot_Pos.X = 2;
      Robot_Pos.Y = 2;
      memset(&Robot_Vel, 0, sizeof(Robot_Vel));
  }*/
}

void origincar_base::Publish_ImuSensor()
{
  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, Robot_Pos.Z);

  sensor_msgs::msg::Imu Imu_Data_Pub;
  Imu_Data_Pub.header.stamp = rclcpp::Node::now();
  Imu_Data_Pub.header.frame_id = gyro_frame_id;
  Imu_Data_Pub.orientation = tf2::toMsg(q);
  Imu_Data_Pub.orientation_covariance[0] = 1e6;
  Imu_Data_Pub.orientation_covariance[4] = 1e6;
  Imu_Data_Pub.orientation_covariance[8] = 1e-6;
  Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
  Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
  Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
  Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
  Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
  Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
  Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
  Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
  Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;

  imu_publisher->publish(Imu_Data_Pub);
}

void origincar_base::Publish_Odom()
{
  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  tf2::Quaternion q;
  q.setRPY(0, 0, Robot_Pos.Z);
  geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);

  origincar_msg::msg::Data robotpose;
  origincar_msg::msg::Data robotvel;
  nav_msgs::msg::Odometry odom;

  odom.header.stamp = rclcpp::Node::now();
  odom.header.frame_id = odom_frame_id;
  odom.child_frame_id = robot_frame_id;

  odom.pose.pose.position.x = Robot_Pos.X;
  odom.pose.pose.position.y = Robot_Pos.Y;

  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;
  const auto covariance = wall_filter_ ? wall_filter_->covariance() : ::origincar_wall::WallKalmanNoise{odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_};
  odom.pose.covariance[0] = covariance.x;
  odom.pose.covariance[7] = covariance.y;
  odom.pose.covariance[35] = covariance.theta;
  // odom_quat;
  // odom_quat;

  odom.twist.twist.linear.x = Robot_Vel.X;
  odom.twist.twist.linear.y = Robot_Vel.Y;
  odom.twist.twist.angular.z = Robot_Vel.Z;

  robotpose.x = Robot_Pos.X;
  robotpose.y = Robot_Pos.Y;
  robotpose.z = Robot_Pos.Z;

  robotvel.x = Robot_Vel.X;
  robotvel.y = Robot_Vel.Y;
  robotvel.z = Robot_Vel.Z;

  geometry_msgs::msg::TransformStamped t;

  t.header.stamp = rclcpp::Node::now();
  t.header.frame_id = odom_frame_id;
  t.child_frame_id = robot_frame_id;

  t.transform.translation.x = Robot_Pos.X;
  t.transform.translation.y = Robot_Pos.Y;
  t.transform.translation.z = 0.0;

  t.transform.rotation = odom_quat;
  if (publish_tf_)
  {
    tf_broadcaster_->sendTransform(t);
  }
  odom_publisher->publish(odom);
  if (has_pending_odom_timing_frame_)
  {
    scan_odom_timing_logger_.finishFrame(pending_odom_timing_frame_id_);
    has_pending_odom_timing_frame_ = false;
  }
  Print_Timing_Log_If_Due();
  robotpose_publisher->publish(robotpose);
  robotvel_publisher->publish(robotvel);
}

void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
{
  {
    std::lock_guard<std::mutex> lock(wall_scan_mutex_);
    if (has_latest_scan_timing_frame_ && !latest_scan_consumed_)
    {
      scan_odom_timing_logger_.cancelFrame(latest_scan_timing_frame_id_, "replaced_by_new_scan");
      has_latest_scan_timing_frame_ = false;
    }
  }

  const uint64_t timing_frame_id = scan_odom_timing_logger_.startFrame();
  const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_);
  const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_);
  std::lock_guard<std::mutex> lock(wall_scan_mutex_);
  latest_scan_points_ = pose_frame_points;
  latest_scan_timing_frame_id_ = timing_frame_id;
  has_latest_scan_ = true;
  latest_scan_consumed_ = false;
  has_latest_scan_timing_frame_ = true;
}

void origincar_base::Apply_Wall_Update()
{
  if (!wall_filter_)
  {
    return;
  }
  std::vector<::origincar_wall::WallPoint> scan_points;
  uint64_t timing_frame_id = 0;
  bool has_timing_frame = false;
  {
    std::lock_guard<std::mutex> lock(wall_scan_mutex_);
    if (!has_latest_scan_ || latest_scan_consumed_)
    {
      return;
    }
    scan_points = latest_scan_points_;
    timing_frame_id = latest_scan_timing_frame_id_;
    has_timing_frame = has_latest_scan_timing_frame_;
    has_latest_scan_timing_frame_ = false;
    latest_scan_consumed_ = true;
  }
  if (has_timing_frame)
  {
    pending_odom_timing_frame_id_ = timing_frame_id;
    has_pending_odom_timing_frame_ = true;
  }

  const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose());
  if (!result.ok)
  {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
                         "wall scan update skipped: %s", result.message.c_str());
    return;
  }

  ::origincar_wall::WallKalmanNoise noise{0.015, 0.015, 0.025};
  if (result.quality.wall_count < 3 || (result.quality.has_mean_error && result.quality.mean_error > 0.18))
  {
    noise = {0.08, 0.08, 0.12};
  }
  wall_filter_->correct(result.pose, noise);
}

void origincar_base::Print_Timing_Log_If_Due()
{
  if (!scan_odom_timing_to_terminal_)
  {
    return;
  }
  std::string timing_summary;
  if (scan_odom_timing_logger_.makeSummaryLineIfDue(std::chrono::steady_clock::now(), &timing_summary))
  {
    RCLCPP_INFO(this->get_logger(), "%s", timing_summary.c_str());
  }
}

void origincar_base::Publish_Voltage()
{
  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  std_msgs::msg::Float32 voltage_msgs;
  static float Count_Voltage_Pub = 0;

  if (Count_Voltage_Pub++ > 10)
  {
    Count_Voltage_Pub = 0;
    voltage_msgs.data = Power_voltage;
    voltage_publisher->publish(voltage_msgs);
  }
}

void origincar_base::Publish_GyroDebug()
{
  std::lock_guard<std::mutex> lock(sensor_data_mutex_);
  origincar_msg::msg::Data gyro_debug;
  gyro_debug.x = gyro_z_filtered_pre_bias_;
  gyro_debug.y = gyro_z_bias_model_;
  gyro_debug.z = gyro_z_final_for_yaw_;
  gyrodebug_publisher->publish(gyro_debug);
}

unsigned char origincar_base::Check_Sum(unsigned char Count_Number, unsigned char mode)
{
  unsigned char check_sum = 0, k;

  if (mode == 0)
  {
    for (k = 0; k < Count_Number; k++)
    {
      check_sum = check_sum ^ Receive_Data.rx[k];
    }
  }
  else if (mode == 1)
  {
    for (k = 0; k < Count_Number; k++)
    {
      check_sum = check_sum ^ Send_Data.tx[k];
    }
  }

  return check_sum;
}

bool origincar_base::Get_Sensor_Data()
{
  short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
  uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
  std::array<uint8_t, RECEIVE_DATA_SIZE> frame{};
  if (!serial_frame_parser_.popFrame(&frame)) {
    try
    {
      std::string incoming;
      {
        std::lock_guard<std::mutex> serial_lock(serial_mutex_);
        const auto available = Stm32_Serial.available();
        if (available == 0) {
          return false;
        }
        incoming = Stm32_Serial.read(std::min<std::size_t>(available, 256));
      }
      if (incoming.empty()) {
        return false;
      }
      serial_frame_parser_.append(
          reinterpret_cast<const uint8_t *>(incoming.data()), incoming.size());
    }
    catch (const std::exception &e)
    {
      RCLCPP_ERROR_THROTTLE(
          this->get_logger(), *this->get_clock(), 2000,
          "Unable to read STM32 serial data: %s", e.what());
      return false;
    }
    if (!serial_frame_parser_.popFrame(&frame)) {
      return false;
    }
  }
  std::copy(frame.begin(), frame.end(), Receive_Data_Pr);
  for (j = 0; j < 24; j++)
  {
    if (Receive_Data_Pr[j] == FRAME_HEADER)
      Header_Pos = j;
    else if (Receive_Data_Pr[j] == FRAME_TAIL)
      Tail_Pos = j;
  }

  if (Tail_Pos == (Header_Pos + 23))
  {
    memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
  }
  else if (Header_Pos == (1 + Tail_Pos))
  {
    for (j = 0; j < 24; j++)
      Receive_Data.rx[j] = Receive_Data_Pr[(j + Header_Pos) % 24];
  }
  else
  {
    return false;
  }

  std::lock_guard<std::mutex> sensor_lock(sensor_data_mutex_);
  Receive_Data.Frame_Header = Receive_Data.rx[0];
  Receive_Data.Frame_Tail = Receive_Data.rx[23];
  if (Receive_Data.Frame_Header == FRAME_HEADER)
  {
    if (Receive_Data.Frame_Tail == FRAME_TAIL)
    {
      if (Receive_Data.rx[22] == Check_Sum(22, READ_DATA_CHECK) || (Header_Pos == (1 + Tail_Pos)))
      {
        Receive_Data.Flag_Stop = Receive_Data.rx[1];
        Robot_Vel.X = Odom_Trans(Receive_Data.rx[2], Receive_Data.rx[3]);

        Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4], Receive_Data.rx[5]);

        // Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]);

        Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8], Receive_Data.rx[9]);
        Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10], Receive_Data.rx[11]);
        Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12], Receive_Data.rx[13]);
        Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14], Receive_Data.rx[15]);
        Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16], Receive_Data.rx[17]);
        Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18], Receive_Data.rx[19]);

        Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
        Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
        Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;

        Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
        Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
        gyro_z_raw_from_mcu_ = Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;

        if (Init_imu_num < 100)
        {
          Init_imu_num += 1;
          gyro_z_sum += Mpu6050_Data.gyros_z_data;
          gyro_z_after_startup_bias_ = 0.0f;
          gyro_z_after_median_ = 0.0f;
          gyro_z_filtered_pre_bias_ = 0.0f;
          gyro_z_bias_model_ = 0.0f;
          gyro_z_final_for_yaw_ = 0.0f;
          Mpu6050.angular_velocity.z = 0;
        }
        else
        {
          gyro_z_after_startup_bias_ =
              (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num)) * GYROSCOPE_RATIO;
          gyro_z_median_window_[gyro_z_median_index_] = gyro_z_after_startup_bias_;
          gyro_z_median_index_ = (gyro_z_median_index_ + 1) % gyro_z_median_window_.size();
          if (gyro_z_median_count_ < gyro_z_median_window_.size())
          {
            gyro_z_median_count_ += 1;
          }

          float median_gyro_z = gyro_z_after_startup_bias_;
          if (gyro_z_median_count_ == gyro_z_median_window_.size())
          {
            median_gyro_z = median3(
                gyro_z_median_window_[0],
                gyro_z_median_window_[1],
                gyro_z_median_window_[2]);
          }
          gyro_z_after_median_ = median_gyro_z;

          if (!gyro_z_low_pass_initialized_)
          {
            gyro_z_low_pass_ = median_gyro_z;
            gyro_z_low_pass_initialized_ = true;
          }
          else
          {
            gyro_z_low_pass_ =
                gyro_z_low_pass_alpha_ * gyro_z_low_pass_ +
                (1.0f - gyro_z_low_pass_alpha_) * median_gyro_z;
          }
          gyro_z_filtered_pre_bias_ = gyro_z_low_pass_;
          gyro_z_bias_model_ = 0.0f;
          gyro_z_final_for_yaw_ = gyro_z_filtered_pre_bias_;
          if (shouldForceZeroYawRate(Robot_Vel.X, Robot_Vel.Y))
          {
            gyro_z_final_for_yaw_ = 0.0f;
          }
          Mpu6050.angular_velocity.z = gyro_z_final_for_yaw_;
        }
        Robot_Vel.Z = Mpu6050.angular_velocity.z;
        transition_16 = 0;
        transition_16 |= Receive_Data.rx[20] << 8;
        transition_16 |= Receive_Data.rx[21];
        Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001;

        sensor_sequence_.fetch_add(1, std::memory_order_release);
        return true;
      }
    }
  }

  return false;
}

void origincar_base::Control()
{
  sensor_thread_stop_.store(false, std::memory_order_release);
  sensor_thread_ = std::thread(&origincar_base::Sensor_Receive_Loop, this);
  rclcpp::spin(this->get_node_base_interface());
  sensor_thread_stop_.store(true, std::memory_order_release);
  if (sensor_thread_.joinable()) {
    sensor_thread_.join();
  }
}

void origincar_base::Sensor_Receive_Loop()
{
  while (rclcpp::ok() && !sensor_thread_stop_.load(std::memory_order_acquire)) {
    if (!Get_Sensor_Data()) {
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
  }
}

void origincar_base::Control_Timer_Callback()
{
  const auto sequence = sensor_sequence_.load(std::memory_order_acquire);
  if (sequence == 0 || sequence == last_processed_sensor_sequence_) {
    return;
  }

  const auto current_time = rclcpp::Node::now();
  Sampling_Time = (current_time - _Last_Time).seconds();
  if (Sampling_Time <= 0.0f || Sampling_Time > 0.5f) {
    Sampling_Time = static_cast<float>(control_period_ms_) / 1000.0f;
  }
  _Last_Time = current_time;

  Vel_Pos_Data velocity;
  {
    std::lock_guard<std::mutex> lock(sensor_data_mutex_);
    velocity = Robot_Vel;
  }
  if (wall_filter_)
  {
    wall_filter_->predict(
        1.03 * velocity.X, 1.01 * velocity.Y, velocity.Z, Sampling_Time);
    Apply_Wall_Update();
    const auto fused_pose = wall_filter_->pose();
    std::lock_guard<std::mutex> lock(sensor_data_mutex_);
    Robot_Pos.X = static_cast<float>(fused_pose.x);
    Robot_Pos.Y = static_cast<float>(fused_pose.y);
    Robot_Pos.Z = static_cast<float>(fused_pose.theta);
  }

  Publish_ImuSensor();
  Publish_GyroDebug();
  Publish_Voltage();
  Publish_Odom();
  last_processed_sensor_sequence_ = sequence;
}

void origincar_base::Tx_Timer_Callback()
{
  if (!command_gate_) {
    return;
  }
  Send_Command(command_gate_->commandAt(std::chrono::steady_clock::now()));
}

void origincar_base::Send_Command(const origincar_base_core::Command & command)
{
  const auto frame = origincar_base_core::encodeCommandFrame(command);
  try
  {
    std::lock_guard<std::mutex> lock(serial_mutex_);
    if (Stm32_Serial.isOpen()) {
      Stm32_Serial.write(frame.data(), frame.size());
    }
  }
  catch (const std::exception &e)
  {
    RCLCPP_ERROR_THROTTLE(
        this->get_logger(), *this->get_clock(), 2000,
        "Unable to send STM32 command: %s", e.what());
  }
}

origincar_base::origincar_base()
    : rclcpp::Node("origincar_base"),
      scan_odom_timing_logger_(this->get_name())
{
  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
  memset(&Receive_Data, 0, sizeof(Receive_Data));
  memset(&Send_Data, 0, sizeof(Send_Data));
  memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));
  gyro_z_raw_from_mcu_ = 0.0f;
  gyro_z_after_startup_bias_ = 0.0f;
  gyro_z_after_median_ = 0.0f;
  gyro_z_filtered_pre_bias_ = 0.0f;
  gyro_z_bias_model_ = 0.0f;
  gyro_z_final_for_yaw_ = 0.0f;
  gyro_z_median_window_.fill(0.0f);
  gyro_z_median_index_ = 0;
  gyro_z_median_count_ = 0;
  gyro_z_low_pass_ = 0.0f;
  gyro_z_low_pass_initialized_ = false;
  has_latest_scan_ = false;
  latest_scan_consumed_ = true;
  latest_scan_timing_frame_id_ = 0;
  pending_odom_timing_frame_id_ = 0;
  has_latest_scan_timing_frame_ = false;
  has_pending_odom_timing_frame_ = false;

  int serial_baud_rate = 921600;

  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
  this->declare_parameter<int>("serial_baud_rate", 921600);
  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
  this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
  this->declare_parameter<std::string>("odom_frame_id", "odom");
  this->declare_parameter<std::string>("robot_frame_id", "base_footprint");
  this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
  this->declare_parameter<std::string>("combined_odom_topic", "odom_combined");
  this->declare_parameter<std::string>("scan_topic", "/scan");
  this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
  this->declare_parameter<bool>("publish_tf", true);
  this->declare_parameter<bool>("scan_odom_timing_to_terminal", false);
  this->declare_parameter<int>("wall_scan_stride", 2);
  this->declare_parameter<double>("laser_x", 0.0);
  this->declare_parameter<double>("laser_y", 0.0);
  this->declare_parameter<double>("laser_yaw", 0.0);
  this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
  this->declare_parameter<int>("cmd_watchdog_timeout_ms", 150);
  this->declare_parameter<int>("tx_period_ms", 20);
  this->declare_parameter<int>("serial_read_timeout_ms", 20);
  this->declare_parameter<int>("control_period_ms", 50);

  // Odom covariance parameters (tunable via YAML)
  this->declare_parameter<double>("odom_pose_cov_x", 0.01);
  this->declare_parameter<double>("odom_pose_cov_y", 0.01);
  this->declare_parameter<double>("odom_pose_cov_yaw", 0.0225);

  this->get_parameter("serial_baud_rate", serial_baud_rate);
  this->get_parameter("usart_port_name", usart_port_name);
  this->get_parameter("cmd_vel", cmd_vel);
  this->get_parameter("akm_cmd_vel", akm_cmd_vel);
  this->get_parameter("odom_frame_id", odom_frame_id);
  this->get_parameter("robot_frame_id", robot_frame_id);
  this->get_parameter("gyro_frame_id", gyro_frame_id);
  this->get_parameter("combined_odom_topic", combined_odom_topic_);
  this->get_parameter("scan_topic", scan_topic_);
  this->get_parameter("wall_config_path", wall_config_path_);
  this->get_parameter("publish_tf", publish_tf_);
  this->get_parameter("scan_odom_timing_to_terminal", scan_odom_timing_to_terminal_);
  this->get_parameter("wall_scan_stride", wall_scan_stride_);
  laser_pose_ = {
      this->get_parameter("laser_x").as_double(),
      this->get_parameter("laser_y").as_double(),
      this->get_parameter("laser_yaw").as_double(),
  };
  gyro_z_low_pass_alpha_ =
      static_cast<float>(this->get_parameter("gyro_z_low_pass_alpha").as_double());
  if (gyro_z_low_pass_alpha_ < 0.0f)
  {
    gyro_z_low_pass_alpha_ = 0.0f;
  }
  else if (gyro_z_low_pass_alpha_ > 0.99f)
  {
    gyro_z_low_pass_alpha_ = 0.99f;
  }
  this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_);
  this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
  this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
  this->get_parameter("cmd_watchdog_timeout_ms", cmd_watchdog_timeout_ms_);
  this->get_parameter("tx_period_ms", tx_period_ms_);
  this->get_parameter("serial_read_timeout_ms", serial_read_timeout_ms_);
  this->get_parameter("control_period_ms", control_period_ms_);
  cmd_watchdog_timeout_ms_ = std::max(1, cmd_watchdog_timeout_ms_);
  tx_period_ms_ = std::max(1, tx_period_ms_);
  serial_read_timeout_ms_ = std::max(1, serial_read_timeout_ms_);
  control_period_ms_ = std::max(1, control_period_ms_);
  command_gate_ = std::make_unique<origincar_base_core::CommandGate>(
      std::chrono::milliseconds(cmd_watchdog_timeout_ms_));

  wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_);
  const auto initial_pose = ::origincar_wall::Pose2D{
      wall_fit_config_.coordinate_frame.odom_zero_map.x,
      wall_fit_config_.coordinate_frame.odom_zero_map.y,
      0.0,
  };
  wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
  scan_odom_timing_logger_.setPrintToTerminal(scan_odom_timing_to_terminal_);
  Robot_Pos.X = static_cast<float>(initial_pose.x);
  Robot_Pos.Y = static_cast<float>(initial_pose.y);
  Robot_Pos.Z = static_cast<float>(initial_pose.theta);

  odom_publisher = create_publisher<nav_msgs::msg::Odometry>(combined_odom_topic_, 10);

  imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 10);

  voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);

  robotpose_publisher = create_publisher<origincar_msg::msg::Data>("robotpose", 10);

  robotvel_publisher = create_publisher<origincar_msg::msg::Data>("robotvel", 10);

  gyrodebug_publisher = create_publisher<origincar_msg::msg::Data>("gyro_debug", 10);

  pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
      "/set_pose",
      rclcpp::SystemDefaultsQoS().reliable());

  tf_bro = std::make_shared<tf2_ros::TransformBroadcaster>(this);
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
  Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>(
      cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1));
  Akm_Cmd_Vel_Sub = create_subscription<ackermann_msgs::msg::AckermannDriveStamped>(
      akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1));
  Scan_Sub = create_subscription<sensor_msgs::msg::LaserScan>(
      scan_topic_, 10, std::bind(&origincar_base::Scan_Callback, this, _1));

  Sign_Switch_Sub = create_subscription<std_msgs::msg::Int32>(
      "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1));
  try
  {
    Stm32_Serial.setPort(usart_port_name);
    Stm32_Serial.setBaudrate(serial_baud_rate);
    serial::Timeout _time = serial::Timeout::simpleTimeout(serial_read_timeout_ms_);
    Stm32_Serial.setTimeout(_time);
    Stm32_Serial.open();
  }
  catch (serial::IOException &e)
  {
    RCLCPP_ERROR(this->get_logger(), "origincar_base can not open serial port,Please check the serial port cable! ");
  }
  if (Stm32_Serial.isOpen())
  {
    RCLCPP_INFO(this->get_logger(), "origincar_base serial port opened");
  }
  if (scan_odom_timing_logger_.isOpen())
  {
    RCLCPP_INFO(this->get_logger(), "scan_to_odom timing log: %s", scan_odom_timing_logger_.logPath().c_str());
  }
  else
  {
    RCLCPP_WARN(this->get_logger(), "scan_to_odom timing log file could not be opened: %s", scan_odom_timing_logger_.logPath().c_str());
  }
  _Last_Time = rclcpp::Node::now();
  tx_timer = create_wall_timer(
      std::chrono::milliseconds(tx_period_ms_),
      std::bind(&origincar_base::Tx_Timer_Callback, this));
  odom_timer = create_wall_timer(
      std::chrono::milliseconds(control_period_ms_),
      std::bind(&origincar_base::Control_Timer_Callback, this));
}

void sigintHandler(int sig)
{
  (void)sig;
  rclcpp::shutdown();
}

origincar_base::~origincar_base()
{
  sensor_thread_stop_.store(true, std::memory_order_release);
  if (sensor_thread_.joinable()) {
    sensor_thread_.join();
  }
  Send_Command(origincar_base_core::Command{});
  RCLCPP_INFO(this->get_logger(), "Shutting down");
}
