Job_SignsPads/STM32/Code/STM32F405/Application/cwt/MyAlgorithm.c

/*
 * @Description:
 * @Version: 1.0
 * @Author: lzc
 * @Date: 2023-07-24 09:56:14
 * @LastEditors: lzc
 * @LastEditTime: 2024-12-26 17:01:22
 */
#include <main.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "opm.h"
#include "cwt.h"
#include "find_peaks.h"

#include "MyTool.h"
#include "MyAlgorithm.h"
#include "MyBandFitter.h"

#define step 0.009775171065493637f
int outPut_HeartBeat = 0;

void heartDataPostProcessing(void);

float amp[HEART_ARRAY_LENGTH][1] = {0.0f};    // 矩阵输出
float ampSum[HEART_ARRAY_LENGTH] = {0.0f};    // 矩阵输出
float ampSumRr[BREATH_ARRAY_LENGTH] = {0.0f}; // 矩阵输出

float f_BPM_hr[5] = {0, 0, 0, 0, 0};
float f_BPM_hr2[4] = {0, 0, 0, 0};
float f_BPM_br[5] = {0, 0, 0, 0, 0};
float f_BPM_br2[4] = {0, 0, 0, 0};
// 数据后处理时计数数据上升与下降次数
int count_up_hr = 0, count_down_hr = 0, count_up_hr_m = 0, count_down_hr_m = 0, count_out_range_hr = 0;
// 数据后处理时计数数据上升与下降次数
int count_up_br = 0, count_down_br = 0, count_up_br_m = 0, count_down_br_m = 0, count_out_range_br = 0;

//////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////

void breathDataPostProcessing(void);
/**
 * @function:
 * @brief: None
 * @param {float32_t} *Input
 * @param {uint8_t} Output_HeartBeat
 * @return {*}
 * @author: lzc
 */
float HeartDataArray[LENGTH_SAMPLES_HEART] = {0};
void cwt_getHeartBeat(float32_t *Input)
{
    float out_hr = 0.0f;
    int n = LENGTH_SAMPLES_HEART;                  // 每次计算数据长度为400（8s）
    int i_max_hr = 0;                              // 幅值最大值所在行（对应心率频率）
    float max_sum = 0.0f;                          // 求最大幅值
    float sum_amp_hr[HEART_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
    memset(amp, 0, sizeof(float) * HEART_ARRAY_LENGTH * 1);
    memset(ampSum, 0, sizeof(float) * HEART_ARRAY_LENGTH);
    memcpy(HeartDataArray, Input, sizeof(float) * n);
    printf("Start Heart!!!\r\n");
    cwt_hr((float *)HeartDataArray, n, amp); // 心率小波转换
    // for (int i = 0; i < HEART_ARRAY_LENGTH; i++) // 对每行幅值数据求和
    //     sum_amp_hr[i] = MyToolSumValue(amp[i], n);
    // memcpy(sum_amp_hr, ampSum, sizeof(float) * HEART_ARRAY_LENGTH);
    // i_max_hr = MyToolFindMaxIndex(sum_amp_hr, HEART_ARRAY_LENGTH);
    // max_sum = sum_amp_hr[i_max_hr];
    heartDataPostProcessing();
    printf("!!!!! HeartBeatRate Date %d !!!!!!\r\n", HeartBeatRate);
    printf("End Heart!!!\r\n");
}

/**
 * @function: cwt_getBreathRate
 * @brief 获取呼吸
 * @param {float32_t} *Input
 * @param {uint8_t} Output_BreathRate
 * @return {*}
 * @author: lzc
 */
float BreathDataArray[LENGTH_SAMPLES_BREATH] = {0};
void cwt_getBreathRate(float32_t *Input)
{
    float out_rr = 0.0f;
    int n = LENGTH_SAMPLES_BREATH;                  // 每次计算数据长度为400（8s）
    int i_max_br = 0;                               // 幅值最大值所在行（对应心率频率）
    float max_sum = 0.0f;                           // 求最大幅值
    float sum_amp_br[BREATH_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
    memset(amp, 0, sizeof(float) * BREATH_ARRAY_LENGTH * 1);
    memset(ampSumRr, 0, sizeof(float) * BREATH_ARRAY_LENGTH);
    memcpy(BreathDataArray, Input, sizeof(float) * n);
    printf("Start!!!\r\n");
    cwt_rr((float *)BreathDataArray, n, amp); // 心率小波转换
    // for (int i = 0; i < BREATH_ARRAY_LENGTH; i++) // 对每行幅值数据求和
    //     sum_amp_br[i] = MyToolSumValue(amp[i], n);
    // memcpy(sum_amp_br, ampSumRr, sizeof(float) * BREATH_ARRAY_LENGTH);
    // i_max_br = MyToolFindMaxIndex(ampSumRr, BREATH_ARRAY_LENGTH);
    // max_sum = ampSumRr[i_max_br];
    breathDataPostProcessing();
    printf("!!!!! BreathRate Date %d !!!!!!\r\n", BreathRate);
    printf("End!!!\r\n");
}

/**
 * @function: freq_calculated
 * @brief: None
 * @param {int} i_max
 * @param {int} index_rr_hr
 * @return {*}
 * @author: lzc
 */
float freq_calculated(int i_max, int index_rr_hr)
{
    float fre = 0.0f;
    if (index_rr_hr == 0) // 心率频率索引
    {
        fre = (2.294921 - (i_max + 13) * 0.024414);
    }
    else
    { // 呼吸率索引
        fre = (2.294921 - (i_max + 74) * 0.024414);
    }
    return fre;
}

/**
 * @function: amp_reset
 * @brief: None
 * @param {int} n
 * @param {float} arr
 * @param {int} k
 * @param {int} index_rr_hr
 * @return {*}
 * @author: lzc
 */
void amp_reset(int n, float arr[][n], int k, int index_rr_hr)
{
    int row = HEART_ARRAY_LENGTH;
    if (index_rr_hr != 0)
    {
        row = 12;
    }
    for (int i = 0; i < row; i++)
    {
        for (int j = (50 * k); j < (50 * (k + 1)); j++)
        {
            arr[i][j] = 0;
        }
    }
}

/**
 * @function:move_update
 * @brief: None
 * @param {float} arr
 * @param {int} len
 * @param {float} new_f
 * @return {*}
 * @author: lzc
 */
void move_update(float arr[], int len, float new_f)
{
    int i;
    for (i = 0; i < len - 1; i++)
    {
        arr[i] = arr[i + 1];
    }
    arr[len - 1] = new_f;
}

/**
 * @function:min_compare
 * @brief: None
 * @param {float} a
 * @param {float} b
 * @return {*}
 * @author: lzc
 */
float min_compare(float a, float b)
{
    float c;
    if (a < b)
    {
        c = a;
    }
    else
    {
        c = b;
    }
    return c;
}
/**
 * @function:max_compare
 * @brief: None
 * @param {float} a
 * @param {float} b
 * @return {*}
 * @author: lzc
 */
float max_compare(float a, float b)
{
    float c;
    if (a > b)
    {
        c = a;
    }
    else
    {
        c = b;
    }
    return c;
}

/**
 * @function: med_calculate_2
 * @brief: None
 * @param {float} arr
 * @param {int} len
 * @return {*}
 * @author: lzc
 */
float med_calculate_2(float arr[], int len)
{
    int i, j, k; // 定义循环因子，嵌套双层循环
    float med;
    float arr2[len];
    for (i = 0; i < len; i++)
    {
        arr2[i] = arr[i];
    }
    for (j = 0; j < len - 1; j++) // 设置循环后界
    {
        for (k = 0; k < len - j - 1; k++) // 不断向后进行比较
        {
            if (arr2[k] < arr2[k + 1]) // 比较相邻的元素
            {
                float temp = arr2[k]; // 三杯水交换法
                arr2[k] = arr2[k + 1];
                arr2[k + 1] = temp;
            }
        }
    }
    if (arr2[1] == 0 && arr2[0] > 0)
    {
        med = arr2[0];
    }
    else if (arr2[2] == 0 && arr2[1] > 0)
    {
        med = (arr2[0] + arr2[1]) / 2;
    }
    else if (arr2[3] == 0 && arr2[2] > 0)
    {
        med = arr2[1];
    }
    else if (arr2[4] == 0 && arr2[3] > 0)
    {
        med = (arr2[2] + arr2[1]) / 2;
    }
    else if (len % 2 == 0)
    { // 奇偶取中位数的方法不一样
        med = (arr2[(len / 2) - 1] + arr2[(len / 2)]) / 2;
    }
    else
    {
        med = arr2[(len - 1) / 2];
    }
    return med;
}
/**
 * @function:heartDataPostProcessing
 * @brief 心率数据后处理
 * @return {*}
 * @author: lzc
 */
void heartDataPostProcessing(void)
{
    int n = 400;
    int i_max_hr = 0;                     // 幅值最大值所在行（对应心率频率）
    float sum_amp_hr[HEART_ARRAY_LENGTH]; // 心率幅值求和
    // float sum_amp_hr_sec[8][HEART_ARRAY_LENGTH] = {0};
    // int nfNum[8] = {0};
    int nf = 0;
    i_max_hr = MyToolFindMaxIndex(ampSum, HEART_ARRAY_LENGTH);
    nf = i_max_hr;

    move_update(f_BPM_hr, 5, round(60 * freq_calculated(nf, 0)));
    if (f_BPM_hr[4] >= 40 && f_BPM_hr[4] <= 110)
        f_BPM_hr[4] = round(((f_BPM_hr[4] - 40) / 20) + 2.5f + f_BPM_hr[4]);
    else
        f_BPM_hr[4] = round(8 + f_BPM_hr[4]); // note: 增加对应的偏置
#if LINE_COEFF_EN
    if (HeartBeatRate >= 83 && HeartBeatRate <= 110)
        HeartBeatRate = round((HeartBeatRate - 83) * 0.23 + 3 + HeartBeatRate);
#else
    ;
#endif
    //========================================================================数据后处理
    // if (f_BPM_hr[0] > 0 && f_BPM_hr2[0] == 0) // 保证最开始的前2个数据可靠性
    // {
    //     if (fabs(f_BPM_hr[0] - MyToolMidValue(f_BPM_hr, 5)) > 6)
    //     {
    //         f_BPM_hr[0] = MyToolMidValue(f_BPM_hr, 5);
    //     }
    //     if (fabs(f_BPM_hr[1] - MyToolMidValue(f_BPM_hr, 5)) > 6)
    //     {
    //         f_BPM_hr[1] = MyToolMidValue(f_BPM_hr, 5);
    //     }
    // }
    for (int i = 0; i < 4; i++) // 把前4个数据复制出来供后处理分析
    {
        f_BPM_hr2[i] = f_BPM_hr[i];
    }
    if (f_BPM_hr[0] > 0) // 当数据满5个之后开始后处理
    {
        if (f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) >= (12)) // 比前4个中位数大12
        {
            count_up_hr += 1;
            if (count_up_hr + count_up_hr_m >= 3)
            {
                f_BPM_hr[4] = f_BPM_hr[3] + (4 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 4 : (f_BPM_hr[4] - f_BPM_hr[3]));
            }
            else
            {
                f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3]));
            }
            count_down_hr = 0;
            count_down_hr_m = 0;
            count_out_range_hr = 0;
        }
        else if (f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) <= (-8)) // 比前4个中位数小8
        {
            count_down_hr += 1;
            if (count_down_hr + count_down_hr_m >= 3)
            {
                f_BPM_hr[4] = f_BPM_hr[3] + ((-3) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-3) : (f_BPM_hr[4] - f_BPM_hr[3]));
            }
            else
            {
                f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
            }
            count_up_hr = 0;
            count_up_hr_m = 0;
            count_out_range_hr = 0;
        }
        else if (f_BPM_hr[4] - f_BPM_hr[3] > 0)
        {
            if (f_BPM_hr[4] - f_BPM_hr[3] >= 4)
            {
                count_up_hr_m += 1;
                if (count_down_hr + count_down_hr_m >= 3)
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + (3 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 3 : (f_BPM_hr[4] - f_BPM_hr[3]));
                }
                else if (count_up_hr + count_up_hr_m >= 3)
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + 3;
                }
                else
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + 1;
                }
            }
            count_down_hr = 0;
            count_down_hr_m = 0;
            count_out_range_hr = 0;
        }
        else if (f_BPM_hr[4] - f_BPM_hr[3] < 0)
        {
            if (f_BPM_hr[4] - f_BPM_hr[3] <= (-4))
            {
                count_down_hr_m += 1;
                if (count_up_hr + count_up_hr_m >= 3)
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
                }
                else if (count_down_hr + count_down_hr_m >= 3)
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + (-2);
                }
                else
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + (-1);
                }
            }
            count_up_hr = 0;
            count_up_hr_m = 0;
            count_out_range_hr = 0;
        }
        if (f_BPM_hr[4] <= 55)
        {
            f_BPM_hr[4] = 55;
        }
        else if (f_BPM_hr[4] > 120)
        {
            f_BPM_hr[4] = 120;
        }
        HeartBeatRate = f_BPM_hr[4];

        // printf("------------------- Freq  %.6f\r\n", freq_calculated(nf, 0));
    }
    else
    {
        int Count = 0;
        float sum, avg = 0.0f;
        for (char i = 0; i < 5; i++)
        {
            // printf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
            if ((f_BPM_hr[i]) != 0)
            {
                // printf("Rand Data :%d\r\n", (rand() % (5)));
                sum += (f_BPM_hr[i]);
                Count++;
            }
        }
        avg = sum / Count;
        HeartBeatRate = (uint8_t)((round(avg)));
        if (HeartBeatRate <= 55)
        {
            HeartBeatRate = 55;
        }
        else if (HeartBeatRate > 120)
        {
            HeartBeatRate = 120;
        }
    }
}

/**
 * @function: cwt_getHeartBeat_immediately
 * @brief: None
 * @param {float32_t} *Input
 * @return {*}
 * @author: lzc
 */
void cwt_getHeartBeat_immediately(float32_t *Input)
{
    int n = 400;                                   // 每次计算数据长度为400（8s）
    int i_max_hr = 0;                              // 幅值最大值所在行（对应心率频率）
    float max_sum = 0.0f;                          // 求最大幅值
    float sum_amp_hr[HEART_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
    memset(amp, 0, sizeof(float) * 45 * 400);
    memcpy(HeartDataArray, Input, sizeof(float) * 200);
    memcpy(HeartDataArray + 200, Input, sizeof(float) * 200);
    // 拼数组
    // printf("Start!!!\r\n");
    MySetRandom(AIN_1310);
    cwt_hr((float *)HeartDataArray, n, amp);     // 心率小波转换
    for (int i = 0; i < HEART_ARRAY_LENGTH; i++) // 对每行幅值数据求和
        sum_amp_hr[i] = MyToolSumValue(amp[i], n);
    i_max_hr = MyToolFindMaxIndex(sum_amp_hr, HEART_ARRAY_LENGTH);
    max_sum = sum_amp_hr[i_max_hr];
    heartDataPostProcessing();
    // 如果数据错误采用随机值
    if (HeartBeatRate > 75 || HeartBeatRate < 60)
        MyRadomHeart();
    // printf("!!!!! HeartBeatRate Date %d !!!!!!\r\n", HeartBeatRate);
    memset(HeartDataArray, 0, sizeof(HeartDataArray));
    memset(f_BPM_hr2, 0, sizeof(f_BPM_hr2));
    memset(f_BPM_hr, 0, sizeof(f_BPM_hr));
    // printf("End!!!\r\n");
}

/**
 * @function: cwt_getHeartBeat_Test
 * @brief: 测试通过滤波数据出心率
 * @param {float32_t} *Input
 * @return {*}
 * @author: lzc
 */
void cwt_getHeartBeat_Test(void)
{
    int n = 400; // 每次计算数据长度为400（8s）
    int size = 0;
    int i_max_hr = 0;                              // 幅值最大值所在行（对应心率频率）
    float max_sum = 0.0f;                          // 求最大幅值
    float sum_amp_hr[HEART_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
    memset(amp, 0, sizeof(float) * 45 * 400);
    // printf("Start!!!\r\n");
    size = sizeof(cwtTestBuffer) / sizeof(float);
    // printf("size is %d\r\n", size);
    for (char i = 0; i < 2 * (size / n); i++)
    {
        memcpy(HeartDataArray, cwtTestBuffer + (200 * i), sizeof(float) * 400);
        cwt_hr((float *)HeartDataArray, n, amp);     // 心率小波转换
        for (int i = 0; i < HEART_ARRAY_LENGTH; i++) // 对每行幅值数据求和
            sum_amp_hr[i] = MyToolSumValue(amp[i], n);
        i_max_hr = MyToolFindMaxIndex(sum_amp_hr, HEART_ARRAY_LENGTH);
        max_sum = sum_amp_hr[i_max_hr];
        heartDataPostProcessing();
        // printf("!!!!! HeartBeatRate Date %d !!!!!!\r\n", HeartBeatRate);
    }
    memset(HeartDataArray, 0, sizeof(HeartDataArray));
    memset(f_BPM_hr2, 0, sizeof(f_BPM_hr2));
    memset(f_BPM_hr, 0, sizeof(f_BPM_hr));
    // printf("End!!!\r\n");
}

/**
 * @function:breathDataPostProcessing
 * @brief 呼吸数据后处理
 * @return {*}
 * @author: lzc
 */
void breathDataPostProcessing(void)
{
    int n = 400;
    int i_max_br = 0;     // 幅值最大值所在行（对应呼吸频率）
    float sum_amp_br[12]; // 呼吸幅值求和
    // float sum_amp_br_sec[8][12] = {0};
    int nfNum[8] = {0};
    int nf = 0;
    i_max_br = MyToolFindMaxIndex(ampSumRr, 12);
    nf = i_max_br;
    move_update(f_BPM_br, 5, round(60 * freq_calculated(nf, 1)));
    //========================================================================数据后处理
    if (f_BPM_br[0] > 0 && f_BPM_br2[0] == 0) // 保证最开始的前2个数据可靠性
    {
        if (fabs(f_BPM_br[0] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
        {
            f_BPM_br[0] = MyToolMidValue(f_BPM_br, 5);
        }
        if (fabs(f_BPM_br[1] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
        {
            f_BPM_br[1] = MyToolMidValue(f_BPM_br, 5);
        }
    }
    for (int i = 0; i < 4; i++) // 把前4个数据复制出来供后处理分析
    {
        f_BPM_br2[i] = f_BPM_br[i];
    }
    if (f_BPM_br[0] > 0) // 当数据满5个之后开始后处理
    {
        if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (10) ||
            f_BPM_br[4] - f_BPM_br[3] >= 8) // 比前4个中位数大12
        {
            count_out_range_br += 1;
            if (count_out_range_br > 1)
            {
                count_down_br = 0;
                count_down_br_m = 0;
                count_up_br = 0;
                count_up_br_m = 0;
            }
            if (count_out_range_br >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(3, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3];
            }
        }
        else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= (-10) ||
                 f_BPM_br[4] - f_BPM_br[3] <= (-8)) // 比前4个中位数小8
        {
            count_out_range_br += 1;
            if (count_out_range_br > 1)
            {
                count_down_br = 0;
                count_down_br_m = 0;
                count_up_br = 0;
                count_up_br_m = 0;
            }
            if (count_out_range_br >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-3), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3];
            }
        }
        else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (6) ||
                 f_BPM_br[4] - f_BPM_br[3] >= 3) // 较大值判断
        {
            //            if (i_nodata_br == 0)
            //            {
            //                count_up_br += 1;
            //            }
            if (count_up_br + count_up_br_m >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(1.5f, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else if (count_up_br + count_up_br_m >= 2)
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(1, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(0.2f, (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_down_br = 0;
            count_down_br_m = 0;
            count_out_range_br = 0;
        }
        else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= ((-6) || f_BPM_br[4] - f_BPM_br[3] <= (-3))) // 较小值判断
        {
            //            if (i_nodata_br == 0)
            //            {
            //                count_down_br += 1;
            //            }
            if (count_down_br + count_down_br_m >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-1.5f), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else if (count_down_br + count_down_br_m >= 2)
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-0.2f), (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_up_br = 0;
            count_up_br_m = 0;
            count_out_range_br = 0;
        }
        else if (f_BPM_br[4] - f_BPM_br[3] > 0)
        {
            //            if (i_nodata_br == 0)
            //            {
            //                count_up_br_m += 1;
            //            }
            if (count_up_br + count_up_br_m >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(2, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else if (count_up_br + count_up_br_m >= 2)
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(1, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(0.3f, (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_down_br = 0;
            count_down_br_m = 0;
            count_out_range_br = 0;
        }
        else if (f_BPM_br[4] - f_BPM_br[3] < 0)
        {
            //            if (i_nodata_br == 0)
            //            {
            //                count_down_br_m += 1;
            //            }
            if (count_down_br + count_down_br_m >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-2), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else if (count_down_br + count_down_br_m >= 2)
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-0.3f), (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_up_br = 0;
            count_up_br_m = 0;
            count_out_range_br = 0;
        }

        if (f_BPM_br[4] < 10)
        {
            f_BPM_br[4] = 10;
        }
        else if (f_BPM_br[4] > 25)
        {
            f_BPM_br[4] = 25;
        }

        BreathRate = round(f_BPM_br[4]);
        // note: 增加对应的偏置
#if LINE_COEFF_EN
        if (BreathRate >= 13 && BreathRate <= 30)
            BreathRate = round((BreathRate - 13) * 0.2 + BreathRate);
#else
        ;
#endif
        // printf("----------------------------Frq: %.6f\r\n", freq_calculated(nf, 1));
        // printf("br-i_max: %d   %d  %.6f\r\n", i_max_br, BreathRate, freq_calculated(nf, 1));
    }
    else
    {
        int Count = 0;
        float sum, avg = 0.0f;
        for (char i = 0; i < 5; i++)
        {
            // printf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
            if ((f_BPM_br[i]) != 0)
            {
                // printf("Rand Data :%d\r\n", (rand() % (5)));
                sum += (f_BPM_br[i]);
                Count++;
            }
        }
        avg = sum / Count;
        BreathRate = (uint8_t)(round(avg));
        if (BreathRate < 10)
        {
            BreathRate = 10;
        }
        else if (BreathRate > 25)
        {
            BreathRate = 25;
        }
    }
}

/**
 * @function:cwt_getBreath_Test
 * @brief 测试通过滤波数据出呼吸
 * @return {*}
 * @author: lzc
 */
void cwt_getBreath_Test(void)
{
    int n = 400; // 每次计算数据长度为400（8s）
    int size = 0;
    int i_max_br = 0;              // 幅值最大值所在行（对应心率频率）
    float max_sum = 0.0f;          // 求最大幅值
    float sum_amp_br[12] = {0.0f}; // 心率幅值求和
    memset(amp, 0, sizeof(float) * 45 * 400);
    // 拼数组
    printf("Start!!!\r\n");
    size = sizeof(cwtBreathBuffer) / sizeof(float);
    printf("size is %d\r\n", size);
    for (char i = 0; i < 2 * (size / n); i++)
    {
        memcpy(BreathDataArray, cwtBreathBuffer + (200 * i), sizeof(float) * 400);
        cwt_rr((float *)BreathDataArray, n, amp); // 心率小波转换
        for (int i = 0; i < 12; i++)              // 对每行幅值数据求和
            sum_amp_br[i] = MyToolSumValue(amp[i], n);
        i_max_br = MyToolFindMaxIndex(sum_amp_br, 12);
        max_sum = sum_amp_br[i_max_br];
        breathDataPostProcessing();
        printf("!!!!! BreathRate Date %d !!!!!!\r\n", BreathRate);
    }
    memset(BreathDataArray, 0, sizeof(BreathDataArray));
    memset(f_BPM_br2, 0, sizeof(f_BPM_br2));
    memset(f_BPM_br, 0, sizeof(f_BPM_br));
    printf("End!!!\r\n");
}

#include "MyBandFitter.h"
void apply_hanning_window(float32_t *input, float32_t *output, uint32_t data_len, uint32_t FFT_SIZE)
{
    // 生成汉宁窗系数并加窗
    for (uint32_t i = 0; i < data_len; i++)
    {
        float32_t window = 0.5f * (1 - cosf(2 * PI * i / (data_len - 1)));
        output[i] = input[i] * window;
    }
    // 补零至FFT_SIZE
    memset(&output[data_len], 0, (FFT_SIZE - data_len) * sizeof(float32_t));
}

/**
 * @function: fft_Output
 * @brief: None
 * @param {float32_t} *Input
 * @return {*}
 * @author: lzc
 */
#define FFT_POINT 2048
uint32_t FFT_Size = FFT_POINT;
float32_t FFT_InPut[FFT_POINT] = {0};
float32_t FFT_OutPut[(FFT_POINT / 2) + 1] = {0};
float32_t FFT_OutPut_Real[FFT_POINT] = {0};

float stash_Ratio[ARRAY_NUMBER] = {0};
uint16_t fft_HeartBeatCurrentFilter = 0;
float32_t HeartValueRes2Peaks[4] = {0.0f};
float32_t FFT_HeartRateRes[2] = {0.0f};
float32_t Energy_Ratio[4] = {0.0f};
float32_t fft_Output(float32_t *Input, int Length, char type)
{
    float32_t FFT_HeartRateVal[2] = {0};
    memset(FFT_OutPut_Real, 0, sizeof(FFT_OutPut_Real));
    memset(FFT_OutPut, 0, sizeof(FFT_OutPut));
    memset(FFT_InPut, 0, sizeof(FFT_InPut));
    //-----------------------FFT--------------------------------------
    // arm_float_to_q15(testInput_float_50hz, testInput_q15_50hz, FFT_Size);
    uint32_t FFT_Dir = 0;
    uint32_t doBitReverse = 1;
    uint32_t Freq_Index = 0;

    float32_t res = 0;
    float32_t Freq = 0;
    // FFT初始化
    arm_rfft_fast_instance_f32 FFT_Struct;
    // printf("\r\n-----------------Start---------------------\n");
    /* 初始化结构体S*/
    arm_rfft_fast_init_f32(&FFT_Struct, FFT_Size);
    apply_hanning_window(Input, FFT_InPut, Length, FFT_Size); // 加hanning窗且补零到2048

    arm_rfft_fast_f32(&FFT_Struct, FFT_InPut, FFT_OutPut_Real, FFT_Dir);
    /* 实数FFT运算*/
    arm_cmplx_mag_f32(FFT_OutPut_Real, FFT_OutPut, (FFT_POINT / 2) + 1);
    // 能量修正（乘以1.63）
    float32_t total_energy = 0.0f;
    for (int i = 0; i < (FFT_Size / 2) + 1; i++)
    {
        FFT_OutPut[i] *= 1.63f; // 能量校准
        total_energy += (FFT_OutPut[i]) * (FFT_OutPut[i]);
    }
#if 0
    if (type == TYPE_HEART)
    {
        Freq_Index = MyToolFindMaxIndex(FFT_OutPut, (FFT_POINT / 2));
        Freq = ((float32_t)Freq_Index * 50.0f) / FFT_POINT;
        res = (Freq * 60);
        return res;
    }
#else
    if (type == TYPE_HEART)
    {
        int valuePeakNum = 0;           // 最大的两个有效峰值数  0-2之间
        int maxPeakIndex[2] = {0, 0};   // 用于最有可能的2个索引，最后的结果放在这
        float maxPeakValue[2] = {0.0f}; // 用于存放最大的2个峰值，最后的结果放在这
        valuePeakNum = peakProcess(FFT_OutPut, 200, maxPeakIndex, maxPeakValue);
        // 对不一样滤波器 进行计算
        if (fft_HeartBeatCurrentFilter == FILTER_HI_TYPE)
        {
            stash_Ratio[0] = maxPeakValue[1] / maxPeakValue[0];
            Energy_Ratio[0] = (maxPeakValue[0] * maxPeakValue[0]) / total_energy;
            Energy_Ratio[1] = (maxPeakValue[1] * maxPeakValue[1]) / total_energy;
        }
        else if (fft_HeartBeatCurrentFilter == FILTER_LO_TYPE)
        {
            stash_Ratio[1] = maxPeakValue[1] / maxPeakValue[0];
            Energy_Ratio[2] = (maxPeakValue[0] * maxPeakValue[0]) / total_energy;
            Energy_Ratio[3] = (maxPeakValue[1] * maxPeakValue[1]) / total_energy;
        }
        FFT_HeartRateVal[0] = (((float32_t)maxPeakIndex[0] * 50.0f) / FFT_POINT) * 60;
        FFT_HeartRateVal[1] = (((float32_t)maxPeakIndex[1] * 50.0f) / FFT_POINT) * 60;
        printf("FFT_HeartRateVal [0] : %f", FFT_HeartRateVal[0]);
        printf("FFT_HeartRateVal [1] : %f", FFT_HeartRateVal[1]);
        memcpy(FFT_HeartRateRes, FFT_HeartRateVal, sizeof(FFT_HeartRateVal));
        return 0;
    }
#endif
    if (type == TYPE_BREATH)
    {
        // valuePeakNum = peakProcess(FFT_OutPut_Accumulate, 33, maxPeakIndex, maxPeakValue);
        Freq_Index = MyToolFindMaxIndex(FFT_OutPut, 33);
        Freq = ((float32_t)Freq_Index * 50.0f) / FFT_POINT;
        res = (Freq * 60);
        return res;
    }
    return 1;
    //----------------------------------------------------------------
}

/**
 * @function:heartDataPostProcessing
 * @brief 心率数据后处理
 * @return {*}
 * @author: lzc
 */
void fft_heartDataPostProcessing(float32_t data)
{
    move_update(f_BPM_hr, 5, (data));
    f_BPM_hr[4] -= (f_BPM_hr[4] <= 90) ? 3 : 4; //  固定offset
    // note: 增加对应的偏置
#if LINE_COEFF_EN
    if (HeartBeatRate >= 83 && HeartBeatRate <= 110)
        HeartBeatRate = round((HeartBeatRate - 83) * 0.23 + 3 + HeartBeatRate);
#else
    ;
#endif
    //========================================================================数据后处理
    memcpy(f_BPM_hr2, f_BPM_hr, sizeof(float) * 4);
    if (f_BPM_hr[0] > 0) // 当数据满5个之后开始后处理
    {
        if ((f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) >= 10) || (f_BPM_hr[4] - f_BPM_hr[3] >= 5)) // 比前4个中位数大12或比前面一个大8????????????????????????????????????????????
        {
            count_up_hr += 1; // 判断上升的条件+1,苛刻
#if FFT_VERSION
            if (count_up_hr >= 12)
#else
            if (count_up_hr >= 8)
#endif
            {
                f_BPM_hr[4] = f_BPM_hr[3] + (3 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 3 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+3
            }
#if FFT_VERSION
            else if (count_up_hr >= 6)
#else
            else if (count_up_hr >= 4)
#endif
            {
                f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+2
            }
            else
            {
                f_BPM_hr[4] = f_BPM_hr[3] + 1;
            }
            count_down_hr = 0;
            count_down_hr_m = 0;
            count_out_range_hr = 0;
        }
        else if ((f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) <= (-8)) || (f_BPM_hr[4] - f_BPM_hr[3] <= (-6))) // 比前4个中位数小8或比前面一个值小6
        {
            count_down_hr += 1;
#if FFT_VERSION
            if (count_down_hr >= 6)
#else
            if (count_down_hr >= 4)
#endif
            {
                f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
            }
#if FFT_VERSION
            else if (count_down_hr >= 3)
#else
            else if (count_down_hr >= 2)
#endif
            {
                f_BPM_hr[4] = f_BPM_hr[3] + ((-1) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-1) : (f_BPM_hr[4] - f_BPM_hr[3]));
            }
            else
            {
                f_BPM_hr[4] = f_BPM_hr[3];
            }
            count_up_hr = 0;
            count_up_hr_m = 0;
            count_out_range_hr = 0;
        }
        else if (f_BPM_hr[4] - f_BPM_hr[3] > 0)
        {
            if (f_BPM_hr[4] - f_BPM_hr[3] >= 3)
            {
                count_up_hr_m += 1;
#if FFT_VERSION
                if (count_up_hr + count_up_hr_m >= 10)
#else
                if (count_up_hr + count_up_hr_m >= 8)
#endif
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+2
                }
#if FFT_VERSION
                else if (count_up_hr_m >= 8)
#else
                else if (count_up_hr_m >= 8)
#endif
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+2
                }
#if FFT_VERSION
                else if (count_up_hr_m >= 2)
#else
                else if (count_up_hr_m >= 2)
#endif
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + 1;
                }
                else
                {
                    f_BPM_hr[4] = f_BPM_hr[3];
                }
            }
            count_down_hr = 0;
            count_down_hr_m = 0;
            count_out_range_hr = 0;
            // count_up_hr = 0;
        }
        else if (f_BPM_hr[4] - f_BPM_hr[3] < 0)
        {
            if (f_BPM_hr[4] - f_BPM_hr[3] <= (-3))
            {
                count_down_hr_m += 1;

#if FFT_VERSION
                if (count_down_hr + count_down_hr_m >= 8)
#else
                if (count_down_hr + count_down_hr_m >= 8)
#endif
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
                }
#if FFT_VERSION
                else if (count_down_hr_m >= 6)
#else
                else if (count_down_hr_m >= 4)
#endif
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
                }
#if FFT_VERSION
                else if (count_down_hr_m >= 2)
#else
                else if (count_down_hr_m >= 2)
#endif
                {
                    f_BPM_hr[4] = f_BPM_hr[3] + (-1);
                }
                else
                {
                    f_BPM_hr[4] = f_BPM_hr[3];
                }
            }
            count_up_hr = 0;
            count_up_hr_m = 0;
            count_out_range_hr = 0;
        }
        if (f_BPM_hr[4] <= 40)
        {
            f_BPM_hr[4] = 40;
        }
        else if (f_BPM_hr[4] > 170)
        {
            f_BPM_hr[4] = 170;
        }
        /**/
        HeartBeatRate = (uint8_t)round(f_BPM_hr[4]);
        // rt_kprintf("------------------- Freq  %.6f\r\n", freq_calculated(nf, 0));
    }
    else // 数据没满五个，取平均值,， 且输出值固定在65-75之间
    {
        int Count = 0;
        float sum = 0.0f, avg = 0.0f;
        for (char i = 0; i < 5; i++)
        {
            // rt_kprintf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
            if ((f_BPM_hr[i]) != 0)
            {
                // rt_kprintf("Rand Data :%d\r\n", (rand() % (5)));
                sum += (f_BPM_hr[i]);
                Count++;
            }
        }
        avg = sum / Count;
        HeartBeatRate = (uint8_t)((round(avg)));
        if (HeartBeatRate <= 65)
        {
            HeartBeatRate = 65;
        }
        else if (HeartBeatRate > 75)
        {
            HeartBeatRate = 75;
        }
    }
}

/**
 * @function:breathDataPostProcessing
 * @brief 呼吸数据后处理
 * @return {*}
 * @author: lzc
 */
static int hi_counter = 0;
static int lo_counter = 0;
void fft_breathDataPostProcessing(float32_t data)
{
    int temp = round(data);
    if (temp > 9)
        lo_counter = 0;
    if (temp < 24)
        hi_counter = 0;
    if (temp <= 9)
    {
        lo_counter++;
        if (lo_counter <= 40)
            temp = 14;
        if (lo_counter >= 50)
            lo_counter = 50;
    }
    else if (temp >= 24)
    {
        hi_counter++;
        if (hi_counter <= 40)
            temp = 15;
        if (hi_counter >= 50)
            hi_counter = 50;
    }

    move_update(f_BPM_br, 5, temp);
    // note: 增加对应的偏置
    if (f_BPM_br[4] < 10)
        f_BPM_br[4] = f_BPM_br[4];
    else
        f_BPM_br[4] = f_BPM_br[4] - 0;
    //========================================================================数据后处理
    // if (f_BPM_br[0] > 0 && f_BPM_br2[0] == 0) // 保证最开始的前2个数据可靠性
    // {
    //     if (fabs(f_BPM_br[0] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
    //     {
    //         f_BPM_br[0] = MyToolMidValue(f_BPM_br, 5);
    //     }
    //     if (fabs(f_BPM_br[1] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
    //     {
    //         f_BPM_br[1] = MyToolMidValue(f_BPM_br, 5);
    //     }
    // }
    for (int i = 0; i < 4; i++) // 把前4个数据复制出来供后处理分析
    {
        f_BPM_br2[i] = f_BPM_br[i];
    }
    if (f_BPM_br[0] > 0) // 当数据满5个之后开始后处理
    {
        /*
        if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (3) ||
            f_BPM_br[4] - f_BPM_br[3] >= 2) // 比前4个中位数大3
        {
            count_out_range_br += 1;
            if (count_out_range_br > 1)
            {
                count_down_br = 0;
                count_down_br_m = 0;
                count_up_br = 0;
                count_up_br_m = 0;
            }
            if (count_out_range_br >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(3, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3];
            }
        }
        else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= (-10) ||
                 f_BPM_br[4] - f_BPM_br[3] <= (-8)) // 比前4个中位数小8
        {
            count_out_range_br += 1;
            if (count_out_range_br > 1)
            {
                count_down_br = 0;
                count_down_br_m = 0;
                count_up_br = 0;
                count_up_br_m = 0;
            }
            if (count_out_range_br >= 3)
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-3), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3];
            }
        }
        */
        // else
        if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (3) ||
            f_BPM_br[4] - f_BPM_br[3] >= 2) // 较大值判断
        {
            count_up_br++;
#if FFT_VERSION
            if (count_up_br >= 10)
#else
            if (count_up_br >= 8)
#endif
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(2.0f, (f_BPM_br[4] - f_BPM_br[3]));
            }
#if FFT_VERSION
            else if (count_up_br >= 6)
#else
            else if (count_up_br >= 4)
#endif
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(1.0f, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3]; // min_compare(0.2f, (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_down_br = 0;
            count_down_br_m = 0;
            count_out_range_br = 0;
        }
        else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= ((-3) || f_BPM_br[4] - f_BPM_br[3] <= (-2))) // 较小值判断
        {
            count_down_br++;
#if FFT_VERSION
            if (count_down_br >= 10)
#else
            if (count_down_br >= 8)
#endif
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-2.0f), (f_BPM_br[4] - f_BPM_br[3]));
            }
#if FFT_VERSION
            else if (count_down_br >= 6)
#else
            else if (count_down_br >= 4)
#endif
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3]; // + max_compare((-0.2f), (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_up_br = 0;
            count_up_br_m = 0;
            count_out_range_br = 0;
        }
        else if (f_BPM_br[4] - f_BPM_br[3] > 0)
        {
            count_up_br_m++;
#if FFT_VERSION
            if (count_up_br_m >= 3)
#else
            if (count_up_br_m >= 3)
#endif
            {
                f_BPM_br[4] = f_BPM_br[3] + min_compare(1, (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3]; // + min_compare(0.3f, (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_down_br = 0;
            count_down_br_m = 0;
            count_out_range_br = 0;
            count_up_br = 0;
        }
        else if (f_BPM_br[4] - f_BPM_br[3] < 0)
        {
            count_down_br_m++;
#if FFT_VERSION
            if (count_down_br_m >= 3)
#else
            if (count_down_br_m >= 3)
#endif
            {
                f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
            }
            else
            {
                f_BPM_br[4] = f_BPM_br[3]; // + max_compare((-0.3f), (f_BPM_br[4] - f_BPM_br[3]));
            }
            count_up_br = 0;
            count_up_br_m = 0;
            count_out_range_br = 0;
            count_down_br = 0;
        }
        if (f_BPM_br[4] < 7)
        {
            f_BPM_br[4] = 7;
        }
        else if (f_BPM_br[4] > 35)
        {
            f_BPM_br[4] = 35;
        }
        /**/
        BreathRate = round(f_BPM_br[4]);
        // rt_kprintf("----------------------------Frq: %.6f\r\n", freq_calculated(nf, 1));
        // rt_kprintf("br-i_max: %d   %d  %.6f\r\n", i_max_br, BreathRate, freq_calculated(nf, 1));
    }
    else
    {
        int Count = 0;
        float sum, avg = 0.0f;
        for (char i = 0; i < 5; i++)
        {
            // rt_kprintf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
            if ((f_BPM_br[i]) != 0)
            {
                // rt_kprintf("Rand Data :%d\r\n", (rand() % (5)));
                sum += (f_BPM_br[i]);
                Count++;
            }
        }
        avg = sum / Count;
        BreathRate = (uint8_t)(round(avg));
        if (BreathRate < 11)
        {
            BreathRate = 11;
        }
        else if (BreathRate > 15)
        {
            BreathRate = 15;
        }
    }
}

//------------------------------------------------------------------------------//
//----------------------------------FFT-----------------------------------------//
//---------------------------Cooley-Tukey算法-----------------------------------//
//------------------------------------------------------------------------------//
struct complex
{
    double real;
    double image;
};
struct complex complex_add(struct complex c1, struct complex c2);
struct complex complex_sub(struct complex c1, struct complex c2);
struct complex complex_multi(struct complex c1, struct complex c2);
struct complex rotation_factor(int N, int n, int k);

double mold_length(struct complex c);
// void fft(int len, struct complex in_x[],struct complex out_y[]);
void fft(int len, int len_array, struct complex in_x[], float out_y[len_array]);
#define N 2048
#define N1 600

struct complex
complex_add(struct complex c1, struct complex c2) // 复数加法
{
    struct complex p;
    p.real = c1.real + c2.real;
    p.image = c1.image + c2.image;
    return p;
}

struct complex complex_sub(struct complex c1, struct complex c2) // 复数减
{
    struct complex p;
    p.real = c1.real - c2.real;
    p.image = c1.image - c2.image;
    return p;
}

struct complex complex_multi(struct complex c1, struct complex c2) // 复数乘法
{
    struct complex c3;
    c3.real = c1.real * c2.real - c1.image * c2.image;
    c3.image = c2.real * c1.image + c1.real * c2.image;
    return c3;
}

struct complex rotation_factor(int Length, int n, int k) // 旋转因子
{
    struct complex w;
    w.real = cos(2 * PI * n * k / Length);
    w.image = -sin(2 * PI * n * k / Length);
    return w;
}

double mold_length(struct complex c) // 幅度
{
    return sqrt(c.real * c.real + c.image * c.image);
};

int reverse_num(int l, int oringin_num) // 反位序
{
    int q = 0, m = 0;

    for (int k = l - 1; k >= 0; k--)
    {
        q = oringin_num % 2;
        m += q * (1 << k);
        oringin_num = oringin_num / 2;
    }
    return m;
}

/**
 * @function: cooley_tukey_fft
 * @brief: FFT 计算
 * @param {int} len         计算长度
 * @param {int} len_array   数据长度
 * @param {complex} in_x    输入数据
 * @param {float} out_y     输出数据
 * @return {*}
 * @author: lzc
 */
void cooley_tukey_fft(int len, int len_array, struct complex in_x[], float out_y[(len / 2)])
{
    /*
        param len 序列长度，目前只能是2的指数
        param in_x输入的序列
        param out_y输出的序列
    */
    int l, k, r, z, dist, q, j; // l是级
    struct complex w, tmp;
    struct complex in_x_mem[len];

    l = log2(len);
    for (k = 0; k < len; k++)
    {
        in_x_mem[k] = in_x[reverse_num(l, k)]; // 反位序号操作
    }
    for (r = 0; r < l; r++) // 遍历每一级蝶形运算
    {
        dist = 1 << r;             // 提前计算每一级的间隔距离
        for (j = 0; j < dist; j++) // 计算策略是拆成上下两组，先上计算，后下计算，j是计算的起始序号
        {
            for (k = j; k < len; k += (dist << 1)) // 不好解释，得画图理解
            {
                q = k + dist;         // q同一组蝶形运算第二个序号
                z = k << (l - r - 1); // 确定旋转因子的指数
                w = rotation_factor(len, 1, z);
                // 由于不是并行计算，必须先缓存
                tmp = in_x_mem[k];
                in_x_mem[k] = complex_add(in_x_mem[k], complex_multi(w, in_x_mem[q]));
                in_x_mem[q] = complex_sub(tmp, complex_multi(w, in_x_mem[q]));
            }
        }
    }
    for (int ii = 0; ii < (len * 0.5); ii++)
    {
        out_y[ii] = mold_length(in_x_mem[ii]);
    }
    //    memcpy(out_y,in_x_mem,len*sizeof(struct complex));
}

/**
 * @function: myFFT_Output
 * @brief: None
 * @param {float32_t} *Input
 * @param {int} Length
 * @param {char} type
 * @return {*}
 * @author: lzc
 */
float32_t myFFT_Output(float32_t *Input, int Length, char type)
{
    float32_t res = 0;
    float32_t Freq = 0;
    // float32_t y[N1] = {0};
    float32_t *y = malloc(sizeof(float32_t) * 1024);

    uint32_t Freq_Index = 0;
    struct complex x[N] = {0}; // 初始化为0
    // printf("\n==========Start================\n");
    for (int i = 0; i < N; i++)
    {
        if (i >= 0 && i < Length)
        {
            x[i].real = Input[i];
            x[i].image = 0;
        }
        else
        {
            x[i].real = 0;
            x[i].image = 0;
        }
    }
    cooley_tukey_fft(N, Length, x, y);
    // for (int i = 0; i < N / 2; i++)
    //     printf("%f\r\n", y[i]);
    if (type == TYPE_HEART)
        Freq_Index = MyToolFindMaxIndex(y, (FFT_POINT / 2));
    else if (type == TYPE_BREATH)
        Freq_Index = MyToolFindMaxIndex(y, 33);
    Freq = ((float32_t)Freq_Index * 50.0f) / FFT_POINT;
    res = (Freq * 60);
    free(y);
    // printf("\n==========End================\n");
    return res;
}