#include "Poly2FindPeaks.h"

using namespace std;

vector<double> Poly2FindPeaks::SmoothData(const vector<double>& data, int window_size)
{
    if (window_size % 2 == 0)
        // 确保窗口大小为奇数
        window_size++;

    int n = data.size();
    int halfWin = window_size / 2;
    vector<double> smoothed(n, 0.0);

    for (int i = 0; i < n; ++i) {
        int start = max(0, i - halfWin);
        int end = min(n - 1, i + halfWin);
        int count = end - start + 1;
        double sum = 0.0;
        for (int j = start; j <= end; ++j) {
            sum += data[j];
        }
        smoothed[i] = sum / count;
    }
    return smoothed;
}

// 计算基线（使用百分位法，假设大部分数据为基线）
double Poly2FindPeaks::calculateBaseline(const vector<double>& data, double percentile)
{
    vector<double> sorted_data = data;
    sort(sorted_data.begin(), sorted_data.end());
    int idx = static_cast<int>(sorted_data.size() * percentile);
    return sorted_data[idx];
}

vector<Poly2FindPeaks::Peak> Poly2FindPeaks::findPeaks(const vector<double>& data, double height_threshold, int min_distance, int smooth_window)
{
    vector<Peak> peaks;
    int n = data.size();
    if (n < 3)
        // 数据点太少，无法检测峰值
        return peaks;

    // 1. 预处理：平滑数据
    vector<double> smoothed = SmoothData(data, smooth_window);
    // 2. 计算基线
    double baseline = calculateBaseline(smoothed);
    // 3. 初步检测局部最大值（峰值候选）
    vector<int> candidates;
    for (int i = 1; i < n - 1; ++i) {
        // 判断是否为局部最大值
        if (smoothed[i] > smoothed[i - 1] && smoothed[i] > smoothed[i + 1]) {
            // 高度超过阈值（相对于基线）
            double height = smoothed[i] - baseline;
            if (height > height_threshold) {
                candidates.push_back(i);
            }
        }
    }
    // 4. 过滤近距离峰（保留较高的峰）
    vector<int> filtered;
    for (int cand : candidates) {
        bool keep = true;
        // 检查与已保留峰的距离
        for (int peakIdx : filtered) {
            if (abs(cand - peakIdx) < min_distance) {
                // 保留高度更高的峰
                if (smoothed[cand] <= smoothed[peakIdx]) {
                    keep = false;
                    break;
                } else {
                    // 移除较低的峰
                    auto it = find(filtered.begin(), filtered.end(), peakIdx);
                    if (it != filtered.end()) {
                        filtered.erase(it);
                    }
                }
            }
        }
        if (keep) {
            filtered.push_back(cand);
        }
    }
    // 5. 提取峰值特征
    for (int idx : filtered) {
        Peak peak;
        peak.index = idx;
        peak.amplitude = smoothed[idx];
        peak.height = peak.amplitude - baseline;
        // 计算精确位置（抛物线插值）
        if (idx > 0 && idx < n - 1) {
            double y0 = smoothed[idx - 1];
            double y1 = smoothed[idx];
            double y2 = smoothed[idx + 1];
            // 抛物线拟合：y = ax² + bx + c，顶点在 x = -b/(2a)
            double a = (y0 + y2 - 2 * y1) / 2;
            double b = (y2 - y0) / 2;
            peak.position = idx - b / (2 * a); // 精确位置（可能非整数）
        } else {
            peak.position = idx; // 边缘点无法插值，直接用索引
        }
        // 计算半高宽FWHM
        double halfHeight = baseline + peak.height / 2;
        int left = idx, right = idx;
        // 向左找半高处
        while (left > 0 && smoothed[left] > halfHeight) {
            left--;
        }
        // 向右找半高处
        while (right < n - 1 && smoothed[right] > halfHeight) {
            right++;
        }
        // 线性插值计算半高处的精确位置
        double leftPos = left;
        if (left < idx) {
            double dy = smoothed[left + 1] - smoothed[left];
            if (dy != 0) {
                leftPos += (halfHeight - smoothed[left]) / dy;
            }
        }
        double rightPos = right;
        if (right > idx) {
            double dy = smoothed[right] - smoothed[right - 1];
            if (dy != 0) {
                rightPos = right - 1 + (halfHeight - smoothed[right - 1]) / dy;
            }
        }
        peak.fwhm = rightPos - leftPos;
        peaks.push_back(peak);
    }
    // 按位置排序
    sort(peaks.begin(), peaks.end(), [](const Peak& a, const Peak& b) {
        return a.position < b.position;
    });

    return peaks;
}