import numpy as np

class OOPBrakingDetector:
    """
    制动脉冲下 OOP 检测增强
    
    在传统姿态检测基础上:
    1. 检测 reclined 状态
    2. 监测制动事件
    3. 预测制动后位移
    4. 判定 OOP 风险
    """
    
    def __init__(self):
        # 姿态参数
        self.recline_threshold = 25  # 度, 背靠角度
        self.braking_threshold = 0.5  # g, 制动减速度
        self.head_forward_threshold = 30  # 度, 头部前倾
        self.chest_forward_threshold = 20  # 度
        
        # 安全区域
        self.safe_head_zone = [-15, 15]  # 度
        self.safe_chest_zone = [-10, 10]
        
    def assess_risk(self, posture: dict, vehicle: dict) -> dict:
        """
        评估 OOP 风险
        
        Args:
            posture: 乘员姿态数据
                - back_angle: 背靠角度
                - head_pitch: 头部俯仰
                - chest_pitch: 胸部俯仰
                - head_to_wheel: 头到方向盘距离
            vehicle: 车辆数据
                - deceleration: 减速度 (g)
                - speed: 速度
        """
        risk_level = 0
        risks = []
        
        # 1. 检测 reclined 状态
        is_reclined = posture['back_angle'] > self.recline_threshold
        if is_reclined:
            risk_level += 0.2
            risks.append('reclined_posture')
        
        # 2. 检测制动事件
        is_braking = vehicle['deceleration'] > self.braking_threshold
        if is_braking:
            risk_level += 0.3
            risks.append('active_braking')
        
        # 3. 检测头部前倾
        head_forward = abs(posture['head_pitch']) > self.head_forward_threshold
        if head_forward:
            risk_level += 0.25
            risks.append('head_forward_excessive')
        
        # 4. 预测制动后位移
        predicted_displacement = self._predict_displacement(
            vehicle['deceleration'], posture['back_angle']
        )
        if predicted_displacement > 0.15:  # 15cm
            risk_level += 0.25
            risks.append(f'predicted_displacement_{predicted_displacement:.2f}m')
        
        # 5. 方向盘距离
        if posture['head_to_wheel'] < 0.25:  # 25cm
            risk_level += 0.2
            risks.append('close_to_steering_wheel')
        
        return {
            'risk_level': min(risk_level, 1.0),
            'is_oop': risk_level > 0.5,
            'is_reclined': is_reclined,
            'is_braking': is_braking,
            'predicted_displacement': predicted_displacement,
            'risks': risks,
            'recommendation': self._recommend(risk_level),
        }
    
    def _predict_displacement(self, decel_g, back_angle):
        """预测制动后位移"""
        # 简化模型: 减速度越大 → 位移越大
        # reclined 姿态 → 位移更大
        base = decel_g * 0.08  # ~8cm per g
        recline_factor = 1 + (back_angle - 25) / 100  # 每增加1度多1%
        return base * max(recline_factor, 1.0)
    
    def _recommend(self, risk):
        if risk > 0.7:
            return 'CRITICAL: 预紧安全带 + 气囊调整'
        elif risk > 0.5:
            return 'WARNING: 座椅回正 + 安全带预紧'
        elif risk > 0.3:
            return 'CAUTION: 提醒乘员调整坐姿'
        return 'NORMAL'


# 测试
if __name__ == "__main__":
    detector = OOPBrakingDetector()
    
    # 正常驾驶
    normal = detector.assess_risk(
        posture={'back_angle': 20, 'head_pitch': 5, 'chest_pitch': 3, 'head_to_wheel': 0.35},
        vehicle={'deceleration': 0.1, 'speed': 60}
    )
    print("=== 正常驾驶 ===")
    print(f"风险: {normal['risk_level']:.0%}, OOP: {normal['is_oop']}")
    print(f"建议: {normal['recommendation']}\n")
    
    # Reclined + 制动
    risky = detector.assess_risk(
        posture={'back_angle': 35, 'head_pitch': 28, 'chest_pitch': 15, 'head_to_wheel': 0.50},
        vehicle={'deceleration': 0.8, 'speed': 80}
    )
    print("=== Reclined + 制动 ===")
    print(f"风险: {risky['risk_level']:.0%}, OOP: {risky['is_oop']}")
    print(f"风险项: {risky['risks']}")
    print(f"预测位移: {risky['predicted_displacement']:.2f}m")
    print(f"建议: {risky['recommendation']}")