#pragma once

#include <algorithm>
#include <array>
#include <atomic>
#include <condition_variable>
#include <mutex>
#include <string>
#include <thread>
#include <vector>

#include <boost/asio.hpp>

#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_component_interface_params.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/float32.hpp"
#include "agv_pro_msgs/srv/set_digital_output.hpp"
#include "agv_pro_msgs/srv/get_digital_input.hpp"
#include "agv_pro_msgs/srv/set_led_color.hpp"
#include "agv_pro_msgs/srv/set_led_mode.hpp"

namespace agv_pro_hardware
{

// ── ESP32 serial protocol constants (firmware >= V1.0.8) ─────────────────────
// Send frame: 0xFE 0xFE 0x0B <cmd_id> <8-byte payload> <CRC16-MSB> <CRC16-LSB>
// Recv frame: 0xFE 0xFE 0x1C <28-byte payload incl cmd_id and CRC>
static constexpr size_t SEND_FRAME_SIZE = 14;
static constexpr size_t RECV_FRAME_SIZE = 31;
static constexpr uint8_t RECV_PAYLOAD_LEN = RECV_FRAME_SIZE - 3;   // 28  (0x1C)

static constexpr uint8_t CMD_POWER_ON      = 0x10;
static constexpr uint8_t CMD_GET_POWER     = 0x12;
static constexpr uint8_t CMD_AUTO_REPORT   = 0x23;
static constexpr uint8_t CMD_VELOCITY_CMD  = 0x21;
static constexpr uint8_t CMD_VELOCITY_RPT  = 0x25;
static constexpr uint8_t CMD_SET_LED_COLOR = 0x34;
static constexpr uint8_t CMD_SET_LED_MODE  = 0x3A;
static constexpr uint8_t CMD_SET_IO_OUT    = 0x40;
static constexpr uint8_t CMD_GET_IO_IN     = 0x41;


class AgvProHardwareInterface : public hardware_interface::SystemInterface
{
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(AgvProHardwareInterface)

  // ── ros2_control lifecycle ──────────────────────────────────────────────────
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareComponentInterfaceParams & params) override;

  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_cleanup(
    const rclcpp_lifecycle::State & previous_state) override;

  // ── Interface export ────────────────────────────────────────────────────────
  std::vector<hardware_interface::StateInterface::ConstSharedPtr>
  on_export_state_interfaces() override;

  std::vector<hardware_interface::CommandInterface::SharedPtr>
  on_export_command_interfaces() override;

  // ── Control loop callbacks ──────────────────────────────────────────────────
  hardware_interface::return_type read(
    const rclcpp::Time & time,
    const rclcpp::Duration & period) override;

  hardware_interface::return_type write(
    const rclcpp::Time & time,
    const rclcpp::Duration & period) override;

private:
  // ── Serial protocol helpers ─────────────────────────────────────────────────
  uint16_t crc16_ibm(const uint8_t * data, size_t length);
  std::vector<uint8_t> build_frame(uint8_t cmd_id, const std::vector<uint8_t> & payload);

  // All send/receive helpers acquire write_mutex_ internally (or the caller must
  // already hold it, indicated by the _locked suffix).
  bool send_frame(const std::vector<uint8_t> & frame);
  std::vector<uint8_t> send_and_receive(
    const std::vector<uint8_t> & cmd_frame,
    const std::vector<uint8_t> & expected_header,
    size_t payload_size,
    double timeout_sec);

  bool power_on();
  void set_auto_report(bool enable);
  void clear_serial_buffer(int fd);
  void disable_dtr_rts(int fd);

  // ── Background serial reader ────────────────────────────────────────────────
  void reader_thread_func();
  void process_byte(uint8_t byte);
  void on_complete_frame(const std::vector<uint8_t> & frame);

  enum class ParseState { SEEK_FE1, SEEK_FE2, SEEK_LEN, READ_PAYLOAD };
  ParseState parse_state_{ParseState::SEEK_FE1};
  std::vector<uint8_t> frame_payload_;

  // ── Service handlers ────────────────────────────────────────────────────────
  void pause_reader_for_service();
  void resume_reader_after_service();

  void handle_set_digital_output(
    const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
    std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response);
  void handle_get_digital_input(
    const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
    std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
  void handle_set_led_color(
    const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
    std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
  void handle_set_led_mode(
    const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
    std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);

  // ── Configuration (from <hardware><param> in URDF) ─────────────────────────
  std::string port_name_;
  double wheel_radius_;   // metres
  double lx_;             // half wheelbase: robot centre → axle (x-axis), metres
  double ly_;             // half track:     robot centre → wheel (y-axis), metres

  // Index of each physical wheel position in the info_.joints array.
  // Resolved in on_init() by matching joint names from hardware_parameters.
  size_t fl_idx_{0};      // physical Front-Left
  size_t fr_idx_{1};      // physical Front-Right
  size_t rl_idx_{2};      // physical Rear-Left
  size_t rr_idx_{3};      // physical Rear-Right

  // ── Serial port ─────────────────────────────────────────────────────────────
  boost::asio::io_service io_;
  std::unique_ptr<boost::asio::serial_port> serial_port_;

  // Protects all writes to the serial port (shared between write(), service
  // handlers, and power_on/set_auto_report calls from the lifecycle methods).
  std::mutex write_mutex_;

  // ── Reader thread ────────────────────────────────────────────────────────────
  std::thread reader_thread_;
  std::atomic<bool> reader_running_{false};

  // Pause/resume: service handlers request a pause so they can own the serial
  // port for a request-response exchange.
  std::atomic<bool> reader_pause_req_{false};
  std::mutex reader_pause_mutex_;
  std::condition_variable reader_pause_cv_;
  bool reader_is_paused_{false};

  // ── Latest data from auto-report (reader thread → read()) ───────────────────
  std::mutex data_mutex_;
  double latest_vx_{0.0};
  double latest_vy_{0.0};
  double latest_vtheta_{0.0};
  sensor_msgs::msg::Imu latest_imu_;
  float latest_voltage_{0.0f};
  bool new_data_{false};

  // ── Joint state storage — these doubles are the backing store for the
  //    StateInterfaces and CommandInterfaces exported to ros2_control.
  //    Indexed by position in info_.joints (not by FL/FR/RL/RR directly).
  std::array<double, 4> wheel_positions_{0.0, 0.0, 0.0, 0.0};
  std::array<double, 4> wheel_velocities_{0.0, 0.0, 0.0, 0.0};
  std::array<double, 4> wheel_commands_{0.0, 0.0, 0.0, 0.0};

  // ── ROS publishers and services (created in on_configure via get_node()) ─────
  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr         imu_pub_;
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr         voltage_pub_;
  rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_srv_;
  rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr  get_input_srv_;
  rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr      set_led_color_srv_;
  rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr       set_led_mode_srv_;
};

}  // namespace agv_pro_hardware
