#include <cstdio>
#include <iostream>
#include <iomanip>
#include "mc/can_driver.h"
#include "mc/can_struct.h"
#include "mc/get_config.h"
#include "rclcpp/rclcpp.hpp"
#include "mc/msg/can_frame.hpp"
#include "mc/msg/mc_ctrl.hpp"

CANDriver canctl;

struct ControlCache
{
  std::mutex mutex; // 防止多线程冲突
  mc::msg::McCtrl latest_msg;
  std::chrono::steady_clock::time_point last_update_time;
  bool has_data = false;

  void update(const mc::msg::McCtrl &msg)
  {
    std::lock_guard<std::mutex> lock(mutex);
    latest_msg = msg;
    last_update_time = std::chrono::steady_clock::now();
    has_data = true;
  }

  bool get(mc::msg::McCtrl &msg)
  {
    std::lock_guard<std::mutex> lock(mutex);
    if (!has_data)
      return false;

    auto now = std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time);

    if (duration.count() > 100) // ms超时
    {
      return false; // 数据过期
    }

    msg = latest_msg;
    return true;
  }
};

ControlCache control_cache;

// 发布检测
mc::msg::McCtrl get_safe_control()
{
  mc::msg::McCtrl msg;
  if (!control_cache.get(msg))
  {
    // 超时或未接收到控制数据，进入安全状态
    msg.mcu_enabled = false;
    msg.brake = 1;
    msg.gear = 0;
    msg.rpm = 0;
    msg.brake_time_ms = 500;
    msg.angle = 0;
    msg.angle_speed = 120;
    msg.main_brush_lift = false;
    msg.edge_brush_lift = false;
    msg.vacuum = false;
    msg.spray = false;
    msg.mud_flap = false;
    msg.dust_shake = false;
    msg.main_brush_spin = false;
    msg.edge_brush_spin = false;
  }
  return msg;
}

void receiveHandler(const CANFrame &frame, void *userData)
{
  // 获取 ROS 2 Publisher
  auto publisher = *static_cast<std::shared_ptr<rclcpp::Publisher<mc::msg::CanFrame>> *>(userData);

  // 创建一个新的消息
  auto msg = mc::msg::CanFrame();
  msg.id = frame.id;
  msg.dlc = frame.dlc;
  msg.data.resize(frame.dlc);

  // 复制数据
  for (int i = 0; i < frame.dlc; ++i)
  {
    msg.data[i] = frame.data[i];
  }

  // 发布消息
  publisher->publish(msg);

  std::cout << "CAN ID: ";
  if (frame.id > 0x7ff)
  {
    std::cout << std::hex << std::uppercase << frame.id;
  }
  else
  {
    std::cout << std::hex << std::uppercase << "     " << frame.id;
  }
  std::cout << " Data: ";

  for (int i = 0; i < frame.dlc; ++i)
  {
    std::cout << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
  }

  std::cout << std::dec << "\n";
}

void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
{
  control_cache.update(*msg);
}

void setupTimers(rclcpp::Node::SharedPtr node)
{
  // MCU、VCU1 控制，50Hz
  static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
      std::chrono::milliseconds(20), []()
      {
        mc::msg::McCtrl msg = get_safe_control();

        mcu_cmd.setEnabled(msg.mcu_enabled);
        mcu_cmd.setGear(msg.gear);
        mcu_cmd.setRPM(msg.rpm);
        mcu_cmd.setBrakeTime(msg.brake_time_ms);
        mcu_cmd.setBrake(msg.brake);
        mcu_cmd.pack();

        canctl.sendFrame(mcu_cmd.toFrame()); });

  // EPS 控制，20Hz
  static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
      std::chrono::milliseconds(50), []()
      {
        mc::msg::McCtrl msg = get_safe_control();

        eps_cmd.setCenterCmd(0);
        eps_cmd.setAngle(static_cast<int16_t>(msg.angle * 5));
        eps_cmd.setAngularSpeed(msg.angle_speed);
        eps_cmd.setControlMode(0x20);
        eps_cmd.pack();

        canctl.sendFrame(eps_cmd.toFrame()); });

  // VCU2 控制，10Hz
  static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
      std::chrono::milliseconds(100), []()
      {
        mc::msg::McCtrl msg = get_safe_control();

        vcu1_cmd.setLeftLight(msg.left_light);
        vcu1_cmd.setNightLightPercent(msg.night_light);

        vcu2_cmd.setBrakeLight(msg.brake_light);
        vcu2_cmd.setDustShake(msg.dust_shake);
        vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
        vcu2_cmd.setHeadlight(msg.headlight);
        vcu2_cmd.setMainBrushLift(msg.main_brush_lift);
        vcu2_cmd.setMudFlap(msg.mud_flap);
        vcu2_cmd.setRightLight(msg.right_light);
        vcu2_cmd.setSpray(msg.spray);
        vcu2_cmd.setVacuum(msg.vacuum);

        canctl.sendFrame(vcu1_cmd.toFrame());
        canctl.sendFrame(vcu2_cmd.toFrame()); });

  // 刷子控制，3.3Hz
  static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer(
      std::chrono::milliseconds(300), []()
      {
        mc::msg::McCtrl msg = get_safe_control();

        main_brush_cmd.setBrushEnable(msg.main_brush_spin);
        main_brush_cmd.setBrushPwm(msg.main_brush_pwm);

        edge_brush_cmd.setBrushEnable(msg.edge_brush_spin);
        edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm);

        canctl.sendFrame(main_brush_cmd.toFrame());
        canctl.sendFrame(edge_brush_cmd.toFrame()); });
}

void VCUWakeUp()
{
  CANFrame frame;

  // 公共设置
  frame.id = 0x18FF8017;
  frame.dlc = 8;
  frame.ext = true;  // 使用扩展帧
  frame.rtr = false; // 不是远程帧

  // 第1帧：握手帧
  frame.data[0] = 0x55;
  frame.data[1] = 0x21;
  for (int i = 2; i < 8; ++i)
    frame.data[i] = 0xFF;
  canctl.sendFrame(frame);

  // 延时，例如 50ms
  std::this_thread::sleep_for(std::chrono::milliseconds(50));

  // 第2帧：启动帧
  frame.data[0] = 0xAA;
  frame.data[1] = 0x21;
  for (int i = 2; i < 8; ++i)
    frame.data[i] = 0xFF;
  canctl.sendFrame(frame);
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv); // 初始化 ROS 2

  printf("Hello world mc package\n");

  // 创建一个 ROS 2 节点
  auto node = rclcpp::Node::make_shared("can_driver_node");

  // 创建一个 Publisher
  auto publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);

  // 创建 Subscriber（收控制指令）
  auto subscriber = node->create_subscription<mc::msg::McCtrl>(
      "mc_ctrl",     // 订阅的 topic 名字
      10,            // 队列长度
      mcCtrlCallback // 收到消息后的回调
  );

  Config mc_config;
  load_config(mc_config);

  if (!canctl.open(mc_config.can_dev))
  {
    std::cerr << "Failed to open CAN interface :";
    std::cerr << mc_config.can_dev << std::endl;
    return -1;
  }

  VCUWakeUp();

  // 设置接收回调并传递 ROS 2 Publisher 作为 userData
  canctl.setReceiveCallback(receiveHandler, (void *)&publisher);

  // 添加定时器设置
  setupTimers(node);

  // ROS 2 spin
  rclcpp::spin(node);

  // 关闭 CAN 接口
  canctl.close();

  rclcpp::shutdown(); // 关闭 ROS 2

  return 0;
}
