/*
    ctrl_gimbal Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/ahrs.h"
#include "module/gimbal.h"
#include "module/config.h"
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Gimbal_t gimbal;
Gimbal_IMU_t gimbal_imu;
Gimbal_CMD_t gimbal_cmd;
/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_ctrl_gimbal(void *argument) {
  (void)argument;
  const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_GIMBAL_FREQ;
  osDelay(CTRL_GIMBAL_INIT_DELAY);
  uint32_t tick = osKernelGetTickCount();

  Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
  
  /* 缓存最近一次收到的有效视觉数据 */
  static Gimbal_Vision_t last_vision_data = {0}; 
  
  /* 新增：记录最后一次收到有效视觉数据的时间戳 */
  static uint32_t last_vision_update_tick = 0; 
  /* 新增：视觉断线超时阈值，此处设置为 100ms */
  const uint32_t vision_timeout_ticks = osKernelGetTickFreq() / 10; 
  
  while (1) {
    tick += delay_tick;

    /* 1. 更新 IMU 数据 */
    if (osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0) == osOK) {
      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
    }

    /* 2. 更新基础控制指令（这会覆盖全局 gimbal_cmd，丢失其内部的 vision 数据） */
    osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);

    /* 3. 获取并缓存 AI 视觉信号，增加超时判断逻辑 */
    Gimbal_Vision_t current_vision;
    if (osMessageQueueGet(task_runtime.msgq.vision.data, &current_vision, NULL, 0) == osOK) {
      last_vision_data = current_vision; /* 有新视觉包时更新缓存 */
      last_vision_update_tick = osKernelGetTickCount(); /* 更新收到数据的时间戳 */
    } else {
      /* 未收到新数据时，检查是否超时断线 */
      if ((osKernelGetTickCount() - last_vision_update_tick) > vision_timeout_ticks) {
        last_vision_data.target_found = 0; /* 超时强制视为目标丢失 */
        /* 将缓存的 yaw/pit 角度清零，避免残留脏数据干扰 */
        last_vision_data.yaw = 0.0f;
        last_vision_data.pit = 0.0f;  
      }
    }

    /* 4. 强制将缓存的视觉数据挂载到当前的控制指令中 */
    gimbal_cmd.vision = last_vision_data;

    /* 5. 执行云台计算与输出 */
    Gimbal_UpdateFeedback(&gimbal);
    osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
    
    Gimbal_Control(&gimbal, &gimbal_cmd);
    Gimbal_Output(&gimbal);

    osDelayUntil(tick);
  }
}