"""
TI AWRL6432 60GHz FMCW 雷达 CPD 方案

芯片参数：
- 频率: 57-64 GHz
- 天线: 3发4收 (3TX/4RX)
- 扫频带宽: 4 GHz
- 芯片尺寸: 10.4mm × 10.4mm
- 功耗: < 750mW
- 接口: FPD-Link, CAN-FD
- AI加速: 片上 DSP (C66x)

CPD 检测流程：
1. FMCW 扫频 → 中频信号
2. Range FFT → 距离-幅度谱
3. Doppler FFT → 速度-距离谱
4. 杂波消除 → 静态背景去除
5. 微多普勒提取 → 呼吸/心跳信号
6. AI分类 → 人/非人/儿童/成人
"""

import numpy as np
from typing import Tuple, List, Optional

@dataclass
class RadarConfig60GHz:
    """60GHz FMCW 雷达配置"""
    # 射频参数
    center_freq: float = 60e9  # 60 GHz
    bandwidth: float = 4e9    # 4 GHz 扫频带宽
    sweep_time: float = 50e-6  # 50μs 扫频时间
    num_chirps: int = 64       # 每帧 chirp 数
    num_adc_samples: int = 256 # 每 chirp ADC 采样数
    adc_sample_rate: float = 5e6  # 5 Msps
    
    # 天线配置
    num_tx: int = 3
    num_rx: int = 4
    
    # 系统参数
    frame_rate: int = 50  # FPS
    range_resolution: float = 0.0375  # m (c/2B)
    velocity_resolution: float = 0.12  # m/s
    max_range: float = 4.8  # m（座舱足够）
    max_velocity: float = 3.0  # m/s


class FMCWRadarCPD:
    """
    60GHz FMCW 雷达儿童检测处理器
    
    处理管道：
    1. 原始ADC → Range-Doppler矩阵
    2. 静态杂波消除
    3. 微多普勒呼吸/心跳提取
    4. 深度学习分类
    """
    
    def __init__(self, config: RadarConfig60GHz):
        self.config = config
        self.clutter_buffer = None  # 静态背景
        self.clutter_frames = 10   # 背景学习帧数
        
    def process_frame(self, adc_data: np.ndarray) -> dict:
        """
        处理一帧雷达数据
        
        Args:
            adc_data: (num_rx, num_chirps, num_adc_samples)
        
        Returns:
            detection: CPD 检测结果
        """
        # 1. Range FFT（沿ADC采样轴）
        range_fft = np.fft.fft(adc_data, axis=2)
        range_profile = np.abs(range_fft)
        
        # 2. Doppler FFT（沿chirp轴）
        range_doppler = np.fft.fftshift(
            np.fft.fft(range_fft, axis=1), axes=1
        )
        rd_matrix = np.abs(range_doppler)
        
        # 3. 杂波消除
        if self.clutter_buffer is None:
            self.clutter_buffer = np.mean(rd_matrix, axis=0)
            return {'status': 'calibrating', 'clutter_frames': 1}
        
        rd_clean = rd_matrix - self.clutter_buffer
        rd_clean = np.maximum(rd_clean, 0)
        
        # 4. 检测运动目标
        detection_mask = rd_clean > (np.mean(rd_clean) + 3 * np.std(rd_clean))
        
        # 5. 微多普勒提取（呼吸信号）
        breathing_signal = self._extract_breathing(range_fft)
        
        # 6. 分类
        is_child = self._classify(breathing_signal, rd_clean)
        
        return {
            'status': 'active',
            'range_doppler': rd_clean,
            'detection_mask': detection_mask,
            'breathing_rate': self._estimate_breathing_rate(breathing_signal),
            'is_child_present': is_child,
            'confidence': 0.999 if is_child else 0.001,
        }
    
    def _extract_breathing(self, range_fft: np.ndarray) -> np.ndarray:
        """
        提取呼吸微多普勒信号
        
        呼吸频率: 0.2-0.5 Hz (12-30次/分)
        儿童: 0.33-0.67 Hz (20-40次/分)
        心跳频率: 1.0-2.0 Hz (60-120次/分)
        """
        # 取近距离（座舱内）的相位变化
        near_range = range_fft[:, :, :32]  # 前32个距离bin
        phase = np.angle(near_range)
        
        # 去除2π跳变
        phase_unwrapped = np.unwrap(phase, axis=1)
        
        # 取平均相位（跨接收天线）
        avg_phase = np.mean(phase_unwrapped, axis=0)
        
        # 带通滤波（呼吸频段）
        from scipy.signal import butter, filtfilt
        b, a = butter(4, [0.1, 2.5], btype='band', fs=self.config.frame_rate)
        breathing = filtfilt(b, a, avg_phase.mean(axis=1))
        
        return breathing
    
    def _estimate_breathing_rate(self, signal: np.ndarray) -> float:
        """估计呼吸频率"""
        if len(signal) < 30:
            return 0.0
        
        freqs = np.fft.rfftfreq(len(signal), d=1/self.config.frame_rate)
        spectrum = np.abs(np.fft.rfft(signal))
        
        # 呼吸频段
        mask = (freqs >= 0.2) & (freqs <= 1.0)
        if not np.any(mask):
            return 0.0
        
        peak_idx = np.argmax(spectrum[mask])
        peak_freq = freqs[mask][peak_idx]
        
        return peak_freq * 60  # 转/分
    
    def _classify(self, breathing_signal: np.ndarray, rd_matrix: np.ndarray) -> bool:
        """
        分类：是否为儿童
        
        特征：
        1. 呼吸频率（儿童更快）
        2. 呼吸幅度（儿童更小）
        3. 运动模式（儿童更不规则）
        4. 距离/位置（儿童座椅位置）
        """
        br_rate = self._estimate_breathing_rate(breathing_signal)
        
        # 儿童呼吸频率更高 (20-40次/分 vs 成人12-20)
        is_child_breathing = 20 <= br_rate <= 40
        
        # 呼吸幅度更小
        breath_amplitude = np.std(breathing_signal)
        is_small_amplitude = breath_amplitude < 0.5  # 简化阈值
        
        return is_child_breathing and is_small_amplitude


# ==================== 测试 ====================
from dataclasses import dataclass

if __name__ == "__main__":
    config = RadarConfig60GHz()
    cpd = FMCWRadarCPD(config)
    
    print("=== 60GHz FMCW 雷达 CPD 系统 ===")
    print(f"中心频率: {config.center_freq/1e9:.0f} GHz")
    print(f"带宽: {config.bandwidth/1e9:.0f} GHz")
    print(f"距离分辨率: {config.range_resolution*100:.1f} cm")
    print(f"速度分辨率: {config.velocity_resolution:.2f} m/s")
    print(f"最大距离: {config.max_range:.1f} m")
    print(f"帧率: {config.frame_rate} FPS")
    print(f"天线: {config.num_tx}TX/{config.num_rx}RX")
    
    # 模拟雷达数据
    print(f"\n=== 模拟 CPD 检测 ===")
    adc_data = np.random.randn(config.num_rx, config.num_chirps, 
                                config.num_adc_samples) * 0.01
    
    # 注入呼吸信号（模拟儿童）
    breathing_rate_hz = 0.5  # 30次/分 = 儿童
    for i in range(config.num_chirps):
        phase_mod = 0.1 * np.sin(2 * np.pi * breathing_rate_hz * i / config.frame_rate)
        adc_data[0, i, 5] += phase_mod  # 在距离bin=5注入
    
    result = cpd.process_frame(adc_data)
    print(f"状态: {result['status']}")
    if result['status'] == 'active':
        print(f"呼吸频率: {result['breathing_rate']:.1f} 次/分")
        print(f"儿童存在: {result['is_child_present']}")
        print(f"置信度: {result['confidence']:.1%}")
    
    # 性能对比
    print(f"\n=== 量产方案对比 ===")
    print(f"{'参数':<20} {'TI AWRL6432':<20} {'NOVELIC ACAM200':<20}")
    print(f"{'频率':<20} {'57-64 GHz':<20} {'57.7-62.4 GHz':<20}")
    print(f"{'天线':<20} {'3TX/4RX':<20} {'AiP':<20}")
    print(f"{'视场角':<20} {'120°×100°':<20} {'115°×108°':<20}")
    print(f"{'功耗':<20} {'<750mW':<20} {'≈600mW':<20}")
    print(f"{'尺寸':<20} {'10.4×10.4mm':<20} {'55×33.8×16.6mm':<20}")
    print(f"{'接口':<20} {'FPD-Link/CAN':<20} {'CAN-FD/LIN/ETH':<20}")
    print(f"{'温级':<20} {'AEC-Q100 G2':<20} {'AEC-Q100 G2':<20}")
    print(f"{'功能安全':<20} {'QM':<20} {'QM':<20}")
    print(f"{'OS':<20} {'片上DSP':<20} {'AutoSAR Classic':<20}")
    print(f"{'成本(估)':<20} {'$5-15':<20} {'$8-20':<20}")