"""Put a bottle in the trash bin (RLBench put_rubbish_in_bin).

The bottle lies on its SIDE, so the jaw must close across its ~4 cm diameter rather than along
its 18 cm length -- the jaw only opens 9.4 cm. `jaw="auto"` picks that automatically.
"""
from ..registry import register_task
from ..envs import YamTaskEnv
from ..solvers import pick_place
from .. import conditions as C


@register_task("bottle_bin")
class BottleBinTask(YamTaskEnv):
    title = "BOTTLE -> TRASH BIN"
    tags = ["pick-place", "robotwin-asset", "jaw-rotation"]
    instruction = {"default": "Throw the bottle in the bin."}

    # 0.095 gave an 18 cm bottle, and chasing a bin big enough for it ended at a 0.64 x 0.73 m
    # dustbin standing on the table. Shrinking the bottle to 14 cm lets the actual TABLETOP bin
    # work, which is what this task wanted in the first place.
    rw_objects = {"rw_bottle": "001_bottle/base0.usd:0.075:0.10"}
    bottle_spawn = (-0.03, 0.10)
    bottle_spawn_jitter = (0.02, 0.015)
    # 17 x 14.5 cm footprint, 17 cm tall -- a bin, not furniture
    bin_spec = {"usd": "063_tabletrashbin/base0_mesh.usd", "scale": 0.09, "rpy": (90, 0, 0),
                "xy": (0.02, -0.28)}
    # Clamp window measured on this bottle: 60 crushes through the shell (jaws never stall),
    # 42 stalls but cannot hold it. 50 sits between.
    gripper_effort, gripper_damping = 50, 68

    def _load_scene(self):
        self.scene.spawn_container({"name": "bin", **self.bin_spec, "xy_jitter": (0.015, 0.015)})
        self._placed = [self.scene.place_object(
            {"name": "rw_bottle", "xy": self.bottle_spawn, "xy_jitter": self.bottle_spawn_jitter})]

    def solve(self):
        return pick_place.solve(self, obj="rw_bottle", target="bin", jaw="auto")

    def evaluate(self):
        return self.check(C.labelled("bottle in bin", C.object_in_region("rw_bottle", "bin", pad=0.04)),
                          C.labelled("bottle lifted", C.object_lifted("rw_bottle", 0.05)))