"""Constrained pull: draw a wooden bar straight out of a slotted block (drawer-like motion).

The bar starts most of the way inside a through-channel with only its end sticking out. The
channel caps it top and sides, so the only motion that gets it free is a straight pull along
+x -- lift it or turn it and it binds. That constraint is the whole point of the task.
"""
from ..registry import register_task
from ..envs import YamTaskEnv
from ..solvers import pull
from .. import conditions as C


@register_task("bar_pull")
class BarPullTask(YamTaskEnv):
    title = "PULL THE BAR OUT OF THE BLOCK"
    tags = ["constrained-motion", "pull", "primitive"]
    instruction = {"default": "Pull the wooden bar out of the block."}

    sleeve_xy = (-0.06, 0.02)
    sleeve_length = 0.10
    bar_length = 0.15                 # must match the CuboidCfg registered in the env
    pull_distance = 0.14
    # a light clamp: the bar is a small dry cuboid, and squeezing hard wedges it in the channel
    gripper_effort, gripper_damping = 62, 85

    def _load_scene(self):
        s = self.scene.build_sleeve({"name": "sleeve", "xy": self.sleeve_xy,
                                     "length": self.sleeve_length, "width": 0.046, "height": 0.046,
                                     "xy_jitter": (0.015, 0.020)})
        # slide the bar in so it is flush at the -x end: the overhang at +x is what we grab
        self._overhang = self.bar_length-self.sleeve_length
        self._placed = [self.scene.place_object({
            "name": "bar",
            "xy": (s["xy"][0]+self._overhang/2.0, s["xy"][1]),
            # vary how deep it starts in the slot
            "xy_jitter": (0.010, 0.0),
            # ride on the channel floor, not the table
            "z_offset": s["channel_z"]-0.45+0.002,
        })]

    def solve(self):
        # grab the MIDDLE OF THE EXPOSED STUB, not the centroid: the centroid is buried in the
        # housing, and the very tip leaves too little bar between the jaws
        grip_x = self.bar_length/2.0-self._overhang/2.0
        return pull.solve(self, obj="bar", direction=(1.0, 0.0), distance=self.pull_distance,
                          grasp_offset=(grip_x, 0.0), max_gap=0.05)

    def evaluate(self):
        return self.check(
            C.labelled("bar drawn clear of the block",
                       C.object_clear_of("bar", "sleeve", extent=self.bar_length/2.0, margin=0.02)),
            C.labelled("bar stayed flat", C.object_level("bar", 25.0)))