#include "find_peaks.h"

void compute_dynamic_threshold_optimized(const float *data, float *thresholds)
{
    float buffer[WINDOW_SIZE]; // 循环缓冲区
    int buf_idx = 0;           // 缓冲区当前写入位置
    float window_sum = 0.0f;

    // 第一阶段：初始化缓冲区
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        float energy = data[i] * data[i];
        buffer[buf_idx] = energy;
        window_sum += energy;
        buf_idx = (buf_idx + 1) % WINDOW_SIZE;
    }

    // 第二阶段：计算首个有效阈值
    float first_threshold = DYNAMIC_THRESH_RATIO * sqrtf(window_sum / WINDOW_SIZE);

    // 填充前WINDOW_SIZE个阈值（与原算法行为一致）
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        thresholds[i] = first_threshold;
    }

    // 第三阶段：滑动窗口处理后续数据
    for (int i = WINDOW_SIZE; i < DATA_LENGTH; i++)
    {
        float new_energy = data[i] * data[i];
        float old_energy = buffer[buf_idx]; // 获取最早的能量值

        window_sum += new_energy - old_energy; // 更新滑动总和
        buffer[buf_idx] = new_energy;          // 更新缓冲区
        buf_idx = (buf_idx + 1) % WINDOW_SIZE; // 移动循环指针

        // 计算动态阈值
        thresholds[i] = DYNAMIC_THRESH_RATIO * sqrtf(window_sum / WINDOW_SIZE);
    }
}

// 滑动平均预处理
void moving_average(float *data)
{
    float buffer[MOVING_AVG_WINDOW] = {0.0f};
    float sum = 0.0f;

    // 初始化窗口
    for (int i = 0; i < MOVING_AVG_WINDOW; i++)
    {
        buffer[i] = data[i];
        sum += data[i];
    }

    // 滑动处理
    for (int i = MOVING_AVG_WINDOW; i < DATA_LENGTH; i++)
    {
        sum = sum - buffer[i % MOVING_AVG_WINDOW] + data[i];
        buffer[i % MOVING_AVG_WINDOW] = data[i];
        data[i - MOVING_AVG_WINDOW / 2] = sum / MOVING_AVG_WINDOW;
    }
}

PeakResult find_peaks(const float *input)
{
    PeakResult result_struct = {
        .peaks = {0},         // 初始化peaks数组全为0
        .thresholds = {0.0f}, // 初始化thresholds数组全为0.0f
        .count = 0}; // 初始化count为0

    // memset(&result_struct,0,sizeof(result_struct));
    float data[DATA_LENGTH] = {0.0f};
    int i = 0, j = 0;

    // 拷贝并预处理数据
    memcpy(data, input, sizeof(float) * DATA_LENGTH);
    moving_average(data);

    // 第一阶段：候选波峰检测
    int candidates[MAX_CANDIDATES] = {0};
    int candidate_count = 0;
    float prev_diff = 0.0f;

    for (i = 1; i < DATA_LENGTH; i++)
    {
        float diff = data[i] - data[i - 1];
        if (prev_diff > 0 && diff < 0)
        { // 斜率由正转负
            candidates[candidate_count++] = i - 1;
        }
        prev_diff = diff;
    }

    // 第二阶段：动态阈值计算
    compute_dynamic_threshold_optimized(data, result_struct.thresholds); // 动态阈值

    // 第三阶段：峰值筛选
    int last_peak = -MIN_PEAK_DISTANCE;
    for (j = 0; j < candidate_count; j++)
    {
        const int idx = candidates[j];
        if (data[idx] > result_struct.thresholds[idx] &&
            (idx - last_peak) >= MIN_PEAK_DISTANCE)
        {
            result_struct.peaks[result_struct.count++] = idx;
            last_peak = idx;
        }
        if (result_struct.count >= sizeof(result_struct.peaks) / sizeof(int))
            break;
    }

    return result_struct;
}

int calculate_heart_rate(const PeakResult *result)
{
    const int sample_rate = SAMPLE_RATE; // 采样率50Hz
    float avg_interval = 0.0f;

    if (result->count < 2)
    {
        return -1; // 波峰不足无法计算心率
    }

    // 计算相邻波峰的平均间隔（网页1和网页7的结合）
    int total_intervals = 0;
    for (int i = 1; i < result->count; i++)
    {
        int interval = result->peaks[i] - result->peaks[i - 1]; //  间隔的点数

        if (interval >= 30 && interval <= 116)
        {
            total_intervals += interval;
        }
        else
        {
            return -1;
        }
    }

    // 计算平均间隔时间（秒）
    avg_interval = total_intervals / (float)(result->count - 1) / sample_rate;
    int hp_output = 0;
    hp_output = (int)round(60.0f / avg_interval); // 60秒 / 平均间隔秒数
    return hp_output;
}

int time_domain_heart_rate(float *hp_data)
{
    PeakResult time_result = find_peaks(hp_data);

    int hr = calculate_heart_rate(&time_result);
    if (hr < 30 || hr > 150)
    {              // 生理范围验证
        return -2; // 异常心率代码
    }
    return hr;
}