"""Drop a ring over a standing post (RLBench put_ring_on_peg / classic peg-and-ring).

Inverse of peg insertion: the hole is in the *held* object and the fixture is the obstacle. The
ring has to clear the post's tip before it can descend, so the carry has to go high and the
alignment has to be good to within the ring's radial clearance.
"""
from ..registry import register_task
from ..envs import YamTaskEnv
from ..solvers import insert
from .. import conditions as C


@register_task("ring_post")
class RingPostTask(YamTaskEnv):
    title = "PUT THE RING OVER THE POST"
    tags = ["precision", "insertion", "alignment"]
    instruction = {"default": "Put the ring over the post."}

    # generated red ring: outer d 7.2 cm (inside the 9.4 cm jaw), hole d 4.6 cm, 3.2 cm tall.
    # It has to stay thick enough to grip -- a flat 1.4 cm washer left the jaws nothing to close
    # on and they shut beside it every time.
    rw_objects = {"rw_ring": "custom_ring/base0.usd:1.0:0.05"}
    # 0.15 put the ring 0.42 m from the shoulder and the descent stalled 7.8 cm short;
    # comfortable reach here is ~0.25 m and 0.34 m still works
    ring_xy = (0.03, 0.05)
    spawn_jitter = (0.020, 0.015)
    post_xy = (-0.04, -0.16)
    post_radius = 0.011               # 1.2 cm radial clearance inside the ring's hole
    post_height = 0.13
    post_color = (0.10, 0.35, 0.95)   # saturated blue, so the post reads against the pale table
    # 68 let the ring slip out mid-carry: it was aligned 0.10 m off the post because it was no
    # longer in the jaws by then
    gripper_effort, gripper_damping = 84, 92

    def _load_scene(self):
        self.scene.build_post({"name": "post", "xy": self.post_xy, "radius": self.post_radius,
                               "height": self.post_height, "base_radius": 0.050,
                               "color": self.post_color, "xy_jitter": (0.012, 0.012)})
        self._placed = [self.scene.place_object(
            {"name": "rw_ring", "xy": self.ring_xy, "xy_jitter": self.spawn_jitter})]

    def solve(self):
        # grasp_top=0 -> pinch the rim at mid-height; the ring is only 1.4 cm thick
        return insert.solve(self, obj="rw_ring", target="post", grasp_top=None, max_gap=0.085)

    def evaluate(self):
        return self.check(
            C.labelled("ring centred on the post",
                       C.object_moved_to("rw_ring", "post", tol=self.post_radius+0.020)),
            C.labelled("ring slid down the post rather than hanging on the tip",
                       C.object_settled_on("rw_ring", "post", max_dz=0.035)))