from cs1robots import *
load_world("worlds/maze1.wld")
hubo = Robot(beepers = 1)

hubo.set_trace("BLUE")
hubo.set_pause(0.1)

def turn_right():
    hubo.turn_left()
    hubo.turn_left()
    hubo.turn_left()

    
hubo.drop_beeper()
hubo.turn_left()
hubo.move()


while(not hubo.on_beeper()):
    if(not hubo.front_is_clear() and not hubo.on_beeper()):
        hubo.turn_left()
    elif(not hubo.on_beeper()):
        hubo.move()
        turn_right()
        if(hubo.front_is_clear() and not hubo.on_beeper()):
            hubo.move()
            turn_right()
            if(hubo.front_is_clear() and not hubo.on_beeper()):
                hubo.move()
            elif(not hubo.on_beeper()):
                hubo.turn_left()
        elif(not hubo.on_beeper()):
            hubo.turn_left()

hubo.pick_beeper()
turn_right()
hubo.move()
turn_right()
hubo.move()

while(not hubo.on_beeper()):
    if(not hubo.front_is_clear() and not hubo.on_beeper()):
        hubo.turn_left()
    elif(not hubo.on_beeper()):
        hubo.move()
        turn_right()
        if(hubo.front_is_clear() and not hubo.on_beeper()):
            hubo.move()
            turn_right()
            if(hubo.front_is_clear() and not hubo.on_beeper()):
                hubo.move()
            elif(not hubo.on_beeper()):
                hubo.turn_left()
        elif(not hubo.on_beeper()):
            hubo.turn_left()
            
hubo.drop_beeper()