import math

### Actions

# A conservative speed factor
speed = 0.1

# An action is a list: the first element is a name string, the second is a
# list of arguments for motorOutput.

# Some basic robot actions

stop = ["stop", [0,0]]
go = ["go", [speed,0]]                  # forward velocity = speed (in meters/s)
left = ["left", [0,speed]]              # rotational velocity = speed (in radians/s)
right = ["right", [0,-speed]]

# Selecting out the parts of the action: the args for motorOutput and a string
# that describes the action (for debugging)

def actionArgs(action):
   return action[1]

def actionString(action):
   return action[0]

# List of all available actions
# Note that this is a list of procedures
allActions = [stop, go, left, right]

######################################################################
#
#    Behaviors
# 
######################################################################


### A behavior is represented as a procedure (a utility
### function), that that takes an action as input and returns a number
### that indicates how much that behavior "prefers" that action.

# Primitive wandering behavior
def wander(poseValues, rangeValues):
   def uf(action): 
      if action == stop: return 0
      elif action == go: return 10
      elif action == left: return 2
      elif action == right: return 2
      else: return 0
   return uf

# Primitive behavior for avoiding obstacles
# rangeValues is a list of the distances reported by the sonars
def avoid(poseValues, rangeValues):

   # Parameters for clip
   mindist = 0.3
   maxdist = 1.2

   # clip a given value between mindist and maxdist and scale from 0 to 1
   def clip(value, mindist, maxdist):
      return (max(mindist, min(value, maxdist)) - mindist)/(maxdist - mindist)

   # Stopping is not that useful.
   stopU = 0

   # Going forward away from obstacles, is useful when there are nearby
   # obstacles.  The utility of going forward is greater with greater free space
   # in front of the robot.  To compute the utility we read the front sonars and
   # find the minimum distance to a perceived object.  We clip shortest observed
   # distance, and scale the result between 0 and 10

   minAnyDist = min(rangeValues)
   minFrontDist =  min(rangeValues[2:6])
   if minAnyDist <= maxdist/3:
      goU = clip(minFrontDist, mindist, maxdist) * 10
   else:
      goU = 0

   # For turning, it's always good to turn, but bias the turn in favor of
   # the free direction.  In fact, the robot can sometimes get stuck when it
   # tries to turn in place, because it isn't circular and the back
   # hits an obstacle as it swings around.  Think about ways to fix
   # this bug.

   minLeftDist = min(rangeValues[0:3])
   minRightDist = min(rangeValues[5:8])
   closerToLeft = minLeftDist < minRightDist
   if closerToLeft:
      rightU = 10 - clip(minLeftDist,mindist, maxdist/3) * 10
      leftU = 0
   else:
      leftU = 10 - clip(minRightDist, mindist, maxdist/3) * 10
      rightU = 0

   # Construct the utility function and return it
   def uf(action): 
      if action == stop: return stopU
      elif action == go: return goU
      elif action == left: return leftU
      elif action == right: return rightU
      else: return 0
   return uf

######################################################################
#
#    Combining utility functions
# 
######################################################################

# addUf takes two utility functions and returns a new utility
# function, whose value any action is the sum of the two utilities

def addUf(u1, u2):
   print "u1:", [(actionString(a), u1(a)) for a in allActions]
   print "u2:", [(actionString(a), u2(a)) for a in allActions]
   return lambda action: u1(action) + u2(action)

# scaleUf takes a utility function and a number and returns a utility
# function whose value any action is the original utility, multiplied
# by the number

# Uncomment the next line and complete
# def scaleUf(u, scale):

######################################################################
#
#    Action selection
# 
######################################################################

## Here are two subprocedures called in the basic step.  We've isolated them
## as subprocedures to make the code more readable.

# Pick the best action for a utility function
def bestAction(u):
   values = [u(a) for a in allActions]
   maxValueIndex = values.index(max(values))
   return allActions[maxValueIndex]

# Do an action
def doAction(action):
   if action in allActions:
      motorOutput(actionArgs(action)[0], actionArgs(action)[1])
   else:
      print "error, unknown action", actionString(action)

# to use a utility function u, pick the best action for that
# utility function and do that action
def useUf(u):
   action = bestAction(u)
   # Print the name of the selected action procedure for debugging
   print "Best Action: ", actionString(action)
   doAction(action)

######################################################################
#
#    Main brain
# 
######################################################################

# If we're debugging in the simulator, we can cheat and get the real
# pose of the robot in the simulated world.  Take this out if you're
# running on the real robot!
def cheatPose():
    return app().output.abspose.get()   

# We've put a blank setup method here, which you can add to later if you
# want to initialize anything
def setup():
   # do nothing
   pass

## Here is the basic step
def step():
   # for debugging it's best to read the sonar distances at a
   # well-defined point in the step look, rather than sprinkling
   # calls to sonarDistances() throughout the entire program
   rangeValues = sonarDistances()
   #poseValues = pose()
   poseValues = cheatPose()

   print "pose:", poseValues, "min range:", min(rangeValues)

   target = [1.0, 0.5, 0.0]

   # after you verify that the program runs, comment out the
   # u = wander(poseValues, rangeValues) line and uncomment the u=addUf(...) line
   # and see how the behavior changes
   u = wander(poseValues,rangeValues)
   # u = addUf(wander(poseValues, rangeValues), avoid(poseValues, rangeValues))
   print "Sum:", [(actionString(a), u(a)) for a in allActions]

   useUf(u)