#!/sw/bin/vpython2.4


# TO DO:
# create user interface
# allow environments to be specified by file
# fix behavior when a Yellow cube is colored Red.  Currently, Bob backtracks
#   even if that Yellow cube is not 'between' Bob and Start.  Solution:
#   record cube path taken.
# rework how to compute shortest path to T for dimension 3.
# make coloring cubes Yellow also affect neighbors, so paths are through White cubes


from visual import *

from random import uniform


#######################################################
###                 Class Extensions                ###
#######################################################
#######################################################
# Extend the sphere, box, and cylinder classes to     #
# have an intersects method, computing whether two    #
# objects intesect (like Bob and an obstacle).        #
#                                                     #
#   WARNING: CYLINDER INTERSECTS METHOD IGNORES THE   #
#     Z-COORDINATE!  MADE FOR DIMENSION 2 CASE!       #
#######################################################


class sphere(sphere):
    def intersects(self,a):
        if (type(a) == sphereType):   # intersects test for spheres
            return (mag(self.pos-a.pos) <= self.radius+a.radius)
        elif (type(a) == pointType):  # intersects test for sphere and point
            return (mag(self.pos-a) <= self.radius)
        elif (type(a) == boxType):    # intersects test for sphere and box
            distance=0                # compute distance from sphere center to box
            cornerlo = a.pos - 0.5*a.size
            cornerhi = a.pos + 0.5*a.size
            center = self.pos
            for i in [0,1,2]:
                if   center[i] < cornerlo[i]: distance += (center[i] - cornerlo[i])**2
                elif center[i] > cornerhi[i]: distance += (center[i] - cornerhi[i])**2
                else: pass # this coordinate does not contribute to distance
            if distance <= self.radius**2: return True
            else: return False 
        else: pass

class box(box):
    def intersects(self,a):
        if (type(a) == sphereType):   # intersects test for box and sphere
            distance=0                # compute distance from sphere center to box
            cornerlo = self.pos - 0.5*self.size
            cornerhi = self.pos + 0.5*self.size
            center = a.pos
            for i in [0,1,2]:
                if   center[i] < cornerlo[i]: distance += (center[i] - cornerlo[i])**2
                elif center[i] > cornerhi[i]: distance += (center[i] - cornerhi[i])**2
                else: pass            # this coordinate does not contribute to distance
            if distance <= a.radius**2: return True
            else: return False
        elif (type(a) == pointType):  # intersects test for box and point
            return self.intersects(sphere(pos=a,radius=0))
        elif (type(a) == boxType):    # intersects test for boxes
            acornerlo = a.pos - 0.5*a.size
            acornerhi = a.pos + 0.5*a.size
            scornerlo = self.pos - 0.5*self.size
            scornerhi = self.pos + 0.5*self.size
            for i in [0,1,2]:         # test by coordinate if it's okay
                if   acornerhi[i] < scornerlo[i]: return False
                elif acornerlo[i] > scornerhi[i]: return False
                else: pass            # this coordinate is okay
            return True
        else: pass

class cylinder(cylinder):
    def intersects(self,a):
        #WARNING:  IGNORES Z-COORDINATE!  MADE FOR DIMENSION 2!
        selfp = vector(self.pos)
        selfp[2] = 0
        p = vector(a.pos)
        p[2] = 0
        if (type(a) == sphereType):   # intersects test for spheres
            return (mag(selfp-p) <= self.radius+a.radius)
        elif (type(a) == pointType):  # intersects test for sphere and point
            return (mag(selfp-p) <= self.radius)
        elif (type(a) == boxType):    # intersects test for sphere and box
            distance=0                # compute distance from sphere center to box
            cornerlo = p - 0.5*a.size
            cornerhi = p + 0.5*a.size
            center = self.pos
            for i in [0,1]:
                if   center[i] < cornerlo[i]: distance += (center[i] - cornerlo[i])**2
                elif center[i] > cornerhi[i]: distance += (center[i] - cornerhi[i])**2
                else: pass            # this coordinate does not contribute to distance
            if distance <= self.radius**2: return True
            else: return False 
        else: pass
        



#######################################################
###                   Object Types                  ###
#######################################################
#######################################################
#  * boxType                                          #
#  * cylinderType                                     #
#  * ellipsoidType                                    #
#  * sphereType                                       #
#  * pointType                                        #
#######################################################
A = box()
boxType = type(A)
A.visible=0
A = cylinder()
cylinderType = type(A)
A.visible=0
A = ellipsoid()
ellipsoidType = type(A)
A.visible=0
A = sphere()
sphereType = type(A)
A.visible=0
A = vector(0,0,0)
pointType = type(A)
A.visible=0



#######################################################
###                 Global Variables                ###
#######################################################
#######################################################
#  * r                                                #
#  * epsilon                                          #
#  * l                                                #
#  * t                                                #
#  * dt                                               #
#  * dimension                                        #
#  * startPos                                         #
#  * targetPos                                        #
#  * centerPos                                        #
#  * maxCoord                                         #
#  * centerCoord                                      #
#  * startCoord                                       #
#  * targetCoord                                      #
#######################################################

r = 3
epsilon = float(r)/10
l = r
t = 1

moveCounter = 0


#Have the user enter the dimension
x = None
while not (x == 2 or x == -2 or x == 3 or x == -3):
    try:
        x = int(raw_input('What dimension would you like? '))
    except ValueError:
        print 'Invalid Dimension'
    if not (x == 2 or x == -2 or x == 3 or x == -3):
        print "Dimension must be 2 or 3 (negative for verbose)."

if x > 0:
    dimension = x
    printing = False
if x < 0:
    dimension = -x
    printing = True

speed = 150  #50 is reasonable, 150 fast


# Initialize default start and target positions
startPos  = vector(-50,-50,-50*(dimension-2))
targetPos = vector( 50, 50, 50*(dimension-2))

centerPos = (startPos+targetPos)/2


maxCoord    = int(floor(1.5*mag(startPos-targetPos)/l)+1)
if dimension == 2:
    centerCoord = vector(int(floor(maxCoord/2)),int(floor(maxCoord)/2),0)
if dimension == 3:
    centerCoord = vector(int(floor(maxCoord/2)),int(floor(maxCoord)/2),int(floor(maxCoord)/2))



start  = sphere(pos=startPos , color=(0,1,0), radius=2)
target = sphere(pos=targetPos, color=(0,1,0), radius=2)
Bob    = sphere(pos=startPos , color=(1,1,0), radius=r)


# set the camera direction and range
if dimension == 2:
    scene.forward = vector(.1,.3,1)
    scene.range = (50,50,50)
if dimension == 3:
    scene.forward = vector(1,.1,.3)
    scene.range = (20,20,20)


#######################################################
###                  Math Functions                 ###
#######################################################
#######################################################
#  * getCoord(v)                                      #
#  * getPos(coordv)                                   #
#  * normalize(v)                                     #
#######################################################


# Return the grid coordinates of the cube containing
# the point v
def getCoord(v):
    cubeCoord = range(3)
    for i in range(0,3):
        cubeCoord[i] = int(centerCoord[i]-floor((centerPos[i]-v[i])/l+.5))
    return cubeCoord


# Return the vector corresponding to the center of the
# cube with the given coordinates
def getPos(coord):
    cubePos = range(3)
    for i in range(0,3):
        cubePos[i] = centerPos[i] - l*(centerCoord[i]-coord[i])
    return vector(cubePos)


# Return the vector of magnitude 1 in the direction
# of someVector
def normalize(v):
    if mag(v) == 0: return v
    temp = v
    for i in range(0,3):
        temp[i] /= mag(v)
    return temp



def binaryString(i):
#    b = '(' + str(i) + ')'
    b = ''
    if dimension == 2:
        i >>= 9
    while i > 0:
        j = i & 1
        b = str(j) + b
        i >>= 1
    return b



startCoord  = getCoord(startPos)
targetCoord = getCoord(targetPos)



#######################################################
###                Class Definitions                ###
#######################################################
#######################################################
#  In this section, we define:                        #
#  * the Obstacles class, used to store obstacle sets #
#  * the Cubulation class, used to store the          #
#    subdivision grid and colors of cubes             #
#######################################################



# The Obstacles class defines an object which contains
# an arbitrary number of obstacles.  Each obstacle is
# added using the Obstacles.add command.  One can also
# test for whether Bob intersects an obstacle using
# the Obstacles.intersects test.  Random spheres and
# boxes may also be added using addRandom*** methods.
class Obstacles:
    contents = []
    def add(self,a): 
        self.contents.append(a)
    def intersects(self,bot):
        for thing in self.contents:
            if thing.intersects(bot): return True
        return False
    # We want to be able to add a bunch of boxes as obstacles
    def addRandomBox(self):
        a = range(0,6)
        a[0] = uniform(-50,50)
        a[1] = uniform(-50,50)
        a[2] = uniform(-50,50)
        for j in range(3,6): a[j] = uniform(0,15)
        if dimension == 2:
            a[2] = 0
            a[5] = 10
        self.add( box( pos=a[:3], size=a[3:], color=(0,.5,1) ) )
    # Let's be able to add random spheres too
    def addRandomSphere(self):
        randomp = range(0,3)
        randomp[0] = uniform(-50,50)
        randomp[1] = uniform(-50,50)
        randomp[2] = uniform(-50,50)
        self.add( sphere( pos=randomp, radius = uniform(1,8), color = (0,.3,1) ) )
    # and cylinders when dimension == 2
    def addRandomCylinder(self):
        randomp = range(0,3)
        randomp[0] = uniform(-50,50)
        randomp[1] = uniform(-50,50)
        randomp[2] = -4.5
        self.add(cylinder(pos=randomp,axis=(0,0,9),radius=uniform(1,8), color = (0,.3,1)))





# CubeArray class is used to store Bob's memory.  It contains an array of
# boxes, called contents.  The structure of contents is an array, where
# contents[x][y][z] records information about the cube with center
# coordinate centerPos+l(x,y,z).  Each cube has 5 attributes.
# 0:  The 0 attribute is color:
#   0 = White
#   1 = Yellow
#   2 = Pink
#   3 = Red
# 1:  The 1 attribute is distance to T through White and Yellow cubes.
# 2:  The 2 attribute indicates which neighbors of the cube are exactly
# one step closer to T.
# 3:  The 3 attribute indicates which neighbors of the cube are exactly
# one step farther from T.
# 4:  The 4 attribute records which messages from T have reached the
# cube, and is used for computing distance to T.
#
# The 2 and 3 attributes are recorded via bits, with each
# neighbor getting one bit.  If the bit is 1 (on), the neighbor is
# exactly one step closer to/farther from T; otherwise, the bit is
# 0 (off).  The $i^{th}$ bit of the integer -- corresponding to
# $2^{i-1}$ -- represents the $i^{th}$ neighbor in the reverse
# lexicographic ordering of the neighbors.  Thus, if
# $a,b \in \{-1,0,1\}$, then neighbor [x+a][y+b][z+c] corresponds to
# index
#   $$1*(a+1)+ 3*(b+1) + 9(c+1).$$
# Index $13$ -- corresonding to $[x][y][z]$ -- of each attribute is
# always 0.
#
# If dimension == 2, note z and c are always 0.
#
# CubeArray also contains a number of methods:
#
#    PRIVATE METHODS:
#    arg(self,argument,coord):
#           Deals with the structure of contents[].
#    influences(self,coord,closefar):
#           Used to calculate the (important) neighbors of a cube.  If
#           closefar is 2, the neighbors are closer to T; if it's 3, farther.
#    getIndex(self,currentCoord,neighborCoord):
#           Deals with the structure of contents[] by returning index of
#           the appropriate bit corresponding to the relationship
#           between currentCoord and neighborCoord.
#    notifyNeighbors(self,coord,position):
#           Tell neighbors coord is now Red.
#
#    PUBLIC METHODS:
#    recolor(self, coord, newcolor):
#           Records new colors and generates visible cubes.
#    getColor(self,position):
#           Returns the color of the cube containing position.
#    addObstacle(self,position):
#           Add an obstacle point, and all the colorings and
#           calculations that result.
#    countFromT(self,expanding):
#           Label each cube with its distance through White cubes to T.
#           Also used in expanding the virtual bounding ellipsoid.
#    bestD(self,position):
#           Return the coordinates of an adjacent cube on a geodesic to
#           T which is not Yellow.
#    expandEllipse(self):
#           Expands virtual ellipsoid.  Currently undeveloped.
class CubeArray:
    cubeCount = 0
    if dimension == 2:
        pinkList = []
        contents = [[[[0,0,0,0,0]] for i in range(maxCoord)] for i in range(maxCoord)]
    if dimension == 3:
        contents = [[[[0,0,0,0,0] for i in range(maxCoord)] for i in range(maxCoord)] for i in range(maxCoord)]

########PRIVATE METHODS########
    def arg(self,argument,coord):
        return self.contents[coord[0]][coord[1]][coord[2]][argument]

#Returns a list of all cubes closer to (when closefar == 2) or farther from
#(when closefar ==3) T.
    def influences(self,coord,closefar):
        #if coord is unreachable from T through White cubes, return
        if self.arg(4,coord) != self.arg(4,targetCoord):
            return []
        theyAre = []
        bitString = self.arg(closefar,coord)
        for index in range(0,27):
            if bitString&(1<<index):
                theyAre.append([int(coord[0]-1+(index)%3),int(coord[1]-1+(index//3)%3),int(coord[2]-1+(index//9)%9)])
                neighborCoord = [int(coord[0]-1+(index)%3),int(coord[1]-1+(index//3)%3),int(coord[2]-1+(index//9)%9)]
        return theyAre

    def getIndex(self,currentCoord,neighborCoord):
        return 1*(neighborCoord[0]-currentCoord[0]+1) + 3*(neighborCoord[1]-currentCoord[1]+1) + 9*(neighborCoord[2]-currentCoord[2]+1)

    def printMemory(self):
        for i in range(targetCoord[0]+3,startCoord[0]-3,-1):
            for j in range(targetCoord[1]+3,startCoord[1]-3,-1):
                if self.arg(0,[j,i,0]) > 1:
                    if mag(vector(getCoord(Bob.pos)) - (j,i,0)) < 1:
                        print "X",
                    else:
                        print "*",
                elif mag(vector(getCoord(Bob.pos)) - (j,i,0)) < 1:
                    print "B",
                elif mag(vector(startCoord)-(j,i,0)) < 1:
                    print "S",
                elif mag(vector(targetCoord)-(j,i,0)) < 1:
                    print "T",
                elif self.arg(0,[j,i,0]) == 1:
                    print "Y",
                else:
                    print (self.arg(1,[j,i,0]))%10,
            print ''


    def findNewNeighbor(self,initial):          #recalculates influences locally
        needRedo = False

        affectedList = [initial]
        self.contents[initial[0]][initial[1]][initial[2]][0] += 4

        for coord in affectedList:
            for neighbor in self.influences(coord,3):
                neighborArgs = self.contents[neighbor[0]][neighbor[1]][neighbor[2]]
                neighborArgs[2] -= (1<<self.getIndex(neighbor,coord))
                if neighborArgs[2] == 0:
                    neighborArgs[0] += 4         #temporarily color puce or yellowy puce
                    affectedList.append(neighbor)
            self.contents[coord[0]][coord[1]][coord[2]][3] = 0
        if len(affectedList) > 150:             #if there are too many Puce cubes,
                                                #just re-countFromT
            for coord in affectedList:
                self.contents[coord[0]][coord[1]][coord[2]][0] -= 4
            return True

        while affectedList != []:
            tempList = []                       #stores coord, bestdist, bestneighbors
            for coord in affectedList:
                bestdist = self.arg(1,startCoord)+10
                bestneighbors = []
                for i0 in range(-1,2):
                    for i1 in range(-1,2):
                        for i2 in range(2-dimension,dimension-1):
                        #for every neighbor cube
                            neighborCoord = [coord[0]+i0,coord[1]+i1,coord[2]+i2]
                            neighborArgs = self.contents[neighborCoord[0]][neighborCoord[1]][neighborCoord[2]]
                            if neighborArgs[0] == 0:
                            #if neighbor is White 
                                if neighborArgs[1]+1 < bestdist:
                                    bestdist = neighborArgs[1]+1
                                    bestneighbors = [neighborCoord]
                                elif neighborArgs[1]+1 == bestdist:
                                    bestneighbors.append(neighborCoord)
                tempList.append([coord,bestdist,bestneighbors])
                
            bestElement = tempList[0]
            for element in tempList:
                if element[1] < bestElement[1]:
                    bestElement = element
            bestCoord = bestElement[0]
            bestArgs = self.contents[bestCoord[0]][bestCoord[1]][bestCoord[2]]

            bestArgs[0] -= 4
            bestArgs[1] = bestElement[1]
            for bestNeighbor in bestElement[2]:
                self.contents[bestNeighbor[0]][bestNeighbor[1]][bestNeighbor[2]][3] += (1<<self.getIndex(bestNeighbor,bestCoord))
                bestArgs[2] += (1<<self.getIndex(bestCoord,bestNeighbor))
            for i0 in range(-1,2):
                for i1 in range(-1,2):
                    for i2 in range(2-dimension,dimension-1):
                    #for every neighbor cube
                        neighborCoord = [bestCoord[0]+i0,bestCoord[1]+i1,bestCoord[2]+i2]
                        neighborArgs = self.contents[neighborCoord[0]][neighborCoord[1]][neighborCoord[2]]
                        if neighborArgs[0] < 2 and neighborArgs[1] == bestArgs[1]+1:
                        #if neighbor is exactly one step farther, add influence 
                            neighborArgs[2] += (1<<self.getIndex(neighborCoord,bestCoord))
                            bestArgs[3] += (1<<self.getIndex(bestCoord,neighborCoord))
            if bestElement[2] == []:
                needRedo = True

            affectedList.remove(bestCoord)
            
        return needRedo
        

    def notifyNeighbors(self,coord,position):     # tell neighbors coord is now Red
        if self.arg(2,coord) - 2 == 0:
            return False
        needRedo = False
        for neighbor in self.influences(coord,3): # those further from T
            self.contents[neighbor[0]][neighbor[1]][neighbor[2]][2] -= (1<<self.getIndex(neighbor,coord))
            self.contents[coord[0]][coord[1]][coord[2]][3] -= (1<<self.getIndex(coord,neighbor))
            if int(self.arg(2,neighbor)) == 0:
                if self.findNewNeighbor(neighbor):
                    return True                
        for neighbor in self.influences(coord,2): # those closer to T
            self.contents[neighbor[0]][neighbor[1]][neighbor[2]][3] -= (1<<(self.getIndex(neighbor,coord)))
            self.contents[coord[0]][coord[1]][coord[2]][2] -= (1<<self.getIndex(coord,neighbor))
        return needRedo    



########PUBLIC METHODS########
    def recolor(self, coord, newcolor):
        self.contents[coord[0]][coord[1]][coord[2]][0] = newcolor
        if newcolor - 1 == 0:                     # Yellow
            self.cubeCount += 1
            box(pos=getPos(coord),size=vector(l,l,l),color=vector(1,1,0))
        elif newcolor - 2 == 0:                   # Pink
            self.cubeCount += 1
#            box(pos=getPos(coord),size=vector(l,l,l),color=vector(1,.3,0))
        elif newcolor - 3 == 0:                   # Red
            self.cubeCount += 1
            box(pos=getPos(coord),size=vector(l,l,l),color=vector(1,0,0))

    def getColor(self,position):
        return self.arg(0,getCoord(position))

    def addObstacle(self,position,next):
        # for now, we'll color cubes red if their center is within r of position
        maxCoordDiff = int(floor((r)/l))
        coord = getCoord(position)
        needRedo = False
        needReturn = False
        returnToCoords = []
        numReturns=0

        self.recolor(getCoord(next),3)
        if self.arg(0,getCoord(next)) == 1 or self.notifyNeighbors(getCoord(next),position):
            needRedo = True

        for i1 in range(-maxCoordDiff,maxCoordDiff+1):
            for i2 in range(-maxCoordDiff,maxCoordDiff+1):
                for i3 in range(-maxCoordDiff*(dimension-2),maxCoordDiff*(dimension-2)+1):
                    possibleCoord = [int(coord[0]+i1),int(coord[1]+i2),int(coord[2]+i3)]
                    possiblePos = getPos(possibleCoord)
                    possibleColor = self.arg(0,possibleCoord)
                    if mag(position-possiblePos) < r and possibleColor < 3:
                        self.recolor(possibleCoord,3)
                        if self.notifyNeighbors(possibleCoord,position):
                            needRedo = True
                        if possibleColor == 1:
                            needReturn = True
                            returnToCoords.append(possibleCoord)
                            numReturns += 1
        if needReturn:
            return [-1,numReturns,returnToCoords]
        if needRedo:
            self.countFromT(False)
        return [0]

#Breadth-first search which labels each White cube with its distance to T.
#Also records best paths toward (and away from) T.
    def countFromT(self,expanding):
        #label each white cube with its distance through white cubes to T
        currentArgs = self.contents[targetCoord[0]][targetCoord[1]][targetCoord[2]]
        currentArgs[1] = 0
        currentArgs[2] = 0
        currentArgs[3] = 0
        currentArgs[4] += 1
        iteration = int(self.arg(4,targetCoord))
        toSearch = [targetCoord]
        while len(toSearch) > 0:
            currentCoord = toSearch[0]
            del toSearch[0]
            currentArgs = self.contents[currentCoord[0]][currentCoord[1]][currentCoord[2]]
            currentArgs[3] = 0
            #if the current cube is Red, reset current args
            if currentArgs[0] == 3:
                currentArgs[1] = -1
                currentArgs[2] = 0
                currentArgs[4] = iteration
            #elif current cube should be Pink, color it pink and reset
            #current args
            elif mag(startPos - getPos(currentCoord)) + mag(targetPos-getPos(currentCoord)) > ellipseSize:
                currentArgs[0] = 2
                currentArgs[1] = -1
                currentArgs[2] = 0
                currentArgs[4] = iteration
            #else look at every neighbor for influences
            else:
                for i0 in range(-1,2):
                    for i1 in range(-1,2):
                        for i2 in range(2-dimension,dimension-1):
                        #for every adjacent neighboring cube
                            neighborCoord = [currentCoord[0]+i0,currentCoord[1]+i1,currentCoord[2]+i2]
                            if neighborCoord[0] < 0 or neighborCoord[1] < 0 or neighborCoord[2] < 0 or neighborCoord[0] >= maxCoord or neighborCoord[1] >= maxCoord or neighborCoord[2] >= maxCoord:
                                break
                            neighborArgs = self.contents[neighborCoord[0]][neighborCoord[1]][neighborCoord[2]]
                            #if neighbor is Red or Pink, put in queue if needed
                            #but do nothing else.  Yellow cubes are untouched
                            #unless we're expanding the ellipsoid
                            if neighborArgs[0] > 1:
                                if (iteration-neighborArgs[4]>0):
                                    toSearch.append(neighborCoord)
                            #elif current cube is White, or Yellow and we're expanding
                            elif currentArgs[0] == 0 or (currentArgs[0] == 1 and expanding == True):
                                currentArgs[0] = 0      #color the cube White
                                #if cube hasn't been seen yet, add current cube as
                                #only influence
                                if (iteration-neighborArgs[4]>0):
                                    toSearch.append(neighborCoord)
                                    neighborArgs[1] = currentArgs[1]+1
                                    neighborArgs[2] = (1<<self.getIndex(neighborCoord,currentCoord))
                                    currentArgs[3] += (1<<self.getIndex(currentCoord,neighborCoord))
                                    neighborArgs[4] = iteration
                                #if neighbor is labeled as too far from T, add
                                #current cube as only influence
                                elif neighborArgs[1]-currentArgs[1]>1:
                                    neighborArgs[1] = currentArgs[1]+1
                                    neighborArgs[2] = (1<<self.getIndex(neighborCoord,currentCoord))
                                    currentArgs[3] += (1<<self.getIndex(currentCoord,neighborCoord))
                                #elif neighbor is influenced by current cube, record
                                #influence
                                elif neighborArgs[1] - currentArgs[1] == 1:
                                    neighborArgs[2] += (1<<self.getIndex(neighborCoord,currentCoord))
                                    currentArgs[3] += (1<<self.getIndex(currentCoord,neighborCoord))

        global printing
#        if printing == True and dimension == 2:
#            print "Printing memory..."
#            self.printMemory()


# Return the best choice, called D in GraphTraverse, of next cube to go to.
    def bestD(self,position):
        bestCoord = getCoord(position)
        bestDist = mag(startPos-targetPos)+l
        for neighborCoord in self.influences(getCoord(position),2):
            if self.arg(0,neighborCoord) == 0 and mag(getPos(neighborCoord)-targetPos) < bestDist:
                bestCoord = neighborCoord
                bestDist = mag(getPos(neighborCoord)-targetPos)
        if mag(startPos-targetPos)+l - bestDist > 0:
            return bestCoord
        return False


    def expandEllipse(self, starting):
        global ellipseSize


        if not starting:
            ellipseSize *= 2**(.5)
            print "Expanding ellipse to ",ellipseSize

        for i1 in range(0,maxCoord):
            for i2 in range(0,maxCoord):
                for i3 in range(0,(maxCoord-1)*(dimension-2)+1):
                    if self.contents[i1][i2][i3][0] == 2:
                        self.contents[i1][i2][i3][0] = 0
                        
        self.countFromT(True)

        if not starting and printing == True:
            self.printMemory()


        #We'll put in Pink cubes for 2-D
        if dimension == 2:
            for pinkCube in self.pinkList:
                pinkCube.visible = False
            for i1 in range(maxCoord):
                for i2 in range(maxCoord):
                    currentPos = getPos([i1,i2,0])
                    eDist = mag(startPos-currentPos) + mag(targetPos-currentPos)
                    if eDist > ellipseSize and eDist < ellipseSize+1.5*l:
                        self.contents[i1][i2][0][0] = 2
                        newCube = box(pos=currentPos,size=vector(l,l,l),color=vector(1,.3,0))
                        self.pinkList.append(newCube)







#######################################################
###               Secondary Functions               ###
#######################################################
#######################################################
#  In this section, we define:                        #
#  * the makeEnvironment function, used to populate   #
#    an Obstacles object                              #
#  * the moveTo command, which handles all movement   #
#######################################################





# Populate the environment with obstacles, and mark S and T
def makeEnvironment(obstacles):
    #for now, we'll populate the environment with random boxes and spheres
    if dimension == 2:
        for i in range(0,25):
            obstacles.addRandomBox()
            obstacles.addRandomCylinder()
    if dimension == 3:
        for i in range(0,100):
            obstacles.addRandomBox()
            obstacles.addRandomSphere()


# color all cubes red near any intersection points between the obstacles
# and Bob's current position
def colorCubesRed(next):
    for thing in obstacles.contents:
        if thing.intersects(Bob):
            if (type(thing) == sphereType):
                intersectionPoint = thing.pos + (thing.radius/mag(Bob.pos-thing.pos))*(Bob.pos-thing.pos)
            elif (type(thing) == cylinderType):
                thingpos = vector(thing.pos)
                thingpos[2] = 0
                intersectionPoint = thingpos + (thing.radius/mag(Bob.pos-thingpos))*(Bob.pos-thingpos)
            elif (type(thing) == boxType):
                cornerlo = thing.pos - 0.5*thing.size
                cornerhi = thing.pos + 0.5*thing.size
                center = vector(Bob.pos)
                intersectionPoint = vector(Bob.pos)
                for i in [0,1,2]:
                    if   center[i] < cornerlo[i]:
                        intersectionPoint[i] = cornerlo[i]
                    elif center[i] > cornerhi[i]:
                        intersectionPoint[i] = cornerhi[i]
            return cubes.addObstacle(intersectionPoint,next)


def moveTo(nextPos):
    global moveCounter
    global speed

    moveCounter += 1
    if printing == True:
        print 'Moving to',getCoord(nextPos), '(dist to T:',cubes.arg(1,getCoord(nextPos)),' influences:',binaryString(cubes.arg(2,getCoord(nextPos))),')'
    originalPos = vector(Bob.pos)
    while mag(Bob.pos-nextPos)>epsilon/2:
        rate(speed)
        Bob.pos += .1*normalize(nextPos-Bob.pos)
        scene.center = Bob.pos
        if obstacles.intersects(Bob):       #if an obstacle is encountered,
            if printing == True:
                print "OBSTACLE."
                print "..Back to", getCoord(originalPos), '(dist to T:',cubes.arg(1,getCoord(nextPos)),' influences:',binaryString(cubes.arg(2,getCoord(nextPos))),')'
            returnValue = colorCubesRed(nextPos)    
            if printing == True:
                print  "                        after recolor, influences:",binaryString(cubes.arg(2,getCoord(originalPos)))
            while mag(Bob.pos-originalPos)>epsilon/2:   #return to original position
                rate(speed)
                Bob.pos += .1*normalize(originalPos-Bob.pos)
                scene.center = Bob.pos
            return returnValue              #return that moveTo failed
    cubes.recolor(getCoord(nextPos),1)      #next cube successfully reached;
    return [1]                              #color next Yellow, return True




        

#######################################################
###             Boxes and GraphTraverse             ###
#######################################################
cubes = CubeArray()
ellipseSize = 1.1*mag(startPos-targetPos)

def Boxes():
    global ellipseSize
    
    cubes.expandEllipse(True)
    moveReturned = moveTo(getPos(startCoord))
    if moveReturned[0] != 1: return False
    while(1):
        GTReturned = GraphTraverse([])
        if GTReturned[0] < 3 and GTReturned[0] != 1:
            if printing == True:
                print "GTReturned: ", GTReturned[0]
            return False
        if printing == True:
            print "color of current cube: ",cubes.arg(0,getCoord(Bob.pos))
        if mag(vector(targetCoord)-vector(getCoord(Bob.pos)))<1:
            break                               #Break the loop if we're near T.
        if cubes.arg(0,getCoord(Bob.pos)) < 3:  #if current cube is not Red,
            cubes.expandEllipse(False)          #expand the bounding ellipse
        else:                                   #otherwise, current cube is Red.
            return False                        #In this case, stop.
    moveReturned = moveTo(targetPos)
    if moveReturned[0] != 1: return False
    else: return True
    

def GraphTraverse(thisPath):
    global moveCounter
    if printing == True and not (moveCounter%10):
        print "Cube count:  ", cubes.cubeCount
    thisPos = vector(Bob.pos)                   #Record the current position.
    newPath = thisPath                  
    newPath.insert(0,thisPos)

    if mag(vector(targetCoord)-vector(getCoord(Bob.pos)))<1:
        return [1]                              #We're at the target cube.

    D = cubes.bestD(Bob.pos)                    #pick the best neighbor.
    while D:                                    #While a (best) neighbor exists,
        moveReturned = moveTo(getPos(D))        #go to it.
        if moveReturned[0] == 1:                #If we successfully get to D,
            GTReturned = GraphTraverse(newPath) #recursively explore from D.
            #After the recursion:

            if GTReturned[0] == -1:             #If we need to backtrack:
                moveReturned = moveTo(thisPos)  #Return to thisPos.
                if moveReturned[0] == -1:       #If thisPos is now unreachable,
                    return [-2]                 #stop immediately.
                rList = moveReturned[1]         #Check the return list.
                if rList[1] == thisPos:         #If thisPos is on the rList,
                    del rList[1]                #delete it from the list.
                    rList[0] -= 1
                if rList[0] > 0:                #If there are still cubes in rList,
                    return [-1,rList]           #backtrack.
                return [1]                      #otherwise, explore from cube
                                                #before thisPos.

            if GTReturned[0] == -2:             #Path to S is blocked (rounding
                return GTReturned               #error), so stop immediately.


            if cubes.arg(0,targetCoord) == 3:   #If target cube is Red, stop
                return [2]                      #Target cube is Red
            if GTReturned[0] == 2:
                return GTReturned               #Target cube is Red

            #If you still have neighbors, make sure you're in the right spot
            if GTReturned[0] == 3:              #If D ran out of neighbors,
                if cubes.bestD(thisPos):        #see if thisPos has any left.
                    while GTReturned[1] != []:  #If so, backtrack to thisPos.
                        if printing:  print "..Back to ",getCoord(GTReturned[1][0])
                        moveReturned = moveTo(GTReturned[1][0])
                        if moveReturned[0]== -1:#If we can't go back,
                            return [-2]         #stop immediately
                        del GTReturned[1][0]
                else:                           #If thisPos has no unexplored
                    GTReturned[1].append(thisPos)#neighbor, append thisPos to
                    return GTReturned           #the backtrack list and return.
                

        elif moveReturned[0] == -1:             #If some Yellow cube is now Red,
            pathCutOff = False                  #check to see if this cuts off S.
            rList = [0]                         #Initiate a return list.
            for position in newPath:            #Run through cubes to S,
                posCoord = getCoord(position)   #and check each against Red
                for i in range(0,moveReturned[1]):#cubes in moveReturned.
                    if posCoord == moveReturned[2][i]:#if it's now Red,
                        rList[0] += 1           #increment rList counter and
                        rList.append(position)  #add it to the rList.
            if pathCutOff == True:              #If S is indeed cut off,
                moveReturned2 = moveTo(thisPos) #return to thisPos.
                if moveReturned2[0] < 1:        #If thisPos is now unreachable,
                    return [-2]                 #stop immediately.
                if rList[1] == thisPos:         #If thisPos is on the rList,
                    del rList[1]                #delete it from the list.
                    rList[0] -= 1
                if rList[0] > 0:                #If there are still cubes in rList,
                    return [-1,rList]           #backtrack.

            return [1]                          #Otherwise, continue from cube
                                                #before thisPos.

        #if moveReturned[0] == 0:  target cube is now Red but no Yellow cubes
        #got recolored, continue exploring neighbors of thisPos

        if mag(vector(targetCoord)-vector(getCoord(Bob.pos)))<1:
            return [1]                          #We're at the target cube.

        D = cubes.bestD(Bob.pos)                #Loop by choosing a different
                                                #best neighbor of thisPos.
    return [3,[thisPos]]



#######################################################
###                   Main Function                 ###
#######################################################


print "Loading..."

obstacles = Obstacles()

makeEnvironment(obstacles)

Boxes()
if dimension == 2:
    cubes.printMemory()
print "total number of cubes: ", cubes.cubeCount