/* =============================================================================
 * chassis_task.c - 底盘运动控制(2ms)
 * 状态机:
 *   F_WAIT    等待"底盘姿态就绪+四轮在线+云台自检结果下发(0x1A9)"
 *   F_RUN     正常模式控制(跟随/小陀螺/停车;符号用参数回退值直至自检完成)
 *   F_DETECT  跟随符号自检——**仅由操作手手势显式触发,绝不自动执行**:
 *             遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
 *             检测给四轮约 5% 电流脉冲 80ms,落地时整车轻转一下,操作手
 *             应确认车周围无人后再做手势。失败重试 3 次后回退参数符号。
 * 功率限制:缓冲能量不足时按比例缩放输出(裁判系统 0x0202)
 * SHIFT:临时速度增益(超电驱动 TODO,接入后替换)
 * ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "robot_typedef.h"
#include "chassis.h"
#include "attitude.h"
#include "referee.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
{
    F_WAIT = 0,
    F_RUN,
    F_DETECT,
} follow_state_e;

void chassis_task(void const * argument)
{
    TickType_t last_wake = xTaskGetTickCount();
    follow_state_e state = F_WAIT;
    uint8_t detect_retry = 0;
    uint16_t gesture_cnt = 0;

    while (!app_ready)
        osDelay(2);

    for (;;)
    {
        /* 校准手势检测(遥控在线=ctoc_alive 时才可能成立;摇杆值来自 0x149 转发) */
        if (cmd_robot.online &&
            ctoc_remote.rc_l_rl < CALIB_GESTURE_L_RL && ctoc_remote.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 F_WAIT:
                chassis_output_zero();
                /* 自身就绪即可进入运行(符号先用参数回退值) */
                if (!chassis_motor_offline() && att_ready)
                    state = F_RUN;
                break;

            case F_DETECT:
                if (chassis_detect_follow_sign())
                {
                    chassis_follow_reset();  // 检测期间车被动转过,重锁基准
                    state = F_RUN;
                }
                else if (++detect_retry >= 3)
                {
                    detect_retry = 0;
                    chassis_follow_reset();
                    state = F_RUN;  // 回退参数符号
                }
                else
                    chassis_output_zero();
                break;

            case F_RUN:
            default:
                if (gesture && !chassis_motor_offline() && att_ready && ctoc_encoder_ccw != 0)
                {
                    detect_retry = 0;
                    chassis_output_zero();
                    chassis_follow_reset();
                    state = F_DETECT;  // 操作手主动请求自检
                }
                else if (cmd_robot.online && !chassis_motor_offline())
                {
                    float w = 0;

                    switch (cmd_robot.chassis_mode)
                    {
                        case CHASSIS_MODE_SPIN_SLOW:
                            w = MECANUM_SPIN_SLOW_W;
                            break;

                        case CHASSIS_MODE_SPIN_FAST:
                            w = MECANUM_SPIN_FAST_W;
                            break;

                        case CHASSIS_MODE_FOLLOW:
                        default:
                            w = chassis_follow_w();  // 编码器相对角,零漂移
                            break;
                    }

                    /* SHIFT 加速(超电接入前的临时方案,受功率限制约束) */
                    float boost = 1.0f;
                    if (ctoc_remote.shift_push)
                        boost = MECANUM_SHIFT_BOOST_RATE;
                    float vx = cmd_robot.vx * boost;
                    float vy = cmd_robot.vy * boost;

                    /* 简易功率限制:缓冲能量线性映射为缩放系数 */
                    float scale = 1.0f;
                    if (referee.buffer_energy < POWER_LIMIT_BUFFER_LOW)
                        scale = (referee.buffer_energy - POWER_LIMIT_BUFFER_MIN) /
                                (POWER_LIMIT_BUFFER_LOW - POWER_LIMIT_BUFFER_MIN);
                    if (scale < 0.0f)
                        scale = 0.0f;
                    else if (scale > 1.0f)
                        scale = 1.0f;

                    chassis_control(vx, vy, w, scale);
                }
                else
                {
                    chassis_follow_reset();
                    chassis_output_zero();
                }
                break;
        }

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