#include "MyBandFitter.h"

BandFitterConfig_t BandFitterConfig;
uint32_t blockSize = BLOCK_SIZE;

float32_t FitterOPMData_CheckBed[LENGTH_SAMPLES_IN_OFF_BED] = {0.0f};

float32_t FitterOPMData_Heart[HEART_FITTER_LENGTH] = {0.0f};
float32_t FitterOPMData_Breath[BREATH_FITTER_LENGTH - BREATH_IGNORE_LENGTH] = {0.0f};

float32_t outPutFitterOPMData_Heart[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0.0f};
float32_t outPutFitterOPMData_Breath[BREATH_FITTER_LENGTH - BREATH_IGNORE_LENGTH] = {0.0f};
float32_t outPutFitterOPMData_Heart_Lo[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0.0f};

uint32_t numBlocks = LENGTH_SAMPLES_HEART / BLOCK_SIZE; /* 需要调用arm_biquad_cascade_df1_f32的次数 */
/* 巴特沃斯带通滤波器系数140Hz 400Hz  （测试）*/
const float32_t IIRCoeffs32BP[5 * numStages] = {
    1.0f, 0.0f, -1.0f, -1.127651872054164616798743736580945551395f,
    -0.470013145087532668853214090631809085608f,
    1.0f, 0.0f, -1.0f, 0.774953058046049081397654845204669982195f,
    -0.367077500556684199750634434167295694351f};

const float32_t IIRs32BPScaleValue[numStages] = {
    0.558156585760773649163013487850548699498f,
    0.558156585760773649163013487850548699498f};

/* 巴特沃斯带通滤波器系数 0.8Hz 10Hz  （心率）*/
const float32_t IIRCoeffs32BP_Heart[5 * numStages] = {
    1.0f, 0.0f, -1.0f, 1.904441300281213944600722243194468319416f, -0.912635882040455093111575024522608146071f,
    1.0f, 0.0f, -1.0f, 1.626929945180070724575216445373371243477f, -0.721710170252540583746281299681868404150f};

const float32_t IIRs32BPScaleValue_Heart[numStages] = {
    0.133977542766808566110370293245068751276, 0.133977542766808566110370293245068751276};

/* 巴特沃斯带通滤波器系数 0.8Hz 10Hz  （二次滤波心率参数）*/
const float32_t IIRCoeffs32BP_Heart_NoCut[5 * numStages] = {
    1.0f, 0.0f, -1.0f, 1.799723076782722586131058051250874996185f, -0.830942864047083173950625223369570448995f,
    1.0f, 0.0f, -1.0f, 1.19173153206713600127386598614975810051f, -0.523449283678365517857855593319982290268f};

const float32_t IIRs32BPScaleValue_Heart_NoCut[numStages] = {
    0.246519525895492974676415087742498144507f, 0.246519525895492974676415087742498144507f};

/* 巴特沃斯带通滤波器系数 0.08Hz 2.5Hz  （呼吸）*/
const float32_t IIRCoeffs32BP_Breath[5 * numStages] = {
    1.0f, 0.0f, -1.0f, 1.985837455305179410913751780753955245018f, -0.985944130217127789883591049147071316838f,
    1.0f, 0.0f, -1.0f, 1.583264731971575800528739819128531962633f, -0.659798038006273568356618852703832089901f};

const float32_t IIRs32BPScaleValue_Breath[numStages] = {
    0.137599347839688912475963888937258161604f, 0.137599347839688912475963888937258161604f};

// float32_t OPM_Data[LENGTH_SAMPLES_BREATH] = {};

extern float Diff;
extern uint8_t BreathRate;
extern uint8_t HeartBeatRate;
extern uint8_t BreathRate_Record;
extern uint8_t HeartBeatRate_Record;
static float std_calc(float data[], int n);

/**
 * @function: arm_iir_f32_bp
 * @brief 调用函数arm_iir_f32_bp实现带通滤波器
 * @param {float32_t} *Input
 * @param {float32_t} *Output
 * @param {BandFitterConfig_t} FitterConfig
 * @return {*}
 * @author: lzc
 */
void arm_iir_f32_bp(float32_t *Input, float32_t *Output, BandFitterConfig_t FitterConfig)
{
    uint32_t i;
    arm_biquad_casd_df1_inst_f32 S;
    float32_t ScaleValue;
    float32_t *inputF32, *outputF32;

    /* 初始化输入输出缓存指针 */
    inputF32 = Input;
    outputF32 = Output;

    /* 初始化 */
    arm_biquad_cascade_df1_init_f32(&S, numStages, (float32_t *)&FitterConfig.IIR_Coeff[0],
                                    (float32_t *)&IIRStateF32[0]);

    /* 实现IIR滤波，这里每次处理1个点 */
    for (i = 0; i < numBlocks; i++)
    {
        arm_biquad_cascade_df1_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize),
                                   blockSize);
    }

    /*放缩系数 */
    ScaleValue = FitterConfig.IIR_ScaleValue[0] * FitterConfig.IIR_ScaleValue[1];

    /* 打印滤波后结果 */
    for (i = 0; i < LENGTH_SAMPLES; i++)
    {
        printf("%f\r\n", Input[i]);
    }
    printf("\r\n-----------------------------------------\n");
    for (i = IGNORE_LENGTH; i < LENGTH_SAMPLES; i++)
    {
        printf("%f\r\n", Output[i] * ScaleValue);
    }
}

/**
 * @function:
 * @brief: None
 * @return {*}
 * @author: lzc
 */
void arm_iir_f32_bp_stream_Init(BandFitterConfig_t FitterConfig)
{
    /* 初始化 */
}

/**
 * @function: arm_iir_f32_bp_stream_Output
 * @brief: 实现输出
 * @param {float32_t} *Input
 * @param {float32_t} *Output
 * @param {BandFitterConfig_t} FitterConfig
 * @return {*}
 * @author: lzc
 */
// float32_t DataMaria[400 * 10] = {0.0f};
void arm_iir_f32_bp_stream_Output(float32_t *Input, float32_t *Output, BandFitterConfig_t FitterConfig)
{
    uint32_t i;
    float32_t Value = 0;
    float32_t ScaleValue;
    float32_t *inputF32, *outputF32;
    arm_biquad_casd_df1_inst_f32 Part;

    /* 初始化输入输出缓存指针 */
    inputF32 = Input;
    outputF32 = Output;
    /* 实现IIR滤波，这里每次处理1个点 */
    arm_biquad_cascade_df1_init_f32(&Part, numStages, (float32_t *)&FitterConfig.IIR_Coeff[0],
                                    (float32_t *)&IIRStateF32[0]);
    for (i = 0; i < numBlocks; i++)
    {
        arm_biquad_cascade_df1_f32(&Part, inputF32 + (i * blockSize), outputF32 + (i * blockSize),
                                   blockSize);
    }

    /*放缩系数 */
    ScaleValue = FitterConfig.IIR_ScaleValue[0] * FitterConfig.IIR_ScaleValue[1];

    // printf("\r\n----------------In----------------------\n");
    // for (i = IGNORE_LENGTH; i < LENGTH_SAMPLES; i++)
    // {
    //     Value = Input[i];
    //     printf("%f\r\n", Input[i]);
    // }
    // printf("\r\n-----------------------------------------\n");
    /* 打印滤波后结果 */
    printf("\r\n----------------Out----------------------\n");
    for (i = IGNORE_LENGTH; i < LENGTH_SAMPLES; i++)
    {
        Value = Output[i] * ScaleValue;
        if (Value > 0.03f)
            Value = 0.03f;
        else if (Value < -0.03f)
            Value = -0.03f;
        printf("%f\r\n", Value);
    }
    printf("\r\n-----------------------------------------\n");
}

/**
 * @function: arm_iir_f32_bp_stream_Heart_Output
 * @brief: None
 * @param {float32_t} *Input
 * @param {float32_t} *Output
 * @return {*}
 * @author: lzc
 */
arm_biquad_casd_df1_inst_f32 HeartPart;
arm_biquad_casd_df1_inst_f32 HeartPart_NoCut;
void arm_iir_f32_bp_stream_Heart_Output(float32_t *Input, float32_t *Output)
{
    float32_t Value = 0;
    float32_t ScaleValue;
    float32_t *inputF32, *outputF32;
    float32_t Output_ResultData[HEART_FITTER_LENGTH] = {0};

    // printf("FitterOPMData_Heart In \r\n");
    // printf("-----------------------------------------------------\r\n");
    for (int i = 0; i < HEART_FITTER_LENGTH; i++)
    {
        // printf("%f\r\n", FitterOPMData_Heart[i]);
    }
    // printf("-----------------------------------------------------\r\n");
    /*放缩系数 */
    ScaleValue = IIRs32BPScaleValue_Heart[0] * IIRs32BPScaleValue_Heart[1];
    // 滤波
    myFitterLocal_Hr(&HeartPart, Input, Output_ResultData, HEART_FITTER_LENGTH, ScaleValue);

    /* 打印滤波后结果 */
    // printf("\r\n-------------Heart   Out--------------------\n");
    for (int i = (HEART_IGNORE_LENGTH); i < HEART_FITTER_LENGTH; i++)
    {
        Value = Output_ResultData[i];
        //        if (Value > 0.03f)
        //            Value = 0.03f;
        //        else if (Value < -0.03f)
        //            Value = -0.03f;
        Output[i - (HEART_IGNORE_LENGTH)] = Value;
        // printf("%f\r\n", Output[i - (HEART_IGNORE_LENGTH)]);
    }
    // printf("\r\n-----------------------------------------\n");
}

/**
 * @function: arm_iir_f32_bp_stream_Heart_Output_NoCut
 * @brief: 高频版本
 * @param {float32_t} *Input
 * @param {float32_t} *Output
 * @return {*}
 * @author: lzc
 */
void arm_iir_f32_bp_stream_Heart_Output_NoCut(float32_t *Input, float32_t *Output)
{
    float32_t Value = 0;
    float32_t ScaleValue;
    float32_t *inputF32, *outputF32;
    float32_t Output_ResultData[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0};

    // printf("\r\n-------------Heart   In--------------------\n");
    for (int i = 0; i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
    {
        Value = Input[i];
        // printf("%f\r\n", Value);
    }
    // printf("\r\n-----------------------------------------\n");
    /*放缩系数 */
    ScaleValue = IIRs32BPScaleValue_Heart_NoCut[0] * IIRs32BPScaleValue_Heart_NoCut[1];
    // 滤波
    myFitterLocal_Hr(&HeartPart_NoCut, Input, Output_ResultData, HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH, ScaleValue);

    /* 打印滤波后结果 */
    // printf("\r\n-------------Heart   Out--------------------\n");
    for (int i = (0); i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
    {
        Value = Output_ResultData[i];
        Output[i] = Value;
        // printf("%f\r\n", Output[i]);
    }
    // printf("\r\n-----------------------------------------\n");
}

// 低频版本
void arm_iir_f32_bp_stream_Heart_Output_NoCut_2(float32_t *Input, float32_t *Output)
{
    float32_t Value = 0;
    float32_t ScaleValue;
    float32_t *inputF32, *outputF32;
    float32_t Output_ResultData[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0};

    // printf("\r\n-------------Heart   In--------------------\n");
    for (int i = 0; i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
    {
        Value = Input[i];
        // printf("%f\r\n", Value);
    }
    // printf("\r\n-----------------------------------------\n");
    /*放缩系数 */
    ScaleValue = IIRs32BPScaleValue_Heart[0] * IIRs32BPScaleValue_Heart[1];
    // 滤波
    myFitterLocal_Hr(&HeartPart, Input, Output_ResultData, HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH, ScaleValue);

    /* 打印滤波后结果 */
    // printf("\r\n-------------Heart   Out--------------------\n");
    for (int i = (0); i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
    {
        Value = Output_ResultData[i];
        Output[i] = Value;
        // printf("%f\r\n", Output[i]);
    }
    // printf("\r\n-----------------------------------------\n");
}

/**
 * @function: arm_iir_f32_bp_stream_Breath_Output
 * @brief: None
 * @param {float32_t} *Input
 * @param {float32_t} *Output
 * @return {*}
 * @author: lzc
 */
extern char equipment_status_value;
arm_biquad_casd_df1_inst_f32 BreathPart;
void arm_iir_f32_bp_stream_Breath_Output(float32_t *Input, float32_t *Output)
{
    float32_t Value = 0;
    float32_t ScaleValue;
    float32_t s_Buffer[400] = {0};
    float32_t *inputF32, *outputF32;
    float32_t Input_ResultData[BREATH_FITTER_LENGTH] = {0};
    float32_t Output_ResultData[BREATH_FITTER_LENGTH] = {0};

    for (int i = 0; i < BREATH_FITTER_LENGTH / LENGTH_SAMPLES_BREATH; i++)
    {
        for (int j = 0; j < LENGTH_SAMPLES_BREATH; j++)
        {
            Input_ResultData[(LENGTH_SAMPLES_BREATH * i) + j] = Input[j];
        }
    }
    // printf("\r\n-------------Breath 2 IN--------------------\n");
    // for (int i = 0; i < LENGTH_SAMPLES_BREATH; i++)
    // {
    //     Input_ResultData[i] = Input[i];
    //     printf("%f\r\n", Input[i]);
    // }
    // printf("\r\n-----------------------------------------\n");
    /* 初始化输入输出缓存指针 */
    inputF32 = Input_ResultData;
    outputF32 = Output_ResultData;
    for (int i = 0; i < BREATH_FITTER_LENGTH; i++)
    {
        arm_biquad_cascade_df1_f32(&BreathPart, inputF32 + (i * blockSize), outputF32 + (i * blockSize),
                                   blockSize);
    }
    /*放缩系数 */
    ScaleValue = IIRs32BPScaleValue_Breath[0] * IIRs32BPScaleValue_Breath[1];
    /* 打印滤波后结果 */
    // printf("\r\n-------------Breath 2 Out--------------------\n");
    for (int i = BREATH_IGNORE_LENGTH; i < BREATH_FITTER_LENGTH; i++)
    {
        Value = Output_ResultData[i] * ScaleValue;
        Output[i - BREATH_IGNORE_LENGTH] = Value;
        // printf("%f\r\n", Output[i - BREATH_IGNORE_LENGTH]);
    }
    // printf("\r\n-----------------------------------------\n");
}
/**
 * @function: breathInBedCheck
 * @brief: V1.9 2024年7月8日
 * @param {float32_t} *Input
 * @return {*}
 * @author: lzc
 */
#define COUNT_BODY_CHECK_VAL 12
#define COUNT_BODY_CHECK_THRESHOLD 8

#define OPM_BODY_CHECK_THRESHOLD 1.5f
#define OPM_BODY_CHECK_HUGE_THRESHOLD 3.5f

#define LEN_Max_array 20 // Max_array长度

float i_count_Std = 0.0f;
float i_count_Max = 0.0f;
float Diff_detail_Past_2 = 0.0f; // Diff_detail(t-2)
float Diff_detail_Past_1 = 0.0f; // Diff_detail(t-1)
float Diff_detail_Record = 0.0f; // 记录阶梯值

float FFT_br = 0.0f;              // 未经后处理的FFT呼吸输出值
char i_count_No_Breath = 0;       // 呼吸暂停指数
char i_count_No_Breath_alarm = 0; // 呼吸暂停预警参数

char i_wait_FFT = 0;
char i_count_off_bed = 0; // 计数Max_array中符合离床要求的max数量
char i_count_no_human = COUNT_BODY_CHECK_VAL;
char i_count_change = 0;  // 空床状态下光功大幅变化计数
char i_count_change2 = 0; // 空床状态下光功大幅变化计数

char OPM_Count_2 = 0;        // 离床次数
char OPM_CountDown = 0;      // 倒计时25秒
char OPM_CountDown_2 = 0;    // 在床判断倒计时
char OPM_CountDown_3 = 0;    // 倒计时25秒
char OffBed_Flag_Record = 0; // 离床标志记录

float OPM_Current = 0.0f;
float OPM_Record = 0.0f;   // 下降沿判断时的光功记录
float OPM_Record_2 = 0.0f; // 记录空床光功
float OPM_Original = 0.0f; // 记录原始空床光功
float OPM_Past_2 = 0.0f;   // OPM(t-2)
float OPM_Past_1 = 0.0f;   // OPM(t-1)

uint16_t FiveMinutesCount = 0; // 5分钟倒计时 （需要加入到定时器）

float32_t Max_array[LEN_Max_array] = {0};

float32_t TempBuffer[LENGTH_SAMPLES_IN_OFF_BED] = {0};

extern char OffBed_Flag;
extern char FFT_Count_State_Flag;

extern float Diff_Detail;
extern float f_BPM_hr[5];
extern float f_BPM_br[5];
extern void ConvertMoveStd(void);

arm_biquad_casd_df1_inst_f32 CheckPart;
void breathInBedCheck(float32_t *Input)
{
    float32_t Max = 0.0f; //
    float32_t Std = 0.0f; //
    float32_t SumOPM = 0.0f;
    float32_t Value = 0;
    float32_t ScaleValue;
    float32_t s_Buffer[LENGTH_SAMPLES_IN_OFF_BED] = {0};
    float32_t *inputF32, *outputF32;
    float32_t Output[LENGTH_SAMPLES_IN_OFF_BED] = {0};
    float32_t Input_ResultData[BREATH_FITTER_LENGTH] = {0};
    float32_t Output_ResultData[BREATH_FITTER_LENGTH] = {0};
    float32_t AvgOPM[LENGTH_SAMPLES_IN_OFF_BED / 50] = {0};
    // 每50个数作均值
    for (int i = 0; i < (LENGTH_SAMPLES_IN_OFF_BED / 50); i++)
    {
        SumOPM = 0.0f;
        for (int j = (i * 50); j < ((i * 50) + 50); j++)
        {
            SumOPM += FitterOPMData_CheckBed[j];
        }
        AvgOPM[i] = SumOPM / 50;
    }
    Max = MyToolFindMaxValue(AvgOPM, 5) - MyToolFindMinValue(AvgOPM, 5);
    Std = std_calc(AvgOPM, 5);

    i_count_off_bed = 0;
    for (int i = 0; i < LEN_Max_array; i++) // 循环更新Max_array，此数组用于离床保险机制
    {
        if (OffBed_Flag == 0) // 离床状态时数组不起作用
        {
            Max_array[i] = 0;
        }
        else
        {
            if (i == (LEN_Max_array - 1))
            {
                Max_array[i] = Max;
            }
            else
            {
                Max_array[i] = Max_array[i + 1];
            }
            if ((Max_array[i] > 0) && (Max_array[i] < 0.001f))
            {
                i_count_off_bed++;
            }
        }
    }
    if ((dbm_value_1310 - OPM_Past_2 < ((-1) * OPM_BODY_CHECK_THRESHOLD)) &&
        (OPM_Record_2 < 0) && (OPM_CountDown_2 == 0) && (OPM_Past_2 < 0)) // 检测下降沿并开启在床判断倒计时
    {
        // 离床情况下进行判断
        if (OffBed_Flag == 0)
        {
            OPM_CountDown = 0; // 离床倒计时index
            OPM_Record = OPM_Past_2;
            OPM_CountDown_2 = 25; // 在床倒计时index
        }
    }
    if ((dbm_value_1310 - OPM_Past_2 > (OPM_BODY_CHECK_THRESHOLD)) && (OffBed_Flag == 4)) // 检测上升沿
    {
        FiveMinutesCount = 120;
    }
    if ((OffBed_Flag == 0) && (Max < 0.005f)) // 记录空床光功
    {
        if (OPM_CountDown_2 == 0) // 进行在床倒计时（即在床判断过程中）不记录空床光功
        {
            if (OPM_Record_2 < 0)
            {
                if (fabs(OPM_Record_2 - dbm_value_1310) > 0.3) // 当空床状态下光功数值发生大幅变化时,连续检测到3次变化再记录新空床光功
                {
                    i_count_change++;
                    if (i_count_change == 5)
                    {
                        OPM_Record_2 = dbm_value_1310;
                    }
                }
                else
                {
                    i_count_change = 0;
                    OPM_Record_2 = dbm_value_1310;
                }
            }
            else // 初次赋值空床光功
            {
                OPM_Record_2 = dbm_value_1310;
            }
            // NOTE:初次上电赋值
            if (OPM_Original == -10.00f) // 上电时在稳定离床状态下记录一次初始光功
            {
                OPM_Original = OPM_Record_2;
                printf("Ready To Write %f", OPM_Original); // 记录original专用
                EN25QXX_Write((uint8_t *)&OPM_Original,
                              FLASH_IN_OFF_BED_OPM_RECORD_ADDRESS,
                              sizeof(float32_t));
            }

            if (fabs(OPM_Record_2 - OPM_Original) > 0.3) // 当空床状态下光功数值发生大幅变化时,连续检测到25次变化再更新初始光功
            {
                i_count_change2++;
                if (i_count_change2 == 25)
                {
                    OPM_Original = OPM_Record_2;
                    printf("Ready To Write %f", OPM_Original); // 记录original专用
                    EN25QXX_Write((uint8_t *)&OPM_Original,
                                  FLASH_IN_OFF_BED_OPM_RECORD_ADDRESS,
                                  sizeof(float32_t));
                }
            }
            else
            {
                i_count_change2 = 0;
            }
        }
    }
    else // 不满足条件时将count指数清零
    {
        i_count_change = 0;
        i_count_change2 = 0;
    }
    if (OPM_Record_2 == 0.0f) // 记录original版专用
    {
        OPM_Record_2 = OPM_Original;
    }
    /*
    if ((OffBed_Flag == 4) && (Diff_Detail < 0.2f)) // 在床状态下检测Diff_detail下降沿
    {
        if (((Diff_Detail - Diff_detail_Past_2) < -0.008f) && (i_count_No_Breath == 0))
        {
            if (Diff_Detail < 0.03f)
                Diff_detail_Record = Diff_detail_Past_2;
        }
    }

    if ((Diff_detail_Record > 0) && ((Diff_Detail - Diff_detail_Record) < -0.008f)) // 检测下降沿保持时间
    {
        if (OffBed_Flag == 4)
        {
            i_count_No_Breath++;
        }

        if (i_count_No_Breath >= 5)
        {
            i_count_No_Breath = 5;
        }
    }
    else
    {
        Diff_detail_Record = 0;
        i_count_No_Breath = 0;
    }
    Diff_detail_Past_2 = Diff_detail_Past_1; //
    Diff_detail_Past_1 = Diff_Detail;        //
*/
    // StdRecord = Std;
    // printf("\r\n-----------------------------------------\n");
    // printf("dbm_value_1310 %f\r\n", dbm_value_1310);
    // printf("MAX %f\r\n", Max);
    // printf("Diff %f\r\n", Diff);
    // printf("i_count is %d\r\n", i_count_no_human);
    // printf("i_count_off_bed is %d\r\n", i_count_off_bed);
    // printf("OPM_Record %f\r\n", OPM_Record);
    // printf("OPM_Record_2 %f\r\n", OPM_Record_2);
    // printf("OPM_CountDown_2 %d\r\n", OPM_CountDown_2);
    // printf("\r\n-----------------------------------------\n");

    if (OffBed_Flag == 4) // 在床状态前提下进行离床判断
    {
#if SPEED_UP_MODE_EN
        if ((dbm_value_1310 - OPM_Record_2 > -0.9f) && (Max < 150.0f)) // 若和空床光功连续接近6s则判为离床
        {
            OPM_CountDown_3++;
            if (OPM_CountDown_3 >= 3)
            {
                OPM_Count_2 = 0;
                i_count_no_human = 12;
            }
        }
#else
        if ((dbm_value_1310 - OPM_Record_2 > -0.8f) && (Max < 1.5f)) // 若和空床光功连续接近6s则判为离床
        {
            OPM_CountDown_3++;
            if (OPM_CountDown_3 >= 6)
            {
                OPM_Count_2 = 0;
                i_count_no_human = 12;
            }
        }
#endif
        else
        {
            OPM_CountDown_3 = 0;
        }
        // DONE: 增加时间判断条件
        if ((i_count_off_bed >= round(0.8 * LEN_Max_array)) &&
            ((OPM_Record_2 == 0) || (dbm_value_1310 - OPM_Record_2 > -1.5f))) // 新离床判断的保险机制，追溯长度中80%满足离床条件即判为离床
        {
            i_count_no_human = 12;
        }
    }
    else if (OffBed_Flag == 0) // 离床状态前提下进行在床判断
    {
        if ((OPM_CountDown_2 > 0) && (OPM_Record < 0) && (OPM_Record_2 < 0)) // 在倒计时范围内，持续检测到下降沿台阶，进行在床判断
        {
            OPM_CountDown_2--;
            if ((dbm_value_1310 - OPM_Record < ((-1) * OPM_BODY_CHECK_THRESHOLD)))
            {
                OPM_CountDown_3 = 0;
                if ((Max > 0.003f))
                {
                    i_count_no_human--;
                    if (i_count_no_human <= 0)
                        i_count_no_human = 0;
                }
            }
        }
        else if (OPM_CountDown_2 == 0) // 若倒计时结束还在离床状态，将索引恢复
        {
            i_count_no_human = 12;
        }
        if ((Max > 0.02f)) // 在床判断的保险机制，连续20s有体征则判为在床
        {
            OPM_Count_2++;
            OPM_CountDown_3 = 0;
            if (OPM_Count_2 >= 20)
            {
                OPM_Count_2 = 20;
                i_count_no_human = 0;
                OPM_Record_2 = OPM_Original; // 防止空床光功误报，将其初始化
            }
        }
        else
        {
            OPM_Count_2 = 0;
        }
        if (i_count_no_human < COUNT_BODY_CHECK_THRESHOLD) // 已经进入在床状态后将倒计时关闭
        {
            OPM_CountDown_2 = 0;
        }
    }
    // 光功正常 开始判断在离床
    if (dbm_value_1310 > WEAK_OPM && dbm_value_1310 < HI_OPM)
    {
        if (i_count_no_human >= COUNT_BODY_CHECK_THRESHOLD)
        {
            OffBed_Flag = 0; // 离床
            FiveMinutesCount = 0;
            // 可屏蔽用于一直判断离床
            equipment_status_value = OffBed_Status;
            i_count_No_Breath = 0;
            if (OffBed_Flag_Record == 4)
            {
                // NOTE: 清除缓存
                BreathRate = 0;
                HeartBeatRate = 0;
                memset(&fft_HeartBeatRate_StashBuffer, 0,
                       sizeof(fft_HeartBeatRate_StashBuffer));
                // BreathRate_Record = 0;
                // HearDtBeaFtRate_Record = 0;
                memset(&f_BPM_hr, 0, sizeof(f_BPM_hr));
                memset(&f_BPM_br, 0, sizeof(f_BPM_br));
            }
        }
        else if (i_count_no_human < COUNT_BODY_CHECK_THRESHOLD)
        {
            OffBed_Flag = 4; // 在床
            ConvertMoveStd();
            OPM_CountDown_2 = 0;
            equipment_status_value = InBed_Status;
        }
    }
    i_count_Std = Std;
    i_count_Max = Max;
    OPM_Past_2 = OPM_Past_1;  //
    OPM_Past_1 = OPM_Current; //
    OPM_Current = dbm_value_1310;
    memcpy(TempBuffer, FitterOPMData_CheckBed, sizeof(FitterOPMData_CheckBed));
    memset(FitterOPMData_CheckBed, 0, sizeof(FitterOPMData_CheckBed));
    for (int i = 0; i < 200; i++)
        FitterOPMData_CheckBed[i] = TempBuffer[i + 50];
    OffBed_Flag_Record = OffBed_Flag;
    // printf("-----------------------------------------------------InOff End\n");
}

/**
 * @function: std_calc
 * @brief: None
 * @param {float} data
 * @param {int} n
 * @return {*}
 * @author: lzc
 */
static float std_calc(float data[], int n)
{
    float sum = 0.0, mean, standardDeviation = 0.0;
    int i;
    for (i = 0; i < n; i++)
    {
        sum += data[i];
    }
    mean = sum / n;
    for (i = 0; i < n; i++)
        standardDeviation += pow(data[i] - mean, 2);
    return sqrt(standardDeviation / n);
}

// 函数定义：将固定长度的一维数组倒置
void reverseArray(float *arr, int length)
{
    int start = 0;
    int end = length - 1;
    // 交换数组元素直到中间位置
    while (start < end)
    {
        // 交换 arr[start] 和 arr[end]
        float temp = arr[start];
        arr[start] = arr[end];
        arr[end] = temp;
        // 移动索引
        start++;
        end--;
    }
}

/**
 * @function:myFitterLocal_Hr
 * @brief: None
 * @return {*}
 * @author: lzc
 */
void myFitterLocal_Hr(const arm_biquad_casd_df1_inst_f32 *S,
                      float32_t *Input, float32_t *Output, int Length, float ScaleValue)
{
    float Value = 0.0f;
    // float ScaleValue = 0.0f; //
    float left_ext[200] = {0};
    float right_ext[200] = {0};
    float ext[HEART_FITTER_LENGTH + (200 * 2)] = {0};
    float ext_output_1[HEART_FITTER_LENGTH + (200 * 2)] = {0};
    // rt_memset(ext, 0, sizeof(ext));
    // rt_memset(left_ext, 0, sizeof(left_ext));
    // rt_memset(right_ext, 0, sizeof(right_ext));
    // rt_memset(ext_output_1, 0, sizeof(ext_output_1));
    // 取X的第一个数为left_end
    float left_end = Input[0];
    // 按顺序取X第28至第2个数，存为数组left_ext
    for (int i = 1; i <= 200; i++)
        left_ext[i - 1] = Input[(200 + 1) - i];
    // 取X的最后一个数为right_end
    float right_end = Input[Length - 1];
    // 按顺序取X倒数第2个数至倒数第28个数，存为数组right_ext
    for (int i = 1; i <= 200; i++)
    {
        right_ext[i - 1] = Input[Length - 1 - i];
    }
    // 将数组（2*left_end - left_ext）拼接在X的头端
    for (int i = 1; i <= 200; i++)
    {
        ext[i - 1] = left_ext[i - 1];
    }
    for (int i = 0; i < Length; i++)
    {
        ext[i + 200] = Input[i];
        // dbg_printf("%f\n", Input[i]);
    }
    // 将数组（2*right_end - right_ext）拼接在X的尾端
    for (int i = 0; i < 200; i++)
    {
        ext[Length + 200 + i] = (right_ext[i]);
    }
    // 将ext进行一次滤波
    for (int i = 0; i < (Length + (200 * 2)); i++)
        arm_biquad_cascade_df1_f32(S, ext + (i * blockSize), ext_output_1 + (i * blockSize), blockSize);
    /* 打印滤波后结果 */
    for (int i = 0; i < (Length + (200 * 2)); i++)
    {
        Value = ext_output_1[i] * ScaleValue;
        ext_output_1[i] = Value;
    }
    // 假设第一次滤波后的数组为ext,将滤波后的数组倒置
    reverseArray(ext_output_1, (Length + (200 * 2)));

    // 将倒置后的数组ext再次滤波
    for (int i = 0; i < (Length + (200 * 2)); i++)
        arm_biquad_cascade_df1_f32(S, ext_output_1 + (i * blockSize), ext + (i * blockSize), blockSize);
    /* 打印滤波后结果 */
    for (int i = 0; i < (Length + (2 * 200)); i++)
    {
        Value = ext[i] * ScaleValue;
        ext[i] = Value;
    }
    // 将第二次滤波后的数组ext再次倒置
    reverseArray(ext, (Length + (200 * 2)));
    // 截取滤波后的X_filtered
    for (int i = 0; i < Length; i++)
    {
        Output[i] = ext[i + 200];
    }
}

/**
 * @function:myFitterLocal_Br
 * @brief: None
 * @return {*}
 * @author: lzc
 */
void myFitterLocal_Br(const arm_biquad_casd_df1_inst_f32 *S,
                      float32_t *Input, float32_t *Output, int Length, float ScaleValue)
{
    float Value = 0.0f;
    // float ScaleValue = 0.0f; //
    float left_ext[400] = {0};
    float right_ext[400] = {0};
    float ext[HEART_FITTER_LENGTH + (400 * 2)] = {0};
    float ext_output_1[HEART_FITTER_LENGTH + (400 * 2)] = {0};
    // rt_memset(ext, 0, sizeof(ext));
    // rt_memset(left_ext, 0, sizeof(left_ext));
    // rt_memset(right_ext, 0, sizeof(right_ext));
    // rt_memset(ext_output_1, 0, sizeof(ext_output_1));
    // 取X的第一个数为left_end
    float left_end = Input[0];
    // 按顺序取X第28至第2个数，存为数组left_ext
    for (int i = 1; i <= 400; i++)
        left_ext[i - 1] = Input[(400 + 1) - i];
    // 取X的最后一个数为right_end
    float right_end = Input[Length - 1];
    // 按顺序取X倒数第2个数至倒数第28个数，存为数组right_ext
    for (int i = 1; i <= 400; i++)
    {
        right_ext[i - 1] = Input[Length - 1 - i];
    }
    // 将数组（2*left_end - left_ext）拼接在X的头端
    for (int i = 1; i <= 400; i++)
    {
        ext[i - 1] = left_ext[i - 1];
    }
    for (int i = 0; i < Length; i++)
    {
        ext[i + 400] = Input[i];
        // dbg_printf("%f\n", Input[i]);
    }
    // 将数组（2*right_end - right_ext）拼接在X的尾端
    for (int i = 0; i < 400; i++)
    {
        ext[Length + 400 + i] = (right_ext[i]);
    }
    // 将ext进行一次滤波
    for (int i = 0; i < (Length + (400 * 2)); i++)
        arm_biquad_cascade_df1_f32(&HeartPart, ext + (i * blockSize), ext_output_1 + (i * blockSize), blockSize);
    /* 打印滤波后结果 */
    for (int i = 0; i < (Length + (400 * 2)); i++)
    {
        Value = ext_output_1[i] * ScaleValue;
        ext_output_1[i] = Value;
    }
    // 假设第一次滤波后的数组为ext,将滤波后的数组倒置
    reverseArray(ext_output_1, (Length + (400 * 2)));
    // 将倒置后的数组ext再次滤波
    for (int i = 0; i < (Length + (400 * 2)); i++)
        arm_biquad_cascade_df1_f32(&HeartPart, ext_output_1 + (i * blockSize), ext + (i * blockSize), blockSize);
    /* 打印滤波后结果 */
    for (int i = 0; i < (Length + (2 * 400)); i++)
    {
        Value = ext[i] * ScaleValue;
        ext[i] = Value;
    }
    // 将第二次滤波后的数组ext再次倒置
    reverseArray(ext, (Length + (400 * 2)));
    // 截取滤波后的X_filtered
    for (int i = 0; i < Length; i++)
    {
        Output[i] = ext[i + 400];
    }
}