// C++ port of fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py.
//
// This node owns the only hardware-realtime loop in the FORI stack (20Hz RS485
// Modbus RTU to the ZLAC8015D BLDC drivers, or the Arduino serial fallback).
// The control algorithm (accel ramp + IMU yaw-rate PI correction, differential
// kinematics, odometry integration, battery filtering) is kept identical to the
// Python original; only the low-level serial transport is replaced with the
// CRC-validated, short-poll implementation verified on the real robot in
// zlac8015d_motor_test/fori_zltech_motor_test/xbox_motor_control.cpp.

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <deque>
#include <memory>
#include <optional>
#include <string>

#include <fcntl.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <unistd.h>

#include "rclcpp/rclcpp.hpp"

#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "sensor_msgs/msg/battery_state.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "std_msgs/msg/string.hpp"
#include "tf2_ros/transform_broadcaster.h"

#include "fori_serial_bridge_cpp/modbus_serial.hpp"

using namespace std::chrono_literals;

namespace fori_serial_bridge_cpp {

namespace {
constexpr double kPi = 3.14159265358979323846;

double quatToYaw(double w, double x, double y, double z) {
  double siny_cosp = 2.0 * (w * z + x * y);
  double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
  return std::atan2(siny_cosp, cosy_cosp);
}
} // namespace

class ForiSerialBridge : public rclcpp::Node {
public:
  ForiSerialBridge() : rclcpp::Node("fori_serial_bridge") {
    declare_parameter("control_method", std::string("direct_pc"));
    declare_parameter("port", std::string("/dev/ttyUSB0"));
    declare_parameter("baud", 115200);
    declare_parameter("target_linear_speed", 0.5);
    declare_parameter("accel_limit", 0.5);
    declare_parameter("wheel_base", 0.374);
    declare_parameter("wheel_radius", 0.127);
    // FAST_LIO/FAST-LIVO2가 함께 도는 실주행 모드에서는 그쪽이 이미
    // camera_init->aft_mapped->base_link TF 체인을 소유하고 있어서, 여기서 동시에
    // odom->base_link를 쏘면 base_link에 부모가 둘(aft_mapped, odom) 생겨 TF 트리가
    // 충돌한다. LIDAR SLAM 없이 순수 휠 오도메트리로만 굴릴 때(모의주행/벤치 테스트)만
    // true로 켠다.
    declare_parameter("publish_odom_tf", true);

    control_method_ = get_parameter("control_method").as_string();
    port_ = get_parameter("port").as_string();
    baud_ = get_parameter("baud").as_int();
    limit_v_ = get_parameter("target_linear_speed").as_double();
    accel_limit_ = get_parameter("accel_limit").as_double();
    publish_odom_tf_ = get_parameter("publish_odom_tf").as_bool();
    wheel_base_ = get_parameter("wheel_base").as_double();
    wheel_radius_ = get_parameter("wheel_radius").as_double();

    last_time_ = now();

    RCLCPP_INFO(get_logger(), "==========================================");
    RCLCPP_INFO(get_logger(), "     FORI BLDC ZLAC8015D CONTROL BRIDGE (C++)   ");
    RCLCPP_INFO(get_logger(), "==========================================");
    RCLCPP_INFO(get_logger(), "    Control Method: %s", control_method_.c_str());
    RCLCPP_INFO(get_logger(), "    Serial Port: %s at %d baud", port_.c_str(), baud_);
    RCLCPP_INFO(get_logger(), "==========================================");

    if (control_method_ == "direct_pc") {
      if (modbus_port_.openPort(port_, baud_)) {
        RCLCPP_INFO(get_logger(), "BLDC 드라이버 초기화 대기 중...");
        initDirectDrivers();
        RCLCPP_INFO(get_logger(), "BLDC 드라이버 초기화 완료!");
      } else {
        RCLCPP_WARN(get_logger(), "!!! 드라이버 연결 실패: %s !!!", port_.c_str());
        RCLCPP_WARN(get_logger(), "실제 모터 드라이버 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다.");
        is_mock_mode_ = true;
      }
    } else if (control_method_ == "arduino") {
      if (arduino_port_.openPort(port_, baud_)) {
        RCLCPP_INFO(get_logger(), "아두이노 초기화 대기 중 (3초)...");
        std::this_thread::sleep_for(3s);
        RCLCPP_INFO(get_logger(), "아두이노 메가 통신 준비 완료!");
      } else {
        RCLCPP_WARN(get_logger(), "!!! 아두이노 시리얼 연결 실패: %s !!!", port_.c_str());
        RCLCPP_WARN(get_logger(), "아두이노 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다.");
        is_mock_mode_ = true;
      }
    }

    rclcpp::QoS qos_depth1(1);

    cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "/cmd_vel", qos_depth1,
        [this](const geometry_msgs::msg::Twist::SharedPtr msg) { cmdVelCallback(msg); });
    imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
        "/livox/imu", qos_depth1,
        [this](const sensor_msgs::msg::Imu::SharedPtr msg) { imuCallback(msg); });
    initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "/initialpose", 10,
        [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
          initialPoseCallback(msg);
        });
    mode_status_sub_ = create_subscription<std_msgs::msg::String>(
        "/robot_mode_status", 10,
        [this](const std_msgs::msg::String::SharedPtr msg) { modeStatusCallback(msg); });

    odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("/odom_wheels", qos_depth1);
    joint_pub_ = create_publisher<sensor_msgs::msg::JointState>("/joint_states", qos_depth1);
    battery_pub_ = create_publisher<sensor_msgs::msg::BatteryState>("/battery_state", qos_depth1);
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    control_timer_ = create_wall_timer(50ms, [this]() { controlLoop(); });
    battery_timer_ = create_wall_timer(1000ms, [this]() { publishBatteryStatus(); });
  }

  // Safe stop invoked from the shutdown hook (Ctrl+C), mirrors the Python
  // KeyboardInterrupt handler in main().
  void safeStop() {
    if (control_method_ == "direct_pc") {
      disableDirectDrivers();
    } else if (control_method_ == "arduino") {
      uint8_t packet[7] = {0};
      packet[0] = 0xFE;
      packet[5] = 0;
      uint8_t checksum = 0;
      for (int i = 1; i < 6; ++i)
        checksum += packet[i];
      packet[6] = checksum;
      if (arduino_port_.isOpen()) {
        ssize_t written = write(arduino_port_.fd(), packet, sizeof(packet));
        (void)written;
      }
    }
    RCLCPP_INFO(get_logger(), "ROS2 BRIDGE STOPPED");
  }

private:
  // -------------------------------------------------------------------
  // Subscription callbacks
  // -------------------------------------------------------------------
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    target_v_ = std::clamp(msg->linear.x, -limit_v_, limit_v_);
    target_w_ = msg->angular.z;
  }

  void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) {
    current_imu_w_ = msg->angular_velocity.z;
  }

  void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
    odom_x_ = msg->pose.pose.position.x;
    odom_y_ = msg->pose.pose.position.y;
    const auto &q = msg->pose.pose.orientation;
    odom_th_ = quatToYaw(q.w, q.x, q.y, q.z);
    RCLCPP_INFO(get_logger(), "[MOCK] Reset robot pose to: x=%.2f, y=%.2f, yaw=%.2f", odom_x_, odom_y_, odom_th_);
  }

  void modeStatusCallback(const std_msgs::msg::String::SharedPtr msg) {
    is_parked_ = msg->data.find("State: PARKED") != std::string::npos;
  }

  // -------------------------------------------------------------------
  // Direct-PC (RS485 Modbus) driver control, ported 1:1 from the Python
  // ZLAC8015D register sequence but using the CRC-validated SerialPort.
  // -------------------------------------------------------------------
  void initDirectDrivers() {
    modbus_port_.writeReg(1, 0x200D, 3); // Velocity mode
    std::this_thread::sleep_for(10ms);
    modbus_port_.writeReg(2, 0x200D, 3);
    std::this_thread::sleep_for(10ms);
    disableDirectDrivers();
    drivers_initialized_ = true;
  }

  void enableDirectDrivers() {
    if (drivers_enabled_)
      return;
    modbus_port_.writeReg(1, 0x201A, 0);
    modbus_port_.writeReg(1, 0x201B, 0);
    modbus_port_.writeReg(2, 0x201A, 0);
    modbus_port_.writeReg(2, 0x201B, 0);
    std::this_thread::sleep_for(10ms);
    modbus_port_.writeReg(1, 0x200E, 8);
    modbus_port_.writeReg(2, 0x200E, 8);
    drivers_enabled_ = true;
    RCLCPP_INFO(get_logger(), "BLDC Drivers and Brakes Released (ENABLED)");
  }

  void disableDirectDrivers() {
    modbus_port_.writeRegs(1, 0x2088, {0, 0});
    modbus_port_.writeRegs(2, 0x2088, {0, 0});
    modbus_port_.writeReg(1, 0x201A, 1);
    modbus_port_.writeReg(1, 0x201B, 1);
    modbus_port_.writeReg(2, 0x201A, 1);
    modbus_port_.writeReg(2, 0x201B, 1);
    std::this_thread::sleep_for(10ms);
    modbus_port_.writeReg(1, 0x200E, 7);
    modbus_port_.writeReg(2, 0x200E, 7);
    drivers_enabled_ = false;
    RCLCPP_INFO(get_logger(), "BLDC Brakes Locked (DISABLED)");
  }

  // -------------------------------------------------------------------
  // Battery telemetry (1Hz)
  // -------------------------------------------------------------------
  void publishBatteryStatus() {
    sensor_msgs::msg::BatteryState msg;
    msg.header.stamp = now();
    msg.header.frame_id = "base_link";
    msg.design_capacity = 60.0f;
    msg.capacity = 60.0f;
    msg.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_LIFE;

    constexpr double v_min = 22.4;
    constexpr double v_max = 29.2;

    std::optional<double> voltage;

    if (!is_mock_mode_ && control_method_ == "direct_pc") {
      std::vector<uint16_t> v_resp;
      if (modbus_port_.readRegs(1, 0x20A0, 2, v_resp) && !v_resp.empty()) {
        double raw_a0 = std::abs(static_cast<double>(static_cast<int16_t>(v_resp[0])));
        double raw_a1 = v_resp.size() > 1
                             ? std::abs(static_cast<double>(static_cast<int16_t>(v_resp[1])))
                             : raw_a0;

        double raw_v = 0.0;
        if (raw_a0 >= 200 && raw_a0 <= 320) {
          raw_v = raw_a0 * 0.1;
        } else if (raw_a0 >= 2000 && raw_a0 <= 3200) {
          raw_v = raw_a0 * 0.01;
        } else if (raw_a1 >= 200 && raw_a1 <= 320) {
          raw_v = raw_a1 * 0.1;
        } else if (raw_a1 >= 2000 && raw_a1 <= 3200) {
          raw_v = raw_a1 * 0.01;
        } else {
          raw_v = raw_a1 > 1000 ? raw_a1 * 0.01 : (raw_a1 > 100 ? raw_a1 * 0.1 : raw_a1);
        }

        if (raw_v >= 20.0 && raw_v <= 32.0) {
          voltage = raw_v;
          last_valid_voltage_ = voltage;
          RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 3000,
                                "[BATTERY] Driver raw: %.2fV  |  %.1f%%", raw_v,
                                std::clamp((raw_v - v_min) / (v_max - v_min) * 100.0, 0.0, 100.0));
        } else {
          RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
                                "[BATTERY] Raw voltage out of plausible range: raw_a0=%.0f, raw_a1=%.0f, computed=%.2fV",
                                raw_a0, raw_a1, raw_v);
        }
      } else {
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "[BATTERY] Modbus read returned empty response.");
      }
    }

    if (!voltage.has_value()) {
      if (last_valid_voltage_.has_value()) {
        voltage = last_valid_voltage_;
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000,
                              "[BATTERY] Using last valid voltage reading as fallback.");
      } else {
        voltage = (v_min + v_max) / 2.0;
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000,
                              "[BATTERY] No valid reading yet - using nominal fallback: %.1fV", *voltage);
      }
    }

    voltage_history_.push_back(*voltage);
    if (voltage_history_.size() > 10)
      voltage_history_.pop_front();

    std::vector<double> sorted_v(voltage_history_.begin(), voltage_history_.end());
    std::sort(sorted_v.begin(), sorted_v.end());
    double median_v = sorted_v[sorted_v.size() / 2];

    if (!filtered_battery_voltage_.has_value()) {
      filtered_battery_voltage_ = median_v;
    } else {
      filtered_battery_voltage_ = *filtered_battery_voltage_ * 0.95 + median_v * 0.05;
    }

    double final_voltage = *filtered_battery_voltage_;
    double percentage = std::clamp((final_voltage - v_min) / (v_max - v_min), 0.0, 1.0);

    msg.voltage = static_cast<float>(final_voltage);
    msg.percentage = static_cast<float>(percentage);
    msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
    battery_pub_->publish(msg);
  }

  // -------------------------------------------------------------------
  // 20Hz realtime control loop
  // -------------------------------------------------------------------
  void controlLoop() {
    rclcpp::Time current_now = now();
    double dt = (current_now - last_time_).seconds();
    last_time_ = current_now;
    if (dt <= 0.0)
      return;

    // 1. Acceleration ramp
    double dv = target_v_ - current_v_;
    double max_dv = accel_limit_ * dt;
    current_v_ += std::clamp(dv, -max_dv, max_dv);

    double dw = target_w_ - current_w_;
    double max_dw = 2.0 * dt;
    current_w_ += std::clamp(dw, -max_dw, max_dw);

    // 2. Yaw rate PI control
    double yaw_error = current_w_ - current_imu_w_;
    bool is_idle = std::abs(target_v_) < 0.01 && std::abs(target_w_) < 0.01;
    double correction = 0.0;
    if (is_idle) {
      yaw_error_integral_ = 0.0;
      current_v_ = 0.0;
      current_w_ = 0.0;
    } else {
      yaw_error_integral_ += yaw_error * dt;
      yaw_error_integral_ = std::clamp(yaw_error_integral_, -max_integral_, max_integral_);
      correction = kp_yaw_ * yaw_error + ki_yaw_ * yaw_error_integral_;
    }

    // 3. Inverse kinematics -> wheel RPM
    double v_left = current_v_ - (current_w_ + correction) * wheel_base_ / 2.0;
    double v_right = current_v_ + (current_w_ + correction) * wheel_base_ / 2.0;

    int rpm_left = static_cast<int>(std::lround(v_left / (2.0 * kPi * wheel_radius_) * 60.0));
    int rpm_right = static_cast<int>(std::lround(v_right / (2.0 * kPi * wheel_radius_) * 60.0));
    rpm_left = std::clamp(rpm_left, -kMaxRpm, kMaxRpm);
    rpm_right = std::clamp(rpm_right, -kMaxRpm, kMaxRpm);

    // 4. Command the motors
    int feedback_speeds[4] = {0, 0, 0, 0}; // FL, FR, RL, RR

    if (is_mock_mode_) {
      feedback_speeds[0] = rpm_left;
      feedback_speeds[1] = rpm_right;
      feedback_speeds[2] = rpm_left;
      feedback_speeds[3] = rpm_right;
    } else if (control_method_ == "direct_pc") {
      if (is_idle && is_parked_) {
        if (drivers_enabled_)
          disableDirectDrivers();
      } else {
        if (!drivers_enabled_)
          enableDirectDrivers();
        modbus_port_.writeRegs(1, 0x2088,
                                {static_cast<uint16_t>(static_cast<int16_t>(rpm_left)),
                                 static_cast<uint16_t>(static_cast<int16_t>(-rpm_right))});
        modbus_port_.writeRegs(2, 0x2088,
                                {static_cast<uint16_t>(static_cast<int16_t>(rpm_left)),
                                 static_cast<uint16_t>(static_cast<int16_t>(-rpm_right))});
      }

      std::vector<uint16_t> front_resp, rear_resp;
      if (modbus_port_.readRegs(1, 0x20AD, 2, front_resp)) {
        feedback_speeds[0] = static_cast<int16_t>(front_resp[0]);
        feedback_speeds[1] = -static_cast<int16_t>(front_resp[1]);
      }
      if (modbus_port_.readRegs(2, 0x20AD, 2, rear_resp)) {
        feedback_speeds[2] = static_cast<int16_t>(rear_resp[0]);
        feedback_speeds[3] = -static_cast<int16_t>(rear_resp[1]);
      }
    } else if (control_method_ == "arduino") {
      runArduinoIo(rpm_left, rpm_right, is_idle && is_parked_, feedback_speeds);
    }

    // 5. Odometry & telemetry
    processFeedback(feedback_speeds, dt);

    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
                          "[BLDC FEEDBACK] FL:%d FR:%d | RL:%d RR:%d RPM | Target_W:%.2f IMU_W:%.2f",
                          feedback_speeds[0], feedback_speeds[1], feedback_speeds[2], feedback_speeds[3],
                          current_w_, current_imu_w_);
  }

  void runArduinoIo(int rpm_left, int rpm_right, bool disable, int feedback_speeds[4]) {
    if (!arduino_port_.isOpen())
      return;

    uint8_t packet[7];
    packet[0] = 0xFE;
    packet[1] = static_cast<uint8_t>((rpm_left >> 8) & 0xFF);
    packet[2] = static_cast<uint8_t>(rpm_left & 0xFF);
    packet[3] = static_cast<uint8_t>((rpm_right >> 8) & 0xFF);
    packet[4] = static_cast<uint8_t>(rpm_right & 0xFF);
    packet[5] = disable ? 0 : 1;
    uint8_t checksum = 0;
    for (int i = 1; i < 6; ++i)
      checksum += packet[i];
    packet[6] = checksum;

    if (write(arduino_port_.fd(), packet, sizeof(packet)) != static_cast<ssize_t>(sizeof(packet))) {
      RCLCPP_ERROR(get_logger(), "아두이노 송신 오류");
    }

    int bytes_avail = 0;
    if (ioctl(arduino_port_.fd(), FIONREAD, &bytes_avail) == 0 && bytes_avail >= 10) {
      uint8_t head = 0;
      if (read(arduino_port_.fd(), &head, 1) == 1 && head == 0xFD) {
        uint8_t data[9];
        ssize_t got = read(arduino_port_.fd(), data, 9);
        if (got == 9) {
          uint8_t sum = 0;
          for (int i = 0; i < 8; ++i)
            sum += data[i];
          if (sum == data[8]) {
            feedback_speeds[0] = static_cast<int16_t>((data[0] << 8) | data[1]);
            feedback_speeds[1] = static_cast<int16_t>((data[2] << 8) | data[3]);
            feedback_speeds[2] = static_cast<int16_t>((data[4] << 8) | data[5]);
            feedback_speeds[3] = static_cast<int16_t>((data[6] << 8) | data[7]);
          }
        }
      }
    }
  }

  void processFeedback(const int feedback_speeds[4], double dt) {
    double actual_rpm_l = (feedback_speeds[0] + feedback_speeds[2]) / 2.0;
    double actual_rpm_r = (feedback_speeds[1] + feedback_speeds[3]) / 2.0;

    double v_l = actual_rpm_l * 2.0 * kPi * wheel_radius_ / 60.0;
    double v_r = actual_rpm_r * 2.0 * kPi * wheel_radius_ / 60.0;

    double linear_vel = (v_r + v_l) / 2.0;
    double angular_vel = (v_r - v_l) / wheel_base_;

    double delta_th = angular_vel * dt;
    odom_th_ += delta_th;
    odom_x_ += linear_vel * std::cos(odom_th_) * dt;
    odom_y_ += linear_vel * std::sin(odom_th_) * dt;

    double rads_l = v_l / wheel_radius_;
    double rads_r = v_r / wheel_radius_;
    left_wheel_joint_pos_ += rads_l * dt;
    right_wheel_joint_pos_ += rads_r * dt;

    auto stamp = now();

    sensor_msgs::msg::JointState joint_state;
    joint_state.header.stamp = stamp;
    joint_state.name = {"front_left_wheel_joint", "front_right_wheel_joint", "rear_left_wheel_joint",
                         "rear_right_wheel_joint"};
    joint_state.position = {left_wheel_joint_pos_, right_wheel_joint_pos_, left_wheel_joint_pos_,
                             right_wheel_joint_pos_};
    joint_state.velocity = {rads_l, rads_r, rads_l, rads_r};
    joint_pub_->publish(joint_state);

    nav_msgs::msg::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";
    odom.pose.pose.position.x = odom_x_;
    odom.pose.pose.position.y = odom_y_;
    odom.pose.pose.position.z = 0.0;

    double cy = std::cos(odom_th_ * 0.5);
    double sy = std::sin(odom_th_ * 0.5);
    odom.pose.pose.orientation.w = cy;
    odom.pose.pose.orientation.z = sy;

    odom.twist.twist.linear.x = linear_vel;
    odom.twist.twist.angular.z = angular_vel;

    odom_pub_->publish(odom);

    // Nav2's local_costmap needs a live odom->base_link TF (not just the /odom_wheels
    // topic) to transform sensor data into the robot frame. Only broadcast it when no
    // LIDAR SLAM (FAST_LIO/FAST-LIVO2) is providing base_link's TF via aft_mapped —
    // see publish_odom_tf_ declaration above for why both can't be active at once.
    if (publish_odom_tf_) {
      geometry_msgs::msg::TransformStamped tf_msg;
      tf_msg.header.stamp = stamp;
      tf_msg.header.frame_id = "odom";
      tf_msg.child_frame_id = "base_link";
      tf_msg.transform.translation.x = odom.pose.pose.position.x;
      tf_msg.transform.translation.y = odom.pose.pose.position.y;
      tf_msg.transform.translation.z = 0.0;
      tf_msg.transform.rotation = odom.pose.pose.orientation;
      tf_broadcaster_->sendTransform(tf_msg);
    }
  }

  static constexpr int kMaxRpm = 250;

  // Parameters
  std::string control_method_;
  std::string port_;
  int baud_ = 115200;
  double limit_v_ = 0.5;
  double accel_limit_ = 0.5;
  double wheel_base_ = 0.374;
  double wheel_radius_ = 0.127;
  bool publish_odom_tf_ = true;

  // Yaw PI gains
  const double kp_yaw_ = 0.50;
  const double ki_yaw_ = 0.20;
  double yaw_error_integral_ = 0.0;
  const double max_integral_ = 1.0;

  // Robot state
  double target_v_ = 0.0, target_w_ = 0.0;
  double current_v_ = 0.0, current_w_ = 0.0;
  double current_imu_w_ = 0.0;
  rclcpp::Time last_time_;

  double odom_x_ = 0.0, odom_y_ = 0.0, odom_th_ = 0.0;
  double left_wheel_joint_pos_ = 0.0, right_wheel_joint_pos_ = 0.0;

  bool drivers_initialized_ = false;
  bool drivers_enabled_ = false;
  bool is_parked_ = false;
  bool is_mock_mode_ = false;

  SerialPort modbus_port_;
  SerialPort arduino_port_;

  std::optional<double> last_valid_voltage_;
  std::optional<double> filtered_battery_voltage_;
  std::deque<double> voltage_history_;

  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_status_sub_;

  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

  rclcpp::TimerBase::SharedPtr control_timer_;
  rclcpp::TimerBase::SharedPtr battery_timer_;
};

} // namespace fori_serial_bridge_cpp

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<fori_serial_bridge_cpp::ForiSerialBridge>();
  rclcpp::on_shutdown([node]() { node->safeStop(); });
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
