#include "mc/timer_tasks.hpp"
#include "mc/control_cache.hpp"
#include "mc/can_struct.h"

// 定时器回调：MCU 控制
void mcuTimerTask(CANDriver &canctl)
{
  auto 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 控制
void epsTimerTask(CANDriver &canctl)
{
  auto 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 扫地控制
void vcuTimerTask(CANDriver &canctl)
{
  static bool vcu_initialized = false;

  static int last_roll_brush_suction_direction = 0;
  static int last_roll_brush_suction = 0;
  static int last_side_brush = 0;
  static int last_dust_shake = 0;
  static int last_brush_deck_pusher = 0;
  static int last_suction_squeeqee_pusher = 0;
  static int last_water_spray = 0;

  auto msg = get_safe_control();

  // 首次运行时初始化VCU控制
  if (!vcu_initialized)
  {
    // 发送CAN使能指令
    vcu_enable_cmd.setCANEnable(true);
    canctl.sendFrame(vcu_enable_cmd.toFrame());
    vcu_initialized = true;
    RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled");
  }

  // 检查状态变化
  bool roll_brush_suction_direction_changed = (msg.roll_brush_suction_direction != last_roll_brush_suction_direction);
  bool roll_brush_suction_changed = (msg.roll_brush_suction != last_roll_brush_suction);
  bool side_brush_changed = (msg.side_brush != last_side_brush);
  bool dust_shake_changed = (msg.dust_shake != last_dust_shake);
  bool brush_deck_pusher_changed = (msg.brush_deck_pusher != last_brush_deck_pusher);
  bool suction_squeeqee_pusher_changed = (msg.suction_squeeqee_pusher != last_suction_squeeqee_pusher);
  bool water_spray_changed = (msg.water_spray != last_water_spray);

  bool motor1_needs_update = roll_brush_suction_direction_changed || roll_brush_suction_changed || side_brush_changed || dust_shake_changed || brush_deck_pusher_changed || suction_squeeqee_pusher_changed || water_spray_changed;
  bool motor2_needs_update = water_spray_changed;

  // ===== 控制0x211报文 (电机M1-M7) =====
  if (motor1_needs_update)
  {
    vcu_motor1_cmd.setM1Direction(msg.roll_brush_suction_direction);
    vcu_motor1_cmd.setM1Throttle(msg.roll_brush_suction);
    vcu_motor1_cmd.setM2Speed(msg.side_brush);
    vcu_motor1_cmd.setM3Speed(msg.dust_shake);
    vcu_motor1_cmd.setM4Speed(msg.brush_deck_pusher);
    vcu_motor1_cmd.setM5Speed(msg.suction_squeeqee_pusher);

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

  // ===== 控制0x212报文 (电机M8和LED输出) =====
  if (motor2_needs_update)
  {
    // 喷水电机控制
    vcu_motor2_cmd.setM8_Spray(msg.water_spray);

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

  // 更新状态记录
  last_roll_brush_suction_direction = msg.roll_brush_suction_direction;
  last_roll_brush_suction = msg.roll_brush_suction;
  last_side_brush = msg.side_brush;
  last_dust_shake = msg.dust_shake;
  last_brush_deck_pusher = msg.brush_deck_pusher;
  last_suction_squeeqee_pusher = msg.suction_squeeqee_pusher;
  last_water_spray = msg.water_spray;
}

// 注册所有定时器任务
void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl)
{
  static auto timer_mcu = node->create_wall_timer(
      std::chrono::milliseconds(50),
      [&canctl]()
      { mcuTimerTask(canctl); });

  static auto timer_eps = node->create_wall_timer(
      std::chrono::milliseconds(50),
      [&canctl]()
      { epsTimerTask(canctl); });

  static auto timer_vcu = node->create_wall_timer(
      std::chrono::milliseconds(100),
      [&canctl]()
      { vcuTimerTask(canctl); });

  RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed");
}