# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

import math
from re import T
from tkinter import N
import torch
from isaaclab.actuators import ImplicitActuatorCfg
import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import ObservationGroupCfg as ObsGroup
from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.managers import RewardTermCfg as RewTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.managers import CurriculumTermCfg as CurrTerm
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.utils import configclass
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.envs.mdp.actions.actions_cfg import (
    DifferentialInverseKinematicsActionCfg,
    BinaryJointPositionActionCfg,
)

from . import mdp

##
# Pre-defined configs
##

from mindbot.robot.mindbot import MINDBOT_CFG

from mindbot.utils.assets import MINDBOT_ASSETS_DIR

# ====== 其他物体配置 ======
from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
from isaaclab.sim.schemas.schemas_cfg import (
    RigidBodyPropertiesCfg,
    CollisionPropertiesCfg,
)


TABLE_CFG = RigidObjectCfg(
    prim_path="{ENV_REGEX_NS}/Table",
    init_state=RigidObjectCfg.InitialStateCfg(
        pos=[0.95, -0.3, 0.005],
        rot=[0.7071, 0.0, 0.0, -0.7071],
        lin_vel=[0.0, 0.0, 0.0],
        ang_vel=[0.0, 0.0, 0.0],
    ),
    spawn=UsdFileCfg(
        usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/table/Table_C.usd",
        rigid_props=RigidBodyPropertiesCfg(
            rigid_body_enabled=True,
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=16,
            disable_gravity=False,
        ),
        collision_props=CollisionPropertiesCfg(
            collision_enabled=True,
            contact_offset=0.0005,  # original 0.02
            rest_offset=0,
        ),
    ),
)

DRYINGBOX_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=f"{MINDBOT_ASSETS_DIR}/assets/Collected_Equipment_BB_0B/Equipment/Equipment_BB_13.usd",
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False, max_depenetration_velocity=1.0
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=False,  #
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=16,
            stabilization_threshold=1e-6,
            # fix_root_link=True,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=[0.95, -0.45, 0.9], rot=[-0.7071, 0.0, 0.0, 0.7071]
    ),
    # actuators={}
    actuators={
        "passive_damper": ImplicitActuatorCfg(
            # ".*" 表示匹配该USD文件内的所有关节（无论是轮子、屏幕转轴还是其他）
            joint_names_expr=["RevoluteJoint"],
            stiffness=10000.0,
            damping=1000.0,
            effort_limit_sim=10000.0,
            velocity_limit_sim=100.0,
        ),
    },
)

LID_CFG = RigidObjectCfg(
    prim_path="{ENV_REGEX_NS}/Lid",
    init_state=RigidObjectCfg.InitialStateCfg(
        pos=[0.2, 0.65, 0.9],
        rot=[1.0, 0.0, 0.0, 0.0],
        lin_vel=[0.0, 0.0, 0.0],
        ang_vel=[0.0, 0.0, 0.0],
    ),
    spawn=UsdFileCfg(
        usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound_lid.usd",
        # scale=(0.2, 0.2, 0.2),
        rigid_props=RigidBodyPropertiesCfg(
            rigid_body_enabled=True,
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=16,
            max_angular_velocity=1000.0,
            max_linear_velocity=1000.0,
            max_depenetration_velocity=0.5,  # original 5.0
            linear_damping=5.0,  # original 0.5
            angular_damping=5.0,  # original 0.5
            disable_gravity=False,
        ),
        collision_props=CollisionPropertiesCfg(
            collision_enabled=True,
            contact_offset=0.0005,  # original 0.02
            rest_offset=0,
        ),
    ),
)


ULTRASOUND_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=f"{MINDBOT_ASSETS_DIR}/equipment/ultrasound/ultrasound.usd",
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False, max_depenetration_velocity=1.0
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=False,  #
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=16,
            stabilization_threshold=1e-6,
            # fix_root_link=True,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(pos=(0.25, 0.65, 0.1)),
    # actuators={}
    actuators={
        "passive_damper": ImplicitActuatorCfg(
            # ".*" 表示匹配该USD文件内的所有关节（无论是轮子、屏幕转轴还是其他）
            joint_names_expr=[".*"],
            stiffness=0.0,
            damping=1000.0,
            effort_limit_sim=10000.0,
            velocity_limit_sim=100.0,
        ),
    },
)

##
# Scene definition
##
from isaaclab.sensors import CameraCfg
import isaaclab.sim as sim_utils


@configclass
class PullSceneCfg(InteractiveSceneCfg):
    """Configuration for a cart-pole scene."""

    # ground plane
    ground = AssetBaseCfg(
        prim_path="/World/ground",
        spawn=sim_utils.GroundPlaneCfg(size=(100.0, 100.0)),
    )

    # robot
    Mindbot: ArticulationCfg = MINDBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Mindbot")
    table: RigidObjectCfg = TABLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Table")
    dryingbox: ArticulationCfg = DRYINGBOX_CFG.replace(
        prim_path="{ENV_REGEX_NS}/Dryingbox"
    )
    # ultrasound: ArticulationCfg = ULTRASOUND_CFG.replace(prim_path="{ENV_REGEX_NS}/ultrasound")
    # lid: RigidObjectCfg = LID_CFG.replace(prim_path="{ENV_REGEX_NS}/Lid")

    # lights
    dome_light = AssetBaseCfg(
        prim_path="/World/DomeLight",
        spawn=sim_utils.DomeLightCfg(color=(0.9, 0.9, 0.9), intensity=500.0),
    )
    # # ------------- 新增 6 路相机 -------------
    # cam_head = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/robot_head/cam_head",
    #     spawn=sim_utils.PinholeCameraCfg(
    #         focal_length=1.93,  # 使用你给的 1.93
    #         # 由 fx = W * f / aperture 推得 aperture = W * f / fx
    #         horizontal_aperture=1280 * 1.93 / 911.77,  # ≈ 2.71，单位与 focal_length 一致
    #         clipping_range=(0.1, 1e5),
    #     ),
    #     offset=CameraCfg.OffsetCfg(
    #         pos=(0.1226748226, 0.0, 0.1130563345),
    #         rot=(-0.5, 0.5, -0.5, 0.5),
    #         # convention="ros",

    #         # rot=(0.70710678, 0, 0.70710678, 0),
    #         convention="ros",
    #     ),
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    #     update_latest_camera_pose=True,
    # )

    # cam_chest = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/robot_trunk/cam_chest",
    #     spawn=sim_utils.PinholeCameraCfg(
    #         focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1e5)
    #     ),
    #     offset=CameraCfg.OffsetCfg(
    #         pos=(-0.14909, 0.3549, -0.23295),
    #         rot=(0.99452, 0.0, 0.10453, 0.0),#(0.99452, 0.0, 0.10453, 0.0),#
    #         convention="ros",
    #     ),
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    #     update_latest_camera_pose=True,
    # )

    # cam_left_hand = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand",
    #     spawn=sim_utils.PinholeCameraCfg(
    #         focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1e5)
    #     ),
    #     offset=CameraCfg.OffsetCfg(
    #         pos=(0.0610568560, 1.13e-8, 0.0683504405),#()
    #         rot=(0.99452, 0, 0.10453, 0),#(0.99452, 0, 0.10453, 0)
    #         convention="ros",
    #     ),
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    #     update_latest_camera_pose=True,
    # )

    # cam_right_hand = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand",
    #     spawn=sim_utils.PinholeCameraCfg(
    #         focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1e5)
    #     ),
    #     offset=CameraCfg.OffsetCfg(
    #         pos=(0.0610571709, 2.58e-7, 0.0711255539),#（0.0610568560, 1.13e-8, 0.0683504405）
    #         rot=(0.445, -0.549, -0.549, -0.445),#（0.99452, 0, 0.10453, 0）
    #         convention="ros",
    #     ),
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    #     update_latest_camera_pose=True,
    # )

    # cam_top = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/cam_top",
    #     spawn=sim_utils.PinholeCameraCfg(
    #         focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1e5)
    #     ),
    #     offset=CameraCfg.OffsetCfg(
    #         pos=(0.4627602211, 0.0, 2.0977711170),
    #         rot=(0.83526, 0, 0.54986, 0),
    #         convention="ros",
    #     ),
    #     data_types=["rgb"],
    #     width=960,
    #     height=720,
    #     update_latest_camera_pose=False,
    # )

    # cam_side = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/cam_side",
    #     spawn=sim_utils.PinholeCameraCfg(
    #         focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1e5)
    #     ),
    #     offset=CameraCfg.OffsetCfg(
    #         pos=(0.76709, -0.16995, 1.13007),
    #         rot=(0.61427, 0, 0.02598, 0.78866),
    #         convention="ros",
    #     ),
    #     data_types=["rgb"],
    #     width=960,
    #     height=720,
    #     update_latest_camera_pose=False,
    # )
    # # ------------- 相机结束 -------------


##
# MDP settings
##


def right_gripper_pos_w(
    env, asset_cfg: SceneEntityCfg = SceneEntityCfg("Mindbot")
) -> torch.Tensor:
    asset = env.scene[asset_cfg.name]
    body_ids, _ = asset.find_bodies(["right_hand_body"], preserve_order=True)
    pos_w = asset.data.body_pos_w[:, body_ids[0]] - env.scene.env_origins
    return pos_w


def get_body_pos_rel(env, asset_cfg: SceneEntityCfg) -> torch.Tensor:
    asset = env.scene[asset_cfg.name]
    body_ids, _ = asset.find_bodies(asset_cfg.body_names, preserve_order=True)
    return asset.data.body_pos_w[:, body_ids[0]] - env.scene.env_origins


# 新增：通用获取 body 世界姿态的函数
def get_body_quat_w(env, asset_cfg: SceneEntityCfg) -> torch.Tensor:
    asset = env.scene[asset_cfg.name]
    body_ids, _ = asset.find_bodies(asset_cfg.body_names, preserve_order=True)
    return asset.data.body_quat_w[:, body_ids[0]]


@configclass
class ActionsCfg:
    """Action specifications for the MDP."""

    # 使用任务空间控制器：策略输出末端执行器位置+姿态增量（6维）
    right_arm_ee = DifferentialInverseKinematicsActionCfg(
        asset_name="Mindbot",
        joint_names=["r_joint[1-6]"],  # 左臂的6个关节
        body_name="right_hand_body",  # 末端执行器body名称
        controller=DifferentialIKControllerCfg(
            command_type="pose",  # 控制位置+姿态
            use_relative_mode=True,  # 相对模式：动作是增量
            ik_method="dls",  # Damped Least Squares方法
        ),
        scale=(0.01, 0.01, 0.01, 0.025, 0.025, 0.025),
    )

    grippers_position = mdp.BinaryJointPositionActionCfg(
        asset_name="Mindbot",
        joint_names=["right_hand_joint_left", "right_hand_joint_right"],
        # 修正：使用字典格式
        # open_command_expr={"关节名正则": 值}
        open_command_expr={"right_hand_joint_left": 0.0, "right_hand_joint_right": 0.0},
        # close_command_expr={"关节名正则": 值}
        close_command_expr={
            "right_hand_joint_left": -0.03,
            "right_hand_joint_right": 0.03,
        },
    )

    trunk_position = mdp.JointPositionActionCfg(
        asset_name="Mindbot",
        joint_names=["PrismaticJoint"],
        scale=0.00,
        offset=0.25,
        clip=None,
    )


@configclass
class ObservationsCfg:
    """Observation specifications for the MDP."""

    @configclass
    class PolicyCfg(ObsGroup):
        """Observations for policy group."""

        mindbot_joint_pos_rel = ObsTerm(
            func=mdp.joint_pos_rel, params={"asset_cfg": SceneEntityCfg("Mindbot")}
        )
        mindbot_root_pos = ObsTerm(
            func=mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("Mindbot")}
        )
        mindbot_root_quat = ObsTerm(
            func=mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("Mindbot")}
        )

        right_gripper_pos = ObsTerm(
            func=right_gripper_pos_w,
            params={
                "asset_cfg": SceneEntityCfg("Mindbot", body_names=["right_hand_body"])
            },
        )
        dryingbox_handle_pos = ObsTerm(
            func=get_body_pos_rel,
            params={
                "asset_cfg": SceneEntityCfg(
                    "dryingbox", body_names=["Equipment_BB_13_C"]
                )
            },
        )
        dryingbox_handle_quat = ObsTerm(
            func=get_body_quat_w,
            params={
                "asset_cfg": SceneEntityCfg(
                    "dryingbox", body_names=["Equipment_BB_13_C"]
                )
            },
        )
        # lid_pos_abs = ObsTerm(func=mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("lid")})
        # ultrasound_pos_abs = ObsTerm(func=mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("ultrasound")})

        def __post_init__(self) -> None:
            self.enable_corruption = False
            self.concatenate_terms = True

    # observation groups
    policy: PolicyCfg = PolicyCfg()


@configclass
class EventCfg:
    """Configuration for events."""

    # 重置所有关节到 init_state（无偏移）
    reset_mindbot_all_joints = EventTerm(
        func=mdp.reset_joints_by_offset,
        mode="reset",
        params={
            "asset_cfg": SceneEntityCfg("Mindbot"),
            "position_range": (0.0, 0.0),
            "velocity_range": (0.0, 0.0),
        },
    )
    # 只对左臂添加随机偏移
    reset_mindbot_right_arm = EventTerm(
        func=mdp.reset_joints_by_offset,
        mode="reset",
        params={
            "asset_cfg": SceneEntityCfg("Mindbot", joint_names=["r_joint[1-6]"]),
            "position_range": (0.0, 0.0),
            "velocity_range": (0.0, 0.0),
        },
    )
    reset_table = EventTerm(
        func=mdp.reset_root_state_uniform,
        mode="reset",
        params={
            "asset_cfg": SceneEntityCfg("table"),
            "pose_range": {"x": (0.0, 0.0)},
            "velocity_range": {"x": (0.0, 0.0)},
        },
    )
    reset_dryingbox = EventTerm(
        func=mdp.reset_root_state_uniform,
        mode="reset",
        params={
            "asset_cfg": SceneEntityCfg("dryingbox"),
            "pose_range": {"x": (0.0, 0.0)},
            "velocity_range": {"x": (0.0, 0.0)},
        },
    )


@configclass
class RewardsCfg:
    """Reward terms for the MDP."""

    # 1. 姿态对齐：保持不变，这是基础
    gripper_handle_orientation_alignment = RewTerm(
        func=mdp.gripper_handle_orientation_alignment,
        params={
            "dryingbox_cfg": SceneEntityCfg("dryingbox"),
            "robot_cfg": SceneEntityCfg("Mindbot"),
            "handle_name": "Equipment_BB_13_C",
            "gripper_body_name": "right_hand_body",
            "gripper_forward_axis": (0.0, 0.0, 1.0),
            "gripper_width_axis": (0.0, 1.0, 0.0),
            "lid_handle_axis": (0.0, 0.0, 1.0),
            "max_angle_deg": 60.0,  # 稍微放宽一点，或者保持 30.0，看前期训练难度
        },
        weight=4.0,
    )
    # 2. 位置对齐 (合并了之前的粗略和精细对齐)
    # std=0.05 提供了从 15cm 到 0cm 全程较好的梯度，解决了之前的梯度消失问题
    gripper_handle_position_alignment = RewTerm(
        func=mdp.gripper_handle_position_alignment,
        params={
            "dryingbox_cfg": SceneEntityCfg("dryingbox"),
            "robot_cfg": SceneEntityCfg("Mindbot"),
            "handle_name": "Equipment_BB_13_C",
            "left_gripper_name": "right_hand_l",
            "right_gripper_name": "right_hand__r",
            "height_offset": 0.00,
            "std": 0.05,  # 关键修改：0.3太松，0.03太紧导致梯度消失，0.05是平衡点
        },
        weight=6.0,  # 提高权重，作为主导奖励
    )
    # 3. 远距离闭合惩罚 (逻辑已修正)
    # 现在它只惩罚“在远处就闭眼”的行为，不会阻挡机器人靠近
    gripper_closed_penalty = RewTerm(
        func=mdp.gripper_open_penalty_when_far,
        params={
            "dryingbox_cfg": SceneEntityCfg("dryingbox"),
            "robot_cfg": SceneEntityCfg("Mindbot"),
            "handle_name": "Equipment_BB_13_C",
            "left_gripper_name": "right_hand_l",
            "right_gripper_name": "right_hand__r",
            "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],
            "dist_threshold": 0.06,  # 2.5cm 保护区，进入后允许闭合
        },
        weight=0.5,  # 权重降低，作为辅助约束，不要喧宾夺主
    )
    # ... 在 RewardsCfg 类中修改 ...

    # 4. 近距离抓取交互 (改用 V2 版本或增加稳定性项)
    gripper_close_interaction = RewTerm(
        func=mdp.gripper_close_interaction_v2,  # 切换到带稳定性的版本
        params={
            "dryingbox_cfg": SceneEntityCfg("dryingbox"),
            "robot_cfg": SceneEntityCfg("Mindbot"),
            "handle_name": "Equipment_BB_13_C",
            "left_gripper_name": "right_hand_l",
            "right_gripper_name": "right_hand__r",
            "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],
            "target_close_pos": 0.012,
            "height_offset": 0.00,
            "grasp_radius": 0.05,
        },
        weight=8.0,  # 适当调低一点点，为稳定性奖励腾出空间
    )

    # 5. 新增：抓取稳定性奖励 (核心：惩罚抖动)
    gripper_grasp_stability = RewTerm(
        func=mdp.gripper_grasp_stability,
        params={
            "robot_cfg": SceneEntityCfg("Mindbot"),
            "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],
            "vel_threshold": 0.005,
        },
        weight=4.0,  # 只要保持静止就一直给分
    )
    # 4. 近距离抓取交互 (修正了目标值)
    # gripper_close_interaction = RewTerm(
    #     func=mdp.gripper_close_when_near,
    #     params={
    #         "dryingbox_cfg": SceneEntityCfg("dryingbox"),
    #         "robot_cfg": SceneEntityCfg("Mindbot"),
    #         "handle_name":"Equipment_BB_13_C",
    #         "left_gripper_name": "right_hand_l",
    #         "right_gripper_name": "right_hand__r",
    #         "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],

    #         # 关键修改：物理接触点计算
    #         # 夹爪空载全闭是0.03 (对应0cm)
    #         # 把手宽3.88cm -> 单侧行程 (6-3.88)/2 = 1.06cm = 0.0106m
    #         # 设为 0.012 确保接触时计算出的奖励是满分 (ratio >= 1.0)
    #         "target_close_pos": 0.012,

    #         "height_offset": 0.00,
    #         "grasp_radius": 0.05,
    #     },
    #     weight=10.0 # 只有成功抓到才给的大奖
    # )


# @configclass
# class RewardsCfg:
#     """Reward terms for the MDP."""

#     gripper_handle_orientation_alignment = RewTerm(
#         func=mdp.gripper_handle_orientation_alignment,
#         params={
#             "dryingbox_cfg": SceneEntityCfg("dryingbox"),
#             "robot_cfg": SceneEntityCfg("Mindbot"),
#             "handle_name": "Equipment_BB_13_C",
#             "gripper_body_name": "right_hand_body",
#             "gripper_forward_axis": (0.0, 0.0, 1.0),
#             "gripper_width_axis": (0.0, 1.0, 0.0),
#             "lid_handle_axis": (0.0, 0.0, 1.0),
#             # 删除了 handle_axis，因为新函数中不再使用它
#             "max_angle_deg": 30.0,  # 建议改为 30.0 或更小，85.0 对指向性约束来说太宽松
#         },
#         weight=4.0 # 建议保持在 4.0 或 5.0，确保姿态约束有足够的引导力
#     )

#     # stage 2
#     # 位置对齐奖励：鼓励夹爪位于 lid 两侧，且 Z 方向在 lid 上方 0.1m

#     gripper_handle_position_alignment = RewTerm(
#         func=mdp.gripper_handle_position_alignment,
#         params={
#             "dryingbox_cfg": SceneEntityCfg("dryingbox"),
#             "robot_cfg": SceneEntityCfg("Mindbot"),
#             "handle_name":"Equipment_BB_13_C",
#             "left_gripper_name": "right_hand_l",
#             "right_gripper_name": "right_hand__r",
#             "height_offset": 0.00,  # Z方向：lid 上方 0.1m
#             "std": 0.3,  # 位置对齐的容忍度0.3 ###0.1
#         },
#         weight=3.0
#     )

#         # 强制远距离打开奖励
#     # 远距离闭合惩罚
#     gripper_closed_penalty = RewTerm(
#         func=mdp.gripper_open_penalty_when_far,
#         params={
#             "dryingbox_cfg": SceneEntityCfg("dryingbox"),
#             "robot_cfg": SceneEntityCfg("Mindbot"),
#             "handle_name": "Equipment_BB_13_C",
#             "left_gripper_name": "right_hand_l",
#             "right_gripper_name": "right_hand__r",
#             "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],
#             "dist_threshold": 0.02, # 3cm 以外不允许闭合
#             "max_penalty_dist":0.055 #0.055
#         },
#         weight=1.0 # 提高权重，让扣分很痛8.0
#     )


#     # 【新增】精细对齐 (引导进入 2cm 圈)
#     gripper_handle_fine_alignment = RewTerm(
#         func=mdp.gripper_handle_position_alignment,
#         params={
#             "dryingbox_cfg": SceneEntityCfg("dryingbox"),
#             "robot_cfg": SceneEntityCfg("Mindbot"),
#             "handle_name":"Equipment_BB_13_C",
#             "left_gripper_name": "right_hand_l",
#             "right_gripper_name": "right_hand__r",
#             "height_offset": 0.00,  # Z方向：lid 上方 0.1m
#             "std": 0.03,  # 位置对齐的容忍度0.05
#         },
#         weight=5.0 # 高权重
#     )

#     # gripper_close_interaction = RewTerm(
#     #     func=mdp.gripper_close_when_near,
#     #     params={
#     #         "dryingbox_cfg": SceneEntityCfg("dryingbox"),
#     #         "robot_cfg": SceneEntityCfg("Mindbot"),
#     #         "handle_name": "Equipment_BB_13_C",
#     #         "left_gripper_name": "right_hand_l",
#     #         "right_gripper_name": "right_hand__r",
#     #         "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],
#     #         # 关键匹配把手尺寸
#     #         "handle_thickness": 0.0388,  # 把手厚度（X/宽度向抓取方向）
#     #         "thickness_tol": 0.008,      # 误差容忍，约 0.8cm
#     #         "grasp_radius": 0.04,        # 进入此半径才计算闭合奖励
#     #         "height_offset": 0.00,
#     #         "target_close_pos": 0.03,    # 保留给关节闭合幅度
#     #     },
#     #     weight=10.0
#     # )
#     gripper_close_interaction = RewTerm(
#         func=mdp.gripper_close_when_near,
#         params={
#             "dryingbox_cfg": SceneEntityCfg("dryingbox"),
#             "robot_cfg": SceneEntityCfg("Mindbot"),
#             "handle_name":"Equipment_BB_13_C",
#             "left_gripper_name": "right_hand_l",
#             "right_gripper_name": "right_hand__r",
#             "joint_names": ["right_hand_joint_left", "right_hand_joint_right"],
#             "target_close_pos": 0.03,
#             "height_offset": 0.00,#0.07
#             "grasp_radius": 0.04,#original 0.03
#         },
#         weight=10.0
#     )

#     # lid_lifted_reward = RewTerm(
#     #     func=mdp.lid_is_lifted, # 使用 rewards.py 中已有的函数
#     #     params={
#     #         "lid_cfg": SceneEntityCfg("lid"),
#     #         "minimal_height": 1.0, # 根据你的场景调整，比初始高度高 5cm 左右
#     #     },
#     #     weight=10.0 # 给一个足够大的权重
#     # )

#     # lid_lifting_reward = RewTerm(
#     #     func=mdp.lid_lift_progress_reward,
#     #     params={
#     #         "lid_cfg": SceneEntityCfg("lid"),
#     #         "initial_height": 0.8, # 请务必核实这个值！在 print 里看一下 lid 的 z 坐标
#     #         "target_height_lift": 0.25,
#     #         "height_offset": 0.07, # 与其他奖励保持一致
#     #         "std": 0.1
#     #     },
#     #     # 权重设大一点，告诉它这是最终目标
#     #     weight=10.0
#     # )


@configclass
class TerminationsCfg:
    """Termination terms for the MDP."""

    time_out = DoneTerm(func=mdp.time_out, time_out=True)

    mindbot_fly_away = DoneTerm(
        func=mdp.base_height_failure,
        params={"asset_cfg": SceneEntityCfg("Mindbot"), "maximum_height": 2.0},
    )

    # lid_fly_away = DoneTerm(
    #     func=mdp.base_height_failure,
    #     params={"asset_cfg": SceneEntityCfg("lid"), "maximum_height": 2.0},
    # )

    # # 新增：盖子掉落判定
    # # 如果盖子掉回桌面（高度接近初始高度），且已经运行了一段时间（避开刚开始重置的瞬间）
    # lid_dropped = DoneTerm(
    #     func=mdp.base_height_failure, # 复用高度判定函数
    #     params={
    #         "asset_cfg": SceneEntityCfg("lid"),
    #         "minimum_height": 0.79, # 假设初始高度是 0.9，低于 0.88 说明掉下去了或者被砸进去了
    #     },
    # )


##
# Environment configuration
##


@configclass
class CurriculumCfg:
    pass
    # action_rate = CurrTerm(func=mdp.modify_reward_weight,
    #                        params={"term_name": "action_rate", "weight": -1e-1, "num_steps": 10000})

    # joint_vel = CurrTerm(func=mdp.modify_reward_weight,
    #                      params={"term_name": "joint_vel", "weight": -1e-1, "num_steps": 10000})

    # position_std_scheduler = CurrTerm(
    #     func=mdp.modify_term_cfg, # 直接使用 Isaac Lab 内置的类
    #     params={
    #         # 路径：rewards管理器 -> 你的奖励项名 -> params字典 -> std键
    #         "address": "rewards.gripper_lid_position_alignment.params.std",

    #         # 修改逻辑：使用我们刚才写的函数
    #         "modify_fn": mdp.annealing_std,

    #         # 传给函数的参数
    #         "modify_params": {
    #             "start_step": 00, # 约 600 轮
    #             "end_step":   5_000, # 约 1200 轮
    #             "start_std": 0.3,
    #             "end_std": 0.05,
    #         }
    #     }
    # )


@configclass
class PullEnvCfg(ManagerBasedRLEnvCfg):
    # Scene settings
    scene: PullSceneCfg = PullSceneCfg(num_envs=5, env_spacing=3.0)
    # Basic settings
    observations: ObservationsCfg = ObservationsCfg()
    actions: ActionsCfg = ActionsCfg()
    events: EventCfg = EventCfg()
    # MDP settings
    rewards: RewardsCfg = RewardsCfg()
    terminations: TerminationsCfg = TerminationsCfg()
    # curriculum: CurriculumCfg = CurriculumCfg()

    # Post initialization
    def __post_init__(self) -> None:
        """Post initialization."""
        # general settings
        self.decimation = 4  # original 2  # 从 2 增加到 4，控制频率从 25Hz 降到 12.5Hz
        self.episode_length_s = 10
        # viewer settings
        self.viewer.eye = (3.5, 0.0, 3.2)
        # simulation settings
        self.sim.dt = 1 / 120  # original 1 / 60
        self.sim.render_interval = self.decimation
        # # 1. 基础堆内存
        self.sim.physx.gpu_heap_capacity = 256 * 1024 * 1024  # 256MB
        self.sim.physx.gpu_collision_stack_size = 1024 * 1024 * 1024
        self.sim.physx.gpu_temp_buffer_capacity = 128 * 1024 * 1024
        self.sim.physx.gpu_max_rigid_contact_count = 16 * 1024 * 1024  # 200万个接触点
        self.sim.physx.gpu_max_rigid_patch_count = 1024 * 1024  # 32万个 patch

        # # 5. 聚合对容量 (针对复杂的 Articulation)
        self.sim.physx.gpu_found_lost_pairs_capacity = 16 * 1024 * 1024
        self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 16 * 1024 * 1024
        self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 * 1024
