margin = 0.15
forwardGain = 1.0
rotationalGain = 0.2

class AvoidWanderTB:
    def __init__(self, sonarFun, motorFun):
        self.sonarFun = sonarFun
        self.motorFun = motorFun
        self.history = [None, None, None, None, None]

    def start(self):
        pass

    def done(self):
        return False

    def pushHistory(self, value):
        self.history.pop(0)
        self.history.append(value)

    # Input:
    #  ufs: list of n utility functions (action -> value)
    #  weights: list of n positive weights
    #  actions: list of actions
    # Returns the action that has the best weighted utility value
    #  computed by the weighted some of the utilities for that action
    #  given by each utility function

    def mediate(self, ufs, weights, actions):

        # Given a list actions and their utilities, return the action
        # with the best utility
        def pickBestAction(utils, actions):
            bestUtilSoFar = -1
            bestActionSoFar = None
            for (u, a) in zip(utils, actions):
                if u > bestUtilSoFar:
                    bestUtilSoFar = u
                    bestActionSoFar = a
            return bestActionSoFar

        # This is the body of the mediate function
        # Compute list of utilities, one for each action
        utils = [sum([w*uf(a) for (uf, w) in zip(ufs, weights)]) \
                    for a in actions]
        bestAction = pickBestAction(utils, actions)
        self.pushHistory(bestAction)
        return bestAction

    # Stick with one rotational direction
    def stick(self):
        nleft = len([a for a in self.history if a == 'left'])
        nright = len([a for a in self.history if a == 'right'])
        pLeft = float(nleft+0.1)/float(nleft+nright+0.2)
        pRight = float(nright+0.1)/float(nleft+nright+0.2)
      
        def uf(action):
            if action == 'stop': return 5
            elif action == 'go': return 5
            elif action == 'left': return pLeft*8
            elif action == 'right': return pRight*8
            elif action == 'forward-left': return 5
            elif action == 'forward-right': return 5
            else: return 0
        return uf

    # Wander behavior.  Returns a mapping from actions to values
    def wander(self):
        def uf(action): 
            if action == 'stop': return 0
            elif action == 'go': return 10
            elif action == 'left': return 5
            elif action == 'right': return 5
            elif action == 'forward-left': return 5
            elif action == 'forward-right': return 5
            else: return 0
        return uf

    # Return a utility function for avoiding obstacles
    # ranges is a list of objects of type pyrobot.robot.device.SensorValue
    def avoid(self, rangevalues):

        def clipAndNormalize(v, vmin, vmax):
            return max(min(v, vmax), vmin) - vmin

        # Stopping is always fine with us!
        stopU = 10

        # Clip readings between mindist and maxdist, then scale linearly
        # 0 to 10
        minDist = 0.1
        maxDist = 1.2
        minLeftDist = min(rangevalues[0:2])
        minRightDist = min(rangevalues[6:8])
        minFrontDist =  min(rangevalues[2:6])
        goU = clipAndNormalize(minFrontDist, minDist, maxDist) *10

        # Always good to turn, but bias in favor of free direction
        # In fact, the robot can sometimes crash when it tries to turn
        # in place, because it isn't circular and the back hits an
        # obstacle as it swings around.  We should check for this.
        closerToLeft = minLeftDist < minRightDist
        if closerToLeft:
            leftU = 5
            rightU = 10
        else:
            leftU = 10
            rightU = 5

        # Move while turning.
        # Use the utilities for moving forward, but modified by what's
        # on the side
        leftFrontU = goU* clipAndNormalize(minLeftDist, minDist, maxDist)
        rightFrontU = goU* clipAndNormalize(minRightDist, minDist, maxDist)

        # Now, construct the function we're going to return
        def uf(action): 
            if action == 'stop': return stopU
            elif action == 'go': return goU
            elif action == 'left': return leftU
            elif action == 'right': return rightU
            elif action == 'forward-left': return leftFrontU
            elif action == 'forward-right': return rightFrontU
            else: 
                print "Error: Wrong action" 
                return 0
            # And return it
        return uf
   
    # Given a symbolic action, call the relevant motor command
    def doAction(self, action):
        fvel = 0.2
        rvel = 0.2
        if action == 'stop': self.motorFun(0,0)
        elif action == 'go': self.motorFun(fvel,0)
        elif action == 'left': self.motorFun(0,rvel)
        elif action == 'right': self.motorFun(0,-rvel)
        elif action == 'forward-left': self.motorFun(fvel,rvel)
        elif action == 'forward-right': self.motorFun(fvel,-rvel)
        else: 
            print "Error: Wrong action" 
            return self.motorFun(0,0)

    def step(self):
        # Get the best action
        action = self.mediate([self.wander(),self.stick(), 
                               self.avoid(self.sonarFun())],
                              [1.0, 1.0, 1.0],
                              ['stop','go','left','right',
                               'forward-left','forward-right'])
        self.doAction(action)
      
        
