# A Behavior sequencing sample 
# D.S. Blank 
 
# This Pyro example will go (roughly) in a square 
 
# This example has two states, "edge" that goes straight, and "turn" 
# that turns 90 degrees to the left. It bounces back and forth between 
# these two states. 
 
# Note how it uses onActivate() to remember where it was when it 
# started in both cases. It then moves (forward or to the left) until 
# it has moved enough. 
 
from pyro.brain.fuzzy import * 
from pyro.brain.behaviors import * 
from pyro.brain.behaviors.core import *   # import distance function 
 
import math 
from random import random 
 
class TurnLeftBehavior (Behavior): 
    def init(self): 
        self.Effects('rotate', .1) 
        self.Effects('translate', .1)  
 
    def update(self): 
        self.IF(1, 'rotate', .1) 
        self.IF(1, 'translate', 0) 
 
class StraightBehavior (Behavior): 
    def init(self): # method called when created 
        self.Effects('translate', .1)  
        self.Effects('rotate', .1)  
 
    def update(self): 
        self.IF(1, 'translate', .1)  
        self.IF(1, 'rotate', 0) 
 
class edge (State): 
    def init(self): 
        self.add(StraightBehavior(1)) 
 
    def onActivate(self): # method called when activated or gotoed 
        self.startX = self.getRobot().get('robot', 'x')  
        self.startY = self.getRobot().get('robot', 'y')  
         
    def update(self): 
        x = self.getRobot().get('robot', 'x') 
        y = self.getRobot().get('robot', 'y') 
        dist = distance( self.startX, self.startY, x, y)  
        print "actual = (%f, %f) start = (%f, %f); dist = %f" % (x, y, self.startX, self.startY, dist) 
        if dist > 1.0: 
            self.goto('turn') 
 
class turn (State): 
    def init(self): 
        self.count = 0 
        self.add(TurnLeftBehavior(1)) 
 
    def onActivate(self): 
        self.th = self.getRobot().get('robot', 'th') 
 
    def update(self): 
        th = self.getRobot().get('robot', 'th') 
        print "actual = %f start = %f" % (th, self.th) 
        if angleAdd(th, - self.th) > 90:  
            self.goto('edge') 
 
def INIT(robot): # passes in robot, if you need it 
    brain = BehaviorBasedBrain({'translate' : robot.translate,  
                                'rotate' : robot.rotate,  
                                'update' : robot.update }, robot) 
    # add a few states: 
    brain.add(edge(1)) 
    brain.add(turn()) 
 
    brain.init() 
    return brain