"""
NOVELIC + Smart Eye Camera-Radar 融合方案
核心逻辑复现

关键设计：
1. 两个传感器独立运行，各自输出检测结果
2. 融合层进行冲突仲裁和置信度加权
3. 最终输出统一检测结果到 CAN-Bus
"""

from dataclasses import dataclass
from typing import Optional
import numpy as np


@dataclass
class CameraDetection:
    """摄像头检测结果"""
    seat_occupied: bool = False
    person_type: str = ""  # adult, child, empty, pet
    confidence: float = 0.0
    fatigue_level: float = 0.0  # 0-1
    distraction_level: float = 0.0  # 0-1
    gaze_direction: tuple = (0.0, 0.0)  # (pitch, yaw) in degrees
    alcohol_impairment: float = 0.0  # 0-1
    emotion: str = "neutral"


@dataclass
class RadarDetection:
    """60GHz 雷达检测结果"""
    seat_occupied: bool = False
    presence_confidence: float = 0.0
    vital_signs_breathing: float = 0.0  # breaths per minute
    vital_signs_heartbeat: float = 0.0  # beats per minute
    micro_motion_detected: bool = False
    range_profile: np.ndarray = None  # range-Doppler profile
    intrusion_detected: bool = False


class SensorFusionEngine:
    """
    Camera-Radar 传感器融合引擎
    
    策略：
    - Camera 主导：疲劳、分心、视线、酒驾、情绪
    - Radar 主导：CPD（遮挡场景）、生命体征、座椅占用
    - 双重确认：无遮挡 CPD、座椅占用
    - 冲突仲裁：当两个传感器结果矛盾时，根据场景选择可信度更高的
    """
    
    def __init__(self):
        # 场景可信度权重
        self.camera_dominant_scenes = {
            'fatigue', 'distraction', 'gaze', 
            'alcohol', 'emotion', 'face_occluded'
        }
        self.radar_dominant_scenes = {
            'cpd_occluded', 'vital_signs', 'seat_occupancy',
            'intrusion', 'dark_environment'
        }
    
    def fuse_occupancy(self, cam: CameraDetection, 
                       radar: RadarDetection) -> dict:
        """
        融合座椅占用检测
        
        冲突仲裁逻辑：
        - Camera 检测到人 + Radar 检测到生命体征 → 确认占用
        - Camera 未检测到 + Radar 检测到生命体征 → 可能遮挡，Radar 主导
        - Camera 检测到人 + Radar 未检测到 → 可能是无生命物体，需进一步判断
        """
        result = {
            'seat_occupied': False,
            'person_type': 'empty',
            'confidence': 0.0,
            'source': 'none'
        }
        
        cam_says_occupied = cam.seat_occupied and cam.confidence > 0.5
        radar_says_occupied = radar.seat_occupied and radar.presence_confidence > 0.5
        
        if cam_says_occupied and radar_says_occupied:
            # 双重确认
            result['seat_occupied'] = True
            result['person_type'] = cam.person_type
            result['confidence'] = min(
                (cam.confidence + radar.presence_confidence) / 2 + 0.1, 1.0
            )
            result['source'] = 'both'
            
        elif not cam_says_occupied and radar_says_occupied:
            # Camera 看不到（可能被遮挡），Radar 检测到生命体征
            # Radar 主导
            result['seat_occupied'] = True
            result['person_type'] = 'child'  # 被遮挡的通常是儿童
            result['confidence'] = radar.presence_confidence
            result['source'] = 'radar'
            
        elif cam_says_occupied and not radar_says_occupied:
            # Camera 看到但 Radar 没检测到生命体征
            # 可能是无生命物体（如包包）
            if radar.vital_signs_breathing > 0:
                # 有呼吸信号但 presence 低
                result['seat_occupied'] = True
                result['person_type'] = cam.person_type
                result['confidence'] = cam.confidence * 0.7
                result['source'] = 'camera+radar_vital'
            else:
                # 无生命体征，可能是物体
                result['seat_occupied'] = False
                result['person_type'] = 'object'
                result['confidence'] = 0.6
                result['source'] = 'radar_veto'
        
        return result
    
    def fuse_cpd(self, cam: CameraDetection, 
                 radar: RadarDetection) -> dict:
        """
        融合儿童遗留检测 (CPD)
        
        Euro NCAP CPD 要求：
        - 检测后座是否有儿童/婴儿遗留
        - 必须穿透遮挡（毯子、安全座椅等）
        - 误报率 < 1 次/1000 次检测
        """
        result = {
            'child_detected': False,
            'confidence': 0.0,
            'breathing_confirmed': False,
            'source': 'none'
        }
        
        # Radar 检测到呼吸信号
        radar_breathing = radar.vital_signs_breathing > 0
        radar_micro_motion = radar.micro_motion_detected
        
        # Camera 检测到儿童
        cam_child = cam.person_type == 'child' and cam.confidence > 0.5
        
        if cam_child and radar_breathing:
            # 双重确认：儿童 + 呼吸
            result['child_detected'] = True
            result['confidence'] = 0.95
            result['breathing_confirmed'] = True
            result['source'] = 'both'
            
        elif not cam_child and radar_breathing:
            # Camera 看不到（遮挡），但 Radar 检测到呼吸
            # 这是 Radar 的核心价值场景
            result['child_detected'] = True
            result['confidence'] = 0.85
            result['breathing_confirmed'] = True
            result['source'] = 'radar'
            
        elif cam_child and not radar_breathing:
            # Camera 看到儿童但 Radar 没检测到呼吸
            # 可能是儿童座椅被误判为儿童
            result['child_detected'] = False
            result['confidence'] = 0.5
            result['source'] = 'radar_veto'
        
        return result


# ========== 测试用例 ==========

if __name__ == "__main__":
    engine = SensorFusionEngine()
    
    # 场景1：正常座位，Camera 和 Radar 都检测到
    cam1 = CameraDetection(
        seat_occupied=True, person_type='adult', confidence=0.95
    )
    radar1 = RadarDetection(
        seat_occupied=True, presence_confidence=0.92,
        vital_signs_breathing=16, vital_signs_heartbeat=72,
        micro_motion_detected=True
    )
    result1 = engine.fuse_occupancy(cam1, radar1)
    print(f"场景1 - 正常占用:")
    print(f"  占用: {result1['seat_occupied']}, "
          f"类型: {result1['person_type']}, "
          f"置信度: {result1['confidence']:.2f}, "
          f"来源: {result1['source']}")
    
    # 场景2：儿童被毯子遮挡（Camera 看不到）
    cam2 = CameraDetection(
        seat_occupied=False, person_type='empty', confidence=0.3
    )
    radar2 = RadarDetection(
        seat_occupied=True, presence_confidence=0.88,
        vital_signs_breathing=28, vital_signs_heartbeat=120,
        micro_motion_detected=True
    )
    result2 = engine.fuse_cpd(cam2, radar2)
    print(f"\n场景2 - CPD（遮挡）:")
    print(f"  儿童检测: {result2['child_detected']}, "
          f"置信度: {result2['confidence']:.2f}, "
          f"呼吸确认: {result2['breathing_confirmed']}, "
          f"来源: {result2['source']}")
    
    # 场景3：安全座椅被误判
    cam3 = CameraDetection(
        seat_occupied=True, person_type='child', confidence=0.6
    )
    radar3 = RadarDetection(
        seat_occupied=True, presence_confidence=0.4,
        vital_signs_breathing=0, vital_signs_heartbeat=0,
        micro_motion_detected=False
    )
    result3 = engine.fuse_cpd(cam3, radar3)
    print(f"\n场景3 - 安全座椅误判:")
    print(f"  儿童检测: {result3['child_detected']}, "
          f"置信度: {result3['confidence']:.2f}, "
          f"来源: {result3['source']}")

"""
CAN-Bus 消息格式定义
基于 NOVELIC + Smart Eye 方案的通信协议参考
"""

# CAN ID 分配
CAN_ID_DMS_STATUS = 0x710      # DMS 状态消息
CAN_ID_CPD_ALERT = 0x711       # CPD 告警
CAN_ID_OCCUPANT = 0x712        # 乘员信息
CAN_ID_VITAL_SIGNS = 0x713     # 生命体征
CAN_ID_FUSION_RESULT = 0x714   # 融合结果

# DMS 状态消息 (8 bytes)
# Byte 0: 状态标志位
#   bit 0: 驾驶员在场
#   bit 1: 疲劳警告 L1
#   bit 2: 疲劳警告 L2
#   bit 3: 分心警告 L1
#   bit 4: 分心警告 L2
#   bit 5: 酒驾警告
#   bit 6: 突发疾病
#   bit 7: 保留
# Byte 1: PERCLOS 值 (0-100, 0.39% per bit)
# Byte 2: 视线偏离角度 (0-180, 0.7° per bit)
# Byte 3: 眨眼频率 (0-63, 1 次/分 per bit)
# Byte 4-5: 功能就绪度评分 (0-10000, 0.01 per bit)
# Byte 6: 情绪状态 (0=neutral, 1=happy, 2=angry, 3=sad, 4=stressed)
# Byte 7: 校验和

DMS_STATUS_FORMAT = {
    'byte_0': 'status_flags',
    'byte_1': 'perclos',
    'byte_2': 'gaze_deviation',
    'byte_3': 'blink_rate',
    'byte_4_5': 'readiness_score',
    'byte_6': 'emotion',
    'byte_7': 'checksum'
}

# CPD 告警消息 (8 bytes)
# Byte 0: 告警级别 (0=none, 1=info, 2=warning, 3=critical)
# Byte 1: 检测来源 (0=camera, 1=radar, 2=both)
# Byte 2: 呼吸频率 (0-60, 1 bpm per bit)
# Byte 3: 心率 (0-120, 2 bpm per bit)
# Byte 4-5: 置信度 (0-10000, 0.01% per bit)
# Byte 6: 位置标志 (bit0=FL, bit1=FR, bit2=RL, bit3=RR, bit4=trunk)
# Byte 7: 校验和

CPD_ALERT_FORMAT = {
    'byte_0': 'alert_level',
    'byte_1': 'detection_source',
    'byte_2': 'breathing_rate',
    'byte_3': 'heart_rate',
    'byte_4_5': 'confidence',
    'byte_6': 'seat_position',
    'byte_7': 'checksum'
}