#Behaviors Example Code
#
# Jay Summet (Georgia Tech)
#
# June 2009

from myro import *

init("/dev/rfcomm1")



def myMove():
    print "Moving"
    trans = -1
    rot = 0.0
    rec = True
    myRetValue = [ rec, trans, rot ]

    return(myRetValue)
    # return(    [ True, -1.0, 0.0]  )

    


def myTurn():
    print "checking sensors"
    L = getIR("left")
    R = getIR("right")
    if ( (R == 0) or (L == 0) ):
        print "turn left!"
        trans = 0.0
        rot = 1.0
        recomendation = True
        return( [recomendation, trans, rot])
    else:
        return( [False, 0, 0])


def seekLight():
    lightValues = getLight()
    R = lightValues[0]
    L = lightValues[2]

    if (R < 200  or L < 200):
        recomendation = True
        trans = - 0.5
        if (R < L):
            rot  = 0.5
        else:
            rot = -0.5

        return(  [ recomendation, trans, rot] )
    
    else:
        return( [ False, 0, 0] )
    
    


def arbitrate():
    myFuncs = [  seekLight, myTurn,  myMove ]

    for behavior in myFuncs:
        retVals = behavior()
        rec = retVals[0]
        trans = retVals[1]
        rot    = retVals[2]

        if (rec == True):
            translate(trans)
            rotate(rot)
            return()
        
    
            


while timeRemaining(20):
    arbitrate()

stop()
print "All done!"