import math
import lib601.util as util
import lib601.sm as sm
import lib601.gfx as gfx
from soar.io import io

class MySMClass(sm.SM):
    startState = 'nothing'
    def getNextValues(self, state, inp):
        #return (state, io.Action(fvel = 0, rvel = 0))
        # 3 is if it sees something in front
        # 4 is if it sees something at right

        if (self.state == 'nothing'):
            if (inp.sonars[4] <= .53):
                return ('something', io.Action(fvel = 0.05, rvel = 0))
            return ('nothing', io.Action(fvel = .2, rvel = 0))
        else:
            if(inp.sonars[7]<.3 or inp.sonars[3] <.3):
               return (state, io.Action(fvel = -0.05, rvel = .1))
                #something too close to me, turn right (cw)
            elif (inp.sonars[3] <.5 ):
                return (state, io.Action(fvel = 0.1, rvel = .3))
                #something in front of me, turn right (ccw)
            elif(inp.sonars[7] >.5):
                return (state, io.Action(fvel = 0.1, rvel = -.2))
                #nothing to the right of me, turn left (cw)
            else:
                #if something at right, go forward
                return (state, io.Action(fvel = .2, rvel = 0))



mySM = MySMClass()
mySM.name = 'brainSM'

######################################################################
###
###          Brain methods
###
######################################################################

def plotSonar(sonarNum):
    robot.gfx.addDynamicPlotFunction(y=('sonar'+str(sonarNum),
                                        lambda:
                                        io.SensorInput().sonars[sonarNum]))

# this function is called when the brain is (re)loaded
def setup():
    robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False, # slime trails
                                  sonarMonitor=True) # sonar monitor widget

    # set robot's behavior
    robot.behavior = mySM

# this function is called when the start button is pushed
def brainStart():
    robot.behavior.start(traceTasks = robot.gfx.tasks())

# this function is called 10 times per second
def step():
    inp = io.SensorInput()
    print inp.sonars[3]
    robot.behavior.step(inp).execute()
    io.done(robot.behavior.isDone())

# called when the stop button is pushed
def brainStop():
    pass

# called when brain or world is reloaded (before setup)
def shutdown():
    pass