"""
镜摄融合 + 雷达全舱感知融合算法框架
"""

import numpy as np
from typing import Dict, Optional

class MirrorRadarFusion:
    """
    后视镜摄像头 + 舱内雷达融合
    
    融合策略:
    1. 摄像头: 前排视觉特征 (面部/眼睛/手势/姿态)
    2. 雷达: 全舱存在/呼吸/位置
    3. 冲突仲裁: 雷达 > 摄像头 (穿透场景)
    """
    
    def __init__(self):
        self.camera_confidence = 0.0
        self.radar_confidence = 0.0
        self.fused_result = {}
        
    def fuse(self, camera_data: Dict, radar_data: Dict):
        """
        Args:
            camera_data: {
                'driver_face': {...},
                'driver_gaze': (x, y),
                'driver_eyes_open': bool,
                'phone_detected': bool,
                'seatbelt_visible': bool,
                'front_passenger': {...}
            }
            radar_data: {
                'occupant_positions': [(x, y, z), ...],
                'breathing_detected': [bool, ...],
                'breathing_rate': [float, ...],
                'total_occupants': int
            }
        Returns:
            fused: 综合感知结果
        """
        fused = {}
        
        # 1. 驾驶员状态 (摄像头主导)
        fused['driver_state'] = self._assess_driver(camera_data, radar_data)
        
        # 2. 乘员计数 (雷达主导)
        fused['occupant_count'] = radar_data.get('total_occupants', 0)
        # 摄像头可验证前排乘员
        if camera_data.get('front_passenger'):
            fused['front_passenger_confirmed'] = True
        
        # 3. CPD 生命检测 (雷达主导)
        breathing = radar_data.get('breathing_detected', [])
        fused['life_detected'] = any(breathing)
        fused['life_locations'] = [
            pos for pos, breath in zip(
                radar_data.get('occupant_positions', []),
                breathing
            ) if breath
        ]
        
        # 4. 安全带状态 (视觉+雷达确认)
        if camera_data.get('seatbelt_visible'):
            fused['seatbelt_status'] = 'visible_buckled'
        elif any(radar_data.get('breathing_detected', [])):
            fused['seatbelt_status'] = 'occupant_belt_uncertain'
        else:
            fused['seatbelt_status'] = 'empty_seat'
        
        # 5. 手机使用 (摄像头主导)
        fused['phone_use'] = camera_data.get('phone_detected', False)
        
        return fused
    
    def _assess_driver(self, cam, rad):
        """评估驾驶员状态"""
        state = {
            'fatigue': None,
            'distraction': None,
            'unresponsive': None
        }
        
        # 疲劳: 眼睛闭合度
        if not cam.get('driver_eyes_open', True):
            state['fatigue'] = 'drowsy'
        
        # 分心: 视线偏离
        gaze = cam.get('driver_gaze', (0.5, 0.5))
        if abs(gaze[0] - 0.5) > 0.3 or abs(gaze[1] - 0.5) > 0.3:
            state['distraction'] = 'visual_distraction'
        
        # 无响应: 眼闭 + 呼吸正常
        if not cam.get('driver_eyes_open', True):
            breathing = rad.get('breathing_detected', [False])[0]
            if breathing:
                state['unresponsive'] = 'unresponsive_driver'
        
        return state