"""
OOP异常姿态检测系统

基于3D人体关键点+座椅角度传感器的多模态方案

检测维度：
1. 座椅靠背角度（CAN总线获取）
2. 乘员躯干角度（3D关键点计算）
3. 腿部位置（脚是否在踏板区域）
4. 头部位置（相对于安全带路径）
5. 手部位置（是否操作中控）
"""

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

@dataclass
class OccupantPosture:
    """乘员姿态数据"""
    seat_back_angle: float       # 座椅靠背角度 (度)
    torso_angle: float          # 躯干前倾角度 (度)
    hip_angle: float            # 髋关节角度 (度)
    knee_angle: float           # 膝关节角度 (度)
    foot_position: str          # 脚位置: 'floor', 'dashboard', 'seat'
    head_position: tuple       # 头部3D坐标 (x, y, z)
    shoulder_position: tuple   # 肩膀3D坐标
    hip_position: tuple        # 髋部3D坐标
    knee_position: tuple       # 膝盖3D坐标
    ankle_position: tuple      # 脚踝3D坐标
    seatbelt_on: bool          # 安全带是否佩戴
    occupant_size: str         # 'small', 'medium', 'large'

class OOPDetector:
    """OOP异常姿态检测器"""
    
    # 阈值定义
    THRESHOLDS = {
        'seat_back_max_safe': 25,      # 座椅靠背最大安全角度
        'torso_forward_warn': 20,      # 躯干前倾警告角度
        'torso_forward_danger': 35,   # 躯干前倾危险角度
        'hip_recline_warn': 110,      # 髋关节后仰警告
        'knee_to_dashboard': 0.15,    # 膝盖到仪表台距离
        'head_below_window': 0.0,     # 头部低于窗户线
    }
    
    def detect_oop(self, posture: OccupantPosture) -> dict:
        """
        检测OOP异常姿态
        
        Returns:
            {
                'is_oop': bool,
                'oop_type': str,
                'risk_level': str,
                'recommendation': str
            }
        """
        results = []
        
        # 1. 座椅过度倾斜
        if posture.seat_back_angle > self.THRESHOLDS['seat_back_max_safe']:
            results.append({
                'type': 'reclined_seat',
                'level': 'danger',
                'detail': f"座椅倾斜{posture.seat_back_angle:.1f}°"
            })
        
        # 2. 躯干前倾
        if posture.torso_angle > self.THRESHOLDS['torso_forward_danger']:
            results.append({
                'type': 'forward_lean',
                'level': 'danger',
                'detail': f"躯干前倾{posture.torso_angle:.1f}°"
            })
        elif posture.torso_angle > self.THRESHOLDS['torso_forward_warn']:
            results.append({
                'type': 'forward_lean',
                'level': 'warning',
                'detail': f"躯干前倾{posture.torso_angle:.1f}°"
            })
        
        # 3. 脚踩仪表台
        if posture.foot_position == 'dashboard':
            results.append({
                'type': 'feet_on_dashboard',
                'level': 'danger',
                'detail': '脚踩仪表台'
            })
        
        # 4. 髋关节后仰（倾斜坐姿）
        if posture.hip_angle > self.THRESHOLDS['hip_recline_warn']:
            results.append({
                'type': 'reclined_posture',
                'level': 'warning',
                'detail': f"髋关节角度{posture.hip_angle:.1f}°"
            })
        
        # 5. 头部位置异常
        if posture.head_position[2] < self.THRESHOLDS['head_below_window']:
            results.append({
                'type': 'head_low',
                'level': 'warning',
                'detail': '头部低于安全区域'
            })
        
        # 6. 小身材乘员 + 倾斜 = 极高风险
        if (posture.occupant_size == 'small' and 
            posture.seat_back_angle > self.THRESHOLDS['seat_back_max_safe']):
            results.append({
                'type': 'small_occupant_reclined',
                'level': 'danger',
                'detail': '小身材乘员+倾斜座椅=骨盆骨折高风险'
            })
        
        # 汇总
        is_oop = len(results) > 0
        danger_count = sum(1 for r in results if r['level'] == 'danger')
        
        if danger_count > 0:
            risk_level = 'danger'
            recommendation = '立即调整坐姿，否则碰撞中骨盆骨折风险极高'
        elif len(results) > 0:
            risk_level = 'warning'
            recommendation = '建议调整坐姿至正常位置'
        else:
            risk_level = 'safe'
            recommendation = '坐姿正常'
        
        return {
            'is_oop': is_oop,
            'oop_type': [r['type'] for r in results],
            'risk_level': risk_level,
            'recommendation': recommendation,
            'details': results
        }


# 测试
if __name__ == "__main__":
    print("=" * 60)
    print("OOP异常姿态检测系统测试")
    print("=" * 60)
    
    detector = OOPDetector()
    
    # 场景1: 正常坐姿
    print("\n场景1: 正常坐姿")
    posture = OccupantPosture(
        seat_back_angle=15, torso_angle=5, hip_angle=95,
        knee_angle=90, foot_position='floor',
        head_position=(0.0, 0.0, 0.8),
        shoulder_position=(0.0, 0.0, 0.7),
        hip_position=(0.0, 0.0, 0.3),
        knee_position=(0.3, 0.0, 0.2),
        ankle_position=(0.5, 0.0, 0.1),
        seatbelt_on=True, occupant_size='medium'
    )
    result = detector.detect_oop(posture)
    print(f"  OOP: {result['is_oop']}, 风险: {result['risk_level']}")
    print(f"  建议: {result['recommendation']}")
    
    # 场景2: 倾斜座椅 + 小身材女性
    print("\n场景2: 倾斜座椅 + 小身材女性（最高风险）")
    posture = OccupantPosture(
        seat_back_angle=35, torso_angle=30, hip_angle=115,
        knee_angle=45, foot_position='floor',
        head_position=(0.0, 0.0, 0.7),
        shoulder_position=(0.0, 0.0, 0.6),
        hip_position=(0.0, 0.0, 0.25),
        knee_position=(0.25, 0.0, 0.15),
        ankle_position=(0.4, 0.0, 0.08),
        seatbelt_on=True, occupant_size='small'
    )
    result = detector.detect_oop(posture)
    print(f"  OOP: {result['is_oop']}, 风险: {result['risk_level']}")
    print(f"  异常类型: {result['oop_type']}")
    print(f"  建议: {result['recommendation']}")
    for d in result['details']:
        print(f"    - {d['type']}: {d['detail']} ({d['level']})")
    
    # 场景3: 脚踩仪表台
    print("\n场景3: 脚踩仪表台")
    posture = OccupantPosture(
        seat_back_angle=20, torso_angle=10, hip_angle=100,
        knee_angle=30, foot_position='dashboard',
        head_position=(0.0, 0.0, 0.75),
        shoulder_position=(0.0, 0.0, 0.65),
        hip_position=(0.0, 0.0, 0.28),
        knee_position=(0.4, 0.2, 0.6),
        ankle_position=(0.5, 0.3, 0.7),
        seatbelt_on=True, occupant_size='medium'
    )
    result = detector.detect_oop(posture)
    print(f"  OOP: {result['is_oop']}, 风险: {result['risk_level']}")
    print(f"  异常类型: {result['oop_type']}")
    print(f"  建议: {result['recommendation']}")