/*
    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 ---------------------------------------------------------- */
extern uint8_t AI_mode;      // AI 模式变量，供其他模块查询当前AI控制模式
#define MAX_RX_BUF_SIZE 128 // 或 (sizeof(VisionToGimbal_t) * 2)
// MCU 数据结构（MCU -> 上位机）
typedef struct __attribute__((packed)) {
    uint8_t head[2];         // {'M', 'R'}
    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;   // 子弹累计发送次数
    uint16_t crc16;
} GimbalToVision_t;
Gimbal_Vision_t vision_data1;
// 确保结构体大小符合要求 (C11 标准)
#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
_Static_assert(sizeof(GimbalToVision_t) <= 64, "GimbalToVision_t size exceeds 64 bytes");
#endif

// AI 控制数据结构（上位机 -> MCU）
typedef struct __attribute__((packed)) {
    uint8_t head[2];         // {'M', 'R'}
    uint8_t mode;            // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
    float yaw;               // 目标偏航角/偏移量
    float yaw_vel;           // 偏航角速度
    float yaw_acc;           // 偏航角加速度
    float pitch;             // 目标俯仰角/偏移量
    float pitch_vel;         // 俯仰角速度
    float pitch_acc;         // 俯仰角加速度
    uint16_t crc16;
} VisionToGimbal_t;

#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
_Static_assert(sizeof(VisionToGimbal_t) <= 64, "VisionToGimbal_t size exceeds 64 bytes");
#endif

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

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

/**
 * @brief  AI串口空闲中断回调函数 (适配无参 BSP 架构)
 */
static void AI_RxIdleCallback(void) {
    /* 1. 获取本次空闲中断触发时的实际接收长度 */
    uint16_t Size = BSP_UART_GetRxCount(BSP_UART_AI, MAX_RX_BUF_SIZE);
    
    /* 2. 必须手动停止当前 DMA，以便在函数末尾重新对齐接收指针 */
    HAL_UART_DMAStop(BSP_UART_GetHandle(BSP_UART_AI));

    /* 3. 长度校验：严格匹配结构体长度 */
    if (Size != sizeof(VisionToGimbal_t)) {
        goto RESTART_RX;
    }

    /* 4. 包头校验 */
    if (ai_rx_buf[0] != 'M' || ai_rx_buf[1] != 'R') {
        goto RESTART_RX;
    }

    /* 5. CRC校验 */
    if (!CRC16_Verify(ai_rx_buf, Size)) {
        goto RESTART_RX;
    }

    /* --- 下方保留原有的解析和入队逻辑 --- */
    VisionToGimbal_t *p_rx_data = (VisionToGimbal_t *)ai_rx_buf;
    Gimbal_Vision_t vision_data;
    memset(&vision_data, 0, sizeof(Gimbal_Vision_t));
    
    uint8_t is_valid_packet = 1;

    switch (p_rx_data->mode) {
        case 0:
            vision_data.target_found = 0;
						__NOP();
            break;
        case 1:
        case 2:
            vision_data.target_found = 1;
            vision_data.yaw      = p_rx_data->yaw;
            vision_data.pit      = p_rx_data->pitch;
            vision_data.yaw_v_ff = p_rx_data->yaw_vel;
            vision_data.pit_v_ff = p_rx_data->pitch_vel;
            break;
        default:
            is_valid_packet = 0; 
            break;
    }
		
    if (is_valid_packet) {
        osMessageQueuePut(task_runtime.msgq.vision.data, &vision_data, 0, 0);
				vision_data1 = vision_data;
    }

RESTART_RX:
    /* 6. 重新开启空闲中断定长接收 */
    BSP_UART_Receive_IDLE(BSP_UART_AI, ai_rx_buf, MAX_RX_BUF_SIZE);
}
/**
 * @brief  向上位机发送MCU状态数据
 */
static void AI_Send_MCU_Data(void) {
    AHRS_Quaternion_t current_quat;

    memset(&tx_pkg, 0, sizeof(GimbalToVision_t));
    
    /* 写入包头 */
    tx_pkg.head[0] = 'M';
    tx_pkg.head[1] = 'R';
    
    /* 1. 获取四元数 (从消息队列读取) */
    if (osMessageQueueGet(task_runtime.msgq.ai.quat, &current_quat, NULL, 0) == osOK) {
        tx_pkg.q[0] = current_quat.q0;
        tx_pkg.q[1] = current_quat.q1;
        tx_pkg.q[2] = current_quat.q2;
        tx_pkg.q[3] = current_quat.q3;
    }

    /* 2. 获取云台状态 (直接读取全局 gimbal 结构体) */
    tx_pkg.yaw = gimbal.feedback.imu.eulr.yaw;
    tx_pkg.yaw_vel = gimbal.feedback.imu.gyro.z;
    tx_pkg.pitch = gimbal.feedback.imu.eulr.rol; /* 电控中 Pitch 对应 IMU 的 Rol */
    tx_pkg.pitch_vel = gimbal.feedback.imu.gyro.y;
    
    /* 3. 获取发射机构与模式状态 (直接读取全局 shoot 结构体) */
    /* 模式回传，取值为实际枚举映射 */
    tx_pkg.mode = AI_mode; /* 直接回传当前AI模式，供上位机显示 */
    
    /* 弹速反馈：暂用摩擦轮目标转速或估算转速代替。若接入裁判系统，应替换为真实弹速。 */
    tx_pkg.bullet_speed = shoot.target_variable.target_rpm; 
    
    /* 弹量统计：如有独立计数变量，可进行赋值 */
    tx_pkg.bullet_count = -1;
    
    /* 4. 计算 CRC16（扣除末尾 2 字节） */
    tx_pkg.crc16 = CRC16_Calc((const uint8_t *)&tx_pkg, sizeof(GimbalToVision_t) - 2, CRC16_INIT);
    
    /* 5. 通过 DMA 发送至上位机 */
    BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&tx_pkg, sizeof(GimbalToVision_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 已添加 IDLE 中断回调对应的枚举 BSP_UART_IDLE_CB
		BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB, AI_RxIdleCallback);
    BSP_UART_Receive(BSP_UART_AI, ai_rx_buf, sizeof(VisionToGimbal_t), true);
    /* USER CODE INIT END */
  
    while (1) {
        tick += delay_tick; 
        
        /* USER CODE BEGIN */
        /* 定期向上位机发送包含姿态、云台、射击反馈的 MCU 状态包 */
        AI_Send_MCU_Data();
        /* USER CODE END */
        
        osDelayUntil(tick); 
    }
}