#include "FindPeaksBySvd.h"

using namespace arma;

std::vector<FindPeaksBySvd::PeakInfo> FindPeaksBySvd::FindPeaks(const arma::mat &spec_data, int step_w) throw(std::string)
{
    std::vector<FindPeaksBySvd::PeakInfo> peak_info_vec;
    try {
        const arma::vec& spec_data_col0 = spec_data.col(0);
        const arma::vec& spec_data_col1 = spec_data.col(1);
        int m = spec_data_col1.n_elem; // 能谱长度
        int n = (step_w == -1) ? 7 : step_w; // 窗口大小（默认7）
        int ns = ceil((n - 1) / 2.0); // 窗口半宽

        // 初始化变量
        vec kosin(m, fill::zeros);
        vec Smax(m, fill::zeros);
        vec Smin(m, fill::zeros);
        int k = 0; // 索引计数器（对应Matlab的k）

        // 滑动窗口遍历+SVD分析
        for (int i = 0; i < m; i++) { // Armadillo默认0索引，Matlab是1索引，需对应转换
            k++;
            vec vec2; // 局部窗口数据
            // 边界处理：左边界
            if (k <= ns) {
                vec2 = spec_data_col1.subvec(0, n - 1);
            }
            // 右边界
            else if (k >= m - ns) { // Matlab: k>=m-ns-1，因索引差异调整
                vec2 = spec_data_col1.subvec(m - n, m - 1);
            }
            // 中间区域：以i为中心的n点窗口
            else {
                int start = i - floor((n - 1) / 2.0);
                int end = i + ceil((n - 1) / 2.0);
                vec2 = spec_data_col1.subvec(start, end);
            }
            // 构造矩阵M（2列：基线参考+局部数据）
            vec vec1(n, fill::ones);
            double min_val = vec2.min();
            mat M(n, 2);
            M.col(0) = min_val * vec1;
            M.col(1) = vec2;
            // SVD分解（Armadillo svd返回U,S,V^T，与Matlab一致）
            mat U, V;
            vec S;
            svd(U, S, V, M); // S为奇异值向量（降序排列）
            // 存储最大/最小奇异值
            Smax(i) = S(0);
            Smin(i) = S(1);
            // 计算余弦相似度（kosin）
            double mean_vec2 = mean(vec2);
            vec mean_vec1 = mean_vec2 * vec1;
            double dot_prod = dot(vec2, mean_vec1);
            double norm_vec2 = norm(vec2);
            double norm_mean1 = norm(mean_vec1);
            kosin(i) = dot_prod / (norm_vec2 * norm_mean1 + 1e-8); // 加小值避免除零
        }
        // 计算峰识别指标DistV
        vec sin_kosin = sqrt(1 - pow(kosin, 2));
        vec DistV1 = sin_kosin % Smax;
        vec DistV2 = sin_kosin % Smin;
        double mean_DistV2 = mean(DistV2);
        vec DistV = DistV2 - 3 * sqrt(mean_DistV2);
        // 峰边界识别与参数提取
        int Lc = 0, Rc = 1;
        int nos = DistV.n_elem;
        for (int ch = 0; ch < nos - 1; ch++) { // 遍历
            if (DistV(ch) < 0 && DistV(ch + 1) >= 0) {
                // 左边界：DistV从负→正
                Lc = ch;
            } else if (DistV(ch) >= 0 && DistV(ch + 1) <= 0) {
                // 右边界：DistV从正→负
                Rc = ch;
            } else {
                continue;
            }
            // 计算峰参数
            if (Rc > Lc) {
                int wop = abs(Rc - Lc);
                if (wop <= 0)
                    continue; // 无效宽度跳过
                // 提取峰区域数据
                vec peak_region = spec_data_col1.subvec(Lc, Rc);
                double height = peak_region.max();
                uword HWP_idx = peak_region.index_max(); // 峰值位置
                // 计算半高宽(FWHM)
                double half_height = height / 2.0;
                int left_half = 0, right_half = peak_region.n_elem - 1;
                // 找到左侧半高位置
                for (int i = HWP_idx; i >= 0; i--) {
                    if (peak_region(i) <= half_height) {
                        left_half = i;
                        break;
                    }
                }
                // 找到右侧半高位置
                for (int i = HWP_idx; i < peak_region.n_elem; i++) {
                    if (peak_region(i) <= half_height) {
                        right_half = i;
                        break;
                    }
                }
                // 计算半高宽（转换为原始坐标）
                double fwhm = (right_half - left_half + 1);
                // 计算峰面积（使用梯形法则）
                double area = 0.0;
                for (int i = 0; i < peak_region.n_elem - 1; i++) {
                    // 梯形面积 = (上底 + 下底) * 高 / 2
                    area += (peak_region(i) + peak_region(i + 1)) / 2.0;
                }
                // 峰中心
                int pl = round((Lc + Rc) / 2.0);
                // 筛选有效峰：峰宽>2且中心DistV>0
                // 修改20251126: 去除峰宽>2及“wop > 2”的判断条件
                if (DistV(pl - 1) > 0) { // pl-1转换为0索引
                    int left = Lc;
                    int width = wop + 1;
                    int pos = Lc + HWP_idx;
                    PeakInfo peak_info;
                    peak_info.left = spec_data_col0.at(left);
                    peak_info.width = spec_data_col0.at(width);
                    peak_info.height = height;
                    peak_info.pos = spec_data_col0.at(pos);
                    peak_info.fwhm = fwhm;
                    peak_info.area = area;
                    peak_info_vec.push_back(peak_info);
                }
            }
        }
    }
    catch(const std::exception& e) {
        throw std::string("Find Peaks Exception, ") + std::string(e.what());
    }
    catch (...) {
        throw std::string("Find peaks unkonow exception");
    }
    return peak_info_vec;
}