"""
OOP异常姿态检测方案
基于Geely E2和CUPRA Raval的Euro NCAP评测发现

缺失场景:
- 乘客脚放在仪表板上
- 乘客异常前倾
- 乘客侧躺/睡觉
- 儿童坐在成人位置（无安全座椅）

技术方案: 3D姿态估计 + 规则判定
"""

import numpy as np
from typing import Tuple, List

class OOPDetector:
    """
    OOP异常姿态检测器
    
    检测流程:
    1. 乘员检测 (座椅压力 + 摄像头)
    2. 3D姿态估计 (关键点检测)
    3. 姿态分类 (正常 vs 异常)
    4. 风险评估 (碰撞风险等级)
    """
    
    # 正常坐姿关键角度范围 (度)
    NORMAL_RANGES = {
        'torso_angle': (75, 105),      # 躯干与垂直方向
        'head_pitch': (-15, 25),        # 头部俯仰
        'knee_angle': (60, 150),        # 膝盖弯曲
        'hip_angle': (80, 110),         # 髋关节
        'shoulder_height': (0.3, 0.7),  # 肩高/座高比
    }
    
    # OOP场景定义
    OOP_SCENARIOS = {
        'feet_on_dash': {
            'description': '脚放在仪表板上',
            'risk': 'critical',
            'key_indicators': ['knee_angle > 160', 'foot_height > 0.8 * dashboard_height'],
        },
        'extreme_forward_lean': {
            'description': '极端前倾',
            'risk': 'high',
            'key_indicators': ['torso_angle < 45', 'head_distance_to_dashboard < 0.2m'],
        },
        'lying_down': {
            'description': '躺平',
            'risk': 'high',
            'key_indicators': ['torso_angle < 30', 'shoulder_height < 0.3'],
        },
        'child_no_seat': {
            'description': '儿童未坐安全座椅',
            'risk': 'critical',
            'key_indicators': ['shoulder_height < 0.4', 'head_circumference < threshold'],
        }
    }
    
    def detect_oop(self, keypoints_3d: np.ndarray, 
                   seat_pressure: np.ndarray) -> dict:
        """
        OOP检测主函数
        
        Args:
            keypoints_3d: (17, 3) 3D关键点 (COCO格式)
            seat_pressure: 座椅压力分布 (H, W)
        
        Returns:
            OOP检测结果
        """
        # 计算关键角度
        torso_angle = self._compute_torso_angle(keypoints_3d)
        head_pitch = self._compute_head_pitch(keypoints_3d)
        knee_angle = self._compute_knee_angle(keypoints_3d)
        hip_angle = self._compute_hip_angle(keypoints_3d)
        shoulder_height = self._compute_shoulder_height(keypoints_3d)
        
        # 判定每种OOP场景
        results = {}
        for scenario, config in self.OOP_SCENARIOS.items():
            is_detected = self._check_scenario(
                scenario, torso_angle, head_pitch, knee_angle,
                hip_angle, shoulder_height, keypoints_3d
            )
            results[scenario] = {
                'detected': is_detected,
                'risk': config['risk'],
                'description': config['description']
            }
        
        # 综合风险评估
        max_risk = max(
            (r['risk'] for r in results.values() if r['detected']),
            key=['low', 'medium', 'high', 'critical'].index
        ) if any(r['detected'] for r in results.values()) else 'none'
        
        return {
            'scenarios': results,
            'overall_risk': max_risk,
            'metrics': {
                'torso_angle': torso_angle,
                'knee_angle': knee_angle,
                'shoulder_height': shoulder_height
            }
        }
    
    def _compute_torso_angle(self, kp: np.ndarray) -> float:
        """计算躯干角度"""
        shoulder = (kp[5] + kp[6]) / 2  # 左右肩中点
        hip = (kp[11] + kp[12]) / 2    # 左右髋中点
        torso = hip - shoulder
        return np.degrees(np.arctan2(torso[0], -torso[1]))
    
    def _compute_head_pitch(self, kp: np.ndarray) -> float:
        """计算头部俯仰角"""
        ear = kp[3]  # 左耳
        nose = kp[0]  # 鼻子
        return np.degrees(np.arctan2(nose[1] - ear[1], nose[2] - ear[2]))
    
    def _compute_knee_angle(self, kp: np.ndarray) -> float:
        """计算膝盖角度"""
        hip = kp[11]  # 左髋
        knee = kp[13]  # 左膝
        ankle = kp[15]  # 左踝
        v1 = hip - knee
        v2 = ankle - knee
        cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
        return np.degrees(np.arccos(np.clip(cos_angle, -1, 1)))
    
    def _compute_hip_angle(self, kp: np.ndarray) -> float:
        """计算髋关节角度"""
        shoulder = kp[5]
        hip = kp[11]
        knee = kp[13]
        v1 = shoulder - hip
        v2 = knee - hip
        cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
        return np.degrees(np.arccos(np.clip(cos_angle, -1, 1)))
    
    def _compute_shoulder_height(self, kp: np.ndarray) -> float:
        """肩高/座高比"""
        shoulder = (kp[5] + kp[6]) / 2
        hip = (kp[11] + kp[12]) / 2
        seat_height = np.abs(hip[1] - shoulder[1])
        return float(seat_height)
    
    def _check_scenario(self, scenario, torso, head, knee, hip, shoulder_h, kp):
        """检查特定OOP场景"""
        if scenario == 'feet_on_dash':
            return knee > 160 and kp[15][1] < 0.8 * 0.5  # 脚踝高度
        elif scenario == 'extreme_forward_lean':
            return torso < 45
        elif scenario == 'lying_down':
            return torso < 30 and shoulder_h < 0.3
        elif scenario == 'child_no_seat':
            return shoulder_h < 0.4
        return False