icf_body_config = {
    "sensors": {
        "rgb_camera": {
            "resolution": "1920x1080",
            "fps": 30,
            "location": "A柱/后视镜上方",
            "purpose": "面部特征、上半身姿态"
        },
        "depth_camera": {
            "type": "ToF (Time-of-Flight)",
            "resolution": "640x480",
            "fps": 30,
            "range": "0.5-5m",
            "purpose": "3D骨架、深度信息"
        },
        "infrared_camera": {
            "resolution": "1280x1024",
            "fps": 30,
            "wavelength": "940nm",
            "purpose": "夜间姿态、眼球追踪"
        },
        "seat_pressure": {
            "type": "压敏阵列",
            "resolution": "64x64 sensors",
            "fps": 10,
            "purpose": "坐姿分布、体重估计"
        },
        "radar": {
            "type": "60GHz mmWave",
            "resolution": "range-Doppler map",
            "fps": 20,
            "purpose": "穿透遮挡检测、微动感知"
        }
    },
    "ground_truth": {
        "system": "Vicon optical motion capture",
        "markers": 52,  # 全身标记点
        "accuracy": "<1mm",
        "purpose": "3D骨架真值"
    },
    "participants": {
        "count": 40,  # 多样化人体测量
        "height_range": "150-195cm",
        "weight_range": "45-110kg",
        "gender": "balanced",
        "clothing": "varied (winter/summer)"
    },
    "scenarios": {
        "postures": [
            "normal_seating",
            "forward_leaning",
            "side_leaning",
            "backward_reclined",
            "feet_on_dashboard",
            "reading_phone",
            "sleeping",
            "reaching_rear"
        ],
        "conditions": ["daylight","night","tunnel","backlit"],
        "vehicles": ["sedan","SUV","compact"]
    }
}

dataset_stats = {
    "total_sequences": 560,      # 40人×14场景
    "total_frames": 168000,      # 560×30fps×10s
    "annotated_keypoints": 2856000,  # 168000×17
    "sensor_modalities": 5,      # RGB+Depth+IR+Pressure+Radar
    "vehicle_types": 3,          # sedan+SUV+compact
    "lighting_conditions": 4,    # daylight+night+tunnel+backlit
    "storage_size": "~180GB"
}

import torch
import torch.nn as nn

class ICFFusionModel(nn.Module):
    """
    ICF-Fusion: 多模态座舱乘员姿态融合模型
    
    融合5种传感器模态，输出3D姿态+人体测量+OOP分类
    
    参考框架来自: Preu et al., Vehicles 8(8), 2026
    """
    
    def __init__(self, num_keypoints=17, num_oop_classes=5, 
                 num_anthro=4):
        super().__init__()
        
        # 1. RGB编码器（面部+上半身特征）
        self.rgb_encoder = RGBEncoder(in_channels=3, feat_dim=256)
        
        # 2. 深度编码器（3D结构信息）
        self.depth_encoder = DepthEncoder(in_channels=1, feat_dim=256)
        
        # 3. 红外编码器（夜间/低光）
        self.ir_encoder = IREncoder(in_channels=1, feat_dim=128)
        
        # 4. 座椅压力编码器
        self.pressure_encoder = PressureEncoder(
            input_size=64, feat_dim=128
        )
        
        # 5. 雷达编码器
        self.radar_encoder = RadarEncoder(feat_dim=128)
        
        # 跨模态注意力融合
        total_dim = 256 + 256 + 128 + 128 + 128  # =896
        self.cross_modal_attention = CrossModalAttention(
            d_model=total_dim, nhead=8
        )
        
        # 输出头
        self.pose_head = nn.Linear(total_dim, num_keypoints * 3)
        self.anthro_head = nn.Sequential(
            nn.Linear(total_dim, 128),
            nn.ReLU(),
            nn.Linear(128, num_anthro)
        )
        self.oop_head = nn.Sequential(
            nn.Linear(total_dim, 128),
            nn.ReLU(),
            nn.Linear(128, num_oop_classes)
        )
        self.head_pos_head = nn.Linear(total_dim, 3)  # 头部3D坐标
    
    def forward(self, rgb, depth, ir, pressure, radar):
        # 各模态编码
        rgb_feat = self.rgb_encoder(rgb)       # (B, 256)
        depth_feat = self.depth_encoder(depth) # (B, 256)
        ir_feat = self.ir_encoder(ir)          # (B, 128)
        pressure_feat = self.pressure_encoder(pressure)  # (B, 128)
        radar_feat = self.radar_encoder(radar) # (B, 128)
        
        # 拼接
        fused = torch.cat([
            rgb_feat, depth_feat, ir_feat,
            pressure_feat, radar_feat
        ], dim=1)  # (B, 896)
        
        # 跨模态注意力
        fused = self.cross_modal_attention(fused.unsqueeze(1)).squeeze(1)
        
        # 多任务输出
        pose = self.pose_head(fused).view(-1, 17, 3)
        anthro = self.anthro_head(fused)
        oop = self.oop_head(fused)
        head_pos = self.head_pos_head(fused)
        
        return {
            "pose_3d": pose,
            "anthropometrics": anthro,
            "oop_class": oop,
            "head_position": head_pos
        }


class RGBEncoder(nn.Module):
    """RGB特征提取（基于ResNet-18简化）"""
    def __init__(self, in_channels=3, feat_dim=256):
        super().__init__()
        self.backbone = nn.Sequential(
            nn.Conv2d(in_channels, 64, 7, 2, 3),
            nn.BatchNorm2d(64), nn.ReLU(),
            nn.MaxPool2d(3, 2, 1),
            *self._make_layer(64, 64, 2),
            *self._make_layer(64, 128, 2, stride=2),
            *self._make_layer(128, 256, 2, stride=2),
            nn.AdaptiveAvgPool2d(1),
        )
        self.fc = nn.Linear(256, feat_dim)
    
    def _make_layer(self, in_c, out_c, num_blocks, stride=1):
        layers = [nn.Conv2d(in_c, out_c, 3, stride, 1, bias=False),
                  nn.BatchNorm2d(out_c), nn.ReLU()]
        for _ in range(num_blocks - 1):
            layers += [nn.Conv2d(out_c, out_c, 3, 1, 1, bias=False),
                       nn.BatchNorm2d(out_c), nn.ReLU()]
        return layers
    
    def forward(self, x):
        x = self.backbone(x)
        return self.fc(x.flatten(1))


class DepthEncoder(nn.Module):
    """深度图编码器"""
    def __init__(self, in_channels=1, feat_dim=256):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Conv2d(in_channels, 32, 5, 2, 2),
            nn.BatchNorm2d(32), nn.ReLU(),
            nn.Conv2d(32, 64, 3, 2, 1),
            nn.BatchNorm2d(64), nn.ReLU(),
            nn.Conv2d(64, 128, 3, 2, 1),
            nn.BatchNorm2d(128), nn.ReLU(),
            nn.Conv2d(128, 256, 3, 2, 1),
            nn.BatchNorm2d(256), nn.ReLU(),
            nn.AdaptiveAvgPool2d(1)
        )
        self.fc = nn.Linear(256, feat_dim)
    
    def forward(self, x):
        return self.fc(self.encoder(x).flatten(1))


class PressureEncoder(nn.Module):
    """座椅压力阵列编码器"""
    def __init__(self, input_size=64, feat_dim=128):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Conv2d(1, 16, 3, 1, 1), nn.ReLU(),
            nn.Conv2d(16, 32, 3, 2, 1), nn.ReLU(),
            nn.Conv2d(32, 64, 3, 2, 1), nn.ReLU(),
            nn.AdaptiveAvgPool2d(1)
        )
        self.fc = nn.Linear(64, feat_dim)
    
    def forward(self, x):  # x: (B, 1, 64, 64)
        return self.fc(self.encoder(x).flatten(1))


class CrossModalAttention(nn.Module):
    """跨模态注意力机制"""
    def __init__(self, d_model=896, nhead=8):
        super().__init__()
        self.attention = nn.MultiheadAttention(d_model, nhead, batch_first=True)
        self.norm = nn.LayerNorm(d_model)
        self.ffn = nn.Sequential(
            nn.Linear(d_model, d_model * 2),
            nn.ReLU(),
            nn.Linear(d_model * 2, d_model)
        )
    
    def forward(self, x):
        attn_out, _ = self.attention(x, x, x)
        x = self.norm(x + attn_out)
        x = self.norm(x + self.ffn(x))
        return x


# 测试
if __name__ == "__main__":
    model = ICFFusionModel()
    
    # 模拟输入
    rgb = torch.randn(4, 3, 224, 224)
    depth = torch.randn(4, 1, 224, 224)
    ir = torch.randn(4, 1, 224, 224)
    pressure = torch.randn(4, 1, 64, 64)
    radar = torch.randn(4, 1, 64, 100)  # range-Doppler map
    
    # 简化测试（实际需要radar encoder）
    # outputs = model(rgb, depth, ir, pressure, radar)
    print(f"模型参数量: {sum(p.numel() for p in model.parameters()):,}")

class OOPDetectionSystem:
    """
    基于ICF-Fusion的OOP检测系统
    对接Euro NCAP 2026 OOP检测要求
    """
    
    OOP_THRESHOLDS = {
        "head_to_wheel_distance": 250,  # mm, 安全下限
        "head_to_dashboard_distance": 300,  # mm
        "torso_angle_threshold": 25,  # 度, 前倾阈值
        "side_lean_threshold": 15,  # 度, 侧倾阈值
        "recline_angle_threshold": 30,  # 度, 后仰阈值
    }
    
    def __init__(self):
        self.fusion_model = ICFFusionModel()
        self.safety_level = "NORMAL"
    
    def assess_oop_risk(self, sensor_data):
        """
        评估OOP风险等级
        
        Returns:
            risk_level: NORMAL/ELEVATED/HIGH/CRITICAL
            intervention: 安全干预策略
        """
        outputs = self.fusion_model(**sensor_data)
        pose_3d = outputs["pose_3d"]  # 17关键点3D
        head_pos = outputs["head_position"]  # 头部3D坐标
        oop_class = outputs["oop_class"]  # OOP类别
        
        # 计算安全指标
        head_to_wheel = self._calc_distance(head_pos, "steering_wheel")
        torso_angle = self._calc_torso_angle(pose_3d)
        
        # 风险评级
        if head_to_wheel < self.OOP_THRESHOLDS["head_to_wheel_distance"]:
            return "CRITICAL", "DISABLE_AIRBAG_LOW_POWER"
        elif torso_angle > self.OOP_THRESHOLDS["torso_angle_threshold"]:
            return "HIGH", "ADJUST_AIRBAG_DEPLOYMENT_ANGLE"
        else:
            return "NORMAL", "STANDARD_DEPLOYMENT"