"""
YFORE UWB CPD 系统仿真

UWB (Ultra-Wideband) 雷达特点:
- 工作频率: 6-8 GHz (UWB 频段)
- 脉冲宽度: 纳秒级
- 穿透能力: 毛毯、衣物、塑料座椅
- 分辨率: ~1cm
- 功耗: <100mW

对比 60GHz FMCW:
- 60GHz 分辨率更高 (~5mm) 但穿透差
- UWB 穿透更好但分辨率稍低
- 两者互补，UWB 更适合 CPD 全舱覆盖
"""

import numpy as np
from dataclasses import dataclass
from typing import List, Tuple

@dataclass
class RadarDetection:
    """单次雷达检测结果"""
    distance_m: float        # 距离 (米)
    angle_deg: float         # 角度 (度)
    velocity_mps: float     # 速度 (米/秒)
    rcs_db: float            # 雷达截面积 (dB)
    confidence: float        # 置信度 (0-1)

class UWBCPDSimulator:
    """
    UWB CPD 系统仿真器
    
    模拟 YFORE UWB 雷达的座舱检测场景
    """
    
    def __init__(self):
        self.detection_threshold = 0.5
        self.breathing_freq_range = (0.15, 0.5)  # Hz, 成人12-30次/分
        self.child_breathing_range = (0.3, 0.8)  # Hz, 儿童更快
        
    def detect_presence(self, radar_data: np.ndarray) -> dict:
        """
        检测乘员存在和状态
        
        Args:
            radar_data: (N, 4) [distance, angle, velocity, rcs]
        
        Returns:
            dict: 检测结果
        """
        results = {
            'occupants': [],
            'total_count': 0,
            'cpd_alert': False,
            'breathing_detected': False,
        }
        
        # 过滤有效检测点
        valid = radar_data[radar_data[:, 3] > -30]  # RCS > -30dB
        valid = valid[valid[:, 0] < 3.0]  # 距离 < 3m (座舱范围)
        
        if len(valid) == 0:
            results['total_count'] = 0
            return results
        
        # 聚类检测点 (简化版)
        # 按距离分段
        distances = valid[:, 0]
        clusters = self._cluster_by_distance(distances, threshold=0.3)
        
        for cluster_idx, cluster_points in enumerate(clusters):
            if len(cluster_points) < 3:
                continue
            
            points = valid[cluster_points]
            
            # 计算位置
            avg_dist = np.mean(points[:, 0])
            avg_angle = np.mean(points[:, 1])
            avg_velocity = np.mean(points[:, 2])
            avg_rcs = np.mean(points[:, 3])
            
            # 判断是否有呼吸运动
            velocity_variance = np.var(points[:, 2])
            breathing = abs(avg_velocity) < 0.05 and velocity_variance > 0.001
            
            # 估计体型
            if avg_rcs > -10:
                body_type = 'adult'
            elif avg_rcs > -20:
                body_type = 'child'
            else:
                body_type = 'infant/small_object'
            
            occupant = {
                'position': (avg_dist, avg_angle),
                'body_type': body_type,
                'rcs': avg_rcs,
                'breathing': breathing,
                'velocity': avg_velocity,
            }
            
            results['occupants'].append(occupant)
            results['total_count'] += 1
            
            if breathing and body_type in ['child', 'infant/small_object']:
                results['cpd_alert'] = True
                results['breathing_detected'] = True
        
        return results
    
    def _cluster_by_distance(self, distances: np.ndarray, 
                              threshold: float = 0.3) -> list:
        """按距离聚类"""
        clusters = []
        if len(distances) == 0:
            return clusters
        
        sorted_idx = np.argsort(distances)
        current_cluster = [sorted_idx[0]]
        
        for i in range(1, len(sorted_idx)):
            if distances[sorted_idx[i]] - distances[sorted_idx[i-1]] < threshold:
                current_cluster.append(sorted_idx[i])
            else:
                clusters.append(current_cluster)
                current_cluster = [sorted_idx[i]]
        
        clusters.append(current_cluster)
        return clusters


# ==================== 测试 ====================
if __name__ == "__main__":
    np.random.seed(42)
    simulator = UWBCPDSimulator()
    
    # 场景1: 后排有儿童（被毛毯遮挡）
    print("=" * 60)
    print("场景1: 后排儿童（被毛毯遮挡，仅雷达可检测）")
    print("=" * 60)
    
    # 模拟 UWB 雷达数据：后排 1.5m 处有微呼吸的儿童
    n_points = 50
    child_radar = np.column_stack([
        np.random.normal(1.5, 0.1, n_points),  # 距离
        np.random.normal(120, 10, n_points),     # 角度（后排）
        np.random.normal(0.02, 0.01, n_points),  # 极低速度（呼吸）
        np.random.normal(-15, 3, n_points),      # RCS（较小体型）
    ])
    
    result = simulator.detect_presence(child_radar)
    print(f"检测乘员数: {result['total_count']}")
    print(f"CPD 告警: {result['cpd_alert']}")
    print(f"呼吸检测: {result['breathing_detected']}")
    
    for i, occ in enumerate(result['occupants']):
        print(f"  乘员{i+1}: {occ['body_type']}, "
              f"位置=({occ['position'][0]:.1f}m, {occ['position'][1]:.0f}°), "
              f"RCS={occ['rcs']:.1f}dB, 呼吸={occ['breathing']}")
    
    # 场景2: 空座舱
    print("\n" + "=" * 60)
    print("场景2: 空座舱")
    print("=" * 60)
    
    empty_radar = np.array([]).reshape(0, 4)
    result_empty = simulator.detect_presence(empty_radar)
    print(f"检测乘员数: {result_empty['total_count']}")
    print(f"CPD 告警: {result_empty['cpd_alert']}")
    
    # 场景3: 成人驾驶员
    print("\n" + "=" * 60)
    print("场景3: 成人驾驶员正常驾驶")
    print("=" * 60)
    
    adult_radar = np.column_stack([
        np.random.normal(0.5, 0.05, 30),
        np.random.normal(0, 5, 30),
        np.random.normal(0.1, 0.05, 30),  # 有运动
        np.random.normal(-5, 2, 30),      # 较大 RCS
    ])
    
    result_adult = simulator.detect_presence(adult_radar)
    print(f"检测乘员数: {result_adult['total_count']}")
    print(f"CPD 告警: {result_adult['cpd_alert']}")
    
    for i, occ in enumerate(result_adult['occupants']):
        print(f"  乘员{i+1}: {occ['body_type']}, 呼吸={occ['breathing']}")
    
    print("\n" + "=" * 60)
    print("YFORE UWB CPD 关键参数")
    print("=" * 60)
    print("生命检测率: >99%")
    print("Euro NCAP 响应时间: <13s ✅")
    print("窗户信号泄漏: <20cm ✅")
    print("穿透能力: 毛毯/衣物/座椅靠背 ✅")