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

CANDriver canctl;

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)
{
  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();

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

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

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

  CANFrame mcu_frame = mcu_cmd.toFrame();
  CANFrame eps_frame = eps_cmd.toFrame();
  CANFrame vcu1_frame = vcu1_cmd.toFrame();
  CANFrame vcu2_frame = vcu2_cmd.toFrame();
  CANFrame main_brush_frame = main_brush_cmd.toFrame();
  CANFrame edge_brush_frame = edge_brush_cmd.toFrame();
  canctl.sendFrame(mcu_frame);
  canctl.sendFrame(eps_frame);
  canctl.sendFrame(vcu1_frame);
  canctl.sendFrame(vcu2_frame);
  canctl.sendFrame(main_brush_frame);
  canctl.sendFrame(edge_brush_frame);
}

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 // 收到消息后的回调
  );

  if (!canctl.open("can0"))
  {
    std::cerr << "Failed to open CAN interface\n";
    return 1;
  }

  VCUWakeUp();

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

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

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

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

  return 0;
}
