################################################################################
# obstaclesInPocket.py: utility methods for dealing with pockets
# This file was specifically created to be used when dealing with obstacles
# that occur inside pockets (Matysik 2008: goalObstPerim.py)

from hexsim import *
from robot import *

################################################################################
def findPocketCells(obstacleCells,goalCells,start):
    """
    Given a cell array of contigious cells, usually of obstacle cells, 
    and a starting perimeter cell, return all pockets cells in a
    new cell array. 
    
    A pocket cell falls along a straight
    line that goes through the center of two cells in the
    list of cells, where the line is normal to the sides
    of the the hexagons it passes through. For example,
    in the below figure, if O are the obstacle cells
    listed in 'cells', all the cells labeled P would
    be pocket cells.
                    __
                 __/O \
              __/P \__/
           __/P \__/
        __/P \__/
     __/P \__/
    /O \__/
    \__/
    """
    
    pocketCells = newCellArray()

    # move along the perimeter of the cells, finding pocket cells
    curPerim = None
    while curPerim != start:
	if curPerim == None: curPerim = start

	# move outward in all 6 directions
	for direction in rotation():
	    curCell = curPerim
	    
	    isPath=0
	    isObs=0
            isPerim=0

	    if curCell.nearby(opposite(direction)).path:
		isPath = 1
	
	    if curCell.nearby(opposite(direction)).obstacle:
		isObs = 1

	    if curCell.nearby(opposite(direction)).perim:
                isPerim = 1

	    # do not move in directions that do not have
	    # an obstacle on the opposite side
	    if isPath ==0 and isObs ==0 and isPerim ==0:
		continue
	    
	    # continue looking out in this direction until we hit the 
	    # bounds of this cell array, or we know the cells we've
	    # crossed are pocket cells.
	    possiblePocketCells = []
	    while curCell.x >= obstacleCells.minX and \
		    curCell.x <= obstacleCells.maxX and \
		    curCell.y >= obstacleCells.minY and \
		    curCell.y <= obstacleCells.maxY:

		# if we find another cell in the array at the other end
		# we mark all the cells in between as pockets
		if obstacleCells.at(curCell.x,curCell.y) != None:
		    for cell in possiblePocketCells: 
                        if cell.goal:
                            pocketCells.add(cell)
		    break

		# add this cell as a possible pocket cell and
		# move to the next cell in this direction
		possiblePocketCells.append(curCell)
		curCell = curCell.nearby(direction)
		
	# move to the next cell in the perimeter
	curPerim = obstacleCells.findNextPerimeter(CW,curPerim)
	if curPerim == None:
	    print "ERROR: curPerim was set to None"
	    break

    # define the pocket flag
    defineCellAttribute("pocket",(0.5,0.7,0.0))
    for cell in pocketCells: cell.pocket = 1


    # return all the pocket cells
    return pocketCells

################################################################################
def seperatePockets(pocketCells):
    """
    Given all the pocket cells, divide them into several contiguous 
    pocket groups.

    PARAMS:
    pocketCells - a cell array of all pocket cells. The flag pocket is
       assumed to be set to 1 for all such cells, and all cells with the
       flag pocket set to 1 are assumed to be in the cell array.

    RESULTS:
    Returns a list of several HSCellArray objects, each a contiguous 
    collection of pocket cells. The 'pocket' flag for
    all pocket cells  are set to 1 + index where index is the index 
    of the HSCellArray in the returned list that contains that pocket cell.
    """

    # do a breadth first search to find all cells
    # in all contiguous blocks

    pockets = []

    while len(pocketCells) > 0:
	firstCell = pocketCells.first
	pocketCells.remove(firstCell)

	pocketQueue = [firstCell]
	newPocket = newCellArray()
	pockets.append(newPocket)

	while len(pocketQueue) > 0:
	    curCell = pocketQueue.pop(0)
	    for neighbor in curCell.neighbors():
		if neighbor.pocket and pocketCells.at(neighbor.x,neighbor.y):
		    pocketQueue.append(neighbor)
		    pocketCells.remove(neighbor)
		    
	    newPocket.add(curCell)
	    curCell.pocket = len(pockets)

    return pockets

################################################################################
def separateObstacles(obstacleCells):
    """
    Given all the obstacle cells, divide them into several contiguous 
    obstacle groups.

    PARAMS:
    obstacleCells - a cell array of all obstacle cells. The flag obstacle is
       assumed to be set to 1 for all such cells, and all cells with the
       flag pocket set to 1 are assumed to be in the cell array.

    RESULTS:
    Returns a list of several HSCellArray objects, each a contiguous 
    collection of obstacle cells. The 'obstacle' flag for
    all obstacle cells is set to 1 + index where index is the index 
    of the HSCellArray in the returned list that contains that obstacle cell.
    """

    # do a breadth first search to find all cells
    # in all contiguous blocks

    obstacles= []

    while len(obstacleCells) > 0:
	firstCell = obstacleCells.first
	obstacleCells.remove(firstCell)

	obQueue = [firstCell]
	newObst = newCellArray()
	obstacles.append(newObst)

	while len(obQueue) > 0:
	    curCell = obQueue.pop(0)
	    for neighbor in curCell.neighbors():
		if neighbor.obstacle and obstacleCells.at(neighbor.x,neighbor.y):
		    obQueue.append(neighbor)
		    obstacleCells.remove(neighbor)
		    
	    newObst.add(curCell)
	    curCell.obstacle = len(obstacles)

    return obstacles


################################################################################
def orderPocket(firstPocketCell,pocketDir,pocketCells,perimeterCells,obstacleCells):
    """
    Orders the cells of a pocket

    PARAMS:
    firstPocketCell - a valid perimeter pocket cell of this pocket 
      where modules will enter from.
    pocketDir - the direction module will rotate into this cell
    pocketCells - the pocket cells of this pocket
    perimeterCells - the goal cells that make this pocket.
    obstacleCells - the freestanding obstacles inside this pocket.

    RESULT:
    The ordered pocket as a list of HSCell objects. 
    """
    obstacleList = []
    obstaclePerimeterList = []

    # Call method to separate obstacle cells, returning a list of obstacles 
    obstacleList = countObstacles(obstacleCells)
    obstaclePerimeterList = findObstaclePerimeters(obstacleList)

    for cell in perimeterCells:
        for nCell in cell.neighbors():
            if nCell.obnb:
                print "Perimeter cell adjacent to obstacle boundary"
                return None

    # print "inside pocket.orderPocket, pocket cells are "+str(pocketCells)
    # create the pocket perimeter

    # Code below implements the Algorithm FillPocket from the WBA paper on
    # filling a pocket.  This code is modified to include pockets that 
    # contain obstacles.
    pocketPerim = []
    curPerim = firstPocketCell
    #print "first pocket cell = "+str(firstPocketCell)
    perimeterIndex = 1

    for cell in pocketCells: cell.perimeter = 0
   
    # initially, find perimeter lining the perimeterCells HSCellArray
    while curPerim.pocket:
        pocketPerim.append(curPerim)
        curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
        curPerim.perimeter = perimeterIndex
        perimeterIndex += 1	

    #print "current perimeter is "+str(curPerim)
    #print "perimeter length is "+str(perimeterIndex)
    ##print "perimeter is "+str(pocketPerim)

    # calculate the free sides for all pocket cells
    for cell in pocketCells:
	cell.freeSides = 6
	for neighbor in cell.neighbors():
	    if perimeterCells.at(neighbor.x,neighbor.y): cell.freeSides -= 1
	    if obstacleCells.at(neighbor.x,neighbor.y):cell.freeSides -= 1

    #for cell in pocketCells:
    #    print "Cell "+str(cell)+" has "+str(cell.freeSides)+" free sides"

    # while there is still a perimeter
    pocketOrder = []
    while len(pocketPerim) > 0:
        # find the cell to fill and add it to the pocket order
        cellIndex = findNextPocketIndex(pocketPerim)
        cellToAdd = pocketPerim[cellIndex]
        #print "Inside pocket.orderPocket, just adding cell "+str(cellToAdd)
	#print "added cell has perimeter # "+str(cellIndex)
	#print "cell thinks its perimeter number is "+str(cellToAdd.perimeter)
        cellToAdd.perimeter = 0
        pocketOrder.append(cellToAdd)
        perimeterCells.add(cellToAdd)
		
	# remove the cell from the perimeter
	del pocketPerim[cellIndex]
	
	### Insert code to check how close last perimeter cell is to any
	### freestanding obstacles inside pocket.  If last filled cell is within
	### one cell of a freestanding obstacle:
	###        find a linking cell to obstacle that does not block pocket,
	###        add linking cell to the pocket order, and 
	###        add linking cell and all cells in linked obstacle to obstacle        

	# renumber the perimeter
	if len(pocketPerim) > 0:
	    curPerim = pocketPerim[0]
	    pocketPerim = []
	    perimeterIndex = 1
	    if not curPerim == None:
	        while not curPerim == None and curPerim.pocket:
                    pocketPerim.append(curPerim)
                    curPerim.perimeter = perimeterIndex
                    curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
                    perimeterIndex = perimeterIndex + 1

        #print "Cells on pocket perimeter are "+str(pocketPerim)

	# reset the number of freesides for all neighbors of cellToAdd that
	# are not on the pocketPerim to 6
	#for cell in cellToAdd.neighbors():
	#    if not cell.freeSides:
        #        print "cell "+str(cell)+" has "+str(cell.freeSides)+" free sides"
        #        cell.freeSides = 6
            
	# then remove a free side for each of the neighbors of the cell we added
	for cell in cellToAdd.neighbors(): 
	    if cell.pocket: 
	        cell.freeSides -= 1
	
	# if the newly numbered perimeter contains a obnb cell, find that cell and 
	# add it to the surface (perimeterCells) and renumber the perimeter around
	# obstacle
	if len(pocketPerim) > 0:
            curPerim = pocketPerim[0]
	    cellIndex = 0
	    if not curPerim == None:
                while not curPerim == None and curPerim.pocket:
                    curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
		    cellIndex += 1
		    if curPerim.obnb:
			obnbIndex = cellIndex
                        break
	    if curPerim.obnb:
                newObNbCell = curPerim
                curPerim.perimeter = 0
                pocketOrder.append(curPerim)
                perimeterCells.add(curPerim)
                #print "Just added cell "+str(curPerim)
		#print "Just adding obstacle "+str(curPerim.perim-1)
		for cell in obstacleList[curPerim.perim-1]:
                    perimeterCells.add(cell)
		for cell in obstaclePerimeterList[curPerim.perim-1]:
		    cell.obnb = 0
	        del pocketPerim[obnbIndex]
	        # renumber perimeter around bridge and obstacle
                if len(pocketPerim) > 0:
	            curPerim = pocketPerim[0]
	            pocketPerim = []
	            perimeterIndex = 1
	            if not curPerim == None:
	                while not curPerim == None and curPerim.pocket:
                            pocketPerim.append(curPerim)
                            curPerim.perimeter = perimeterIndex
                            curPerim = perimeterCells.findNextPerimeter(pocketDir,curPerim)
                            perimeterIndex = perimeterIndex + 1
	        #print "Cells on pocket perimeter after obnb are "+str(pocketPerim)
                # reset the number of freesides for all neighbors of cellToAdd that
		# are not in perimeter to 6
                #for cell in newObNbCell.neighbors():
	        #    if not cell.freeSides and cell.pocket:
	        #        cell.freeSides = 6
            
	        # then remove a free side for each of the neighbors of the cell we added
	        for cell in newObNbCell.neighbors(): 
	            if cell.pocket: 
	                cell.freeSides -= 1


    ### END OF WHILE len(pocket) > 0

    # remove pocket cells from the perimeterCells 
    for cell in pocketOrder: perimeterCells.remove(cell)
    
    for cell in pocketCells:
        if not cell in pocketOrder:
            pocketOrder.append(cell)

    ##print "The pocket order is: " + str(pocketOrder) + "."

    # return the pocket order
    return pocketOrder
	

################################################################################
def findObstaclePerimeters(obstacleList):

    obstPerimeter = []
    defineCellAttribute("obnb",(0.5,0.0,0.05),4)
    i = 1
    # Steve Matysik:  Find obstacle perimeter
    for obst in obstacleList:
        #Find the perimeter of obstacles cells
        obstacleNB = [] 
        for obstacleCell in obst:
            for cell in obstacleCell.neighbors():
                if not cell.obstacle:
                    obstacleNB.append(cell)
                    cell.obnb = 1
		    cell.perim = i 
        i = i + 1
        #print "ObstacleNB: " + str(obstacleNB)
        obstPerimeter.append(obstacleNB)

    return obstPerimeter

################################################################################
def findNextPocketIndex(pocketPerim):

    minFreeSides = 6
    minFreeSideIndex = 0
    startIndex, endIndex = findNPPRange(pocketPerim)
    #print "Range of indicies is: " + str(startIndex)+", "+str(pocketPerim[startIndex]) + " - " + \
    #	str(endIndex)+", "+str(pocketPerim[endIndex]) + "."
    for index in range(startIndex,endIndex+1):
	if pocketPerim[index].freeSides < minFreeSides:
	    minFreeSides = pocketPerim[index].freeSides
	    minFreeSideIndex = index
	    if minFreeSides == 2:
                break

    return minFreeSideIndex

################################################################################
# This method is used for pocket filling, when finding the deepest NPP that
# is closest to the entry point. Last modified for 9/15/08 icra09 deadline.

def findNPPRange(pocketPerim):
    startIndex = index = 0
    endIndex = len(pocketPerim)-1
    nppEnd = 0
    while index < endIndex:
        # for each pocket cell on the perimeter,
        # check for neighbors with nonconsecutive perimeter values
	lowest = endIndex
        for cell in pocketPerim[index].neighbors():
	    if cell.pocket and cell.perimeter and \
                abs(pocketPerim[index].perimeter - cell.perimeter) > 1 and \
                pocketPerim[index].perimeter < cell.perimeter:
		    # The next line was crucial for correctness in icra09 paper
		    if cell.perimeter < lowest:  
                        nppEnd = cell.perimeter
		        lowest = nppEnd
			
        if nppEnd > 0 and nppEnd <= endIndex:
            startIndex = index
            endIndex = nppEnd
	    
        index += 1
	nppEnd = 0
	

    if startIndex == 0 and endIndex == len(pocketPerim)-1:
	return startIndex, endIndex
    else:
	return startIndex+1, endIndex-1

###############################################################################
def findNPPDelay(firstPocketCell,pocketCells,obstacleCells,pocketDir):
   curPerim = firstPocketCell
   pocketPerim = []
   while curPerim.pocket:
	pocketPerim.append(curPerim)
	curPerim = obstacleCells.findNextPerimeter(pocketDir,curPerim)

   #print "current perimeter is "+str(pocketPerim)

   startIndex, endIndex = findNPPRange(pocketPerim)

   return endIndex - startIndex


###############################################################################

# As appears in the Multiple Pocket paper, count pockets
# separates pocket cells into discrete pockets.
def countPockets(pocketCells):
    pc = []
    for i in pocketCells:
	pc.append(i)
    i = 0
    q = []
    pockets = []
    while len(pc) > 0:
	x = pc[0]
	q.append(x)
	pc.remove(x)
	pockets.append([])
	while len(q) > 0:
	    #print q
	    u = q.pop(0)
	    contacts = []
	    for cell in u.neighbors():
		#print cell
		if cell in pc:
		    contacts.append(cell)
	    for c in contacts:
		q.append(c)
		pc.remove(c)
	    pockets[i].append(u)
	i += 1
    return pockets
	
		
############################################################################
# Takes in a HSCellArray and copies it into a list.  Then does a BFS to
# find each connected cluster of obstacle cells and returns the clusters
# as lists within a list
def countObstacles(obstacleCells):
    ob = []
    for i in obstacleCells:
	ob.append(i)
    i = 0
    q = []
    obstacles = []
    while len(ob) > 0:
	x = ob[0]
	q.append(x)
	ob.remove(x)
	obstacles.append([])
	while len(q) > 0:
	    u = q.pop(0)
	    contacts = []
	    for cell in u.neighbors():
		if cell in ob:
		    contacts.append(cell)
	    for c in contacts:
		q.append(c)
		ob.remove(c)
	    obstacles[i].append(u)
	i += 1
    return obstacles
	
#############################################################################

def orderObstacles(obstacles):
    northeastarray = [];
    i = 0
    for obs in obstacles:
	northeastarray.append((findNortheast(obs), i))
	i += 1
    sortedNE = sortNEarray(northeastarray)
    orderedObstacles = []
    for ob in sortedNE:
	orderedObstacles.append(obstacles[ob[1]])
    return orderedObstacles
    
def sortNEarray(array):
    if len(array) < 2:
	return array
    middle = len(array)/2
    end = len(array)
    first = []
    second = []
    for i in range(0, middle):
        first.append(array[i])
    for i in range(middle, end):
        second.append(array[i])
    return merge(sortNEarray(first), sortNEarray(second))

def merge(a1, a2):
    counter1 = 0
    counter2 = 0
    newarray = []
    while counter1 < len(a1) and counter2 < len(a2):
	if a1[counter1][0][0] < a2[counter2][0][0]:
	    newarray.append(a1[counter1])
	    counter1 = counter1 + 1
	elif a1[counter1][0][0] == a2[counter2][0][0] and a1[counter1][0][1] > a2[counter2][0][1]:
	    newarray.append(a1[counter1])
	    counter1 = counter1 + 1
	else:
	    newarray.append(a2[counter2])
	    counter2 = counter2 + 1
    while counter1 < len(a1):
	newarray.append(a1[counter1])
	counter1 = counter1 + 1
    while counter2 < len(a2):
	newarray.append(a2[counter2])
	counter2 = counter2 + 1
    return newarray
    
def findNortheast(obstacle):
    if len(obstacle) < 1:
	print "no obstacle given to findNortheast"
    northmost = obstacle[0].y
    eastmost =  obstacle[0].x
    for obs in obstacle:
	if obs.x < eastmost:
	    northmost = obs.y
	    eastmost = obs.x
	elif obs.x == eastmost and obs.y > northmost:
	    northmost = obs.y
    return (eastmost,northmost)

###############################################################################


def pocketMember(cell, pockets):
    for pocket in pockets:
    	for pCell in pocket:
	    if cell.x == pCell.x and cell.y == pCell.y:
	    	return pocket

# Last change 8/27/08
#
# Problem was still in finding the lowest NPP that modules
# must traverse first.  I changed the code to check that
# the cell.perimeter < lowest before resetting nppEnd.
# If this was not done, the method would return the wrong
# NPP and modules would collide in more "inner" NPPs