# 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 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="C:/Users/PC/workpalce/maic_usd_assets/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="C:/Users/PC/workpalce/maic_usd_assets/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="C:/Users/PC/workpalce/maic_usd_assets/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="C:/Users/PC/workpalce/maic_usd_assets/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),
    )

    # # ------------- 修改后的相机配置 (直接使用 USD 内置相机) -------------
    
    # # 1. 头部相机
    # # USD 路径: /root/robot_head/cam_head
    # cam_head = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/robot_head/cam_head",
    #     update_latest_camera_pose=True,
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    #     # 移除 spawn 和 offset，直接使用 USD 中的参数和位置
    # )

    # # 2. 胸部相机
    # # USD 路径: /root/robot_trunk/cam_chest
    # cam_chest = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/robot_trunk/cam_chest",
    #     update_latest_camera_pose=True,
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    # )

    # # 3. 左手相机
    # # USD 路径: /root/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand
    # cam_left_hand = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand",
    #     update_latest_camera_pose=True,
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    # )

    # # 4. 右手相机
    # # USD 路径: /root/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand
    # cam_right_hand = CameraCfg(
    #     prim_path="{ENV_REGEX_NS}/Mindbot/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand",
    #     update_latest_camera_pose=True,
    #     data_types=["rgb"],
    #     width=1280,
    #     height=720,
    # )

    # ------------- 相机结束 -------------


    # # ------------- 新增 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."""

    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
