#include "signal_processor.h"
#include <iostream>
#include <algorithm>
#include <cmath>
#include <numeric>

// 自定义clamp函数，替代std::clamp（C++17特性）
template<typename T>
T clamp(T value, T min_val, T max_val) {
    if (value < min_val) return min_val;
    if (value > max_val) return max_val;
    return value;
}

// 新增：简化的通道级滤波处理（测试版本）
std::vector<SensorData> SignalProcessor::process_channel_based_filtering_simple(
    const std::vector<SensorData>& data_packets) {
    
    std::cout << "=== 开始简化版通道级滤波处理 ===" << std::endl;
    
    if (data_packets.empty()) {
        std::cout << "输入数据包为空，返回空结果" << std::endl;
        return {};
    }
    
    std::cout << "输入数据包数量: " << data_packets.size() << std::endl;
    
    // 按数据类型分组
    std::map<DataType, std::vector<SensorData>> grouped_data;
    for (const auto& packet : data_packets) {
        grouped_data[packet.data_type].push_back(packet);
    }
    
    std::cout << "按数据类型分组完成，共 " << grouped_data.size() << " 种类型" << std::endl;
    
    std::vector<SensorData> processed_packets;
    
    // 对每种数据类型分别处理
    for (auto& [data_type, packets] : grouped_data) {
        if (packets.empty()) continue;
        
        std::cout << "处理数据类型: " << static_cast<int>(data_type) << "，数据包数量: " << packets.size() << std::endl;
        
        // 获取第一个数据包作为模板
        SensorData template_packet = packets[0];
        
        // 检查通道数据类型
        if (!std::holds_alternative<std::vector<std::vector<float>>>(packets[0].channel_data)) {
            std::cout << "警告: 数据类型 " << static_cast<int>(data_type) << " 不是多通道格式，跳过" << std::endl;
            continue;
        }
        
        // 获取通道信息
        auto& first_channels = std::get<std::vector<std::vector<float>>>(packets[0].channel_data);
        size_t num_channels = first_channels.size();
        size_t samples_per_packet = first_channels.empty() ? 0 : first_channels[0].size();
        
        std::cout << "通道数量: " << num_channels << ", 每包采样点数: " << samples_per_packet << std::endl;
        
        if (num_channels == 0 || samples_per_packet == 0) {
            std::cout << "警告: 通道数据无效，跳过此类型" << std::endl;
            continue;
        }
        
        // 计算总采样点数
        size_t total_samples = samples_per_packet * packets.size();
        std::cout << "总采样点数: " << total_samples << std::endl;
        
        // 创建合并后的数据包
        SensorData merged_packet = template_packet;
        merged_packet.packet_sn = 0; // 合并后的包序号为0
        
        // 创建合并后的通道数据
        auto& merged_channels = merged_packet.channel_data.emplace<std::vector<std::vector<float>>>();
        merged_channels.resize(num_channels);
        
        // 合并所有数据包的通道数据
        for (size_t ch = 0; ch < num_channels; ch++) {
            merged_channels[ch].reserve(total_samples);
            
            for (const auto& packet : packets) {
                if (auto* channels = std::get_if<std::vector<std::vector<float>>>(&packet.channel_data)) {
                    if (ch < channels->size() && !(*channels)[ch].empty()) {
                        merged_channels[ch].insert(merged_channels[ch].end(),
                                                  (*channels)[ch].begin(),
                                                  (*channels)[ch].end());
                    }
                }
            }
            
            std::cout << "通道 " << ch << " 合并完成，采样点数: " << merged_channels[ch].size() << std::endl;
        }
        
        // 对合并后的数据进行基本信号处理
        std::cout << "开始基本信号处理..." << std::endl;
        for (size_t ch = 0; ch < num_channels; ch++) {
            if (merged_channels[ch].empty()) continue;
            
            try {
                // 根据数据类型应用不同的基本处理
                switch (data_type) {
                    case DataType::ECG_2LEAD:
                        // ECG 2导联基本处理：去除直流分量
                        merged_channels[ch] = remove_dc_offset(merged_channels[ch]);
                        break;
                    case DataType::ECG_12LEAD:
                        // ECG 12导联专业滤波处理
                        std::cout << "通道 " << ch << " 开始12导联心电专业滤波..." << std::endl;
                        
                        // 1. 去除直流分量
                        merged_channels[ch] = remove_dc_offset(merged_channels[ch]);
                        std::cout << "   - 直流分量去除完成" << std::endl;
                        
                        // 2. 0.1Hz高通滤波（去除基线漂移，比0.5Hz更温和）
                        merged_channels[ch] = filter(merged_channels[ch], 250.0, 0, 0.1, filtertype::highpass);
                        std::cout << "   - 0.1Hz高通滤波完成" << std::endl;
                        
                        // 3. 50Hz陷波滤波（去除工频干扰，带宽1.0Hz更精确）
                        merged_channels[ch] = filter(merged_channels[ch], 250.0, 49.5, 50.5, filtertype::notchpass);
                        std::cout << "   - 50Hz陷波滤波完成" << std::endl;
                        
                        std::cout << "通道 " << ch << " 12导联心电滤波处理完成" << std::endl;
                        break;
                    case DataType::EEG:
                        // EEG基本处理：去除直流分量
                        merged_channels[ch] = remove_dc_offset(merged_channels[ch]);
                        break;
                    case DataType::PPG:
                        // PPG基本处理：去除直流分量
                        merged_channels[ch] = remove_dc_offset(merged_channels[ch]);
                        // 2. 0.5-8Hz带通滤波（更精确的PPG频带）
                         merged_channels[ch] = bandpass_filter(merged_channels[ch], 50, 0.5, 8.0);
                        // // 3. 50Hz陷波滤波（去除工频干扰）
                         //merged_channels[ch] = filter(merged_channels[ch], 50, 49.5, 50.5, filtertype::notchpass);
                        // // 4. 运动伪迹检测和去除（优化版本）
                         merged_channels[ch] = remove_motion_artifacts(merged_channels[ch], 50);
                        break;
                    case DataType::RESPIRATION:
                        // 呼吸信号基本处理：去除直流分量
                        merged_channels[ch] = remove_dc_offset(merged_channels[ch]);
                        break;
                    default:
                        // 通用处理：去除直流分量
                        merged_channels[ch] = remove_dc_offset(merged_channels[ch]);
                        break;
                }
                
                std::cout << "通道 " << ch << " 基本处理完成" << std::endl;
            } catch (const std::exception& e) {
                std::cerr << "通道 " << ch << " 处理失败: " << e.what() << std::endl;
                // 继续处理其他通道
            }
        }
        
        // 添加到处理结果中
        processed_packets.push_back(merged_packet);
        
        std::cout << "数据类型 " << static_cast<int>(data_type) << " 处理完成" << std::endl;
    }
    
    std::cout << "=== 简化版通道级滤波处理完成 ===" << std::endl;
    std::cout << "总共创建 " << processed_packets.size() << " 个合并数据包" << std::endl;
    
    return processed_packets;
}

// 新增：对完整通道数据应用滤波器
std::vector<std::vector<float>> SignalProcessor::apply_channel_filters(
    const std::vector<std::vector<float>>& channels, DataType data_type) {
    
    std::vector<std::vector<float>> filtered_channels = channels;
    
    switch (data_type) {
        case DataType::EEG:
            filtered_channels = apply_eeg_filters(channels);
            break;
        case DataType::ECG_2LEAD:
        case DataType::ECG_12LEAD:
            filtered_channels = apply_ecg_filters(channels);
            break;
        case DataType::PPG:
            filtered_channels = apply_ppg_filters(channels);
            break;
        case DataType::RESPIRATION:
            filtered_channels = apply_respiration_filters(channels);
            break;
        case DataType::SNORE:
            filtered_channels = apply_snore_filters(channels);
            break;
        case DataType::STETHOSCOPE:
            filtered_channels = apply_stethoscope_filters(channels);
            break;
        default:
            // 通用滤波
            for (auto& channel : filtered_channels) {
                channel = bandpass_filter(channel, 250.0, 0.5, 45.0);
            }
            break;
    }
    
    return filtered_channels;
}

// 新增：EEG通道滤波
std::vector<std::vector<float>> SignalProcessor::apply_eeg_filters(
    const std::vector<std::vector<float>>& channels) {
    
    const double SAMPLE_RATE = 250.0;
    std::vector<std::vector<float>> filtered_channels = channels;
    
    if (channels.size() < 8) return filtered_channels;
    
    // 分离EEG和EOG通道
    std::vector<std::vector<float>> eeg_channels(channels.begin(), channels.begin() + 6);
    std::vector<std::vector<float>> eog_channels(channels.begin() + 6, channels.end());
    
    // 处理EEG通道
    for (auto& channel : eeg_channels) {
        // 眼电伪迹补偿
        if (eog_channels.size() >= 2) {
            channel = compensate_eog_artifact(channel, eog_channels[0], eog_channels[1]);
        }
        
        // 50Hz自适应陷波滤波
        channel = adaptive_notch_filter(channel, SAMPLE_RATE, 50.0, 5.0);
        
        // 0.5-45Hz带通滤波
        channel = bandpass_filter(channel, SAMPLE_RATE, 0.5, 45.0);
    }
    
    // 处理EOG通道
    for (auto& channel : eog_channels) {
        channel = bandpass_filter(channel, SAMPLE_RATE, 0.5, 30.0);
    }
    
    // 合并处理后的通道
    filtered_channels.clear();
    filtered_channels.insert(filtered_channels.end(), eeg_channels.begin(), eeg_channels.end());
    filtered_channels.insert(filtered_channels.end(), eog_channels.begin(), eog_channels.end());
    
    return filtered_channels;
}

// 新增：ECG通道滤波
std::vector<std::vector<float>> SignalProcessor::apply_ecg_filters(
    const std::vector<std::vector<float>>& channels) {
    
    const double SAMPLE_RATE = 250.0;
    std::vector<std::vector<float>> filtered_channels = channels;
    
    std::cout << "开始ECG专业滤波处理..." << std::endl;
    
    for (size_t ch = 0; ch < filtered_channels.size(); ch++) {
        if (filtered_channels[ch].empty()) continue;
        
        std::cout << "处理ECG通道 " << ch << "/" << filtered_channels.size() << std::endl;
        
        try {
            // 1. 去除直流分量
            filtered_channels[ch] = remove_dc_offset(filtered_channels[ch]);
            std::cout << "  通道 " << ch << " - 直流分量去除完成" << std::endl;

            // 2. 0.1Hz高通滤波（去除基线漂移，比0.5Hz更温和）
            filtered_channels[ch] = filter(filtered_channels[ch], SAMPLE_RATE, 0, 0.1, filtertype::highpass);
            std::cout << "  通道 " << ch << " - 0.1Hz高通滤波完成" << std::endl;
            
            // 3. 50Hz陷波滤波（去除工频干扰，带宽1.0Hz更精确）
            filtered_channels[ch] = filter(filtered_channels[ch], SAMPLE_RATE, 49.5, 50.5, filtertype::notchpass);
            std::cout << "  通道 " << ch << " - 50Hz陷波滤波完成" << std::endl;
            
            // 4. 25-40Hz带阻滤波（去除肌电干扰）
            filtered_channels[ch] = filter(filtered_channels[ch], SAMPLE_RATE, 25.0, 40.0, filtertype::bandstop);
            std::cout << "  通道 " << ch << " - 25-40Hz带阻滤波完成" << std::endl;
            
            std::cout << "ECG通道 " << ch << " 滤波处理完成" << std::endl;
            
        } catch (const std::exception& e) {
            std::cerr << "ECG通道 " << ch << " 滤波失败: " << e.what() << std::endl;
            // 继续处理其他通道
        }
    }
    
    std::cout << "ECG专业滤波处理完成" << std::endl;
    return filtered_channels;
}

// 新增：PPG通道滤波
std::vector<std::vector<float>> SignalProcessor::apply_ppg_filters(
    const std::vector<std::vector<float>>& channels) {
    
    const double SAMPLE_RATE = 50.0;
    std::vector<std::vector<float>> filtered_channels = channels;
    
    for (auto& channel : filtered_channels) {
        // 1. 移除直流分量
        channel = remove_dc_offset(channel);
        
        // 2. 0.5-8Hz带通滤波（更精确的PPG频带）
        channel = bandpass_filter(channel, SAMPLE_RATE, 0.5, 8.0);
        
        // 3. 50Hz陷波滤波（去除工频干扰）
        channel = filter(channel, SAMPLE_RATE, 49.5, 50.5, filtertype::notchpass);
        
        // 4. 运动伪迹检测和去除（简单版本）
        channel = remove_motion_artifacts(channel, SAMPLE_RATE);
    }
    
    return filtered_channels;
}

// 新增：呼吸通道滤波
std::vector<std::vector<float>> SignalProcessor::apply_respiration_filters(
    const std::vector<std::vector<float>>& channels) {
    
    const double SAMPLE_RATE = 100.0;
    std::vector<std::vector<float>> filtered_channels = channels;
    
    for (auto& channel : filtered_channels) {
        // 0.1Hz高通滤波
        channel = filter(channel, SAMPLE_RATE, 0, 0.1, filtertype::highpass);
        
        // 50Hz陷波滤波
        channel = adaptive_notch_filter(channel, SAMPLE_RATE, 50.0, 5.0);
        
        // 振幅归一化
        normalize_amplitude(channel);
    }
    
    return filtered_channels;
}

// 新增：打鼾通道滤波
std::vector<std::vector<float>> SignalProcessor::apply_snore_filters(
    const std::vector<std::vector<float>>& channels) {
    
    const double SAMPLE_RATE = 2000.0;
    std::vector<std::vector<float>> filtered_channels = channels;
    
    for (auto& channel : filtered_channels) {
        // 50-2000Hz带通滤波
        channel = bandpass_filter(channel, SAMPLE_RATE, 50.0, 2000.0);
        
        // 振幅归一化
        normalize_amplitude(channel);
    }
    
    return filtered_channels;
}

// 新增：听诊器通道滤波
std::vector<std::vector<float>> SignalProcessor::apply_stethoscope_filters(
    const std::vector<std::vector<float>>& channels) {
    
    const double SAMPLE_RATE = 4000.0;
    std::vector<std::vector<float>> filtered_channels = channels;
    
    for (auto& channel : filtered_channels) {
        // 20-2000Hz带通滤波
        channel = bandpass_filter(channel, SAMPLE_RATE, 20.0, 2000.0);
        
        // 振幅归一化
        normalize_amplitude(channel);
    }
    
    return filtered_channels;
}

SensorData SignalProcessor::preprocess_generic(const SensorData& data) {
    SensorData processed = data;
    
    // 通用预处理：带通滤波
    if (auto* channels = std::get_if<std::vector<std::vector<float>>>(&processed.channel_data)) {
        for (auto& channel : *channels) {
            channel = bandpass_filter(channel, 100.0, 0.5, 45.0);
        }
    }
    return processed;
}
SensorData SignalProcessor::preprocess_signals(const SensorData& raw_data ) {
    // 1. 创建处理后的数据结构
    SensorData processed = raw_data;
    
    // 2. 设备特定预处理
    switch (processed.data_type) {
        case DataType::EEG:
            processed = preprocess_eeg(raw_data);
            break;
        case DataType::ECG_2LEAD:
            processed = preprocess_ecg_2lead(raw_data);
            break;
        case DataType::ECG_12LEAD:
            processed = preprocess_ecg_12lead(raw_data);
            break;
        case DataType::PPG:
            processed = preprocess_ppg(raw_data);
            break;          
        case DataType::RESPIRATION:   
            processed = preprocess_respiration(raw_data);
            break;
        case DataType::SNORE:
            processed = preprocess_snore(raw_data);
            break;
        case DataType::STETHOSCOPE:
            processed = preprocess_stethoscope(raw_data);
            break;   
        default:
            processed = preprocess_generic(raw_data);
            break;
    }
    
    // 3. 通用后处
    return processed;
}

SensorData SignalProcessor::preprocess_eeg(const SensorData& data) {
const double SAMPLE_RATE = 250.0; // 脑电标准采样率250Hz
    
    SensorData processed = data;
    
    // 获取通道数据
    auto& channels = std::get<std::vector<std::vector<float>>>(processed.channel_data);
    if (channels.size() < 8) {
        throw std::runtime_error("Invalid channel count for EEG");
    }
    
    // 分离EEG和EOG通道
    std::vector<std::vector<float>> eeg_channels(channels.begin(), channels.begin() + 6);
    std::vector<std::vector<float>> eog_channels(channels.begin() + 6, channels.end());
    
    // 处理EEG通道
    for (auto& channel : eeg_channels) {
        // 1. 眼电伪迹补偿（使用EOG通道）
        if (eog_channels.size() >= 2) {
            channel = compensate_eog_artifact(channel, eog_channels[0], eog_channels[1]);
        }
        
        // 2. 50Hz自适应陷波滤波 (去除工频干扰)
        channel = adaptive_notch_filter(channel, SAMPLE_RATE, 50.0, 5.0);
        
        // 3. 0.5-45Hz带通滤波 (保留有效频段)
        channel = bandpass_filter(channel, SAMPLE_RATE, 0.5, 45.0);
    }
    
    // 处理EOG通道
    for (auto& channel : eog_channels) {
        // 0.5-30Hz带通滤波
        channel = bandpass_filter(channel, SAMPLE_RATE, 0.5, 30.0);
    }
    
    // 合并处理后的通道
    channels.clear();
    channels.insert(channels.end(), eeg_channels.begin(), eeg_channels.end());
    channels.insert(channels.end(), eog_channels.begin(), eog_channels.end());
    
    // 计算并存储信号质量指数
    float avg_sqi = 0.0f;
    for (const auto& channel : eeg_channels) {
        avg_sqi += calculate_snr(channel);
    }
    processed.sqi = avg_sqi / eeg_channels.size();
    
    return processed;
}
SensorData SignalProcessor::preprocess_ecg_2lead(const SensorData& data)
{
    const double SAMPLE_RATE = 250.0; // 2导联心电标准采样率500Hz
    
    SensorData processed = data;
    
    // 获取通道数据
    auto& channels = std::get<std::vector<std::vector<float>>>(processed.channel_data);
    if (channels.size() < 2) {
        throw std::runtime_error("Invalid channel count for 2-lead ECG");
    }
    
    // 对每个导联独立进行信号处理
    for (auto& channel : channels) {
        // 1. 0.5Hz高通滤波 (去除基线漂移)
        channel = Highpass_filter(channel, SAMPLE_RATE, 0.5);
        
        // 2. 50Hz自适应陷波滤波 (去除工频干扰)
        channel = adaptive_notch_filter(channel, SAMPLE_RATE, 50.0, 5.0);
        
        // 3. 25-40Hz带阻滤波 (去除肌电干扰)
        channel = bandstop_filter(channel, SAMPLE_RATE, 25.0, 40.0);
    }
    
    // 计算并存储信号质量指数
    float avg_sqi = 0.0f;
    for (const auto& channel : channels) {
        avg_sqi += calculate_ecg_sqi(channel, SAMPLE_RATE);
    }
    processed.sqi = avg_sqi / channels.size();
    
    return processed;
    
}
// 12导联心电预处理函数
SensorData SignalProcessor::preprocess_ecg_12lead(const SensorData& data) {
    const double SAMPLE_RATE = 250.0; // 12导联心电标准采样率250.0Hz
    
    // 创建处理后的数据结构
    SensorData processed = data;
    
    // 获取通道数据
    auto& channels = std::get<std::vector<std::vector<float>>>(processed.channel_data);
    if (channels.size() != 12) {
        throw std::runtime_error("Invalid channel count for 12-lead ECG");
    }

    
    // 对每个导联独立进行信号处理
    for (auto& channel : channels) {
        // 1. 0.5Hz高通滤波 (去除基线漂移)
        //channel = remove_dc_offset(channel);
        channel = filter(channel, SAMPLE_RATE,0,0.5, filtertype::highpass);
        
        // 2. 50Hz自适应陷波滤波 (去除工频干扰)
        channel = filter(channel, SAMPLE_RATE, 49.5, 51.5, filtertype::notchpass);
        
        // 3. 25-40Hz带阻滤波 (去除肌电干扰)
        channel = filter(channel, SAMPLE_RATE, 0.5, 0.6, filtertype::bandstop);
    }
    
    // 计算并存储信号质量指数
    float avg_sqi = 0.0f;
    for (const auto& channel : channels) {
        avg_sqi += calculate_ecg_sqi(channel, SAMPLE_RATE);
    }
    processed.sqi = avg_sqi / channels.size();
    
    return processed;
}
SensorData SignalProcessor::preprocess_ppg(const SensorData& data) {
    // 1. 创建处理后的数据结构
    double SAMPLE_RATE = 50;
    SensorData processed = data;
    
    // 2. 获取通道数据（红光和红外光）
    auto& channels = std::get<std::vector<std::vector<float>>>(processed.channel_data);
    if (channels.size() < 2) {
        throw std::runtime_error("PPG数据需要至少两个通道（红光和红外光）");
    }
    
    std::cout << "开始PPG信号预处理，采样率: " << SAMPLE_RATE << "Hz" << std::endl;
    
    // 3. 预处理红光通道（通道0）
    std::cout << "处理红光通道..." << std::endl;
    // a. 移除直流分量
    channels[0] = remove_dc_offset(channels[0]);
    
    // b. 带通滤波 (0.5-8Hz，更精确的PPG频带)
    channels[0] = bandpass_filter(channels[0], SAMPLE_RATE, 0.5, 8.0);
    
    // c. 50Hz陷波滤波（去除工频干扰）
    channels[0] = filter(channels[0], SAMPLE_RATE, 49.5, 50.5, filtertype::notchpass);
    
    // d. 运动伪迹检测和去除
    channels[0] = remove_motion_artifacts(channels[0], SAMPLE_RATE);
    
    // 4. 预处理红外光通道（通道1）
    std::cout << "处理红外光通道..." << std::endl;
    // a. 移除直流分量
    channels[1] = remove_dc_offset(channels[1]);
    
    // b. 带通滤波 (0.5-8Hz)
    channels[1] = bandpass_filter(channels[1], SAMPLE_RATE, 0.5, 8.0);
    
    // c. 50Hz陷波滤波
    channels[1] = filter(channels[1], SAMPLE_RATE, 49.5, 50.5, filtertype::notchpass);
    
    // d. 运动伪迹检测和去除
    channels[1] = remove_motion_artifacts(channels[1], SAMPLE_RATE);
    
    // 5. 计算信号质量指数（SQI）
    std::cout << "计算PPG信号质量..." << std::endl;
    processed.sqi = calculate_PPG_sqi(channels[0], channels[1]);
    
    // 6. 更新附加数据（心率和血氧）
    if (processed.sqi > 0.7) { // 降低阈值，提高容错性
        std::cout << "信号质量良好 (SQI: " << processed.sqi << ")" << std::endl;
        // 高质量信号，保持设备提供的值
    } else {
        std::cout << "信号质量较差 (SQI: " << processed.sqi << ")，标记为不可靠" << std::endl;
        // 低质量信号，标记为不可靠
        processed.additional.hr = 0;
        processed.additional.spo2 = 0;
    }
    
    std::cout << "PPG预处理完成，最终SQI: " << processed.sqi << std::endl;
    return processed;
}
SensorData SignalProcessor::preprocess_respiration(const SensorData& data)
{
 const double SAMPLE_RATE = 100.0; // 呼吸信号标准采样率100Hz
    
    SensorData processed = data;
    
    // 获取通道数据
    auto& channels = std::get<std::vector<std::vector<float>>>(processed.channel_data);
    if (channels.empty()) {
        throw std::runtime_error("No channel data for respiration");
    }
    
    // 对每个通道进行处理
    for (auto& channel : channels) {
        // 1. 0.1Hz高通滤波 (去除基线漂移)
        channel = filter(channel, SAMPLE_RATE,0, 0.1,filtertype::highpass);
        
        // 2. 50Hz陷波滤波 (去除工频干扰)
        channel = adaptive_notch_filter(channel, SAMPLE_RATE, 50.0, 5.0);
        
        // 3. 振幅归一化 (归一化到-1到1之间)
        normalize_amplitude(channel);
    }
    
    // 计算并存储信号质量指数
    processed.sqi = calculate_snr(channels[0]);
    
    return processed;
}
SensorData SignalProcessor::preprocess_snore(const SensorData& data)
{
    const double SAMPLE_RATE = 2000.0; // 鼾声信号标准采样率2000Hz
    
    SensorData processed = data;
    
    // 获取通道数据
    auto& channel = std::get<std::vector<float>>(processed.channel_data);
    
    // 1. 50-2000Hz带通滤波 (保留有效频段)
    std::vector<float> filtered = bandpass_filter(channel, SAMPLE_RATE, 50.0, 2000.0);
    
    // 2. 振幅归一化
    normalize_amplitude(filtered);
    
    processed.channel_data = filtered;
    processed.sqi = calculate_snr(filtered);
    
    return processed;
}
SensorData SignalProcessor::preprocess_stethoscope(const SensorData& data)
{
const double SAMPLE_RATE = 4000.0; // 听诊信号标准采样率4000Hz
    
    SensorData processed = data;
    
    // 获取通道数据
    auto& channels = std::get<std::vector<std::vector<float>>>(processed.channel_data);
    if (channels.size() < 2) {
        throw std::runtime_error("Invalid channel count for stethoscope");
    }
    
    // 对每个通道进行处理
    for (auto& channel : channels) {
        // 1. 20-2000Hz带通滤波 (保留有效频段)
        channel = bandpass_filter(channel, SAMPLE_RATE, 20.0, 2000.0);
        
        // 2. 振幅归一化
        normalize_amplitude(channel);
    }
    
    // 计算并存储信号质量指数
    float avg_sqi = 0.0f;
    for (const auto& channel : channels) {
        avg_sqi += calculate_snr(channel);
    }
    processed.sqi = avg_sqi / channels.size();
    
    return processed;
}
// 添加预处理辅助函数
std::vector<float> SignalProcessor::remove_dc_offset(const std::vector<float>& signal) {
    if (signal.empty()) return {}; // 处理空输入
    std::vector<float> result = signal;
    float dc_remove = 0;
    for(auto& val:signal) dc_remove += val;
    dc_remove /= result.size();
    for(auto& value:result) value -= dc_remove;      return result;
}
std::vector<float> SignalProcessor::apply_gain(const std::vector<float>& signal, float gain) {
    std::vector<float> result = signal;
    return result;
}
// 实现眼电伪迹补偿
std::vector<float> SignalProcessor::compensate_eog_artifact(const std::vector<float>& eeg,
                                             const std::vector<float>& eog1,
                                             const std::vector<float>& eog2) {
    std::vector<float> result = eeg;
    return result;
}
// 实现自适应陷波滤波器（成员函数）
std::vector<float> SignalProcessor::adaptive_notch_filter(const std::vector<float>& input, 
                                    double sample_rate,
                                    double target_freq,
                                    double bandwidth) {
    if (input.empty()) return {};

    // 使用更稳定的实现
    const double omega0 = 2 * M_PI * target_freq / sample_rate;
    const double alpha = sin(omega0) * sinh(log(2) / 2 * bandwidth * omega0 / sin(omega0));

    // 系数计算
    const double b0 = 1.0;
    const double b1 = -2 * cos(omega0);
    const double b2 = 1.0;
    const double a0 = 1 + alpha;
    const double a1 = -2 * cos(omega0);
    const double a2 = 1 - alpha;

    // 归一化系数
    const double inv_a0 = 1.0 / a0;
    const double nb0 = b0 * inv_a0;
    const double nb1 = b1 * inv_a0;
    const double nb2 = b2 * inv_a0;
    const double na1 = a1 * inv_a0;
    const double na2 = a2 * inv_a0;

    // 应用滤波器
    std::vector<float> output(input.size());
    double x1 = 0.0, x2 = 0.0;
    double y1 = 0.0, y2 = 0.0;

    for (size_t n = 0; n < input.size(); ++n) {
        double x0 = input[n];
        double y = nb0 * x0 + nb1 * x1 + nb2 * x2 - na1 * y1 - na2 * y2;
        
        // 防止不稳定
        if (!std::isfinite(y)) y = 0.0;
        
        output[n] = static_cast<float>(y);
        
        // 更新状态
        x2 = x1;
        x1 = x0;
        y2 = y1;
        y1 = y;
    }

    return output;
}


std::vector<float> SignalProcessor::filter(const std::vector<float>& input,
                                            double sample_rate,    
                                            double low_cutoff,  
                                            double high_cutoff,
                                            filtertype type){
                                                            
    switch(type)
    {
        case filtertype::lowpass:
            return Lowpass_filter(input,sample_rate,low_cutoff);
        case filtertype::highpass:
            return Highpass_filter(input,sample_rate,high_cutoff);
        case filtertype::notchpass:
            return adaptive_notch_filter(input,sample_rate,0.5*(high_cutoff+low_cutoff),high_cutoff-low_cutoff);
        case filtertype::bandpass:
            return bandpass_filter(input,sample_rate,low_cutoff,high_cutoff);
        case filtertype::bandstop:
            return bandstop_filter(input,sample_rate,low_cutoff,high_cutoff);
        default:
            return input;
    }
}
//低通滤波器
std::vector<float> SignalProcessor::Lowpass_filter(const std::vector<float>& input,
                                                    double sample_rate,
                                                    double low_cutoff){
    if (input.empty() || low_cutoff <= 0 || low_cutoff >= sample_rate/2) {
            return input;
        }

        const double nyquist = sample_rate / 2.0;
        const double omega = 2.0 * 3.14159265358979323846 * low_cutoff / sample_rate;
        const double k = 1.0 / tan(omega / 2.0);  // 双线性变换预矫正
        const double k2 = k * k;
        const double sqrt2 = std::sqrt(2.0);
        
        // 计算归一化系数
        const double a0 = k2 + sqrt2 * k + 1;
        const double b0 = 1.0 / a0;
        const double b1 = 2 * b0;
        const double b2 = b0;
        const double a1 = 2 * (1 - k2) * b0;
        const double a2 = (k2 - sqrt2 * k + 1) * b0;

        // 应用滤波器
        std::vector<float> output(input.size());
        double x1 = 0.0, x2 = 0.0;  // 输入延迟
        double y1 = 0.0, y2 = 0.0;  // 输出延迟

        for (size_t i = 0; i < input.size(); ++i) {
            const double x0 = input[i];
            const double y0 = b0 * x0 + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
            
            output[i] = static_cast<float>(y0);
            
            // 更新延迟
            x2 = x1;
            x1 = x0;
            y2 = y1;
            y1 = y0;
        }

        return output;
}
//改进后的高通滤波实现
std::vector<float> SignalProcessor::Highpass_filter(const std::vector<float>& input,
                                        double sample_rate,
                                        double cutoff) {
    if (input.empty()) return input;
    int a = input[0];
    // 1. 计算滤波器系数（二阶巴特沃斯）
    const double omega = 2.0 * 3.14159265358979323846 * cutoff / sample_rate;
    const double sn = sin(omega);
    const double cs = cos(omega);
    const double alpha = sn / (2.0 * 0.707);  // Q=0.707 (Butterworth)
    
    const double b0 = (1 + cs) / 2.0;
    const double b1 = -(1 + cs);
    const double b2 = b0;
    const double a0 = 1 + alpha;
    const double a1 = -2 * cs;
    const double a2 = 1 - alpha;
    
    // 2. 归一化系数
    const double inv_a0 = 1.0 / a0;
    const double nb0 = b0 * inv_a0;
    const double nb1 = b1 * inv_a0;
    const double nb2 = b2 * inv_a0;
    const double na1 = a1 * inv_a0;
    const double na2 = a2 * inv_a0;
    
    // 3. 初始化状态（使用前两个样本值）
    double x1 = input.size() > 0 ? input[0] : 0;
    double x2 = x1;
    double y1 = 0;
    double y2 = 0;
    
    // 4. 应用滤波器（处理边界条件）
    std::vector<float> output(input.size());
    for (size_t n = 0; n < input.size(); ++n) {
        const double x0 = input[n];
        
        // 计算当前输出
        double y = nb0 * x0 + nb1 * x1 + nb2 * x2 - na1 * y1 - na2 * y2;
        
        // 5. 稳定化处理（防止NaN/Inf）
        if (!std::isfinite(y)) y = 0.0;
        
        output[n] = static_cast<float>(y);
        
        // 6. 更新状态变量
        x2 = x1;
        x1 = x0;
        y2 = y1;
        y1 = y;
    }
    
    // 7. 可选：去除初始瞬态（前100ms数据）
    /*const size_t transient_samples = static_cast<size_t>(0.1 * sample_rate);
    if (output.size() > transient_samples) {
        const float initial_value = output[transient_samples];
        for (size_t i = 0; i < transient_samples; ++i) {
            output[i] = initial_value;
        }
    }*/
    
    return output;
}
// 带通滤波器
std::vector<float> SignalProcessor::bandpass_filter(const std::vector<float>& input, 
                                                  double sample_rate,
                                                  double low_cutoff,
                                                  double high_cutoff) {
    std::vector<float> output(input.size(), 0.0f);

    // 计算滤波器系数
    double omega0 = 2 * 3.14159265358979323846 * (low_cutoff + high_cutoff) / (2 * sample_rate);
    double BW = (high_cutoff - low_cutoff) / sample_rate;
    double Q = (low_cutoff + high_cutoff) / (2 * (high_cutoff - low_cutoff));

    double alpha = sin(omega0) / (2 * Q);
    double b0 = alpha;
    double b1 = 0;
    double b2 = -alpha;
    double a0 = 1 + alpha;
    double a1 = -2 * cos(omega0);
    double a2 = 1 - alpha;

    // 归一化系数
    b0 /= a0;
    b1 /= a0;
    b2 /= a0;
    a1 /= a0;
    a2 /= a0;

    // 初始化滤波器状态
    double x1 = 0, x2 = 0;
    double y1 = 0, y2 = 0;

    // 应用滤波器
    for (size_t n = 0; n < input.size(); ++n) {
        double x0 = input[n];
        output[n] = b0 * x0 + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;

        // 更新状态
        x2 = x1;
        x1 = x0;
        y2 = y1;
        y1 = output[n];
    }

    return output;
}
//带阻滤波器
std::vector<float> SignalProcessor::bandstop_filter(const std::vector<float>& input, 
                                                  double sample_rate,
                                                  double low_cutoff,
                                                  double high_cutoff) 
{
    if (input.empty()) return {};
    if (low_cutoff >= high_cutoff) {
        throw std::invalid_argument("Low cutoff must be less than high cutoff");
    }
    if (input.size() < 4) return input;  // 太短的信号无法有效滤波

    const double f0 = (low_cutoff + high_cutoff) / 2.0;
    const double bw = high_cutoff - low_cutoff;
    
    // 1. 使用双线性变换进行频率预矫正
    const double T = 1.0 / sample_rate;
    const double w0 = 2.0 * 3.14159265358979323846 * f0;
    const double wd = 2.0 * 3.14159265358979323846 * bw;
    
    // 预矫正模拟频率
    const double wa = 2.0 / T * tan(w0 * T / 2.0);
    const double Ba = 2.0 / T * tan(wd * T / 2.0);
    
    // 2. 计算Butterworth滤波器系数
    const double Q = wa / Ba;  // 更精确的Q值计算
    const double alpha = sin(w0 * T) / (2 * Q);
    
    const double b0 = 1.0;
    const double b1 = -2.0 * cos(w0 * T);
    const double b2 = 1.0;
    const double a0 = 1.0 + alpha;
    const double a1 = -2.0 * cos(w0 * T);
    const double a2 = 1.0 - alpha;

    // 3. 更精确的系数归一化
    const double gain = 1.0 / a0;  // 保证通带增益为1
    const double nb0 = b0 * gain;
    const double nb1 = b1 * gain;
    const double nb2 = b2 * gain;
    const double na1 = a1 * gain;
    const double na2 = a2 * gain;

    // 4. 应用滤波器（带合理状态初始化）
    std::vector<float> output(input.size());
    double x1 = input[0], x2 = input[0];  // 输入状态初始化
    double y1 = 0.0, y2 = 0.0;           // 输出状态初始化
    
    // 使用前两个样本计算初始输出状态
    if (input.size() >= 2) {    
        const double x0 = input[0];
        y1 = nb0 * x0 + nb1 * x1 + nb2 * x2;
        x2 = x1;
        x1 = x0;
    }
    
    for (size_t n = 0; n < input.size(); ++n) {
        const double x0 = input[n];
        double y = nb0 * x0 + nb1 * x1 + nb2 * x2 - na1 * y1 - na2 * y2;
        
        // 5. 添加输出限幅保护
        const double input_max = *std::max_element(input.begin(), input.end());
        const double safety_margin = 2.0 * input_max;
        if (std::abs(y) > safety_margin) {
            y = std::copysign(safety_margin, y);
        }
        
        output[n] = static_cast<float>(y);
        
        // 更新状态
        x2 = x1;
        x1 = x0;
        y2 = y1;
        y1 = y;
    }
    return output;
}
// 运动补偿
std::vector<float> SignalProcessor::compensate_motion_artifact(const std::vector<float>& ppg,
                                             const std::vector<float>& motion) {
    std::vector<float> output = ppg;
    return ppg;   
}
// 辅助函数：计算PPG信号质量指数
float SignalProcessor::calculate_PPG_sqi(const std::vector<float>& red_channel, 
                                    const std::vector<float>& ir_channel) {
    if (red_channel.empty() || ir_channel.empty()) return 0.0f;
    if (red_channel.size() != ir_channel.size()) return 0.0f;
    
    const size_t min_samples = 100; // 至少需要100个样本
    if (red_channel.size() < min_samples) return 0.0f;
    
    // 1. 信号幅度检测
    float red_max = *std::max_element(red_channel.begin(), red_channel.end());
    float red_min = *std::min_element(red_channel.begin(), red_channel.end());
    float red_pp = red_max - red_min;
    
    float ir_max = *std::max_element(ir_channel.begin(), ir_channel.end());
    float ir_min = *std::min_element(ir_channel.begin(), ir_channel.end());
    float ir_pp = ir_max - ir_min;
    
    // 检查信号幅度是否合理
    if (red_pp < 0.01f || ir_pp < 0.01f) return 0.0f;
    
    // 2. 信噪比计算
    float red_snr = calculate_snr(red_channel);
    float ir_snr = calculate_snr(ir_channel);
    
    // 3. 信号连续性检测（检测信号丢失）
    int red_gaps = 0, ir_gaps = 0;
    const float gap_threshold = 0.001f; // 间隙阈值
    
    for (size_t i = 1; i < red_channel.size(); ++i) {
        if (std::abs(red_channel[i] - red_channel[i-1]) > gap_threshold) {
            red_gaps++;
        }
        if (std::abs(ir_channel[i] - ir_channel[i-1]) > gap_threshold) {
            ir_gaps++;
        }
    }
    
    float red_continuity = 1.0f - (float)red_gaps / red_channel.size();
    float ir_continuity = 1.0f - (float)ir_gaps / ir_channel.size();
    
    // 4. 红光和红外光信号相关性
    float correlation = calculate_correlation(red_channel, ir_channel);
    
    // 5. 综合质量评分
    float sqi = 0.0f;
    
    // 幅度因子（权重0.2）
    float amp_factor = std::min(red_pp, ir_pp) / std::max(red_pp, ir_pp);
    
    // SNR因子（权重0.3）
    float snr_factor = (red_snr + ir_snr) / 2.0f;
    
    // 连续性因子（权重0.2）
    float continuity_factor = (red_continuity + ir_continuity) / 2.0f;
    
    // 相关性因子（权重0.3）
    float corr_factor = std::max(0.0f, correlation);
    
    // 加权平均
    sqi = 0.2f * amp_factor + 0.3f * snr_factor + 0.2f * continuity_factor + 0.3f * corr_factor;
    
    // 确保在[0,1]范围内
    return clamp(sqi, 0.0f, 1.0f);
}

// 增强版运动伪迹检测和去除函数
std::vector<float> SignalProcessor::remove_motion_artifacts(const std::vector<float>& signal, double sample_rate) {
    if (signal.empty()) return signal;
    
    std::vector<float> result = signal;
    const size_t window_size = static_cast<size_t>(0.5 * sample_rate); // 0.5秒窗口
    
    if (signal.size() < window_size) return result;
    
    // 使用滑动窗口检测运动伪迹
    for (size_t i = window_size; i < signal.size(); ++i) {
        // 计算窗口内的统计特性
        float sum = 0.0f, sum_sq = 0.0f;
        for (size_t j = i - window_size; j < i; ++j) {
            sum += signal[j];
            sum_sq += signal[j] * signal[j];
        }
        
        float mean = sum / window_size;
        float variance = (sum_sq / window_size) - (mean * mean);
        float std_dev = std::sqrt(std::max(0.0f, variance));
        
        // 检测异常值（可能的运动伪迹）
        float current_val = signal[i];
        float z_score = std::abs(current_val - mean) / (std_dev + 1e-6f);
        
        // 如果Z-score > 3，认为是运动伪迹
        if (z_score > 3.0f) {
            // 使用中值滤波替换异常值
            std::vector<float> window_values;
            for (size_t j = std::max(static_cast<size_t>(0), i - window_size/2); 
                 j < std::min(signal.size(), i + window_size/2); ++j) {
                window_values.push_back(signal[j]);
            }
            
            if (!window_values.empty()) {
                std::sort(window_values.begin(), window_values.end());
                result[i] = window_values[window_values.size() / 2]; // 中值
            }
        }
    }
    
    return result;
}

// 辅助函数：计算信号的信噪比（SNR）
float SignalProcessor::calculate_snr(const std::vector<float>& signal) {
    if (signal.size() < 2) return 0.0f;
    
    // 计算信号功率
    float signal_power = 0.0f;
    for (float s : signal) {
        signal_power += s * s;
    }
    signal_power /= signal.size();
    
    // 计算噪声功率（通过差分近似）
    float noise_power = 0.0f;
    for (size_t i = 1; i < signal.size(); ++i) {
        float diff = signal[i] - signal[i-1];
        noise_power += diff * diff;
    }
    noise_power /= (signal.size() - 1);
    
    // 计算SNR (dB)
    if (noise_power < 1e-6f) return 1.0f; // 避免除以零
    float snr_db = 10.0f * std::log10(signal_power / noise_power);
    
    // 将SNR转换为0-1的质量指数
    return clamp(snr_db / 40.0f, 0.0f, 1.0f); // 假设40dB为最大质量
}
// 辅助函数：计算两个信号的相关系数
float SignalProcessor::calculate_correlation(const std::vector<float>& x, 
                                           const std::vector<float>& y) {
    if (x.empty() || y.empty() || x.size() != y.size()) return 0.0f;
    
    const size_t n = x.size();
    if (n < 2) return 0.0f;
    
    // 计算均值
    float x_mean = 0.0f, y_mean = 0.0f;
    for (size_t i = 0; i < n; ++i) {
        x_mean += x[i];
        y_mean += y[i];
    }
    x_mean /= n;
    y_mean /= n;
    
    // 计算协方差和方差
    float covariance = 0.0f;
    float x_variance = 0.0f;
    float y_variance = 0.0f;
    
    for (size_t i = 0; i < n; ++i) {
        float x_diff = x[i] - x_mean;
        float y_diff = y[i] - y_mean;
        
        covariance += x_diff * y_diff;
        x_variance += x_diff * x_diff;
        y_variance += y_diff * y_diff;
    }
    
    // 计算相关系数
    if (x_variance < 1e-6f || y_variance < 1e-6f) return 0.0f;
    
    float correlation = covariance / std::sqrt(x_variance * y_variance);
    
    // 确保在[-1, 1]范围内
    return clamp(correlation, -1.0f, 1.0f);
}
//ecg sqi
float SignalProcessor::calculate_ecg_sqi(const std::vector<float>& signal, double sample_rate) {
    // 1. 检查输入有效性
    if (signal.empty()) return 0.0f;
    if (sample_rate <= 0) return 0.0f;
    const size_t min_samples = static_cast<size_t>(0.5 * sample_rate); // 至少0.5秒数据
    if (signal.size() < min_samples) return 0.0f;

    // 3. 幅度检测（检测导联脱落或信号丢失）
    float max_val = *std::max_element(signal.begin(), signal.end());
    float min_val = *std::min_element(signal.begin(), signal.end());
    float pp_amp = max_val - min_val; // 峰峰值幅度
    if (pp_amp < 0.1f) return 0.0f;  // 幅度过低（假设单位是mV）

    // 4. 噪声水平评估
    float noise_level = 0.0f;
    for (size_t i = 1; i < signal.size(); ++i) {
        float diff = signal[i] - signal[i-1];
        noise_level += diff * diff;
    }
    noise_level = std::sqrt(noise_level / signal.size());
    
    // 5. 功率谱分析（QRS频带能量比）
    float total_power = 0.0f;
    float qrs_power = 0.0f;
    for (float s : signal) {
        total_power += s * s;
    }

    // 5-20Hz带通滤波（QRS主要能量带）
    std::vector<float> qrs_band = bandpass_filter(signal, sample_rate, 5.0, 20.0);
    for (float s : qrs_band) {
        qrs_power += s * s;
    }
    
    float qrs_ratio = (total_power > 0) ? qrs_power / total_power : 0.0f;

    // 6. 基于特征的SQI计算
    float sqi = 0.0f;
    
    // 幅度因子（0.5-5mV为理想范围）
    float amp_factor = clamp((pp_amp - 0.5f) / 4.5f, 0.0f, 1.0f);
    
    // 噪声因子（经验阈值）
    float noise_factor = std::exp(-noise_level * 50.0f);
    
    // QRS能量因子
    float qrs_factor = clamp((qrs_ratio - 0.3f) * 2.5f, 0.0f, 1.0f);
    
    // 综合评分（加权平均）
    sqi = 0.4f * amp_factor + 0.4f * qrs_factor + 0.2f * noise_factor;
    
    // 确保在[0,1]范围内
    return clamp(sqi, 0.0f, 1.0f);
}
void SignalProcessor::normalize_amplitude(std::vector<float>& signal) {
    if (signal.empty()) return;
    
    // 找到最大绝对值
    float max_val = 0.0f;
    for (float value : signal) {
        float abs_val = std::abs(value);
        if (abs_val > max_val) {
            max_val = abs_val;
        }
    }
    
    // 归一化处理
    if (max_val > 0.0f) {
        float scale = 1.0f / max_val;
        for (float& value : signal) {
            value *= scale;
        }
    }
}