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

class CPDDetector:
    """
    60GHz FMCW 雷达儿童遗留检测
    
    核心原理：
    1. 从雷达中频信号提取相位
    2. 相位变化反映目标微运动
    3. 带通滤波分离呼吸(0.1-0.5Hz)和心跳(0.8-2.0Hz)
    4. 能量阈值判定是否存在活体
    """
    
    def __init__(self, sample_rate=20, breath_range=(0.1, 0.5), 
                 heart_range=(0.8, 2.0), threshold=0.01):
        self.fs = sample_rate
        self.breath_range = breath_range
        self.heart_range = heart_range
        self.threshold = threshold
    
    def _bandpass(self, signal, freq_range):
        """带通滤波"""
        nyq = self.fs / 2
        low = freq_range[0] / nyq
        high = min(freq_range[1] / nyq, 0.99)
        b, a = butter(4, [low, high], btype='band')
        return filtfilt(b, a, signal)
    
    def detect(self, radar_if_signal, window_sec=10):
        """
        检测是否存在活体（儿童/宠物）
        
        Args:
            radar_if_signal: 中频信号, shape=(N,)
            window_sec: 检测窗口秒数
            
        Returns:
            dict: 检测结果
        """
        n_window = int(window_sec * self.fs)
        results = []
        
        for i in range(0, len(radar_if_signal) - n_window, n_window // 2):
            window = radar_if_signal[i:i+n_window]
            
            # 提取相位
            phase = np.angle(window)
            # 解卷绕
            phase = np.unwrap(phase)
            # 去趋势
            phase = phase - np.mean(phase)
            
            # 呼吸频带能量
            breath_signal = self._bandpass(phase, self.breath_range)
            breath_energy = np.sum(breath_signal**2) / n_window
            
            # 心跳频带能量
            heart_signal = self._bandpass(phase, self.heart_range)
            heart_energy = np.sum(heart_signal**2) / n_window
            
            # 总微运动能量
            total_energy = breath_energy + heart_energy
            
            # 检测峰值（呼吸频率）
            peaks, _ = find_peaks(breath_signal, distance=self.fs*2)
            breath_rate = len(peaks) / window_sec * 60 if len(peaks) > 0 else 0
            
            results.append({
                'has_presence': total_energy > self.threshold,
                'breath_energy': breath_energy,
                'heart_energy': heart_energy,
                'breath_rate_bpm': breath_rate,
                'confidence': min(total_energy / (self.threshold * 5), 1.0)
            })
        
        # 聚合结果
        presence_count = sum(1 for r in results if r['has_presence'])
        presence_ratio = presence_count / len(results) if results else 0
        
        return {
            'child_present': presence_ratio > 0.6,
            'confidence': np.mean([r['confidence'] for r in results]) if results else 0,
            'avg_breath_rate': np.mean([r['breath_rate_bpm'] for r in results 
                                       if r['breath_rate_bpm'] > 0]) if results else 0,
            'presence_ratio': presence_ratio
        }


if __name__ == "__main__":
    detector = CPDDetector(sample_rate=20, threshold=0.005)
    
    # 模拟呼吸信号 (0.3Hz = 18 bpm)
    t = np.linspace(0, 30, 600)
    breath = 0.1 * np.sin(2*np.pi*0.3*t)
    heartbeat = 0.02 * np.sin(2*np.pi*1.2*t)
    noise = 0.001 * np.random.randn(600)
    
    # 模拟雷达中频信号（相位调制）
    signal = np.exp(1j * (breath + heartbeat + noise))
    
    result = detector.detect(signal, window_sec=10)
    
    print("=== CPD 检测结果 ===")
    print(f"儿童存在: {'是 ⚠️' if result['child_present'] else '否'}")
    print(f"置信度: {result['confidence']:.1%}")
    print(f"平均呼吸率: {result['avg_breath_rate']:.0f} bpm")
    print(f"存在比例: {result['presence_ratio']:.0%}")
    
    print("\n=== 部署参数 ===")
    print("频段: 60GHz FMCW")
    print("扫描带宽: 4GHz (距离分辨率~3.75cm)")
    print("帧率: 20fps")
    print("功耗: <1W")
    print("检测延迟: <10s")
    print("误报率目标: <5%")