from isaaclab.assets import ArticulationCfg, AssetBaseCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils import configclass
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab import sim as sim_utils

import os

_DEMO_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
T1_USD_PATH = os.path.join(_DEMO_DIR, "asset", "t1", "T1_locomotion_physics_lab.usd")

@configclass
class T1SceneCfg(InteractiveSceneCfg):
    """最终修正版：彻底解决 Unknown asset config type 报错"""

    # 1. 地面配置：直接在 spawn 内部定义材质
    ground = AssetBaseCfg(
        prim_path="/World/ground",
        spawn=sim_utils.GroundPlaneCfg(
            physics_material=sim_utils.RigidBodyMaterialCfg(
                static_friction=1.0,
                dynamic_friction=1.0,
                restitution=0.3,
                friction_combine_mode="average",
                restitution_combine_mode="average",
            )
        ),
    )

    # 2. 机器人配置
    robot = ArticulationCfg(
        prim_path="{ENV_REGEX_NS}/Robot",
        spawn=sim_utils.UsdFileCfg(
            usd_path=T1_USD_PATH,
            activate_contact_sensors=True,
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=False,
                max_depenetration_velocity=10.0,
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=True,
                solver_position_iteration_count=8,
                solver_velocity_iteration_count=4,
            ),
        ),
        init_state=ArticulationCfg.InitialStateCfg(
            pos=(0.0, 0.0, 0.2), # 掉落高度
            joint_pos={".*": 0.0},
        ),
        actuators={
            # 1. 核心承重关节：大腿、膝盖、腰部
            "heavy_joints": ImplicitActuatorCfg(
                joint_names_expr=["L.*_Hip_.*", "R.*_Hip_.*", "L.*_Knee_.*", "R.*_Knee_.*", "Waist.*"],
                effort_limit=800.0,
                velocity_limit=15.0,
                stiffness=600.0,
                damping=30.0,
            ),
            # 2. 末端/轻型关节：手臂、脚踝、头部
            "light_joints": ImplicitActuatorCfg(
                joint_names_expr=["L.*_Shoulder_.*", "R.*_Shoulder_.*", "L.*_Elbow_.*", "R.*_Elbow_.*", ".*Ankle.*", ".*Head.*"],
                effort_limit=300.0,
                velocity_limit=25.0,
                stiffness=200.0,
                damping=10.0,
            ),
        },
    )

    contact_sensor = ContactSensorCfg(
        prim_path="{ENV_REGEX_NS}/Robot/.*",
        update_period=0.0,
        history_length=3,
    )

    # 3. 光照配置
    light = AssetBaseCfg(
        prim_path="/World/light",
        spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0),
    )
