#include "board.h"
#include "tdc_tof.h"

#define TOF_PWFW_DIFF ((uint16_t)(0x1620u * 25 / 100))
#define TOF_PWFW_LOW ((uint16_t)(0x1620u * 40 / 100))
#define TOF_PWFW_HIGH ((uint16_t)(0x1620u * 50 / 100))
#define TOF_OFST_LOW (10)
#define TOF_OFST_HIGH (26)

uint8_t TdcFwOfst = (TOF_OFST_LOW + TOF_OFST_HIGH) / 2;

uint32_t CntErrEvent = 0;
uint8_t TdcIntStep = 0xFFu;

float SumToF;
float LastSumToF;
float UToFAvg;
float DToFAvg;
float DiffToF;
float DiffToF_LAST; // add by yuewei 20260325 用于记录上次飞行时间，便于低通平滑滤波
float APSR_LAST;    // add by yuewei 20260325 用于记录上次APSR值，便于防止二次修正来回跳变
float DiffToFAvg;
float DiffToFAvg2;
float DiffToFAvg128;
double CumFlow;

uint16_t CalibTime;
float CalibSpdLvl;
float CalibTemp;

tdc_itf_t *pTdcItf;
void set_tdc_itf(tdc_itf_t *itf)
{
    pTdcItf = itf;
}

uint32_t get_tof_flag(void)
{
    return tof_flag;
}

void set_tof_flag(uint32_t flag)
{
    tof_flag = flag;
}

uint16_t get_tof_state(void)
{
    return tof_state;
}

void set_tof_state(uint16_t state)
{
    tof_state = state;
}

float get_diff_tof(void)
{
    return DiffToF;
}

uint8_t get_tdc_fw_ofst(void)
{
    return TdcFwOfst;
}

void set_tdc_fw_ofst(uint8_t fw_ofst)
{
    TdcFwOfst = fw_ofst;
}

uint8_t get_tdc_int_step(void)
{
    return TdcIntStep;
}

void set_tdc_int_step(uint8_t int_step)
{
    TdcIntStep = int_step;
}

// 可调滤波参数
#define BUF_SIZE            3       // 减小窗口，避免整段温漂进入缓存
#define LPF_ALPHA           0.22f   // 大幅降低，强化平滑，压制缓慢漂移

// 卡尔曼参数：大幅降低Q、提高R，不信任微小缓慢波动
#define KALMAN_Q            0.002f
#define KALMAN_R            0.025f
#define KALMAN_INIT_P       0.1f

#define TOF_MAX_CHANNELS    1

typedef struct
{
    float filter_buf[BUF_SIZE];
    uint8_t buf_index;
    uint8_t buffer_ready;

    float x_est;
    float p_est;
    const float Q;
    const float R;

    float lpf_out;
    uint8_t lpf_ready;

    float last_filtered;
} TOF_Channel_t;

static TOF_Channel_t tof_channels[TOF_MAX_CHANNELS] = {
    {
        .Q = KALMAN_Q,
        .R = KALMAN_R,
        .buf_index = 0,
        .buffer_ready = 0,
        .lpf_ready = 0,
    }
};

extern uint8_t G_STATIC_OT_SETTING_FLAG;

static float filter_single_channel(float raw)
{
    TOF_Channel_t *f = &tof_channels[0];
    float temp_buf[BUF_SIZE];
    uint8_t i, j;

    f->filter_buf[f->buf_index++] = raw;
    if (f->buf_index >= BUF_SIZE)
    {
        f->buf_index = 0;
        f->buffer_ready = 1;
    }

    if (!f->buffer_ready)
    {
        f->x_est = raw;
        f->p_est = KALMAN_INIT_P;
        f->lpf_out = raw;
        f->lpf_ready = 1;
        f->last_filtered = raw;
        return raw;
    }

    memcpy(temp_buf, f->filter_buf, sizeof(temp_buf));
    for (i = 0; i < BUF_SIZE - 1; i++)
    {
        for (j = 0; j < BUF_SIZE - i - 1; j++)
        {
            if (temp_buf[j] > temp_buf[j + 1])
            {
                float tmp = temp_buf[j];
                temp_buf[j] = temp_buf[j + 1];
                temp_buf[j + 1] = tmp;
            }
        }
    }
    float median_val = temp_buf[BUF_SIZE / 2];

    // 卡尔曼更新
    float x_pred = f->x_est;
    float p_pred = f->p_est + f->Q;
    float k_gain = p_pred / (p_pred + f->R);
    float x_cur = x_pred + k_gain * (median_val - x_pred);
    float p_cur = (1.0f - k_gain) * p_pred;

    f->x_est = x_cur;
    f->p_est = p_cur;

    // 一阶低通
    if (!f->lpf_ready)
    {
        f->lpf_out = x_cur;
        f->lpf_ready = 1;
    }
    f->lpf_out = LPF_ALPHA * x_cur + (1.0f - LPF_ALPHA) * f->lpf_out;

    // 仅保存上一帧输出，不再计算方差
    f->last_filtered = f->lpf_out;

    return f->lpf_out;
}

// 重置滤波全部状态
void tof_filter_reset(void)
{
    TOF_Channel_t *f = &tof_channels[0];
    memset(f->filter_buf, 0, sizeof(f->filter_buf));
    f->buf_index = 0;
    f->buffer_ready = 0;
    f->x_est = 0.0f;
    f->p_est = KALMAN_INIT_P;
    f->lpf_out = 0.0f;
    f->lpf_ready = 0;
    f->last_filtered = 0.0f;
}

// 初始化
void tof_filter_init(uint8_t ch_num)
{
    (void)ch_num; // 消除未使用警告
    tof_filter_reset();
}

// 输入原始数据，返回滤波结果（兼容旧接口，ch传0即可）
float tof_filter_input_poll(uint8_t ch, float raw)
{
    (void)ch; // 消除入参未使用警告
    if (G_STATIC_OT_SETTING_FLAG == 1)
    {
        return raw;
    }
    return filter_single_channel(raw);
}

void DiffToF2Flow(float dtof)
{
    float TmpFloat;
    float apsr;
    double vol;

    TmpFloat = dtof * UsonicSpd * UsonicSpd * 0.00001f;
    apsr = bilinear_quad_apsr(TmpFloat, WaterTemp);

    vol = TmpFloat * apsr * pTdcItf->pipe_coe;

    pTdcItf->flow_vol(vol, TmpFloat);
}

void tof_isr_convert_tof_to_flow(void)
{
    int16_t Tmp16;
    float TmpFloat;
    uint32_t Tu32;
    uint32_t ch = get_mux_ch();

    switch (TdcIntStep)
    {
//    case (0x00u):
//        Tu32 = (RTC_CntInt & 0x00000001u) ? TDC_CFG_FIRE_DOWN : TDC_CFG_FIRE_UP;
//        tdc_read_tof(TDC_CFG_DO_CKENA | Tu32);
//        //            if(Tu32 == TDC_CFG_FIRE_UP)
//        //            {
//        //								LOG("ch-%d-0-U, %.6f\n", ch, TdcScale*TdcUToF[0]);
//        //            }
//        //            else
//        //            {
//        //                LOG("ch-%d-0-D, %.6f\n", ch, TdcScale*TdcDToF[0]);
//        //            }
//        // LOG("ch-%d-0-U:%.6f-D:%.6f\n\n", ch, TdcScale*TdcUToF[0],TdcScale*TdcDToF[0]);
//        Tu32 = (RTC_CntInt & 0x00000001u) ? TDC_CFG_FIRE_UP : TDC_CFG_FIRE_DOWN;
//        tdc_start_tof(TDC_CFG_DO_CKDIS | Tu32);
//        TdcIntStep = 0x01u;

//        break;

    case (0x01u):      
       
//        // RTC_CntInt奇数UP、偶数DOWN
//        Tu32 = (RTC_CntInt & 0x00000001u) ? TDC_CFG_FIRE_UP : TDC_CFG_FIRE_DOWN;

        // 根据RTC_CntInt设置校准、采集温度，读取飞行时间
        if ((RTC_CntInt & RTC_CNTMSK_CAL) == 0x0u)
        {
            // 间隔8秒进行一次校准
            tdc_read_tof(TDC_CFG_DO_CKENA);
            tdc_start_cal(TDC_CFG_DO_WREG | TDC_CFG_DO_CKDIS);
            if (tof_flag & TOF_FLAG_UNALGND_CHOP)
            {
                TdcIntStep = 0x03u;
            }
            else
            {
                TdcIntStep = 0x02u;
            }
        }
        else if ((RTC_CntInt & RTC_CNTMSK_TEMP) == 0x4u)
        {
            // 间隔4秒采集一次温度
            tdc_read_tof(TDC_CFG_DO_CKENA);
            tdc_start_temp(TDC_CFG_DO_WREG | TDC_CFG_DO_CKDIS);
            TdcIntStep = 0x04u;
        }
        else
        {
            // 读取飞行时间
            tdc_read_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
            TdcIntStep = 0xFFu;
        }

        //            LOG("ch-%d-1-U:%.6f-D:%.6f\n\n", ch, TdcScale*TdcUToF[0],TdcScale*TdcDToF[0]);
        //            if(Tu32 == TDC_CFG_FIRE_UP)
        //            {
        //                LOG("ch-%d-1-U, %.6f\n\n", ch, TdcScale*TdcUToF[0]);
        //            }
        //            else
        //            {
        //                LOG("ch-%d-1-D, %.6f\n\n", ch, TdcScale*TdcDToF[0]);
        //            }

        // 清反向计量标识
        pTdcItf->bw_flow_flag = 0;
        // 空管
        if (tof_flag & TOF_FLAG_BLANK_PIPE)
        {
            // 设置对应德通道为空管
            pTdcItf->set_blank(ch);

            // DiffTof from normal channel
            // 正常的通道进行计量
            if (pTdcItf->ch_bit_map ^ TdcItf.blank_flag)
            {
                DiffToF2Flow(DiffToF);
                SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, UToFAvg, DToFAvg, CumFlow);
            }
            break;
        }
        else
        {
            // 指定通道清除空管
            pTdcItf->clr_blank(ch);
        }

        // 计算脉冲宽度
        Tmp16 = TdcUPwFw - TdcDPwFw;
        // 判断脉冲宽度有效性
        if (Tmp16 < (0 - TOF_PWFW_DIFF) || Tmp16 > TOF_PWFW_DIFF)
        {
            // 无效脉冲宽度
            tof_flag |= TOF_FLAG_UNPAIR_WAVE;
            CntErrEvent++;
        }
        else
        {
            // 有效脉冲宽度
            tof_flag &= ~TOF_FLAG_UNPAIR_WAVE;
            // 记录UP和DOWM平均飞行时间
            UToFAvg = TdcScale * TdcUToF[0];
            DToFAvg = TdcScale * TdcDToF[0];
        }

        // 判断offset有效性
        if (TdcFwOfst < TOF_OFST_LOW || TdcFwOfst > TOF_OFST_HIGH)
        {
            // out of range
            TdcFwOfst = (TOF_OFST_LOW + TOF_OFST_HIGH) / 2; // default offset
        }
        else if (tof_flag & TOF_FLAG_UNPAIR_WAVE)
        {
            // first wave for up and down is different
            TdcFwOfst += (TdcFwOfst <= (TOF_OFST_LOW + 3)) ? 2 : -2;
        }
        else if (TdcUPwFw < TOF_PWFW_LOW && TdcDPwFw < TOF_PWFW_LOW && TdcFwOfst > TOF_OFST_LOW)
        {
            TdcFwOfst -= 1;
        }
        else if (TdcUPwFw > TOF_PWFW_HIGH && TdcDPwFw > TOF_PWFW_HIGH && TdcFwOfst < TOF_OFST_HIGH)
        {
            TdcFwOfst += 1;
        }

        // 本次UP和DOWN飞行时间求和
        SumToF = UToFAvg + DToFAvg; // alomost fixed regardless of water speed
        // 本次飞行时间和与上次飞行时间和德差值在50内认为有效
        if (fabsf(SumToF - LastSumToF) < 50.0f)
        {
            // 计算飞行时间
            DiffToF = UToFAvg - DToFAvg - pTdcItf->path_skew[ch];
            // add by yuewei 20260325 低通平滑滤波
            // DiffToF = DiffToF * DIFF_TOFF_FILTER_ALPHA + (1 - DIFF_TOFF_FILTER_ALPHA) * DiffToF_LAST;

            // DiffToF = tof_filter(DiffToF);//20260429
            DiffToF = tof_filter_input_poll(ch, DiffToF);
            // LOG("ch%d, U=%.6f, D=%.6f\n", ch, UToFAvg, DToFAvg);
            // LOG("int_cnt=%d, step=%d, ch=%d, DiffToF=%.6f\n\n", int_cnt, get_tdc_int_step(), ch, DiffToF);
        }
        // 本次飞行时间和赋值给上次飞行时间和
        LastSumToF = SumToF;

        if (tof_state & (TOF_ST_METER | TOF_ST_CALIB))
        {
            DiffToFAvg += (DiffToF - DiffToFAvg) * (1.0f / 6);
            DiffToFAvg2 += (DiffToF - DiffToFAvg2) * (1.0f / 2);
            DiffToFAvg128 += (DiffToF - DiffToFAvg128) * (1.0f / 128);
            if (tof_flag & TOF_FLAG_VALID_FLOW)
            {
                if (fabsf(DiffToFAvg) < 0.35f)
                {
                    tof_flag &= ~TOF_FLAG_VALID_FLOW;
                    if (tof_state & TOF_ST_CALIB && CalibTime > CALIB_TIME_LTH)
                    {
                        tof_flag |= TOF_FLAG_CALIB_DONE;
                    }
                }
            }
            else
            {

                // modified by yuewei 20260519,加入n值，始动流量
                // if (fabsf(DiffToFAvg) > 0.45f || fabsf(DiffToFAvg2) > 1.0f)
                if (fabsf(DiffToFAvg * 100.00f) > Paramx.n || fabsf(DiffToFAvg2) > 1.0f)
                {
                    tof_flag |= TOF_FLAG_VALID_FLOW;
                    if (tof_state & TOF_ST_CALIB)
                    {
                        tof_flag |= TOF_FLAG_CALIB_FLOW;
                        CumFlow = 0;
                        CalibTime = 0;
                        CalibSpdLvl = 0;
                        CalibTemp = 0;
                    }
                }
            }

            // 是有效德流量
            if (tof_flag & TOF_FLAG_VALID_FLOW)
            {
                TmpFloat = DiffToF * UsonicSpd * UsonicSpd * 0.00001f;

                // 一次校准模式
                if (tof_state & TOF_ST_CALIB)
                {
                    CumFlow += TmpFloat * pTdcItf->pipe_coe;
                    pTdcItf->check_flow(CumFlow, TmpFloat, WaterTemp);
                    if (tof_flag & TOF_FLAG_CALIB_FLOW)
                    {
                        if (DiffToFAvg2 > 0.5f)
                        {
                            CalibSpdLvl += TmpFloat;
                            CalibTemp += WaterTemp;
                            CalibTime++;
                        }
                        else
                        {
                            tof_flag &= ~TOF_FLAG_CALIB_FLOW;
                            if (CalibTime > CALIB_TIME_LTH)
                            {
                                CalibSpdLvl /= CalibTime;
                                CalibTemp /= CalibTime;
                                tof_flag |= TOF_FLAG_CALIB_FALL;
                            }
                        }
                    }
                }
                else
                {
                    // 正常计量模式
                    DiffToF2Flow(DiffToF);
                    // DiffToF2Flow(DiffToFAvg);
                }
            }
            else
            {
                FlowVol = 0; // 始动流量切除时，瞬时流量清零yw2026-6-25
            }
            // 串口打印，功能未开启
            // SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, UToFAvg, DToFAvg, DiffToFAvg);
            SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, DiffToF * UsonicSpd * UsonicSpd * 0.00001f, DToFAvg, DiffToFAvg);
        }
        break;

    case (0x02u):
        tdc_read_cal(TDC_CFG_DO_CKENA);
        tdc_start_cal(TDC_CFG_DO_CKDIS);
        TdcIntStep = 0x03u;
        break;

    case (0x03u):
        tdc_read_cal(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
        TdcIntStep = 0xFFu;
        break;

    case (0x04u):
        tdc_read_temp(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
        TdcIntStep = 0xFFu;
        break;

    case (0x80u):
        TdcIntStep = 0xFFu;
    }
}
uint8_t temp_flag=0;
void PORTD_F_IRQHandler(void)
{
    uint32_t ClkEn;
    uint32_t ClkOff;
    rt_base_t level;
    uint16_t ISRB, ISRD, ISRE;
    temp_flag=1;
    // M0P_GPIO->PD_ICLR = (1u << 3);
    //  M0P_GPIO->PD_ICLR = (1u << 4);
    //	M0P_GPIO->PD_ICLR &= ~(1 << 3);
    //	M0P_GPIO->PD_ICLR &= ~(1 << 4);
    //  ISRD = M0P_GPIO->PD_STAT;
    // M0P_GPIO->PD_ICLR = ~ISRD;
    //	return;

    ///////////add key isr handler wxl2026-5-22///////////////////
    extern uint8_t key_irq_handler(void);
    key_irq_handler();

    if (GpioGetEvents(GpioPortD, GpioBitMsk3) == 0) // not tdc int wxl2026-5-26
    {
        return;
    }
    //    get_internal_time();
    level = rt_hw_interrupt_disable();

    ClkEn = M0P_SYSCTRL->PERI_CLKEN0;

    ClkOff = (~ClkEn) & (1u << 28); // check GPIO clk is off or not

    if (ClkOff)
    {
        M0P_SYSCTRL->PERI_CLKEN0 = ClkEn | (1u << 28); // set GPIO clk on
    }

    //    ISRB = M0P_GPIO->PB_STAT;
    //    M0P_GPIO->PB_ICLR = ~ISRB;

    //    ISRE = M0P_GPIO->PE_STAT;
    //    M0P_GPIO->PE_ICLR = ~ISRE;

    if (ClkOff)
    {
        M0P_SYSCTRL->PERI_CLKEN0 = ClkEn; // set back GPIO clk off
    }

    rt_hw_interrupt_enable(level);

    // 只处理 TDC 中断
    // if (ISRD & GpioBitMsk3)
    //{
    // 发送信号量，唤醒线程处理
    rt_sem_release(&tdc_isr_sem);
    // LOG("rt_sem_release\n");

    //}
    ISRD = M0P_GPIO->PD_STAT;
    M0P_GPIO->PD_ICLR = ~ISRD;
    //	M0P_GPIO->PD_ICLR = (1u << 3);
    // G_TDC_DEEP_SLEEP = 0;
}