"""
Seeing Machines Cause-Agnostic Impairment Detection
论文：Technical Paper Series on Intoxication, Part 2 (2026)

核心方法：通过视觉行为特征评估功能损伤，不依赖化学物质检测
"""

import numpy as np
from dataclasses import dataclass
from typing import List, Tuple, Optional

@dataclass
class ImpairmentIndicator:
    """损伤指标数据结构"""
    name: str
    value: float  # 0.0 = 正常, 1.0 = 严重损伤
    confidence: float  # 0.0-1.0
    trend: str  # 'improving', 'stable', 'deteriorating'

class CauseAgnosticImpairmentDetector:
    """
    Cause-Agnostic 功能损伤检测器
    
    不检测具体物质，而是评估驾驶功能状态：
    1. 眼动模式异常（注视点离散度、扫视频率）
    2. 面部表情特征（微表情迟滞、面部肌肉松弛）
    3. 头部姿态稳定性（头部晃动频率、保持能力）
    4. 眨眼参数（频率、持续时间、PERCLOS）
    5. 行为模式异常（车道保持、方向盘微调）
    """
    
    def __init__(self, config: dict):
        # 眼动参数
        self.gaze_disruption_threshold = config.get('gaze_disruption', 0.65)
        self.saccade_frequency_baseline = config.get('saccade_freq', 2.5)  # Hz
        
        # 面部参数
        self.facial_latency_threshold = config.get('facial_latency', 0.3)  # seconds
        self.muscle_tone_baseline = config.get('muscle_tone', 0.75)
        
        # 头部参数
        self.head_stability_window = config.get('head_window', 30)  # seconds
        self.head_drift_threshold = config.get('head_drift', 5.0)  # degrees
        
        # PERCLOS 参数
        self.perclos_threshold = config.get('perclos_eye', 0.2)  # eye openness ratio
        self.perclos_window = config.get('perclos_window', 60)  # seconds
        
        # 综合阈值
        self.impairment_alert_threshold = config.get('alert_threshold', 0.45)
        self.impairment_critical_threshold = config.get('critical_threshold', 0.70)
        
        # 基线模型
        self.baseline_gaze_entropy = None
        self.baseline_blink_rate = None
        self.baseline_steering_entropy = None
        
    def compute_gaze_entropy(self, gaze_points: np.ndarray) -> float:
        """
        计算注视点熵
        
        损伤状态下注视模式变得过度集中（隧道视觉）或过度分散（注意力涣散）
        
        Args:
            gaze_points: (N, 2) 注视点序列，归一化到 [0,1]×[0,1]
        
        Returns:
            entropy: Shannon 熵值，0-1 归一化
        """
        # 将注视点空间离散化为网格
        grid_size = 20
        hist, _, _ = np.histogram2d(
            gaze_points[:, 0], gaze_points[:, 1],
            bins=grid_size, range=[[0, 1], [0, 1]]
        )
        
        # 归一化为概率分布
        prob = hist / (hist.sum() + 1e-8)
        
        # Shannon 熵
        entropy = -np.sum(prob[prob > 0] * np.log2(prob[prob > 0]))
        max_entropy = np.log2(grid_size * grid_size)
        
        return entropy / max_entropy
    
    def detect_saccade_anomalies(self, gaze_sequence: np.ndarray, 
                                   fps: int = 30) -> Tuple[float, float]:
        """
        检测扫视异常
        
        损伤特征：
        - 扫视频率降低（反应迟钝）
        - 扫视幅度异常（过短=隧道视觉，过长=注意力涣散）
        - 扫视延迟增加（决策时间延长）
        
        Returns:
            (frequency_score, amplitude_score): 0=正常, 1=严重异常
        """
        dt = 1.0 / fps
        velocities = np.diff(gaze_sequence, axis=0) / dt
        
        # 扫视速度（正常 > 200°/s）
        speeds = np.linalg.norm(velocities, axis=1)
        saccade_threshold = 200.0  # degrees/second
        saccade_mask = speeds > saccade_threshold
        
        # 扫视频率
        saccade_count = np.sum(saccade_mask)
        duration_sec = len(gaze_sequence) / fps
        saccade_freq = saccade_count / duration_sec
        
        # 频率得分（低于基线的程度）
        freq_ratio = saccade_freq / self.saccade_frequency_baseline
        frequency_score = max(0.0, min(1.0, 1.0 - freq_ratio))
        
        # 幅度得分
        if saccade_count > 0:
            saccade_amplitudes = np.linalg.norm(
                np.diff(gaze_sequence[saccade_mask], axis=0), axis=1
            )
            # 正常扫视幅度 5-25°
            abnormal_low = np.sum(saccade_amplitudes < 3.0) / saccade_count
            abnormal_high = np.sum(saccade_amplitudes > 40.0) / saccade_count
            amplitude_score = min(1.0, abnormal_low + abnormal_high)
        else:
            amplitude_score = 0.5  # 无扫视 = 不确定
            
        return frequency_score, amplitude_score
    
    def compute_perclos(self, eye_openness: np.ndarray, 
                         fps: int = 30) -> float:
        """
        计算 PERCLOS（眼睑闭合百分比）
        
        损伤状态下 PERCLOS 升高，反映警觉性下降
        
        Args:
            eye_openness: (N,) 眼睑开度序列 0-1
            fps: 帧率
        
        Returns:
            perclos: 0-100%
        """
        window_frames = int(self.perclos_window * fps)
        if len(eye_openness) < window_frames:
            return 0.0
        
        # 滑动窗口计算
        closed_mask = eye_openness < self.perclos_threshold
        perclos_values = []
        
        for i in range(len(eye_openness) - window_frames):
            window = closed_mask[i:i+window_frames]
            perclos_values.append(np.sum(window) / window_frames * 100)
        
        return np.mean(perclos_values[-10:]) if perclos_values else 0.0
    
    def detect_facial_latency(self, facial_landmarks: np.ndarray,
                               fps: int = 30) -> float:
        """
        检测面部表情迟滞
        
        损伤状态下面部微表情反应时间延长
        （对刺激的响应延迟 > 300ms 视为异常）
        
        Args:
            facial_landmarks: (N, 68, 2) 面部关键点序列
        
        Returns:
            latency_score: 0=正常, 1=严重迟滞
        """
        # 提取嘴部区域（关键点 48-67）
        mouth_points = facial_landmarks[:, 48:68, :]
        
        # 计算嘴部变化率
        mouth_movement = np.linalg.norm(
            np.diff(mouth_points, axis=0), axis=2
        ).mean(axis=1)
        
        # 检测运动爆发后的响应延迟
        movement_threshold = np.percentile(mouth_movement, 90)
        bursts = np.where(mouth_movement > movement_threshold)[0]
        
        if len(bursts) < 2:
            return 0.0
        
        # 计算连续爆发间的间隔（反映反应速度）
        intervals = np.diff(bursts) / fps
        normal_interval = 1.0  # 正常间隔 ~1秒
        delayed_ratio = np.sum(intervals > normal_interval * 2) / len(intervals)
        
        return min(1.0, delayed_ratio * 2)
    
    def assess_impairment(self, gaze_data: np.ndarray,
                           eye_openness: np.ndarray,
                           facial_landmarks: np.ndarray,
                           steering_signal: np.ndarray,
                           fps: int = 30) -> Tuple[float, List[ImpairmentIndicator]]:
        """
        综合功能损伤评估
        
        Cause-Agnostic 方案：不区分酒精/药物/疲劳，直接评估功能状态
        
        Args:
            gaze_data: (N, 2) 注视点序列
            eye_openness: (N,) 眼睑开度
            facial_landmarks: (N, 68, 2) 面部关键点
            steering_signal: (N,) 方向盘角度序列
            fps: 帧率
        
        Returns:
            (impairment_score, indicators)
        """
        indicators = []
        
        # 1. 眼动熵
        gaze_entropy = self.compute_gaze_entropy(gaze_data)
        # 损伤时熵值偏离基线（过高或过低）
        if self.baseline_gaze_entropy is not None:
            entropy_deviation = abs(gaze_entropy - self.baseline_gaze_entropy)
            entropy_score = min(1.0, entropy_deviation * 3)
        else:
            entropy_score = 0.3  # 无基线时给中等分
        indicators.append(ImpairmentIndicator(
            'gaze_entropy', entropy_score, 0.7, 'stable'
        ))
        
        # 2. 扫视异常
        freq_score, amp_score = self.detect_saccade_anomalies(gaze_data, fps)
        saccade_score = (freq_score + amp_score) / 2
        indicators.append(ImpairmentIndicator(
            'saccade_anomaly', saccade_score, 0.65, 'stable'
        ))
        
        # 3. PERCLOS
        perclos = self.compute_perclos(eye_openness, fps)
        perclos_score = min(1.0, perclos / 20.0)  # 20% PERCLOS = 满分损伤
        indicators.append(ImpairmentIndicator(
            'perclos', perclos_score, 0.85, 'deteriorating' if perclos > 10 else 'stable'
        ))
        
        # 4. 面部迟滞
        facial_score = self.detect_facial_latency(facial_landmarks, fps)
        indicators.append(ImpairmentIndicator(
            'facial_latency', facial_score, 0.6, 'stable'
        ))
        
        # 5. 转向熵（方向盘微调的随机性）
        steering_diff = np.abs(np.diff(steering_signal))
        steering_entropy = -np.sum(
            (p := steering_diff / (steering_diff.sum() + 1e-8)) * np.log2(p + 1e-8)
        )
        # 损伤时转向熵增加（操作不规律）
        steering_score = min(1.0, steering_entropy / 5.0)
        indicators.append(ImpairmentIndicator(
            'steering_entropy', steering_score, 0.55, 'stable'
        ))
        
        # 加权综合
        weights = np.array([ind.confidence for ind in indicators])
        scores = np.array([ind.value for ind in indicators])
        impairment_score = np.average(scores, weights=weights)
        
        return impairment_score, indicators


# ==================== 测试用例 ====================
if __name__ == "__main__":
    np.random.seed(42)
    fps = 30
    duration_sec = 120  # 2分钟
    n_frames = fps * duration_sec
    
    # 模拟正常驾驶数据
    normal_gaze = np.column_stack([
        0.5 + 0.15 * np.sin(np.linspace(0, 4*np.pi, n_frames)) + np.random.normal(0, 0.05, n_frames),
        0.5 + 0.10 * np.cos(np.linspace(0, 3*np.pi, n_frames)) + np.random.normal(0, 0.05, n_frames)
    ])
    normal_gaze = np.clip(normal_gaze, 0, 1)
    
    normal_eye = np.clip(
        0.8 + 0.05 * np.sin(np.linspace(0, 10*np.pi, n_frames)) + np.random.normal(0, 0.03, n_frames),
        0, 1
    )
    
    # 模拟损伤驾驶数据（酒精/疲劳混合效应）
    impaired_gaze = np.column_stack([
        0.5 + 0.08 * np.sin(np.linspace(0, 2*np.pi, n_frames)) + np.random.normal(0, 0.08, n_frames),
        0.5 + 0.05 * np.cos(np.linspace(0, 1.5*np.pi, n_frames)) + np.random.normal(0, 0.08, n_frames)
    ])
    impaired_gaze = np.clip(impaired_gaze, 0, 1)
    
    # 模拟损伤眼动（更多闭眼）
    impaired_eye = np.clip(
        0.7 + 0.08 * np.sin(np.linspace(0, 4*np.pi, n_frames)) + np.random.normal(0, 0.05, n_frames),
        0, 1
    )
    # 注入额外闭眼段
    for _ in range(5):
        start = np.random.randint(0, n_frames - 90)
        impaired_eye[start:start+45] *= 0.1  # 1.5秒闭眼
    
    # 生成模拟面部关键点
    normal_face = np.random.normal(0, 1, (n_frames, 68, 2))
    impaired_face = np.random.normal(0, 1.5, (n_frames, 68, 2))  # 更大抖动
    
    # 转向信号
    normal_steer = np.cumsum(np.random.normal(0, 0.5, n_frames))
    impaired_steer = np.cumsum(np.random.normal(0, 1.2, n_frames))  # 更不规律
    
    # 初始化检测器
    detector = CauseAgnosticImpairmentDetector({
        'gaze_disruption': 0.65,
        'saccade_freq': 2.5,
        'perclos_eye': 0.2,
        'perclos_window': 60,
        'alert_threshold': 0.45,
        'critical_threshold': 0.70,
    })
    
    # 设置基线
    detector.baseline_gaze_entropy = detector.compute_gaze_entropy(normal_gaze[:fps*30])
    print(f"基线注视熵: {detector.baseline_gaze_entropy:.4f}")
    
    # 评估正常驾驶
    print("\n=== 正常驾驶评估 ===")
    normal_score, normal_inds = detector.assess_impairment(
        normal_gaze, normal_eye, normal_face, normal_steer, fps
    )
    print(f"综合损伤得分: {normal_score:.3f} (应 < 0.3)")
    for ind in normal_inds:
        print(f"  {ind.name}: {ind.value:.3f} (conf={ind.confidence:.2f}, trend={ind.trend})")
    
    # 评估损伤驾驶
    print("\n=== 损伤驾驶评估 ===")
    impaired_score, impaired_inds = detector.assess_impairment(
        impaired_gaze, impaired_eye, impaired_face, impaired_steer, fps
    )
    print(f"综合损伤得分: {impaired_score:.3f} (应 > 0.45)")
    for ind in impaired_inds:
        print(f"  {ind.name}: {ind.value:.3f} (conf={ind.confidence:.2f}, trend={ind.trend})")
    
    # 判定
    print("\n=== 判定结果 ===")
    if impaired_score >= detector.impairment_critical_threshold:
        print("⚠️ 严重损伤 — 触发紧急干预（靠边停车+紧急联系）")
    elif impaired_score >= detector.impairment_alert_threshold:
        print("⚠️ 中度损伤 — 触发一级警告（声音+触觉提醒）")
    else:
        print("✅ 功能状态正常")