/* =============================================================================
 * shoot_detect_task.c - 摩擦轮电流/电压波动发射检测实现
 * 前提条件: 摩擦轮开启且加速稳定后(STATE_ACTIVE)才进行发射检测判定
 * ========================================================================== */
#include "shoot_detect_task.h"
#include "gimbal.h"
#include "shoot.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include <math.h>

static shoot_detect_t g_shoot_detect;

void shoot_detect_init(void)
{
    g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
    g_shoot_detect.is_ready = 0;
    g_shoot_detect.spinup_start_tick = 0;

    g_shoot_detect.shoot_count = 0;
    g_shoot_detect.fired_pulse = 0;
    g_shoot_detect.shoot_freq_hz = 0.0f;
    g_shoot_detect.last_shoot_tick = 0;

    g_shoot_detect.current_l = 0.0f;
    g_shoot_detect.current_r = 0.0f;
    g_shoot_detect.baseline_l = 0.0f;
    g_shoot_detect.baseline_r = 0.0f;
    g_shoot_detect.delta_l = 0.0f;
    g_shoot_detect.delta_r = 0.0f;
    g_shoot_detect.delta_max = 0.0f;

    g_shoot_detect.threshold_a = SHOOT_DETECT_CURRENT_THRESHOLD_A;
    g_shoot_detect.cooldown_ms = SHOOT_DETECT_COOLDOWN_MS;
    g_shoot_detect.speed_min_rpm = SHOOT_DETECT_SPEED_MIN_RPM;
    g_shoot_detect.spinup_timeout_ms = SHOOT_DETECT_SPINUP_TIMEOUT_MS;
}

void shoot_detect_update(void)
{
    uint32_t now = HAL_GetTick();

    /* 检查摩擦轮电机在线状态 */
    if (motor_friction_l.fdb.last_online_tick == 0 ||
        motor_friction_r.fdb.last_online_tick == 0 ||
        (now - motor_friction_l.fdb.last_online_tick > 100) ||
        (now - motor_friction_r.fdb.last_online_tick > 100))
    {
        g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
        g_shoot_detect.is_ready = 0;
        return;
    }

    /* 核心前提: 摩擦轮必须处于启动开启状态 (shoot_friction_flag == 1) */
    if (!shoot_friction_flag)
    {
        /* 摩擦轮未启动: 强制处于 STOPPED 状态, 复位所有检测信号 */
        g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
        g_shoot_detect.is_ready = 0;
        g_shoot_detect.current_l = 0.0f;
        g_shoot_detect.current_r = 0.0f;
        g_shoot_detect.delta_l = 0.0f;
        g_shoot_detect.delta_r = 0.0f;
        g_shoot_detect.delta_max = 0.0f;
        g_shoot_detect.baseline_l = 0.0f;
        g_shoot_detect.baseline_r = 0.0f;
        return;
    }

    /* 读取当前两轮绝对电流(A)与实际转速(RPM) */
    float cur_l = fabsf(motor_friction_l.fdb.torque_current);
    float cur_r = fabsf(motor_friction_r.fdb.torque_current);
    float spd_l = fabsf((float)motor_friction_l.fdb.rotor_speed);
    float spd_r = fabsf((float)motor_friction_r.fdb.rotor_speed);

    g_shoot_detect.current_l = cur_l;
    g_shoot_detect.current_r = cur_r;

    /* ---------------- 状态机流转 ---------------- */
    switch (g_shoot_detect.state)
    {
        case SHOOT_DETECT_STATE_STOPPED:
            /* 摩擦轮刚刚开启, 记录起转时刻并进入起转加速过渡期 */
            g_shoot_detect.spinup_start_tick = now;
            g_shoot_detect.state = SHOOT_DETECT_STATE_SPINUP;
            g_shoot_detect.is_ready = 0;
            break;

        case SHOOT_DETECT_STATE_SPINUP:
            /* 加速过渡期: 屏蔽电机刚启动加速时的满幅大电流,
             * 等待起转时间满足(默认 500ms) 且 双轮转速均达到额定工作点(默认 4500 RPM) */
            if ((now - g_shoot_detect.spinup_start_tick >= g_shoot_detect.spinup_timeout_ms) &&
                (spd_l >= g_shoot_detect.speed_min_rpm) &&
                (spd_r >= g_shoot_detect.speed_min_rpm))
            {
                /* 捕获稳态旋转初始基线, 正式就绪激活检测 */
                g_shoot_detect.baseline_l = cur_l;
                g_shoot_detect.baseline_r = cur_r;
                g_shoot_detect.state = SHOOT_DETECT_STATE_ACTIVE;
                g_shoot_detect.is_ready = 1;
            }
            break;

        case SHOOT_DETECT_STATE_ACTIVE:
            /* 正常工作检测期: 检查转速是否异常跌落过低(如严重堵转或掉电) */
            if (spd_l < (g_shoot_detect.speed_min_rpm - 1000.0f) ||
                spd_r < (g_shoot_detect.speed_min_rpm - 1000.0f))
            {
                /* 转速异常跌落, 退回起转过渡等待重新稳定 */
                g_shoot_detect.spinup_start_tick = now;
                g_shoot_detect.state = SHOOT_DETECT_STATE_SPINUP;
                g_shoot_detect.is_ready = 0;
                break;
            }

            /* 计算两轮当前电流相对于空载基线的突变量 ΔI */
            g_shoot_detect.delta_l = cur_l - g_shoot_detect.baseline_l;
            g_shoot_detect.delta_r = cur_r - g_shoot_detect.baseline_r;

            float delta_l_valid = (g_shoot_detect.delta_l > 0.0f) ? g_shoot_detect.delta_l : 0.0f;
            float delta_r_valid = (g_shoot_detect.delta_r > 0.0f) ? g_shoot_detect.delta_r : 0.0f;
            g_shoot_detect.delta_max = (delta_l_valid > delta_r_valid) ? delta_l_valid : delta_r_valid;

            /* 自适应基线平滑更新:
             * 仅在电流未出现大波动(处于空载稳态区间)时平滑更新基线,
             * 避免弹丸过轮脉冲把基线强行拉高 */
            if (g_shoot_detect.delta_max < g_shoot_detect.threshold_a * 0.6f)
            {
                g_shoot_detect.baseline_l += SHOOT_DETECT_BASELINE_ALPHA * (cur_l - g_shoot_detect.baseline_l);
                g_shoot_detect.baseline_r += SHOOT_DETECT_BASELINE_ALPHA * (cur_r - g_shoot_detect.baseline_r);
            }

            /* 发射事件波峰判定 */
            if (g_shoot_detect.delta_max >= g_shoot_detect.threshold_a)
            {
                /* 防抖不应期(Cooldown): 单发过轮持续 5~15ms,
                 * 冷却时间内不重复计数, 确保单发只计一次 */
                if (now - g_shoot_detect.last_shoot_tick >= g_shoot_detect.cooldown_ms)
                {
                    uint32_t dt = now - g_shoot_detect.last_shoot_tick;
                    if (dt > 0 && dt <= 1000)
                    {
                        g_shoot_detect.shoot_freq_hz = 1000.0f / (float)dt;
                    }
                    else if (dt > 1000)
                    {
                        g_shoot_detect.shoot_freq_hz = 1.0f;
                    }

                    g_shoot_detect.last_shoot_tick = now;
                    g_shoot_detect.shoot_count++;
                    g_shoot_detect.fired_pulse = 1;
                }
            }
            break;
    }
}

uint8_t shoot_detect_is_ready(void)
{
    return g_shoot_detect.is_ready;
}

shoot_detect_state_e shoot_detect_get_state(void)
{
    return g_shoot_detect.state;
}

uint8_t shoot_detect_has_fired(void)
{
    if (g_shoot_detect.fired_pulse)
    {
        g_shoot_detect.fired_pulse = 0;
        return 1;
    }
    return 0;
}

uint32_t shoot_detect_get_count(void)
{
    return g_shoot_detect.shoot_count;
}

void shoot_detect_clear_count(void)
{
    g_shoot_detect.shoot_count = 0;
}

float shoot_detect_get_freq(void)
{
    uint32_t now = HAL_GetTick();
    /* 若超过 1s 无新发射, 瞬时射频归零 */
    if (now - g_shoot_detect.last_shoot_tick > 1000)
    {
        g_shoot_detect.shoot_freq_hz = 0.0f;
    }
    return g_shoot_detect.shoot_freq_hz;
}

const shoot_detect_t * shoot_detect_get_data(void)
{
    return &g_shoot_detect;
}

void shoot_detect_set_threshold(float threshold_a)
{
    if (threshold_a > 0.1f)
    {
        g_shoot_detect.threshold_a = threshold_a;
    }
}

void shoot_detect_task(void const * argument)
{
    (void)argument;
    shoot_detect_init();

    extern volatile uint8_t app_ready;
    while (!app_ready)
        osDelay(2);

    TickType_t last_wake = xTaskGetTickCount();
    for (;;)
    {
        /* 前提: 若摩擦轮未开启, 低频 10ms 休眠等待, 不占用密集 CPU 计算 */
        if (!shoot_friction_flag)
        {
            shoot_detect_update();
            osDelay(10);
            last_wake = xTaskGetTickCount();
            continue;
        }

        /* 摩擦轮开启中, 以 2ms 高精度周期进行波动检测 */
        shoot_detect_update();
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
    }
}