/********************************** (C) COPYRIGHT *******************************
 * File Name          : main.c
 * Author             : WCH
 * Version            : V1.1
 * Date               : 2020/08/06
 * Description        : 外设从机应用主函数及任务系统初始化
 *********************************************************************************
 * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd.
 * Attention: This software (modified or not) and binary are used for 
 * microcontroller manufactured by Nanjing Qinheng Microelectronics.
 *******************************************************************************/

/******************************************************************************/
/* 头文件包含 */
#include "CONFIG.h"
#include "HAL.h"
#include "gattprofile.h"
#include "peripheral.h"
#include "storage.h"
#include <stdlib.h>
#include "cmcng.h"



uint8_t g_flag_bt_state = 0;  //  0 蓝牙没有连接，1 有蓝牙连接
uint32_t g_counter_bt_timeout = 0;
uint8_t g_flag_output = 0;
uint32_t g_xn_counter = 0;
uint8_t g_function_mode = 0;

#define RLY1_ON               (R32_PA_OUT |= BV(8))    //(R32_PB_OUT &= ~BV(22))
#define RLY1_OFF              (R32_PA_OUT &= ~BV(8))     //(R32_PB_OUT |= BV(22))
#define RLY2_ON               (R32_PB_OUT |= BV(23))
#define RLY2_OFF              (R32_PB_OUT &= ~BV(23))
#define LEDA_ON               (R32_PB_OUT &= ~BV(22))
#define LEDA_OFF              (R32_PB_OUT |= BV(22))
#define LEDA_ON_OFF()         (GPIOB_ReadPortPin(GPIO_Pin_22) ? GPIOB_ResetBits(GPIO_Pin_22) : GPIOB_SetBits(GPIO_Pin_22))
#define FLEVEL_H_LOW          (R32_PA_OUT &= ~BV(12))
#define FLEVEL_H_HIGH         (R32_PA_OUT |= BV(12))
#define FLEVEL_L_LOW          (R32_PA_OUT &= ~BV(13))
#define FLEVEL_L_HIGH         (R32_PA_OUT |= BV(13))

#define KEY_SA1               (GPIOA_ReadPortPin(GPIO_Pin_14))
#define KEY_SA2               (GPIOA_ReadPortPin(GPIO_Pin_15))

const uint8_t MAX_Store_Size = 80;
uint8_t g_loop_power_up_state = 0;   // 开机启动时线圈是否连接
__IO uint32_t TimingDelayInc;


#define     MAX_CMP_LOOP_UNIT       4       // 5 组
#define     MAX_CMP_DIF             20      //
#define     MAX_TIMEOUT_CMP         120000      // 5毫秒 为单位， 表示10分钟， 600秒

#define   ALFA_CAP1  79
#define   ALFA_CAP2  64
#define   ALFA_ORG1  64
#define   ALFA_ORG2  64

uint32_t mstick(void){
    return TimingDelayInc;
}

uint32_t u32CountWdt = 0;
uint8_t g_loop_cut_last = 0;
uint32_t g_loop_cut_amount = 0;

uint8_t g_flag_stable_delta2_flt = 0;
uint8_t g_flag_reset_delta = 0;

// 继电器状态防抖计数器（多重确认机制）
static uint8_t relay_on_confirm_cnt = 0;    // 吸合确认计数
static uint8_t relay_off_confirm_cnt = 0;   // 释放确认计数
#define RELAY_ON_CONFIRM_THRESHOLD   3      // 吸合需要连续3次确认
#define RELAY_OFF_CONFIRM_THRESHOLD  5      // 释放需要连续5次确认（更严格）

// 快速变化检测（用于识别线圈中间移动）
static int32_t last_delta_for_rapid = 0;    // 上一次的一阶变化量
static uint8_t rapid_change_detected = 0;   // 检测到快速变化标志
static uint16_t rapid_change_duration = 0;  // 快速变化持续时间


/*********************************************************************
 * GLOBAL TYPEDEFS
 */
__attribute__((aligned(4))) uint32_t MEM_BUF[BLE_MEMHEAP_SIZE / 4];

#if(defined(BLE_MAC)) && (BLE_MAC == TRUE)
const uint8_t MacAddr[6] = {0x84, 0xC2, 0xE4, 0x03, 0x02, 0x02};
#else
uint8_t MACAddr[6] = {0x84, 0xC2, 0xE4, 0x03, 0x02, 0x02};
#endif

uint8_t g_dev_number[6] = "";           // 设备编号  产品编号
char g_flag_debug = 1;


uint8_t g_freq_sens;
uint8_t loop1_SensLevel;
uint8_t g_freq_level;

uint16_t g_safe_max_cnt = LC_HOLD_TIME;

uint16_t Hold_CNT  = 0;
uint8_t  Flt_Reg   = 0;
uint8_t  Delay_300ms_cnt   = 0;
uint8_t TM1cnt = 0;
uint16_t g_hold_time = 0;

uint16_t loop1_Xn;
uint16_t loop1_HoldTime;
uint16_t loop1_dlt_ORG;
uint32_t loop1_Origin;
uint16_t loop1_CapLast;

uint16_t loop1_LPCNT;
uint16_t loop1_CapCnt;
uint32_t loop1_CapSum;
uint32_t loop1_CAPVD;
uint32_t loop1_Value;
uint16_t loop1_Flt;

uint32_t loop1_ORG_SUM;
uint16_t loop1_ORG_CNT;

uint8_t loop1_INCNT;
uint8_t loop1_OUTCNT;

uint8_t loop1_INI_LOOP;
uint8_t loop1_CAP_OK;
uint8_t loop1_CAP_FLAG;
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_FLAG_PLUSE_DELAY;
uint8_t loop1_FLAG_IN_PLUSE;
uint8_t loop1_FLAG_CUT;
uint8_t loop1_FLAG_HOLD_TIMEOUT;    // 存在超时

uint8_t  loop1_LC_HOLD = 0;
uint8_t  loop1_LC_Reset = 0;
uint32_t LC_Hold_CNT = 0;

//const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54, 37, 27};      //{162, 108, 54, 37, 27}; 9
//const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 27, 14};       //{108, 81, 32, 27, 14}; 6
const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 108, 54,  28};    //{162, 108, 54, 37, 27};
const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 32, 16}; //{108, 81, 32, 27, 14};
// const uint16_t SensTable[MAX_LOOP_SENS_AMOUNT] = { 162, 81,  54};    //{162, 108, 54, 37, 27};
// const uint16_t SensTable1[MAX_LOOP_SENS_AMOUNT] = {81, 54, 32}; //{108, 81, 32, 27, 14};

#define  LOOP_WINDOW_SIZE_NORMAL        50  // 200
#define  LOOP_WINDOW_SIZE_FAST          50         // 快速补偿
uint16_t g_moving_average_window_size = LOOP_WINDOW_SIZE_NORMAL;    // 滑动窗口大小

uint32_t g_sys_freq = 0;

SecondOrderState second_order_state;
FltHistoryManager flt_mgr = {0};  // 初始化为0
StageRangeConfig stage_config[STAGE_COUNT] = {
    {0, 0,0, 20, RANGE_PERCENT},    // 开机阶段：基准值±10%
    {0, 0,0, 20, RANGE_PERCENT}//,   //最近5秒
//    {0, 0,0, 2, RANGE_PERCENT},   // 5分钟阶段：基准值±50
//    {0, 0,0, 9,  RANGE_PERCENT},    // 10分钟阶段：基准值±8%
//    {0, 0,0, 12, RANGE_ABSOLUTE},   // 30分钟阶段：基准值±30
//    {0, 0,0, 15,  RANGE_PERCENT}     // 1小时阶段：基准值±5%
};

uint16_t g_loop_out_delay = 10;     // 0、0.6秒、1.6秒、3秒{0,12,32,60};
uint32_t g_loop_release_ori_timeout = MAX_TIMEOUT_CMP;

DBN_BLE_State g_dbn_ble_state_acs_enable = {0};
Loop_ACS_Info g_loop_acs_info = {0,};

#define  MAX_ALLOW_DIFF_FREQ                3500                    //频率最大偏移值 3500Hz
uint16_t g_MaxAllowDiff = 0;            // 采样值允许最大偏移值
float g_freq_tmp = 0.0;

//uint8_t g_filter_step = 0;
//uint16_t g_loop_bad_cnt = 0;            // 采样 数值异常计数

uint8_t loop1_FLAG_PLUSE_IN_F = 0;
uint8_t loop1_FLAG_PLUSE_IN = 0;




/* 用于APP判断文件有效性 */
const uint32_t Address = 0xFFFFFFFF;

__attribute__((aligned(4))) uint32_t Image_Flag __attribute__((section(".ImageFlag"))) = (uint32_t)&Address;



uint8_t loop_cng_default[7] = {2,1,0,0,4,0,0};


void set_factory_param(void)
{
    uint8_t i;

    memcpy(&g_loop_cng_unit.sensitvity, loop_cng_default, 7);

//    g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT;  // max 5
    for(i = 0; i < g_loop_sens_list.total; i++)
    {
        g_loop_sens_list.sens[i].sens_in = SensTable[i];
        g_loop_sens_list.sens[i].sens_out = SensTable1[i];
    }

}


void factory_dev(void){
    uint8_t i = 0, k = 0;

    uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size);
    if(rBuf == NULL){
//        PRINT("Not enough memory!!");
        SYS_ResetExecute();
    }
    for(i = 0; i < MAX_Store_Size; i++){
        rBuf[i] = 0;
    }

    rBuf[0] = 0x33;
    rBuf[1] = 0xBB;
    rBuf[2] = 0xC3;
    rBuf[3] = 0x3C;

//    g_freq_sens = 1;
    i = 4;
    memcpy(&rBuf[i], &g_loop_cng_unit.sensitvity, 7);
    i += 7;

//    i = Addr_Loop_PlanB_Cng_Offset;
//    rBuf[i++] = g_loop_balance_planB.sample_cng.flag;
//    rBuf[i++] = g_loop_balance_planB.sample_cng.max_amplitude;
//    rBuf[i++] = g_loop_balance_planB.sample_cng.max_amplitude >> 8;
//    rBuf[i++] = g_loop_balance_planB.balance_ori_cng.flag;
//    rBuf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt;
//    rBuf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt >> 8;
//    rBuf[i++] = g_loop_balance_planB.release_ori_planB.flag_weight;
//    rBuf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude;
//    rBuf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude >> 8;
//    rBuf[i++] = g_loop_balance_planB.release_ori_planB.timeout;
//
//    rBuf[i++] = g_loop_balance_planB.release_change_rate.flag_weight;
//    rBuf[i++] = g_loop_balance_planB.release_change_rate.rate_first;
//    rBuf[i++] = g_loop_balance_planB.release_change_rate.rate_second;
//    rBuf[i++] = g_loop_balance_planB.release_change_rate.mode;

    i = Addr_Loop_Sens_List_Offset;
    if(g_loop_sens_list.total > MAX_LOOP_SENS_AMOUNT)
    {
        g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT;
    }
    rBuf[i++] = g_loop_sens_list.total;
    for(k = 0; k < MAX_LOOP_SENS_AMOUNT; k++)
    {
        rBuf[i++] = g_loop_sens_list.sens[k].sens_in;
        rBuf[i++] = g_loop_sens_list.sens[k].sens_in >> 8;
        rBuf[i++] = g_loop_sens_list.sens[k].sens_out;
        rBuf[i++] = g_loop_sens_list.sens[k].sens_out >> 8;
    }

    write_cfg_to_store(0, rBuf, MAX_Store_Size);

    free(rBuf);

}

void poll_sens_key(void)
{
    static uint8_t _last_sens = 3;

    #if DEBUG
        g_freq_sens = 2;  // 默认最高灵敏度
    #else
    if(KEY_SA1){
        if(KEY_SA2){
            g_freq_sens = 0;
        }
        else {
            g_freq_sens = 2;
        }
    }
    else{
        if (KEY_SA2) {
            g_freq_sens = 1;
        }
    }
    #endif

    if(_last_sens == 3){
        _last_sens = g_freq_sens;
    }

    if(_last_sens != g_freq_sens){
        //TODO: reboot?
        _last_sens = g_freq_sens;
        reload_lower_upper(g_freq_sens);
    }

}


void para_store_init(void)
{
    uint8_t i;
    uint8_t *rBuf = (uint8_t *)malloc(MAX_Store_Size);

    if(rBuf == NULL){
//        PRINT("Not enough memory!!");
        SYS_ResetExecute();
    }
    memset(rBuf, 0, MAX_Store_Size);

    read_cfg_from_store(0, rBuf, MAX_Store_Size);
//    PRINT("Read_Cng_Store:\n");
//    for(i = 0; i < MAX_Store_Size; i++){
//        PRINT("%02X ", rBuf[i]);
//    }
//    PRINT("\n");

    if(rBuf[0] == 0x33 && rBuf[1] == 0xBB && rBuf[2] == 0xC3 && rBuf[3] == 0x3C){
        memcpy(&g_loop_cng_unit, &rBuf[4], sizeof(g_loop_cng_unit));
//        memcpy(&g_loop_balance_planB, &rBuf[Addr_Loop_PlanB_Cng_Offset], sizeof(g_loop_balance_planB));
        memcpy(&g_loop_sens_list, &rBuf[Addr_Loop_Sens_List_Offset], sizeof(g_loop_sens_list));
//        g_freq_sens = g_loop_cng_unit.sensitvity & 0x0F;
//        if(g_freq_sens > g_loop_sens_list.total ){
//            g_freq_sens = g_loop_sens_list.total;
//        }
//        if(g_freq_sens){
//            g_freq_sens--;
//        }
        poll_sens_key();

        g_loop_out_delay = g_loop_cng_unit.delay_time * 2;                  // 定时器以 50ms 为单位， 延时以100ms为单位
//        g_loop_release_ori_timeout = g_loop_balance_planB.release_ori_planB.timeout * 60 * 1000 / 5;
        g_hold_time = g_loop_cng_unit.exist_mode * 30 * 20;  //50ms/per,  30s
        // g_hold_time = g_loop_cng_unit.exist_mode * 100;  //50ms/per,   5s
        // if(g_hold_time < 400){
        //     g_hold_time = 400;
        // }
        g_safe_max_cnt = g_loop_cng_unit.loopSafe_Timeout * 10 * (1000 / 50);
        g_freq_level = g_loop_cng_unit.loopFreq_Level - 1;
        g_function_mode = g_loop_cng_unit.direction_mode >> 4;

        flt_mgr.max_cnt_flat_release = g_loop_cng_unit.delay_time * 20 /2;
        if(flt_mgr.max_cnt_flat_release < 40){  // 至少检查400毫秒
            flt_mgr.max_cnt_flat_release = 40;
        }

        #ifdef DEBUG
            g_freq_level = 0;
        #endif

        switch(g_freq_level)
        {
            case 0: {
                // 高频
                FLEVEL_H_LOW; FLEVEL_L_LOW;
            } break;
            case 1: {
                // 中高
                FLEVEL_H_LOW; FLEVEL_L_HIGH;
            } break;
            case 2: {
                // 中低
                FLEVEL_H_HIGH; FLEVEL_L_LOW;
            } break;
            case 3: {
                // 低频
                FLEVEL_H_HIGH; FLEVEL_L_HIGH;
            } break;
            default:{
                // 默认低频
                g_freq_level = 3;
                FLEVEL_H_HIGH; FLEVEL_L_HIGH;
            } break;
        }
        free(rBuf);
    }
    else{
        free(rBuf);
        set_factory_param();
//        DelayMs(10);
        factory_dev();
//
        DelayMs(10);
        SYS_ResetExecute();
    }

}



void tm_init(void){

    // Cap 捕获
    GPIOA_ResetBits(GPIO_Pin_9);
    GPIOA_ModeCfg(GPIO_Pin_9, GPIO_ModeIN_Floating);

    TMR0_CapInit(FallEdge_To_FallEdge);
    TMR0_CAPTimeoutCfg(0xFFFFFFFF);
    TMR0_ClearITFlag(TMR0_3_IT_DATA_ACT);
    TMR0_ITCfg(ENABLE, TMR0_3_IT_DATA_ACT);
    PFIC_EnableIRQ(TMR0_IRQn);


    // TMR1
    TMR1_TimerInit(FREQ_SYS / 200);         // 设置定时时间 5ms
    TMR1_ITCfg(ENABLE, TMR0_3_IT_CYC_END); // 开启中断
    PFIC_EnableIRQ(TMR1_IRQn);
}

void gpio_uart_init(void)
{
    /* 配置串口0：先配置IO口模式，再配置串口 */
//    GPIOPinRemap(ENABLE, RB_PIN_UART0);
    #ifdef DEBUG
    GPIOPinRemap(ENABLE, RB_PIN_UART0);
    GPIOA_SetBits(GPIO_Pin_14);
    GPIOA_ModeCfg(GPIO_Pin_14, GPIO_ModeOut_PP_5mA); // TXD-配置推挽输出，注意先让IO口输出高电平
    // UART0_DefInit();

    UART0_BaudRateCfg(230400); //115200
    R8_UART0_FCR = (2 << 6) | RB_FCR_TX_FIFO_CLR | RB_FCR_RX_FIFO_CLR | RB_FCR_FIFO_EN; // FIFO打开，触发点4字节
    R8_UART0_LCR = RB_LCR_WORD_SZ;
    R8_UART0_IER = RB_IER_TXD_EN;
    R8_UART0_DIV = 1;

    GPIOA_ModeCfg(GPIO_Pin_15, GPIO_ModeIN_PU);      // 拨动开关，调节灵敏度
    #else

    GPIOA_ModeCfg(GPIO_Pin_15|GPIO_Pin_14, GPIO_ModeIN_PU);      // 拨动开关，调节灵敏度

    #endif
    

    GPIOA_SetBits(GPIO_Pin_12|GPIO_Pin_13);
    GPIOA_ModeCfg(GPIO_Pin_12|GPIO_Pin_13, GPIO_ModeOut_PP_5mA);
    FLEVEL_H_LOW; FLEVEL_L_LOW;

    GPIOA_SetBits(GPIO_Pin_8);
    GPIOA_ModeCfg(GPIO_Pin_8, GPIO_ModeOut_PP_5mA);
    GPIOB_SetBits(GPIO_Pin_22|GPIO_Pin_23);
    GPIOB_ModeCfg(GPIO_Pin_22|GPIO_Pin_23, GPIO_ModeOut_PP_5mA);
    RLY1_OFF; RLY2_OFF; LEDA_OFF;

#if 0 // 中断方式：接收数据后发送出去
    UART0_ByteTrigCfg(UART_7BYTE_TRIG);
    trigB = 7;
    UART0_INTCfg(ENABLE, RB_IER_RECV_RDY | RB_IER_LINE_STAT);
    PFIC_EnableIRQ(UART0_IRQn);
#endif


}




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;
}

void init_second_order(SecondOrderState *state, int32_t initial_delta) {
    state->delta_prev = initial_delta;
    state->delta2_flt = 0;
}

/**
 * @brief 计算滤波后的二阶变化量（加速度）
 * @param delta_current 当前一阶变化量（new_Flt - previous_new_Flt）
 * @param state         二阶计算状态（需外部维护）
 * @param filter_coef   二阶滤波系数（0~255，对应浮点系数 filter_coef/256）
 * @return 滤波后的二阶变化量 delta2_flt
 */
int32_t get_second_order(int32_t delta_current, SecondOrderState *state, uint8_t filter_coef) {
    // 计算当前二阶变化量（一阶变化的差分）
    int32_t delta2 = delta_current - state->delta_prev;

    // --- 关键改进点：处理一阶持续为0的情况 ---
    if (delta_current == 0 && state->delta_prev == 0) {
        // 连续两次一阶变化为0，强制二阶归零
        state->delta2_flt = 0;
    } else {
        // 常规滤波：delta2_flt = delta2_flt + (delta2 - delta2_flt) * filter_coef / 256
        state->delta2_flt += ((delta2 - state->delta2_flt) * filter_coef)/256; //((delta2 - state->delta2_flt) * (uint32_t)filter_coef) >> 8;
    }

    // 更新历史一阶变化量
    state->delta_prev = delta_current;

    return state->delta2_flt;
}



/**
 * @brief 更新滑动平均值（通过指针传递状态）
 * @param p_sum     累加值指针（uint32_t*）
 * @param p_cnt     计数器指针（uint8_t*）
 * @param p_origin  平均值指针（uint16_t*）
 * @param new_value 新输入值
 * @param window    窗口大小（例如100）
 */
void 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; // 可添加错误处理
    }

    *p_sum += new_value; // 累加新值
    (*p_cnt)++;

    if (*p_cnt >= window) {  // 窗口满（如window=100时，计数器0~99）
        *p_origin = (uint32_t)(*p_sum / window); // 计算平均值
        *p_cnt = 0;
        *p_sum = 0;
        // PRINT("window:%d, new_origin:%d\n", window, *p_origin);
    }
}


void update_lower_upper(StageRangeConfig * cfg)
{
    if(cfg->base_value == 0) return;

    // 计算区间上下限
    uint32_t lower, upper;
    if (cfg->range_type == RANGE_ABSOLUTE) {
        cfg->lower = (cfg->base_value >= cfg->range_size) ?
                (cfg->base_value - cfg->range_size) : 0;
        cfg->upper = cfg->base_value + cfg->range_size;
    } else { // RANGE_PERCENT
//        uint32_t offset = (cfg->base_value * cfg->range_size) >> 16;
        uint32_t dlt = (cfg->base_value * (g_loop_sens_list.sens[g_freq_sens].sens_out - 3))>>16;   // 采取比当前释放灵敏度更高的值，解决平坦算法释放的时候不断误触发的bug
        cfg->lower = (cfg->base_value >= dlt) ?
                (cfg->base_value - dlt) : 0;
        cfg->upper = cfg->base_value + dlt;
    }
}

void reload_lower_upper(uint8_t sens_level){
    uint32_t dlt = 0 ;
//    stage_config[0].lower =
    uint8_t i;
    for(i = 0; i < STAGE_COUNT; i++){
        if(stage_config[i].base_value == 0){
            continue;
        }
        dlt = (stage_config[i].base_value * (g_loop_sens_list.sens[sens_level].sens_out - 3))>>16;
        stage_config[i].lower = (stage_config[i].base_value >= dlt) ? (stage_config[i].base_value - dlt): 0;
        stage_config[i].upper = stage_config[i].base_value + dlt;
    }
}



/**
 * @brief 更新一阶滤波历史管理器
 * @param new_flt 当前一阶滤波值
 * @param current_time 系统时间戳（毫秒）
 */
void update_flt_history(uint32_t new_flt, int32_t firstOrder, int32_t secondOrder ) {
    static uint8_t _first_cnt = 0;
    if(firstOrder || secondOrder){
        if(_first_cnt < 3){ //连续三次有变动
            _first_cnt++;
        }

        flt_mgr.cnt_not_idle++;
        if(flt_mgr.cnt_idle)
        {
            flt_mgr.cnt_idle--;
            if(flt_mgr.cnt_not_idle > 5){
                flt_mgr.cnt_idle = 0;
            }
        }
    }
    else {
        if(_first_cnt){
            _first_cnt--;
        }
    }

    if((firstOrder || secondOrder) && (_first_cnt >= 3))
    {
        flt_mgr.counter_not_idle = 0;
        flt_mgr.flag_not_idle = 1;
    }
    if(flt_mgr.flag_not_idle)
    {
        flt_mgr.cnt_sec_samples = 0;
        flt_mgr.sum_sec_samples = 0;
        if(!flt_mgr.is_initialized)
        {
            flt_mgr.sum_samples = 0;
            flt_mgr.init_sum  = 0;
        }
        else
        {
            flt_mgr.sum_samples = 0;
            flt_mgr.cnt_samples = 0;
        }
        return ;
    }

    flt_mgr.cnt_idle++;
    if(flt_mgr.cnt_not_idle){
        flt_mgr.cnt_not_idle--;
        if(flt_mgr.cnt_idle > 100){
            flt_mgr.cnt_not_idle = 0;
        }
    }

    // 1. 初始化阶段：累加采样值
    if (!flt_mgr.is_initialized) {
        flt_mgr.init_sum += new_flt;
        flt_mgr.init_samples++;

        // 达到采样次数后计算稳定初始值
        if (flt_mgr.init_samples >= INIT_SAMPLE_COUNT) {
            flt_mgr.stable_init_value = flt_mgr.init_sum / INIT_SAMPLE_COUNT;
            flt_mgr.is_initialized = 1;
            LEDA_OFF;
            stage_config[0].base_value = flt_mgr.stable_init_value;
            update_lower_upper(&stage_config[0]);
            flt_mgr.init_freq = (float)(g_sys_freq )/((float)(flt_mgr.stable_init_value << 5)/((float)(loop1_LPCNT))) + 360;
        }
        return ;
    }

    if(flt_mgr.flag_5sec)
    {
        flt_mgr.cnt_sec_samples++;
        flt_mgr.sum_sec_samples += new_flt;
        if(flt_mgr.cnt_sec_samples >= TMP_SAMPLE_COUNT)
        {
            flt_mgr.recent_5sec = flt_mgr.sum_sec_samples/TMP_SAMPLE_COUNT;

            stage_config[1].base_value = flt_mgr.recent_5sec;
            update_lower_upper(&stage_config[1]);
            flt_mgr.flag_5sec = 0;
            flt_mgr.cnt_sec_samples = 0;
            flt_mgr.sum_sec_samples = 0;
        }
    }

}


/**
 * @brief 判断新值是否在指定阶段的区间内
 * @param new_flt 新的一阶滤波值
 * @param result  输出匹配结果
 */
uint8_t check_flt_by_stage(uint16_t new_flt) {

    for (uint8_t i = 1; i < STAGE_COUNT_SHORT; i++) {   //offset从1开始，表示开机的稳定参考值不参与释放判断
        StageRangeConfig *cfg = &stage_config[i];
        if (cfg->base_value == 0) continue; // 忽略未初始化的阶段


        // 判断是否在区间内
        if (new_flt >= cfg->lower && new_flt <= cfg->upper) {
            return (i+1);
        }
    }
    return 0;
}


uint8_t check_flat_leave(FltHistoryManager* fltMgr, int16_t firstOrder, int16_t secondOrder, uint16_t datCurrent)
{
    static uint8_t _flag = 0;
    if(firstOrder || secondOrder)
    {
//        if(firstOrder > 0)
//        {
//            fltMgr->cnt_release_Up++;
//        }
//        else if(firstOrder < 0)
//        {
//            fltMgr->min_release_capvd = datCurrent;
//            fltMgr->cnt_release_Down++;
//        }

        fltMgr->cnt_flat_release = 0;
        fltMgr->flag_flat_release = 1;

        return 0;
    }
    if(fltMgr->flag_flat_release)
    {        
        return 0;
    }
    
    fltMgr->flag_second_judge = 1;
    if(g_function_mode & FUNCTION_B){
        // 初始化二次判断的时间计数器
        fltMgr->second_judge_enter_time = 0;
        fltMgr->cnt_stable_to_idle = 0;
    }
    return 0;
}



void vd1_task(void){
    static uint32_t new_Flt_prev = 0;  // 保存上一次滤波值
    int32_t delta_current = 0;
    int32_t delta2_flt = 0;

    if(g_loop_acs_info.flag_change == 0){
        g_loop_acs_info.loop_capvd = loop1_Value;  //记录 有车无车变化的参数
    }

    // if(g_function_mode & FUNCTION_B)
    {
        if(g_flag_reset_delta == 0){
            new_Flt_prev = loop1_Origin;
            loop1_CAPVD = loop1_Origin;
            g_flag_reset_delta = 1;
        }
    }

    loop1_CAPVD = get_flt_value(loop1_Value, loop1_CAPVD);

    // 4. 一阶变化量计算（new_Flt - 前次值）
    if(new_Flt_prev == 0)
    {
        new_Flt_prev = loop1_CAPVD;
    }
    delta_current = (int32_t)(loop1_CAPVD - new_Flt_prev);

    if(g_function_mode & FUNCTION_B){
        // 检测快速变化：如果变化量符号频繁反转且幅度较大，说明是线圈中间移动
        if(abs(delta_current) > 10 && abs(last_delta_for_rapid) > 10)
        {
            if((delta_current > 0 && last_delta_for_rapid < 0) || 
            (delta_current < 0 && last_delta_for_rapid > 0))
            {
                rapid_change_detected = 1;
                rapid_change_duration++;
            }
            else
            {
                // 变化方向一致，逐渐清除快速变化标志
                if(rapid_change_duration > 0)
                {
                    rapid_change_duration--;
                }
                if(rapid_change_duration == 0)
                {
                    rapid_change_detected = 0;
                }
            }
        }
        last_delta_for_rapid = delta_current;
    }
    
    // if(abs(delta_current) > 30){
    //     PRINT("Xn_:%d, tmp_value:%d, loop1_Value:%d, last_prev:%d, loop1_CAPVD:%d, delta_current:%d, freq:%d, rapid:%d\n",loop1_Xn, tmp_value, tmp_loop1_CAPVD, new_Flt_prev, loop1_CAPVD, delta_current, flt_mgr.current_freq, rapid_change_detected);
    // }

    new_Flt_prev = loop1_CAPVD;  // 更新历史值

    if(flt_mgr.flag_loop_reconnect)  // 处理线圈重连的情况
    {
        if(flt_mgr.flag_loop_reconnect >= 2)
        {

            if(!flt_mgr.last_loop_vd_flag)
            {
                LEDA_OFF;
                flt_mgr.flag_loop_reconnect = 0;
                return;
            }

            loop1_dlt_ORG = ((uint32_t)flt_mgr.last_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;

            delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
            new_Flt_prev = loop1_CAPVD;  // 更新历史值

            // if(( flt_mgr.last_loop_Origin - loop1_dlt_ORG ) < loop1_CAPVD ){

            // }
            if(( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD)    //
            { // 车离开
                //TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？
                // 1、如果增加平坦算法，会有延时，不能及时落杆；

                if(loop1_VD_FLAG)
                {
                    loop1_FLAG_OUT = 1;
                }
                loop1_VD_FLAG = 0;

                g_loop_acs_info.car_state = 0;
//                LED_LP1_OFF();
                loop1_ORG_CNT = 0;
                loop1_ORG_SUM = 0;
                loop1_VD_HOLD = 0 ;
                Hold_CNT = 0;

                flt_mgr.flag_5sec = 0;
                flt_mgr.counter_5sec = 0;
                flt_mgr.last_loop_vd_flag = 0;
                flt_mgr.flag_loop_reconnect = 0;

                loop1_Origin = flt_mgr.last_loop_Origin;
            } // end if(( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD)  
            else if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD))
            {
//                PRINT("FLAT_TRIGER_CAR_OFF_init_re:%d,5sec:%d,5min:%d,10min:%d,30min:%d,60min:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n",
//                flt_mgr.stable_init_value, flt_mgr.recent_5sec, flt_mgr.recent_5min, flt_mgr.recent_10min, flt_mgr.recent_30min, flt_mgr.recent_60min,
//                 loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);

//                loop1_FLAG_OUT = 1;
                if(loop1_VD_FLAG)
                {
                    loop1_FLAG_OUT = 1;
                }
                loop1_VD_FLAG = 0;

                g_loop_acs_info.car_state = 0;

//                LED_LP1_OFF();
                loop1_ORG_CNT = 0;
                loop1_ORG_SUM = 0;
                loop1_VD_HOLD = 0 ;
                Hold_CNT = 0;

                flt_mgr.flag_5sec = 0;
                flt_mgr.counter_5sec = 0;
                flt_mgr.last_loop_vd_flag = 0;
                flt_mgr.flag_loop_reconnect = 0;
                loop1_Origin = flt_mgr.last_loop_Origin;
            } // end else if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD))
            else
            {
                if(!loop1_VD_FLAG)
                {
                    if(( flt_mgr.last_loop_Origin - loop1_dlt_ORG ) < loop1_CAPVD ){
                        if(loop1_VD_FLAG)
                        {
                            loop1_FLAG_OUT = 1;
                        }
                        loop1_VD_FLAG = 0;
                        g_loop_acs_info.car_state = 0;

        //                LED_LP1_OFF();
                        loop1_ORG_CNT = 0;
                        loop1_ORG_SUM = 0;
                        loop1_VD_HOLD = 0 ;
                        Hold_CNT = 0;

                        flt_mgr.flag_5sec = 0;
                        flt_mgr.counter_5sec = 0;
                        flt_mgr.last_loop_vd_flag = 0;
                        flt_mgr.flag_loop_reconnect = 0;
                        loop1_Origin = flt_mgr.last_loop_Origin;

                        return;
                    }

                    if(loop1_CAPVD < (flt_mgr.last_loop_Origin - loop1_dlt_ORG))    //
                    {// 有车
                        if(loop1_VD_FLAG == 0){
                            g_loop_acs_info.flag_change = 1;
                        }

                        loop1_VD_FLAG = 1;
                        loop1_FLAG_IN = 1;
                        g_loop_acs_info.car_state = 1;
                        if(g_loop_cng_unit.exist_mode)        //有限存在10分钟
                        {
                            loop1_VD_HOLD = 1 ;
                        }

                    }
                } // end if(!loop1_VD_FLAG)
                flt_mgr.flag_loop_reconnect = 4;
                if(flt_mgr.flag_second_judge)
                {
                    if(!flt_mgr.second_loop_vd_flag)
                    {
                        update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size);

                        // 5. 调用二阶变化量计算函数
                        delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
//                        PRINT("Second_CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);

                        if(!delta_current || !delta2_flt)
                        {
                            return;
                        }
                        if(flt_mgr.second_loop_Origin == 0)
                        {
                            return;
                        }

                        flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens & 0x0F].sens_in) >> 16;     //loop1_dlt_ORG??:?--26;?--13;?--6
                        if(loop1_CAPVD < (flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG))    //
                        {// 有车
                            flt_mgr.second_loop_vd_flag = 1;
                        }
                    } // end if(!flt_mgr.second_loop_vd_flag)
                    else
                    {
                        flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;

                        delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
                        new_Flt_prev = loop1_CAPVD;  // 更新历史值
                //        PRINT("CarON_LPCNT:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);

                        if(( flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG ) < loop1_CAPVD)    //
                        { // 车离开
                            //TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？
                            // 1、如果增加平坦算法，会有延时，不能及时落杆；
                            if(loop1_VD_FLAG == 1){
                                g_loop_acs_info.flag_change = 1;
                            }
                            loop1_VD_FLAG = 0;
                            loop1_FLAG_OUT = 1;
                            g_loop_acs_info.car_state = 0;

//                            LED_LP1_OFF();
                            loop1_ORG_CNT = 0;
                            loop1_ORG_SUM = 0;
                            loop1_VD_HOLD = 0 ;
                            Hold_CNT = 0;

                            flt_mgr.flag_5sec = 0;
                            flt_mgr.counter_5sec = 0;
                            flt_mgr.last_loop_vd_flag = 0;
                            flt_mgr.second_loop_vd_flag = 0;
                            flt_mgr.flag_second_judge = 0;
                            flt_mgr.second_loop_ORG_CNT = 0;
                            flt_mgr.second_loop_ORG_SUM = 0;

                            loop1_Origin = flt_mgr.second_loop_Origin;
                        }
                    } // end else
                } // end if(flt_mgr.flag_second_judge)
            } // end else
        } // end if(flt_mgr.flag_loop_reconnect >= 2)
        return;
    }



    if(!loop1_VD_FLAG){

        update_moving_average(&loop1_ORG_SUM, &loop1_ORG_CNT, &loop1_Origin, loop1_CAPVD, g_moving_average_window_size);

        // 5. 调用二阶变化量计算函数
        delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
        update_flt_history(loop1_CAPVD, delta_current, delta2_flt);
//        PRINT("CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);

        if(!delta_current)  // || !delta2_flt)
        {
            return;
        }
//        PRINT("CarOFF_5sec:%d, Origin:%d, loop1_CAPVD:%d\n", flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD);

        if(!flt_mgr.is_initialized)
        {
            return;
        }

        if(loop1_LOOP_OK0 == 0){
            return;
        }

        loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens & 0x0F].sens_in) >> 16;
        if(g_flag_bt_state){        
            if(g_loop_acs_info.flag_change == 0){
                g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD);
                if(g_loop_acs_info.variation > 5 || g_loop_acs_info.variation < -5){
                    g_loop_acs_info.flag_event = 1;
                }
            }
        }
        else{
            g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD);
        }

        if(loop1_CAPVD < (loop1_Origin - loop1_dlt_ORG))    //
        {
            if(g_function_mode & FUNCTION_B){
                if(flt_mgr.cnt_simple_release)
                {
                    flt_mgr.cnt_simple_release = 0;
                }
                flt_mgr.cnt_simple_busy++;
                if(flt_mgr.cnt_simple_busy < 3){
                    return;
                }

                // 改进：增加吸合多重确认机制，避免误触发
                // 如果检测到快速变化（线圈中间移动），不立即吸合
                if(rapid_change_detected && rapid_change_duration < 10)
                {
                    relay_on_confirm_cnt = 0;  // 重置确认计数
                    return;  // 等待稳定后再判断
                }
                
                relay_on_confirm_cnt++;
                if(relay_on_confirm_cnt < RELAY_ON_CONFIRM_THRESHOLD){
                    return;  // 需要连续多次确认才吸合
                }
                relay_on_confirm_cnt = RELAY_ON_CONFIRM_THRESHOLD;  // 达到阈值后保持最大值
                
                // 检测到车辆进入，重置释放确认计数
                relay_off_confirm_cnt = 0;

                // 重置二次判断增强条件的计数器
                flt_mgr.second_judge_enter_time = 0;
                flt_mgr.cnt_stable_to_idle = 0;
            }

            flt_mgr.last_loop_Origin = loop1_Origin;   // Mark
            flt_mgr.last_loop_vd_flag = 1;
            flt_mgr.second_loop_ORG_CNT = 0;
            flt_mgr.second_loop_ORG_SUM = 0;
            flt_mgr.second_loop_Origin = 0;
            flt_mgr.second_loop_vd_flag = 0;
            flt_mgr.flag_second_judge = 0;

            if(loop1_VD_FLAG == 0){
                g_loop_acs_info.flag_change = 1;
            }

            loop1_VD_FLAG = 1;         //???????1
            loop1_FLAG_IN = 1;         //??????
//            PRINT("Car_In,sens_level:%d, counter:%d\n", g_freq_sens, g_tmp_counter);
            if(g_loop_cng_unit.exist_mode)
            {
                loop1_VD_HOLD = 1;      // 有限存在10分钟
            }

//            LEDA_ON;
            g_loop_acs_info.flag_event = 1;
            g_loop_acs_info.car_state = 1;

            // if(g_loop_cng_unit.loopSafe_Timeout > 0)
            // {
            //     loop1_LC_HOLD = 1;
            // }
            // else
            // {
            //     loop1_LC_HOLD = 0;
            // }
            if(loop1_LC_Reset)
                loop1_LC_Reset = 0;
        }
    }// end if !loop1_VD_FLAG
    else{
        loop1_dlt_ORG = ((uint32_t)loop1_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;
        if(g_loop_acs_info.flag_change == 0)
        {
            g_loop_acs_info.variation = (int16_t)(loop1_Origin - loop1_CAPVD);
            if(g_loop_acs_info.variation > 5 || g_loop_acs_info.variation < -5){
                g_loop_acs_info.flag_event = 1;
            }
        }

        delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
        new_Flt_prev = loop1_CAPVD;  // 更新历史值

        if(g_function_mode & FUNCTION_B){
            if(flt_mgr.cnt_simple_busy)
            {
                flt_mgr.cnt_simple_busy = 0;
            }
        }

        if((( loop1_Origin - loop1_dlt_ORG ) < loop1_CAPVD ))    //
        {
            if(g_function_mode & FUNCTION_B){
                // 改进：如果检测到快速变化，说明可能是线圈中间移动，不是真正的离开
                // 此时不应该释放继电器，而是继续监测
                if(rapid_change_detected)
                {
                    // 快速变化期间，重置释放确认计数，防止误释放
                    relay_off_confirm_cnt = 0;
                    PRINT("RAPID_CHANGE_IGNORE_RELEASE\n");
                    return;  // 不释放，继续监测
                }
                
                // 多重确认机制：需要连续多次满足释放条件才真正释放
                relay_off_confirm_cnt++;
                if(relay_off_confirm_cnt < RELAY_OFF_CONFIRM_THRESHOLD){
                    PRINT("RELEASE_PENDING cnt:%d/%d\n", relay_off_confirm_cnt, RELAY_OFF_CONFIRM_THRESHOLD);
                    return;  // 还未达到确认次数，暂不释放
                }
            }

            if(loop1_VD_FLAG == 1){
                g_loop_acs_info.flag_change = 1;
            }
            loop1_VD_FLAG = 0;        //???????0
            loop1_FLAG_OUT = 1;       //??????
            loop1_VD_HOLD = 0 ;
            loop1_LC_HOLD = 0;

            if(g_function_mode & FUNCTION_B){
                // 释放后重置所有确认计数器
                relay_on_confirm_cnt = 0;
                relay_off_confirm_cnt = 0;
                rapid_change_detected = 0;
                rapid_change_duration = 0;
            }

            g_loop_acs_info.flag_event = 1;
            g_loop_acs_info.car_state = 0;
            //          g_led_A_state = 0;
//            LEDA_OFF;
            loop1_ORG_CNT = 0;
            loop1_ORG_SUM = 0;
            Hold_CNT = 0;

            flt_mgr.flag_5sec = 0;
            flt_mgr.counter_5sec = 0;
            flt_mgr.last_loop_vd_flag = 0;
            flt_mgr.second_loop_vd_flag = 0;
            flt_mgr.second_loop_ORG_CNT = 0;
            flt_mgr.second_loop_ORG_SUM = 0;
            flt_mgr.flag_second_judge = 0;
        }
        else if(g_function_mode & FUNCTION_A){
            if(check_flat_leave(&flt_mgr, delta_current, delta2_flt, loop1_CAPVD)){
    //            PRINT("FLAT_TRIGER_CAR_OFF_init:%d,5sec:%d,5min:%d,10min:%d,30min:%d,60min:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n",
    //            flt_mgr.stable_init_value, flt_mgr.recent_5sec, flt_mgr.recent_5min, flt_mgr.recent_10min, flt_mgr.recent_30min, flt_mgr.recent_60min,
    //             loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);
                if(g_function_mode & FUNCTION_B){
                // 平坦算法触发释放也需要多重确认
                    relay_off_confirm_cnt++;
                    if(relay_off_confirm_cnt < RELAY_OFF_CONFIRM_THRESHOLD){
                        return;
                    }
                }
                
                if(loop1_VD_FLAG == 1){
                    g_loop_acs_info.flag_change = 1;
                }
                loop1_VD_FLAG = 0;
                loop1_FLAG_OUT = 1;
                g_loop_acs_info.car_state = 0;

    //            LEDA_OFF;
                loop1_ORG_CNT = 0;
                loop1_ORG_SUM = 0;
                loop1_VD_HOLD = 0 ;
                Hold_CNT = 0;

                if(g_function_mode & FUNCTION_B){
                // 释放后重置所有确认计数器
                    relay_on_confirm_cnt = 0;
                    relay_off_confirm_cnt = 0;
                    rapid_change_detected = 0;
                    rapid_change_duration = 0;
                }

                flt_mgr.flag_5sec = 0;
                flt_mgr.counter_5sec = 0;
                flt_mgr.last_loop_vd_flag = 0;
                flt_mgr.second_loop_vd_flag = 0;
                flt_mgr.second_loop_ORG_CNT = 0;
                flt_mgr.second_loop_ORG_SUM = 0;
                flt_mgr.flag_second_judge = 0;
            }
            else{   //
                if(flt_mgr.flag_second_judge){ //有车情况下二次判断
                    if(g_function_mode & FUNCTION_B){
                    // 条件1：检查是否满足最小时间限制（10秒）
                        if(flt_mgr.second_judge_enter_time < flt_mgr.second_judge_min_time)
                        {
                            // 未达到最小时间，继续监测，不进入二次判断释放逻辑
                            // 但仍需要更新二次判断的基准值
                            if(!flt_mgr.second_loop_vd_flag)
                            {
                                update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size);
                            }
                            return;
                        }
                    }

                    if(!flt_mgr.second_loop_vd_flag)
                    {
                        update_moving_average(&flt_mgr.second_loop_ORG_SUM, &flt_mgr.second_loop_ORG_CNT, &flt_mgr.second_loop_Origin, loop1_CAPVD, g_moving_average_window_size);

                        // 5. 调用二阶变化量计算函数
                        delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
    //                    PRINT("Second_CarOFF_LPCNT:%d, 5sec:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, flt_mgr.recent_5sec, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);

                        if(!delta_current || !delta2_flt)
                        {
                            return;
                        }
                        if(flt_mgr.second_loop_Origin == 0)
                        {
                            return;
                        }

                        flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;     //loop1_dlt_ORG??:?--26;?--13;?--6
                        if(loop1_CAPVD < (flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG))    //
                        {// 有车
                            flt_mgr.second_loop_vd_flag = 1;
                            if(g_function_mode & FUNCTION_B){
                            // 重置稳定计数
                                flt_mgr.cnt_stable_to_idle = 0;
                            }
                        }
                    }
                    else {
                        flt_mgr.second_loop_dlt_ORG = ((uint32_t)flt_mgr.second_loop_Origin * g_loop_sens_list.sens[g_freq_sens].sens_out) >> 16;

                        delta2_flt = get_second_order(delta_current, &second_order_state, SECOND_ORDER_FILTER_COEF);
                        new_Flt_prev = loop1_CAPVD;  // 更新历史值
                //        PRINT("CarON_LPCNT:%d, Value:%d, Origin:%d, loop1_CAPVD:%d, delta_1：%d, delta_2:%d\n", loop1_LPCNT, loop1_Value, loop1_Origin, loop1_CAPVD, delta_current, delta2_flt);

                        if(( flt_mgr.second_loop_Origin - flt_mgr.second_loop_dlt_ORG ) < loop1_CAPVD)    //
                        { // 车离开
                            if(g_function_mode & FUNCTION_B){
                            // 条件2：检查是否连续多次检测到恢复到空闲值
                                // 计算当前值与初始空闲值的偏差
                                uint32_t deviation_from_init = 0;
                                if(flt_mgr.stable_init_value > loop1_CAPVD)
                                {
                                    deviation_from_init = flt_mgr.stable_init_value - loop1_CAPVD;
                                }
                                else
                                {
                                    deviation_from_init = loop1_CAPVD - flt_mgr.stable_init_value;
                                }
                                
                                // 判断是否接近空闲值（偏差小于初始值的5%）
                                uint32_t idle_threshold = (flt_mgr.stable_init_value * 5) / 100;
                                if(deviation_from_init <= idle_threshold)
                                {
                                    flt_mgr.cnt_stable_to_idle++;
                                    PRINT("STABLE_TO_IDLE cnt:%d/%d, dev:%d, thresh:%d\n", 
                                        flt_mgr.cnt_stable_to_idle, flt_mgr.stable_to_idle_threshold,
                                        deviation_from_init, idle_threshold);
                                }
                                else
                                {
                                    // 偏差不够，重置计数
                                    flt_mgr.cnt_stable_to_idle = 0;
                                    return;
                                }
                                
                                // 需要连续多次确认才释放
                                if(flt_mgr.cnt_stable_to_idle < flt_mgr.stable_to_idle_threshold)
                                {
                                    return;  // 还未达到稳定次数，暂不释放
                                }
                            }

                            //TODO: 这里是否考虑平坦算法？ 增加可选项，比如是否有大车的情况、是考虑触发优先还是防砸优先？
                            // 1、如果增加平坦算法，会有延时，不能及时落杆；
                            if(loop1_VD_FLAG == 1){
                                g_loop_acs_info.flag_change = 1;
                            }
                            loop1_VD_FLAG = 0;
                            loop1_FLAG_OUT = 1;

                            g_loop_acs_info.car_state = 0;
    //                        LEDA_OFF;
                            loop1_ORG_CNT = 0;
                            loop1_ORG_SUM = 0;
                            loop1_VD_HOLD = 0 ;
                            Hold_CNT = 0;

                            flt_mgr.flag_5sec = 0;
                            flt_mgr.counter_5sec = 0;
                            flt_mgr.last_loop_vd_flag = 0;
                            flt_mgr.second_loop_vd_flag = 0;
                            flt_mgr.flag_second_judge = 0;
                            flt_mgr.second_loop_ORG_CNT = 0;
                            flt_mgr.second_loop_ORG_SUM = 0;

                            if(g_function_mode & FUNCTION_B){
                            // 重置二次判断增强条件的计数器
                                flt_mgr.second_judge_enter_time = 0;
                                flt_mgr.cnt_stable_to_idle = 0;
                            }

                            loop1_Origin = flt_mgr.second_loop_Origin;
                        }
                    }
                } // end if(flt_mgr.flag_second_judge)
            }
        } //end if(g_function_mode & FUNCTION_A)

    }
}


void INIT_VD(void)
{
    loop1_INCNT = 0;
    loop1_OUTCNT = 0;
//    loop1_PLUSECNT =0;
    loop1_CapCnt = 0;

//    Cap.loop1_CapThis = 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_LPCNT=0;
//   loop1_INI_LOOP=1;

    loop1_FLAG_IN = 0;
    loop1_FLAG_OUT = 0;
    loop1_FLAG_PLUSE = 0;
    loop1_FLAG_CUT = 0;


    flt_mgr.is_initialized = 0;
    flt_mgr.recent_5sec = 0;
    stage_config[0].base_value = 0;
    stage_config[1].base_value = 0;

    g_loop_acs_info.car_state = 0;
    loop1_FLAG_HOLD_TIMEOUT = 0;

    // 初始化二次判断增强条件
    flt_mgr.second_judge_enter_time = 0;
    flt_mgr.second_judge_min_time = 2000;  // 10秒（10000ms / 5ms = 2000）
    flt_mgr.cnt_stable_to_idle = 0;
    flt_mgr.stable_to_idle_threshold = 10;  // 连续10次检测到恢复到空闲值
    
}


void cjq_srv(void){
    
    if(loop1_LOOP_OK){
        if(loop1_FLAG_HOLD_TIMEOUT){
            INIT_VD();
        }
        if(loop1_CAP_OK){
            if(!g_loop_power_up_state){
                g_loop_power_up_state = 1;   // // 表示连接了线圈
            }
            else{
                if(loop1_FLAG_CUT){
                    
                }
            }
        }        
        if(loop1_FLAG_CUT){
            // 线圈断开后重新连上 需要复位
            loop1_FLAG_CUT = 0;
//            SYS_ResetExecute();
            if(loop1_VD_FLAG){
                if(g_loop_cng_unit.exist_mode){
                    Hold_CNT = 0;
                    loop1_VD_HOLD = 1;
                }
            }
            flt_mgr.flag_loop_reconnect = 1;
        }
        if(g_loop_cut_last != loop1_FLAG_CUT){
            g_loop_cut_last = loop1_FLAG_CUT;
        }
        if(!loop1_LOOP_OK0){
            DelayMs(200);
            loop1_LOOP_OK0 = 1;
        }
        if(loop1_CAP_OK){
            loop1_CAP_OK = 0;
            vd1_task();
        }// end if loop1_CAP_OK
    }// end if loop1_LOOP_OK
    else{
        loop1_FLAG_CUT = 1;
        loop1_VD_FLAG = 0;
        loop1_FLAG_IN = 0;
        loop1_FLAG_OUT = 0;
        loop1_FLAG_PLUSE = 0;

        // 线圈断开，继电器释放，状态为0，车辆状态也为0
        RLY1_OFF; RLY2_OFF;
        g_loop_acs_info.relay1_state = 0;
        g_loop_acs_info.relay2_state = 0;
        g_loop_acs_info.car_state = 0;

        if(g_loop_cut_last != loop1_FLAG_CUT){
            g_loop_cut_last = loop1_FLAG_CUT;
            if(g_loop_power_up_state){
                g_loop_cut_amount++;
                if(g_loop_cut_amount >= 255){
                    g_loop_cut_amount = 255;
                }
            }
            
        }

    }
}




/*********************************************************************
 * @fn      Main_Circulation
 *
 * @brief   主循环
 *
 * @return  none
 */
__HIGH_CODE
__attribute__((noinline))
void Main_Circulation()
{

    while(1)
    {
        TMOS_SystemProcess();
        // poll_sens_key();
        #ifndef DEBUG
        poll_dbn_ble();
        #endif

        cjq_srv();

        if(g_flag_bt_state == 0){
            // 如果蓝牙断开，主动上报需要禁用
            if(g_dbn_ble_state_acs_enable.enable){
                g_dbn_ble_state_acs_enable.enable = 0;
            }
        }

        #ifdef DEBUG
        if(g_flag_output){
            g_flag_output = 0;
            uint32_t tmp =  (g_sys_freq )/(loop1_Xn);
            PRINT("Xn: %d, loop1_LPCNT: %d, counter:%d, ms_counter:%d, capvd:%d, loop1_Origin:%d, freq:%d, init_freq:%d\n", 
               loop1_Xn, loop1_LPCNT, g_xn_counter, g_xn_counter/2000, loop1_CAPVD, loop1_Origin, tmp, flt_mgr.init_freq);
                        
            g_xn_counter = 0;
        }
        #endif
    }
}


/*********************************************************************
 * @fn      main
 *
 * @brief   主函数
 *
 * @return  none
 */
int main(void)
{
    uint8_t i;
#if(defined(DCDC_ENABLE)) && (DCDC_ENABLE == TRUE)
    PWR_DCDCCfg(ENABLE);
#endif
    SetSysClock(CLK_SOURCE_PLL_60MHz);
#if(defined(HAL_SLEEP)) && (HAL_SLEEP == TRUE)
    GPIOA_ModeCfg(GPIO_Pin_All, GPIO_ModeIN_PU);
    GPIOB_ModeCfg(GPIO_Pin_All, GPIO_ModeIN_PU);
#endif

    gpio_uart_init();
    g_sys_freq = FREQ_SYS;
    GetMACAddress(MACAddr);
    for(i = 0; i < 6; i++)
    {
        g_dev_number[i] = MACAddr[i];
    }    
    g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT;  // max
    INIT_VD();
    para_store_init();
//    DelayMs(10);
    init_second_order(&second_order_state, 0);

    tm_init();

    CH59x_BLEInit();
    HAL_Init();
    GAPRole_PeripheralInit();
    Peripheral_Init();
    Main_Circulation();
}



/* @fn      TMR0_IRQHandler
*
* @brief   TMR0中断函数
*
* @return  none
*/
__INTERRUPT
__HIGH_CODE
void TMR0_IRQHandler(void) // TMR0 定时中断
{
   if(TMR0_GetITFlag(TMR0_3_IT_DATA_ACT)){
       loop1_Xn = TMR0_CAPGetData();

        #ifdef DEBUG
        g_xn_counter++;
        #endif

       if(!loop1_RF_FLAG){
           loop1_RF_FLAG = 1;
       }
       else{
           loop1_CapCnt++;

           if(loop1_INI_LOOP){
//                if(g_filter_step < 2){
                   if(loop1_LPCNT == 0){
                       if(loop1_CapCnt > 200){
                           loop1_LPCNT = (32768/loop1_Xn)<<6;
                           if(loop1_LPCNT == 0){
                               loop1_LPCNT = 10;
                           }

                           g_freq_tmp = (float)g_sys_freq/((float)loop1_Xn);

                           loop1_CAPVD = 0;
                           loop1_CapSum = 0;
                           loop1_CapCnt = 0;
                       }
                   }

//                }//end if g_filter_step < 2
               else{
                   loop1_CapSum += loop1_Xn;
                   if(loop1_CapCnt >= loop1_LPCNT){
                       g_loop_acs_info.loop_capvd = (loop1_CapSum >> 5);
                       loop1_CAPVD = g_loop_acs_info.loop_capvd;
                       loop1_Origin = loop1_CAPVD;
                       loop1_INI_LOOP = 0;
                       loop1_CapSum = 0;
                       loop1_Value = 0;
                       loop1_CapCnt = 0;
                   }
               }
           }// end if loop1_INI_LOOP
           else {
               loop1_CapSum += loop1_Xn;

               if(loop1_CapCnt >= loop1_LPCNT){
                   loop1_Value = (loop1_CapSum >> 5);       // g_loop_acs_info.loop_capvd = (loop1_CapSum >> 5);
                //    loop1_Value = g_loop_acs_info.loop_capvd;
                   loop1_CAP_OK = 1;
                   loop1_CapSum = 0;
                   loop1_CapCnt = 0;
                   loop1_INI_LOOP = 0;
               }
           }
       }

       R8_TMR0_INT_FLAG |= 0xff;
   }
}



/*********************************************************************
 * @fn      TMR1_IRQHandler
 *
 * @brief   TMR1中断函数
 *
 * @return  none
 */
__INTERRUPT
__HIGH_CODE
void TMR1_IRQHandler(void) // TMR1 定时中断  5ms/per
{
    static uint16_t _counter1_init = 0;
    static uint16_t _cnt = 0;

    if(TMR1_GetITFlag(TMR0_3_IT_CYC_END))
    {
        #ifdef DEBUG
        _cnt++;
        if(_cnt >= 400){
            g_flag_output = 1;
            _cnt = 0;
        }

        #endif

        if(g_dbn_ble_state_acs_enable.enable && g_flag_bt_state){
            g_loop_acs_info.report_counter++;
        }        

        TimingDelayInc++;


        if(loop1_FLAG_CUT == 0)
        {
            if(!flt_mgr.is_initialized)
            {
                _counter1_init++;
                if(_counter1_init >=40)  // 30:150ms, 40: 200ms, 50:250ms
                {
                    _counter1_init = 0;
                    LEDA_ON_OFF();
                }
            }
        }
        if(flt_mgr.flag_flat_release)
        {
            flt_mgr.cnt_flat_release++;
            if(flt_mgr.cnt_flat_release >= flt_mgr.max_cnt_flat_release)
            {
                flt_mgr.flag_flat_release = 0;
                flt_mgr.cnt_flat_release = 0;
            }
        }

        if(flt_mgr.flag_loop_reconnect == 1)
        {
            flt_mgr.cnt_loop_reconnect++;
            if(flt_mgr.cnt_loop_reconnect >= 200)
            {
                flt_mgr.flag_loop_reconnect = 2;
                flt_mgr.cnt_loop_reconnect = 0;
            }
        }
        if(flt_mgr.flag_not_idle)
        {
            flt_mgr.counter_not_idle++;
            if(flt_mgr.counter_not_idle >= 200) //400) // 5ms per unit, 400 mean 2second
            {
                flt_mgr.counter_not_idle = 0;
                flt_mgr.flag_not_idle = 0;
            }
        }
        if(flt_mgr.flag_5sec == 0)
        {
            flt_mgr.counter_5sec++;
            if(flt_mgr.counter_5sec >= 1000)
            {
                flt_mgr.counter_5sec = 0;       // 5*1000/5=1000
                flt_mgr.flag_5sec = 1;
            }
        }

        if(loop1_FLAG_CUT)
        {
          _counter1_init++;
          if(_counter1_init >= 30)  // 30:150ms, 40: 200ms, 50:250ms
          {
              _counter1_init = 0;
              LEDA_ON_OFF();
          }
        }
//        else
//        {
//          if(loop1_LOOP_OK == 0)
//          {
//              if(g_led_A_cnt >= 100) // 200: 1000ms,1s
//              {
//                  g_led_A_cnt = 0;
//                  LEDA_ON_OFF();
//              }
//          }
//          else
//          {
//              if(loop1_VD_FLAG)     // 有车  car busy
//              {
//              }
//              else  // No car , idle
//              {
//                  if(g_led_A_cnt >= 100)    // 100: 500ms
//                  {
//                      g_led_A_cnt = 0;
//                      LEDA_OFF;
//                  }
//              }
//          }
//        }

        if(g_function_mode & FUNCTION_B){
            // 更新二次判断进入时间（当有车时累加）
            if(loop1_VD_FLAG && flt_mgr.flag_second_judge)
            {
                flt_mgr.second_judge_enter_time++;
            }
        }


        u32CountWdt++;

        TM1cnt++;
        if(TM1cnt >= 10)          //about=50ms
        {
          TM1cnt = 0;
          if(loop1_FLAG_IN)     // 线圈1  从无车到有车
          {
              loop1_FLAG_OUT = 0;
              loop1_OUTCNT = 0;
              loop1_FLAG_IN = 0;
              if(g_loop_cng_unit.output_mode == 1) // 进入脉冲
              {
                  loop1_FLAG_PLUSE_IN_F = 1;
              }
              loop1_INCNT = 0;
              LEDA_ON;
          }

          if(loop1_FLAG_PLUSE_IN_F)
          {
              loop1_INCNT++;
              if(loop1_INCNT > g_loop_out_delay)
              {
                  loop1_INCNT = 0;
                  loop1_FLAG_PLUSE_IN_F = 0;
                  loop1_FLAG_PLUSE_IN = 1;
              }
          }

          if(loop1_FLAG_PLUSE_IN)
          {
              loop1_INCNT++;
              if(loop1_INCNT > IN_DELAY)        // 500ms
              {
                  loop1_INCNT = 0;
                  loop1_FLAG_PLUSE_IN = 0;
              }
          }


          if(loop1_FLAG_OUT)     // 从有车到无车的标志
          {
                loop1_OUTCNT++;
                if(loop1_OUTCNT > g_loop_out_delay)     //
                {
                    loop1_FLAG_OUT = 0;
                    loop1_FLAG_PLUSE = 1;
                    loop1_OUTCNT = 0;
                    LEDA_OFF;
                }
          }

          if(loop1_FLAG_PLUSE)  // Give 500ms
          {
              loop1_OUTCNT++;
              if(loop1_OUTCNT > PLUSE_DELAY)
              {
                  loop1_FLAG_PLUSE = 0;
                  loop1_OUTCNT = 0;
              }
          }


          if(loop1_VD_HOLD)
          {
              Hold_CNT++;
              if(Hold_CNT > g_hold_time)   // 超过有限存在时间
              {
                  loop1_VD_HOLD = 0;
                  Hold_CNT = 0;
                  if(loop1_VD_FLAG)
                  {
                    //   flt_mgr.flag_second_judge = 1; // 二次判断
                    //  loop1_FLAG_CUT = 1;   //
                    loop1_FLAG_HOLD_TIMEOUT = 1;
                    g_moving_average_window_size = LOOP_WINDOW_SIZE_FAST;       // 进入快速补偿模式
                    RLY1_OFF; RLY2_OFF;
        //                                      Reset_RstPeripheralAll();
                  }
              }
          }

            if(loop1_LC_HOLD)
            {
                LC_Hold_CNT++;
                if(LC_Hold_CNT > g_safe_max_cnt)
                {
                    //TODO: 这里需要验证是否能够实现安全复位的功能
                    loop1_LC_Reset = 1;
                    loop1_INI_LOOP = 1;
                    loop1_LOOP_OK0 = 0;
                    LC_Hold_CNT = 0;
                    loop1_ORG_CNT = 0;
                    loop1_ORG_SUM = 0;
                }
            }
        }

        if(loop1_RF_FLAG)
        {
        //    loop1_LOOP_OK0 = loop1_LOOP_OK;
            loop1_LOOP_OK = 1;
            loop1_RF_FLAG = 0;

            if(loop1_LC_Reset == 0)
            {
                 switch(g_loop_cng_unit.output_mode)
                 {     // 输出方式
                     case 0:  // 存在输出
                     {
                        if(loop1_VD_FLAG | loop1_FLAG_OUT)// 输出存在信号
                        {
                            if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
                                RLY1_ON;
                                g_loop_acs_info.relay1_state = 1;
                                RLY2_ON;
                                g_loop_acs_info.relay2_state = 1;
                            }
                          
                        }
                        else
                        {
                          RLY1_OFF;
                          g_loop_acs_info.relay1_state = 0;
                          RLY2_OFF;
                          g_loop_acs_info.relay2_state = 0;
                        }
                     } break;
                     case 1:   // 进入脉冲
                     {
                         if(loop1_VD_FLAG)  // 继电器2存在信号
                         {
                            if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
                            
                                RLY2_ON;
                                g_loop_acs_info.relay2_state = 1;
                            }
                         }
                         else
                         {
                             RLY2_OFF;
                             g_loop_acs_info.relay2_state = 0;
                         }

                         if(loop1_FLAG_PLUSE_IN)
                         {
                             RLY1_ON;
                            g_loop_acs_info.relay1_state = 1;
                         }
                         else
                         {
                             RLY1_OFF;
                          g_loop_acs_info.relay1_state = 0;
                         }
                     } break;
                     case 2:   // 离开脉冲
                     {
                         if(loop1_VD_FLAG)  // 继电器2存在信号
                         {
                             if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
                            
                                RLY2_ON;
                                g_loop_acs_info.relay2_state = 1;
                            }
                         }
                         else
                         {
                             RLY2_OFF;
                             g_loop_acs_info.relay2_state = 0;
                         }

                        if(loop1_FLAG_PLUSE)  // 输出离开脉冲
                        {
                           RLY1_ON;
                           g_loop_acs_info.relay1_state = 1;
                        }
                        else
                        {
                           RLY1_OFF;
                           g_loop_acs_info.relay1_state = 0;
                        }
                     } break;
                     default: {
                         if(loop1_VD_FLAG)  // 继电器2存在信号
                         {
                            if((flt_mgr.flag_loop_reconnect & 0x03) == 0){
                            
                                RLY2_ON;
                                g_loop_acs_info.relay2_state = 1;
                            }
                         }
                         else
                         {
                             RLY2_OFF;
                             g_loop_acs_info.relay2_state = 0;
                         }
                     } break;
                 }
            }// end if loop1_LC_Reset == 0
        } // end if loop1_RF_FLAG
        else
        {
            loop1_LOOP_OK0 = loop1_LOOP_OK;
            loop1_LOOP_OK = 0;
        }



        TMR1_ClearITFlag(TMR0_3_IT_CYC_END); // 清除中断标志
    }
}





/******************************** endfile @ main ******************************/