"""
座舱多传感器特征级融合

将各传感器特征拼接后输入统一模型
"""

import numpy as np
from typing import Dict, Tuple

class SensorFusionSystem:
    """
    座舱多传感器融合系统
    
    传感器:
    - DMS摄像头: 人脸关键点(68), PERCLOS, 视线方向, 眨眼率
    - 60GHz雷达: 呼吸频率, 存在检测, 位置
    - 方向盘ECG: HRV(Mean NN, RMSSD, LF/HF, W)
    - CAN总线: 方向盘操作, 车道偏移, 速度
    
    融合策略:
    - 特征级: 拼接所有特征向量
    - 决策级: 加权投票
    - 自适应: 传感器失效时自动降权
    """
    
    def __init__(self):
        self.sensor_weights = {
            "camera": 0.40,   # 视觉是主传感器
            "radar": 0.25,    # 雷达补充存在/呼吸
            "ecg": 0.20,     # ECG补充认知负荷
            "behavior": 0.15  # 行为是累积指标
        }
        
        # 特征维度
        self.feature_dims = {
            "camera": 12,  # PERCLOS, 眨眼率, 扫视熵, 瞳孔直径, 注视偏离, 闭眼时长, 6个人脸关键点偏差
            "radar": 4,    # 呼吸频率, 存在置信度, 距离, 微动强度
            "ecg": 4,      # Mean NN, RMSSD, LF/HF, W分量
            "behavior": 3   # 方向盘频率, 车道偏移, 速度波动
        }
        
        total_dim = sum(self.feature_dims.values())
        # 融合模型(简化: 使用MLP)
        self.fusion_weights = np.random.randn(total_dim, 4) * 0.1  # 4个输出等级
    
    def extract_camera_features(self, dms_frame, oms_frame=None) -> np.ndarray:
        """提取摄像头特征"""
        # DMS: 人脸检测→关键点→PERCLOS→视线
        features = np.array([
            0.08,   # PERCLOS (8%)
            12.5,   # 眨眼率(次/分)
            2.1,    # 扫视熵
            0.35,   # 瞳孔直径方差
            0.15,   # 注视偏离比率
            0.12,   # 平均闭眼时长(秒)
            # 6个人脸关键点偏差(简化)
            0.02, 0.01, 0.03, 0.02, 0.01, 0.04
        ])
        return features[:self.feature_dims["camera"]]
    
    def extract_radar_features(self, radar_data) -> np.ndarray:
        """提取雷达特征"""
        features = np.array([
            18.0,   # 呼吸频率(次/分)
            0.95,   # 存在置信度
            1.2,    # 距离(m)
            0.45,   # 微动强度
        ])
        return features[:self.feature_dims["radar"]]
    
    def extract_ecg_features(self, ecg_data) -> np.ndarray:
        """提取ECG特征"""
        features = np.array([
            0.85,   # Mean NN(秒)
            0.045,  # RMSSD(秒)
            2.3,    # LF/HF比
            0.012,  # W分量
        ])
        return features[:self.feature_dims["ecg"]]
    
    def extract_behavior_features(self, can_data) -> np.ndarray:
        """提取行为特征"""
        features = np.array([
            15.0,   # 方向盘操作频率(次/分)
            0.08,   # 车道偏移标准差(m)
            1.2,    # 速度波动标准差(km/h)
        ])
        return features[:self.feature_dims["behavior"]]
    
    def check_sensor_health(self, sensor_data: Dict) -> Dict[str, float]:
        """
        传感器健康检查, 返回各传感器可信度(0-1)
        
        当传感器失效时自动降权
        """
        health = {}
        
        # 摄像头: 检查人脸检测置信度
        health["camera"] = 1.0 if sensor_data.get("face_detected", True) else 0.0
        
        # 雷达: 检查信号强度
        health["radar"] = min(sensor_data.get("radar_snr", 15) / 15, 1.0)
        
        # ECG: 检查电极接触
        health["ecg"] = 1.0 if sensor_data.get("hands_on_wheel", True) else 0.0
        
        # CAN: 总线正常
        health["behavior"] = 1.0
        
        return health
    
    def adaptive_weights(self, health: Dict[str, float]) -> Dict[str, float]:
        """
        根据传感器健康度自适应调整权重
        """
        weights = self.sensor_weights.copy()
        for sensor, h in health.items():
            weights[sensor] *= h
        
        # 归一化
        total = sum(weights.values())
        if total > 0:
            for k in weights:
                weights[k] /= total
        
        return weights
    
    def fuse_features(self, features_dict: Dict[str, np.ndarray], 
                      health: Dict[str, float]) -> Tuple[np.ndarray, Dict[str, float]]:
        """
        特征级融合: 拼接+加权
        
        Returns:
            fused_features: 融合后的特征向量
            adaptive_weights: 自适应权重
        """
        # 自适应权重
        weights = self.adaptive_weights(health)
        
        # 拼接特征(加权)
        fused = np.concatenate([
            features_dict["camera"] * weights["camera"] / self.sensor_weights["camera"],
            features_dict["radar"] * weights["radar"] / self.sensor_weights["radar"],
            features_dict["ecg"] * weights["ecg"] / self.sensor_weights["ecg"],
            features_dict["behavior"] * weights["behavior"] / self.sensor_weights["behavior"],
        ])
        
        return fused, weights
    
    def detect(self, sensor_data: Dict) -> Dict:
        """
        多传感器融合检测主函数
        
        输入: 各传感器原始/预处理数据
        输出: 综合状态评估
        """
        # 1. 健康检查
        health = self.check_sensor_health(sensor_data)
        
        # 2. 特征提取
        cam_feat = self.extract_camera_features(sensor_data.get("dms_frame"))
        rad_feat = self.extract_radar_features(sensor_data.get("radar_data"))
        ecg_feat = self.extract_ecg_features(sensor_data.get("ecg_data"))
        beh_feat = self.extract_behavior_features(sensor_data.get("can_data"))
        
        # 3. 特征级融合
        fused, weights = self.fuse_features(
            {"camera": cam_feat, "radar": rad_feat, "ecg": ecg_feat, "behavior": beh_feat},
            health
        )
        
        # 4. 各模态独立评分
        cam_score = np.dot(cam_feat[:6], [0.3, 0.01, -0.1, 0.5, 0.3, 0.3])
        rad_score = np.dot(rad_feat, [0.1, -0.5, 0.2, 0.3])
        ecg_score = np.dot(ecg_feat, [0.3, 0.3, 0.2, 0.2])
        beh_score = np.dot(beh_feat, [0.01, 0.5, 0.1])
        
        # 5. 加权决策
        total_score = (
            weights["camera"] * cam_score +
            weights["radar"] * rad_score +
            weights["ecg"] * ecg_score +
            weights["behavior"] * beh_score
        )
        
        # 6. 分级
        if total_score < 0.3:
            level = 0  # 正常
        elif total_score < 0.5:
            level = 1  # 轻度
        elif total_score < 0.7:
            level = 2  # 中度
        else:
            level = 3  # 重度
        
        return {
            "fatigue_level": level,
            "confidence": min(total_score, 1.0),
            "sensor_health": health,
            "adaptive_weights": weights,
            "modal_scores": {
                "camera": float(cam_score),
                "radar": float(rad_score),
                "ecg": float(ecg_score),
                "behavior": float(beh_score)
            },
            "fused_feature_dim": len(fused)
        }


# ========== 测试 ==========
if __name__ == "__main__":
    fusion = SensorFusionSystem()
    
    # 场景1: 全传感器正常
    print("=== 场景1: 全传感器正常 ===")
    result = fusion.detect({
        "dms_frame": {}, "radar_data": {}, "ecg_data": {}, "can_data": {},
        "face_detected": True, "radar_snr": 15, "hands_on_wheel": True
    })
    print(f"疲劳等级: {result['fatigue_level']}, 置信度: {result['confidence']:.3f}")
    print(f"权重: {result['adaptive_weights']}")
    
    # 场景2: 夜间摄像头失效,靠雷达+ECG
    print("\n=== 场景2: 摄像头失效(夜间) ===")
    result2 = fusion.detect({
        "dms_frame": {}, "radar_data": {}, "ecg_data": {}, "can_data": {},
        "face_detected": False, "radar_snr": 15, "hands_on_wheel": True
    })
    print(f"疲劳等级: {result2['fatigue_level']}, 置信度: {result2['confidence']:.3f}")
    print(f"权重(摄像头降权): {result2['adaptive_weights']}")
    print(f"摄像头可信度: {result2['sensor_health']['camera']}")
    
    # 场景3: 方向盘脱手,靠摄像头+雷达
    print("\n=== 场景3: 方向盘脱手 ===")
    result3 = fusion.detect({
        "dms_frame": {}, "radar_data": {}, "ecg_data": {}, "can_data": {},
        "face_detected": True, "radar_snr": 15, "hands_on_wheel": False
    })
    print(f"疲劳等级: {result3['fatigue_level']}, 置信度: {result3['confidence']:.3f}")
    print(f"权重(ECG降权): {result3['adaptive_weights']}")
    print(f"ECG可信度: {result3['sensor_health']['ecg']}")

"""
多传感器时间同步方案

关键: 各传感器帧率不同,需对齐到统一时间轴
"""

import numpy as np

class TimeSynchronizer:
    """
    传感器时间同步
    
    DMS摄像头: 30fps (33ms间隔)
    OMS摄像头: 30fps
    雷达: 20fps (50ms间隔)
    ECG: 250Hz (4ms间隔)
    CAN: 10Hz (100ms间隔)
    
    策略: 以DMS摄像头为主时钟,其他传感器最近邻插值
    """
    
    def __init__(self):
        self.rates = {
            "camera": 30,
            "radar": 20,
            "ecg": 250,
            "behavior": 10
        }
        self.master_rate = 30  # DMS摄像头为主时钟
    
    def align(self, sensor_buffers: dict, master_timestamps: np.ndarray) -> dict:
        """
        将所有传感器数据对齐到主时钟
        
        Args:
            sensor_buffers: 各传感器数据+时间戳
            master_timestamps: 主时钟时间戳
            
        Returns:
            aligned: 对齐后的同步数据
        """
        aligned = {}
        t_master = master_timestamps
        
        for sensor, data in sensor_buffers.items():
            if sensor == "camera":
                aligned[sensor] = data  # 已对齐
                continue
            
            t_sensor = data["timestamps"]
            values = data["values"]
            
            # 最近邻插值
            aligned_values = []
            for t in t_master:
                idx = np.argmin(np.abs(t_sensor - t))
                aligned_values.append(values[idx])
            
            aligned[sensor] = np.array(aligned_values)
        
        return aligned
    
    def check_sync_quality(self, sensor_buffers: dict) -> dict:
        """检查时间同步质量"""
        quality = {}
        t_master = sensor_buffers["camera"]["timestamps"]
        
        for sensor, data in sensor_buffers.items():
            if sensor == "camera":
                quality[sensor] = {"max_lag_ms": 0, "avg_lag_ms": 0}
                continue
            
            t_sensor = data["timestamps"]
            # 计算最大时间偏差
            lags = []
            for t in t_master:
                nearest = t_sensor[np.argmin(np.abs(t_sensor - t))]
                lags.append(abs(t - nearest) * 1000)  # 转ms
            
            quality[sensor] = {
                "max_lag_ms": round(max(lags), 2),
                "avg_lag_ms": round(np.mean(lags), 2),
                "acceptable": max(lags) < (1000 / self.rates[sensor]) / 2
            }
        
        return quality


if __name__ == "__main__":
    sync = TimeSynchronizer()
    
    # 模拟10帧
    t_master = np.arange(10) / 30  # 30fps, 10帧
    t_radar = np.arange(0, 10/30, 1/20)  # 20fps
    
    sensor_buffers = {
        "camera": {"timestamps": t_master, "values": np.random.randn(10, 12)},
        "radar": {"timestamps": t_radar, "values": np.random.randn(len(t_radar), 4)},
    }
    
    quality = sync.check_sync_quality(sensor_buffers)
    print("=== 时间同步质量 ===")
    for s, q in quality.items():
        print(f"{s}: 最大延迟={q['max_lag_ms']:.2f}ms, 平均={q['avg_lag_ms']:.2f}ms, 可接受={q['acceptable']}")