import numpy as np
from typing import Dict, Tuple

class CPDRequirementValidator:
    """
    ANCAP 2026-2028 CPD合规验证器
    
    验证场景：
    1. 后座有儿童/婴儿 → 锁车后≤10秒检测到
    2. 后座无儿童 → 不误报
    3. 儿童在安全座椅中 → 检测到安全座椅+占用
    4. 儿童离开 → ≤30秒检测到空座
    5. 覆盖物遮挡 → 穿透毯子（≤5cm）检测
    """
    
    SCENARIOS = {
        'CPD-01': {'name': '后座有儿童', 'timeout': 10, 'required': True},
        'CPD-02': {'name': '后座无儿童', 'timeout': 30, 'required': False},
        'CPD-03': {'name': '安全座椅+儿童', 'timeout': 10, 'required': True},
        'CPD-04': {'name': '儿童离开', 'timeout': 30, 'required': False},
        'CPD-05': {'name': '毯子遮挡(5cm)', 'timeout': 15, 'required': True},
    }
    
    def validate_detection(self, scenario_id: str, 
                           detection_result: dict) -> Dict:
        """
        验证CPD检测结果是否符合ANCAP要求
        
        Args:
            scenario_id: 场景编号
            detection_result: {
                'detected': bool,
                'detection_time': float (秒),
                'confidence': float,
                'sensor_type': str
            }
        """
        scenario = self.SCENARIOS[scenario_id]
        passed = True
        issues = []
        
        if scenario['required'] and not detection_result['detected']:
            passed = False
            issues.append(f"未检测到儿童，违反{scenario['name']}")
        
        if detection_result['detection_time'] > scenario['timeout']:
            passed = False
            issues.append(
                f"检测时间{detection_result['detection_time']:.1f}s "
                f"超过限制{scenario['timeout']}s"
            )
        
        if not scenario['required'] and detection_result['detected']:
            passed = False
            issues.append(f"误报：{scenario['name']}不应检测到")
        
        return {
            'scenario': scenario_id,
            'passed': passed,
            'issues': issues,
            'result': detection_result
        }


class SeatbeltMisuseDetector:
    """
    ANCAP 2026-2028 安全带误用检测方案
    
    检测类别：
    1. 未佩戴 → 行车前强制检测
    2. 手臂下方佩戴 → 一级警告
    3. 背后佩戴 → 一级警告
    4. 扭曲佩戴 → 二级警告
    5. 松弛佩戴 → 二级警告
    """
    
    MISUSE_TYPES = {
        'SB-01': {'name': '未佩戴', 'level': 'critical', 'action': '禁止起步'},
        'SB-02': {'name': '手臂下方', 'level': 'L1', 'action': '一级警告'},
        'SB-03': {'name': '背后佩戴', 'level': 'L1', 'action': '一级警告'},
        'SB-04': {'name': '扭曲佩戴', 'level': 'L2', 'action': '二级警告'},
        'SB-05': {'name': '松弛佩戴', 'level': 'L2', 'action': '二级警告'},
    }
    
    def detect_misuse(self, keypoints: np.ndarray) -> Dict:
        """
        基于关键点的安全带误用检测
        
        Args:
            keypoints: 安全带路径关键点 (N, 2) 像素坐标
            
        Returns:
            detection: 误用类型和警告等级
        """
        # 计算安全带路径特征
        path_length = self._path_length(keypoints)
        curvature = self._curvature(keypoints)
        crossing_pattern = self._belt_crossing_pattern(keypoints)
        
        # 分类逻辑
        if len(keypoints) < 3:
            return {'type': 'SB-01', **self.MISUSE_TYPES['SB-01']}
        
        if crossing_pattern == 'under_arm':
            return {'type': 'SB-02', **self.MISUSE_TYPES['SB-02']}
        
        if crossing_pattern == 'behind_back':
            return {'type': 'SB-03', **self.MISUSE_TYPES['SB-03']}
        
        if curvature > 0.5:  # 扭曲阈值
            return {'type': 'SB-04', **self.MISUSE_TYPES['SB-04']}
        
        # 检查松弛度
        expected_path = self._expected_path(keypoints[0], keypoints[-1])
        if path_length > expected_path * 1.15:
            return {'type': 'SB-05', **self.MISUSE_TYPES['SB-05']}
        
        return {'type': 'OK', 'name': '正常佩戴', 'level': 'none'}
    
    def _path_length(self, pts):
        return sum(np.linalg.norm(pts[i+1]-pts[i]) 
                   for i in range(len(pts)-1))
    
    def _curvature(self, pts):
        if len(pts) < 3:
            return 0
        angles = []
        for i in range(len(pts)-2):
            v1 = pts[i+1] - pts[i]
            v2 = pts[i+2] - pts[i+1]
            cos_a = np.dot(v1, v2) / (np.linalg.norm(v1)*np.linalg.norm(v2)+1e-8)
            angles.append(np.arccos(np.clip(cos_a, -1, 1)))
        return np.mean(angles) if angles else 0
    
    def _belt_crossing_pattern(self, pts):
        # 简化：检查路径是否经过胸前
        if len(pts) < 4:
            return 'unknown'
        # 正常：肩→胸→腰
        # 手臂下方：路径偏低
        # 背后：路径绕过肩部
        return 'normal'  # 简化
    
    def _expected_path(self, start, end):
        return np.linalg.norm(end - start)


class OOPPostureDetector:
    """
    ANCAP 2026-2028 异常姿态检测方案
    
    五类姿态：
    1. 身体前倾 → 气囊伤害 → 降级气囊
    2. 身体侧倾 → 安全带失效 → 警告
    3. 后仰过度 → 安全带脱离 → 警告
    4. 脚搭仪表 → 碰撞伤害 → 警告
    5. 跪姿/反坐 → 严重碰撞风险 → 紧急警告
    """
    
    POSTURE_TYPES = {
        'OOP-01': {'name': '身体前倾', 'risk': '气囊伤害', 'action': '降级气囊'},
        'OOP-02': {'name': '身体侧倾', 'risk': '安全带失效', 'action': '警告'},
        'OOP-03': {'name': '后仰过度', 'risk': '安全带脱离', 'action': '警告'},
        'OOP-04': {'name': '脚搭仪表', 'risk': '碰撞伤害', 'action': '警告'},
        'OOP-05': {'name': '跪姿/反坐', 'risk': '严重碰撞', 'action': '紧急警告'},
    }
    
    def classify_posture(self, body_keypoints: np.ndarray) -> Dict:
        """
        基于3D关键点的姿态分类
        
        Args:
            body_keypoints: (17, 3) 17个3D关键点 (x, y, z)
                0: nose, 1-4: L/R shoulder/elbow/wrist
                5-8: R shoulder/elbow/wrist
                9-12: L/R hip/knee/ankle
                13-16: R hip/knee/ankle
        """
        # 计算关键角度
        torso_lean = self._torso_lean_angle(body_keypoints)
        torso_lateral_tilt = self._lateral_tilt(body_keypoints)
        back_angle = self._back_angle(body_keypoints)
        leg_position = self._leg_position(body_keypoints)
        
        # 分类
        if torso_lean > 25:  # 前倾>25°
            return {'type': 'OOP-01', **self.POSTURE_TYPES['OOP-01'],
                    'lean_angle': torso_lean}
        
        if abs(torso_lateral_tilt) > 20:
            return {'type': 'OOP-02', **self.POSTURE_TYPES['OOP-02'],
                    'tilt_angle': torso_lateral_tilt}
        
        if back_angle < 70:  # 后仰
            return {'type': 'OOP-03', **self.POSTURE_TYPES['OOP-03'],
                    'back_angle': back_angle}
        
        if leg_position == 'dashboard':
            return {'type': 'OOP-04', **self.POSTURE_TYPES['OOP-04']}
        
        if leg_position == 'kneeling_reversed':
            return {'type': 'OOP-05', **self.POSTURE_TYPES['OOP-05']}
        
        return {'type': 'normal', 'name': '正常坐姿', 'action': 'none'}
    
    def _torso_lean_angle(self, pts):
        """前倾角度：肩-髋连线与垂直方向的夹角"""
        shoulder = (pts[5] + pts[6]) / 2  # 双肩中点
        hip = (pts[11] + pts[12]) / 2     # 双髋中点
        torso = shoulder - hip
        # 与垂直方向(y轴)的夹角
        vertical = np.array([0, 1, 0])
        cos_a = np.dot(torso, vertical) / (np.linalg.norm(torso) + 1e-8)
        return np.degrees(np.arccos(np.clip(cos_a, -1, 1)))
    
    def _lateral_tilt(self, pts):
        """侧倾角度"""
        shoulder = (pts[5] + pts[6]) / 2
        hip = (pts[11] + pts[12]) / 2
        torso = shoulder - hip
        # 在xz平面的投影
        horizontal = np.array([1, 0, 0])
        cos_a = np.dot(torso[[0,2]], horizontal[[0,2]]) / (
            np.linalg.norm(torso[[0,2]]) + 1e-8)
        return np.degrees(np.arccos(np.clip(cos_a, -1, 1)))
    
    def _back_angle(self, pts):
        """后仰角度"""
        return self._torso_lean_angle(pts)  # 负值表示后仰
    
    def _leg_position(self, pts):
        """腿部位置分类"""
        ankle = pts[16]  # 右踝
        hip = pts[12]     # 右髋
        # 脚高于髋 → 搭在仪表
        if ankle[1] > hip[1]:
            return 'dashboard'
        return 'normal'


# 测试
if __name__ == "__main__":
    print("=" * 60)
    print("ANCAP 2026-2028 合规验证 - 测试")
    print("=" * 60)
    
    # CPD测试
    cpd = CPDRequirementValidator()
    result = cpd.validate_detection('CPD-01', {
        'detected': True, 'detection_time': 7.5, 
        'confidence': 0.92, 'sensor_type': '60GHz_radar'
    })
    print(f"\nCPD-01: {result}")
    
    result = cpd.validate_detection('CPD-05', {
        'detected': True, 'detection_time': 12.0,
        'confidence': 0.78, 'sensor_type': 'UWB'
    })
    print(f"CPD-05: {result}")
    
    # 安全带测试
    sb = SeatbeltMisuseDetector()
    pts = np.array([[100, 50], [120, 100], [110, 200], [105, 300]])
    result = sb.detect_misuse(pts)
    print(f"\n安全带: {result}")
    
    # 姿态测试
    oop = OOPPostureDetector()
    # 模拟前倾姿态
    keypoints = np.random.randn(17, 3) * 100
    keypoints[5] = [0, 150, 0]   # 左肩前倾
    keypoints[6] = [50, 150, 0]  # 右肩前倾
    keypoints[11] = [0, 50, 0]   # 左髋
    keypoints[12] = [50, 50, 0]  # 右髋
    result = oop.classify_posture(keypoints)
    print(f"\n姿态: {result}")