"""
UWB CPD 信号处理算法

核心: 从 Channel Impulse Response (CIR) 中提取微动呼吸信号
CIR 可视为车内3D地形图的时间切片

数据流:
  原始 UWB 回波 → CIR 序列 → 静态背景消除 → 
  微动信号提取 → 频域分析 → 生命体征判定
"""

import numpy as np
from scipy.signal import butter, filtfilt, fft

class UWBCPDProcessor:
    """
    UWB CPD 信号处理器
    
    按 Euro NCAP 要求:
    - 响应时间 < 13 秒
    - 检测率 > 99%
    - 误报率 < 1%
    """
    
    # 婴儿呼吸频率范围 (次/分钟)
    INFANT_BREATHING_RANGE = (20, 40)
    # 成人呼吸频率范围
    ADULT_BREATHING_RANGE = (12, 20)
    
    def __init__(self, fs=100, n_samples=512):
        """
        Args:
            fs: CIR 采样率 (Hz), 每秒采集 100 个 CIR 快照
            n_samples: 每个 CIR 快照的采样点数
        """
        self.fs = fs
        self.n_samples = n_samples
        # 静态背景缓存
        self.background = None
        self.background_count = 0
        
    def update_background(self, cir_snapshot):
        """更新静态背景模型"""
        if self.background is None:
            self.background = cir_snapshot.copy()
            self.background_count = 1
        else:
            # 指数移动平均
            alpha = 0.95
            self.background = alpha * self.background + (1 - alpha) * cir_snapshot
            self.background_count += 1
    
    def extract_micro_motion(self, cir_sequence):
        """
        从 CIR 序列中提取微动信号
        
        Args:
            cir_sequence: (n_frames, n_samples) CIR 时间序列
        Returns:
            motion_signal: (n_frames,) 微动强度序列
        """
        # 1. 静态背景减除
        if self.background is not None:
            diff = cir_sequence - self.background
        else:
            diff = cir_sequence - np.mean(cir_sequence, axis=0)
        
        # 2. 沿距离轴取绝对值并求和
        motion_signal = np.sum(np.abs(diff), axis=1)
        
        # 3. 带通滤波 (0.1-1.5 Hz, 对应 6-90 次/分呼吸)
        motion_signal = self._bandpass(motion_signal, 0.1, 1.5)
        
        return motion_signal
    
    def detect_breathing(self, motion_signal, duration_sec=10):
        """
        检测呼吸信号
        
        Args:
            motion_signal: 微动信号序列
            duration_sec: 分析窗口长度
        Returns:
            result: dict with detection info
        """
        window_samples = int(duration_sec * self.fs)
        if len(motion_signal) < window_samples:
            return {'detected': False, 'reason': 'insufficient_data'}
        
        # 取最近窗口
        signal = motion_signal[-window_samples:]
        
        # FFT 频域分析
        freqs, power = self._compute_spectrum(signal)
        
        # 检查呼吸频率范围
        infant_mask = (
            (freqs >= self.INFANT_BREATHING_RANGE[0] / 60) & 
            (freqs <= self.INFANT_BREATHING_RANGE[1] / 60)
        )
        adult_mask = (
            (freqs >= self.ADULT_BREATHING_RANGE[0] / 60) & 
            (freqs <= self.ADULT_BREATHING_RANGE[1] / 60)
        )
        
        infant_power = np.sum(power[infant_mask])
        adult_power = np.sum(power[adult_mask])
        total_power = np.sum(power) + 1e-10
        
        # 信噪比判断
        snr = (infant_power + adult_power) / total_power
        
        # 找到主频
        if np.any(infant_mask) and infant_power > 0:
            peak_freq = freqs[infant_mask][np.argmax(power[infant_mask])]
            peak_bpm = peak_freq * 60
        else:
            peak_bpm = 0
        
        result = {
            'detected': snr > 0.3,
            'snr': snr,
            'peak_bpm': peak_bpm,
            'infant_power': infant_power,
            'adult_power': adult_power,
            'duration_analyzed': duration_sec
        }
        
        return result
    
    def _bandpass(self, signal, low, high):
        """带通滤波"""
        nyq = self.fs / 2
        b, a = butter(4, [low / nyq, high / nyq], btype='band')
        return filtfilt(b, a, signal)
    
    def _compute_spectrum(self, signal):
        """计算功率谱"""
        n = len(signal)
        freqs = np.fft.rfftfreq(n, 1 / self.fs)
        fft_vals = np.fft.rfft(signal * np.hanning(n))
        power = np.abs(fft_vals) ** 2
        return freqs, power


# ============================================
# 完整 CPD 系统模拟
# ============================================

def simulate_cpd_scenario():
    """模拟 CPD 检测场景"""
    fs = 100  # 100 Hz CIR 采样
    duration = 15  # 15 秒数据
    n_samples = 512  # 每个CIR快照512点
    
    processor = UWBCPDProcessor(fs=fs, n_samples=n_samples)
    
    # 场景1: 空车 (只有静态背景)
    print("=== 场景1: 空车 ===")
    noise = np.random.normal(0, 0.01, (duration * fs, n_samples))
    motion = processor.extract_micro_motion(noise)
    result = processor.detect_breathing(motion, duration_sec=10)
    print(f"检测到生命: {result['detected']}")
    print(f"SNR: {result['snr']:.4f}")
    
    # 场景2: 有婴儿 (模拟呼吸微动)
    print("\n=== 场景2: 婴儿呼吸 ===")
    breathing_freq = 30 / 60  # 30次/分 = 0.5 Hz
    t = np.arange(duration * fs) / fs
    breathing_signal = 0.05 * np.sin(2 * np.pi * breathing_freq * t)
    
    # 将呼吸信号注入特定距离bin
    cir_with_breathing = noise.copy()
    target_bin = 200  # 模拟2米处
    cir_with_breathing[:, target_bin] += breathing_signal
    
    motion = processor.extract_micro_motion(cir_with_breathing)
    result = processor.detect_breathing(motion, duration_sec=10)
    print(f"检测到生命: {result['detected']}")
    print(f"SNR: {result['snr']:.4f}")
    print(f"主频: {result['peak_bpm']:.1f} BPM")
    
    # 场景3: 被毯子覆盖的婴儿 (信号衰减)
    print("\n=== 场景3: 毯子覆盖婴儿 ===")
    attenuated_signal = breathing_signal * 0.3  # 信号衰减70%
    cir_covered = noise.copy()
    cir_covered[:, target_bin] += attenuated_signal
    motion = processor.extract_micro_motion(cir_covered)
    result = processor.detect_breathing(motion, duration_sec=10)
    print(f"检测到生命: {result['detected']}")
    print(f"SNR: {result['snr']:.4f}")
    print(f"主频: {result['peak_bpm']:.1f} BPM")


if __name__ == "__main__":
    simulate_cpd_scenario()