/**
 * @file uart_transmitter_node.cpp
 * @brief UART 串口收发模块测试节点 (CH340)
 *
 * 默认波特率：115200
 * 协议帧格式：
 * | 0xBB | 0x77 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 拨杆 |
 * CRC8 | 帧尾 | | 0xAA  | 0x55  |  2 bytes |  2 bytes |  2 bytes |  2 bytes | 2
 * bytes|1 byte| 1byte| 0xEE |
 */

#include <cstring>
#include <errno.h>
#include <fcntl.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <termios.h>
#include <thread>
#include <unistd.h>
#include <vector>

// 帧定义
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
constexpr uint8_t FRAME_HEADER_2 = 0x77;
constexpr uint8_t FRAME_TAIL = 0xEE;
constexpr int FRAME_LENGTH = 15;

// 默认串口设备
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyCH340";
constexpr int DEFAULT_BAUDRATE = 115200;

class UartTransmitterNode : public rclcpp::Node {
public:
  UartTransmitterNode() : Node("uart_transmitter_node") {
    // 声明参数
    this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
    this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
    this->declare_parameter("send_frequency", 50.0); // Hz

    // 控制参数
    this->declare_parameter("x_move", 0);       // 平动左右 [-660, 660]
    this->declare_parameter("y_move", 0);       // 平动前后 [-660, 660]
    this->declare_parameter("yaw", 0);          // 云台偏航 [-660, 660]
    this->declare_parameter("pitch", 0);        // 云台俯仰 [-660, 660]
    this->declare_parameter("feed", 0);         // 拨弹轮 [-660, 660]
    this->declare_parameter("left_switch", 0);  // 左拨杆 [1, 3]
    this->declare_parameter("right_switch", 0); // 右拨杆 [1, 3]

    // 获取参数
    serial_port_ = this->get_parameter("serial_port").as_string();
    baudrate_ = this->get_parameter("baudrate").as_int();
    send_frequency_ = this->get_parameter("send_frequency").as_double();

    RCLCPP_INFO(this->get_logger(), "---------------------------------");
    RCLCPP_INFO(this->get_logger(), "UART 收发节点启动");
    RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
    RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
    RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
    RCLCPP_INFO(this->get_logger(), "---------------------------------");

    // 初始化串口
    if (!initSerial()) {
      RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
      rclcpp::shutdown();
      return;
    }

    // 创建发布者
    connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
        "transmitter/connection_status", 10);

    // 创建定时器 - 发送数据
    auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
    send_timer_ = this->create_wall_timer(
        std::chrono::duration_cast<std::chrono::milliseconds>(send_period),
        std::bind(&UartTransmitterNode::sendControlFrame, this));

    // 创建定时器 - 发布连接状态
    status_timer_ = this->create_wall_timer(
        std::chrono::seconds(1),
        std::bind(&UartTransmitterNode::publishStatus, this));

    // 创建接收线程
    receive_thread_ = std::thread(&UartTransmitterNode::receiveLoop, this);

    RCLCPP_INFO(this->get_logger(), "UART 节点初始化完成");
  }

  ~UartTransmitterNode() {
    running_ = false;
    if (receive_thread_.joinable()) {
      receive_thread_.join();
    }
    if (serial_fd_ >= 0) {
      close(serial_fd_);
    }
    RCLCPP_INFO(this->get_logger(), "串口已关闭");
  }

private:
  bool initSerial() {
    RCLCPP_INFO(this->get_logger(), "正在打开串口 %s...", serial_port_.c_str());

    // 打开串口
    serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (serial_fd_ < 0) {
      RCLCPP_ERROR(this->get_logger(), "无法打开串口 %s: %s",
                   serial_port_.c_str(), strerror(errno));
      return false;
    }

    // 配置串口
    struct termios tty;
    memset(&tty, 0, sizeof(tty));

    if (tcgetattr(serial_fd_, &tty) != 0) {
      RCLCPP_ERROR(this->get_logger(), "tcgetattr 错误: %s", strerror(errno));
      close(serial_fd_);
      serial_fd_ = -1;
      return false;
    }

    // 设置波特率
    speed_t baud = convertBaudrate(baudrate_);
    cfsetospeed(&tty, baud);
    cfsetispeed(&tty, baud);

    // 8N1
    tty.c_cflag &= ~PARENB; // 无校验
    tty.c_cflag &= ~CSTOPB; // 1位停止位
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;            // 8位数据
    tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略控制线

    // 禁用硬件流控
    tty.c_cflag &= ~CRTSCTS;

    // 原始模式
    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
    tty.c_oflag &= ~OPOST;

    // 设置超时
    tty.c_cc[VMIN] = 0;
    tty.c_cc[VTIME] = 1; // 100ms 超时

    if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
      RCLCPP_ERROR(this->get_logger(), "tcsetattr 错误: %s", strerror(errno));
      close(serial_fd_);
      serial_fd_ = -1;
      return false;
    }

    // 清空缓冲区
    tcflush(serial_fd_, TCIOFLUSH);

    RCLCPP_INFO(this->get_logger(), "串口打开成功");
    is_connected_ = true;
    return true;
  }

  speed_t convertBaudrate(int baudrate) {
    switch (baudrate) {
    case 9600:
      return B9600;
    case 19200:
      return B19200;
    case 38400:
      return B38400;
    case 57600:
      return B57600;
    case 115200:
      return B115200;
    case 230400:
      return B230400;
    case 460800:
      return B460800;
    case 921600:
      return B921600;
    default:
      RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d，使用 115200",
                  baudrate);
      return B115200;
    }
  }

  void sendControlFrame() {
    if (serial_fd_ < 0) {
      RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                           "串口未打开");
      return;
    }

    // 获取控制参数
    int16_t x_move =
        static_cast<int16_t>(this->get_parameter("x_move").as_int());
    int16_t y_move =
        static_cast<int16_t>(this->get_parameter("y_move").as_int());
    int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
    int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
    int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
    uint8_t left_switch =
        static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) &
        0x0F;
    uint8_t right_switch =
        static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) &
        0x0F;

    // 构建数据帧
    uint8_t frame[FRAME_LENGTH];
    int idx = 0;

    // 帧头
    frame[idx++] = FRAME_HEADER_1;
    frame[idx++] = FRAME_HEADER_2;

    // 平动左右 (2 bytes, int16, 小端序)
    frame[idx++] = x_move & 0xFF;
    frame[idx++] = (x_move >> 8) & 0xFF;

    // 平动前后 (2 bytes, 小端序)
    frame[idx++] = y_move & 0xFF;
    frame[idx++] = (y_move >> 8) & 0xFF;

    // 云台偏航 (2 bytes, 小端序)
    frame[idx++] = yaw & 0xFF;
    frame[idx++] = (yaw >> 8) & 0xFF;

    // 云台俯仰 (2 bytes, 小端序)
    frame[idx++] = pitch & 0xFF;
    frame[idx++] = (pitch >> 8) & 0xFF;

    // 拨弹轮 (2 bytes, 小端序)
    frame[idx++] = feed & 0xFF;
    frame[idx++] = (feed >> 8) & 0xFF;

    // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
    frame[idx++] = (left_switch << 4) | right_switch;

    // CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身)
    // 当前idx=13, 数据长度=11 (从frame[2]到frame[12])
    // uint8_t crc_value = calculateCRC8(frame + 2, 11);
    frame[idx++] = 0xCC; // crc_value

    // 帧尾
    frame[idx++] = FRAME_TAIL;

    // 调试输出：打印完整帧内容
    std::string frame_hex;
    char buf[4];
    for (int i = 0; i < FRAME_LENGTH; i++) {
      snprintf(buf, sizeof(buf), "%02X ", frame[i]);
      frame_hex += buf;
    }
    RCLCPP_INFO(this->get_logger(), "[TX](%dB): %s", FRAME_LENGTH,
                frame_hex.c_str());

    // 发送数据
    ssize_t written = write(serial_fd_, frame, FRAME_LENGTH);
    if (written != FRAME_LENGTH) {
      RCLCPP_WARN(this->get_logger(), "发送数据不完整: %zd/%d", written,
                  FRAME_LENGTH);
    }
  }

  uint8_t calculateCRC8(const uint8_t *data, size_t len) {
    uint8_t crc = 0xFF; // 初始值
    for (size_t i = 0; i < len; i++) {
      crc ^= data[i];
      for (int j = 0; j < 8; j++) {
        if (crc & 0x80) {
          crc = (crc << 1) ^ 0x31; // CRC8-MAXIM 多项式
        } else {
          crc <<= 1;
        }
      }
    }
    return crc;
  }

  void receiveLoop() {
    uint8_t buffer[256];
    std::vector<uint8_t> frame_buffer;
    frame_buffer.reserve(FRAME_LENGTH);

    while (running_ && rclcpp::ok()) {
      if (serial_fd_ < 0) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        continue;
      }

      // 读取数据
      ssize_t n = read(serial_fd_, buffer, sizeof(buffer));
      if (n > 0) {
        for (ssize_t i = 0; i < n; i++) {
          frame_buffer.push_back(buffer[i]);

          // 查找帧头
          if (frame_buffer.size() >= 2 && frame_buffer[0] == FRAME_HEADER_1 &&
              frame_buffer[1] == FRAME_HEADER_2) {

            // 等待完整帧
            if (frame_buffer.size() >= FRAME_LENGTH) {
              // 检查帧尾
              if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
                // 验证 CRC
                uint8_t rx_crc = frame_buffer[FRAME_LENGTH - 2];
                uint8_t calc_crc =
                    calculateCRC8(frame_buffer.data() + 2, FRAME_LENGTH - 4);

                if (rx_crc == calc_crc) {
                  // 解析数据（小端序）
                  int16_t x_move = frame_buffer[2] | (frame_buffer[3] << 8);
                  int16_t y_move = frame_buffer[4] | (frame_buffer[5] << 8);
                  int16_t yaw = frame_buffer[6] | (frame_buffer[7] << 8);
                  int16_t pitch = frame_buffer[8] | (frame_buffer[9] << 8);
                  int16_t feed = frame_buffer[10] | (frame_buffer[11] << 8);
                  uint8_t key = frame_buffer[12];
                  uint8_t left_switch = (key >> 4) & 0x0F;
                  uint8_t right_switch = key & 0x0F;

                  // 输出详细日志
                  RCLCPP_INFO(this->get_logger(),
                              "[RX]: %02X %02X | x:%d y:%d yaw:%d "
                              "pitch:%d feed:%d | L:%d R:%d | CRC:%02X | %02X",
                              frame_buffer[0], frame_buffer[1], x_move, y_move,
                              yaw, pitch, feed, left_switch, right_switch,
                              rx_crc, frame_buffer[FRAME_LENGTH - 1]);
                } else {
                  RCLCPP_WARN(
                      this->get_logger(),
                      "[CRC错误] 接收=%02X, 计算=%02X, 帧内容: %s", rx_crc,
                      calc_crc,
                      bytesToHex(frame_buffer.data(), FRAME_LENGTH).c_str());
                }
              } else {
                RCLCPP_WARN(this->get_logger(),
                            "[帧尾错误] 期望=%02X, 实际=%02X", FRAME_TAIL,
                            frame_buffer[FRAME_LENGTH - 1]);
              }

              // 清空缓冲区，准备下一帧
              frame_buffer.clear();
            }
          } else if (frame_buffer.size() > FRAME_LENGTH) {
            // 缓冲区溢出，清空
            frame_buffer.clear();
          }
        }
      } else if (n < 0 && errno != EAGAIN) {
        RCLCPP_ERROR(this->get_logger(), "读取错误: %s", strerror(errno));
      }

      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
  }

  std::string bytesToHex(const uint8_t *data, size_t len) {
    std::string result;
    char buf[4];
    for (size_t i = 0; i < len; i++) {
      snprintf(buf, sizeof(buf), "%02X ", data[i]);
      result += buf;
    }
    return result;
  }

  void publishStatus() {
    auto msg = std_msgs::msg::Bool();
    msg.data = is_connected_ && (serial_fd_ >= 0);
    connection_status_pub_->publish(msg);
  }

  // 成员变量
  std::string serial_port_;
  int baudrate_;
  double send_frequency_;

  int serial_fd_ = -1;
  bool is_connected_ = false;
  bool running_ = true;
  std::thread receive_thread_;

  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
  rclcpp::TimerBase::SharedPtr send_timer_;
  rclcpp::TimerBase::SharedPtr status_timer_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  auto node = std::make_shared<UartTransmitterNode>();

  if (rclcpp::ok()) {
    rclcpp::spin(node);
  }

  rclcpp::shutdown();
  return 0;
}