/*
    ai Task
    功能：
    1. 接收姿态任务 (atti_esti) 发送的四元数。
    2. 直接读取全局云台(gimbal)与发射机构(shoot)状态，打包发送给上位机。
    3. 解析上位机指令并转化为系统的云台与发射控制信号。
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include <stdint.h>
#include <string.h>
#include "bsp/uart.h"
#include "component/crc16.h"
#include "module/gimbal.h"  /* 引用云台结构体获取反馈 */
#include "module/shoot.h"   /* 引用发射机构结构体获取反馈 */
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
#define ID_MCU (0xC4)  // MCU 数据包 (发送)
#define ID_REF (0xA8)  // 裁判系统数据包 (发送)
#define ID_AI  (0xB5)  // AI 控制数据包 (接收)

// MCU 数据结构（MCU -> 上位机）
typedef struct __attribute__((packed)) {
    uint8_t mode;           // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
    float q[4];             // 四元数 wxyz 顺序
    float yaw;              // 偏航角
    float yaw_vel;          // 偏航角速度
    float pitch;            // 俯仰角
    float pitch_vel;        // 俯仰角速度
    float bullet_speed;     // 弹速
    uint16_t bullet_count;  // 子弹累计发送次数
} DataMCU_t;

typedef struct __attribute__((packed)) {
    uint8_t id;             // 数据包 ID: 0xC4
    DataMCU_t data;
    uint16_t crc16;
} PackageMCU_t;

// AI 控制数据结构（上位机 -> MCU）
typedef struct __attribute__((packed)) {
    uint8_t mode;        // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
    float yaw;           // 目标偏航角/偏移量
    float yaw_vel;       // 偏航角速度
    float yaw_acc;       // 偏航角加速度
    float pitch;         // 目标俯仰角/偏移量
    float pitch_vel;     // 俯仰角速度
    float pitch_acc;     // 俯仰角加速度
    float vx;            // X 方向速度
    float vy;            // Y 方向速度
    float wz;            // Z 方向角速度
    uint8_t reserved;    // 预留字段
} DataAI_t;

typedef struct __attribute__((packed)) {
    uint8_t id;          // 数据包 ID: 0xB5
    DataAI_t data;
    uint16_t crc16;
} PackageAI_t;

/* Private variables -------------------------------------------------------- */
static uint8_t ai_rx_buf[sizeof(PackageAI_t)]; 
static PackageAI_t ai_rx_data;                

/* 声明外部全局变量，用于直接获取高频反馈数据 */
extern Gimbal_t gimbal;
extern Shoot_t shoot;

/* USER FUNCTION BEGIN */

/**
 * @brief  AI串口接收完成回调函数
 * @note   验证包头ID与CRC16校验，校验通过后更新控制指令，并重新开启DMA接收
 */
static void AI_RxCpltCallback(void) {
    if (ai_rx_buf[0] == ID_AI) {
        /* 校验 CRC，确保数据包完整 */
        if (CRC16_Verify(ai_rx_buf, sizeof(PackageAI_t))) {
            memcpy(&ai_rx_data, ai_rx_buf, sizeof(PackageAI_t));
            
            Gimbal_CMD_t ai_gimbal_cmd;
            Shoot_CMD_t ai_shoot_cmd;
            
            /* 1. 解析指令并生成系统控制信号 */
            switch (ai_rx_data.data.mode) {
                case 0: /* 不控制 */
                    ai_gimbal_cmd.mode = GIMBAL_MODE_RELAX;
                    // ai_shoot_cmd.mode = SHOOT_MODE_STOP; /* 需根据实际枚举名称修改 */
                    break;
                case 1: /* 控制云台但不开火 */
                    ai_gimbal_cmd.mode = GIMBAL_MODE_ABSOLUTE;
                    // ai_shoot_cmd.mode = SHOOT_MODE_READY; /* 摩擦轮开启，不拨弹 */
                    break;
                case 2: /* 控制云台且开火 */
                    ai_gimbal_cmd.mode = GIMBAL_MODE_ABSOLUTE;
                    // ai_shoot_cmd.mode = SHOOT_MODE_FIRE;  /* 摩擦轮开启，拨弹 */
                    break;
                default:
                    ai_gimbal_cmd.mode = GIMBAL_MODE_RELAX;
                    break;
            }

            /* 云台位姿控制信号赋值 (假设协议中发下来的 yaw/pitch 直接作为速度增量) */
            ai_gimbal_cmd.delta_yaw = ai_rx_data.data.yaw;
            ai_gimbal_cmd.delta_pit = ai_rx_data.data.pitch;
            
            /* 2. 将命令压入控制队列（非阻塞） */
            osMessageQueuePut(msgq.gimbal.cmd, &ai_gimbal_cmd, 0, 0);
            
            // 注意：若 ctrl_shoot.c 中使用的是 task_runtime.msgq.shoot.shoot_cmd，
            // 则替换为该全局结构体的队列句柄。此处采用常规 msgq 定义示范：
            osMessageQueuePut(msgq.shoot.shoot_cmd, &ai_shoot_cmd, 0, 0);
        }
    }
    /* 重新开启DMA定长接收 */
    BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(PackageAI_t), true);
}

/**
 * @brief  向上位机发送MCU状态数据
 */
static void AI_Send_MCU_Data(void) {
    PackageMCU_t tx_pkg;
    quat_t current_quat;

    tx_pkg.id = ID_MCU;
    memset(&tx_pkg.data, 0, sizeof(DataMCU_t));
    
    /* 1. 获取四元数 (从消息队列读取) */
    if (osMessageQueueGet(msgq.ai.quat, &current_quat, NULL, 0) == osOK) {
        tx_pkg.data.q[0] = current_quat.q0;
        tx_pkg.data.q[1] = current_quat.q1;
        tx_pkg.data.q[2] = current_quat.q2;
        tx_pkg.data.q[3] = current_quat.q3;
    }

    /* 2. 获取云台状态 (直接读取全局 gimbal 结构体) */
    tx_pkg.data.yaw = gimbal.feedback.imu.eulr.yaw;
    tx_pkg.data.yaw_vel = gimbal.feedback.imu.gyro.z;
    tx_pkg.data.pitch = gimbal.feedback.imu.eulr.rol; /* 电控中 Pitch 对应 IMU 的 Rol */
    tx_pkg.data.pitch_vel = gimbal.feedback.imu.gyro.y;
    
    /* 3. 获取发射机构与模式状态 (直接读取全局 shoot 结构体) */
    /* 模式回传，取值为实际枚举映射 */
    tx_pkg.data.mode = (uint8_t)shoot.mode; 
    
    /* 弹速反馈：由于硬件可能无法直接测弹速，暂用摩擦轮目标转速或估算转速代替。
       若接入了裁判系统，应替换为裁判系统下发的真实弹速。 */
    tx_pkg.data.bullet_speed = shoot.target_variable.target_rpm; 
    
    /* 弹量统计：如有独立计数变量，可进行赋值 */
    // tx_pkg.data.bullet_count = shoot.feedback.xxxx; 

    /* 4. 计算 CRC16（扣除末尾 2 字节） */
    tx_pkg.crc16 = CRC16_Calc((const uint8_t *)&tx_pkg, sizeof(PackageMCU_t) - 2, CRC16_INIT);
    
    /* 5. 通过 DMA 发送至上位机 */
    BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&tx_pkg, sizeof(PackageMCU_t), true);
}

/* USER FUNCTION END */

void Task_ai(void *argument) {
    (void)argument; 
    
    /* 计算任务运行到指定频率需要等待的tick数 */
    const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
    osDelay(AI_INIT_DELAY); 
    uint32_t tick = osKernelGetTickCount(); 
  
    /* USER CODE INIT BEGIN */
    /* 注册串口接收完成回调并启动第一次DMA接收 */
    BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, AI_RxCpltCallback);
    BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(PackageAI_t), true);
    /* USER CODE INIT END */
  
    while (1) {
        tick += delay_tick; 
        
        /* USER CODE BEGIN */
        /* 定期向上位机发送包含姿态、云台、射击反馈的 MCU 状态包 */
        AI_Send_MCU_Data();
        /* USER CODE END */
        
        osDelayUntil(tick); 
    }
}