"""Scene construction from a task's YAML: object placement, containers, markers, randomization.

All the asset quirks discovered the hard way are handled here once, so a task YAML never has to
know about them:

* RoboTwin GLBs are authored Y-up -- a cup spawns upside-down and a basket on its side, so every
  asset carries an `rpy` and the default for RoboTwin containers is a +90 deg roll;
* the GLB->USD converter reads RoboTwin's `model_data` scale but does not bake it in, so the
  scale must be applied at spawn or a basket arrives 1.9 m wide;
* objects written in at a fixed height DROP onto the table and tall ones topple, so every object
  is re-seated at its measured height before the episode starts;
* a container spawned on the arm's home pose silently blocks all motion, so placements are
  checked against it and a warning is printed.
"""
from __future__ import annotations

import numpy as np

TABLE_TOP = 0.45
# where the right arm parks its end-effector; a container placed on top of this blocks the arm
HOME_EEF_WORLD = np.array([0.078, -0.19])
HOME_EEF_Z = 0.60          # the wrist's resting height; props below this cannot foul it


def euler_quat(roll_deg=0.0, pitch_deg=0.0, yaw_deg=0.0):
    r, p, y = np.radians([roll_deg, pitch_deg, yaw_deg])
    cr, sr = np.cos(r/2), np.sin(r/2)
    cp, sp = np.cos(p/2), np.sin(p/2)
    cy, sy = np.cos(y/2), np.sin(y/2)
    return np.array([cr*cp*cy+sr*sp*sy, sr*cp*cy-cr*sp*sy, cr*sp*cy+sr*cp*sy, cr*cp*sy-sr*sp*cy])


class Randomizer:
    """Per-episode scene randomization, driven by the YAML and a seed.

    A task declares nominal poses plus jitter ranges; the same seed reproduces an episode exactly,
    which is what makes a "it worked once" result checkable.
    """

    def __init__(self, seed=0, enabled=True):
        self.rng = np.random.default_rng(seed)
        self.enabled = enabled
        self.log = {}

    def xy(self, name, nominal, jitter):
        base = np.asarray(nominal, float)
        if not self.enabled or jitter in (None, 0):
            self.log[name] = base.tolist(); return base
        j = np.asarray(jitter, float)
        if j.ndim == 0:
            j = np.array([float(j), float(j)])
        out = base + self.rng.uniform(-j, j)
        self.log[name] = out.tolist()
        return out

    def scalar(self, name, nominal, jitter):
        if not self.enabled or not jitter:
            self.log[name] = float(nominal); return float(nominal)
        out = float(nominal) + float(self.rng.uniform(-jitter, jitter))
        self.log[name] = out
        return out


class SceneBuilder:
    """Places YAML-declared objects and props into a live Isaac Lab scene."""

    def __init__(self, env, origin, rand: Randomizer):
        self.env = env
        self.u = env.unwrapped
        self.origin = np.asarray(origin, float)
        self.rand = rand
        self.objects = {}          # name -> rigid-body handle
        self.articulations = {}    # name -> jointed-fixture handle (doors, drawers, lids)
        self._artic_ext = {}       # name -> measured extent of each fixture
        self.regions = {}          # name -> {"xy":..., "half":..., "top_z":...}

    # ---------------- objects ----------------
    def place_object(self, spec, settle_steps=70, step_fn=None):
        """Place one YAML object: xy (+jitter), orientation, then re-seat it on the table."""
        import torch
        name = spec["name"]
        if name not in self.u.scene.rigid_objects:
            print(f"[scene] object {name!r} is not registered in the env -- skipped", flush=True)
            return None
        obj = self.u.scene.rigid_objects[name]
        xy = self.rand.xy(name, spec.get("xy", [0.0, 0.0]), spec.get("xy_jitter"))
        yaw = self.rand.scalar(f"{name}.yaw", spec.get("yaw", 0.0), spec.get("yaw_jitter"))
        quat = euler_quat(spec.get("roll", 0.0), spec.get("pitch", 0.0), yaw)
        dev = obj.data.root_pos_w.device
        obj.write_root_pose_to_sim(torch.tensor(
            np.concatenate([self.origin+np.array([xy[0], xy[1], 0.58]), quat]),
            dtype=torch.float32, device=dev).view(1, 7))
        obj.write_root_velocity_to_sim(torch.zeros((1, 6), device=dev))
        self.objects[name] = obj
        # z_offset raises the seating height: an object that rests inside a fixture (a bar in a
        # sleeve) sits on the fixture's floor, not on the table, and the default seat buries it.
        return {"name": name, "xy": xy, "quat": quat, "z_offset": float(spec.get("z_offset", 0.0))}

    def _prim_aabb(self, name):
        """World AABB of an articulation AT ITS CURRENT POSE, plus that pose's z.

        Queried before the fixture is moved, so the numbers describe the asset as authored: the
        difference between the root z and the AABB's floor is how far the origin sits above the
        bottom of the body, which is exactly the lift needed to stand it on the table.
        """
        try:
            import omni.usd
            from pxr import UsdGeom, Usd
            stage = omni.usd.get_context().get_stage()
            bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(),
                                   [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
            art = self.u.scene.articulations[name]
            pp = art.root_physx_view.prim_paths[0]
            rng = bb.ComputeWorldBound(stage.GetPrimAtPath(pp)).ComputeAlignedRange()
            root_z = float(art.data.root_pos_w[0, 2].item())
            return (np.array(rng.GetMin()), np.array(rng.GetMax())), root_z
        except Exception as exc:
            print(f"[scene] could not measure {name}: {exc}", flush=True)
            return None, 0.0

    def fixture_extent(self, name):
        """Measured size of a jointed fixture, for tasks that need its face positions."""
        return self._artic_ext.get(name)

    def place_articulation(self, spec):
        """Seat a JOINTED fixture (a door, a drawer, a lid) and record its joint layout.

        Articulations are not rigid objects: they live in `scene.articulations`, their pose is
        written to the root body, and the thing a task cares about is a JOINT value, not the
        object's centre. They are converted with a fixed base, so once written in they stay put
        while the arm works against the joint.
        """
        import torch
        name = spec["name"]
        if name not in self.u.scene.articulations:
            print(f"[scene] articulation {name!r} is not registered in the env -- skipped", flush=True)
            return None
        art = self.u.scene.articulations[name]
        xy = self.rand.xy(name, spec.get("xy", [0.0, -0.20]), spec.get("xy_jitter"))
        yaw = self.rand.scalar(f"{name}.yaw", spec.get("yaw", 0.0), spec.get("yaw_jitter"))
        # SEAT IT ON THE TABLE. These meshes are centred on their origin, so writing the root at
        # table height buries half the body: the microwave's door link came out at z=0.356 with
        # the table top at 0.45, and the arm was swinging at a door that was underground.
        # Measure the gap from the prim origin down to its lowest point while it is still at its
        # registration pose, and lift by exactly that.
        # SAPIEN articulations are authored Y-UP, the same convention as the RoboTwin GLBs: the
        # cabinet's drawers stack along its Y and slide along its Z, the microwave door hinges
        # about its Y. Left unrotated, the drawers are asked to slide straight UP and the door
        # swings about a horizontal axis -- which is why every joint refused to move. roll=90
        # maps asset (x, y, z) -> sim (x, -z, y), putting all of that right.
        roll = float(spec.get("roll", 90.0))
        bbox, scale = spec.get("bbox"), float(spec.get("scale", 1.0))
        if bbox is not None:
            mn = np.asarray(bbox[0], float)*scale
            mx = np.asarray(bbox[1], float)*scale
            if abs(roll-90.0) < 1e-6:                       # sim z is the asset's y
                lift, ext = -float(mn[1]), np.array([mx[0]-mn[0], mx[2]-mn[2], mx[1]-mn[1]])
            else:
                lift, ext = -float(mn[2]), mx-mn
            self._artic_ext[name] = ext
            print(f"[scene] {name} stands {ext[2]*100:.1f} cm tall "
                  f"({ext[0]*100:.1f} x {ext[1]*100:.1f} cm), seated +{lift:.3f} m", flush=True)
        else:
            lift = float(spec.get("seat", 0.0))
        z = TABLE_TOP+lift+float(spec.get("z_offset", 0.0))
        quat = euler_quat(roll, spec.get("pitch", 0.0), yaw)
        dev = art.data.root_pos_w.device
        art.write_root_pose_to_sim(torch.tensor(
            np.concatenate([self.origin+np.array([xy[0], xy[1], z]), quat]),
            dtype=torch.float32, device=dev).view(1, 7))
        art.write_root_velocity_to_sim(torch.zeros((1, 6), device=dev))
        if spec.get("joint_init") is not None:
            q = art.data.joint_pos.clone()
            for j, v in dict(spec["joint_init"]).items():
                q[0, int(j)] = float(v)
            art.write_joint_state_to_sim(q, torch.zeros_like(q))
        self.articulations[name] = art
        names = list(getattr(art.data, "joint_names", []) or [])
        self.regions[name] = {"xy": xy, "half": float(spec.get("half", 0.12)),
                              "top_z": z, "yaw": yaw}
        print(f"[scene] articulation {name} at ({xy[0]:.3f},{xy[1]:.3f}) joints={names}", flush=True)
        return {"name": name, "xy": xy, "quat": quat, "joints": names}

    def link_index(self, name, link_name):
        """Index of a link BY NAME. A generated chain interleaves massless spacer links between
        its segments, so body index 13 is not seg13 -- indexing by number silently reads the
        wrong body."""
        names = list(getattr(self.articulations[name].data, "body_names", []) or [])
        return names.index(link_name) if link_name in names else None

    def joint_pos(self, name, index=0):
        """Live value of one joint (radians for a hinge, metres for a slider)."""
        return float(self.articulations[name].data.joint_pos[0, int(index)].item())

    def link_pos(self, name, index):
        """World xyz of one link's body, relative to the env origin -- e.g. a door's panel."""
        return (self.articulations[name].data.body_pos_w[0, int(index)].cpu().numpy()-self.origin)

    def reseat_objects(self, placed, step_fn, settle_steps=80):
        """Second pass: drop each object to its own measured height so nothing topples.

        Objects are written in above the table; a tall one (cup, bottle, peg) lands on its side
        and no top-down grasp can recover it.
        """
        import torch
        for _ in range(settle_steps):
            step_fn()
        for p in placed:
            if p is None:
                continue
            name = p["name"]
            obj = self.objects[name]
            ext = self.object_size(name)
            dev = obj.data.root_pos_w.device
            obj.write_root_pose_to_sim(torch.tensor(
                np.concatenate([self.origin+np.array([p["xy"][0], p["xy"][1],
                                                      TABLE_TOP+float(ext[2])/2.0+0.004
                                                      + float(p.get("z_offset", 0.0))]), p["quat"]]),
                dtype=torch.float32, device=dev).view(1, 7))
            obj.write_root_velocity_to_sim(torch.zeros((1, 6), device=dev))
            print(f"[scene] seated {name} at ({p['xy'][0]:.3f},{p['xy'][1]:.3f}) size={np.round(ext,3)}",
                  flush=True)
        for _ in range(settle_steps):
            step_fn()

    def object_size(self, name):
        """Authored bbox SIZE. Pose-independent, so it is only valid for extents, never poses."""
        try:
            import omni.usd
            from pxr import UsdGeom, Usd
            stage = omni.usd.get_context().get_stage()
            bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
            pp = self.u.scene.rigid_objects[name].root_physx_view.prim_paths[0]
            rng = bb.ComputeWorldBound(stage.GetPrimAtPath(pp)).ComputeAlignedRange()
            return np.array(rng.GetMax())-np.array(rng.GetMin())
        except Exception as e:
            print(f"[scene] size probe failed for {name}: {e}", flush=True)
            return np.array([0.05, 0.05, 0.05])

    def object_pos(self, name):
        """LIVE position from physics -- always use this for grasp targets, never the bbox."""
        return self.u.scene.rigid_objects[name].data.root_pos_w[0].cpu().numpy()-self.origin

    # ---------------- containers and props ----------------
    def spawn_container(self, spec):
        """A RoboTwin container/stand: kinematic, scaled, rolled upright, standing ON the table."""
        import isaaclab.sim as sim_utils
        import os
        usd_root = os.environ.get("ROBOTWIN_USD", "/home/yu/internship_yu/robotwin_usd")
        name = spec.get("name", "container")
        xy = self.rand.xy(name, spec.get("xy", [0.0, -0.25]), spec.get("xy_jitter"))
        scale = float(spec.get("scale", 1.0))
        rpy = spec.get("rpy", [90.0, 0.0, 0.0])      # RoboTwin default: Y-up mesh needs a roll
        usd_path = f"{usd_root}/{spec['usd']}"
        if not os.path.exists(usd_path):
            # fail with the actual cause instead of a deep Isaac traceback
            avail = sorted(p.name for p in __import__("pathlib").Path(usd_root).glob("*/*.usd"))[:8]
            raise SystemExit(
                f"[scene] container asset missing: {usd_path}\n"
                f"        convert it first, e.g.\n"
                f"          ROBOTWIN_USD={usd_root} python scripts/robotwin_convert.py --headless \\\n"
                f"            --collision none --suffix _mesh --jobs {spec['usd'].split('/')[0]}:base0\n"
                f"        (containers need the _mesh variant so their cavity is hollow)\n"
                f"        nearby files: {avail}")
        cfg = sim_utils.UsdFileCfg(usd_path=usd_path,
                                   rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True),
                                   scale=(scale,)*3)
        prim = f"/World/envs/env_0/{name}"
        cfg.func(prim, cfg, translation=tuple((self.origin+np.array([xy[0], xy[1], TABLE_TOP])).tolist()),
                 orientation=tuple(float(v) for v in euler_quat(*rpy)))
        top_z, half = self._stand_on_table(prim)
        self.regions[name] = {"xy": xy, "half": half, "top_z": top_z}
        self._warn_if_blocking(name, xy, half, top_z)
        return self.regions[name]

    def _stand_on_table(self, prim_path):
        """Raise a spawned prim so its bbox bottom meets the table; return (top_z, half_span)."""
        import omni.usd
        from pxr import UsdGeom, Usd, Gf
        stage = omni.usd.get_context().get_stage()
        prim = stage.GetPrimAtPath(prim_path)
        bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
        rng = bb.ComputeWorldBound(prim).ComputeAlignedRange()
        dz = float(self.origin[2]+TABLE_TOP)-float(rng.GetMin()[2])
        for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
            if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
                t = op.Get(); op.Set(Gf.Vec3d(float(t[0]), float(t[1]), float(t[2])+dz)); break
        rng = bb.ComputeWorldBound(prim).ComputeAlignedRange()
        ext = np.array(rng.GetMax())-np.array(rng.GetMin())
        top_z = float(rng.GetMax()[2])-self.origin[2]
        print(f"[scene] {prim_path.split('/')[-1]}: raised {dz:+.3f} m to stand on the table, "
              f"extents={np.round(ext,3)} top_z={top_z:.3f}", flush=True)
        return top_z, float(min(ext[0], ext[1]))/2.0

    def build_box(self, spec):
        """A primitive open box/tray from cuboids (license-clean, exact dimensions)."""
        import isaaclab.sim as sim_utils
        name = spec.get("name", "box")
        xy = self.rand.xy(name, spec.get("xy", [0.0, -0.25]), spec.get("xy_jitter"))
        S = float(spec.get("span", 0.26)); H = float(spec.get("wall_h", 0.05)); T = 0.010
        base = self.origin+np.array([xy[0], xy[1], TABLE_TOP])

        def cub(tag, size, off, color=(0.55, 0.38, 0.22)):
            c = sim_utils.CuboidCfg(size=tuple(size),
                                    visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color),
                                    collision_props=sim_utils.CollisionPropertiesCfg())
            c.func(f"/World/envs/env_0/{name}_{tag}", c, translation=tuple((base+np.array(off)).tolist()))

        cub("floor", (S, S, T), (0, 0, T/2))
        cub("xp", (T, S, H), (S/2, 0, H/2)); cub("xn", (T, S, H), (-S/2, 0, H/2))
        cub("yp", (S, T, H), (0, S/2, H/2)); cub("yn", (S, T, H), (0, -S/2, H/2))
        self.regions[name] = {"xy": xy, "half": S/2, "top_z": TABLE_TOP+H}
        self._warn_if_blocking(name, xy, S/2, TABLE_TOP+H)
        print(f"[scene] primitive box {name} at ({xy[0]:.3f},{xy[1]:.3f}) span={S} wall_h={H}", flush=True)
        return self.regions[name]

    def set_table_friction(self, static=1.2, dynamic=1.0, restitution=0.0, size=0.52, depth=None):
        """Give the work area a high-friction surface for tasks that push things ALONG it.

        The stock lab table is slippery, which is fine for pick-and-place but wrong for sweeping:
        debris skitters ahead of the tool and keeps going after the stroke ends.

        This lays a thin mat rather than re-binding the table's own material. The table USD is
        `table_instanceable.usd`, and a physics material cannot be bound onto an instanced prim
        -- `bind_physics_material` walks the hierarchy, finds only the instance proxy, and warns
        that it applied to nothing. The mat is 2 mm thick, so it does not change any working
        height noticeably.
        """
        import isaaclab.sim as sim_utils
        # sized to the WORK AREA, not the whole table: a 0.9 m mat overhung the table on both
        # sides and buried the target marker under it
        mat = sim_utils.CuboidCfg(
            size=(float(size), float(depth if depth is not None else size*0.68), 0.002),
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.34, 0.33, 0.35),
                                                        roughness=0.98),
            collision_props=sim_utils.CollisionPropertiesCfg(),
            physics_material=sim_utils.RigidBodyMaterialCfg(
                static_friction=float(static), dynamic_friction=float(dynamic),
                restitution=float(restitution)),
        )
        mat.func("/World/envs/env_0/work_mat", mat,
                 translation=tuple((self.origin+np.array([0.0, 0.0, TABLE_TOP+0.001])).tolist()))
        print(f"[scene] work mat laid: mu_s={static} mu_d={dynamic} size={size} m", flush=True)
        return True

    def build_shelf(self, spec):
        """An open cupboard: back panel, two sides, a top and one raised shelf.

        Open toward -y (the robot). The reachable target is the SHELF surface, not the floor of
        the unit -- placing there means clearing the shelf lip on the way in, which is what makes
        this different from dropping into an open-topped box.
        """
        import isaaclab.sim as sim_utils
        name = spec.get("name", "cupboard")
        xy = self.rand.xy(name, spec.get("xy", [-0.28, -0.05]), spec.get("xy_jitter"))
        W = float(spec.get("width", 0.22))       # along x
        D = float(spec.get("depth", 0.16))       # along y
        SH = float(spec.get("shelf_h", 0.085))   # shelf surface above the table
        H = float(spec.get("height", 0.20))
        T = 0.010
        base = self.origin+np.array([xy[0], xy[1], TABLE_TOP])
        color = tuple(spec.get("color", (0.62, 0.45, 0.28)))

        def cub(tag, size, off):
            c = sim_utils.CuboidCfg(size=tuple(size),
                                    visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color),
                                    collision_props=sim_utils.CollisionPropertiesCfg())
            c.func(f"/World/envs/env_0/{name}_{tag}", c, translation=tuple((base+np.array(off)).tolist()))

        cub("back", (W, T, H), (0, D/2, H/2))
        cub("xp", (T, D, H), (W/2, 0, H/2))
        cub("xn", (T, D, H), (-W/2, 0, H/2))
        # A roof is a trap on a table this size: tall enough for the wrist to fit under the
        # shelf and the unit itself blocks the arm from even reaching the object outside it.
        # An open-topped shelf unit still requires placing between the side walls onto a
        # raised shelf, which is the actual skill.
        if spec.get("roof", True):
            cub("top", (W, D, T), (0, 0, H-T/2))
        cub("shelf", (W-2*T, D, T), (0, 0, SH))

        # the placement target is the shelf surface, sitting inside the unit
        self.regions[name] = {"xy": xy, "half": min(W, D)/2-0.02, "top_z": TABLE_TOP+SH+T/2,
                              "shelf_z": TABLE_TOP+SH+T/2, "clearance_z": TABLE_TOP+H}
        self._warn_if_blocking(name, xy, max(W, D)/2, TABLE_TOP+H)
        print(f"[scene] cupboard {name} at ({xy[0]:.3f},{xy[1]:.3f}) shelf_z={SH:.3f} h={H}", flush=True)
        return self.regions[name]

    def build_post(self, spec):
        """A vertical post standing on the table -- the peg a ring has to be dropped over."""
        import isaaclab.sim as sim_utils
        name = spec.get("name", "post")
        xy = self.rand.xy(name, spec.get("xy", [0.0, -0.18]), spec.get("xy_jitter"))
        R = float(spec.get("radius", 0.011))
        H = float(spec.get("height", 0.13))
        BR = float(spec.get("base_radius", 0.050))
        base = self.origin+np.array([xy[0], xy[1], TABLE_TOP])
        color = tuple(spec.get("color", (0.35, 0.35, 0.40)))

        for tag, cfg, off in [
            ("base", sim_utils.CylinderCfg(radius=BR, height=0.010), (0, 0, 0.005)),
            ("rod", sim_utils.CylinderCfg(radius=R, height=H), (0, 0, 0.010+H/2)),
        ]:
            cfg.visual_material = sim_utils.PreviewSurfaceCfg(diffuse_color=color)
            cfg.collision_props = sim_utils.CollisionPropertiesCfg()
            cfg.func(f"/World/envs/env_0/{name}_{tag}", cfg,
                     translation=tuple((base+np.array(off)).tolist()))

        self.regions[name] = {"xy": xy, "half": BR, "top_z": TABLE_TOP+0.010,
                              "tip_z": TABLE_TOP+0.010+H, "radius": R}
        self._warn_if_blocking(name, xy, BR, TABLE_TOP+0.010+H)
        print(f"[scene] post {name} at ({xy[0]:.3f},{xy[1]:.3f}) r={R} h={H}", flush=True)
        return self.regions[name]

    def build_sleeve(self, spec):
        """A block with a through-channel along x: a bar slides in it and can be drawn out.

        Open at both x ends and capped on top, so the only way to move the bar is a straight
        pull along the channel -- lifting or turning jams it. That constraint IS the task.
        """
        import isaaclab.sim as sim_utils
        name = spec.get("name", "sleeve")
        xy = self.rand.xy(name, spec.get("xy", [0.0, 0.0]), spec.get("xy_jitter"))
        L = float(spec.get("length", 0.11))          # channel length along x
        W = float(spec.get("width", 0.046))          # channel clear width  (bar cross-section + slack)
        H = float(spec.get("height", 0.046))         # channel clear height
        T = float(spec.get("thickness", 0.014))
        base = self.origin+np.array([xy[0], xy[1], TABLE_TOP])
        color = tuple(spec.get("color", (0.42, 0.28, 0.16)))

        def cub(tag, size, off):
            c = sim_utils.CuboidCfg(size=tuple(size),
                                    visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color),
                                    collision_props=sim_utils.CollisionPropertiesCfg())
            c.func(f"/World/envs/env_0/{name}_{tag}", c, translation=tuple((base+np.array(off)).tolist()))

        # channel floor sits ON the table so the bar rides at a known height
        cub("floor", (L, W+2*T, T), (0, 0, T/2))
        cub("yp", (L, T, H), (0,  (W+T)/2, T+H/2))
        cub("yn", (L, T, H), (0, -(W+T)/2, T+H/2))
        cub("top", (L, W+2*T, T), (0, 0, T+H+T/2))

        self.regions[name] = {"xy": xy, "half": L/2, "top_z": TABLE_TOP+2*T+H,
                              "channel_z": TABLE_TOP+T, "length": L, "clear": W}
        self._warn_if_blocking(name, xy, max(L, W+2*T)/2, TABLE_TOP+2*T+H)
        print(f"[scene] sleeve {name} at ({xy[0]:.3f},{xy[1]:.3f}) channel {L}x{W}x{H}", flush=True)
        return self.regions[name]

    def build_whiteboard(self, spec):
        """A whiteboard lying on the table: white writing surface inside a black frame.

        The frame is raised above the surface, so it also stops a wiped smudge (or the eraser)
        from sliding off the edge -- the board is the work area, not just a coloured decal.
        """
        import isaaclab.sim as sim_utils
        name = spec.get("name", "board")
        xy = self.rand.xy(name, spec.get("xy", [0.02, -0.12]), spec.get("xy_jitter"))
        W = float(spec.get("width", 0.30))
        D = float(spec.get("depth", 0.22))
        FR = float(spec.get("frame", 0.016))       # frame rail width
        TH = float(spec.get("thickness", 0.008))
        FH = float(spec.get("frame_h", 0.014))
        base = self.origin+np.array([xy[0], xy[1], TABLE_TOP])

        def cub(tag, size, off, color):
            c = sim_utils.CuboidCfg(size=tuple(size),
                                    visual_material=sim_utils.PreviewSurfaceCfg(
                                        diffuse_color=color, roughness=float(spec.get("rough", 0.35))),
                                    collision_props=sim_utils.CollisionPropertiesCfg())
            c.func(f"/World/envs/env_0/{name}_{tag}", c, translation=tuple((base+np.array(off)).tolist()))

        white = tuple(spec.get("color", (0.97, 0.97, 0.98)))
        black = tuple(spec.get("frame_color", (0.06, 0.06, 0.07)))
        cub("panel", (W, D, TH), (0, 0, TH/2), white)
        cub("fxp", (FR, D, FH), ((W-FR)/2, 0, TH+FH/2-0.001), black)
        cub("fxn", (FR, D, FH), (-(W-FR)/2, 0, TH+FH/2-0.001), black)
        cub("fyp", (W, FR, FH), (0, (D-FR)/2, TH+FH/2-0.001), black)
        cub("fyn", (W, FR, FH), (0, -(D-FR)/2, TH+FH/2-0.001), black)

        # the wipeable area is inside the frame
        self.regions[name] = {"xy": xy, "half": min(W, D)/2-FR, "top_z": TABLE_TOP+TH,
                              "width": W-2*FR, "depth": D-2*FR}
        self._warn_if_blocking(name, xy, max(W, D)/2, TABLE_TOP+TH+FH)
        print(f"[scene] whiteboard {name} at ({xy[0]:.3f},{xy[1]:.3f}) {W}x{D} "
              f"writable half={self.regions[name]['half']:.3f}", flush=True)
        return self.regions[name]

    def build_marker(self, spec):
        """A paper-thin target region drawn on the table (visual goal, no obstruction)."""
        import isaaclab.sim as sim_utils
        name = spec.get("name", "target")
        xy = self.rand.xy(name, spec.get("xy", [0.0, -0.20]), spec.get("xy_jitter"))
        size = float(spec.get("size", 0.11))
        yaw = self.rand.scalar(f"{name}.yaw", spec.get("yaw", 0.0), spec.get("yaw_jitter"))
        m = sim_utils.CuboidCfg(size=(size, size*float(spec.get("aspect", 1.0)), 0.0015),
                                visual_material=sim_utils.PreviewSurfaceCfg(
                                    diffuse_color=tuple(spec.get("color", (0.20, 0.65, 0.30)))))
        # `z` puts the decal on a raised surface (a board, a shelf) instead of the bare table
        z = float(spec.get("z", TABLE_TOP+0.001))
        m.func(f"/World/envs/env_0/{name}", m,
               translation=tuple((self.origin+np.array([xy[0], xy[1], z])).tolist()),
               orientation=tuple(float(v) for v in euler_quat(0, 0, yaw)))
        self.regions[name] = {"xy": xy, "half": size/2.0, "top_z": z+0.001, "yaw": yaw}
        print(f"[scene] target region {name} at ({xy[0]:.3f},{xy[1]:.3f}) size={size} yaw={yaw:.0f}",
              flush=True)
        return self.regions[name]

    def build_socket(self, spec):
        """Four walls leaving a square hole whose floor is the table (peg insertion)."""
        import isaaclab.sim as sim_utils
        name = spec.get("name", "socket")
        xy = self.rand.xy(name, spec.get("xy", [0.0, -0.22]), spec.get("xy_jitter"))
        hole = float(spec.get("hole", 0.042)); WT = 0.045; WH = 0.05
        HW = hole/2.0; SPAN = hole+2*WT
        base = self.origin+np.array([xy[0], xy[1], TABLE_TOP])

        def wall(tag, size, off):
            c = sim_utils.CuboidCfg(size=tuple(size),
                                    visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.35, 0.38, 0.45)),
                                    collision_props=sim_utils.CollisionPropertiesCfg())
            c.func(f"/World/envs/env_0/{name}_{tag}", c, translation=tuple((base+np.array(off)).tolist()))

        wall("xp", (WT, SPAN, WH), (HW+WT/2, 0, WH/2)); wall("xn", (WT, SPAN, WH), (-HW-WT/2, 0, WH/2))
        wall("yp", (SPAN, WT, WH), (0, HW+WT/2, WH/2)); wall("yn", (SPAN, WT, WH), (0, -HW-WT/2, WH/2))
        self.regions[name] = {"xy": xy, "half": HW, "top_z": TABLE_TOP+WH, "hole": hole}
        print(f"[scene] socket {name} at ({xy[0]:.3f},{xy[1]:.3f}) hole={hole}", flush=True)
        return self.regions[name]

    def _warn_if_blocking(self, name, xy, half, top_z=None):
        """Warn only for props TALL enough to foul the arm at its home pose.

        A 5 cm tray under the wrist is harmless; a 23 cm rack in the same spot stops the arm
        dead on its first move, which previously looked like a mysterious tracking failure.
        """
        d = float(np.linalg.norm(np.asarray(xy, float)-HOME_EEF_WORLD))
        tall = top_z is None or float(top_z) > HOME_EEF_Z-0.06
        if d < half+0.05 and tall:
            print(f"[scene] WARNING: {name} at ({xy[0]:.3f},{xy[1]:.3f}) top_z={top_z} overlaps the "
                  f"arm's home pose {HOME_EEF_WORLD.tolist()} (gap {d-half:+.3f} m) and is tall "
                  f"enough to foul it. The arm may be blocked from its first move.", flush=True)