import numpy as np
from env_utils import execute, reset_to_default_pose
from perception_utils import search_object
from plan_utils import get_target_pose, get_operation

# Query: move ee forward for 10cm.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose(f'a point 10cm in front of {movable.position}')
    success = execute(movable, target_pose)
    if success: break

# Query: go back to default.
reset_to_default_pose()

# Query: move the gripper behind the bowl.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 15cm behind the bowl')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: move to the back side of the table.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point on the back side of the table')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: move to the top of the plate.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 10cm above the plate')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: drop the toy inside container.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 15cm above the container')
    operation = get_operation('close everywhere but open when on top of the container')
    success = execute(movable, target_pose=target_pose, operation=operation)
    if success: break

# Query: push close the topmost drawer.
while True:
    movable = search_object('topmost drawer handle')
    target_pose = get_target_pose('a point 30cm into the topmost drawer handle')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: push the second to the left block along the red line.
while True:
    movable = search_object('second to the left block')
    target_pose = get_target_pose('the red line')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: grasp the blue block from the table.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point at the center of blue block')
    operation = get_operation('open everywhere except 1cm around the blue block')
    success = execute(movable, target_pose=target_pose, operation=operation)
    if success: break

# Query: move to the left of the brown block.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 10cm to the left of the brown block')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: move to the top of the tray that contains the lemon.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 10cm above the tray that contains the lemon')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: close drawer by 5cm.
while True:
    movable = search_object('drawer handle')
    target_pose = get_target_pose('a point 5cm into the drawer handle')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: move to 5cm on top of the soda can, when within 20cm of the wooden mug.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 5cm above the soda can')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: wipe the red dot.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('the red dot')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: grasp the mug from the shelf.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point at the center of the mug handle')
    operation = get_operation('open everywhere except 1cm around the mug handle')
    success = execute(movable, target_pose=target_pose, operation=operation)
    if success: break

# Query: move to 10cm on top of the soup bowl, and 5cm to the left of the soup bowl.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point 10cm above and 5cm to the left of the soup bowl')
    success = execute(movable, target_pose=target_pose)
    if success: break

# Query: open gripper.
while True:
    movable = search_object('gripper')
    operation = get_operation('open everywhere')
    success = execute(movable, operation=operation)
    if success: break

# Query: sweep all particles to the left side of the table.
particles = search_object('particles')
for particle in particles:
    while True:
        movable = particle
        target_pose = get_target_pose('a point on the left side of the table')
        success = execute(particle, target_pose=target_pose)
        if success: break

# Query: grasp the bottom drawer handle.
while True:
    movable = search_object('gripper')
    target_pose = get_target_pose('a point at the center of the bottom drawer handle')
    operation = get_operation('open everywhere except 1cm around the bottom drawer handle')
    success = execute(movable, target_pose=target_pose, operation=operation)
    if success: break