from task_utils import subtask_exec
from inference_utils import semantic_structuring

#Query: Please Pick up the beaker and place it onto the chemical station.
semantic_structuring("Extract anchors for beaker’s opening and chemical station's center, and refine the anchor for the beaker's opening")
tasks = [
    "Grasp the beaker",
    "Raise the beaker to a height of 20 cm",
    "Move the breaker 17cm above the chemical station",
    "Open the gripper"
    ]

current_task = 0
while current_task < len(tasks):
    if subtask_exec(tasks[current_task]):
        current_task += 1
    else:
        current_task = max(0, current_task - 1)
# done

#Query: Hold the chopstick by the thick end and place it vertically into the holder.
semantic_structuring("Extract anchors for chopstick’s center and holder's center, and refine the anchors for both the chopstick's thick end and the holder's opening")
tasks = [
    "Grasp the chopstick",
    "Raise the chopstick to a height of 20 cm",
    "Hold the chopstick vertically at a height of 20 cm",
    "Hold the chopstick 10cm vertically above the holder"
    ]

current_task = 0
while current_task < len(tasks):
    if subtask_exec(tasks[current_task]):
        current_task += 1
    else:
        current_task = max(0, current_task - 1)
# done

#Query: Please open the gripper.
tasks = [
    "Open the gripper"
    ]

current_task = 0
while current_task < len(tasks):
    if subtask_exec(tasks[current_task]):
        current_task += 1
    else:
        current_task = max(0, current_task - 1)
# done

#Query: Please pass me a tissue.
semantic_structuring("Extract anchors for tissue, not tissue box")
tasks = [   
    "grasp the tissue",
    "Raise the tissue to a height of 10 cm"
    ]
current_task = 0
while current_task < len(tasks):
    if subtask_exec(tasks[current_task]):
        current_task += 1
    else:
        current_task = max(0, current_task - 1)
# done

#Query: Please back to the inital pose.
tasks = [   
    "Back to the inital pose"
    ]
current_task = 0
while current_task < len(tasks):
    if subtask_exec(tasks[current_task]):
        current_task += 1
    else:
        current_task = max(0, current_task - 1)
# done

#Query: Pick up the volumetric flask and pour the liquid into the dish.
semantic_structuring("Extract anchors for volumetric flask’s opening and dish's center, and refine the anchor for the volumetric flask's opening")
tasks = [
    "Grasp the volumetric flask",
    "Raise the volumetric flask at a height of 25 cm",
    "Move the volumetric flask 15cm above the dish"
    "Tip the volumetric flask at a 180° angle, 10 cm above the dish"
    ]

current_task = 0
while current_task < len(tasks):
    if subtask_exec(tasks[current_task]):
        current_task += 1
    else:
        current_task = max(0, current_task - 1)
# done