from allen import Allen, Sensors, Motors
from control import PlanExecutor
from strips import create_world_from_description
from strips import linear_solver
from strips import print_plan
import time

robot = Allen.getInstance()
robot.open()
time.sleep(0.5)
executor = PlanExecutor()

executor.setGoal(5.5, 1) # go outside
time.sleep(7)
print('goal not reached, press enter')
input()
executor.setGoal(0,0)
print('wait until the robot reaches initial position and press enter')
input()

description = []
description.append("")
description.append("Initial state: Pos(inside),Close(outside,door),Close(inside,door),Close(door,outside),Close(door,inside)")
description.append("Goal state:    Pos(outside)")
description.append("")
description.append("Actions:")
description.append("               // move from X to Y")
description.append("               Move(X,Y)")
description.append("               Preconditions:  Pos(X), Close(X,Y)")
description.append("               Postconditions: !Pos(X), Pos(Y)")

w = create_world_from_description(description)

already_solved = w.goal_reached()
if not already_solved:
    solution = linear_solver(w)
    if solution is None:
        print("No solution found :(")
    else:
        print("Solved!")
        print_plan(solution)
        for s in solution:
            action = s.simple_str()
            print("step:",action)
            _, endpos = action.split(" ", 1)
            endpos = endpos.replace(")","")
            if endpos == "door":
                executor.setGoal(2.6, 0)
            elif endpos == "outside":
                executor.setGoal(5.5, 1)
            elif endpos == "inside":
                executor.setGoal(0, 0)
            while not executor.achieved:
                time.sleep(1)

  
   