"""
LiDAR vs Camera vs Radar 在座舱感知中的能力对比

LiDAR 在座舱中的独特价值：
1. 精确深度信息（不受光照影响）
2. 3D 点云可重建乘员姿态
3. 不受隐私争议（无图像记录）
4. 可穿透透明遮挡（玻璃等）
"""

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

@dataclass
class SensorCapability:
    """传感器能力评估"""
    depth_accuracy: float = 0.0  # 深度精度 (cm), 0=不可用
    angular_resolution: float = 0.0  # 角分辨率 (度)
    dark_performance: float = 0.0  # 黑暗环境性能 (0-1)
    privacy_level: float = 0.0  # 隐私保护 (0-1)
    occlusion_handling: float = 0.0  # 遮挡处理 (0-1)
    frame_rate: float = 0.0  # 帧率 (fps)
    power_consumption: float = 0.0  # 功耗 (W)


# 传感器对比
sensors = {
    'Camera (RGB-IR)': SensorCapability(
        depth_accuracy=5.0,  # 立体视觉约5cm
        angular_resolution=0.5,
        dark_performance=0.7,  # IR补光
        privacy_level=0.3,  # 有图像
        occlusion_handling=0.3,
        frame_rate=30,
        power_consumption=2.0
    ),
    '60GHz Radar': SensorCapability(
        depth_accuracy=2.0,  # 距离分辨率
        angular_resolution=5.7,
        dark_performance=1.0,
        privacy_level=1.0,
        occlusion_handling=0.8,  # 穿透遮挡
        frame_rate=20,
        power_consumption=1.0
    ),
    'LiDAR (Apollo)': SensorCapability(
        depth_accuracy=0.5,  # 高精度深度
        angular_resolution=0.1,
        dark_performance=1.0,
        privacy_level=0.9,  # 点云不泄露身份
        occlusion_handling=0.5,  # 不穿透
        frame_rate=20,
        power_consumption=8.0
    )
}

print("=== 传感器能力对比 ===")
print(f"{'传感器':<20} {'深度cm':<8} {'角度°':<8} "
      f"{'暗光':<6} {'隐私':<6} {'遮挡':<6} "
      f"{'fps':<6} {'功耗W':<6}")
print("-" * 70)
for name, cap in sensors.items():
    print(f"{name:<20} {cap.depth_accuracy:<8.1f} "
          f"{cap.angular_resolution:<8.1f} "
          f"{cap.dark_performance:<6.1f} "
          f"{cap.privacy_level:<6.1f} "
          f"{cap.occlusion_handling:<6.1f} "
          f"{cap.frame_rate:<6.0f} "
          f"{cap.power_consumption:<6.1f}")

"""
Camera + LiDAR + Radar 三传感器融合
LITEON Advanced Overhead Console 方案

融合策略：
1. Camera 主导：面部特征、表情、视线、疲劳
2. LiDAR 主导：3D 姿态、深度距离、空间关系
3. Radar 主导：CPD、生命体征、穿透遮挡
4. 三者交叉验证：乘员分类、占用确认
"""

from typing import Optional, Tuple
import numpy as np

class Point3D:
    """3D 点"""
    def __init__(self, x, y, z):
        self.x = x
        self.y = y
        self.z = z
    
    def distance_to(self, other):
        return np.sqrt(
            (self.x - other.x)**2 +
            (self.y - other.y)**2 +
            (self.z - other.z)**2
        )

class OccupantPose3D:
    """乘员 3D 姿态估计"""
    def __init__(self):
        # 关键 3D 关节点
        self.keypoints_3d = {}
        
    def estimate_from_lidar(self, point_cloud):
        """
        从 LiDAR 点云估计乘员姿态
        
        Args:
            point_cloud: np.ndarray, shape=(N, 3) XYZ
            
        Returns:
            pose_estimate: 头部位置、躯干角度等
        """
        # 聚类提取人体点云
        # 简化的聚类算法
        if len(point_cloud) < 50:
            return None
        
        # 找最高点（可能是头部）
        head_idx = np.argmin(point_cloud[:, 1])  # Y最小
        head_pos = point_cloud[head_idx]
        
        # 找中心点（可能是躯干中心）
        center = np.mean(point_cloud, axis=0)
        
        # 计算躯干角度
        torso_vector = head_pos - center
        torso_angle = np.degrees(
            np.arctan2(torso_vector[2], 
                      np.sqrt(torso_vector[0]**2 + torso_vector[1]**2))
        )
        
        return {
            'head_position': Point3D(*head_pos),
            'torso_center': Point3D(*center),
            'torso_angle_deg': torso_angle,
            'point_count': len(point_cloud),
            'confidence': min(len(point_cloud) / 200, 1.0)
        }


class TriModalFusion:
    """三模态传感器融合"""
    
    def __init__(self):
        self.pose_estimator = OccupantPose3D()
    
    def estimate_occupant_state(self,
                                camera_data: dict,
                                lidar_data: np.ndarray,
                                radar_data: dict) -> dict:
        """
        融合三种传感器估计乘员状态
        
        Args:
            camera_data: 摄像头输出
                - face_detected: bool
                - gaze_direction: (pitch, yaw)
                - fatigue_level: float
                - emotion: str
            lidar_data: LiDAR 点云, shape=(N, 3)
            radar_data: 雷达输出
                - presence: bool
                - breathing_rate: float
                - heart_rate: float
                - position: (x, y, z)
                
        Returns:
            fused_state: 融合后的乘员状态
        """
        result = {
            'occupant_present': False,
            'position_3d': None,
            'posture': 'unknown',
            'fatigue_level': 0.0,
            'gaze': (0, 0),
            'breathing_rate': 0,
            'heart_rate': 0,
            'emotion': 'neutral',
            'confidence': 0.0,
            'source': 'none'
        }
        
        # LiDAR 姿态估计
        lidar_pose = self.pose_estimator.estimate_from_lidar(
            lidar_data
        )
        
        # 融合决策
        cam_face = camera_data.get('face_detected', False)
        radar_presence = radar_data.get('presence', False)
        lidar_present = lidar_pose is not None
        
        # 三重确认
        if cam_face and radar_presence and lidar_present:
            result['occupant_present'] = True
            result['position_3d'] = (
                radar_data.get('position', (0, 0, 0))
            )
            result['posture'] = self._classify_posture(
                lidar_pose['torso_angle_deg']
            )
            result['fatigue_level'] = camera_data.get(
                'fatigue_level', 0
            )
            result['gaze'] = camera_data.get(
                'gaze_direction', (0, 0)
            )
            result['breathing_rate'] = radar_data.get(
                'breathing_rate', 0
            )
            result['heart_rate'] = radar_data.get(
                'heart_rate', 0
            )
            result['emotion'] = camera_data.get('emotion', 'neutral')
            result['confidence'] = 0.95
            result['source'] = 'all_three'
            
        elif cam_face and radar_presence:
            # Camera + Radar（无 LiDAR）
            result['occupant_present'] = True
            result['fatigue_level'] = camera_data.get(
                'fatigue_level', 0
            )
            result['breathing_rate'] = radar_data.get(
                'breathing_rate', 0
            )
            result['confidence'] = 0.85
            result['source'] = 'camera+radar'
            
        elif lidar_present and radar_presence:
            # LiDAR + Radar（Camera 看不到脸，可能是后座）
            result['occupant_present'] = True
            result['posture'] = self._classify_posture(
                lidar_pose['torso_angle_deg']
            )
            result['breathing_rate'] = radar_data.get(
                'breathing_rate', 0
            )
            result['confidence'] = 0.80
            result['source'] = 'lidar+radar'
            
        elif radar_presence:
            # 仅 Radar（严重遮挡）
            result['occupant_present'] = True
            result['breathing_rate'] = radar_data.get(
                'breathing_rate', 0
            )
            result['confidence'] = 0.70
            result['source'] = 'radar_only'
        
        return result
    
    def _classify_posture(self, torso_angle):
        """根据躯干角度分类姿态"""
        if torso_angle < 10:
            return 'upright'
        elif torso_angle < 25:
            return 'slight_recline'
        elif torso_angle < 40:
            return 'reclined'
        else:
            return 'lying_down'


# 测试
if __name__ == "__main__":
    fusion = TriModalFusion()
    
    # 场景1：驾驶员正常驾驶（三种传感器都可见）
    cam1 = {
        'face_detected': True,
        'gaze_direction': (5, -2),
        'fatigue_level': 0.15,
        'emotion': 'neutral'
    }
    lidar1 = np.random.randn(300, 3) * 0.5 + np.array([0, -0.3, 1.5])
    radar1 = {
        'presence': True,
        'breathing_rate': 16,
        'heart_rate': 72,
        'position': (0.0, -0.3, 1.5)
    }
    
    result1 = fusion.estimate_occupant_state(cam1, lidar1, radar1)
    print(f"场景1 - 驾驶员正常:")
    print(f"  存在: {result1['occupant_present']}, "
          f"姿态: {result1['posture']}, "
          f"置信度: {result1['confidence']:.2f}")
    print(f"  来源: {result1['source']}")
    
    # 场景2：后座乘客（Camera 看不到）
    cam2 = {'face_detected': False}
    lidar2 = np.random.randn(200, 3) * 0.5 + np.array([0.8, -0.2, 1.0])
    radar2 = {
        'presence': True,
        'breathing_rate': 20,
        'heart_rate': 90,
        'position': (0.8, -0.2, 1.0)
    }
    
    result2 = fusion.estimate_occupant_state(cam2, lidar2, radar2)
    print(f"\n场景2 - 后座乘客:")
    print(f"  存在: {result2['occupant_present']}, "
          f"姿态: {result2['posture']}, "
          f"置信度: {result2['confidence']:.2f}")
    print(f"  来源: {result2['source']}")