Job_SignsPads/STM32/Code/STM32F405/Application/MyTool.c

/*
 * @Description:
 * @Version: 1.0
 * @Author: lzc
 * @Date: 2023-07-21 15:42:16
 * @LastEditors: lzc
 * @LastEditTime: 2024-12-26 17:07:56
 */

#include "MyTool.h"

/**
 * @function: MyToolFindMaxIndex
 * @brief 获取最大值索引
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {uint32_t} Index 返回最大值的地址
 * @author: lzc
 */
uint32_t MyToolFindMaxIndex(float32_t *Input, uint32_t Length)
{
    uint32_t Index = 0;
    float32_t MaxValue = 0.0f;
    arm_max_f32(Input, Length, &MaxValue, &Index);
    return Index;
}
/**
 * @function: MyToolFindMaxValue
 * @brief 获取最大值
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {float32_t} MaxValue 返回最大值
 * @author: lzc
 */
float32_t MyToolFindMaxValue(float32_t *Input, uint32_t Length)
{
    uint32_t Index = 0;
    float32_t MaxValue = 0.0f;
    arm_max_f32(Input, Length, &MaxValue, &Index);
    return MaxValue;
}
/**
 * @function: MyToolFindMinValue
 * @brief 获取最小值
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {float32_t} MaxValue 返回最小值
 * @author: lzc
 */
float32_t MyToolFindMinValue(float32_t *Input, uint32_t Length)
{
    uint32_t Index = 0;
    float32_t MinValue = 0.0f;
    arm_min_f32(Input, Length, &MinValue, &Index);
    return MinValue;
}
/**
 * @function: MyToolStdValue
 * @brief 标准差
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {*}
 * @author: lzc
 */
float32_t MyToolStdValue(float32_t *Input, uint32_t Length)
{
    float32_t Value = 0.0f;
    arm_std_f32(Input, Length, &Value);
    return Value;
}

/**
 * @function: MyToolAvgValue
 * @brief 平均值
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {*}
 * @author: lzc
 */
float32_t MyToolAvgValue(float32_t *Input, uint32_t Length)
{
    float32_t Value = 0.0f;
    arm_mean_f32(Input, Length, &Value);
    return Value;
}

/**
 * @function: MyToolSumValue
 * @brief 求和
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {*}
 * @author: lzc
 */
float32_t MyToolSumValue(float32_t *Input, uint32_t Length)
{
    float32_t Temp = 0.0f;
    float32_t SumValue = 0.0f;
    for (uint32_t i = 0; i < Length; i++)
    {
        arm_add_f32(&Input[i], &Temp, &SumValue, 1);
        Temp = SumValue;
    }
    return SumValue;
}
// TODO: 此处进行排序
/*
降序 ————选择排序
*/
char sort(float a[], int n)
{
    float temp;
    int i, j, index;
    for (i = 0; i < n - 1; i++)
    {
        index = i;
        for (j = i + 1; j < n; j++)
        {                        // 寻找最小值
            if (a[index] < a[j]) // a[i]若不是最小
                index = j;       // 记录最小值下标
        }
        if (index != i) // 最小值和第i个记录进行互换
        {
            temp = a[i];
            a[i] = a[index];
            a[index] = temp; // a[i]的值赋给被交换的位置
        }
    }
    return 0;
}
/**
 * @function: MyToolSumValue
 * @brief 排序
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {*}
 * @author: lzc
 */
void MyToolSortValue(float32_t *Input, uint32_t Length)
{
    float32_t MyOutput[Length];
    for (int i = 0; i < Length; i++)
        MyOutput[i] = Input[i];
    sort(MyOutput, Length);
    for (int i = 0; i < Length; i++)
    {
        Input[i] = MyOutput[i];
        // printf("MyOutput %d :%f \r\n", i, MyOutput[i]);
    }
}

/**
 * @function: MyToolSumValue
 * @brief 排序
 * @param {float32_t} *Input 浮点数组地址
 * @param {uint32_t} Length 长度
 * @return {*}
 * @author: lzc
 */
float32_t MyToolMidValue(float32_t *Input, uint32_t Length)
{
    int MidPos = 0;
    float32_t MyOutput[Length];
    for (int i = 0; i < Length; i++)
        MyOutput[i] = Input[i];
    sort(MyOutput, Length);
    MidPos = Length % 2;
    if (MidPos == 1) // 奇数
    {
        MidPos = (Length / 2);
        return MyOutput[MidPos];
    }
    else if (MidPos == 0) // 偶数
    {
        MidPos = (Length / 2);
        return (MyOutput[MidPos] + MyOutput[MidPos + 1]) / 2;
    }
    return 0;
}

/**
 * @function: SetRandom
 * @brief: None
 * @return {*}
 * @author: lzc
 */
void MySetRandom(uint16_t value)
{
    uint8_t temp[12];
    uint32_t temp0, temp1, temp2;
    temp0 = *(__IO uint32_t *)(ID_ADDR); // 产品唯一身份标识寄存器（96位）
    temp1 = *(__IO uint32_t *)(ID_ADDR + 4);
    temp2 = *(__IO uint32_t *)(ID_ADDR + 8);
    temp[0] = (uint8_t)(temp0 & 0x000000FF);
    temp[1] = (uint8_t)((temp0 & 0x0000FF00) >> 8);
    temp[2] = (uint8_t)((temp0 & 0x00FF0000) >> 16);
    temp[3] = (uint8_t)((temp0 & 0xFF000000) >> 24);
    temp[4] = (uint8_t)(temp1 & 0x000000FF);
    temp[5] = (uint8_t)((temp1 & 0x0000FF00) >> 8);
    temp[6] = (uint8_t)((temp1 & 0x00FF0000) >> 16);
    temp[7] = (uint8_t)((temp1 & 0xFF000000) >> 24);
    temp[8] = (uint8_t)(temp2 & 0x000000FF);
    temp[9] = (uint8_t)((temp2 & 0x0000FF00) >> 8);
    temp[10] = (uint8_t)((temp2 & 0x00FF0000) >> 16);
    temp[11] = (uint8_t)((temp2 & 0xFF000000) >> 24);
    srand((unsigned)temp + value);
}
/**
 * @function:
 * @brief: None
 * @return {*}
 * @author: lzc
 */
static uint8_t MyBreathRate;
static uint8_t MyHeartBeatRate;
extern float KalmarFilter(float inData);
int MyRadomHeart(void)
{
    MyHeartBeatRate = (int)(((float)(rand() % (6) + 60)) + 2);
    return (MyHeartBeatRate);
}

/**
 * @function:
 * @brief: None
 * @return {*}
 * @author: lzc
 */
int MyRadomBreath(void)
{
    MyBreathRate = (int)(((float)(rand() % (6) + 10)) + 2);
    return (MyBreathRate);
}
/**
 * @function:move_update
 * @brief: None
 * @param {float} arr
 * @param {int} len
 * @param {float} new_f
 * @return {*}
 * @author: lzc
 */
void MyToolMoveUpdate(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;
}

// 比较函数，用于降序排序
int compare(const void *a, const void *b)
{
    float diff = *(float *)b - *(float *)a;
    return (diff > 0) ? 1 : (diff < 0) ? -1
                                       : 0;
}

// 寻找峰值元素的函数
int findPeaks(float *nums, int numsSize, int **peakIndices, float **peakValues)
{
    int count = 0;                                           // 峰值的数量
    *peakIndices = (int *)malloc(numsSize * sizeof(int));    // 分配峰值索引数组
    *peakValues = (float *)malloc(numsSize * sizeof(float)); // 分配峰值数组

    // 检查中间的元素
    for (int i = 1; i < numsSize - 1; i++)
    {
        if (nums[i] > nums[i - 1] && nums[i] > nums[i + 1])
        {
            (*peakIndices)[count] = i;
            (*peakValues)[count] = nums[i];
            count++;
        }
    }
    return count; // 返回峰值的数量
}

int peakProcess(float *nums, int numsSize, int *max_peak_index, float *max_peak_value)
{
    int *peakIndices = NULL;  // 存储峰值索引的数组
    float *peakValues = NULL; // 存储峰值的数组
    int peakCount = 0;        // 峰值的数量
    int value_peak_count = 0;

    // 寻找所有峰值元素
    peakCount = findPeaks(nums, numsSize, &peakIndices, &peakValues);

    // 如果有峰值，按照峰值的大小降序排列索引
    if (peakCount > 0)
    {
        qsort(peakValues, peakCount, sizeof(float), compare);
        // 根据排序后的峰值数组，重新排列峰值索引
        for (int i = 0; i < peakCount; i++)
        {
            if (value_peak_count >= 2)
                break;

            for (int j = 0; j < peakCount; j++)
            {
                if (value_peak_count >= 2)
                    break;
                if (peakValues[i] == nums[peakIndices[j]])
                {
                    // log_i("Index: %d  Value :%f\n", peakIndices[j], peakValues[i]);
                    if (max_peak_index[0] == 0)
                    {
                        max_peak_index[0] = peakIndices[j];
                        max_peak_value[0] = peakValues[i];
                        value_peak_count = value_peak_count + 1;
                    }
                    if ((max_peak_index[0] > 0) && (max_peak_index[1] == 0) && (abs(peakIndices[j] - max_peak_index[0]) > 3))
                    {
                        max_peak_index[1] = peakIndices[j];
                        max_peak_value[1] = peakValues[i];
                        value_peak_count = value_peak_count + 1;
                        break;
                    }

                    peakIndices[j] = -1; // 标记已打印的索引

                    break;
                }
            }
        }
    }
    // 释放内存
    free(peakIndices);
    free(peakValues);
    return value_peak_count;
}

// 查找最接近元素的函数
float find_closest_element(const float arr[], int length, int target)
{
    if (target == 0)
        return arr[1];

    float half_arr0 = arr[0] / 2.0f;

    // 初始化为第一个元素
    float closest = half_arr0;
    float min_diff = fabs(half_arr0 - target); // 初始最小差值
    for (int i = 1; i < length; i++)
    {
        float val = (i < 1) ? (arr[i] / 2.0f) : arr[i]; // 仅前两个元素除以2
        float current_diff = fabs(val - target);
        // 如果找到更小的差值，更新最接近元素
        if (current_diff < min_diff)
        {
            min_diff = current_diff;
            closest = val;
        }
    }
    return closest;
}
int MyToolSelectHp(float *hpList, float *ratioList, float *formerHps, uint16_t time_result)
{
    int res = 0;

    float highMax = hpList[0];
    float highSecond = hpList[1];
    float loMax = hpList[2];
    float loSecond = hpList[3];

    float highFilterRatio = ratioList[0];
    float lowFilterRatio = ratioList[1];

    float noZeroSum = 0.0f;
    int nearest_value = 0;
    int noZeroAvg = 0;
    int noZeroCount = 0;

    for (int i = 0; i < 15; i++)
    {
        if (formerHps[i] != 0.0f)
        {
            noZeroCount++;
            noZeroSum += formerHps[i];
        }
    }

    if (noZeroCount > 0)
        noZeroAvg = (int)round(noZeroSum / noZeroCount);

    if (fabs(loMax * 2.0f - highMax) <= 5 && loMax >= 45 && loMax <= 120) // 倍频明显的直接返回
    {
        res = (int)round(loMax);
        return res;
    }

    if (fabs(loSecond * 2.0f - highMax) <= 4 && loSecond >= 60 && loSecond <= 105) // 倍频明显的直接返回
    {
        res = (int)round(loSecond);
        return res;
    }

    if (fabs(loMax * 2.0f - highSecond) < 5 && loMax >= 55 && loMax <= 105 && highSecond <= 190) // 倍频与本频很接近的直接返回
    {
        res = (int)round(loMax);
        return res;
    }

    if (fabs(loMax - highMax) < 5 && loMax >= 55 && loMax <= 120 && highMax <= 120 && noZeroCount > 10) // 倍频峰和本峰差不多
    {
        if (fabs(loMax - noZeroAvg) < 4)
        {
            res = (int)round(loMax);
            return res;
        }
        else if (fabs(highMax / 2.0f - noZeroAvg) < 4 && highMax > 120)
        {
            res = (int)round(highMax / 2.0f);
            return res;
        }
    }

    if (fabs(loSecond - highMax) < 4 && loSecond >= 55 && loSecond <= 105 && highMax <= 110 && noZeroCount > 10)
    {
        if (fabs(loSecond - noZeroAvg) < 4)
        {
            res = (int)round(loSecond);
            return res;
        }
        else if (fabs(loSecond / 2.0f - noZeroAvg) < 4 && loSecond > 120)
        {
            res = (int)round(loSecond / 2.0f);
            return res;
        }
    }

    if (noZeroCount > 10 && fabs(loMax - noZeroAvg) < 4 && loMax >= 50 && loMax <= 105) // 与前15s的均值相近 直接输出
    {
        res = (int)round(loMax);
        return res;
    }

    if (noZeroCount > 10 && fabs(noZeroAvg * 2.0f - highMax) < 4 && highMax > 100) // 与前15s的均值相近 直接输出
    {
        res = (int)round(highMax / 2.0f);
        return res;
    }

    if (noZeroCount > 10 && fabs(noZeroAvg - loSecond) < 3 && loSecond >= 50 && loSecond <= 105 && noZeroAvg >= 60) // 与前15s的均值相近 直接输出
    {
        res = (int)round(loSecond);
        return res;
    }

    if (lowFilterRatio <= 0.5f && lowFilterRatio <= highFilterRatio && loMax >= 45 && loMax <= 100)
    {
        res = (int)round(loMax);
        return res;
    }

    if (highFilterRatio <= 0.5f && highFilterRatio <= lowFilterRatio && highMax >= 120 && highMax <= 200)
    {
        res = (int)round(highMax / 2.0f);
        return res;
    }

    if (highFilterRatio <= 0.5f && highFilterRatio <= lowFilterRatio && highMax >= 45 && highMax <= 95)
    {
        res = (int)round(highMax);
    }

    else if (fabs(loSecond - highMax) < 5 && highMax >= 120 && highMax <= 210)
    {
        res = (int)round(highMax / 2.0f);
    }

    else if (fabs(loSecond - highMax) < 5 && loSecond >= 60 && loSecond <= 90)
    {
        res = (int)round(loSecond);
    }

    else if (loMax >= 55 && loMax <= 95 && highFilterRatio > lowFilterRatio)
    {
        res = (int)round(loMax);
    }

    else if (highMax > 110)
    {
        res = (int)round(highMax / 2.0f);
    }

    else if (time_result >= 60 && time_result <= 110 && noZeroCount > 10 && fabs(time_result - noZeroAvg) < 3)
    {
        res = time_result;
    }

    else
    {
        res = (int)round(loMax);
        if (noZeroCount > 10 && fabs(noZeroAvg - res) > 20)
            res = noZeroAvg;
        if (res < 48)
            res = noZeroAvg;
    }

    if (res == 0)
    {
        if (time_result >= 55 && time_result <= 110)
        {
            res = time_result;
        }
        else
        {
            res = (int)round(loMax);
        }
    }
    return res;
}