medical_SDK/src/signal_processor/signal_processor.cpp

#include "signal_processor.h"

// 新增：处理多个数据包的通道级滤波
std::vector<SensorData> SignalProcessor::process_channel_based_filtering(
    const std::vector<SensorData>& data_packets) {
    
    if (data_packets.empty()) return {};
    
    // 按数据类型分组
    std::map<DataType, std::vector<SensorData>> grouped_data;
    for (const auto& packet : data_packets) {
        grouped_data[packet.data_type].push_back(packet);
    }
    
    std::vector<SensorData> processed_packets;
    
    // 对每种数据类型分别处理
    for (auto& [data_type, packets] : grouped_data) {
        if (packets.empty()) continue;
        
        // 获取第一个数据包作为模板
        SensorData template_packet = packets[0];
        
        // 收集所有通道的完整数据
        std::vector<std::vector<float>> all_channels;
        size_t num_channels = 0;
        
        // 确定通道数量
        if (auto* channels = std::get_if<std::vector<std::vector<float>>>(&packets[0].channel_data)) {
            num_channels = channels->size();
            all_channels.resize(num_channels);
        }
        
        // 收集所有数据包中相同通道的数据
        for (size_t ch = 0; ch < num_channels; ch++) {
            for (const auto& packet : packets) {
                if (auto* channels = std::get_if<std::vector<std::vector<float>>>(&packet.channel_data)) {
                    if (ch < channels->size()) {
                        all_channels[ch].insert(all_channels[ch].end(), 
                                              (*channels)[ch].begin(), 
                                              (*channels)[ch].end());
                    }
                }
            }
        }
        
        // 对完整通道数据进行滤波处理
        std::vector<std::vector<float>> filtered_channels = 
            apply_channel_filters(all_channels, data_type);
        
        // 将滤波后的数据重新分配回数据包结构
        size_t samples_per_packet = 0;
        if (auto* channels = std::get_if<std::vector<std::vector<float>>>(&packets[0].channel_data)) {
            if (!channels->empty()) {
                samples_per_packet = (*channels)[0].size();
            }
        }
        
        // 重新构建数据包
        size_t total_samples = filtered_channels[0].size();
        size_t num_packets = (total_samples + samples_per_packet - 1) / samples_per_packet;
        
        for (size_t p = 0; p < num_packets; p++) {
            SensorData new_packet = template_packet;
            new_packet.packet_sn = p; // 重新分配包序号
            
            // 创建新的通道数据结构
            auto& new_channels = new_packet.channel_data.emplace<std::vector<std::vector<float>>>();
            new_channels.resize(num_channels);
            
            // 分配数据到各个通道
            for (size_t ch = 0; ch < num_channels; ch++) {
                size_t start_idx = p * samples_per_packet;
                size_t end_idx = std::min(start_idx + samples_per_packet, filtered_channels[ch].size());
                
                if (start_idx < filtered_channels[ch].size()) {
                    new_channels[ch].assign(filtered_channels[ch].begin() + start_idx,
                                           filtered_channels[ch].begin() + end_idx);
                }
            }
            
            processed_packets.push_back(new_packet);
        }
    }
    
    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;
    
    for (auto& channel : filtered_channels) {
        // 0.5Hz高通滤波
        channel = Highpass_filter(channel, SAMPLE_RATE, 0.5);
        
        // 50Hz自适应陷波滤波
        channel = adaptive_notch_filter(channel, SAMPLE_RATE, 50.0, 5.0);
        
        // 25-40Hz带阻滤波
        channel = bandstop_filter(channel, SAMPLE_RATE, 25.0, 40.0);
    }
    
    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) {
        // 移除直流分量
        channel = remove_dc_offset(channel);
        
        // 0.5-10Hz带通滤波
        channel = bandpass_filter(channel, SAMPLE_RATE, 0.5, 10.0);
    }
    
    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数据需要至少两个通道（红光和红外光）");
    }
    
    channels[0] = remove_dc_offset(channels[0]);

    
    // c. 带通滤波 (0.5-10Hz)
    channels[0] = bandpass_filter(channels[0], 50.0, 0.5, 10.0);
    
    
    // 4. 预处理红外光通道（通道1）

        // b. 移除直流分量（DC）
        channels[1] = remove_dc_offset(channels[1]);
        
        // c. 带通滤波 (0.5-10Hz)
        channels[1] = bandpass_filter(channels[1], 50.0, 0.5, 10.0);
    
    
    // 5. 计算信号质量指数（SQI）
    processed.sqi = calculate_PPG_sqi(channels[0], channels[1]);
    
    // 6. 更新附加数据（心率和血氧）
    // 文档中已经提供了hr和spo2值，但我们可以根据信号质量进行修正
    if (processed.sqi > 0.8) {
        // 高质量信号，使用设备提供的值
    } else {
        // 低质量信号，可能需要重新计算或标记为不可靠
        processed.additional.hr = 0; // 设置为0表示不可靠
        processed.additional.spo2 = 0;
    }
    
    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 * M_PI * 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 * M_PI * 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 * M_PI * (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 * M_PI * f0;
    const double wd = 2.0 * M_PI * 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) {
    return 0.0f;
}

// 辅助函数：计算信号的信噪比（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 std::clamp(snr_db / 40.0f, 0.0f, 1.0f); // 假设40dB为最大质量
}
// 辅助函数：计算两个信号的相关系数
float SignalProcessor::calculate_correlation(const std::vector<float>& x, 
                                           const std::vector<float>& y) {
    return 0.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 = std::clamp((pp_amp - 0.5f) / 4.5f, 0.0f, 1.0f);
    
    // 噪声因子（经验阈值）
    float noise_factor = std::exp(-noise_level * 50.0f);
    
    // QRS能量因子
    float qrs_factor = std::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 std::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;
        }
    }
}

// 使用示例：如何正确使用通道级滤波
/*
// 原来的错误用法（对单个数据包滤波）：
std::vector<SensorData> raw_packets = get_raw_data_packets();
for (auto& packet : raw_packets) {
    packet = signal_processor.preprocess_signals(packet); // 错误！对单个包滤波
}

// 新的正确用法（通道级滤波）：
std::vector<SensorData> raw_packets = get_raw_data_packets();
std::vector<SensorData> processed_packets = 
    signal_processor.process_channel_based_filtering(raw_packets);

// 或者，如果你想保持原有的数据包结构，可以这样：
std::vector<SensorData> raw_packets = get_raw_data_packets();
std::vector<SensorData> processed_packets = 
    signal_processor.process_channel_based_filtering(raw_packets);

// 关键区别：
// 1. 原来：对每个数据包内的短时间序列进行滤波（错误）
// 2. 现在：收集所有数据包中相同通道的完整数据，进行滤波，然后重新分配
*/