################################################################################
# pocket.py: utility methods for dealing with pockets

from hexsim 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 classifyCell(cell) != BLOCKED:
                            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 orderPocket(firstPocketCell,pocketDir,pocketCells,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
    obstacleCells - the obstacle cells that make this pocket.

    RESULT:
    The ordered pocket as a list of HSCell objects. 
    """

    ##print "inside pocket.orderPocket, pocket cells are "+str(pocketCells)
    # create the pocket perimeter
    pocketPerim = []
    curPerim = firstPocketCell
    ##print "first pocket cell = "+str(firstPocketCell)
    perimeterIndex = 1

    for cell in pocketCells: cell.perimeter = 0
    
    while curPerim.pocket:
        pocketPerim.append(curPerim)
        curPerim = obstacleCells.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 obstacleCells.at(neighbor.x,neighbor.y): cell.freeSides -= 1

    ##for cell in pocketPerim:
        ##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(cellToAdd.perimeter)
        cellToAdd.perimeter = 0
        pocketOrder.append(cellToAdd)
        obstacleCells.add(cellToAdd)

		
	# remove the cell from the perimeter
	del pocketPerim[cellIndex]
	
	# renumber the perimeter
	if len(pocketPerim) > 0:
	    curPerim = pocketPerim[0]
	    pocketPerim = []
	    perimeterIndex = 1
	    if not curPerim == None:
	        while not curPerim == None and curPerim.pocket:
		    if curPerim in pocketPerim:
		        break
                    pocketPerim.append(curPerim)
                    curPerim.perimeter = perimeterIndex
                    ##print "Current perimeter cell is "+str(curPerim)+ \
		    		##" with index "+str(perimeterIndex)
                    curPerim = obstacleCells.findNextPerimeter(pocketDir,curPerim)
                    perimeterIndex = perimeterIndex + 1

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

	# reset the number of freesides for all neighbors of cellToAdd to 6
	for cell in cellToAdd.neighbors():
	    if not cell.freeSides:
               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

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

    # remove pocket cells from the obstacleCells 
    for cell in pocketOrder: obstacleCells.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 openC(cell):
    return not cell.obstacle

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

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

    return minFreeSideIndex

################################################################################
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

################################################################################
# given a valid range of perimeter cells, this method will iterate through the
# perimeter cells, returning an array of maximum range NPP pairs.  If no NPPs
# are found in the pocket, it will return an empty array.
def findMaxRangeNPPs(pocketPerim):
    startIndex = index = 0
    endIndex = len(pocketPerim)-1
    nppEnd = 0
    npps = []
    while index < len(pocketPerim)-1:
        # for each pocket cell on the perimeter,
        # check for neighbors with nonconsecutive perimeter values
        for cell in pocketPerim[index].neighbors():
	    if cell.pocket and cell.perimeter and \
                abs(pocketPerim[index].perimeter - cell.perimeter) > 2 and \
                pocketPerim[index].perimeter < cell.perimeter:
		    startIndex = index
                    nppEnd = cell.perimeter
                    break;

        if nppEnd > 0 and nppEnd <= len(pocketPerim)-1:
            startIndex = index 
            endIndex = nppEnd-1
	    npp = []
            npp.append(pocketPerim[startIndex])
            npp.append(pocketPerim[endIndex])
	    npps.append(npp)
	    index = endIndex+1
        else:
            index = index + 1 

	nppEnd = 0
	
    return npps


###############################################################################
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
	
		
############################################################################

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

BLOCKED = 0
FREE = 1
PARTITIONING = 2

######################################################################
def classifyCell(cell):
    """
    Classifies a cell as BLOCKED, PARTITIONING, or FREE

    """

    ########################################
    # Count the number and length of open regions.
    # An open region is defined as a set of consecutively
    # located cell neighbors of a cell (to the N,NE,SE,etc.) that are open.
    
    openRegionLength = 0 # length of the current open region
    maxOpenRegionLength = 0  # length of the longest open region
    openRegionCount = 0 # number of open regions
    openRegionFlag = 0 # whether we're currently looking at an open region
        
    neighborhood = cell.neighbors() # find the cell's neighborhood

    # look at each cell in the neighborhood consecutively
    for cell in neighborhood:
	# check for an open region
	if openC(cell):
	    # if we have a new open region, flag this, and start counting
	    # its length
	    if not openRegionFlag:
		openRegionLength = 1
		openRegionCount += 1
		openRegionFlag = 1
	    # if we have a continuing open region, continue
	    # counting its length
	    else: openRegionLength += 1

	# check for closed (i.e. not open) regions
	else:
	    # if we have a new closed region, stop counting
	    # the open region, and note its length
	    if openRegionFlag:
		maxOpenRegionLength = \
		    max(maxOpenRegionLength,openRegionLength)
		openRegionFlag = 0

    maxOpenRegionLength = max(maxOpenRegionLength,openRegionLength)
		
    # below we handle open regions that are divided by the ending and beginning
    # of the loop above, as in this visual example:
    #
    #         Open(Start)
    #        _____
    # Open  /     \  Open
    #      /       \ 
    #      \       /
    #Closed \_____/  Closed
    #        Closed
    #
    # If we start the loop at the top neighbor, as indicated, and look
    # at each side in clockwise order, the loop above would find
    # TWO open regions instead of one. The following code
    # fixes this problem
    if openC(neighborhood[0]) and openC(neighborhood[len(neighborhood)-1]):
	openRegionCount -= 1
	maxOpenRegionLength += 1 

    # note that we may increase the length by too little when
    # handling this exception, but for purposes of the code below
    # this doesn't matter. (This is because we don't care if a region is
    # greater than two in length, only if it's greater than one).

    # blocked modules have no open regions with a length greater than one
    if maxOpenRegionLength < 2: return BLOCKED
    
    # partitioning modules have more than one open region
    elif openRegionLength > 1: return PARTITIONING
 
    # free modules have only one open region, at least two in length
    else: return FREE