"""
RT-VSS 仿真工具链复现

依赖：
- Blender 4.2+: 3D场景建模 + 生理动画
- Sionna RT 0.16+: 光线追踪仿真
- NumPy/SciPy: 信号处理

核心流程：
1. Blender建模：室内场景 + 人体胸壁运动动画
2. Sionna RT：发射FR3信号 → 光线追踪 → 接收信号
3. 信号处理：相位提取 → 呼吸/心跳分离
"""

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

class RTVSSSimulator:
    """
    FR3生命体征感知仿真器
    
    论文方法：
    1. Blender建模: 3D室内场景 + 人体模型
       - 胸壁运动: 呼吸(4-12mm) + 心跳(0.2-0.5mm)
       - 动画: 胸壁随时间的位移函数
    
    2. Sionna Ray Tracing:
       - 发射天线: FR3频段(7-24GHz)
       - 场景: 室内环境(可比拟座舱尺寸)
       - 输出: 多径信道冲激响应
    
    3. 信号处理:
       - 相位提取: 从信道响应中提取相位
       - 微多普勒分析: 分离呼吸和心跳
    """
    
    # FR3频段配置
    FREQ_FR3_LOW = 7e9    # 7 GHz
    FREQ_FR3_HIGH = 24e9  # 24 GHz
    FREQ_FR3_CENTER = 15.5e9  # 中心频率
    
    # 生命体征参数
    BREATHING_RATE_ADULT = 12 / 60  # 12 bpm = 0.2 Hz
    BREATHING_RATE_CHILD = 30 / 60  # 30 bpm = 0.5 Hz
    BREATHING_DISPLACEMENT_MM = 6.0  # 成人胸壁位移
    HEART_RATE = 72 / 60  # 72 bpm = 1.2 Hz
    HEART_DISPLACEMENT_MM = 0.3  # 心跳胸壁位移
    
    def __init__(self, center_freq: float = 15.5e9, 
                 bandwidth: float = 500e6):
        self.fc = center_freq
        self.bw = bandwidth
        self.wavelength = 3e8 / center_freq  # ~1.94 cm @ 15.5 GHz
        
    def simulate_chest_displacement(
        self, duration_s: float = 30, fs: int = 100
    ) -> dict:
        """
        模拟胸壁位移（地面真值）
        
        胸壁运动 = 呼吸正弦 + 心跳正弦 + 随机噪声
        
        Args:
            duration_s: 模拟时长(秒)
            fs: 采样率(Hz)
            
        Returns:
            dict: 位移时间序列 + 频率成分
        """
        t = np.arange(0, duration_s, 1/fs)
        
        # 呼吸: 正弦 + 谐波
        breathing = (
            self.BREATHING_DISPLACEMENT_MM * np.sin(
                2 * np.pi * self.BREATHING_RATE_ADULT * t
            )
            + 1.5 * np.sin(4 * np.pi * self.BREATHING_RATE_ADULT * t)  # 二次谐波
        )
        
        # 心跳: 更高频微小位移
        heartbeat = (
            self.HEART_DISPLACEMENT_MM * np.sin(
                2 * np.pi * self.HEART_RATE * t
            )
            + 0.1 * np.sin(4 * np.pi * self.HEART_RATE * t)  # 心跳二次谐波
        )
        
        # 总位移
        total_displacement = breathing + heartbeat
        noise = np.random.normal(0, 0.05, len(t))  # 0.05mm噪声
        total_displacement += noise
        
        return {
            'time': t,
            'displacement_mm': total_displacement,
            'breathing_mm': breathing,
            'heartbeat_mm': heartbeat,
            'fs': fs,
        }
    
    def displacement_to_phase(
        self, displacement_mm: np.ndarray
    ) -> np.ndarray:
        """
        胸壁位移 → 雷达相位变化
        
        相位变化 = 4π × d / λ
        (往返路径, d = 位移, λ = 波长)
        
        FR3 @ 15.5 GHz: λ ≈ 1.94 cm
        呼吸 6mm → 相位 ≈ 4π × 6/19.4 ≈ 3.88 rad
        心跳 0.3mm → 相位 ≈ 4π × 0.3/19.4 ≈ 0.19 rad
        """
        phase = 4 * np.pi * displacement_mm / (self.wavelength * 1e3)
        return phase
    
    def extract_vital_signs(
        self, phase_signal: np.ndarray, fs: int = 100
    ) -> dict:
        """
        从相位信号中提取呼吸和心跳
        
        方法：
        1. 去趋势（去除DC分量）
        2. 带通滤波分离呼吸和心跳
        3. FFT峰值检测
        """
        # 去趋势
        phase_detrend = phase_signal - np.mean(phase_signal)
        
        # 呼吸带通: 0.1-0.5 Hz (6-30 bpm)
        nyq = fs / 2
        b_br, a_br = butter(4, [0.1/nyq, 0.5/nyq], btype='band')
        breathing = filtfilt(b_br, a_br, phase_detrend)
        
        # 心跳带通: 0.8-2.0 Hz (48-120 bpm)
        b_hr, a_hr = butter(4, [0.8/nyq, 2.0/nyq], btype='band')
        heartbeat = filtfilt(b_hr, a_hr, phase_detrend)
        
        # FFT频率估计
        fft_br = np.fft.rfft(breathing)
        freqs_br = np.fft.rfftfreq(len(breathing), 1/fs)
        br_rate = freqs_br[np.argmax(np.abs(fft_br))] * 60  # bpm
        
        fft_hr = np.fft.rfft(heartbeat)
        freqs_hr = np.fft.rfftfreq(len(heartbeat), 1/fs)
        hr_mask = (freqs_hr >= 0.8) & (freqs_hr <= 2.0)
        hr_rate = freqs_hr[hr_mask][
            np.argmax(np.abs(fft_hr[hr_mask]))
        ] * 60  # bpm
        
        # 误差计算
        br_error = abs(br_rate - self.BREATHING_RATE_ADULT * 60)
        hr_error = abs(hr_rate - self.HEART_RATE * 60)
        
        return {
            'breathing_rate_bpm': br_rate,
            'heart_rate_bpm': hr_rate,
            'breathing_error_bpm': br_error,
            'heart_rate_error_bpm': hr_error,
            'breathing_signal': breathing,
            'heartbeat_signal': heartbeat,
        }


# ==================== 测试 ====================
if __name__ == "__main__":
    sim = RTVSSSimulator(center_freq=15.5e9, bandwidth=500e6)
    
    print("=== RT-VSS FR3 生命体征感知仿真 ===")
    print(f"中心频率: {sim.fc/1e9:.1f} GHz")
    print(f"波长: {sim.wavelength*100:.2f} cm")
    print(f"带宽: {sim.bw/1e6:.0f} MHz\n")
    
    # 1. 模拟胸壁位移
    gt = sim.simulate_chest_displacement(duration_s=30, fs=100)
    print(f"地面真值:")
    print(f"  呼吸频率: {sim.BREATHING_RATE_ADULT*60:.0f} bpm")
    print(f"  心率: {sim.HEART_RATE*60:.0f} bpm")
    print(f"  呼吸位移: {sim.BREATHING_DISPLACEMENT_MM} mm")
    print(f"  心跳位移: {sim.HEART_DISPLACEMENT_MM} mm")
    
    # 2. 位移→相位
    phase = sim.displacement_to_phase(gt['displacement_mm'])
    print(f"\n相位变化范围: {np.max(phase):.2f} rad")
    print(f"呼吸相位幅度: {4*np.pi*sim.BREATHING_DISPLACEMENT_MM/(sim.wavelength*1e3):.2f} rad")
    print(f"心跳相位幅度: {4*np.pi*sim.HEART_DISPLACEMENT_MM/(sim.wavelength*1e3):.4f} rad")
    
    # 3. 提取生命体征
    result = sim.extract_vital_signs(phase, fs=100)
    print(f"\n提取结果:")
    print(f"  呼吸频率: {result['breathing_rate_bpm']:.2f} bpm (误差: {result['breathing_error_bpm']:.2f})")
    print(f"  心率: {result['heart_rate_bpm']:.2f} bpm (误差: {result['heart_rate_error_bpm']:.2f})")
    
    # 4. 跨频段对比
    print(f"\n=== 跨频段相位灵敏度对比 ===")
    freqs = {
        'FR1 (2.4 GHz)': 2.4e9,
        'FR3 (7 GHz)': 7e9,
        'FR3 (15.5 GHz)': 15.5e9,
        'FR3 (24 GHz)': 24e9,
        'FR2 (60 GHz)': 60e9,
    }
    for name, freq in freqs.items():
        wavelength = 3e8 / freq * 100  # cm
        breath_phase = 4 * np.pi * 6.0 / (wavelength * 10)  # rad
        heart_phase = 4 * np.pi * 0.3 / (wavelength * 10)
        print(f"  {name}: λ={wavelength:.2f}cm, "
              f"呼吸相位={breath_phase:.2f}rad, "
              f"心跳相位={heart_phase:.4f}rad")