/*
    ai Task
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/ai.h"
#include "module/gimbal.h"
/* USER INCLUDE END */

/* Private variables -------------------------------------------------------- */
PackageMCU_t mcu;
PackageReferee_t ref_pkg;
AI_result_t ai_result;
Referee_ForAI_t ref_for_ai;
AHRS_Quaternion_t quat;
Gimbal_feedback_t gimbal_motor;
PackageMCU_t shoot_mcu_package;

/* Exported functions ------------------------------------------------------- */
void Task_ai(void *argument) {
  (void)argument;

  const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
  osDelay(AI_INIT_DELAY);

  /* 初始化AI通信：注册IDLE回调 + 启动DMA接收 */
  AI_Init();

  uint32_t tick = osKernelGetTickCount();

  while (1) {
    tick += delay_tick;

    /* 获取其他任务传来的数据 */
    osMessageQueueGet(task_runtime.msgq.gimbal.ai.quat, &quat, NULL, 0);
    osMessageQueueGet(task_runtime.msgq.gimbal.ai.feedback, &gimbal_motor, NULL, 0);
    osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd_ai_bool_count, &shoot_mcu_package, NULL, 0);
    osMessageQueueGet(task_runtime.msgq.referee.ai, &ref_for_ai, NULL, 0);

    /* 发送MCU数据给上位机 */
    mcu.data.bullet_count = shoot_mcu_package.data.bullet_count;
    MCU_Send(&mcu, &gimbal_motor, &quat);
    MCU_StartSend(&mcu);

    /* 发送裁判系统数据给上位机 */
    if (REF_Send(&ref_pkg, &ref_for_ai.robot_status, &ref_for_ai.game_status) == 0) {
      REF_StartSend(&ref_pkg);
    }

    /* 非阻塞获取上位机最新数据（IDLE回调已在中断中解析） */
    AI_GetLatest(&ai_result);

    /* 分发AI结果给各任务 */
    osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
    osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_result, 0, 0);
    osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_result, 0, 0);
    osMessageQueuePut(task_runtime.msgq.navi.c_cmd, &ai_result, 0, 0);

    osDelayUntil(tick);
  }
}