"""Bimanual L-shaped relay push: each arm drives one leg, and neither can do the other's.

The route turns a corner on purpose. The right arm's shoulder is at y=-0.2 and the left arm's at
y=+0.2, so an arm can only push comfortably AWAY from its own shoulder: the right arm can drive
the block in +x, the left arm can drive it in -y. Getting the block from the start to the pad
needs both, in that order.

A straight-line relay does not work here, and not for a tuning reason: pushing along -y means
standing on the block's +y side, which the right arm can only reach by stretching over the block
itself -- the approach swipes it off the table.
"""
from ..registry import register_task
from ..envs import YamTaskEnv
from ..solvers import push
from .. import conditions as C


@register_task("push_relay")
class PushRelayTask(YamTaskEnv):
    title = "RELAY-PUSH THE BLOCK AROUND THE CORNER"
    tags = ["bimanual", "push", "no-grasp", "coordination", "primitive"]
    instruction = {"default": "Push the block onto the pad, using both arms in turn."}

    bimanual = True
    # Both legs run DIAGONALLY (+x and -y together), which is the direction each arm naturally
    # pushes: the left shoulder is at (-0.2,+0.2) and the right at (-0.2,-0.2), so a hand stands
    # on the near side of the block and drives it away and across. A leg 1 out at y=+0.12 with
    # the RIGHT arm never reached the block at all -- it sat at 0.20 m of error for 220 steps.
    block_spawn = (-0.15, 0.12)     # deep in the left arm's band
    spawn_jitter = (0.018, 0.015)
    corner_xy = (0.02, -0.02)       # end of the left arm's diagonal leg
    target_xy = (0.18, -0.10)       # end of the right arm's diagonal leg; left arm cannot reach
    target_size = 0.11
    gripper_effort, gripper_damping = 78, 88

    def _load_scene(self):
        self.scene.build_marker({"name": "corner", "xy": self.corner_xy, "size": 0.085,
                                 "color": (0.55, 0.55, 0.58), "xy_jitter": (0.012, 0.012)})
        self.scene.build_marker({"name": "target", "xy": self.target_xy, "size": self.target_size,
                                 "color": (0.90, 0.68, 0.15), "xy_jitter": (0.012, 0.012)})
        self._placed = [self.scene.place_object(
            {"name": "cube_y", "xy": self.block_spawn, "xy_jitter": self.spawn_jitter})]

    def solve(self):
        return push.relay(self, obj="cube_y", legs=[
            {"arm": "left", "target": "corner"},    # diagonal, away from the left shoulder
            {"arm": "right", "target": "target"},   # diagonal, away from the right shoulder
        ])

    def evaluate(self):
        return self.check(
            C.labelled("block reached the pad", C.object_moved_to("cube_y", "target", tol=0.07)),
            C.labelled("block never left the table", C.object_level("cube_y", 40.0)))