"""Rollout config for converted scenario output.

Each object is spawned individually from a shared USD model library via
``UsdFileCfg``, giving Isaac Lab full control over spawning and physics.
The station is spawned as a static ``AssetBaseCfg``::

    cfg = ScenarioIKRolloutCfg()
    cfg.dynamic_setup(
        scene_path="./configs/3_cabot_breakfast/spawn_config.json",
        library_dir="/path/to/usd_library",
    )
"""

from __future__ import annotations

import copy
import json
from pathlib import Path

import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.envs import ManagerBasedRLEnvCfg, ViewerCfg
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.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg, TiledCameraCfg
from isaaclab.utils import configclass

import sim_improvement.environments.mdp as mdp
from sim_improvement.environments.lbm.robot import (
    IMPLICIT_PANDA,
    GRIPPER_ACTION,
    BINARY_GRIPPER_ACTION,
    LEFT_PANDA_DEFAULT_JOINT_POS,
    # LBM_GRAVITY_COMP_DIFF_IK,
    ARM_IK_ACTION,
    DUAL_ARM_IK_ACTION,
    RIGHT_PANDA_DEFAULT_JOINT_POS,
)
from sim_improvement.environments.lbm.scenario_helper import (
    ResetScenarioStochastic,
    YUP_OBJECTS_REGISTRY,
    _compute_child_frame_z_offset,
    _quat_mul_single,
    _midpoint_translation,
    _midpoint_rotation,
    _RX90_W,
    _RX90_X,
)


# ---------------------------------------------------------------------------
# Scene
# ---------------------------------------------------------------------------

@configclass
class ScenarioSceneCfg(InteractiveSceneCfg):
    """Scene config that dynamically adds objects from converter output."""

    left_panda = IMPLICIT_PANDA.replace(
        prim_path="{ENV_REGEX_NS}/left_panda",
        init_state=ArticulationCfg.InitialStateCfg(
            pos=(-0.5937, -0.34362, -0.08484),
            rot=(0.36811, 0.01027, 0.00078, 0.92973),
            joint_pos=LEFT_PANDA_DEFAULT_JOINT_POS,
        ),
    )

    right_panda = IMPLICIT_PANDA.replace(
        prim_path="{ENV_REGEX_NS}/right_panda",
        init_state=ArticulationCfg.InitialStateCfg(
            pos=(-0.5937, 0.32962, -0.08062),
            rot=(0.91675, 0.01312, 0.01089, 0.39909),
            joint_pos=RIGHT_PANDA_DEFAULT_JOINT_POS,
        ),
    )

    background = AssetBaseCfg(
        prim_path="{ENV_REGEX_NS}/background",
        spawn=sim_utils.DomeLightCfg(
            intensity=500.0,
            texture_file = "envs/2_riverway_shelf/poly_haven_studio_4k.hdr"
        ),
        init_state=AssetBaseCfg.InitialStateCfg(
            pos=(0.0, 0.0, 0.0),
            rot=(0.0, 0.0, 0.0, 1.0),
        )
    )

    # Placeholder — filled by dynamic_setup
    scenario: AssetBaseCfg | None = None

    def dynamic_setup(self, spawn_config: dict, library_dir: str, scene_path_key: str) -> None:
        """Parse converter output and populate scene entities.

        Args:
            spawn_config: Parsed spawn_config.json dict.
            library_dir: Path to shared USD model library.
            scene_path_key: Resolved path string used as key for YUP_OBJECTS_REGISTRY.
        """
        library_path = Path(library_dir)
        yup_objects: set[str] = set()
        YUP_OBJECTS_REGISTRY[scene_path_key] = yup_objects

        # Collect bodies that need contact reporting from predicates
        contact_bodies: set[str] = set()
        for pred in spawn_config.get("task_predicate_configs", []):
            if pred.get("_type") == "contactflagpredicateconfig":
                contact_bodies.add(pred["body_1_name"])
                contact_bodies.add(pred["body_2_name"])

        # 1. Spawn station as static asset
        station = spawn_config.get("station")
        if station and station.get("model"):
            station_usd = str(library_path / station["model"])
            self.scenario = AssetBaseCfg(
                prim_path="{ENV_REGEX_NS}/scenario",
                spawn=sim_utils.UsdFileCfg(usd_path=station_usd),
            )

        # 2. Spawn each manipulable object with UsdFileCfg
        spawned_objects: set[str] = set()
        for obj in spawn_config.get("objects", []):
            name = obj["name"]
            model_path = obj.get("model")
            if not model_path:
                continue
            usd_path = str(library_path / model_path)

            # Compute midpoint of stochastic translation for init_state
            pos = _midpoint_translation(obj.get("translation"))
            orient = _midpoint_rotation(obj.get("rotation"))

            # Track GLTF-sourced models that need Y-up correction at reset
            needs_yup = bool(obj.get("needs_yup"))
            if needs_yup:
                yup_objects.add(name)
                rx90 = (_RX90_W, _RX90_X, 0.0, 0.0)
                orient = _quat_mul_single(orient, rx90)

            # Lift objects whose child_frame is "bottom" so they sit on the surface
            z_off = _compute_child_frame_z_offset(usd_path, obj.get("child_frame"), needs_yup)
            pos = (pos[0], pos[1], pos[2] + z_off)

            # Welded objects are fixed in place → spawn as kinematic.
            is_kinematic = bool(obj.get("welded", False)) or bool(obj.get("kinematic_enabled", False))
            no_gravity = is_kinematic or bool(obj.get("disable_gravity", False))

            rigid_props = None
            if is_kinematic or no_gravity:
                rigid_props = sim_utils.RigidBodyPropertiesCfg(
                    kinematic_enabled=is_kinematic,
                    disable_gravity=no_gravity,
                )

            asset = RigidObjectCfg(
                prim_path=f"{{ENV_REGEX_NS}}/{name}",
                spawn=sim_utils.UsdFileCfg(
                    usd_path=usd_path,
                    activate_contact_sensors=True,
                    rigid_props=rigid_props,
                ),
                init_state=RigidObjectCfg.InitialStateCfg(
                    pos=pos,
                    rot=orient,
                ),
            )
            setattr(self, name, asset)
            spawned_objects.add(name)

        # 3. Spawn uniform_manipuland groups (first variant)
        for group in spawn_config.get("randomized_groups", []):
            if group.get("group_type") != "uniform_manipuland":
                continue
            name = group["name"]
            variants = group.get("variants", [])
            if not variants:
                continue
            first = variants[0]
            model_path = first.get("model")
            if not model_path:
                continue
            usd_path = str(library_path / model_path)

            pos = _midpoint_translation(first.get("translation"))
            orient = _midpoint_rotation(first.get("rotation"))

            # Track GLTF-sourced models that need Y-up correction at reset
            needs_yup = bool(first.get("needs_yup"))
            if needs_yup:
                yup_objects.add(name)
                rx90 = (_RX90_W, _RX90_X, 0.0, 0.0)
                orient = _quat_mul_single(orient, rx90)

            z_off = _compute_child_frame_z_offset(usd_path, first.get("child_frame"), needs_yup)
            pos = (pos[0], pos[1], pos[2] + z_off)

            is_kinematic = bool(first.get("welded", False)) or bool(first.get("kinematic_enabled", False))
            no_gravity = is_kinematic or bool(first.get("disable_gravity", False))

            rigid_props = None
            if is_kinematic or no_gravity:
                rigid_props = sim_utils.RigidBodyPropertiesCfg(
                    kinematic_enabled=is_kinematic,
                    disable_gravity=no_gravity,
                )

            asset = RigidObjectCfg(
                prim_path=f"{{ENV_REGEX_NS}}/{name}",
                spawn=sim_utils.UsdFileCfg(
                    usd_path=usd_path,
                    activate_contact_sensors=True,
                    rigid_props=rigid_props,
                ),
                init_state=RigidObjectCfg.InitialStateCfg(
                    pos=pos,
                    rot=orient,
                ),
            )
            setattr(self, name, asset)
            spawned_objects.add(name)

        # 4. Add ContactSensors for contact-flag predicates
        for pred in spawn_config.get("task_predicate_configs", []):
            if pred.get("_type") != "contactflagpredicateconfig":
                continue
            b1 = pred["body_1_name"]
            b2 = pred["body_2_name"]
            # body1 must be a spawned object (sensor prim_path).
            # body2 can be a spawned object OR a station-embedded prim.
            if b1 not in spawned_objects:
                continue
            if b2 in spawned_objects:
                filter_path = f"{{ENV_REGEX_NS}}/{b2}"
            else:
                # Assume station-embedded: {ENV_REGEX_NS}/scenario/{body_name}
                filter_path = f"{{ENV_REGEX_NS}}/scenario/{b2}"
            sensor_name = f"contact__{b1}__{b2}"
            sensor = ContactSensorCfg(
                prim_path=f"{{ENV_REGEX_NS}}/{b1}",
                filter_prim_paths_expr=[filter_path],
                update_period=0.0,
            )
            setattr(self, sensor_name, sensor)


# ---------------------------------------------------------------------------
# Actions
# ---------------------------------------------------------------------------

@configclass
class ActionsCfg:
    # left_panda_arm = ARM_IK_ACTION.replace(asset_name="left_panda")
    # right_panda_arm = ARM_IK_ACTION.replace(asset_name="right_panda")
    dual_arm_ik_action = DUAL_ARM_IK_ACTION
    # left_panda_gripper = GRIPPER_ACTION.replace(asset_name="left_panda")
    # right_panda_gripper = GRIPPER_ACTION.replace(asset_name="right_panda")
    left_panda_gripper = BINARY_GRIPPER_ACTION.replace(asset_name="left_panda")
    right_panda_gripper = BINARY_GRIPPER_ACTION.replace(asset_name="right_panda")


# ---------------------------------------------------------------------------
# Observations
# ---------------------------------------------------------------------------
@configclass
class VisionCfg(ObsGroup):
    wrist_camera_right = ObsTerm(
        func=mdp.image,
        params={"sensor_cfg": SceneEntityCfg("flir_wrist_right_minus"), "data_type": "rgb", "normalize": False},
    )
    wrist_camera_left = ObsTerm(
        func=mdp.image,
        # params={"sensor_cfg": SceneEntityCfg("wrist_camera_left"), "data_type": "rgb", "normalize": False},
        params={"sensor_cfg": SceneEntityCfg("flir_wrist_left_plus"), "data_type": "rgb", "normalize": False},
    )
    external_camera_right = ObsTerm(
        func=mdp.image,
        params={"sensor_cfg": SceneEntityCfg("scene_camera_right"), "data_type": "rgb", "normalize": False},
    )
    external_camera_left = ObsTerm(
        func=mdp.image,
        params={"sensor_cfg": SceneEntityCfg("scene_camera_left"), "data_type": "rgb", "normalize": False},
    )
    left_ee_pos = ObsTerm(
        func=mdp.link_pos,
        params={"root_asset_cfg": SceneEntityCfg("left_panda"), "link_name": "panda_link8"},
    )
    left_ee_quat = ObsTerm(
        func=mdp.link_quat,
        params={"root_asset_cfg": SceneEntityCfg("left_panda"), "link_name": "panda_link8"},
    )
    right_ee_pos = ObsTerm(
        func=mdp.link_pos,
        params={"root_asset_cfg": SceneEntityCfg("right_panda"), "link_name": "panda_link8"},
    )
    right_ee_quat = ObsTerm(
        func=mdp.link_quat,
        params={"root_asset_cfg": SceneEntityCfg("right_panda"), "link_name": "panda_link8"},
    )
    left_gripper_pos = ObsTerm(
        func=lambda env, asset_cfg: mdp.joint_pos(env, asset_cfg) * -2,
        params={"asset_cfg": SceneEntityCfg(name="left_panda", joint_names=["panda_finger_joint1"])},
    )
    right_gripper_pos = ObsTerm(
        func=lambda env, asset_cfg: mdp.joint_pos(env, asset_cfg) * -2,
        params={"asset_cfg": SceneEntityCfg(name="right_panda", joint_names=["panda_finger_joint1"])},
    )

    def __post_init__(self):
        self.concatenate_terms = False
        self.enable_corruption = False

@configclass
class PolicyCfg(ObsGroup):
    """Observations for policy group."""
    # prev_actions = ObsTerm(func=mdp.last_action)
    left_panda_joint_pos = ObsTerm(func=mdp.joint_pos, params={"asset_cfg": SceneEntityCfg("left_panda")})
    right_panda_joint_pos = ObsTerm(func=mdp.joint_pos, params={"asset_cfg": SceneEntityCfg("right_panda")})

    left_ee_pos = ObsTerm(
        func=mdp.link_pos,
        params={"root_asset_cfg": SceneEntityCfg("left_panda"), "link_name": "panda_link8"},
    )
    left_ee_quat = ObsTerm(
        func=mdp.link_quat,
        params={"root_asset_cfg": SceneEntityCfg("left_panda"), "link_name": "panda_link8"},
    )
    right_ee_pos = ObsTerm(
        func=mdp.link_pos,
        params={"root_asset_cfg": SceneEntityCfg("right_panda"), "link_name": "panda_link8"},
    )
    right_ee_quat = ObsTerm(
        func=mdp.link_quat,
        params={"root_asset_cfg": SceneEntityCfg("right_panda"), "link_name": "panda_link8"},
    )
    left_gripper_pos = ObsTerm(
        func=lambda env, asset_cfg: mdp.joint_pos(env, asset_cfg) * -2,
        params={"asset_cfg": SceneEntityCfg(name="left_panda", joint_names=["panda_finger_joint1"])},
    )
    right_gripper_pos = ObsTerm(
        func=lambda env, asset_cfg: mdp.joint_pos(env, asset_cfg) * -2,
        params={"asset_cfg": SceneEntityCfg(name="right_panda", joint_names=["panda_finger_joint1"])},
    )


@configclass
class ObservationsCfg:
    policy: PolicyCfg = PolicyCfg(concatenate_terms=True, enable_corruption=False)

    def dynamic_setup(self, spawn_config: dict) -> None:
        """Add task-relevant object pose obs to PolicyCfg from spawn config."""
        # Build set of actual spawned scene entities to filter against
        scene_entities: set[str] = set()
        for obj in spawn_config.get("objects", []):
            if obj.get("model"):
                scene_entities.add(obj["name"])
        for group in spawn_config.get("randomized_groups", []):
            if group.get("group_type") == "uniform_manipuland" and group.get("variants"):
                scene_entities.add(group["name"])

        obj_names: set[str] = set()
        for pred in spawn_config.get("task_predicate_configs", []):
            ptype = pred.get("_type", "")
            if ptype == "ontoppredicateconfig":
                obj_names.add(pred["top_object_name"])
                obj_names.add(pred["bottom_object_name"])
            elif ptype == "contactflagpredicateconfig":
                obj_names.add(pred["body_1_name"])
        for rc in spawn_config.get("reward_configs", []):
            for key in ("frame_a_name", "frame_b_name"):
                name = rc.get(key, "").split("::")[0]
                if name:
                    obj_names.add(name)
        for name in spawn_config.get("observation_objects", []):
            obj_names.add(name)

        # Only add observations for names that are actual scene entities
        # (skip reference_frames and other non-spawned names)
        obj_names &= scene_entities

        for obj_name in sorted(obj_names):
            setattr(self.policy, f"{obj_name}_pos", ObsTerm(
                func=mdp.object_pos,
                params={"asset_cfg": SceneEntityCfg(obj_name)},
            ))
            setattr(self.policy, f"{obj_name}_quat", ObsTerm(
                func=mdp.object_quat,
                params={"asset_cfg": SceneEntityCfg(obj_name)},
            ))
        self.policy_dict = copy.deepcopy(self.policy)
        self.policy_dict.concatenate_terms = False
        self.policy_dict.enable_corruption = False




# ---------------------------------------------------------------------------
# Rewards / Terminations
# ---------------------------------------------------------------------------

@configclass
class RewardsCfg:
    # translation_distance is added dynamically when reward_configs are present.

    success = RewTerm(func=mdp.success_reward, weight=10.0)
    # left_singularity = RewTerm(
    #     func=mdp.singularity_penalty, weight=-1.0,
    #     params={"asset_cfg": SceneEntityCfg("left_panda"), "threshold": 0.05},
    # )
    # right_singularity = RewTerm(
    #     func=mdp.singularity_penalty, weight=-1.0,
    #     params={"asset_cfg": SceneEntityCfg("right_panda"), "threshold": 0.05},
    # )

    def dynamic_setup(self, spawn_config: dict) -> None:
        """Add reward terms from spawn_config reward_configs."""
        # Build set of actual spawned scene entities
        scene_entities: set[str] = set()
        for obj in spawn_config.get("objects", []):
            if obj.get("model"):
                scene_entities.add(obj["name"])
        for group in spawn_config.get("randomized_groups", []):
            if group.get("group_type") == "uniform_manipuland" and group.get("variants"):
                scene_entities.add(group["name"])

        for rc in spawn_config.get("reward_configs", []):
            if rc.get("_type") == "relativetranslationdistancecostconfig":
                obj_a = rc.get("frame_a_name", "").split("::")[0]
                obj_b = rc.get("frame_b_name", "").split("::")[0]
                if obj_a and obj_b and obj_a in scene_entities and obj_b in scene_entities:
                    setattr(self, f"obj_dist__{obj_a}__{obj_b}", RewTerm(
                        func=mdp.object_to_object_distance,
                        weight=1 / 450.0,
                        params={
                            "std": 0.3,
                            "object1_cfg": SceneEntityCfg(obj_a),
                            "object2_cfg": SceneEntityCfg(obj_b),
                        },
                    ))
                    setattr(self, f"ee_reach__{obj_b}", RewTerm(
                        func=mdp.ee_to_object_distance,
                        weight=1 / 450.0,
                        params={
                            "std": 0.3,
                            "object_cfg": SceneEntityCfg(obj_b),
                        },
                    ))


@configclass
class TerminationsCfg:
    timeout = DoneTerm(func=mdp.time_out, time_out=True)
    left_singularity = DoneTerm(
        func=mdp.singularity_termination,
        params={"asset_cfg": SceneEntityCfg("left_panda"), "threshold": 0.02},
    )
    right_singularity = DoneTerm(
        func=mdp.singularity_termination,
        params={"asset_cfg": SceneEntityCfg("right_panda"), "threshold": 0.02},
    )
    # task_success is added dynamically when task_predicate_configs are present.

    def dynamic_setup(self, spawn_config: dict) -> None:
        """Add termination terms from spawn_config task_predicate_configs."""
        predicates = spawn_config.get("task_predicate_configs", [])
        if predicates:
            # Build frame lookup from objects' "frames" dicts
            object_frames: dict[str, dict] = {}
            yup_objects: set[str] = set()
            for obj in spawn_config.get("objects", []):
                if "frames" in obj:
                    object_frames[obj["name"]] = obj["frames"]
                if obj.get("needs_yup"):
                    yup_objects.add(obj["name"])
            self.success = DoneTerm(
                func=mdp.scenario_success,
                params={"predicates": predicates, "object_frames": object_frames, "yup_objects": yup_objects},
            )


# ---------------------------------------------------------------------------
# Events
# ---------------------------------------------------------------------------

@configclass
class ScenarioEventCfg:
    reset_default = EventTerm(
        func=mdp.reset_scene_to_default,
        mode="reset",
        params={"reset_joint_targets": True},
    )
    reset_objects = EventTerm(
        func=ResetScenarioStochastic,
        mode="reset",
        params={"scene_path": ""},  # filled by dynamic_setup
    )

    def dynamic_setup(self, scene_path: str, library_dir: str) -> None:
        """Set resolved paths for the stochastic reset event."""
        if hasattr(self, "reset_objects"):
            self.reset_objects.params["scene_path"] = scene_path
            self.reset_objects.params["library_dir"] = library_dir


# ---------------------------------------------------------------------------
# Env
# ---------------------------------------------------------------------------

@configclass
class ScenarioEnvCfg(ManagerBasedRLEnvCfg):
    scene = ScenarioSceneCfg(num_envs=32, env_spacing=5.0)
    observations = ObservationsCfg()
    rewards = RewardsCfg()
    terminations = TerminationsCfg()
    actions = ActionsCfg()
    events = ScenarioEventCfg()
    viewer: ViewerCfg = ViewerCfg(eye=(5.0, 0, 2.0), origin_type="world", env_index=0, asset_name="scenario")

    def __post_init__(self):
        self.seed = 42
        self.decimation = 8
        # self.episode_length_s = 45.0
        self.episode_length_s = 30.0
        self.sim.dt = 1 / (10.0 * self.decimation)

        self.sim.physx.solver_type = 1
        self.sim.physx.max_position_iteration_count = 192
        self.sim.physx.max_velocity_iteration_count = 1
        self.sim.physx.bounce_threshold_velocity = 0.02
        self.sim.physx.friction_offset_threshold = 0.01
        self.sim.physx.friction_correlation_distance = 0.0005

        self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4
        self.sim.physx.gpu_total_aggregate_pairs_capacity = 2**23
        self.sim.physx.gpu_max_rigid_contact_count = 2**23
        self.sim.physx.gpu_max_rigid_patch_count = 2**23
        self.sim.physx.gpu_collision_stack_size = 2**31

        self.sim.render.enable_dlssg = True
        self.sim.render.enable_ambient_occlusion = True
        self.sim.render.enable_reflections = True
        self.sim.render.enable_dl_denoiser = True

        self.wait_for_textures = True
        self.rerender_on_reset = True

    def dynamic_setup(self, scene_path: str, library_dir: str) -> None:
        """Configure the env from converter output directory.

        Loads spawn_config.json once and delegates to each sub-config's
        ``dynamic_setup`` so every manager owns its own wiring logic.

        Args:
            scene_path: Path to the config JSON file (spawn_config.json).
            library_dir: Path to shared USD model library.
        """
        resolved_path = str(Path(scene_path).resolve())
        resolved_lib = str(Path(library_dir).resolve())

        with open(resolved_path) as f:
            spawn_config = json.load(f)

        self.scene.dynamic_setup(spawn_config, resolved_lib, scene_path_key=resolved_path)
        self.events.dynamic_setup(resolved_path, resolved_lib)
        self.terminations.dynamic_setup(spawn_config)
        self.rewards.dynamic_setup(spawn_config)
        self.observations.dynamic_setup(spawn_config)


@configclass
class ScenarioIKStateCfg(ScenarioEnvCfg):
    def __post_init__(self):
        super().__post_init__()
        self.actions.dual_arm_ik_action.controller.use_relative_mode = False

        self.scene.left_panda.spawn.rigid_props.disable_gravity = True
        self.scene.right_panda.spawn.rigid_props.disable_gravity = True

@configclass
class ScenarioIkVisionCfg(ScenarioIKStateCfg):
    def __post_init__(self):
        super().__post_init__()
        # Cameras — same as rollout_cfg.py
        self.scene.scene_camera_right = TiledCameraCfg(
            prim_path="{ENV_REGEX_NS}/scene_camera_right",
            offset=TiledCameraCfg.OffsetCfg(
                pos=(0.54755, 0.18739, 0.83597),
                rot=(0.63083, 0.15475, 0.27309, 0.70960),
                convention="opengl",
            ),
            data_types=["rgb"],
            spawn=sim_utils.PinholeCameraCfg(
                clipping_range=(0.05, 3.0),
                focal_length=0.5953,
                horizontal_aperture=1.0,
                vertical_aperture=0.75,
            ),
            height=480,
            width=640,
        )

        self.scene.scene_camera_left = TiledCameraCfg(
            prim_path="{ENV_REGEX_NS}/scene_camera_left",
            offset=TiledCameraCfg.OffsetCfg(
                pos=(0.49714, -0.36177, 0.77877),
                rot=(0.72133, 0.33411, 0.07049, 0.60257),
                convention="opengl",
            ),
            data_types=["rgb"],
            spawn=sim_utils.PinholeCameraCfg(
                clipping_range=(0.05, 3.0),
                focal_length=0.5953,
                horizontal_aperture=1.0,
                vertical_aperture=0.75,
            ),
            height=480,
            width=640,
        )

        self.scene.flir_wrist_left_plus = TiledCameraCfg(
            prim_path="{ENV_REGEX_NS}/left_panda/panda_link8/flir_wrist_left_plus",
            offset=TiledCameraCfg.OffsetCfg(
                pos=(0.04651, -0.04651, 0.0048),
                rot=(0.92388, 0.0, 0.0, 0.38268),
                convention="ros",
            ),
            data_types=["rgb"],
            spawn=sim_utils.FisheyeCameraCfg(
                clipping_range=(0.05, 3.0),
                projection_type="fisheyePolynomial",
                focal_length=23.151,
                horizontal_aperture=100.0,
                vertical_aperture=62.5,
                fisheye_nominal_width=960.0,
                fisheye_nominal_height=600.0,
                fisheye_optical_centre_x=483.472,
                fisheye_optical_centre_y=299.437,
                fisheye_max_fov=180.0,
                fisheye_polynomial_a=0.0,
                fisheye_polynomial_b=0.00245,
                fisheye_polynomial_c=0.0,
                fisheye_polynomial_d=0.0,
                fisheye_polynomial_e=0.0,
                fisheye_polynomial_f=0.0,
            ),
            height=600,
            width=960,
        )
        # Right arm, minus-side (BFS_23595722)
        self.scene.flir_wrist_right_minus = TiledCameraCfg(
            prim_path="{ENV_REGEX_NS}/right_panda/panda_link8/flir_wrist_right_minus",
            offset=TiledCameraCfg.OffsetCfg(
                pos=(-0.04651, 0.04651, 0.0048),
                rot=(0.38268, 0.0, 0.0, -0.92388),
                convention="ros",
            ),
            data_types=["rgb"],
            spawn=sim_utils.FisheyeCameraCfg(
                clipping_range=(0.05, 3.0),
                projection_type="fisheyePolynomial",
                focal_length=18.627,
                horizontal_aperture=100.0,
                vertical_aperture=62.5,
                fisheye_nominal_width=960.0,
                fisheye_nominal_height=600.0,
                fisheye_optical_centre_x=459.918,
                fisheye_optical_centre_y=301.923,
                fisheye_max_fov=180.0,
                fisheye_polynomial_a=0.0,
                fisheye_polynomial_b=0.00245,
                fisheye_polynomial_c=0.0,
                fisheye_polynomial_d=0.0,
                fisheye_polynomial_e=0.0,
                fisheye_polynomial_f=0.0,
            ),
            height=600,
            width=960,
        )

        self.observations.vision = VisionCfg()


@configclass
class ScenarioRelativeIKStateCfg(ScenarioIKStateCfg):
    def __post_init__(self):
        super().__post_init__()
        self.actions.dual_arm_ik_action.controller.use_relative_mode = True

        self.scene.left_panda.spawn.rigid_props.disable_gravity = True
        self.scene.right_panda.spawn.rigid_props.disable_gravity = True

@configclass
class ScenarioRelativeIKVisionCfg(ScenarioIkVisionCfg):
    def __post_init__(self):
        super().__post_init__()
        self.actions.dual_arm_ik_action.controller.use_relative_mode = True

        self.scene.left_panda.spawn.rigid_props.disable_gravity = True
        self.scene.right_panda.spawn.rigid_props.disable_gravity = True

        # del self.scene.flir_wrist_left_plus
        # del self.scene.flir_wrist_right_minus

        # del self.observations.vision