import numpy as np
from dataclasses import dataclass, field
from typing import List, Optional
import time

"""
IMS 座舱传感器融合系统
整合 TI AWRL6844 雷达 + 摄像头 + 座椅压力

参考:
- TI AWRL6844 (57-64GHz 4T4R, EDN Japan 2026-08)
- HackerNoon 摄像头标定 (2026-08)
"""

@dataclass
class RadarDetection:
    """雷达检测目标"""
    range_m: float
    angle_deg: float
    velocity_mps: float
    rcs_dbsm: float  # 雷达散射截面
    micro_doppler_hz: float  # 微多普勒频率
    
    @property
    def is_breathing(self) -> bool:
        """检测到呼吸"""
        return 0.1 < self.micro_doppler_hz < 1.0
    
    @property
    def is_heartbeat(self) -> bool:
        """检测到心跳"""
        return 1.0 < self.micro_doppler_hz < 2.0


@dataclass
class CameraDetection:
    """摄像头检测目标"""
    bbox: tuple  # (x, y, w, h)
    class_name: str
    confidence: float
    keypoint_3d: Optional[np.ndarray] = None  # 3D 关键点


@dataclass
class SeatPressureData:
    """座椅压力数据"""
    pressure_map: np.ndarray  # 压力分布矩阵
    total_weight_kg: float
    distribution_type: str  # 'adult', 'child', 'empty', 'object'
    
    @property
    def is_occupied(self) -> bool:
        return self.total_weight_kg > 2.0


@dataclass
class FusedResult:
    """融合结果"""
    timestamp: float
    occupant_type: str  # 'adult', 'child', 'infant', 'empty'
    position: tuple  # (x, y, z) in meters
    breathing: bool
    heartbeat: bool
    posture: str  # 'normal', 'leaning', 'sleeping', 'oop'
    seatbelt: str  # 'fastened', 'unfastened', 'misuse'
    confidence: float
    sensors_used: list


class CabinSensorFusion:
    """座舱传感器融合引擎"""
    
    def __init__(self):
        # 传感器权重
        self.weights = {
            'radar': 0.30,    # 雷达：穿透/暗光/微动
            'camera': 0.40,   # 摄像头：分类/姿态/视觉
            'pressure': 0.20, # 压力：重量/分布
            'temp': 0.10      # 温度：辅助
        }
        self.detection_history: list = []
    
    def _fuse_radar_camera(self, radar_dets: List[RadarDetection],
                           camera_dets: List[CameraDetection],
                           seat_data: SeatPressureData) -> FusedResult:
        """融合雷达+摄像头+座椅压力"""
        
        # 座椅占用判断
        if not seat_data.is_occupied:
            return FusedResult(
                timestamp=time.time(),
                occupant_type='empty',
                position=(0, 0, 0),
                breathing=False,
                heartbeat=False,
                posture='empty',
                seatbelt='n/a',
                confidence=0.95,
                sensors_used=['pressure']
            )
        
        # 重量分类
        w = seat_data.total_weight_kg
        if w < 5:
            weight_type = 'infant'
        elif w < 25:
            weight_type = 'child'
        else:
            weight_type = 'adult'
        
        # 雷达呼吸/心跳
        breathing = any(r.is_breathing for r in radar_dets)
        heartbeat = any(r.is_heartbeat for r in radar_dets)
        
        # 摄像头分类
        cam_classes = [d.class_name for d in camera_dets]
        cam_confidence = max((d.confidence for d in camera_dets), default=0)
        
        # 融合决策
        if weight_type == 'infant' and breathing:
            occupant_type = 'infant'
            posture = 'seated_rear'
            confidence = 0.90
        elif weight_type == 'child' and breathing:
            occupant_type = 'child'
            posture = 'seated_rear'
            confidence = 0.88
        elif weight_type == 'adult':
            occupant_type = 'adult'
            # 姿态判断
            if camera_dets:
                kp = camera_dets[0].keypoint_3d
                if kp is not None:
                    shoulder_angle = np.mean(kp[5:11, 1]) - np.mean(kp[11:17, 1])
                    if abs(shoulder_angle) > 0.3:
                        posture = 'leaning'
                    elif not breathing and not heartbeat:
                        posture = 'sleeping'
                    else:
                        posture = 'normal'
                else:
                    posture = 'normal'
            else:
                posture = 'normal'
            confidence = 0.92
        else:
            occupant_type = weight_type
            posture = 'unknown'
            confidence = 0.70
        
        # 安全带状态（简化）
        seatbelt = 'fastened' if breathing and w > 25 else 'unknown'
        
        # 融合置信度
        total_conf = (
            self.weights['radar'] * (0.9 if breathing else 0.5) +
            self.weights['camera'] * cam_confidence +
            self.weights['pressure'] * (0.95 if w > 0 else 0.3) +
            self.weights['temp'] * 0.8
        )
        
        sensors = ['radar', 'camera', 'pressure']
        if breathing:
            sensors.append('breathing')
        if heartbeat:
            sensors.append('heartbeat')
        
        return FusedResult(
            timestamp=time.time(),
            occupant_type=occupant_type,
            position=(0.5, 0.8, 0.3),
            breathing=breathing,
            heartbeat=heartbeat,
            posture=posture,
            seatbelt=seatbelt,
            confidence=round(total_conf, 3),
            sensors_used=sensors
        )
    
    def process(self, radar_dets: List[RadarDetection],
                camera_dets: List[CameraDetection],
                seat_data: SeatPressureData) -> FusedResult:
        """处理一帧"""
        result = self._fuse_radar_camera(radar_dets, camera_dets, seat_data)
        self.detection_history.append(result)
        return result


# ==================== 测试 ====================
if __name__ == "__main__":
    np.random.seed(42)
    fusion = CabinSensorFusion()
    
    print("=" * 70)
    print("IMS 座舱传感器融合系统测试")
    print("=" * 70)
    
    scenarios = [
        {
            'name': '成人正常驾驶',
            'radar': [RadarDetection(1.0, 0, 0.01, -15, 0.3)],
            'camera': [CameraDetection((200, 100, 300, 400), 'adult', 0.95)],
            'seat': SeatPressureData(np.random.rand(8, 8) * 10, 68.5, 'adult')
        },
        {
            'name': '1岁儿童后座',
            'radar': [RadarDetection(0.8, -30, 0.005, -25, 0.4)],
            'camera': [CameraDetection((400, 200, 150, 200), 'child', 0.88)],
            'seat': SeatPressureData(np.random.rand(8, 8) * 3, 9.2, 'child')
        },
        {
            'name': '新生儿 Isofix',
            'radar': [RadarDetection(0.6, -35, 0.002, -30, 0.35)],
            'camera': [CameraDetection((380, 220, 100, 150), 'infant', 0.75)],
            'seat': SeatPressureData(np.random.rand(8, 8) * 0.5, 3.5, 'object')
        },
        {
            'name': '空座',
            'radar': [],
            'camera': [],
            'seat': SeatPressureData(np.zeros((8, 8)), 0, 'empty')
        },
        {
            'name': '成人疲劳驾驶',
            'radar': [RadarDetection(1.0, 0, 0.003, -15, 0.25)],
            'camera': [CameraDetection((200, 100, 300, 400), 'adult', 0.92,
                np.random.randn(17, 3) * 0.1 + 0.5)],
            'seat': SeatPressureData(np.random.rand(8, 8) * 10, 72.0, 'adult')
        },
    ]
    
    print(f"\n{'场景':<20} {'乘员类型':>10} {'姿态':>12} {'呼吸':>6} "
          f"{'心跳':>6} {'安全带':>10} {'置信度':>8} {'传感器':>20}")
    print("-" * 95)
    
    for s in scenarios:
        result = fusion.process(s['radar'], s['camera'], s['seat'])
        
        print(f"{s['name']:<20} {result.occupant_type:>10} "
              f"{result.posture:>12} "
              f"{'✅' if result.breathing else '❌':>6} "
              f"{'✅' if result.heartbeat else '❌':>6} "
              f"{result.seatbelt:>10} "
              f"{result.confidence:>8.3f} "
              f"{','.join(result.sensors_used):>20}")
    
    print(f"\n{'='*70}")
    print("传感器对比:")
    print(f"  {'传感器':<15} {'TI AWRL6844':>15} {'摄像头':>15} {'座椅压力':>15}")
    print(f"  {'暗光':<15} {'✅':>15} {'❌':>15} {'✅':>15}")
    print(f"  {'穿透遮挡':<15} {'✅ 部分':>15} {'❌':>15} {'✅':>15}")
    print(f"  {'分类能力':<15} {'❌':>15} {'✅ 强':>15} {'⚠️ 重量级':>15}")
    print(f"  {'呼吸检测':<15} {'✅':>15} {'⚠️ 间接':>15} {'❌':>15}")
    print(f"  {'姿态估计':<15} {'❌':>15} {'✅ 3D':>15} {'⚠️ 分布':>15}")
    print(f"  {'成本':<15} {'$5-15':>15} {'$20-50':>15} {'$10-30':>15}")
    print(f"  {'功耗':<15} {'<2W':>15} {'2-5W':>15} {'<1W':>15}")
    print(f"{'='*70}")