#
#Example of Behavior based control with three simple behaviors.
#
# Jay Summet
# Sep 24nd, 2008, released to the public domain.


from myro import *
init()

#Move behavior: Always recommends that the robot move.
def move():

    return(  [ True, -1.0 , 0.0 ]  )

#seekLight behavior: turns towards (bright) light
seekRSpeed = 0.33
seekTSpeed = -0.5
seekThreshold = 200

def seekLight():
    global seekRSpeed
    global seekTSpeed
    
    values = getLight()
    leftV = values[0]
    rightV = values[2]

    if ( leftV > seekThreshold) and (rightV > seekThreshold):
        return( [False, 0.0, 0.0]   )
    else:
        if (leftV < rightV):
            return( [True, seekTSpeed, seekRSpeed]   )
        else:
            return( [True, seekTSpeed, - seekRSpeed] )

    
        
    
    
#Avoid behavior, makes the robot turn away from obstacles.
def avoid():
    sensorVals = getIR()
    leftV = sensorVals[0]
    rightV = sensorVals[1]

    if (leftV == 0) or (rightV == 0):
        return(  [ True, 0.0 , 1.0 ]  )
    else:
        return(  [ False, 0.0, 0.0 ] )
    

#Calls each behavior in the ordere listed. If a behavior gives a recommendation,
# arbitrate passes the recommended action on to the robot and then skips the
# rest of the behaviors. (So avoid has a higher priority than move.)
def arbitrate():
    behaviors = [avoid, seekLight, move]

    for behavior in behaviors:
        values = behavior()
        
        hasRec = values[0]
        transVal = values[1]
        rotVal = values[2]

        print "behavior:, ", behavior, "has values:", hasRec, transVal, rotVal

        if (hasRec == True):
            translate(transVal)
            rotate(rotVal)
            return()
    
   
#Main code, does the behavior for 10 seconds, then stops the robot:

while( timeRemaining(10)):
    arbitrate()

stop()