#include "find_peaks.h"
#include "MyTool.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;
}

//
float compute_target_acf(const float *data, int lag, float mean, float total_energy)
{
    const int N = DATA_LENGTH - lag;
    if (N <= 0 || total_energy < 1e-6f)
        return 0.0f;

    float sum_product = 0.0f;
    for (int i = 0; i < N; i++)
    {
        float x = data[i] - mean;
        float y = data[i + lag] - mean;
        sum_product += x * y;
    }
    return sum_product / total_energy;
}
// 快速选择心率
int fast_select_hr(const float candidates[4], const float *data, float *fft_energy_ratios)
{
    float32_t std = 0.0f;
    float32_t avg = 0.0f;
    avg = MyToolAvgValue((float32_t *)data, DATA_LENGTH);
    std = MyToolStdValue((float32_t *)data, DATA_LENGTH);
    // 预处理计算均值和总能量
    float parsed_data[DATA_LENGTH];
    // memset(parsed_data, data, sizeof(float) * DATA_LENGTH);
    float mean = 0.0f, total_energy = 0.0f;
    for (int i = 0; i < DATA_LENGTH; i++)
    {
        parsed_data[i] = (data[i] - avg) / std;
        mean += parsed_data[i];
        total_energy += parsed_data[i] * parsed_data[i];
    }
    mean /= DATA_LENGTH;
    total_energy -= DATA_LENGTH * mean * mean; // 修正总能量
    // 遍历候选心率
    float max_score = -1.0f;
    int selected_idx = -1;
    float temp = 0.0f;
    for (int i = 0; i < 4; i++)
    {
        const float hr = (i < 2) ? candidates[i] / 2 : candidates[i];
        // 转换候选心率为lag值
        int target_lag = (int)(50 * 60.0f / hr + 0.5f);
        if (target_lag <= 0 || target_lag >= DATA_LENGTH / 2)
            continue;
        // 局部窗口ACF峰值检测
        float acf_peak = -1.0f;
        int search_radius = 1;
        for (int offset = 0; offset <= search_radius; offset++)
        {
            int current_lag = target_lag + offset;
            if (current_lag < 1 || current_lag >= DATA_LENGTH - 1)
                continue;
            float acf_val = compute_target_acf(parsed_data, current_lag, mean, total_energy);
            acf_peak = fmax(acf_peak, acf_val);
        }
        // 综合评分（网页6的加权策略）
        float score = 0.7f * acf_peak + 0.3f * fft_energy_ratios[i];
        if (score > max_score)
        {
            max_score = score;
            selected_idx = i;
        }
    }
    return selected_idx;
}