#include "TaskLoop.h"
#include "at32f421_board.h"
#include "cmcng.h"
#include "FreeRTOS.h"
#include "task.h"
#include <stdlib.h>

/*===========================================================================
 * 灵敏度表 — 对齐 M1H 参考实现
 * 进入阈值 = Origin × SensTable[SENS] / 65536
 * 离开阈值（滞回）= Origin × SensTable_1[SENS] / 65536
 * SENS: 0=低灵敏, 3=高灵敏
 *===========================================================================*/
const uint16_t SensTable[4]   = {216, 108,  36,  10};
const uint16_t SensTable_1[4] = {108,  72,  18,   9};

/*===========================================================================
 * 全局状态变量 — 捕获 & 测量
 *===========================================================================*/
uint32_t g_sys_freq   = 0;
uint8_t  g_input_div  = 1;

uint16_t loop1_Xn;
uint16_t loop1_CapThis;
uint16_t loop1_CapLast;
uint16_t loop1_LPCNT;
uint16_t loop1_CapCnt;
uint32_t loop1_CapSum;
uint32_t loop1_Value;
uint32_t loop1_CAPVD;
uint32_t loop1_Origin;
uint32_t loop1_ORG_SUM;
uint16_t loop1_ORG_CNT;
uint16_t loop1_dlt_ORG;
uint8_t  Flt_Reg;

/* M4 优化: 快速 IIR + 进入确认 */
uint32_t loop1_CAPVD_fast;
uint8_t  loop1_entry_cnt;
uint16_t loop1_freeze_cnt;
uint32_t loop1_freeze_ref;

/*===========================================================================
 * 全局状态变量 — 标志位
 *===========================================================================*/
uint8_t  loop1_INI_LOOP;
uint8_t  loop1_CAP_OK;
uint8_t  loop1_VD_FLAG;
uint8_t  loop1_VD_HOLD;
uint8_t  loop1_RF_FLAG;
uint8_t  loop1_LOOP_OK;
uint8_t  loop1_LOOP_OK0;
uint8_t  loop1_FLAG_IN;
uint8_t  loop1_FLAG_OUT;
uint8_t  loop1_FLAG_PLUSE;
uint8_t  loop1_SensLevel;
uint8_t  loop1_cnt_release;
uint8_t  g_loop_stable;
#if USE_FLATNESS_EXIT
uint8_t  g_exit_state;
uint16_t g_max_slope;
uint16_t g_max_slope_rate;
uint16_t g_delta2;
uint16_t g_delta3;
int32_t  g_prev_capvd;
int32_t  g_prev_first_deriv;
uint8_t  g_slope_flat_cnt;
uint8_t  g_flat_ok_cnt;
#endif

/*===========================================================================
 * 全局状态变量 — 计数器
 *===========================================================================*/
uint16_t Hold_CNT  = 0;
uint8_t  loop1_INCNT  = 0;
uint8_t  loop1_OUTCNT = 0;
uint8_t  TM1cnt = 0;

/*===========================================================================
 * 拨码开关状态
 *===========================================================================*/
uint8_t SET_PLUS = 0;
uint8_t SET_DLY  = 0;   // 离开延时: 0=无, 1=500ms
uint8_t SET_SAFE = 0;

/*===========================================================================
 * 拨码开关去抖变量
 *===========================================================================*/
uint8_t sw0, swok, sw1, swcnt, SENS = 0, SENS_LAST = 0;

/*===========================================================================
 * 安全复位相关
 *===========================================================================*/
uint8_t  loop1_LC_HOLD  = 0;
uint8_t  loop1_LC_Reset = 0;
uint32_t LC_Hold_CNT    = 0;
uint16_t g_safe_max_cnt = LC_HOLD_TIME;

/*===========================================================================
 * 故障指示相关 — 黄灯
 *===========================================================================*/
uint8_t  g_loop_power_up_state = 0;
uint8_t  g_disconnect_count    = 0;
uint8_t  g_disconnect_active   = 0;
uint8_t  g_fault_phase         = 0;
uint16_t g_fault_tick          = 0;

/*===========================================================================
 * 调试计数器
 *===========================================================================*/
uint32_t g_xn_counter = 0;

/*===========================================================================
 * 拨码开关轮询（5 次去抖，对齐 M1H）
 *===========================================================================*/
void poll_sw_state(void)
{
    sw0  = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN) << 4;
    sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN) << 3;
    sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN) << 2;
    sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN) << 1;
    sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN) << 0;

    if (sw0 == sw1) {
        if (swok != sw0) {
            swcnt++;
            if (swcnt > 5) {       // 对齐 M1H: 5 次确认
                swok = sw0;

                SET_PLUS = swok & 0x04;
                SET_DLY  = (swok & 0x08) >> 3;
                SET_SAFE = (swok & 0x10) >> 4;
            }
        }
    } else {
        sw1   = sw0;
        swcnt = 0;
    }

    SENS = swok & 0x03;
    if (SENS != SENS_LAST) {
        PRINT("SENS changed: %02X -> %02X, resetting...\n", SENS_LAST, SENS);
        nvic_system_reset();
    }
}

/*===========================================================================
 * 一阶 IIR 低通滤波器（对齐 M1H 公式）
 * CAPVD_new = α × new_value + (1-α) × CAPVD_old
 * α = Flt_Reg / 256 = 79/256 ≈ 0.3086
 *===========================================================================*/
uint32_t get_flt_value(uint32_t new_value, uint32_t last_Value)
{
    uint32_t value_Flt;
    uint32_t delta = (new_value > last_Value)
                     ? (new_value - last_Value)
                     : (last_Value - new_value);
    uint32_t scaled_delta = (delta * (uint32_t)Flt_Reg) >> 8;

    if (new_value > last_Value) {
        value_Flt = last_Value + scaled_delta;
    } else {
        value_Flt = last_Value - scaled_delta;
    }

    return value_Flt;
}

/*===========================================================================
 * 滑动平均基线更新
 * 累加 new_value，当计数达到 window 时计算平均值并重置
 * 返回 1 表示本轮更新了平均值
 *===========================================================================*/
uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt,
                              uint32_t* p_origin, uint32_t new_value,
                              uint8_t window)
{
    if (!p_sum || !p_cnt || !p_origin || window == 0) {
        return 0;
    }

    *p_sum += new_value;
    (*p_cnt)++;

    if (*p_cnt >= window) {
        *p_origin = *p_sum / window;
        *p_cnt = 0;
        *p_sum  = 0;
        return 1;
    }
    return 0;
}

/*===========================================================================
 * 红灯呼吸 (LEDA, PB1) — 每 5ms 由 TMR15 ISR 调用
 *===========================================================================*/
void LEDA_ON_OFF(void)
{
    LED_RED_GPIO->odt ^= LED_RED_PIN;
}

/*===========================================================================
 * 绿灯指示 — LEDA (PA9)
 *
 * 由 TMR15 ISR 每 5ms 驱动一次，是绿灯的唯一控制入口。
 *
 * 模式：
 *   自检/稳定中 (loop1_INI_LOOP || 线圈未连 || !g_loop_stable):
 *     → 慢闪 (200ms 亮 / 200ms 灭)
 *   正常工作:
 *     → 有车亮 (VD_FLAG==1), 无车灭
 *===========================================================================*/
void poll_green_led(void)
{
#define GREEN_SLOW_HALF   40      // 200ms (40 × 5ms)

    static uint16_t _slow_tick = 0;

    /*--- 线圈断开中 → 灭（黄灯快闪负责故障指示） ---*/
    if (g_disconnect_active) {
        LEDA_OFF;
        _slow_tick = 0;
        return;
    }

    /*--- 自检阶段：慢闪 ---*/
    if (loop1_INI_LOOP || !g_loop_power_up_state || !g_loop_stable) {
        _slow_tick++;
        if (_slow_tick >= GREEN_SLOW_HALF) {
            _slow_tick = 0;
            LED_GREEN_GPIO->odt ^= LED_GREEN_PIN;     // toggle
        }
        return;
    }

    /*--- 正常工作：车辆存在指示 ---*/
    _slow_tick = 0;
    if (loop1_VD_FLAG)
        LEDA_ON;
    else
        LEDA_OFF;
}

/*===========================================================================
 * 黄灯故障指示 — LEDC (PA10)
 *
 * 由 TMR15 ISR 每 5ms 驱动一次。
 *
 * 模式判定：
 *   当前线圈断开 或 上电后从未接线圈:
 *     → 快闪 (200ms 亮 / 200ms 灭)
 *   线圈已恢复连接, 且上电后有断开记录 (g_disconnect_count >= 1):
 *     → N 短闪 (80ms亮/200ms灭 × N, 1.2s间隔, N=断开次数≤3)
 *   线圈正常连接, 无断开记录:
 *     → 灭
 *===========================================================================*/
void poll_yellow_led(void)
{
#define FLT_ON_SHORT    16      // 80ms (16 × 5ms)
#define FLT_GAP_SHORT   40      // 200ms
#define FLT_GAP_LONG    240     // 1.2s (240 × 5ms)
#define FLT_FAST_HALF   40      // 200ms 快闪半周期

    g_fault_tick++;

    /*--- 模式1: 快闪 — 当前断开中 或 从无线圈上电 ---*/
    if (g_disconnect_active || !g_loop_power_up_state) {
        if (g_fault_tick >= FLT_FAST_HALF) {
            g_fault_tick = 0;
            LED_YELLOW_GPIO->odt ^= LED_YELLOW_PIN;     // toggle
        }
        return;
    }

    uint8_t N = g_disconnect_count;

    if (N == 0) {
        /*--- 模式2: 无断开记录 → 灭 ---*/
        LED_YELLOW_OFF;
        g_fault_tick = 0;
        g_fault_phase = 0;
        return;
    }

    /*--- 模式3: N 短闪 (连接中, 但有断开历史) ---*/
    if (N > 3) N = 3;

    uint8_t  phases_total = N * 2;    // N 个 ON_short + N 个 GAP_short
    uint16_t duration;

    if (g_fault_phase < phases_total) {
        duration = (g_fault_phase & 1) ? FLT_GAP_SHORT : FLT_ON_SHORT;
        if (g_fault_phase & 1)
            LED_YELLOW_OFF;
        else
            LED_YELLOW_ON;
    } else {
        /* LONG 间隔 */
        duration = FLT_GAP_LONG;
        LED_YELLOW_OFF;
    }

    if (g_fault_tick >= duration) {
        g_fault_tick = 0;
        g_fault_phase++;
        if (g_fault_phase > phases_total)
            g_fault_phase = 0;
    }
}

/*===========================================================================
 * 初始化所有状态变量（对齐 M1H INIT_VD）
 *===========================================================================*/
void INIT_VD(void)
{
    loop1_INCNT     = 0;
    loop1_OUTCNT    = 0;
    loop1_CapCnt    = 0;
    loop1_CapLast   = 0;
    loop1_LPCNT     = 0;
    loop1_INI_LOOP  = 1;

    loop1_VD_FLAG   = 0;
    loop1_RF_FLAG   = 0;
    loop1_LOOP_OK   = 1;
    loop1_LOOP_OK0  = 0;
    loop1_CAP_OK    = 0;
    loop1_CAPVD     = 0;

    loop1_SensLevel = 2;          // 默认中灵敏度
    Flt_Reg         = ALFA_CAP1;  // 79

    loop1_ORG_CNT  = 0;
    loop1_ORG_SUM  = 0;

    loop1_FLAG_IN    = 0;
    loop1_FLAG_OUT   = 0;
    loop1_FLAG_PLUSE = 0;
    loop1_cnt_release = 0;
    g_loop_stable     = 0;

    /* M4 优化: 快速 IIR + 进入确认 */
    loop1_CAPVD_fast  = 0;
    loop1_entry_cnt   = 0;
    loop1_freeze_cnt  = 0;
    loop1_freeze_ref  = 0;
#if USE_FLATNESS_EXIT
    g_exit_state      = 0;
    g_max_slope       = 0;
    g_max_slope_rate  = 0;
    g_delta2          = 0;
    g_delta3          = 0;
    g_prev_capvd      = 0;
    g_prev_first_deriv = 0;
    g_slope_flat_cnt  = 0;
    g_flat_ok_cnt     = 0;
#endif

    loop1_LC_HOLD  = 0;
    loop1_LC_Reset = 0;
    LC_Hold_CNT    = 0;
    Hold_CNT       = 0;

    g_loop_power_up_state = 0;
    g_disconnect_count    = 0;
    g_disconnect_active   = 0;
    g_fault_phase         = 0;
    g_fault_tick          = 0;

    /* 读取拨码初始状态 */
    sw0  = gpio_input_data_bit_read(SW5_BUTTON_PORT, SW5_BUTTON_PIN) << 4;
    sw0 |= gpio_input_data_bit_read(SW4_BUTTON_PORT, SW4_BUTTON_PIN) << 3;
    sw0 |= gpio_input_data_bit_read(SW3_BUTTON_PORT, SW3_BUTTON_PIN) << 2;
    sw0 |= gpio_input_data_bit_read(SW2_BUTTON_PORT, SW2_BUTTON_PIN) << 1;
    sw0 |= gpio_input_data_bit_read(SW1_BUTTON_PORT, SW1_BUTTON_PIN) << 0;

    swok     = sw0;
    sw1      = sw0;
    SENS     = swok & 0x03;
    SENS_LAST = SENS;
    swcnt    = 0;

    switch (SENS) {
        case 0x00: loop1_SensLevel = 0; break;
        case 0x02: loop1_SensLevel = 1; break;
        case 0x01: loop1_SensLevel = 2; break;
        case 0x03: loop1_SensLevel = 3; break;
        default:   loop1_SensLevel = 0; break;
    }

    SET_PLUS = swok & 0x04;
    SET_DLY  = (swok & 0x08) >> 3;
    SET_SAFE = (swok & 0x10) >> 4;
}

/*===========================================================================
 * TMR3 输入捕获中断 — 线圈频率测量（对齐 M1H CAP0 ISR）
 *
 * PA7 → TIM3_CH2, 上升沿触发。
 * 每次捕获中断读取 CCR2，计算相邻边沿差 Xn。
 * 累加 LPCNT 次后产生一个测量窗口的 Value。
 *===========================================================================*/
void TMR3_GLOBAL_IRQHandler(void)
{
    if (tmr_interrupt_flag_get(TMR3, TMR_C2_FLAG) != RESET) {
        tmr_flag_clear(TMR3, TMR_C2_FLAG);
        loop1_CapThis = tmr_channel_value_get(TMR3, TMR_SELECT_CHANNEL_2);

        if (!loop1_RF_FLAG) {
            /* 首次捕获，记录基准 */
            loop1_CapLast = loop1_CapThis;
            loop1_RF_FLAG = 1;
        } else {
            /* 计算相邻边沿周期差（16bit 溢出回绕处理） */
            if (loop1_CapThis > loop1_CapLast) {
                loop1_Xn = loop1_CapThis - loop1_CapLast;
            } else {
                loop1_Xn = (0x10000 - loop1_CapLast) + loop1_CapThis;
            }
            g_xn_counter++;

            loop1_CapLast = loop1_CapThis;
            loop1_CapCnt++;

            if (loop1_INI_LOOP) {
                /*--- 初始化阶段：计算自适应测量窗口大小 ---*/
                if (loop1_LPCNT == 0) {
                    if (loop1_CapCnt > 10) {
                        /*
                         * 自适应测量窗口: MEASUREMENT_BASE / Xn
                         * 使归一化 Value ≈ 131072，无需后续 >>6
                         */
                        loop1_LPCNT = MEASUREMENT_BASE / loop1_Xn;
                        PRINT("First_calc_loop1_LPCNT:%d, loop1_Xn:%d\n",
                              loop1_LPCNT, loop1_Xn);
                        if (loop1_LPCNT == 0) {
                            loop1_LPCNT = 100;
                        }
                        loop1_CAPVD  = 0;
                        loop1_CapSum = 0;
                        loop1_CapCnt = 0;
                    }
                } else {
                    /* 第一个窗口：直接作为基线 Origin */
                    loop1_CapSum += loop1_Xn;
                    if (loop1_CapCnt >= loop1_LPCNT) {
                        loop1_CAPVD  = loop1_CapSum;
                        loop1_Origin = loop1_CAPVD;          // 不再 >>6
                        PRINT("First_capSum:%d, Origin:%d\n",
                              loop1_CapSum, loop1_Origin);
                        loop1_INI_LOOP = 0;
                        loop1_CapSum  = 0;
                        loop1_Value   = 0;
                        loop1_CapCnt  = 0;
                    }
                }
            } else {
                /*--- 正常运行：累加到测量窗口 ---*/
                loop1_CapSum += loop1_Xn;
                if (loop1_CapCnt >= loop1_LPCNT) {
                    loop1_Value   = loop1_CapSum;
                    loop1_CAP_OK  = 1;
                    loop1_CapSum  = 0;
                    loop1_CapCnt  = 0;
                    loop1_INI_LOOP = 0;
                }
            }
        }
    }
}

/*===========================================================================
 * TMR15 定时器中断 — 5ms tick，主状态机（对齐 M1H Timer1 ISR）
 *
 * 职责：
 *   1. 50ms tick 计数器（TM1cnt ÷ 10）
 *   2. 进入/离开/脉冲时序状态机
 *   3. 有限存在超时 / 安全复位超时
 *   4. 线圈载波检测 (RF_FLAG) 及继电器输出刷新
 *   5. 呼吸灯、故障灯驱动
 *===========================================================================*/
void TMR15_GLOBAL_IRQHandler(void)
{
    static uint16_t _counter1_init = 0;

    if (tmr_interrupt_flag_get(TMR15, TMR_OVF_FLAG) != RESET) {

        /*--- 初始化阶段红灯快闪 (200ms 周期) ---*/
        if (!g_loop_power_up_state) {
            _counter1_init++;
            if (_counter1_init >= 40) {      // 40 × 5ms = 200ms
                _counter1_init = 0;
                LEDA_ON_OFF();
            }
        }

        /*--- 50ms tick 分频 ---*/
        TM1cnt++;
        if (TM1cnt >= 10) {                  // 10 × 5ms = 50ms
            TM1cnt = 0;

            /*================================================================
             * 时序状态机（对齐 M1H Timer1 ISR）
             *
             *                     IN_DELAY         OUT_DELAY
             *   [空闲] ──有车──→ [进入延时] ──→ [有车确认] ──离开──→
             *      ↑                                                   │
             *      └──脉冲结束──── [脉冲输出] ←──── [离开延时] ←──────┘
             *                        PULSE_DELAY
             *================================================================*/

            /* FLAG_IN: 进入延时 500ms */
            if (loop1_FLAG_IN) {
                loop1_INCNT++;
                if (loop1_INCNT > IN_DELAY) {
                    loop1_FLAG_IN  = 0;
                    loop1_INCNT    = 0;
                }
            }

            /* FLAG_OUT: 离开延时（SW_4=ON 时 500ms，OFF 时 0） */
            if (loop1_FLAG_OUT) {
                if (!SET_DLY) {
                    loop1_OUTCNT++;
                    if (loop1_OUTCNT > OUT_DELAY) {
                        loop1_FLAG_OUT   = 0;
                        loop1_FLAG_PLUSE = 1;
                        loop1_OUTCNT     = 0;
                    }
                } else {
                    /* 无离开延时：立即触发脉冲 */
                    loop1_FLAG_OUT   = 0;
                    loop1_FLAG_PLUSE = 1;
                    loop1_OUTCNT     = 0;
                }
            }

            /* FLAG_PLUSE: 脉冲宽度 500ms */
            if (loop1_FLAG_PLUSE) {
                loop1_OUTCNT++;
                if (loop1_OUTCNT > PULSE_DELAY) {
                    loop1_FLAG_PLUSE = 0;
                    loop1_OUTCNT     = 0;
                }
            }

            /* 有限存在超时 */
            if (loop1_VD_HOLD) {
                Hold_CNT++;
                if (Hold_CNT > HOLD_TIME) {
                    loop1_VD_HOLD = 0;
                    Hold_CNT      = 0;
                    if (loop1_VD_FLAG) {
                        /* 超时：强制释放 */
                        RLY1_OFF;
                        RLY2_OFF;
                    }
                }
            }

            /* 安全复位超时 */
            if (loop1_LC_HOLD) {
                LC_Hold_CNT++;
                if (LC_Hold_CNT > g_safe_max_cnt) {
                    loop1_LC_Reset  = 1;
                    loop1_INI_LOOP  = 1;
                    loop1_LOOP_OK0  = 0;
                    g_loop_stable   = 0;
#if USE_FLATNESS_EXIT
                    g_exit_state    = 0;
                    g_max_slope     = 0;
                    g_max_slope_rate = 0;
#endif
                    LC_Hold_CNT     = 0;
                    loop1_ORG_CNT   = 0;
                    loop1_ORG_SUM   = 0;
                }
            }
        }

        /*================================================================
         * 线圈载波检测 & 继电器输出刷新（每 5ms）
         * RF_FLAG 由 TMR3 ISR 置位，此处消费后清除。
         *================================================================*/
        if (loop1_RF_FLAG) {
            loop1_LOOP_OK  = 1;
            loop1_RF_FLAG  = 0;

            if (loop1_LC_Reset == 0) {
                /* 继电器 2（辅助/方向） */
                if (loop1_VD_FLAG)
                    RLY2_ON;
                else
                    RLY2_OFF;

                /* 继电器 1（主输出） */
                if (SET_PLUS) {
                    /* 存在输出模式：有车或离开延时中 */
                    if (loop1_VD_FLAG || loop1_FLAG_OUT)
                        RLY1_ON;
                    else
                        RLY1_OFF;
                } else {
                    /* 脉冲输出模式：脉冲期间吸合 */
                    if (loop1_FLAG_PLUSE)
                        RLY1_ON;
                    else
                        RLY1_OFF;
                }
            }
        } else {
            loop1_LOOP_OK0 = loop1_LOOP_OK;
            loop1_LOOP_OK  = 0;
        }

        /*--- 硬件驱动：呼吸灯 + 指示灯 ---*/
        poll_red_pwm();
        poll_green_led();
        poll_yellow_led();

        tmr_flag_clear(TMR15, TMR_OVF_FLAG);
    }
}

/*===========================================================================
 * vd1_task — 核心检测算法（对齐 M1H VD1_TASK）
 *
 * 每次 loop1_CAP_OK 时调用一次。
 * 流程：
 *   1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD
 *   2. 无车时：基线跟踪 + 进入检测
 *   3. 有车时：离开检测（带滞回）
 *===========================================================================*/
void vd1_task(void)
{
#define STABLE_SAMPLES   128     // 稳定期样本数

    static uint16_t _stable_cnt = 0;

    if (loop1_Origin == 0) return;

    /*================================================================
     * 1. 双路 IIR 滤波 (M4 优化)
     *
     *  慢速 IIR (CAPVD):  α=18/256 ≈ 0.07, τ≈135ms
     *    - 斜率限幅: 单次变化 >5% → 截断 (拒绝 EMI/闪电尖峰)
     *    - 用途: 基线跟踪
     *
     *  快速 IIR (CAPVD_fast): α=0.5, τ≈28ms
     *    - 从斜率限幅后的 CAPVD 派生
     *    - 用途: 进入/离开检测判定
     *================================================================*/

    /* 1a. 慢速 IIR — 斜率限幅 */
    if (loop1_CAPVD == 0) {
        loop1_CAPVD = loop1_Value;
        loop1_CAPVD_fast = loop1_Value;
    } else {
        /* 斜率限幅: 物理车辆不可能让频率瞬间跳变 > MAX_SLOPE_RATE% */
        int32_t raw_delta = (int32_t)loop1_Value - (int32_t)loop1_CAPVD;
        int32_t max_step = (int32_t)(loop1_CAPVD * MAX_SLOPE_RATE / 100);
        if (max_step < 100) max_step = 100;  // 最小限幅，防止 origin 很小时锁死
        if (raw_delta > max_step)  raw_delta = max_step;
        if (raw_delta < -max_step) raw_delta = -max_step;
        uint32_t clamped_value = (uint32_t)((int32_t)loop1_CAPVD + raw_delta);
        loop1_CAPVD = get_flt_value(clamped_value, loop1_CAPVD);
    }

    /* 1b. 快速 IIR — α=0.5: (old + new) / 2 */
    if (loop1_CAPVD_fast == 0) {
        loop1_CAPVD_fast = loop1_CAPVD;  // 首次直接锁定
    } else {
        loop1_CAPVD_fast = (loop1_CAPVD_fast + loop1_CAPVD) / 2;
    }

    /*--- 2. 稳定期：只跟踪基线，不检测车辆 ---*/
    if (!g_loop_stable) {
        /* 稳定期内不做斜率限幅和 IIR — 直接用 Value 快速收敛到真实基线 */
        loop1_CAPVD = loop1_Value;
        loop1_CAPVD_fast = loop1_Value;

        update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
                              &loop1_Origin, loop1_CAPVD, 100);
        _stable_cnt++;
        if (_stable_cnt >= STABLE_SAMPLES) {
            g_loop_stable = 1;
            PRINT("Loop stable, Origin:%d\\n", loop1_Origin);
        }
        return;
    }

    if (!loop1_VD_FLAG) {
        /*================================================================
         * 无车状态
         *================================================================*/

        /*--- 基线跟踪（仿 TLD-110：有车时冻结）
         *    额外保护: CAPVD 异常上升时暂停跟踪，防止基线被污染
         *    否则车辆驶入时若 Xn 先增大，Origin 被顶上去后无法释放 ---*/
        loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable[loop1_SensLevel]) >> 16;
        {
            int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin;
            if (dev < (int32_t)(loop1_dlt_ORG * 4)) {
                /* CAPVD 未显著高于基线 → 安全跟踪，重置冻结计数 */
                loop1_freeze_cnt = 0;
                update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT,
                                      &loop1_Origin, loop1_CAPVD, 100);
            } else {
                /* CAPVD 异常偏高 → 冻结跟踪 */
                if (loop1_freeze_cnt == 0) {
                    loop1_freeze_ref = loop1_CAPVD;   // 记录冻结起始值
                } else {
                    /* 稳定性检查: CAPVD 偏离参考值超过 FREEZE_STABILITY_RATE% 则重置 */
                    int32_t drift = (int32_t)loop1_CAPVD - (int32_t)loop1_freeze_ref;
                    if (drift < 0) drift = -drift;
                    if (drift > (int32_t)(loop1_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
                        /* 波动过大 → 不是稳定值，重置计数并以当前值重新计时 */
                        loop1_freeze_cnt = 0;
                        loop1_freeze_ref = loop1_CAPVD;
                    }
                }
                loop1_freeze_cnt++;
                if (loop1_freeze_cnt >= FREEZE_TIMEOUT) {
                    /* 超时: CAPVD 持续偏高且稳定 → 环境变化，接受新基线 */
                    loop1_Origin     = loop1_CAPVD;
                    loop1_freeze_cnt = 0;
                    loop1_freeze_ref = 0;
                    loop1_ORG_CNT    = 0;
                    loop1_ORG_SUM    = 0;
                    PRINT("Baseline timeout update, new Origin:%d\\n", loop1_Origin);
                } else {
                    /* 未超时: 保持冻结，重置累计（防止突然解冻时旧数据污染） */
                    loop1_ORG_CNT = 0;
                    loop1_ORG_SUM = 0;
                }
            }
        }

        /*--- M4 优化: 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值才判有车
         *    使用快速 IIR (CAPVD_fast) 提高响应速度
         *    配合确认机制防止瞬态干扰误触发 ---*/
        if (loop1_CAPVD_fast < (loop1_Origin - loop1_dlt_ORG)) {
            loop1_entry_cnt++;
            if (loop1_entry_cnt >= ENTRY_CONFIRM) {
                PRINT("Car_In, Value:%d, CAPVD:%d, CAPVD_fast:%d, Origin:%d, dlt:%d\\n",
                      loop1_Value, loop1_CAPVD, loop1_CAPVD_fast,
                      loop1_Origin, loop1_dlt_ORG);

                loop1_VD_FLAG = 1;
                loop1_FLAG_IN = 1;
                loop1_entry_cnt = 0;
                loop1_freeze_cnt = 0;   // 入场时重置冻结状态
                loop1_freeze_ref = 0;

                /* 有限存在计时（非安全复位模式下） */
                if (!SET_SAFE) {
                    loop1_LC_HOLD = 1;
                } else {
                    loop1_LC_HOLD = 0;
                }
                if (loop1_LC_Reset)
                    loop1_LC_Reset = 0;

                loop1_ORG_CNT = 0;
                loop1_ORG_SUM = 0;

                /* 重置平坦性状态（专利 CN200910309382） */
#if USE_FLATNESS_EXIT
                g_exit_state      = 0;
                g_max_slope       = 0;
                g_max_slope_rate  = 0;
                g_delta2          = 0;
                g_delta3          = 0;
                g_slope_flat_cnt  = 0;
                g_flat_ok_cnt     = 0;
#endif
            }
        } else {
            if (loop1_entry_cnt > 0) loop1_entry_cnt = 0;  // 离开阈值区域则重置确认计数
        }
    } else {
#if USE_FLATNESS_EXIT
        /*================================================================
         * 有车状态 — 平坦性判定离开（专利 CN200910309382）
         *================================================================*/
#define K1                  8
#define K2                  8
#define SLOPE_FLAT_THRESH   100
#define MIN_DELTA2          5
#define MIN_DELTA3          2
#define FLAT_CONFIRM_CNT    3

        int32_t first_deriv, abs_fd, abs_sd, second_deriv;

        first_deriv  = (int32_t)loop1_CAPVD - g_prev_capvd;
        second_deriv = first_deriv - g_prev_first_deriv;
        abs_fd = (first_deriv >= 0) ? first_deriv : -first_deriv;
        abs_sd = (second_deriv >= 0) ? second_deriv : -second_deriv;

        if (g_exit_state == 0) {
            if (abs_fd > g_max_slope)      g_max_slope      = abs_fd;
            if (abs_sd > g_max_slope_rate) g_max_slope_rate = abs_sd;
            if (abs_fd < SLOPE_FLAT_THRESH) {
                g_slope_flat_cnt++;
                if (g_slope_flat_cnt >= 3) {
                    g_delta2 = g_max_slope / K1;
                    g_delta3 = g_max_slope_rate / K2;
                    if (g_delta2 < MIN_DELTA2) g_delta2 = MIN_DELTA2;
                    if (g_delta3 < MIN_DELTA3) g_delta3 = MIN_DELTA3;
                    g_exit_state = 1;
                    g_flat_ok_cnt = 0;
                }
            } else {
                g_slope_flat_cnt = 0;
            }
        } else {
            int32_t dev = (int32_t)loop1_CAPVD - (int32_t)loop1_Origin;
            int32_t cond1 = (dev >= 0) ? dev : -dev;
            loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16;
            if (cond1 < (int32_t)loop1_dlt_ORG && abs_fd < (int32_t)g_delta2
                && abs_sd < (int32_t)g_delta3) {
                g_flat_ok_cnt++;
                if (g_flat_ok_cnt >= FLAT_CONFIRM_CNT) {
                    PRINT("Car_OFF_FLAT, CAPVD:%d Origin:%d d1:%d d2:%d d3:%d f':%d f'':%d\n",
                          loop1_CAPVD, loop1_Origin, loop1_dlt_ORG,
                          g_delta2, g_delta3, first_deriv, second_deriv);
                    loop1_VD_FLAG  = 0;
                    loop1_FLAG_OUT = 1;
                    loop1_VD_HOLD  = 0;
                    loop1_LC_HOLD  = 0;
                    loop1_ORG_CNT = 0;
                    loop1_ORG_SUM = 0;
                    Hold_CNT      = 0;
                    g_flat_ok_cnt = 0;
                    g_exit_state  = 0;
                }
            } else {
                g_flat_ok_cnt = 0;
            }
        }
        g_prev_capvd       = loop1_CAPVD;
        g_prev_first_deriv = first_deriv;
#else
        /*================================================================
         * 有车状态 — 简单 cnt_release 防抖离开
         *================================================================*/
        loop1_dlt_ORG = ((uint32_t)loop1_Origin * SensTable_1[loop1_SensLevel]) >> 16;

        if ((loop1_Origin - loop1_dlt_ORG) < loop1_CAPVD) {
            loop1_cnt_release++;
            if (loop1_cnt_release >= 3) {
                PRINT("Car_OFF, Value:%d CAPVD:%d Origin:%d dlt:%d\n",
                      loop1_Value, loop1_CAPVD, loop1_Origin, loop1_dlt_ORG);
                loop1_VD_FLAG  = 0;
                loop1_FLAG_OUT = 1;
                loop1_VD_HOLD  = 0;
                loop1_LC_HOLD  = 0;
                loop1_ORG_CNT = 0;
                loop1_ORG_SUM = 0;
                Hold_CNT      = 0;
                loop1_cnt_release = 0;
            }
        } else {
            if (loop1_cnt_release > 0) loop1_cnt_release = 0;
        }
#endif
    }
}

/*===========================================================================
 * loop_task_function — FreeRTOS 主任务（对齐 M1H main 循环）
 *===========================================================================*/
void loop_task_function(void *pvParameters)
{
    g_sys_freq = g_crm_clocks_freq_struct.sclk_freq;

    INIT_VD();

    while (1) {
        if (loop1_LOOP_OK) {
            /*--- 线圈重连 ---*/
            if (!loop1_LOOP_OK0) {
                loop1_LOOP_OK0 = 1;
                g_disconnect_active = 0;     // 重连，清除断开标记
                loop1_CAPVD = 0;             // 强制 IIR 从首个有效值重新收敛
            }

            if (loop1_CAP_OK) {
                loop1_CAP_OK = 0;

                /* 首次成功测量 → 标记线圈已连接 */
                if (!g_loop_power_up_state) {
                    g_loop_power_up_state = 1;
                }

                /* 核心检测算法 */
                vd1_task();

                /* 拨码开关轮询 */
                poll_sw_state();
            }
        } else {
            /*--- 线圈断开 ---*/
            /* 注意: 不清除 loop1_VD_FLAG，保留断开前的检测状态 */
            RLY1_OFF;
            RLY2_OFF;

            /*
             * 断开计数规则:
             *   - 从未连接过上电 (g_loop_power_up_state == 0): 不计入
             *   - g_disconnect_active == 0: 新的断开 → 计数+1
             *   - g_disconnect_active == 1: 同一次断开，不重复计数
             */
            if (g_loop_power_up_state && !g_disconnect_active) {
                g_disconnect_active = 1;
                if (g_disconnect_count < 3) {
                    g_disconnect_count++;
                }
                g_fault_phase = 0;
                g_fault_tick  = 0;
            }

            vTaskDelay(150);
        }

        /*--- 调试输出（每 2 秒） ---*/
#ifdef DEBUG
        {
            static uint32_t _dbg_cnt = 0;
            _dbg_cnt++;
            if (_dbg_cnt >= 200) {      // 200 × 10ms = 2s
                _dbg_cnt = 0;
                PRINT("SET_DLY:%d, SAFE:%d, Xn:%d, LPCNT:%d, "
                      "CAPVD:%d, Origin:%d, VD:%d\n",
                      SET_DLY, SET_SAFE, loop1_Xn, loop1_LPCNT,
                      loop1_CAPVD, loop1_Origin, loop1_VD_FLAG);
            }
        }
#endif

        vTaskDelay(10);  // 10ms tick, M4 优化: 双路 IIR + 进入确认 + 斜率限幅
    }
}