#include "agv_pro_hardware/agv_pro_hardware_interface.hpp"

#include <poll.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <cmath>
#include <cstring>
#include <iomanip>
#include <sstream>
#include <stdexcept>

#include "pluginlib/class_list_macros.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"

PLUGINLIB_EXPORT_CLASS(
  agv_pro_hardware::AgvProHardwareInterface,
  hardware_interface::SystemInterface)

namespace agv_pro_hardware
{

// ── CRC / frame helpers ───────────────────────────────────────────────────────

uint16_t AgvProHardwareInterface::crc16_ibm(const uint8_t * data, size_t length)
{
  uint16_t crc = 0xFFFF;
  for (size_t i = 0; i < length; ++i) {
    crc ^= static_cast<uint16_t>(data[i]);
    for (int j = 0; j < 8; ++j) {
      crc = (crc & 0x0001) ? (crc >> 1) ^ 0xA001 : (crc >> 1);
    }
  }
  return crc;
}

std::vector<uint8_t> AgvProHardwareInterface::build_frame(
  uint8_t cmd_id, const std::vector<uint8_t> & payload)
{
  std::vector<uint8_t> frame(SEND_FRAME_SIZE, 0x00);
  frame[0] = 0xFE;
  frame[1] = 0xFE;
  frame[2] = 0x0B;
  frame[3] = cmd_id;
  for (size_t i = 0; i < payload.size() && i < 8; ++i) {
    frame[4 + i] = payload[i];
  }
  uint16_t crc = crc16_ibm(frame.data(), 12);
  frame[12] = (crc >> 8) & 0xFF;
  frame[13] = crc & 0xFF;
  return frame;
}

bool AgvProHardwareInterface::send_frame(const std::vector<uint8_t> & frame)
{
  std::lock_guard<std::mutex> lock(write_mutex_);
  try {
    boost::asio::write(*serial_port_, boost::asio::buffer(frame));
    return true;
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Serial write error: %s", ex.what());
    return false;
  }
}

// Called while the reader thread is paused (service context).
// write_mutex_ must NOT be held by the caller.
std::vector<uint8_t> AgvProHardwareInterface::send_and_receive(
  const std::vector<uint8_t> & cmd_frame,
  const std::vector<uint8_t> & expected_header,
  size_t payload_size,
  double timeout_sec)
{
  {
    std::lock_guard<std::mutex> lock(write_mutex_);
    try {
      boost::asio::write(*serial_port_, boost::asio::buffer(cmd_frame));
    } catch (const std::exception & ex) {
      RCLCPP_ERROR(get_logger(), "Serial write error in service: %s", ex.what());
      return {};
    }
  }

  // Read response with sliding-window header search.
  int fd = serial_port_->native_handle();
  auto start = std::chrono::steady_clock::now();
  auto timeout = std::chrono::duration<double>(timeout_sec);

  std::vector<uint8_t> window;
  while (std::chrono::steady_clock::now() - start < timeout) {
    struct pollfd pfd = {fd, POLLIN, 0};
    if (::poll(&pfd, 1, 10) <= 0) {
      continue;
    }
    uint8_t byte;
    boost::system::error_code ec;
    if (serial_port_->read_some(boost::asio::buffer(&byte, 1), ec) == 1 && !ec) {
      window.push_back(byte);
      if (window.size() > expected_header.size()) {
        window.erase(window.begin());
      }
      if (window == expected_header) {
        size_t remain = payload_size + 2;  // payload + 2-byte CRC
        std::vector<uint8_t> rest(remain);
        boost::system::error_code ec2;
        boost::asio::read(*serial_port_, boost::asio::buffer(rest), ec2);
        if (ec2) {
          RCLCPP_WARN(get_logger(), "Service response read error: %s", ec2.message().c_str());
          return {};
        }
        std::vector<uint8_t> full = expected_header;
        full.insert(full.end(), rest.begin(), rest.end());
        return full;
      }
    }
  }
  RCLCPP_WARN(get_logger(), "Timeout waiting for service response");
  return {};
}

// ── Serial port setup ─────────────────────────────────────────────────────────

void AgvProHardwareInterface::clear_serial_buffer(int fd)
{
  if (tcflush(fd, TCIOFLUSH) < 0) {
    RCLCPP_WARN(get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
  }
}

void AgvProHardwareInterface::disable_dtr_rts(int fd)
{
  int status;
  if (::ioctl(fd, TIOCMGET, &status) == 0) {
    status &= ~(TIOCM_DTR | TIOCM_RTS);
    if (::ioctl(fd, TIOCMSET, &status) != 0) {
      RCLCPP_WARN(get_logger(), "Failed to clear DTR/RTS: %s", std::strerror(errno));
    }
  }
}

bool AgvProHardwareInterface::power_on()
{
  auto query = build_frame(CMD_GET_POWER, {});
  auto resp = send_and_receive(query, {0xFE, 0xFE, 0x0B, CMD_GET_POWER}, 8, 12.0);

  if (resp.size() != 14) {
    RCLCPP_ERROR(get_logger(), "power_on: no response to GET_POWER query");
    return false;
  }
  uint16_t rx_crc = (resp[12] << 8) | resp[13];
  if (rx_crc != crc16_ibm(resp.data(), 12)) {
    RCLCPP_ERROR(get_logger(), "power_on: CRC mismatch in GET_POWER response");
    return false;
  }

  int state = static_cast<int8_t>(resp[4]);
  RCLCPP_INFO(get_logger(), "GET_POWER status: %d", state);

  if (state == 0) {
    // Motors are off — send POWER_ON
    auto on_cmd = build_frame(CMD_POWER_ON, {});
    send_frame(on_cmd);

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    auto on_resp = send_and_receive(on_cmd, {0xFE, 0xFE, 0x0B, CMD_POWER_ON}, 8, 5.0);
    if (on_resp.size() != 14) {
      RCLCPP_ERROR(get_logger(), "power_on: no response to POWER_ON command");
      return false;
    }
    uint16_t crc2 = (on_resp[12] << 8) | on_resp[13];
    if (crc2 != crc16_ibm(on_resp.data(), 12)) {
      RCLCPP_ERROR(get_logger(), "power_on: CRC mismatch in POWER_ON response");
      return false;
    }
    int ps = static_cast<int8_t>(on_resp[4]);
    if (ps == 1) {
      RCLCPP_INFO(get_logger(), "Motors powered on successfully");
      return true;
    }
    RCLCPP_ERROR(get_logger(), "power_on: firmware error code %d", ps);
    return false;
  }

  RCLCPP_INFO(get_logger(), "Motors already on (status=%d)", state);
  return true;
}

void AgvProHardwareInterface::set_auto_report(bool enable)
{
  auto frame = build_frame(CMD_AUTO_REPORT, {static_cast<uint8_t>(enable ? 1 : 0)});
  send_frame(frame);
}

// ── Background reader thread ──────────────────────────────────────────────────

void AgvProHardwareInterface::reader_thread_func()
{
  int fd = serial_port_->native_handle();

  while (reader_running_) {
    // ── Check for pause request (service handler needs serial) ──────────────
    if (reader_pause_req_) {
      std::unique_lock<std::mutex> lock(reader_pause_mutex_);
      reader_is_paused_ = true;
      reader_pause_cv_.notify_all();
      reader_pause_cv_.wait(lock, [this] { return !reader_pause_req_.load(); });
      reader_is_paused_ = false;
      // Reset state machine: any partial frame in progress is discarded.
      parse_state_ = ParseState::SEEK_FE1;
      frame_payload_.clear();
      continue;
    }

    // ── Poll with 5 ms timeout so we can check pause_req regularly ──────────
    struct pollfd pfd = {fd, POLLIN, 0};
    if (::poll(&pfd, 1, 5) <= 0) {
      continue;
    }

    uint8_t byte;
    boost::system::error_code ec;
    size_t n = serial_port_->read_some(boost::asio::buffer(&byte, 1), ec);
    if (ec || n != 1) {
      if (ec != boost::asio::error::would_block) {
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
          "Serial read error in reader thread: %s", ec.message().c_str());
      }
      continue;
    }

    process_byte(byte);
  }
}

void AgvProHardwareInterface::process_byte(uint8_t byte)
{
  switch (parse_state_) {
    case ParseState::SEEK_FE1:
      if (byte == 0xFE) {
        parse_state_ = ParseState::SEEK_FE2;
      }
      break;

    case ParseState::SEEK_FE2:
      if (byte == 0xFE) {
        parse_state_ = ParseState::SEEK_LEN;
      } else {
        parse_state_ = ParseState::SEEK_FE1;
      }
      break;

    case ParseState::SEEK_LEN:
      if (byte == RECV_PAYLOAD_LEN) {
        frame_payload_.clear();
        frame_payload_.reserve(RECV_PAYLOAD_LEN);
        parse_state_ = ParseState::READ_PAYLOAD;
      } else if (byte == 0xFE) {
        // Could still be a valid header sequence (two consecutive 0xFE bytes)
        parse_state_ = ParseState::SEEK_LEN;
      } else {
        parse_state_ = ParseState::SEEK_FE1;
      }
      break;

    case ParseState::READ_PAYLOAD:
      frame_payload_.push_back(byte);
      if (frame_payload_.size() == RECV_PAYLOAD_LEN) {
        // Reassemble and dispatch
        std::vector<uint8_t> frame = {0xFE, 0xFE, RECV_PAYLOAD_LEN};
        frame.insert(frame.end(), frame_payload_.begin(), frame_payload_.end());
        on_complete_frame(frame);
        parse_state_ = ParseState::SEEK_FE1;
        frame_payload_.clear();
      }
      break;
  }
}

void AgvProHardwareInterface::on_complete_frame(const std::vector<uint8_t> & frame)
{
  if (frame.size() != RECV_FRAME_SIZE) {
    return;
  }

  // Only handle velocity auto-report frames
  if (frame[3] != CMD_VELOCITY_RPT) {
    return;
  }

  // CRC check (over all bytes except the last two)
  uint16_t rx_crc = (static_cast<uint16_t>(frame[RECV_FRAME_SIZE - 2]) << 8) |
                     frame[RECV_FRAME_SIZE - 1];
  if (rx_crc != crc16_ibm(frame.data(), RECV_FRAME_SIZE - 2)) {
    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
      "CRC mismatch in auto-report frame — discarding");
    return;
  }

  // ── Parse velocity ─────────────────────────────────────────────────────────
  double vx     = static_cast<double>(static_cast<int8_t>(frame[4]))  * 0.01;
  double vy     = static_cast<double>(static_cast<int8_t>(frame[5]))  * 0.01;
  double vtheta = static_cast<double>(static_cast<int8_t>(frame[6]))  * 0.01;

  float battery = static_cast<float>(frame[9]) / 10.0f;

  // ── Parse IMU ──────────────────────────────────────────────────────────────
  sensor_msgs::msg::Imu imu;
  imu.header.stamp = get_clock()->now();
  imu.header.frame_id = "imu_link";

  auto i16 = [&frame](size_t hi) -> double {
    return static_cast<double>(
      static_cast<int16_t>((frame[hi] << 8) | frame[hi + 1])) * 0.01;
  };

  imu.linear_acceleration.x = i16(11);
  imu.linear_acceleration.y = i16(13);
  imu.linear_acceleration.z = i16(15);
  imu.angular_velocity.x    = i16(17);
  imu.angular_velocity.y    = i16(19);
  imu.angular_velocity.z    = i16(21);

  double yaw_deg = i16(27);
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, yaw_deg * M_PI / 180.0);
  imu.orientation.x = q.x();
  imu.orientation.y = q.y();
  imu.orientation.z = q.z();
  imu.orientation.w = q.w();

  imu.orientation_covariance[8]      = 1e-6;
  imu.angular_velocity_covariance[8] = 1e-6;

  // ── Store latest data ──────────────────────────────────────────────────────
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    latest_vx_      = vx;
    latest_vy_      = vy;
    latest_vtheta_  = vtheta;
    latest_imu_     = imu;
    latest_voltage_ = battery;
    new_data_       = true;
  }

  // ── Publish (rclcpp publishers are thread-safe for publish()) ─────────────
  if (imu_pub_) {
    imu_pub_->publish(imu);
  }
  if (voltage_pub_) {
    std_msgs::msg::Float32 v;
    v.data = battery;
    voltage_pub_->publish(v);
  }
}

// ── Service pause/resume ──────────────────────────────────────────────────────

void AgvProHardwareInterface::pause_reader_for_service()
{
  reader_pause_req_ = true;
  std::unique_lock<std::mutex> lock(reader_pause_mutex_);
  // Wait up to 50 ms for the reader to pause (it polls every 5 ms)
  reader_pause_cv_.wait_for(
    lock, std::chrono::milliseconds(50),
    [this] { return reader_is_paused_; });
}

void AgvProHardwareInterface::resume_reader_after_service()
{
  reader_pause_req_ = false;
  reader_pause_cv_.notify_all();
}

// ── Service handlers ──────────────────────────────────────────────────────────

void AgvProHardwareInterface::handle_set_digital_output(
  const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> req,
  std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> res)
{
  if (req->pin < 1 || req->pin > 6) {
    res->success = false;
    res->message = "Invalid pin number (must be 1–6)";
    return;
  }
  pause_reader_for_service();
  auto resp = send_and_receive(
    build_frame(CMD_SET_IO_OUT, {
      static_cast<uint8_t>(req->pin),
      static_cast<uint8_t>(req->state)}),
    {0xFE, 0xFE, 0x0B, CMD_SET_IO_OUT}, 8, 5.0);
  resume_reader_after_service();

  if (resp.size() == 14 && resp[4] == 0x01) {
    res->success = true;
    res->message = "Success";
  } else {
    res->success = false;
    res->message = "Hardware reported failure";
  }
}

void AgvProHardwareInterface::handle_get_digital_input(
  const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> req,
  std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> res)
{
  if (req->pin < 1 || req->pin > 6) {
    res->success = false;
    res->message = "Invalid pin number (must be 1–6)";
    return;
  }
  pause_reader_for_service();
  auto resp = send_and_receive(
    build_frame(CMD_GET_IO_IN, {static_cast<uint8_t>(req->pin)}),
    {0xFE, 0xFE, 0x0B, CMD_GET_IO_IN}, 8, 5.0);
  resume_reader_after_service();

  if (resp.size() == 14 && resp[5] != 0xFF) {
    res->state   = static_cast<int32_t>(resp[5]);
    res->success = true;
    res->message = "Success";
  } else {
    res->success = false;
    res->message = "Hardware reported failure";
  }
}

void AgvProHardwareInterface::handle_set_led_color(
  const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> req,
  std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> res)
{
  if (req->position < 0 || req->position > 1 ||
      req->brightness < 0 || req->brightness > 255 ||
      req->r < 0 || req->r > 255 ||
      req->g < 0 || req->g > 255 ||
      req->b < 0 || req->b > 255)
  {
    res->success = false;
    res->message = "Invalid parameter range";
    return;
  }
  pause_reader_for_service();
  auto resp = send_and_receive(
    build_frame(CMD_SET_LED_COLOR, {
      static_cast<uint8_t>(req->position),
      static_cast<uint8_t>(req->brightness),
      static_cast<uint8_t>(req->r),
      static_cast<uint8_t>(req->g),
      static_cast<uint8_t>(req->b)}),
    {0xFE, 0xFE, 0x0B, CMD_SET_LED_COLOR}, 8, 5.0);
  resume_reader_after_service();

  if (resp.size() == 14 && resp[4] == 0x01) {
    res->success = true;
    res->message = "Success";
  } else {
    res->success = false;
    res->message = "Hardware reported failure";
  }
}

void AgvProHardwareInterface::handle_set_led_mode(
  const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> req,
  std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> res)
{
  pause_reader_for_service();
  auto resp = send_and_receive(
    build_frame(0x3A, {static_cast<uint8_t>(req->mode ? 1 : 0)}),
    {0xFE, 0xFE, 0x0B, 0x3A}, 8, 5.0);
  resume_reader_after_service();

  if (resp.size() == 14 && resp[4] == 0x01) {
    res->success = true;
    res->message = "Success";
  } else {
    res->success = false;
    res->message = "Hardware reported failure";
  }
}

// ── Lifecycle methods ─────────────────────────────────────────────────────────

hardware_interface::CallbackReturn AgvProHardwareInterface::on_init(
  const hardware_interface::HardwareComponentInterfaceParams & params)
{
  if (hardware_interface::SystemInterface::on_init(params) !=
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  if (info_.joints.size() != 4) {
    RCLCPP_FATAL(get_logger(),
      "Expected exactly 4 joints in <ros2_control>, got %zu", info_.joints.size());
    return hardware_interface::CallbackReturn::ERROR;
  }

  // ── Parse hardware parameters ──────────────────────────────────────────────
  auto get_param = [&](const std::string & key, const std::string & def) -> std::string {
    auto it = info_.hardware_parameters.find(key);
    return (it != info_.hardware_parameters.end()) ? it->second : def;
  };

  port_name_    = get_param("port_name",    "/dev/agvpro_controller");
  wheel_radius_ = std::stod(get_param("wheel_radius", "0.072"));
  lx_           = std::stod(get_param("lx",           "0.172"));
  ly_           = std::stod(get_param("ly",           "0.180"));

  std::string fl_name = get_param("front_left_joint",  "left_front_wheel_joint");
  std::string fr_name = get_param("front_right_joint", "left_rear_wheel_joint");
  std::string rl_name = get_param("rear_left_joint",   "right_front_wheel_joint");
  std::string rr_name = get_param("rear_right_joint",  "right_rear_wheel_joint");

  // ── Map joint names → array indices ───────────────────────────────────────
  bool found_fl = false, found_fr = false, found_rl = false, found_rr = false;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    const std::string & name = info_.joints[i].name;
    if      (name == fl_name) { fl_idx_ = i; found_fl = true; }
    else if (name == fr_name) { fr_idx_ = i; found_fr = true; }
    else if (name == rl_name) { rl_idx_ = i; found_rl = true; }
    else if (name == rr_name) { rr_idx_ = i; found_rr = true; }
  }

  if (!found_fl || !found_fr || !found_rl || !found_rr) {
    RCLCPP_FATAL(get_logger(),
      "Could not find all four wheel joints.\n"
      "  front_left='%s' (found=%d)\n"
      "  front_right='%s' (found=%d)\n"
      "  rear_left='%s' (found=%d)\n"
      "  rear_right='%s' (found=%d)",
      fl_name.c_str(), found_fl,
      fr_name.c_str(), found_fr,
      rl_name.c_str(), found_rl,
      rr_name.c_str(), found_rr);
    return hardware_interface::CallbackReturn::ERROR;
  }

  RCLCPP_INFO(get_logger(), "Wheel mapping: FL=%s[%zu]  FR=%s[%zu]  RL=%s[%zu]  RR=%s[%zu]",
    fl_name.c_str(), fl_idx_,
    fr_name.c_str(), fr_idx_,
    rl_name.c_str(), rl_idx_,
    rr_name.c_str(), rr_idx_);
  RCLCPP_INFO(get_logger(), "Kinematics: radius=%.4f m  lx=%.4f m  ly=%.4f m",
    wheel_radius_, lx_, ly_);

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn AgvProHardwareInterface::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  auto node = get_node();
  if (!node) {
    RCLCPP_FATAL(get_logger(), "on_configure: get_node() returned null");
    return hardware_interface::CallbackReturn::ERROR;
  }

  imu_pub_     = node->create_publisher<sensor_msgs::msg::Imu>("imu", 20);
  voltage_pub_ = node->create_publisher<std_msgs::msg::Float32>("voltage", 10);

  set_output_srv_ = node->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
    "set_digital_output",
    std::bind(&AgvProHardwareInterface::handle_set_digital_output, this,
      std::placeholders::_1, std::placeholders::_2));

  get_input_srv_ = node->create_service<agv_pro_msgs::srv::GetDigitalInput>(
    "get_digital_input",
    std::bind(&AgvProHardwareInterface::handle_get_digital_input, this,
      std::placeholders::_1, std::placeholders::_2));

  set_led_color_srv_ = node->create_service<agv_pro_msgs::srv::SetLedColor>(
    "set_led_color",
    std::bind(&AgvProHardwareInterface::handle_set_led_color, this,
      std::placeholders::_1, std::placeholders::_2));

  set_led_mode_srv_ = node->create_service<agv_pro_msgs::srv::SetLedMode>(
    "set_led_mode",
    std::bind(&AgvProHardwareInterface::handle_set_led_mode, this,
      std::placeholders::_1, std::placeholders::_2));

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn AgvProHardwareInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // ── Open serial port ───────────────────────────────────────────────────────
  try {
    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
    serial_port_->open(port_name_);
    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
    serial_port_->set_option(boost::asio::serial_port_base::parity(
      boost::asio::serial_port_base::parity::none));
    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(
      boost::asio::serial_port_base::stop_bits::one));
    serial_port_->set_option(boost::asio::serial_port_base::flow_control(
      boost::asio::serial_port_base::flow_control::none));

    int fd = serial_port_->native_handle();
    clear_serial_buffer(fd);
    disable_dtr_rts(fd);

    RCLCPP_INFO(get_logger(), "Serial port %s opened at 1 Mbaud", port_name_.c_str());
  } catch (const std::exception & ex) {
    RCLCPP_FATAL(get_logger(), "Failed to open serial port %s: %s",
      port_name_.c_str(), ex.what());
    return hardware_interface::CallbackReturn::ERROR;
  }

  // ── Allow ESP32 time to reset after DTR/RTS were cleared ──────────────────
  RCLCPP_INFO(get_logger(), "Waiting 3 s for ESP32 to initialise …");
  std::this_thread::sleep_for(std::chrono::seconds(3));

  // ── Power on motors ────────────────────────────────────────────────────────
  if (!power_on()) {
    RCLCPP_FATAL(get_logger(), "Motor power-on failed");
    serial_port_->close();
    return hardware_interface::CallbackReturn::ERROR;
  }

  // ── Start auto-report stream from ESP32 ───────────────────────────────────
  set_auto_report(true);

  // ── Reset state and start reader thread ───────────────────────────────────
  wheel_positions_.fill(0.0);
  wheel_velocities_.fill(0.0);
  wheel_commands_.fill(0.0);
  parse_state_ = ParseState::SEEK_FE1;
  frame_payload_.clear();
  new_data_ = false;

  reader_running_ = true;
  reader_thread_  = std::thread(&AgvProHardwareInterface::reader_thread_func, this);

  RCLCPP_INFO(get_logger(), "AGV Pro hardware interface activated");
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn AgvProHardwareInterface::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // ── Send zero velocity before deactivating ─────────────────────────────────
  auto zero_frame = build_frame(CMD_VELOCITY_CMD, {0, 0, 0, 0, 0, 0, 0, 0});
  send_frame(zero_frame);

  // ── Stop reader thread ─────────────────────────────────────────────────────
  reader_running_   = false;
  reader_pause_req_ = false;   // in case it was paused
  reader_pause_cv_.notify_all();
  if (reader_thread_.joinable()) {
    reader_thread_.join();
  }

  // ── Disable auto-report ────────────────────────────────────────────────────
  set_auto_report(false);

  RCLCPP_INFO(get_logger(), "AGV Pro hardware interface deactivated");
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn AgvProHardwareInterface::on_cleanup(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  if (serial_port_ && serial_port_->is_open()) {
    serial_port_->close();
  }
  serial_port_.reset();
  RCLCPP_INFO(get_logger(), "Serial port closed");
  return hardware_interface::CallbackReturn::SUCCESS;
}

// ── Interface export ──────────────────────────────────────────────────────────

std::vector<hardware_interface::StateInterface::ConstSharedPtr>
AgvProHardwareInterface::on_export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface::ConstSharedPtr> interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    interfaces.push_back(std::make_shared<const hardware_interface::StateInterface>(
      info_.joints[i].name,
      hardware_interface::HW_IF_POSITION,
      &wheel_positions_[i]));
    interfaces.push_back(std::make_shared<const hardware_interface::StateInterface>(
      info_.joints[i].name,
      hardware_interface::HW_IF_VELOCITY,
      &wheel_velocities_[i]));
  }
  return interfaces;
}

std::vector<hardware_interface::CommandInterface::SharedPtr>
AgvProHardwareInterface::on_export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface::SharedPtr> interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    interfaces.push_back(std::make_shared<hardware_interface::CommandInterface>(
      info_.joints[i].name,
      hardware_interface::HW_IF_VELOCITY,
      &wheel_commands_[i]));
  }
  return interfaces;
}

// ── Control loop ──────────────────────────────────────────────────────────────

hardware_interface::return_type AgvProHardwareInterface::read(
  const rclcpp::Time & /*time*/,
  const rclcpp::Duration & period)
{
  double vx, vy, vtheta;
  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    if (!new_data_) {
      return hardware_interface::return_type::OK;
    }
    vx      = latest_vx_;
    vy      = latest_vy_;
    vtheta  = latest_vtheta_;
    new_data_ = false;
  }

  // Mecanum inverse kinematics: body frame → individual wheel angular velocities
  //
  //   ω_FL = (vx - vy - (lx+ly)·ωz) / r
  //   ω_FR = (vx + vy + (lx+ly)·ωz) / r
  //   ω_RL = (vx + vy - (lx+ly)·ωz) / r
  //   ω_RR = (vx - vy + (lx+ly)·ωz) / r
  //
  // The ESP32 reports body-frame velocities (not per-wheel encoders), so these
  // derived wheel velocities are kinematically consistent but not independently
  // measured. Odometry quality is unchanged vs. the original driver.

  const double r    = wheel_radius_;
  const double lxy  = lx_ + ly_;

  wheel_velocities_[fl_idx_] = (vx - vy - lxy * vtheta) / r;
  wheel_velocities_[fr_idx_] = (vx + vy + lxy * vtheta) / r;
  wheel_velocities_[rl_idx_] = (vx + vy - lxy * vtheta) / r;
  wheel_velocities_[rr_idx_] = (vx - vy + lxy * vtheta) / r;

  // Integrate positions
  const double dt = period.seconds();
  for (size_t i = 0; i < 4; ++i) {
    wheel_positions_[i] += wheel_velocities_[i] * dt;
  }

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type AgvProHardwareInterface::write(
  const rclcpp::Time & /*time*/,
  const rclcpp::Duration & /*period*/)
{
  // Mecanum forward kinematics: wheel velocity commands → body frame
  //
  //   vx    = (r/4) · (ω_FL + ω_FR + ω_RL + ω_RR)
  //   vy    = (r/4) · (-ω_FL + ω_FR + ω_RL - ω_RR)
  //   ωz    = r/(4·(lx+ly)) · (-ω_FL + ω_FR - ω_RL + ω_RR)

  const double r   = wheel_radius_;
  const double lxy = lx_ + ly_;

  const double cmd_fl = wheel_commands_[fl_idx_];
  const double cmd_fr = wheel_commands_[fr_idx_];
  const double cmd_rl = wheel_commands_[rl_idx_];
  const double cmd_rr = wheel_commands_[rr_idx_];

  double vx     = (r / 4.0) * (cmd_fl + cmd_fr + cmd_rl + cmd_rr);
  double vy     = (r / 4.0) * (-cmd_fl + cmd_fr + cmd_rl - cmd_rr);
  double vtheta = (r / (4.0 * lxy)) * (-cmd_fl + cmd_fr - cmd_rl + cmd_rr);

  vx     = std::clamp(vx,     -1.5, 1.5);
  vy     = std::clamp(vy,     -1.0, 1.0);
  vtheta = std::clamp(vtheta, -1.0, 1.0);

  const int16_t x_s   = static_cast<int16_t>(vx     * 100.0);
  const int16_t y_s   = static_cast<int16_t>(vy     * 100.0);
  const int16_t rot_s = static_cast<int16_t>(vtheta * 100.0);

  uint8_t buf[SEND_FRAME_SIZE] = {0xFE, 0xFE, 0x0B, CMD_VELOCITY_CMD};
  buf[4]  = (x_s   >> 8) & 0xFF;
  buf[5]  =  x_s         & 0xFF;
  buf[6]  = (y_s   >> 8) & 0xFF;
  buf[7]  =  y_s         & 0xFF;
  buf[8]  = (rot_s >> 8) & 0xFF;
  buf[9]  =  rot_s       & 0xFF;
  buf[10] = 0x00;
  buf[11] = 0x00;
  uint16_t crc = crc16_ibm(buf, 12);
  buf[12] = (crc >> 8) & 0xFF;
  buf[13] =  crc       & 0xFF;

  send_frame(std::vector<uint8_t>(buf, buf + SEND_FRAME_SIZE));

  return hardware_interface::return_type::OK;
}

}  // namespace agv_pro_hardware
