def nextState(self, state, action):
     (board, (x, y)) = state
     (dx, dy) = action
     newSpaceLoc = (util.clip(x + dx, 0, 2),
                    util.clip(y + dy, 0, 2))
     newBoard = swap(board, (x, y), newSpaceLoc)
     return (newBoard, newSpaceLoc)

def actionToPoint(goalPoint, robotPose, forwardGain, rotationGain,
                  maxVel, angleEps):
    """
    Internal procedure that returns an action to take to drive
    toward a specified goal point.
    """
    rvel = 0
    fvel = 0
    robotPoint = robotPose.point()
    
    # Compute the angle between the robot and the goal point
    headingTheta = robotPoint.angleTo(goalPoint)
    
    # Difference between the way the robot is currently heading and
    # the angle to the goal.  This is an angular error relative to the
    # robot's current heading, in the range +pi to -pi.
    headingError = util.fixAnglePlusMinusPi(headingTheta - robotPose.theta)
    
    if util.nearAngle(robotPose.theta, headingTheta, angleEps):
        # The robot is pointing toward the goal, so it's okay to move
        # forward.  Note that we are actually doing two proportional
        # controllers simultaneously:  one to reduce angular error
        # and one to reduce distance error.
        distToGoal = robotPoint.distance(goalPoint)
        fvel = distToGoal * forwardGain
        rvel = headingError * rotationGain
    else:
        # The robot is not pointing close enough to the goal, so don't
        # start moving foward yet.  This is a proportional controller
        # to reduce angular error.
        rvel = headingError * rotationGain
    return io.Action(fvel = util.clip(fvel, -maxVel, maxVel),
                     rvel = util.clip(rvel, -maxVel, maxVel))

 def drawStatusBars(self):
     bottomStatusBarLocation = Location(self.location.x, int(self.location.y - 1.5 * self.size))
     graphics.drawStatusBar(bottomStatusBarLocation,
             int(2.0 * self.size),
             clip((self.timeToStarvation - self.timeSinceLastEaten) / (self.timeToStarvation - self.timeToHunger), 0.0, 1.0))
     graphics.drawStatusBar(Location(bottomStatusBarLocation.x, int(bottomStatusBarLocation.y - 1.2 * graphics.STATUS_BAR_HEIGHT)),
             int(2.0 * self.size),
             clip((self.timeToHunger - self.timeSinceLastEaten) / self.timeToHunger, 0.0, 1.0))

def probToMapColor(p, hue = yellowHue):
    """
    :param p: probability value
    :returns: a  Python color that's good for mapmaking.  It's yellow
     when p = 0.5, black when p = 1, white when p = 1.
    """
    m = 0.51
    x = p - 0.5
    v = util.clip(2*(m - x), 0, 1)
    s = util.clip(2*(m + x), 0, 1)
    return RGBToPyColor(HSVtoRGB(hue, s, v))

 def squareColor(self, indices):
     """
     @param documentme
     """
     (xIndex, yIndex) = indices
     maxValue = 10
     storedValue = util.clip(self.grid[xIndex][yIndex], -maxValue, maxValue)
     v = util.clip((maxValue - storedValue) / maxValue, 0, 1)
     s = util.clip((storedValue + maxValue) / maxValue, 0, 1)
     if self.robotCanOccupy(indices):
         hue = colors.greenHue
     else:
         hue = colors.redHue
     return colors.RGBToPyColor(colors.HSVtoRGB(hue, 0.2 + 0.8 * s, v))

        def getNextValues(self, state, inp):
            # output is average probability mass in range of true loc
            (cheat, b) = inp
            (overallTotal, count) = state
            trueState = xToState(cheat.odometry.x)
            lo = xToState(cheat.odometry.x - probRange)
            hi = xToState(cheat.odometry.x + probRange)
            total = 0
            for i in range(util.clip(lo, 0, numStates-1),
                           util.clip(hi, 1, numStates)):
                total += b.prob(i)

#            print 'score', total, (overallTotal + total) / (count + 1.0)
            return ((overallTotal + total, count + 1),
                    (overallTotal + total) / (count + 1.0))

def newDynamics(vm0, vm1, state):
    if modelinductance:
        (vel, pos,curr) = state
    else:
        (vel, pos) = state
    if clipmotorvoltages: # limit outputs from opamp
        vm0 = util.clip(vm0, 0, sourceVoltage)
        vm1 = util.clip(vm1, 0, sourceVoltage)
    voltage = (vm0 - vm1) - vel * KB # subtract back EMF
    if modelinductance:
        newCurr = (voltage + Lm/Tsim*curr) / (Rm+Lm/Tsim)
    else:
        newCurr = voltage / Rm
    if clipmotorcurrent:             # KA344 opamp limitation
        newCurr = util.clip(newCurr,-1.0,+1.0)
    # check whether stationary and current too small
    if modelfriction and vel == 0 and abs(newCurr) < istiction:
        newPos = pos
        newVel = 0 # stalled
    else:  # calculate current needed to support friction
        if modelfriction:
            if vel == 0:
                sgn = signum(newCurr)
            else:
                sgn = signum(vel)
            ifric = sgn * ifricstatic + ifricslope * vel
        else:
            ifric = 0
        newVel = vel + KM * (newCurr - ifric) * Tsim
        if vel * newVel > 0 or vel == newVel: # normal case
            newPos = pos + Tsim * (vel + newVel) / 2
        else: # velocity changes sign
            # at zero velocity now, determine whether stalled...
            if modelfriction and abs(newCurr) < istiction:
                kTsim = vel / (vel - newVel) * Tsim # time of zero crossing
                newPos = pos + kTsim * vel / 2
                newVel = 0 # stalled
            else: # there is enough current to keep going
                newPos = pos + Tsim * (vel + newVel) / 2

    if (newPos >= potAngle and newVel >= 0) or (newPos <= 0 and newVel <= 0):
        # crashed into the pot limits
        newPos = util.clip(newPos, 0.0, potAngle)
        newVel = 0
    if modelinductance:
        return (newVel, newPos, newCurr)
    else:
        return (newVel, newPos)

def leftSlipTransNoiseModel(nominalLoc, hallwayLength):
    """
    @param nominalLoc: location that the robot would have ended up
    given perfect dynamics
    @param hallwayLength: length of the hallway
    @returns: distribution over resulting locations, modeling noisy
    execution of commands;  in this case, the robot goes to the
    nominal location with probability 0.9, and goes one step too far
    left with probability 0.1.
    If any of these locations are out of bounds, then the associated
    probability mass stays at C{nominalLoc}.
    """
    d = {}
    dist.incrDictEntry(d, util.clip(loc-1, 0, n-1), 0.1)
    dist.incrDictEntry(d, util.clip(loc, 0, n-1), 0.9)
    return dist.DDist(d)

 def xToIndex(self, x):
     """
     @param x: real world x coordinate
     @return: x grid index it maps into
     """
     shiftedX = x - self.xStep/2.0
     return util.clip(int(round((shiftedX-self.xMin)/self.xStep)),
                      0, self.xN-1)

 def transitionGivenState(s):
     # A uniform distribution we mix in to handle teleportation
     transUniform = dist.UniformDist(range(numStates))
     return dist.MixtureDist(dist.triangleDist(\
                                 util.clip(s+a, 0, numStates-1),
                                 transDiscTriangleWidth,
                                 0, numStates-1),
                      transUniform, 1 - teleportProb)

 def yToIndex(self, y):
     """
     @param y: real world y coordinate
     @return: y grid index it maps into
     """
     shiftedY = y - self.yStep/2.0
     return util.clip(int(round((shiftedY-self.yMin)/self.yStep)),
                      0, self.yN-1)

def noisyTransNoiseModel(nominalLoc, hallwayLength):
    """
    @param nominalLoc: location that the robot would have ended up
    given perfect dynamics
    @param hallwayLength: length of the hallway
    @returns: distribution over resulting locations, modeling noisy
    execution of commands;  in this case, the robot goes to the
    nominal location with probability 0.8, goes one step too far left with
    probability 0.1, and goes one step too far right with probability 0.1.
    If any of these locations are out of bounds, then the associated
    probability mass goes is assigned to the boundary location (either
    0 or C{hallwayLength-1}).  
    """
    d = {}
    dist.incrDictEntry(d, util.clip(nominalLoc-1, 0, hallwayLength-1), 0.1)
    dist.incrDictEntry(d, util.clip(nominalLoc, 0, hallwayLength-1), 0.8)
    dist.incrDictEntry(d, util.clip(nominalLoc+1, 0, hallwayLength-1), 0.1)
    return dist.DDist(d)

 def getBounds(self, data, bounds):
     if bounds == 'auto':
         upper = max(data)
         lower = min(data)
         if util.within(upper, lower, 0.0001):
             upper = 2*lower + 0.0001
         boundMargin = util.clip((upper - lower) * 0.1, 1, 100)
         return ((lower - boundMargin) , (upper + boundMargin))
     else:
         return bounds

def standardDynamics(loc, act, hallwayLength):
    """
    @param loc: current loc (integer index) of the robot
    @param act: a positive or negative integer (or 0) indicating the
    nominal number of squares moved
    @param hallwayLength: number of cells in the hallway
    @returns: new loc of the robot assuming perfect execution.  If the action
    would take it out of bounds, the robot stays where it is.
    """
    return util.clip(loc + act, 0, hallwayLength-1)

def max_y_for_zoom(scale_brackets, zoom, max_zoom):
    """return the minimum and maximum y-tiles at the given zoom level for which the
    effective scale will not exceed the maximum zoom level"""
    zdiff = max_zoom - zoom
    if zdiff < 0:
        mid = 2**(zoom - 1)
        return (mid, mid - 1)
 
    max_merc_y = scale_brackets[zdiff] if zdiff < len(scale_brackets) else math.pi
    ybounds = [xy_to_tile(mercator_to_xy((0, s * max_merc_y)), zoom)[1] for s in (1, -1)]
    return tuple(u.clip(y, 0, 2**zoom - 1) for y in ybounds) #needed to fix y=-pi, but also a sanity check

def triangleDist(peak, halfWidth, lo = None, hi = None):
    """
    Construct and return a DDist over integers. The
    distribution will have its peak at index ``peak`` and fall off
    linearly from there, reaching 0 at an index ``halfWidth`` on
    either side of ``peak``.  Any probability mass that would be below
    ``lo`` or above ``hi`` is assigned to ``lo`` or ``hi``
    """
    d = {}
    d[util.clip(peak, lo, hi)] = 1
    total = 1
    fhw = float(halfWidth)
    for offset in range(1, halfWidth):
        p = (halfWidth - offset) / fhw
        incrDictEntry(d, util.clip(peak + offset, lo, hi), p)
        incrDictEntry(d, util.clip(peak - offset, lo, hi), p)
        total += 2 * p
    for (elt, value) in d.items():
        d[elt] = value / total
    return DDist(d)

def oldDynamics(vm0, vm1, state):
    (vel, pos) = state
    current = (vm0 - vm1) / 5 # voltage/5 is input current
    if abs(current) < mincurrent and abs(vel) < minvelocity:
        return (0, pos) # stalled
    else:
        newPos = pos+T*vel
        newVel = B*(vel+KM*current*Tsim)
        if (newPos >= potAngle and newVel >= 0) or (newPos <= 0 and newVel <= 0):
            newPos = util.clip(newPos, 0.0, potAngle)
            newVel = 0
        return (newVel, newPos)

def squareDist(lo, hi, loLimit = None, hiLimit = None):
    """
    Construct and return a DDist over integers.  The
    distribution will have a uniform distribution on integers from
    lo to hi-1 (inclusive).
    Any probability mass that would be below
    ``lo`` or above ``hi`` is assigned to ``lo`` or ``hi``.
    """
    d = {}
    p = 1.0 / (hi - lo)
    for i in range(lo, hi):
        incrDictEntry(d, util.clip(i, loLimit, hiLimit), p)
    return DDist(d)

 def getNextValues(self, state, inp):
     # State is: starting pose
     currentPos = inp.odometry.point()
     if state == 'start':
         print "Starting forward", self.deltaDesired
         startPos = currentPos
     else:
         (startPos, lastPos) = state
     newState = (startPos, currentPos)
     # Drive straight at a speed proportional to remaining distance to
     # be traveled.  No attempt to correct for angular deviations.
     error = self.deltaDesired - startPos.distance(currentPos)
     action = io.Action(fvel = util.clip(self.forwardGain * error,
                                 -self.maxVel, self.maxVel))
     return (newState, action)

    def getNextValues(self, state, inp):
        (goalPoint, sensors) = inp
        robotPose = sensors.odometry
        robotPoint = robotPose.point()
        robotTheta = robotPose.theta

        nearGoal = robotPoint.isNear(goalPoint, self.distEps)

        headingTheta = robotPoint.angleTo(goalPoint)
        r = robotPoint.distance(goalPoint)

        if nearGoal:
            # At the right place, so do nothing
            a = io.Action()
        elif util.nearAngle(robotTheta, headingTheta, self.angleEps):
            # Pointing in the right direction, so move forward
            a = io.Action(fvel = util.clip(r * self.forwardGain,
                                           -self.maxFVel, self.maxFVel))
        else:
            # Rotate to point toward goal
            headingError = util.fixAnglePlusMinusPi(headingTheta - robotTheta)
            a = io.Action(rvel = util.clip(headingError * self.rotationGain,
                                           -self.maxRVel, self.maxRVel))
        return (nearGoal, a)