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

#include "protocol.h"
#include "bsp_motor.h"
#include "bsp_uart.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%

static uint8_t car_running = 0;         // 小车运行状态（1=运行，0=停止）
static uint8_t car_speed_percent = 0;   // 当前整体速度百分比（0~100）
static uint16_t car_target_station = 0; // 目标站点编号

/**
 * @brief 百分比转PWM值
 * @param percent 速度百分比（0~100）
 * @return int16_t PWM占空比
 */
static int16_t CarCtrl_PercentToPwm(uint8_t percent)
{
  if (percent > 100U) {
    percent = 100U;
  }
  return (int16_t)((CAR_PWM_MAX * percent) / 100U);
}

/**
 * @brief 停止所有电机
 */
static void CarCtrl_StopAll(void)
{
  motor_stop(MOTOR_1);
  motor_stop(MOTOR_2);
  motor_stop(MOTOR_3);
  motor_stop(MOTOR_4);
}

/**
 * @brief 按当前 car_speed_percent 设置所有电机速度
 */
static void CarCtrl_ApplySpeed(void)
{
  int16_t pwm = CarCtrl_PercentToPwm(car_speed_percent);

  motor_set_speed(MOTOR_1, pwm);
  motor_set_speed(MOTOR_2, pwm);
  motor_set_speed(MOTOR_3, pwm);
  motor_set_speed(MOTOR_4, pwm);
}

/**
 * @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) {
      car_running = 1;
      elog_i(Protocol_TAG, "小车自动循迹启动, speed=%u%%, station=%u", car_speed_percent,
             car_target_station);
    } else if (strcmp(arg + 1, "STOP") == 0) {
      car_running = 0;
      elog_i(Protocol_TAG, "小车自动循迹暂停");
    } else {
      elog_w(Protocol_TAG, "未知启动/停止命令: %s", cmd_payload);
    }
    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);

      // 如果当前处于运行状态，立即生效
      if (car_running != 0U) {
        CarCtrl_ApplySpeed();
      }
    } else {
      elog_w(Protocol_TAG, "速度参数解析失败: %s", cmd_payload);
    }
    return;
  }

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

    // 解析站点编号（0~999）
    if (sscanf(arg + 1, "%u", &station) == 1) {
      if (station > 999U) {
        station = 999U;
      }

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

  // 未知命令类型
  elog_w(Protocol_TAG, "未支持的控制命令: %s", cmd_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};
  /* Infinite loop */
  for (;;) {
    /* 从消息队列中获取数据，阻塞等待 */
    if (osMessageQueueGet(CmdQueueHandle, cmd_payload, NULL, osWaitForever) ==
        osOK) {
      elog_i(CarCtrlTask_TAG, "CarCtrl: Received command (ASCII: %s) from queue",
             cmd_payload);
      CarCtrl_HandleCommand(cmd_payload);
    }
    // osDelay(1); // osMessageQueueGet 已经是阻塞的，不需要额外的 osDelay
  }
  /* USER CODE END CarCtrl_Task */
}