#FORMAT python

from pyrobot.geometry import *   # import distance function
from pyrobot.brain.behaviors import State, FSMBrain

class edge (State):

    def onActivate(self): # method called when activated or gotoed
        self.startX = self.robot.x
        self.startY = self.robot.y
        
    def step(self):
        x = self.robot.x
        y = self.robot.y
        dist = distance( self.startX, self.startY, x, y) 
        print "EDGE: actual = (%f, %f) start = (%f, %f); dist = %f" \
              % (x, y, self.startX, self.startY, dist)
        if dist > 1.0:
            self.goto('turn')
        else:
            self.robot.move(.3, 0)

class turn (State):

    def onActivate(self):
        self.th = self.robot.th

    def step(self):
        th = self.robot.th
        print "TURN: actual = %f start = %f" % (th, self.th)
        if angleAdd(th, - self.th) > 90: 
            self.goto('edge')
        else:
            self.robot.move(0, .2)

def INIT(engine): 
    brain = FSMBrain(engine=engine)
    # add a few states:
    brain.add(edge(1)) # 1 makes it active
    brain.add(turn())
    return brain