"""
Fraunhofer AktiMeter 骨骼数据接口仿真
基于3D人体姿态估计的座舱行为分析

核心思想：
- 不存储/传输原始视频
- 只处理骨骼关键点序列（3D坐标+置信度）
- 所有行为分析基于骨骼运动模式

硬件配置：
- 边缘设备: 65W USB-C供电
- 摄像头: RGB + 可选IR
- 帧率: 30fps
- 骨骼模型: 17关键点 (COCO格式) 或 33关键点 (BlazePose)
"""

import numpy as np
from dataclasses import dataclass, field
from typing import List, Optional, Tuple
from enum import Enum
import time

class ActivityType(Enum):
    """座舱活动分类"""
    NORMAL_DRIVING = "normal_driving"
    EATING = "eating"
    DRINKING = "drinking"
    PHONE_CALL = "phone_call"
    SLEEPING = "sleeping"
    READING = "reading"
    FEET_ON_DASH = "feet_on_dashboard"
    RECLINED = "reclined"
    SIDEWAYS = "sideways"
    REACHING_BACK = "reaching_back"
    TEXTING = "texting"
    UNKNOWN = "unknown"

class OPLevel(Enum):
    """OOP严重程度"""
    NORMAL = 0      # 正常
    MILD_OOP = 1    # 轻度OOP（如轻微后仰）
    SEVERE_OOP = 2 # 严重OOP（如脚放仪表台）
    CRITICAL = 3   # 危险姿态（气囊展开风险）

@dataclass
class Skeleton3D:
    """
    3D骨骼关键点（单帧）
    COCO-17格式 + 3D深度估计
    
    关键点索引：
    0: nose, 1: left_eye, 2: right_eye, 3: left_ear, 4: right_ear
    5: left_shoulder, 6: right_shoulder, 7: left_elbow, 8: right_elbow
    9: left_wrist, 10: right_wrist, 11: left_hip, 12: right_hip
    13: left_knee, 14: right_knee, 15: left_ankle, 16: right_ankle
    """
    keypoints: np.ndarray  # [17, 3] (x, y, z) in cm relative to camera
    confidence: np.ndarray  # [17] per-keypoint confidence
    timestamp: float       # epoch seconds
    
    def get_joint(self, idx: int) -> np.ndarray:
        return self.keypoints[idx]
    
    def get_torso_angle(self) -> float:
        """计算躯干倾斜角度（垂直=0°，后仰>0°）"""
        shoulder_mid = (self.keypoints[5] + self.keypoints[6]) / 2
        hip_mid = (self.keypoints[11] + self.keypoints[12]) / 2
        torso_vec = hip_mid - shoulder_mid
        # 与垂直方向的夹角
        vertical = np.array([0, 0, -1])
        cos_angle = np.dot(torso_vec, vertical) / (
            np.linalg.norm(torso_vec) * np.linalg.norm(vertical)
        )
        return np.degrees(np.arccos(np.clip(cos_angle, -1, 1)))
    
    def get_torso_rotation(self) -> float:
        """计算躯干旋转角度（正面=0°）"""
        ls = self.keypoints[5]  # left shoulder
        rs = self.keypoints[6]  # right shoulder
        # 肩膀连线的水平角度
        shoulder_vec = rs - ls
        return np.degrees(np.arctan2(shoulder_vec[2], shoulder_vec[0]))
    
    def get_feet_position(self) -> str:
        """判断脚的位置"""
        # 脚踝高度 vs 仪表台高度（假设仪表台在z=0.3m）
        left_ankle = self.keypoints[15]
        right_ankle = self.keypoints[16]
        
        if left_ankle[1] > 0.3 and left_ankle[2] > 0.2:
            return "feet_on_dashboard"
        if right_ankle[1] > 0.3 and right_ankle[2] > 0.2:
            return "feet_on_dashboard"
        return "normal"


class CabinActivityAnalyzer:
    """
    座舱行为分析器
    基于骨骼序列的活动分类 + OOP检测
    """
    
    HISTORY_LEN = 90  # 3秒@30fps
    
    def __init__(self):
        self.history: List[Skeleton3D] = []
        self.current_activity = ActivityType.NORMAL_DRIVING
        self.oop_level = OPLevel.NORMAL
        self.confidence = 0.0
        
    def update(self, skeleton: Skeleton3D) -> dict:
        """处理新骨骼帧，输出行为分析"""
        self.history.append(skeleton)
        if len(self.history) > self.HISTORY_LEN:
            self.history = self.history[-self.HISTORY_LEN:]
        
        if len(self.history) < 10:
            return {'activity': ActivityType.UNKNOWN, 'oop': OPLevel.NORMAL}
        
        # 1. OOP检测
        oop_level = self._detect_oop(skeleton)
        
        # 2. 活动分类
        activity = self._classify_activity()
        
        # 3. 睡眠检测
        if self._detect_sleep():
            activity = ActivityType.SLEEPING
        
        self.current_activity = activity
        self.oop_level = oop_level
        
        return {
            'activity': activity,
            'oop_level': oop_level,
            'torso_angle': skeleton.get_torso_angle(),
            'torso_rotation': skeleton.get_torso_rotation(),
            'feet_position': skeleton.get_feet_position(),
            'confidence': self.confidence,
            'timestamp': skeleton.timestamp,
            'skeleton_data': skeleton.keypoints,  # 隐私安全：只骨骼
            'raw_video': None  # 不输出原始视频
        }
    
    def _detect_oop(self, skel: Skeleton3D) -> OPLevel:
        """OOP严重程度检测"""
        torso_angle = skel.get_torso_angle()
        feet_pos = skel.get_feet_position()
        
        if feet_pos == "feet_on_dashboard":
            return OPLevel.CRITICAL
        if torso_angle > 45:
            return OPLevel.SEVERE_OOP
        if torso_angle > 25:
            return OPLevel.MILD_OOP
        return OPLevel.NORMAL
    
    def _classify_activity(self) -> ActivityType:
        """基于骨骼序列的活动分类"""
        recent = self.history[-30:]  # 最近1秒
        
        # 计算手部运动频率
        left_wrist_y = [s.keypoints[9][1] for s in recent]
        right_wrist_y = [s.keypoints[10][1] for s in recent]
        
        # 手部在嘴附近（吃东西/喝东西/打电话）
        nose = recent[-1].keypoints[0]
        left_wrist = recent[-1].keypoints[9]
        right_wrist = recent[-1].keypoints[10]
        
        dist_l = np.linalg.norm(left_wrist - nose)
        dist_r = np.linalg.norm(right_wrist - nose)
        
        min_hand_dist = min(dist_l, dist_r)
        
        if min_hand_dist < 0.15:  # 15cm内
            # 判断是电话还是吃喝
            # 电话: 手部稳定在耳侧
            left_ear = recent[-1].keypoints[3]
            right_ear = recent[-1].keypoints[4]
            dist_ear = min(
                np.linalg.norm(left_wrist - left_ear),
                np.linalg.norm(right_wrist - right_ear)
            )
            if dist_ear < 0.20:
                return ActivityType.PHONE_CALL
            
            # 吃喝: 手部反复接近嘴部
            if max(left_wrist_y) - min(left_wrist_y) > 0.1:  # 10cm运动范围
                return ActivityType.EATING if dist_l < dist_r else ActivityType.DRINKING
        
        # 正常驾驶
        return ActivityType.NORMAL_DRIVING
    
    def _detect_sleep(self) -> bool:
        """基于骨骼的睡眠检测"""
        if len(self.history) < 60:  # 需要至少2秒
            return False
        
        recent = self.history[-60:]
        
        # 头部下垂角度（鼻子相对于肩膀的位置）
        nose_y = [s.keypoints[0][1] for s in recent]
        shoulder_y = [(s.keypoints[5][1] + s.keypoints[6][1]) / 2 for s in recent]
        
        # 头部低于肩膀 → 睡眠/疲劳
        head_drop = nose_y[-1] - shoulder_y[-1]
        if head_drop > 0.05:  # 鼻子低于肩膀5cm
            # 静止度检测
            movement = np.std([s.keypoints[0] for s in recent], axis=0)
            if np.mean(movement) < 0.01:  # 基本静止
                return True
        
        return False


# ==================== 测试场景 ====================
if __name__ == "__main__":
    analyzer = CabinActivityAnalyzer()
    
    # 模拟正常驾驶骨骼
    print("=== 场景1: 正常驾驶 ===")
    for i in range(30):
        skel = Skeleton3D(
            keypoints=np.random.normal(0, 0.01, (17, 3)) + np.array([
                [0, 0.1, 0.5],  # nose
                [0.05, 0.12, 0.5], [−0.05, 0.12, 0.5],  # eyes
                [0.08, 0.1, 0.5], [−0.08, 0.1, 0.5],  # ears
                [0.1, 0, 0.4], [−0.1, 0, 0.4],  # shoulders
                [0.15, 0, 0.3], [−0.15, 0, 0.3],  # elbows
                [0.2, 0, 0.2], [−0.2, 0, 0.2],  # wrists
                [0.08, −0.3, 0.4], [−0.08, −0.3, 0.4],  # hips
                [0.1, −0.5, 0.3], [−0.1, −0.5, 0.3],  # knees
                [0.1, −0.7, 0.1], [−0.1, −0.7, 0.1],  # ankles
            ]),
            confidence=np.ones(17) * 0.9,
            timestamp=time.time()
        )
        result = analyzer.update(skel)
    
    print(f"活动: {result['activity'].value}")
    print(f"OOP: {result['oop_level'].name}")
    print(f"躯干角度: {result['torso_angle']:.1f}°")
    
    # 模拟脚放仪表台
    print("\n=== 场景2: 脚放仪表台 (严重OOP) ===")
    for i in range(30):
        base = np.random.normal(0, 0.01, (17, 3))
        base[15] = [0.2, 0.4, 0.35]  # 左脚踝在仪表台高度
        base[16] = [0.1, 0.38, 0.33]  # 右脚踝也在高位
        base += np.array([
            [0, 0.1, 0.5], [0.05, 0.12, 0.5], [−0.05, 0.12, 0.5],
            [0.08, 0.1, 0.5], [−0.08, 0.1, 0.5],
            [0.1, 0, 0.4], [−0.1, 0, 0.4],
            [0.15, 0, 0.3], [−0.15, 0, 0.3],
            [0.2, 0, 0.2], [−0.2, 0, 0.2],
            [0.08, −0.3, 0.4], [−0.08, −0.3, 0.4],
            [0.1, −0.5, 0.3], [−0.1, −0.5, 0.3],
            [0.1, −0.7, 0.1], [−0.1, −0.7, 0.1],
        ])
        skel = Skeleton3D(
            keypoints=base,
            confidence=np.ones(17) * 0.85,
            timestamp=time.time()
        )
        result = analyzer.update(skel)
    
    print(f"活动: {result['activity'].value}")
    print(f"OOP: {result['oop_level'].name}")
    print(f"脚部位置: {result['feet_position']}")
    print(f"躯干角度: {result['torso_angle']:.1f}°")