#include "signal_processor.h"
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) {
    SensorData processed = data;
    return processed;
}
SensorData SignalProcessor::preprocess_ecg_2lead(const SensorData& data)
{
    SensorData processed = data;
    return processed;
    
}
// 12导联心电预处理函数
SensorData SignalProcessor::preprocess_ecg_12lead(const SensorData& data) {
    const double SAMPLE_RATE = 250.0; // 12导联心电标准采样率500Hz
    
    // 创建处理后的数据结构
    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 = filter(channel, SAMPLE_RATE, 0.5, 0.0, filtertype::highpass);
        
        // 2. 50Hz自适应陷波滤波 (去除工频干扰)
        channel = filter(channel, SAMPLE_RATE, 50.0, 60,filtertype::notchpass);
        
        // 3. 25-40Hz带阻滤波 (去除肌电干扰)
        channel = filter(channel, SAMPLE_RATE, 25.0, 40.0, 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. 创建处理后的数据结构
    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)
{
    SensorData processed = data;
    return processed;
}
SensorData SignalProcessor::preprocess_snore(const SensorData& data)
{
    SensorData processed = data;
    return processed;
}
SensorData SignalProcessor::preprocess_stethoscope(const SensorData& data)
{
    SensorData processed = data;
    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) {
    std::vector<float> output(input.size(), 0.0f);

    // 计算滤波器系数
    double omega0 = 2 * M_PI * target_freq / sample_rate;
    double alpha = sin(omega0) * sinh(log(2) / 2 * bandwidth * omega0 / sin(omega0));

    double b0 = 1;
    double b1 = -2 * cos(omega0);
    double b2 = 1;
    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::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()) return input;
    std::vector<float> output(input.size());
    double A,f;
    f = 1/sample_rate;
    A = 1.0f/(1+(1/(2*M_PI*f*low_cutoff)));
    output[0] = input[0];
    if (input.size() > 1) output[1] = input[1];
    for (size_t n = 2; n < input.size(); n++)
    {
        output[n] = A*input[n] + (1-A)*output[n-1];
    }
    return output;
}
std::vector<float> SignalProcessor::Highpass_filter(const std::vector<float>& input,
                                        double sample_rate,
                                        double high_cutoff){
    if (input.empty()) return input;
    std::vector<float> output(input.size());
    double A,f;
    f = 1/sample_rate;
    A = 1.0f/(1+(1/(2*M_PI*f*high_cutoff)));
    output[0] = input[0];
    if (input.size() > 1) output[1] = input[1];
    for (size_t n = 2; n < input.size(); n++)
    {
        output[n] = A*output[n-1] + A*(input[n]-input[n-1]);
    }
    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");
    }

    std::vector<float> output(input.size(), 0.0f);
    const double f0 = (low_cutoff + high_cutoff) / 2.0;  // 中心频率
    const double BW = high_cutoff - low_cutoff;          // 带宽
    const double Q = f0 / BW;                            // 品质因数
    const double omega0 = 2 * M_PI * f0 / sample_rate;
    const double alpha = sin(omega0) / (2 * Q);

    // 计算滤波器系数
    const double b0 = 1.0;
    const double b1 = -2 * cos(omega0);
    const double b2 = 1.0;
    const double a0 = 1.0 + alpha;
    const double a1 = -2 * cos(omega0);
    const double a2 = 1.0 - 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;

    // 滤波器状态
    double x1 = 0.0, x2 = 0.0;
    double y1 = 0.0, y2 = 0.0;

    // 应用滤波器
    for (size_t n = 0; n < input.size(); ++n) {
        const double x0 = input[n];
        const double y = nb0 * x0 + nb1 * x1 + nb2 * x2 - na1 * y1 - na2 * y2;
        
        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) {
    return 0.0f;
}
// 辅助函数：计算两个信号的相关系数
float SignalProcessor::calculate_correlation(const std::vector<float>& x, 
                                           const std::vector<float>& y) {
    return 0.0f;    
}
// 在 signal_processor.cpp 文件中添加以下代码
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);
}