#include "robot_cmd_slot.h"
#include "FreeRTOS.h"
#include "task.h"
#include "main.h" // 提供 HAL_GetTick()

/* 全局唯一的指令槽实例 */
static RobotTargetCmd_t g_cmd_slot = {0};

void CmdSlot_Push(float vx, float wz, uint8_t flags)
{
  // 1. 获取当前系统时间作为时间戳
  uint32_t now_ms = HAL_GetTick();

  // 2. 进入临界区，关中断，防止写入一半时被高优先级任务或中断打断（防数据撕裂）
  taskENTER_CRITICAL();
  g_cmd_slot.target_vx = vx;
  g_cmd_slot.target_wz = wz;
  g_cmd_slot.ctrl_flags = flags;
  g_cmd_slot.gen_tick_ms = now_ms;
  taskEXIT_CRITICAL();
}

bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms)
{
  if (out_cmd == NULL) {
    return false;
  }

  // 1. 进入临界区，安全地拷贝走整个结构体
  taskENTER_CRITICAL();
  *out_cmd = g_cmd_slot;
  taskEXIT_CRITICAL();

  // 2. 计算指令老化时间 (利用无符号整型减法自然处理溢出回绕)
  uint32_t now_ms = HAL_GetTick();
  uint32_t age_ms = now_ms - out_cmd->gen_tick_ms;

  // 3. 判定是否超时
  if (age_ms > timeout_ms) {
    return false; // 🚨 指令太老了，算法层可能死机了！
  }

  return true; // ✅ 指令新鲜，可以发送
}