/**
 * @file    protocol.c
 * @brief 协议处理函数实现
 * @details 该文件包含了协议解析和处理的相关函数，主要用于处理从 ESP12F
 * 模块接收到的数据。
 * @author Beihong Wang
 * @date 2026-04-01
 */

#include "protocol.h"
#include "bsp_motor.h"
#include "bsp_hall.h"
#include "bsp_pid.h"
#include "bsp_uart.h"
#include "bsp_rc522.h"
#include "bsp_beep.h"
#include "bsp_sr04.h"
#include "bsp_track_ir.h"
#include "track_ir_algo.h"
#include "checksum.h"
#include "cmsis_os.h"
#include "elog.h"
#include <stdio.h>
#include <string.h>

/* 定义日志 TAG */
#define Protocol_TAG "Protocol"

/* 小车控制状态相关定义 */
#define CAR_PWM_MAX 3599U // PWM最大值，对应100%

/* PID 闭环参数，针对低速高摩擦环境优化 */
#define MOTOR_KP 6.5f 
#define MOTOR_KI 2.5f          // 微增 KI，消除最后 2-3 RPM 的静差
#define MOTOR_KD 0.1f
#define TARGET_MAX_RPM 50.0f   // 100% 对应 50 RPM
#define MOTOR_PWM_DEADZONE 1200 // 保持死区，克服静摩擦
#define MOTOR_PWM_MAX_ARR 3599 // 定时器重装载值
#define CARCTRL_LOOP_MS 50U
#define OBSTACLE_STOP_DISTANCE_CM 18.0f
#define OBSTACLE_RECOVER_DISTANCE_CM 22.0f

/* 手动控制方向斜向速度缩放，避免斜向合速度过大 */
#define MANUAL_DIAG_SCALE 0.7071f

/* 电机位置定义 (基于用户规定) */
#define MOTOR_LR MOTOR_1 // 左后 (Left Rear)
#define MOTOR_LF MOTOR_2 // 左前 (Left Front)
#define MOTOR_RF MOTOR_3 // 右前 (Right Front)
#define MOTOR_RR MOTOR_4 // 右后 (Right Rear)

/* 运动解算临时变量 (RPM 单位) */
static float target_v_x = 0;   // 前进后退分量 (-TARGET_MAX_RPM ~ TARGET_MAX_RPM)
static float target_v_y = 0;   // 左右横移分量
static float target_v_w = 0;   // 原地旋转分量

static uint8_t car_running = 0;         // 小车运行状态（1=运行，0=停止）
static uint8_t car_speed_percent = 0;   // 当前整体速度百分比（0~100）
static uint16_t car_target_station = 0; // 目标站点编号
static uint8_t car_target_reached = 0;  // 到站锁存，避免重复触发
static uint8_t obstacle_locked = 0;     // 避障锁（1=有障碍锁定）
static track_ir_ctrl_output_t track_output = {0};
static uint8_t track_line_mask = 0;

static void CarCtrl_StopAll(void);

typedef enum {
  CTRL_MODE_AUTO = 0,
  CTRL_MODE_MANUAL
} ctrl_mode_t;

typedef enum {
  MAN_DIR_STOP = 0,
  MAN_DIR_FWD,
  MAN_DIR_BWD,
  MAN_DIR_LEFT,
  MAN_DIR_RIGHT,
  MAN_DIR_LF,
  MAN_DIR_RF,
  MAN_DIR_LB,
  MAN_DIR_RB,
  MAN_DIR_CW,
  MAN_DIR_CCW
} manual_dir_t;

static ctrl_mode_t car_ctrl_mode = CTRL_MODE_AUTO;
static manual_dir_t manual_dir = MAN_DIR_STOP;

#define CAR_STATION_MIN ((uint16_t)STATION_1)
#define CAR_STATION_MAX ((uint16_t)STATION_2)

/* 4个电机的 PID 控制器 */
static PID_TypeDef motor_pid[MOTOR_COUNT];

static const char *CarCtrl_ModeToString(ctrl_mode_t mode)
{
  return (mode == CTRL_MODE_MANUAL) ? "MAN" : "AUTO";
}

static const char *CarCtrl_ManualDirToString(manual_dir_t dir)
{
  switch (dir) {
  case MAN_DIR_FWD:  return "FWD";
  case MAN_DIR_BWD:  return "BWD";
  case MAN_DIR_LEFT: return "LEFT";
  case MAN_DIR_RIGHT:return "RIGHT";
  case MAN_DIR_LF:   return "LF";
  case MAN_DIR_RF:   return "RF";
  case MAN_DIR_LB:   return "LB";
  case MAN_DIR_RB:   return "RB";
  case MAN_DIR_CW:   return "CW";
  case MAN_DIR_CCW:  return "CCW";
  case MAN_DIR_STOP:
  default:
    return "STOP";
  }
}

static void CarCtrl_SetManualDirection(manual_dir_t dir)
{
  manual_dir = dir;
  if (dir == MAN_DIR_STOP) {
    car_running = 0U;
    target_v_x = 0.0f;
    target_v_y = 0.0f;
    target_v_w = 0.0f;
    CarCtrl_StopAll();
  } else {
    car_running = 1U;
    car_target_reached = 0U;
  }
}

/**
 * @brief 初始化闭环控制系统
 */
void CarCtrl_InitClosedLoop(void)
{
    for(int i=0; i<MOTOR_COUNT; i++) {
        // 恢复较高的积分限幅(2500)。要想达到稳态，单靠积分项必须能填补：3599(满载) - 1200(死区) = 2399 的差距。
        pid_init(&motor_pid[i], MOTOR_KP, MOTOR_KI, MOTOR_KD, (float)MOTOR_PWM_MAX_ARR, 2500.0f);
    }

  track_ir_algo_init();
  obstacle_locked = 0U;
  car_ctrl_mode = CTRL_MODE_AUTO;
  manual_dir = MAN_DIR_STOP;
  BEEP_Off();
}

/**
 * @brief 停止所有电机
 */
static void CarCtrl_StopAll(void)
{
  for(int i=0; i<MOTOR_COUNT; i++) {
    motor_stop(i);
    pid_reset(&motor_pid[i]);
  }
}

/**
 * @brief 到站蜂鸣提示
 * @param times 蜂鸣次数
 */
static void CarCtrl_BeepTimes(uint8_t times)
{
  for (uint8_t i = 0; i < times; i++) {
    BEEP_On();
    osDelay(120);
    BEEP_Off();
    osDelay(120);
  }
}

/**
 * @brief 运行期间检测是否到达目标站点
 */
static void CarCtrl_CheckTargetStation(void)
{
  rc522_card_info_t card;
  station_id_t station;

  if (car_ctrl_mode != CTRL_MODE_AUTO) {
    return;
  }

  if (car_running == 0U || car_target_reached != 0U || car_target_station == 0U) {
    return;
  }

  if (rc522_get_new_card(&card) == 0U) {
    return;
  }

  station = rc522_match_station(card.uid, card.uid_len);
  if ((uint16_t)station == car_target_station) {
    car_running = 0U;
    car_target_reached = 1U;
    CarCtrl_StopAll();
    elog_i(Protocol_TAG, "到达目标站点%u，停车并蜂鸣", car_target_station);
    CarCtrl_BeepTimes(3U);
  }
}

/**
 * @brief 执行一步 PID 闭环计算并更新电机 (建议 10ms-20ms 调用一次)
 */
void CarCtrl_UpdateClosedLoop(void)
{
  float dist;

    if (!car_running) {
    obstacle_locked = 0U;
    BEEP_Off();
    track_ir_algo_reset();
        CarCtrl_StopAll();
        return;
    }

  dist = sr04_get_distance();
  if ((dist > 0.0f) && (dist < OBSTACLE_STOP_DISTANCE_CM)) {
    obstacle_locked = 1U;
  }

  if (obstacle_locked != 0U) {
    if ((dist > OBSTACLE_RECOVER_DISTANCE_CM) || (dist <= 0.0f)) {
      obstacle_locked = 0U;
      BEEP_Off();
      elog_i(Protocol_TAG, "障碍解除，恢复循迹");
    } else {
      target_v_x = 0.0f;
      target_v_y = 0.0f;
      target_v_w = 0.0f;
      BEEP_On();
      CarCtrl_StopAll();
      return;
    }
  }

  BEEP_Off();

  if (car_ctrl_mode == CTRL_MODE_AUTO) {
    track_line_mask = track_ir_get_line_mask_filtered();
    track_ir_algo_step(track_line_mask, &track_output);

    // 从前进速度分量计算基础速度（自动循迹）
    target_v_x = (float)car_speed_percent * TARGET_MAX_RPM / 100.0f;
    target_v_x = target_v_x * (float)track_output.speed_scale_percent / 100.0f;
    target_v_y = 0.0f;
    target_v_w = track_output.yaw_correction_rpm;
  } else {
    float base_rpm = (float)car_speed_percent * TARGET_MAX_RPM / 100.0f;
    track_line_mask = 0U;
    memset(&track_output, 0, sizeof(track_output));

    /* 手动模式下，依据上位机方向指令生成 Vx/Vy/Vw */
    switch (manual_dir) {
    case MAN_DIR_FWD:
      target_v_x = base_rpm; target_v_y = 0.0f; target_v_w = 0.0f;
      break;
    case MAN_DIR_BWD:
      target_v_x = -base_rpm; target_v_y = 0.0f; target_v_w = 0.0f;
      break;
    case MAN_DIR_LEFT:
      target_v_x = 0.0f; target_v_y = base_rpm; target_v_w = 0.0f; // 修正：左移Vy为正
      break;
    case MAN_DIR_RIGHT:
      target_v_x = 0.0f; target_v_y = -base_rpm; target_v_w = 0.0f; // 修正：右移Vy为负
      break;
    case MAN_DIR_LF:
      target_v_x = base_rpm * MANUAL_DIAG_SCALE;
      target_v_y = -base_rpm * MANUAL_DIAG_SCALE;
      target_v_w = 0.0f;
      break;
    case MAN_DIR_RF:
      target_v_x = base_rpm * MANUAL_DIAG_SCALE;
      target_v_y = base_rpm * MANUAL_DIAG_SCALE;
      target_v_w = 0.0f;
      break;
    case MAN_DIR_LB:
      target_v_x = -base_rpm * MANUAL_DIAG_SCALE;
      target_v_y = -base_rpm * MANUAL_DIAG_SCALE;
      target_v_w = 0.0f;
      break;
    case MAN_DIR_RB:
      target_v_x = -base_rpm * MANUAL_DIAG_SCALE;
      target_v_y = base_rpm * MANUAL_DIAG_SCALE;
      target_v_w = 0.0f;
      break;
    case MAN_DIR_CW:
      target_v_x = 0.0f; target_v_y = 0.0f; target_v_w = base_rpm;
      break;
    case MAN_DIR_CCW:
      target_v_x = 0.0f; target_v_y = 0.0f; target_v_w = -base_rpm;
      break;
    case MAN_DIR_STOP:
    default:
      target_v_x = 0.0f; target_v_y = 0.0f; target_v_w = 0.0f;
      break;
    }
  }

    /* 麦克纳姆轮运动解算 (RPM单位)
     * 根据你的电机位置规定：
     * LR: M1, LF: M2, RF: M3, RR: M4
     */
    float target_rpms[MOTOR_COUNT];
    
    // 麦轮全向解算公式：
    // LF = Vx + Vy - Vw
    // RF = Vx - Vy + Vw
    // LR = Vx - Vy - Vw
    // RR = Vx + Vy + Vw
    target_rpms[MOTOR_LF] = target_v_x + target_v_y - target_v_w; // 左前
    target_rpms[MOTOR_RF] = target_v_x - target_v_y + target_v_w; // 右前
    target_rpms[MOTOR_LR] = target_v_x - target_v_y - target_v_w; // 左后
    target_rpms[MOTOR_RR] = target_v_x + target_v_y + target_v_w; // 右后

    for(int i=0; i<MOTOR_COUNT; i++) {
        float actual_rpm = hall_get_speed(i);
        
        // 限幅目标值，防止叠加后超出物理极限
        if (target_rpms[i] > TARGET_MAX_RPM) target_rpms[i] = TARGET_MAX_RPM;
        if (target_rpms[i] < -TARGET_MAX_RPM) target_rpms[i] = -TARGET_MAX_RPM;

        float pid_out = pid_calculate(&motor_pid[i], target_rpms[i], actual_rpm);
        
        // 优化死区逻辑：当目标速度不为0时，叠加死区偏移
        float final_pwm = 0;
        if (target_rpms[i] > 1.0f || target_rpms[i] < -1.0f) {
            if (pid_out >= 0) {
                final_pwm = pid_out + MOTOR_PWM_DEADZONE;
            } else {
                final_pwm = pid_out - MOTOR_PWM_DEADZONE;
            }
            // 最终限幅，防止总 PWM 超过 ARR (3599)
            if (final_pwm > 3599) final_pwm = 3599;
            if (final_pwm < -3599) final_pwm = -3599;
        }

        motor_set_speed(i, (int16_t)final_pwm);
        
        // 修改打印逻辑：使用位置别名让日志更易读
        const char* motor_names[] = {"LR", "LF", "RF", "RR"};
        static uint32_t last_log_time = 0;
        if (HAL_GetTick() - last_log_time > 500) { 
                     elog_d(Protocol_TAG, "M[%s] T:%.1f A:%.1f Pw:%d", 
                       motor_names[i], target_rpms[i], actual_rpm, (int16_t)final_pwm);
            if (i == MOTOR_COUNT - 1) last_log_time = HAL_GetTick();
        }
    }
}

/**
 * @brief 解析并执行协议命令
 * @param cmd_payload 队列传入的命令字符串（如 "ST:RUN"、"SP:080"、"GS:005"）
 */
static void CarCtrl_HandleCommand(const char *cmd_payload)
{
  const char *arg = NULL;

  if (cmd_payload == NULL || cmd_payload[0] == '\0') {
    return;
  }

  // 查找冒号分隔符，分割命令类型和参数
  arg = strchr(cmd_payload, ':');
  if (arg == NULL) {
    elog_w(Protocol_TAG, "未知控制指令: %s", cmd_payload);
    return;
  }

  // 启动/停止命令（仅作为自动循迹任务的启动/暂停信号，不直接控制电机）
  if (strncmp(cmd_payload, "ST", 2) == 0) {
    if (strcmp(arg + 1, "RUN") == 0) {
      if (car_target_reached != 0U) {
        car_running = 0U;
        CarCtrl_StopAll();
        elog_w(Protocol_TAG, "已到达站点，请先重新设置目标站点 (GS:xxx) 之后再启动");
        CarCtrl_BeepTimes(2U);
        Protocol_SendFeedback("ST", 0);
        return;
      }

      if (car_target_station < CAR_STATION_MIN || car_target_station > CAR_STATION_MAX) {
        car_running = 0U;
        CarCtrl_StopAll();
        elog_w(Protocol_TAG, "未设置有效站点，拒绝启动。请先下发 GS:001 或 GS:002");
        CarCtrl_BeepTimes(2U);
        Protocol_SendFeedback("ST", 0);
        return;
      }

      car_running = 1;
      car_target_reached = 0;
      elog_i(Protocol_TAG, "小车自动循迹启动, speed=%u%%, station=%u", car_speed_percent,
             car_target_station);
      Protocol_SendFeedback("ST", 1);
    } else if (strcmp(arg + 1, "STOP") == 0) {
      car_running = 0;
      car_target_reached = 0;
      CarCtrl_StopAll();
      elog_i(Protocol_TAG, "小车自动循迹暂停");
      Protocol_SendFeedback("ST", 1);
    } else {
      elog_w(Protocol_TAG, "未知启动/停止命令: %s", cmd_payload);
      Protocol_SendFeedback("ST", 0);
    }
    return;
  }

  // 设置整体速度命令
  if (strncmp(cmd_payload, "SP", 2) == 0) {
    unsigned int speed = 0;

    // 解析速度参数（百分比，0~100）
    if (sscanf(arg + 1, "%u", &speed) == 1) {
      if (speed > 100U) {
        speed = 100U;
      }

      car_speed_percent = (uint8_t)speed;
      elog_i(Protocol_TAG, "设置整体速度: %u%%", car_speed_percent);

      Protocol_SendFeedback("SP", 1);
    } else {
      elog_w(Protocol_TAG, "速度参数解析失败: %s", cmd_payload);
      Protocol_SendFeedback("SP", 0);
    }
    return;
  }

  // 设置目标站点命令
  if (strncmp(cmd_payload, "GS", 2) == 0) {
    unsigned int station = 0;

    // 解析站点编号（0~999）
    if (sscanf(arg + 1, "%u", &station) == 1) {
      if (station < CAR_STATION_MIN || station > CAR_STATION_MAX) {
        elog_w(Protocol_TAG, "站点%03u未配置，目前仅支持 %03u~%03u", station,
               CAR_STATION_MIN, CAR_STATION_MAX);
        CarCtrl_BeepTimes(2U);
        return;
      }

      car_target_station = (uint16_t)station;
      car_target_reached = 0;
      elog_i(Protocol_TAG, "设置目标站点: %03u", car_target_station);
      CarCtrl_BeepTimes(1U);
      Protocol_SendFeedback("GS", 1);
    } else {
      elog_w(Protocol_TAG, "站点参数解析失败: %s", cmd_payload);
      Protocol_SendFeedback("GS", 0);
    }
    return;
  }

  // 控制模式切换：MD:AUTO / MD:MAN
  if (strncmp(cmd_payload, "MD", 2) == 0) {
    if (strcmp(arg + 1, "AUTO") == 0) {
      car_ctrl_mode = CTRL_MODE_AUTO;
      manual_dir = MAN_DIR_STOP;
      car_running = 0U;
      obstacle_locked = 0U;
      track_ir_algo_reset();
      CarCtrl_StopAll();
      elog_i(Protocol_TAG, "已切换为自动循迹模式(AUTO)");
      Protocol_SendFeedback("MD", 1);
    } else if (strcmp(arg + 1, "MAN") == 0) {
      car_ctrl_mode = CTRL_MODE_MANUAL;
      obstacle_locked = 0U;
      CarCtrl_SetManualDirection(MAN_DIR_STOP);
      elog_i(Protocol_TAG, "已切换为手动麦轮模式(MAN)");
      Protocol_SendFeedback("MD", 1);
    } else {
      elog_w(Protocol_TAG, "未知模式命令: %s", cmd_payload);
      Protocol_SendFeedback("MD", 0);
    }
    return;
  }

  // 手动运动方向命令：MV:FWD/BWD/LEFT/RIGHT/LF/RF/LB/RB/CW/CCW/STOP
  if (strncmp(cmd_payload, "MV", 2) == 0) {
    if (car_ctrl_mode != CTRL_MODE_MANUAL) {
      elog_w(Protocol_TAG, "当前非手动模式，拒绝执行MV。请先下发 MD:MAN");
      Protocol_SendFeedback("MV", 0);
      return;
    }

    if (strcmp(arg + 1, "FWD") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_FWD);
    } else if (strcmp(arg + 1, "BWD") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_BWD);
    } else if (strcmp(arg + 1, "LEFT") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_LEFT);
    } else if (strcmp(arg + 1, "RIGHT") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_RIGHT);
    } else if (strcmp(arg + 1, "LF") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_LF);
    } else if (strcmp(arg + 1, "RF") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_RF);
    } else if (strcmp(arg + 1, "LB") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_LB);
    } else if (strcmp(arg + 1, "RB") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_RB);
    } else if (strcmp(arg + 1, "CW") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_CW);
    } else if (strcmp(arg + 1, "CCW") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_CCW);
    } else if (strcmp(arg + 1, "STOP") == 0) {
      CarCtrl_SetManualDirection(MAN_DIR_STOP);
    } else {
      elog_w(Protocol_TAG, "未知手动方向命令: %s", cmd_payload);
      Protocol_SendFeedback("MV", 0);
      return;
    }

    elog_i(Protocol_TAG, "手动方向=%s, speed=%u%%", CarCtrl_ManualDirToString(manual_dir),
           car_speed_percent);
    Protocol_SendFeedback("MV", 1);
    return;
  }

  // 未知命令类型
  elog_w(Protocol_TAG, "未支持的控制命令: %s", cmd_payload);
}

/**
 * @brief 打包并发送协议帧到上位机 (TCP)
 * @param payload 有效载荷文本 (不含帧头 LOGI:, 分隔符 :, 校验位 CS 和 帧尾 #)
 */
static void Protocol_SendPacket(const char *payload) {
  char packet[192] = {0};
  uint8_t cs = 0;

  // 1. 组装基础段: LOGI:PAYLOAD
  snprintf(packet, sizeof(packet), "LOGI:%s", payload);
  
  // 2. 计算校验和 (从 LOGI 开始到 payload 结束的累加和)
  cs = Calculate_CheckSum((uint8_t *)packet, 0, (uint16_t)strlen(packet));
  
  // 3. 拼接校验位和帧尾
  size_t current_len = strlen(packet);
  snprintf(packet + current_len, sizeof(packet) - current_len, ":%02X#", cs);

  // 4. 直接发送
  ESP12F_TCP_SendMessage(packet);
}

/**
 * @brief 发送状态报告
 * 格式示例: STAT:SP:080,STA:001,RUN:1,DIS:12.5,TRK:0010,RPM:25:25:25:25
 */
void Protocol_SendStatusReport(void) {
  char payload[160] = {0};
  float dist = sr04_get_distance();
  uint8_t trk_mask = track_line_mask;
  
  // STAT:SP:xxx,STA:xxx,RUN:x,DIS:xxx.x,TRK:xxxx,RPM:m1:m2:m3:m4
  // TRK 此时上报 4 位二进制掩码 (H4 H3 H2 H1)
  // RPM 为各个电机的实际 RPM，保留整数
  snprintf(payload, sizeof(payload), 
           "STAT:SP:%03u,STA:%03u,RUN:%u,MODE:%s,MAN:%s,DIS:%.1f,TRK:%u%u%u%u,DEV:%d,OBS:%u,RPM:%d:%d:%d:%d",
           car_speed_percent, car_target_station, car_running,
           CarCtrl_ModeToString(car_ctrl_mode),
           CarCtrl_ManualDirToString(manual_dir),
           dist,
           (trk_mask & TRACK_IR_H4_BIT) ? 1 : 0,
           (trk_mask & TRACK_IR_H3_BIT) ? 1 : 0,
           (trk_mask & TRACK_IR_H2_BIT) ? 1 : 0,
           (trk_mask & TRACK_IR_H1_BIT) ? 1 : 0,
           (int)track_output.deviation,
           obstacle_locked,
           (int)hall_get_speed(MOTOR_1),
           (int)hall_get_speed(MOTOR_2),
           (int)hall_get_speed(MOTOR_3),
           (int)hall_get_speed(MOTOR_4));
           
  Protocol_SendPacket(payload);
}

/**
 * @brief 发送反馈
 * 格式示例: FB:ST:1 (ST成功), FB:GS:0 (GS失败)
 */
void Protocol_SendFeedback(const char *cmd_type, uint8_t status) {
  char payload[16] = {0};
  snprintf(payload, sizeof(payload), "FB:%s:%u", cmd_type, status);
  Protocol_SendPacket(payload);
}

/* 引用在 freertos.c 中定义的消息队列句柄 */
extern osMessageQueueId_t CmdQueueHandle;
/**
 * @brief 协议处理函数
 * @details 严格按照协议文档：校验范围 = 帧头 + 命令 + 数据
 *          即从下标 0 开始，一直加到最后一个冒号之前
 */
void Protocol_HandleMessage(uint8_t *data, uint16_t len) {
  if (data == NULL)
    return;

  // 1. 基础检查：长度必须足够，且必须以 '#' 结尾
  if (len < 10 || data[len - 1] != '#') {
    elog_w(Protocol_TAG, "协议错误：长度不足或帧尾错误 (len: %d)", len);
    return;
  }

  // 2. 寻找校验位前的分隔符
  // 协议格式：LOGI:CMD:DATA:CS#
  // 我们需要找到最后一个冒号 ':' 的位置，它前面是数据，后面是校验位
  int last_colon_pos = -1;
  for (int i = 0; i < len; i++) {
    if (data[i] == ':') {
      last_colon_pos = i;
    }
  }

  // 如果找不到冒号，说明格式错误
  if (last_colon_pos == -1 || last_colon_pos < 5) {
    elog_w(Protocol_TAG, "协议错误：找不到分隔符 ':' 或位置非法");
    return;
  }

  // 3. 提取接收到的校验位 (从 ASCII 转为 Hex 数值)
  // 校验位紧跟在 last_colon_pos 之后，长度为 2 字节
  char recv_cs_hex_str[3] = {0};
  // 防止越界
  if (last_colon_pos + 3 >= len) {
    elog_w(Protocol_TAG, "协议错误：校验位数据越界");
    return;
  }

  recv_cs_hex_str[0] = data[last_colon_pos + 1];
  recv_cs_hex_str[1] = data[last_colon_pos + 2];

  unsigned int received_checksum = 0;
  sscanf(recv_cs_hex_str, "%02X", &received_checksum);

  // 4. 计算本地校验和
  // 【核心修改点】
  // 严格按照协议文档：从下标 0 开始，长度为 last_colon_pos
  // 也就是计算 "LOGI:SP:080" 的累加和
  uint8_t calculated_checksum =
      Calculate_CheckSum(data, 0, (uint16_t)last_colon_pos);

  // 5. 对比校验和
    if (calculated_checksum == (uint8_t)received_checksum) {
    elog_i(Protocol_TAG, "✅ 校验通过！执行指令: %s", (char *)data);

    /* 提取有效载荷发送到消息队列 */
    char cmd_payload[16] = {0};
    // 将 "LOGI:" 之后到最后一个冒号之前的内容作为指令
    uint16_t payload_len = last_colon_pos - 5; 
    uint16_t copy_len = (payload_len > 15) ? 15 : payload_len;
    if (copy_len > 0) {
      memcpy(cmd_payload, &data[5], copy_len);
    }

    osStatus_t status = osMessageQueuePut(CmdQueueHandle, cmd_payload, 0, 0);
    if (status != osOK) {
      elog_e(Protocol_TAG, "Protocol: Queue put failed: %d", status);
    }
  } else {
    elog_w(Protocol_TAG, "❌ 校验失败！计算值: 0x%02X, 接收值: 0x%02X",
          calculated_checksum, (uint8_t)received_checksum);

    // 辅助调试：打印实际参与计算的数据段
    char debug_buf[32] = {0};
    if (last_colon_pos < 32) {
      memcpy(debug_buf, data, last_colon_pos);
      elog_i(Protocol_TAG, "   -> 单片机正在计算这段数据的校验和: [%s]",
            debug_buf);
      elog_i(Protocol_TAG, "   -> 请检查上位机是否也是按照此范围计算累加和");
    }
  }
}

#define CarCtrlTask_TAG "CarCtrlTask"

void CarCtrl_Task(void *argument) {
  /* USER CODE BEGIN CarCtrl_Task */
  char cmd_payload[16] = {0};
  uint32_t last_report_tick = 0;
  
  /* 初始化闭环 PID 控制器 */
  CarCtrl_InitClosedLoop();

  /* Infinite loop */
  for (;;) {
    uint32_t now = HAL_GetTick();

    /* 1. 处理控制指令 (非阻塞获取，如果没有指令则继续执行闭环) */
    if (osMessageQueueGet(CmdQueueHandle, cmd_payload, NULL, 0) == osOK) {
      elog_d(CarCtrlTask_TAG, "CarCtrl: Command %s", cmd_payload);
      CarCtrl_HandleCommand(cmd_payload);
    }

    /* 1.1 运行中检查 RFID 是否到达目标站点 */
    CarCtrl_CheckTargetStation();

    /* 2. 执行 PID 闭环控制更新 */
    CarCtrl_UpdateClosedLoop();

    /* 3. 定期向应用层/上位机发送状态报告 (每 500ms) */
    if (now - last_report_tick >= 500) {
      Protocol_SendStatusReport();
      last_report_tick = now;
    }

    /* 与测速任务保持同周期，确保 PID 输入输出时基一致。 */
    osDelay(CARCTRL_LOOP_MS);
  }
  /* USER CODE END CarCtrl_Task */
}