#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 扫地控制
// 修改timer_tasks.cpp中的vcuTimerTask函数
void vcuTimerTask(CANDriver &canctl)
{
  static bool vcu_initialized = false;
  static bool last_sweep_state = false; // 记录上一次清扫状态

  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 sweep_state_changed = (msg.sweep != last_sweep_state);

  if (sweep_state_changed)
  {
    // 根据清扫状态设置所有部件
    uint8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止

    // ===== 控制0x211报文 (电机M1-M7) =====
    vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止
    vcu_motor1_cmd.setByte1(target_value);
    vcu_motor1_cmd.setByte2(target_value);
    vcu_motor1_cmd.setByte3(target_value);
    vcu_motor1_cmd.setByte4(target_value);
    vcu_motor1_cmd.setByte5(target_value);
    vcu_motor1_cmd.setByte6(target_value);
    vcu_motor1_cmd.setByte7(target_value);
    canctl.sendFrame(vcu_motor1_cmd.toFrame());

    // ===== 控制0x212报文 (电机M8和LED输出) =====
    vcu_motor2_cmd.setByte0(target_value);
    vcu_motor2_cmd.setByte1(target_value);
    vcu_motor2_cmd.setByte2(target_value);
    vcu_motor2_cmd.setByte3(target_value);
    vcu_motor2_cmd.setByte4(target_value);
    vcu_motor2_cmd.setByte5(target_value);
    vcu_motor2_cmd.setByte6(target_value);
    vcu_motor2_cmd.setByte7(target_value);
    canctl.sendFrame(vcu_motor2_cmd.toFrame());

    RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
                "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");

    // 更新状态记录
    last_sweep_state = msg.sweep;
  }
}

// 注册所有定时器任务
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");
}