/* =============================================================================
 * gimbal_task.c - 云台控制任务(2ms)
 * 状态机:
 *   YAW_WAIT   等待"姿态就绪+云台电机在线"(遥控可后到)
 *   YAW_RUN    正常串级控制(符号用参数回退值,直至手势自检完成)
 *   YAW_DETECT 装配符号自检——**仅由操作手手势显式触发,绝不自动执行**:
 *              遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
 *              检测期间云台会轻转几度(约 20% 电压脉冲 60ms),操作手应
 *              确认云台周围无人后再做手势。失败重试 3 次后回退参数符号。
 * 任意门控失效(遥控离线/电机掉线/姿态不可用/底盘故障)立即清零输出
 * 拨弹电流由 shoot_task 写入共享变量,本任务与 Pitch 共帧发送(0x1FF CAN1)
 * ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "robot_typedef.h"
#include "gimbal.h"
#include "bmi088.h"
#include "attitude.h"
#include "dr16.h"
#include "c_comm.h"
#include "robot_param.h"

/* 校准手势:左摇杆推最左 + 右摇杆推最右(正常操控不可能误触) */
#define CALIB_GESTURE_L_RL   400   // < 400 即最左
#define CALIB_GESTURE_R_RL   1600  // > 1600 即最右
#define CALIB_GESTURE_MS     500   // 保持时长
#define CALIB_GESTURE_TICKS  (CALIB_GESTURE_MS / 2)  // 2ms 任务

typedef enum
{
    YAW_WAIT = 0,
    YAW_RUN,
    YAW_DETECT,
} yaw_state_e;

void gimbal_task(void const * argument)
{
    TickType_t last_wake = xTaskGetTickCount();
    yaw_state_e state = YAW_WAIT;
    uint8_t detect_retry = 0;
    uint16_t gesture_cnt = 0;
    uint8_t force_reset = 0;

    while (!app_ready)
        osDelay(2);

    for (;;)
    {
        const uint8_t gates_ok = (cmd_robot.online && !gimbal_motor_offline() &&
                                  bmi088_online && att_ready && !ctoc_chassis_fault());

        /* 校准手势检测(仅遥控在线时可能成立;500 个采样窗防抖) */
        if (cmd_robot.online && !gimbal_motor_offline() &&
            dr16.rc_l_RL < CALIB_GESTURE_L_RL && dr16.rc_r_RL > CALIB_GESTURE_R_RL)
        {
            if (gesture_cnt < CALIB_GESTURE_TICKS)
                gesture_cnt++;
        }
        else
            gesture_cnt = 0;
        const uint8_t gesture = (gesture_cnt >= CALIB_GESTURE_TICKS);
        if (gesture)
            gesture_cnt = 0;

        switch (state)
        {
            case YAW_WAIT:
                gimbal_output_zero();
                /* 姿态与电机就绪即可进入运行(符号先用参数回退值) */
                if (!gimbal_motor_offline() && bmi088_online && att_ready)
                {
                    force_reset = 1;  // 首次控制同步目标角
                    state = YAW_RUN;
                }
                break;

            case YAW_DETECT:
                if (gimbal_detect_yaw_sign())
                {
                    force_reset = 1;  // 检测期间云台被动转过,重锁目标
                    state = YAW_RUN;
                }
                else if (++detect_retry >= 3)
                {
                    detect_retry = 0;
                    force_reset = 1;
                    state = YAW_RUN;  // 回退参数符号
                }
                else
                    gimbal_output_zero();
                break;

            case YAW_RUN:
            default:
                if (gesture && !gimbal_motor_offline() && att_ready)
                {
                    detect_retry = 0;
                    state = YAW_DETECT;  // 操作手主动请求自检
                }
                else if (gates_ok)
                {
                    gimbal_control(cmd_robot.gimbal_yaw_delta,
                                   cmd_robot.gimbal_pitch_delta,
                                   (uint8_t)(force_reset || cmd_robot.gimbal_reset));
                    force_reset = 0;
                }
                else
                {
                    gimbal_output_zero();
                    force_reset = 1;
                    state = YAW_WAIT;  // 门控失效回等待,恢复后重锁目标
                }
                break;
        }

        monitor_heartbeat(HB_GIMBAL);
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
    }
}