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

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

/*===========================================================================
 * 全局状态
 *===========================================================================*/
Loop154_States g_loop_states = {{0}, {{{0}}}};
uint32_t g_sys_freq   = 0;
uint8_t  g_input_div   = 1;
uint32_t g_safe_max_cnt = LC_HOLD_TIME;

/*===========================================================================
 * 一阶 IIR 低通滤波器（对齐 DLD154V4B）
 * CAPVD_new = α·new_value + (1-α)·CAPVD_old, α = Flt_Reg/256
 *===========================================================================*/
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 * ALFA_CAP1) >> 8;

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

/*===========================================================================
 * 滑动平均基线更新
 *===========================================================================*/
uint8_t update_moving_average(uint32_t* p_sum, uint16_t* p_cnt,
                              uint32_t* p_origin, uint32_t new_value,
                              uint16_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;
}

/*===========================================================================
 * 继电器控制
 *===========================================================================*/
void set_loops_relay_on(uint8_t loop_num)
{
    switch(loop_num) {
        case 0: RLY1_ON; break;
        case 1: RLY2_ON; break;
        case 2: RLY3_ON; break;
        case 3: RLY4_ON; break;
    }
}

void set_loops_relay_off(uint8_t loop_num)
{
    switch(loop_num) {
        case 0: RLY1_OFF; break;
        case 1: RLY2_OFF; break;
        case 2: RLY3_OFF; break;
        case 3: RLY4_OFF; break;
    }
}

/*===========================================================================
 * 单通道初始化
 *===========================================================================*/
void init_vd_single(Loop154_Unit *unit)
{
    unit->INCNT     = 0;
    unit->OUTCNT    = 0;
    unit->loop_CapCnt    = 0;
    unit->loop_CapLast   = 0;
    unit->loop_LPCNT     = 0;
    unit->loop_INI_LOOP  = 1;

    unit->loop_VD_FLAG   = 0;
    unit->loop_RF_FLAG   = 0;
    unit->loop_LOOP_OK   = 1;
    unit->loop_LOOP_OK0  = 0;
    unit->loop_CAP_OK    = 0;
    unit->loop_CAPVD     = 0;

    unit->loop_SensLevel = 2;        // 默认中灵敏度
    unit->Flt_Reg        = ALFA_CAP1;

    unit->loop_entry_cnt  = 0;
    unit->loop_freeze_cnt = 0;
    unit->loop_freeze_ref = 0;

    unit->loop_ORG_CNT   = 0;
    unit->loop_ORG_SUM   = 0;

    unit->loop_FLAG_IN    = 0;
    unit->loop_FLAG_OUT   = 0;
    unit->loop_FLAG_PLUSE = 0;
    unit->loop_cnt_release = 0;
    unit->loop_stable     = 0;

#if USE_FLATNESS_EXIT
    unit->exit_state      = 0;
    unit->max_slope       = 0;
    unit->max_slope_rate  = 0;
    unit->delta2          = 0;
    unit->delta3          = 0;
    unit->prev_capvd      = 0;
    unit->prev_first_deriv = 0;
    unit->slope_flat_cnt  = 0;
    unit->flat_ok_cnt     = 0;
#endif

    unit->LC_HOLD  = 0;
    unit->LC_Reset = 0;
    unit->LC_Hold_CNT = 0;
    unit->Hold_CNT    = 0;

    unit->power_up_state    = 0;
    unit->disconnect_count  = 0;
    unit->disconnect_active = 0;
    unit->fault_phase       = 0;
    unit->fault_tick        = 0;

    unit->SET_PLUS = 1;   // 存在输出
    unit->SET_DLY  = 0;
    unit->SET_SAFE = 1;

    unit->stable_cnt = 0;
    unit->xn_counter = 0;
}

void INIT_VDs(Loop154_States *loops)
{
    uint8_t i;
    for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
        loops->loop_unit[i].loop_num = i;
        init_vd_single(&loops->loop_unit[i]);
    }
}

/*===========================================================================
 * TMR3 输入捕获中断 — 四路线圈频率测量（对齐 DLD154V4B CAP0 ISR）
 *
 * 每个通道独立捕获，使用 MEASUREMENT_BASE 自适应测量窗口。
 * LPCNT = MEASUREMENT_BASE / Xn，使 Value ≈ 131072，无需后续 >> 移位。
 *===========================================================================*/
void TMR3_GLOBAL_IRQHandler(void)
{
    #define PROCESS_CHANNEL(ch_flag, ch_select, idx) \
        if (tmr_interrupt_flag_get(TMR3, ch_flag) != RESET) { \
            tmr_flag_clear(TMR3, ch_flag); \
            uint16_t cap_this = tmr_channel_value_get(TMR3, ch_select); \
            Loop154_Unit *unit = &g_loop_states.loop_unit[idx]; \
            if (!unit->loop_RF_FLAG) { \
                unit->loop_CapLast = cap_this; \
                unit->loop_RF_FLAG = 1; \
            } else { \
                uint16_t xn; \
                if (cap_this > unit->loop_CapLast) { \
                    xn = cap_this - unit->loop_CapLast; \
                } else { \
                    xn = (0x10000 - unit->loop_CapLast) + cap_this; \
                } \
                unit->loop_Xn = xn; \
                unit->xn_counter++; \
                unit->loop_CapLast = cap_this; \
                unit->loop_CapCnt++; \
                if (unit->loop_INI_LOOP) { \
                    if (unit->loop_LPCNT == 0) { \
                        if (unit->loop_CapCnt > 10) { \
                            unit->loop_LPCNT = MEASUREMENT_BASE / xn; \
                            PRINT("Loop%d LPCNT:%d Xn:%d\n", idx+1, unit->loop_LPCNT, xn); \
                            if (unit->loop_LPCNT == 0) unit->loop_LPCNT = 100; \
                            unit->loop_CAPVD  = 0; \
                            unit->loop_CapSum = 0; \
                            unit->loop_CapCnt = 0; \
                        } \
                    } else { \
                        unit->loop_CapSum += xn; \
                        if (unit->loop_CapCnt >= unit->loop_LPCNT) { \
                            unit->loop_CAPVD  = unit->loop_CapSum; \
                            unit->loop_Origin = unit->loop_CAPVD; \
                            PRINT("Loop%d Origin:%d\n", idx+1, unit->loop_Origin); \
                            unit->loop_INI_LOOP = 0; \
                            unit->loop_CapSum  = 0; \
                            unit->loop_Value   = 0; \
                            unit->loop_CapCnt  = 0; \
                        } \
                    } \
                } else { \
                    unit->loop_CapSum += xn; \
                    if (unit->loop_CapCnt >= unit->loop_LPCNT) { \
                        unit->loop_Value   = unit->loop_CapSum; \
                        unit->loop_CAP_OK  = 1; \
                        unit->loop_CapSum  = 0; \
                        unit->loop_CapCnt  = 0; \
                        unit->loop_INI_LOOP = 0; \
                    } \
                } \
            } \
        }

    PROCESS_CHANNEL(TMR_C1_FLAG, TMR_SELECT_CHANNEL_1, 0)
    PROCESS_CHANNEL(TMR_C2_FLAG, TMR_SELECT_CHANNEL_2, 1)
    PROCESS_CHANNEL(TMR_C3_FLAG, TMR_SELECT_CHANNEL_3, 2)
    PROCESS_CHANNEL(TMR_C4_FLAG, TMR_SELECT_CHANNEL_4, 3)

    #undef PROCESS_CHANNEL
}

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

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

        /*--- 上报计数器 ---*/
        g_loop_states.report_counter++;

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

            for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
                Loop154_Unit *unit = &g_loop_states.loop_unit[i];

                /* FLAG_IN: 进入延时 500ms */
                if (unit->loop_FLAG_IN) {
                    unit->INCNT++;
                    if (unit->INCNT > IN_DELAY) {
                        unit->loop_FLAG_IN = 0;
                        unit->INCNT = 0;
                    }
                }

                /* FLAG_OUT: 离开延时 → 脉冲 */
                if (unit->loop_FLAG_OUT) {
                    if (!unit->SET_DLY) {
                        unit->OUTCNT++;
                        if (unit->OUTCNT > unit->relay_delay) {
                            unit->loop_FLAG_OUT   = 0;
                            unit->loop_FLAG_PLUSE = 1;
                            unit->OUTCNT = 0;
                        }
                    } else {
                        unit->loop_FLAG_OUT   = 0;
                        unit->loop_FLAG_PLUSE = 1;
                        unit->OUTCNT = 0;
                    }
                }

                /* FLAG_PLUSE: 脉冲宽度 500ms */
                if (unit->loop_FLAG_PLUSE) {
                    unit->OUTCNT++;
                    if (unit->OUTCNT > PULSE_DELAY) {
                        unit->loop_FLAG_PLUSE = 0;
                        unit->OUTCNT = 0;
                    }
                }

                /* 有限存在超时 — 通道重启，重新建立基线 */
                if (unit->loop_VD_HOLD) {
                    unit->Hold_CNT++;
                    if (unit->Hold_CNT > unit->hold_time) {
                        unit->loop_VD_HOLD = 0;
                        unit->Hold_CNT = 0;
                        set_loops_relay_off(unit->loop_num);
                        unit->loop_VD_FLAG  = 0;
                        unit->loop_FLAG_IN  = 0;
                        unit->loop_FLAG_OUT = 0;
                        unit->loop_FLAG_PLUSE = 0;
                        unit->LC_HOLD  = 0;
                        unit->LC_Reset = 1;
                        unit->loop_INI_LOOP = 1;
                        unit->loop_LOOP_OK0 = 0;
                        unit->loop_stable   = 0;
                        unit->loop_ORG_CNT  = 0;
                        unit->loop_ORG_SUM  = 0;
#if USE_FLATNESS_EXIT
                        unit->exit_state    = 0;
                        unit->max_slope     = 0;
                        unit->max_slope_rate = 0;
#endif
                    }
                }

                /* 安全复位超时 */
                if (unit->LC_HOLD) {
                    unit->LC_Hold_CNT++;
                    if (unit->LC_Hold_CNT > g_safe_max_cnt) {
                        unit->LC_Reset   = 1;
                        unit->loop_INI_LOOP = 1;
                        unit->loop_LOOP_OK0 = 0;
                        unit->loop_stable   = 0;
#if USE_FLATNESS_EXIT
                        unit->exit_state    = 0;
                        unit->max_slope     = 0;
                        unit->max_slope_rate = 0;
#endif
                        unit->LC_Hold_CNT   = 0;
                        unit->loop_ORG_CNT  = 0;
                        unit->loop_ORG_SUM  = 0;
                    }
                }
            }
        }

        /*================================================================
         * 线圈载波检测 & 继电器输出刷新（每 5ms）
         *================================================================*/
        for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
            Loop154_Unit *unit = &g_loop_states.loop_unit[i];

            if (unit->loop_RF_FLAG) {
                unit->loop_LOOP_OK = 1;
                unit->loop_RF_FLAG = 0;

                if (unit->LC_Reset == 0) {
                    if (unit->SET_PLUS) {
                        /* 存在输出模式：有车或离开延时中 */
                        if (unit->loop_VD_FLAG || unit->loop_FLAG_OUT)
                            set_loops_relay_on(unit->loop_num);
                        else
                            set_loops_relay_off(unit->loop_num);
                    } else {
                        /* 脉冲输出模式：脉冲期间吸合 */
                        if (unit->loop_FLAG_PLUSE)
                            set_loops_relay_on(unit->loop_num);
                        else
                            set_loops_relay_off(unit->loop_num);
                    }
                }
            } else {
                unit->loop_LOOP_OK0 = unit->loop_LOOP_OK;
                unit->loop_LOOP_OK  = 0;
            }

            /*--- LED 指示 ---*/
            poll_green_led(unit);
        }

        /*--- 呼吸灯 ---*/
        poll_red_pwm();

        tmr_flag_clear(TMR15, TMR_OVF_FLAG);
    }
}

/*===========================================================================
 * 绿灯指示 — 每通道独立
 *
 * 模式：
 *   自检/稳定中 (loop_INI_LOOP || 未连 || !loop_stable):
 *     → 慢闪 (200ms 亮/灭)
 *   正常工作:
 *     → 有车亮, 无车灭
 *===========================================================================*/
void poll_green_led(Loop154_Unit *unit)
{
#define GREEN_SLOW_HALF   40      // 200ms (40 × 5ms)

    static uint16_t _slow_tick[LOOP_CAPTURE_MAX] = {0};
    uint8_t idx = unit->loop_num;

    if (unit->disconnect_active) {
        at32_led_off(idx);
        _slow_tick[idx] = 0;
        return;
    }

    if (unit->loop_INI_LOOP || !unit->power_up_state || !unit->loop_stable) {
        _slow_tick[idx]++;
        if (_slow_tick[idx] >= GREEN_SLOW_HALF) {
            _slow_tick[idx] = 0;
            at32_led_toggle(idx);
        }
        return;
    }

    _slow_tick[idx] = 0;
    if (unit->loop_VD_FLAG)
        at32_led_on(idx);
    else
        at32_led_off(idx);
}

/*===========================================================================
 * vd1_task_per_channel — 核心检测算法（移植自 DLD154V4B vd1_task）
 *
 * 每次 loop_CAP_OK 时调用。
 * 流程：
 *   1. IIR 滤波：CAPVD = α·Value + (1-α)·CAPVD
 *   2. 稳定期：只跟踪基线，不检测车辆 (STABLE_SAMPLES=128)
 *   3. 无车时：基线跟踪 + 进入检测（带基线污染保护）
 *   4. 有车时：平坦性离开 (USE_FLATNESS_EXIT=1) 或 cnt_release 防抖
 *===========================================================================*/
void vd1_task_per_channel(Loop154_Unit *unit)
{
    if (unit->loop_Origin == 0) return;

    /*================================================================
     * 1. IIR 滤波 + 斜率限幅 (M4 优化)
     *   ALFA_CAP1=79 @10ms → τ≈32ms
     *================================================================*/
    if (unit->loop_CAPVD == 0) {
        unit->loop_CAPVD = unit->loop_Value;
    } else {
        int32_t raw_delta = (int32_t)unit->loop_Value - (int32_t)unit->loop_CAPVD;
        int32_t max_step = (int32_t)(unit->loop_CAPVD * MAX_SLOPE_RATE / 100);
        if (max_step < 100) max_step = 100;
        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)unit->loop_CAPVD + raw_delta);
        unit->loop_CAPVD = get_flt_value(clamped_value, unit->loop_CAPVD);
    }

    /*--- 2. 稳定期：绕过 IIR 和斜率限幅，小窗口快速收敛 ---*/
    if (!unit->loop_stable) {
        unit->loop_CAPVD = unit->loop_Value;

        update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT,
                              &unit->loop_Origin, unit->loop_CAPVD, 100);
        unit->stable_cnt++;
        if (unit->stable_cnt >= STABLE_SAMPLES) {
            unit->loop_stable = 1;
            PRINT("Loop%d stable, Origin:%d\n", unit->loop_num + 1, unit->loop_Origin);
        }
        return;
    }

    if (!unit->loop_VD_FLAG) {
        /*================================================================
         * 无车状态
         *================================================================*/

        unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable[unit->loop_SensLevel]) >> 16;
        {
            int32_t dev = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_Origin;
            if (dev < (int32_t)(unit->loop_dlt_ORG * 4)) {
                unit->loop_freeze_cnt = 0;
                update_moving_average(&unit->loop_ORG_SUM, &unit->loop_ORG_CNT,
                                      &unit->loop_Origin, unit->loop_CAPVD, WINDOW_ORIGIN);
            } else {
                if (unit->loop_freeze_cnt == 0) {
                    unit->loop_freeze_ref = unit->loop_CAPVD;
                } else {
                    int32_t drift = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_freeze_ref;
                    if (drift < 0) drift = -drift;
                    if (drift > (int32_t)(unit->loop_freeze_ref * FREEZE_STABILITY_RATE / 100)) {
                        unit->loop_freeze_cnt = 0;
                        unit->loop_freeze_ref = unit->loop_CAPVD;
                    }
                }
                unit->loop_freeze_cnt++;
                if (unit->loop_freeze_cnt >= FREEZE_TIMEOUT) {
                    unit->loop_Origin     = unit->loop_CAPVD;
                    unit->loop_freeze_cnt = 0;
                    unit->loop_freeze_ref = 0;
                    unit->loop_ORG_CNT    = 0;
                    unit->loop_ORG_SUM    = 0;
                    PRINT("Loop%d Baseline timeout, new Origin:%d\n",
                          unit->loop_num + 1, unit->loop_Origin);
                } else {
                    unit->loop_ORG_CNT = 0;
                    unit->loop_ORG_SUM = 0;
                }
            }
        }

        /*--- 进入确认 — 连续 ENTRY_CONFIRM 次低于阈值 ---*/
        if (unit->loop_CAPVD < (unit->loop_Origin - unit->loop_dlt_ORG)) {
            unit->loop_entry_cnt++;
            if (unit->loop_entry_cnt >= ENTRY_CONFIRM) {
                PRINT("Loop%d Car_In, Value:%d CAPVD:%d Origin:%d dlt:%d\n",
                      unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD,
                      unit->loop_Origin, unit->loop_dlt_ORG);

                unit->loop_VD_FLAG = 1;
                unit->loop_FLAG_IN = 1;
                unit->loop_entry_cnt = 0;
                unit->loop_freeze_cnt = 0;
                unit->loop_freeze_ref = 0;
                if (unit->hold_time > 0) {
                    unit->loop_VD_HOLD = 1;
                }
                at32_led_on(unit->loop_num);

                if (!unit->SET_SAFE) {
                    unit->LC_HOLD = 1;
                } else {
                    unit->LC_HOLD = 0;
                }
                if (unit->LC_Reset)
                    unit->LC_Reset = 0;

                unit->loop_ORG_CNT = 0;
                unit->loop_ORG_SUM = 0;

#if USE_FLATNESS_EXIT
                unit->exit_state     = 0;
                unit->max_slope      = 0;
                unit->max_slope_rate = 0;
                unit->delta2         = 0;
                unit->delta3         = 0;
                unit->slope_flat_cnt = 0;
                unit->flat_ok_cnt    = 0;
#endif
            }
        } else {
            if (unit->loop_entry_cnt > 0) unit->loop_entry_cnt = 0;
        }
    } else {
#if USE_FLATNESS_EXIT
        /*================================================================
         * 有车状态 — 平坦性判定离开 (CN200910309382)
         *================================================================*/
        int32_t first_deriv, abs_fd, abs_sd, second_deriv;

        first_deriv  = (int32_t)unit->loop_CAPVD - unit->prev_capvd;
        second_deriv = first_deriv - unit->prev_first_deriv;
        abs_fd = (first_deriv >= 0) ? first_deriv : -first_deriv;
        abs_sd = (second_deriv >= 0) ? second_deriv : -second_deriv;

        if (unit->exit_state == 0) {
            /* 追踪第一上升坡面最大斜率 */
            if (abs_fd > unit->max_slope)      unit->max_slope      = abs_fd;
            if (abs_sd > unit->max_slope_rate) unit->max_slope_rate = abs_sd;
            if (abs_fd < SLOPE_FLAT_THRESH) {
                unit->slope_flat_cnt++;
                if (unit->slope_flat_cnt >= 3) {
                    unit->delta2 = unit->max_slope / K1;
                    unit->delta3 = unit->max_slope_rate / K2;
                    if (unit->delta2 < MIN_DELTA2) unit->delta2 = MIN_DELTA2;
                    if (unit->delta3 < MIN_DELTA3) unit->delta3 = MIN_DELTA3;
                    unit->exit_state = 1;
                    unit->flat_ok_cnt = 0;
                }
            } else {
                unit->slope_flat_cnt = 0;
            }
        } else {
            int32_t dev = (int32_t)unit->loop_CAPVD - (int32_t)unit->loop_Origin;
            int32_t cond1 = (dev >= 0) ? dev : -dev;
            unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable_1[unit->loop_SensLevel]) >> 16;
            if (cond1 < (int32_t)unit->loop_dlt_ORG && abs_fd < (int32_t)unit->delta2
                && abs_sd < (int32_t)unit->delta3) {
                unit->flat_ok_cnt++;
                if (unit->flat_ok_cnt >= FLAT_CONFIRM_CNT) {
                    PRINT("Loop%d Car_OFF_FLAT, CAPVD:%d Origin:%d d1:%d d2:%d d3:%d f':%d f'':%d\n",
                          unit->loop_num + 1, unit->loop_CAPVD, unit->loop_Origin,
                          unit->loop_dlt_ORG, unit->delta2, unit->delta3,
                          first_deriv, second_deriv);
                    unit->loop_VD_FLAG  = 0;
                    unit->loop_FLAG_OUT = 1;
                    unit->loop_VD_HOLD  = 0;
                    unit->LC_HOLD  = 0;
                    unit->loop_ORG_CNT = 0;
                    unit->loop_ORG_SUM = 0;
                    unit->Hold_CNT = 0;
                    unit->flat_ok_cnt = 0;
                    unit->exit_state  = 0;
                }
            } else {
                unit->flat_ok_cnt = 0;
            }
        }
        unit->prev_capvd       = unit->loop_CAPVD;
        unit->prev_first_deriv = first_deriv;
#else
        /*================================================================
         * 有车状态 — cnt_release 防抖离开
         *================================================================*/
        unit->loop_dlt_ORG = ((uint32_t)unit->loop_Origin * SensTable_1[unit->loop_SensLevel]) >> 16;

        if ((unit->loop_Origin - unit->loop_dlt_ORG) < unit->loop_CAPVD) {
            unit->loop_cnt_release++;
            if (unit->loop_cnt_release >= 3) {
                PRINT("Loop%d Car_OFF, Value:%d CAPVD:%d Origin:%d dlt:%d\n",
                      unit->loop_num + 1, unit->loop_Value, unit->loop_CAPVD,
                      unit->loop_Origin, unit->loop_dlt_ORG);
                unit->loop_VD_FLAG  = 0;
                unit->loop_FLAG_OUT = 1;
                unit->loop_VD_HOLD  = 0;
                unit->LC_HOLD  = 0;
                unit->loop_ORG_CNT = 0;
                unit->loop_ORG_SUM = 0;
                unit->Hold_CNT = 0;
                unit->loop_cnt_release = 0;
            }
        } else {
            if (unit->loop_cnt_release > 0) unit->loop_cnt_release = 0;
        }
#endif
    }
}

/*===========================================================================
 * loop_task_function — FreeRTOS 主任务（对齐 DLD154V4B main 循环）
 * 10ms 周期，独立处理每路线圈。
 *===========================================================================*/
void loop_task_function(void *pvParameters)
{
    uint8_t i;

    g_sys_freq = g_crm_clocks_freq_struct.sclk_freq;
    INIT_VDs(&g_loop_states);

    while (1) {
        for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
            Loop154_Unit *unit = &g_loop_states.loop_unit[i];

            if (unit->loop_LOOP_OK) {
                /* 线圈重连 */
                if (!unit->loop_LOOP_OK0) {
                    unit->loop_LOOP_OK0 = 1;
                    unit->disconnect_active = 0;
                    unit->loop_CAPVD = 0;     // 强制 IIR 重新收敛
                }

                if (unit->loop_CAP_OK) {
                    taskENTER_CRITICAL();
                    unit->loop_CAP_OK = 0;
                    taskEXIT_CRITICAL();

                    if (!unit->power_up_state) {
                        unit->power_up_state = 1;
                    }

                    /* 核心检测算法 */
                    vd1_task_per_channel(unit);
                }
            } else {
                /* 线圈断开 */
                set_loops_relay_off(unit->loop_num);

                if (unit->power_up_state && !unit->disconnect_active) {
                    unit->disconnect_active = 1;
                    if (unit->disconnect_count < 3) {
                        unit->disconnect_count++;
                    }
                    unit->fault_phase = 0;
                    unit->fault_tick  = 0;
                }
            }
        }

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

/*===========================================================================
 * LED 指示灯后台任务 — 线圈未连接时的闪烁指示
 *===========================================================================*/
void led_indicator_task_function(void *pvParameters)
{
    uint8_t i;
    static uint8_t led_step[LOOP_CAPTURE_MAX] = {0};

    while (1) {
        for (i = 0; i < LOOP_CAPTURE_MAX; i++) {
            Loop154_Unit *unit = &g_loop_states.loop_unit[i];

            if (unit->loop_LOOP_OK0 == 0) {
                if (led_step[i] % 2 == 0) {
                    at32_led_toggle(i);
                }
                if (unit->loop_stable) {
                    unit->loop_LOOP_OK0 = 1;
                    continue;
                }
                led_step[i]++;
                if (led_step[i] > 16) {
                    led_step[i] = 0;
                    unit->loop_LOOP_OK0 = 1;
                    at32_led_off(i);
                }
            }
        }
        vTaskDelay(200);
    }
}


