#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 "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"

namespace sweeperMsg = sweeper_interfaces::msg;

CANDriver canctl;

struct CanHandlerContext
{
  rclcpp::Node::SharedPtr node;
  std::shared_ptr<rclcpp::Publisher<sweeperMsg::CanFrame>> publisher;
};

std::atomic<bool> vcu_msg_received = false; // 是否收到过vcu数据帧
std::atomic<bool> vcu_awake = false;        // 是否唤醒成功
rclcpp::Time last_vcu_msg_time;             // 上次收到vcu数据帧的时间

// 全局开关，控制是否打印 CAN 消息到终端
bool g_can_print_enable = false;

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

  void update(const sweeperMsg::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(sweeperMsg::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;

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

// CAN消息接收回调
void receiveHandler(const CANFrame &frame, void *userData)
{
  // 获取 ROS 2 Publisher
  auto context = static_cast<CanHandlerContext *>(userData);
  auto publisher = context->publisher;
  auto node = context->node;
  auto now = node->now();

  // 创建并发布 CAN 消息
  auto msg = sweeperMsg::CanFrame();
  msg.id = frame.id;
  msg.dlc = frame.dlc;
  msg.data.assign(frame.data, frame.data + frame.dlc);
  publisher->publish(msg);

  // 根据开关决定是否打印 CAN 消息
  if (g_can_print_enable)
  {
    std::stringstream ss;
    ss << "CAN ID: ";
    if (frame.id > 0x7FF)
      ss << std::hex << std::uppercase << frame.id;
    else
      ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ') << frame.id;

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

    RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
  }

  // VCU报文检查
  if (frame.id == 0x18FA0121)
  {
    last_vcu_msg_time = now; // 更新时间
    vcu_msg_received.store(true);
    vcu_awake.store(true); // 唤醒成功
  }
}

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

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);
}

void setupTimers(rclcpp::Node::SharedPtr node)
{
  // VCU 唤醒帧发送定时器，1Hz
  static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
      std::chrono::seconds(1), [node]()
      {
        if (!vcu_awake.load())
        {
          RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
          VCUWakeUp();
        } });

  // vcu报文 watchdog 检查，200ms
  static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
      std::chrono::milliseconds(200), [node]()
      {
        auto now = node->now();
        auto elapsed = now - last_vcu_msg_time;

        if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
        {
          RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
          vcu_msg_received.store(false);
          vcu_awake.store(false);
        } });

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

        mcu_cmd.setEnabled(true);
        mcu_cmd.setGear(msg.gear);
        mcu_cmd.setRPM(msg.rpm);
        mcu_cmd.setBrake(msg.brake);

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

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

        eps_cmd.setCenterCmd(0);
        eps_cmd.setAngle(msg.angle);
        eps_cmd.setAngularSpeed(msg.angle_speed);
        eps_cmd.pack();

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

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

        vcu1_cmd.setLeftLight(msg.left_light);
        vcu1_cmd.setDustShake(msg.dust_shake);
        vcu1_cmd.setHeadLight(msg.headlight);

        vcu2_cmd.setBrakeLight(msg.brake_light);
        vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
        vcu2_cmd.setHeadlight(msg.headlight);
        vcu2_cmd.setMudFlap(msg.mud_flap);
        vcu2_cmd.setRightLight(msg.right_light);
        vcu2_cmd.setSpray(msg.spray);
        vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);

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

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

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

  RCLCPP_INFO(node->get_logger(), "Starting mc package...");

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

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

  last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());

  Config mc_config;
  load_config(mc_config);

  if (!canctl.open(mc_config.can_dev))
  {
    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
    return -1;
  }

  // 创建上下文
  auto context = std::make_shared<CanHandlerContext>();
  context->node = node;
  context->publisher = can_publisher;

  // 设置接收回调并传递上下文
  canctl.setReceiveCallback(receiveHandler, (void *)context.get());

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

  // 在 ROS shutdown 时自动清理
  rclcpp::on_shutdown([&]()
                      { canctl.close(); });

  // 事件循环
  rclcpp::spin(node);

  rclcpp::shutdown(); // 关闭 ROS 2
  return 0;
}
