# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# SPDX-License-Identifier: BSD-3-Clause

"""Test scene: spawn room (static), table (rigid body with gravity), centrifuge (articulation with gravity).

.. code-block:: bash

    # Usage
    ~/IsaacLab/isaaclab.sh -p source/mindbot/mindbot/utils/test.sence.py

"""

"""Launch Isaac Sim Simulator first."""

import argparse

from isaaclab.app import AppLauncher

parser = argparse.ArgumentParser(description="Test scene with physics assets.")
AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

"""Rest everything follows."""

import isaaclab.sim as sim_utils
from isaaclab.assets import Articulation, RigidObject

from mindbot.assets.lab import ROOM_CFG
from mindbot.assets.table import TABLE_RIGID_CFG
from mindbot.assets.centrifuge import CENTRIFUGE_CFG


def design_scene() -> dict:
    """搭建场景：地面、灯光、静态房间、动态桌子（刚体）、离心机（关节体）。"""
    # 地面
    cfg_ground = sim_utils.GroundPlaneCfg()
    cfg_ground.func("/World/defaultGroundPlane", cfg_ground)

    # 远光灯
    cfg_light = sim_utils.DistantLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
    cfg_light.func("/World/lightDistant", cfg_light, translation=(1, 0, 10))

    # 静态房间（无物理，直接 spawn USD prim，不受重力影响）
    room_cfg = ROOM_CFG.replace(prim_path="/World/Room")
    room_cfg.spawn.func("/World/Room", room_cfg.spawn)

    # ✅ 动态桌子（RigidObject，有重力）
    # 关键：必须用 RigidObject 实例化，spawn.func() 只放 USD prim，不会应用 rigid_props
    # TABLE_RIGID_CFG 包含 rigid_props(disable_gravity=False)
    table_cfg = TABLE_RIGID_CFG.replace(prim_path="/World/Table")
    table = RigidObject(cfg=table_cfg)

    # ✅ 离心机（Articulation，有重力）
    # 关键：必须用 Articulation 实例化，才能应用 rigid_props 和 init_state
    # CENTRIFUGE_CFG 包含 rigid_props(disable_gravity=False) 和 fix_root_link=False
    centrifuge_cfg = CENTRIFUGE_CFG.replace(prim_path="/World/Centrifuge")
    centrifuge = Articulation(cfg=centrifuge_cfg)

    return {"table": table, "centrifuge": centrifuge}


def main():
    """主函数。"""
    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
    sim = sim_utils.SimulationContext(sim_cfg)
    sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5])

    # 搭建场景（必须在 sim.reset() 之前完成）
    scene_entities = design_scene()
    table: RigidObject = scene_entities["table"]
    centrifuge: Articulation = scene_entities["centrifuge"]

    # 启动仿真（触发 PhysX 物理初始化）
    sim.reset()

    # 将 init_state（位置/旋转/关节角）写入仿真，并初始化内部缓存
    table.reset()
    centrifuge.reset()

    print("[INFO]: Setup complete. Table and centrifuge have gravity enabled.")

    # 仿真主循环
    while simulation_app.is_running():
        # 推进物理步（重力在此生效）
        sim.step()
        # 更新资产内部状态缓存（读取最新物理状态，供后续查询用）
        table.update(sim_cfg.dt)
        centrifuge.update(sim_cfg.dt)


if __name__ == "__main__":
    main()
    simulation_app.close()
