#include "board.h"
#include "tdc_tof.h"
#include "alarm.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;

uint8_t TDC_SAMPLE_CNT = 0;//add by yuewei 261014 to count the tdc sample times
uint8_t TDC_SAMPLE_TIMES_PER_SECOND = 1;//add by yuewei 261014 to define tdc sample times per 1 second

float SumToF;
float LastSumToF;
float UToFAvg;
float DToFAvg;
float DiffToF;
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            5       // 窗口大小
#define LPF_ALPHA           0.20f   // 一阶低通系数
#define TOF_MAX_CHANNELS    1
typedef struct
{
    float filter_buf[BUF_SIZE];
    uint8_t buf_index;
    uint8_t buffer_ready;
    float lpf_out;
    uint8_t lpf_ready;
    float last_filtered;
} TOF_Channel_t;
static TOF_Channel_t tof_channels[TOF_MAX_CHANNELS] = {
    {
        .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;
    float sum_mid = 0.0f;
    uint8_t mid_cnt;

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

    // 取中间2个值求平均（BUF_SIZE=5：索引1,2,3，取中间3点平均；BUF_SIZE偶数取中间2点）
    mid_cnt = (BUF_SIZE % 2 == 1) ? 3 : 2;
    uint8_t start_idx = (BUF_SIZE - mid_cnt) / 2;
    for(i = 0; i < mid_cnt; i++)
    {
        sum_mid += temp_buf[start_idx + i];
    }
    float mid_avg = sum_mid / mid_cnt;

    // 一阶低通
    if (!f->lpf_ready)
    {
        f->lpf_out = mid_avg;
        f->lpf_ready = 1;
    }
    f->lpf_out = LPF_ALPHA * mid_avg + (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->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;
    //modified by yuewei 20260914 start
    //apsr = bilinear_quad_apsr(TmpFloat, WaterTemp);
    apsr = bilinear_quad_apsr(fabs(TmpFloat), WaterTemp);
    //modified by yuewei 20260914 end
    vol = TmpFloat * apsr * pTdcItf->pipe_coe;

    pTdcItf->flow_vol(vol, TmpFloat);
}

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

    // 清反向计量标识
    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);
        }
        if(alarm_status(ERR_BLANK_PIPE))
        {
            tdc_turn_on_off_high_clk(0);
        }
        return;
    }
    else
    {
        // 指定通道清除空管
        pTdcItf->clr_blank(ch);
    }

    // 记录UP和DOWM平均飞行时间
    UToFAvg = TdcScale * TdcUToF[0];
    DToFAvg = TdcScale * TdcDToF[0];

    // 本次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];
        //DiffToF = UToFAvg - DToFAvg;

        DiffToF = tof_filter_input_poll(ch, DiffToF);
    }

    // 本次飞行时间和赋值给上次飞行时间和
    LastSumToF = SumToF;

    if (tof_state & (TOF_ST_METER | TOF_ST_CALIB))
    {
        DiffToFAvg += (DiffToF - DiffToFAvg) * (1.0f / 20);
        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;
                TDC_SAMPLE_TIMES_PER_SECOND = 6;//add by yuewei 261015,有水大于始动流量->流采集频次1秒6次
            }
            else
            {
                tof_flag &= ~TOF_FLAG_VALID_FLOW;
                TDC_SAMPLE_TIMES_PER_SECOND = 2;//add by yuewei 261015,有水小于始动流量->流采集频次1秒2次
            }
        }

        // 是有效的流量
        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);
                //add by yuewei 261015,当采集次数大于等于TDC_SAMPLE_TIMES_PER_SECOND时，才进行流量计算（节约功耗）
                if(TDC_SAMPLE_CNT >= TDC_SAMPLE_TIMES_PER_SECOND)
                {
                    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);
    }
        
}

void tof_isr_convert_tof_to_flow(void)
{
    switch (TdcIntStep)
    {
    case (0x00u):
        tdc_read_cal(TDC_CFG_DO_CKENA);
        tdc_start_cal(TDC_CFG_DO_CKDIS);
        TdcIntStep = 0x01u;
        break;

    case (0x01u):
        tdc_read_cal(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
        tdc_start_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS | TDC_CFG_DO_WREG);
        TdcIntStep = 0x03u;
        break;

    case (0x02u):
        tdc_read_temp(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
        tdc_start_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS | TDC_CFG_DO_WREG);
        TdcIntStep = 0x03u;
        break;

    case (0x03u):
        TDC_SAMPLE_CNT++;
        tdc_read_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
        read_tof_and_convert_to_flow();
        if(TDC_SAMPLE_CNT >= TDC_SAMPLE_TIMES_PER_SECOND)
        {
            TdcIntStep = 0xFFu;
            TDC_SAMPLE_CNT = 0;
        }
        else
        {
            tdc_start_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS | TDC_CFG_DO_WREG);
            TdcIntStep = 0x03u;
        }
        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;

    // 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)
    //{
    // 发送信号量，唤醒线程处理
    temp_flag=1;
    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;
}