def publishChecked(grid):
    global ckd
    print "publishing"
    k=0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = 0.3 # edit for grid size .3 for simple map
    cells.cell_height = 0.3 # edit for grid size

    for i in range(1,10): #height should be set to hieght of grid
        for j in range(1,9): #height should be set to hieght of grid
            #print k # used for debugging
            if (grid[k] == 0):
                point=Point()
                point.x=j*.3+.32 # edit for grid size
                point.y=i*.3-.15 # edit for grid size
                point.z=0
                cells.cells.append(point)
            k=k+1
        k=k+1
        if (grid[k] == 0):
            point=Point()
            point.x=j*.3+.62 # edit for grid size
            point.y=i*.3-.15 # edit for grid size
            point.z=0
            cells.cells.append(point)

    #print cells # used for debugging
    ckd.publish(cells)           

def publishCells(grid, num):
    global ckd
    global bud
    global front
    global xOffset
    global yOffset
    # print "publishing"
    k = 0
    cells = GridCells()
    cells.header.frame_id = "map"
    cells.cell_width = 0.2  # edit for grid size .3 for simple map
    cells.cell_height = 0.2  # edit for grid size

    for square in grid:  # height should be set to hieght of grid
        # print k # used for debugging
        point = Point()
        point.x = square.x * cells.cell_width + cells.cell_width + xOffset  # edit for grid size
        point.y = square.y * cells.cell_height + cells.cell_width + yOffset  # edit for grid size
        point.z = 0
        cells.cells.append(point)

    # print cells # used for debugging
    if num == 100:
        pub.publish(cells)
    if num == 1:
        ckd.publish(cells)
    if num == 2:
        front.publish(cells)
    if num == 4:
        bud.publish(cells)

def map_callback(ret):
    rospy.loginfo("Map Callback")
    #print ret
    map_data = OccupancyMap(ret)
    centroids = map_data.findFrontiers()

    # Generate all grid cells info
    gridCells = GridCells()
    gridCells.cell_width = ret.info.resolution
    gridCells.cell_height = ret.info.resolution
    gridCells.header.frame_id = 'map'
    gridCells.header.stamp = rospy.Time.now()

    #Generate all of the point data
    for centroid in centroids:
        point = Point()
        cell = transform_grid_cells_to_map_meters((centroid[0], centroid[1]), ret.info)
        point.x = cell[0]
        point.y = cell[1]
        point.z = 0
        gridCells.cells.append(point)

    # Publish this data
    global frontierPub
    rospy.loginfo("Publishing frontier")
    frontierPub.publish(gridCells)

def publishCells(nodes, publisher):

    """
    Publish Cells
    Publishes a list of nodes as GridCells to RViz
    Uses the given publisher
    """

    global offsetY
    global offsetX

    print "publishing frontiers"

    # resolution and offset of the map
    k=0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution

    for node in nodes:
        point=Point()
        point.x = (node.x * resolution) + offsetX + (0.72 / resolution)
        point.y = (node.y * resolution) + offsetY + (0.643 / resolution)
        point.z = 0
        cells.cells.append(point)

    publisher.publish(cells)

def setStart(msg):
    global start, pub_start, mapInfo, mapData

    #define a point object that will represent our start point on the grid
    point = msg.pose.pose.position #this needs to be adjuseted depending on how the gridcells object is using the point object.

    #set the starting point for the search to the gridpoint defined by the user mouse click.
    start = globalToGrid(point, mapInfo)

    #convert the point to a grid position
    point.x = round(point.x/mapInfo.resolution) * mapInfo.resolution
    point.y = round(point.y/mapInfo.resolution) * mapInfo.resolution

    #define a new gridcells object 
    gridCells = GridCells()
    
    #start construction our message
    gridCells.header = msg.header
    gridCells.cell_width = mapInfo.resolution
    gridCells.cell_height = mapInfo.resolution
    cells = [point]
    gridCells.cells = cells

    pub_start.publish(gridCells)
    print "startpoint set"

def check_occupancy(current, debug):    #check whether the cell is occupied
    FRONTIER_PUBLISH = rospy.Publisher('/Frontier', GridCells)   #Frontier Grid Cells Publisher   
    
    neighbors_all = find_all_neighbors(current)
    #print neighbors_all
    neighbors_empty = []
    Frontier = []
    
    for neighbor_index, neighbor in enumerate(neighbors_all):
        #cell_index = ( neighbor.y - (RESOLUTION/2) ) * WIDTH
        #if MAP_DATA[int(cell_index)] < 50:   #EMPTY CELL
        for elem in PtArr_Empty:
            if numpy.allclose([elem.x], [neighbor.position.x]) and numpy.allclose([elem.y], [neighbor.position.y]):
            #if elem.x == neighbor.position.x and elem.y == neighbor.position.y:
            #print "index:", cell_index
            #print int(cell_index)
                Frontier.append(Point(neighbor.position.x, neighbor.position.y, 0))
                neighbors_empty.append(neighbor.position)
               # print "adding neighbor: ", neighbor_index
            #if elem.x == neighbor.position.x:
               # print "element_x: ", elem.x
                #print "element_y: ", elem.y
    #print PtArr_Empty
    Frontier_Empty  = GridCells()
    Frontier_Empty.header = HEADER
    Frontier_Empty.cell_width = RESOLUTION
    Frontier_Empty.cell_height = RESOLUTION
    Frontier_Empty.cells = Frontier
    if debug:
        count = 0
        while count < 1500:
            FRONTIER_PUBLISH.publish(Frontier_Empty)
            count = count+1
    return neighbors_empty

def MakeGridCellsFromList (cellList):
	gridCells = GridCells()
	gridCells.cell_width = .2
	gridCells.cell_height = .2
	gridCells.cells = cellList
	gridCells.header.frame_id = 'map'
	return gridCells

def mapCallback(OccupancyGrid):
    print 'Got Occupancy Map'

    global expandGrid
    global ocGridMeta
    global ocGrid
    expandGrid = GridCells()
    ocGridMeta = OccupancyGrid.info
    ocGrid = [[0 for _ in range(OccupancyGrid.info.height)] for _ in range(OccupancyGrid.info.width)] #Generate the ocgrid as a 2-d array
    for row in range(OccupancyGrid.info.height):
        for col in range(OccupancyGrid.info.width):
            i = ( row * OccupancyGrid.info.width) + col
            if( OccupancyGrid.data[i] >= 30):  #if there is a 60% propabability of obsticle
                ocGrid[col][row] = 1 #flag
                expandObsticals(col, row) #expand the discovered obstical
            elif OccupancyGrid.data[i] == -1:  #if the cell is unknown
                ocGrid[col][row] = 0 #flag
                
    expandGrid.cell_height = ocGridMeta.resolution 
    expandGrid.cell_width = ocGridMeta.resolution
    expandGrid.header.frame_id = 'map'
    ExpandPub.publish(expandGrid)
    global position
    global startNode
    x = position.x
    y = position.y
    x = int(round((x - ocGridMeta.origin.position.x)/ocGridMeta.resolution))
    y = int(round((y - ocGridMeta.origin.position.y)/ocGridMeta.resolution))
    print "Got a start Position"
    startNode =  node(x, y,None,0,0)
    if goalNode != None:
        aStar(startNode, goalNode)
    print 'import done'

def newStartCallback(msg):
	global startpos
	global regen_map
	global start_pub
	global has_start
	global curmap
	global has_map
	if (has_map):
		#extract point from message
		point = msg.pose.pose.position
		print 'Got new starting position, regenerating path'
		#round point values to nearest integer
		startpos = point;
		print "x: ", startpos.x
		print "y: ", startpos.y
		#send rounded start point as a GridCells message to /lab3/astar/start
		start = GridCells()
		start.cell_width = curmap.info.resolution
		start.cell_height = curmap.info.resolution
		start.cells = [mapToWorldPos(curmap, worldToMapCell(curmap, startpos))]
		start.header.frame_id = 'map'
		start_pub.publish(start)
		#trigger A* path regeneration
		regen_map = 1
		#indicate that we have receive a start position
		has_start = 2

def rvizObstacles(w_map):
    global obstacles_pub

    obstacles_GC = GridCells()
    obstacles_GC.cell_width = w_map.info.resolution
    obstacles_GC.cell_height = w_map.info.resolution
    obstacles_GC.cells = []
    obstacle_pts = []
    expanded_pts = set([])
    e_o_i = set([])

    for index, node in enumerate(w_map.data):
        if node > 50:
            obstacle_pts.append(index)

    for index in obstacle_pts:
        for i in range(-4, 5):
            for j in range(-4, 5):
                point = i_to_p(index, w_map.info.width)
                point.x += i
                point.y += j
                e_o_i.add(p_to_i(point, w_map.info.width))
                expanded_pts.add(lab_4.gridToWorld(point, w_map))
    
    obstacles_GC.cells = list(expanded_pts)

    obstacles_GC.header.frame_id = 'map'
    obstacles_pub.publish(obstacles_GC)
    return list(e_o_i)

def newGoalCallback(msg):
	global endpos
	global regen_map
	global goal_pub	
	global has_goal	
	global curmap
	global has_map
	if (has_map):
		#extract point from message
		point = msg.pose.position
		print 'Got new gloal position, regenerating map'
		#round point values to nearest integer
		endpos = point; 
		print "x: ", endpos.x
		print "y: ", endpos.y	
		#send rounded goal point as a GridCells message to /lab3/astar/goal
		end = GridCells()
		end.cell_width = curmap.info.resolution
		end.cell_height = curmap.info.resolution
		end.cells = [mapToWorldPos(curmap, worldToMapCell(curmap, endpos))]
		end.header.frame_id = 'map'
		goal_pub.publish(end)
		#trigger A* path regeneration
		regen_map = 1
		#indicate that we have received a goal position
		has_goal = 1

def publishPointListAsGridCells(points, publisher):
    gridCells = GridCells()  # make an empty grid cells
    gridCells.cell_height = G_GridResolution
    gridCells.cell_width = G_GridResolution
    gridCells.header.frame_id = "map"
    gridCells.cells = points
    publisher.publish(gridCells)

def publishCells(grid):
    global wallpub
    print "publishing"
    k = 0
    cells = GridCells()
    cells.header.frame_id = "map"
    cells.cell_width = 0.3  # edit for grid size .3 for simple map
    cells.cell_height = 0.3  # edit for grid size

    for i in range(1, height):  # height should be set to hieght of grid
        for j in range(1, width):  # height should be set to hieght of grid
            # print k # used for debugging
            if grid[k] == 100:
                point = Point()
                point.x = j * 0.3 + 0.32  # edit for grid size
                point.y = i * 0.3 - 0.15  # edit for grid size
                point.z = 0
                cells.cells.append(point)
            k = k + 1
        k = k + 1
        if grid[k] == 100:
            point = Point()
            point.x = j * 0.3 + 0.62  # edit for grid size
            point.y = i * 0.3 - 0.15  # edit for grid size
            point.z = 0
            cells.cells.append(point)

    # print cells # used for debugging
    wallpub.publish(cells)

def publishGridCells(listOfPoints, topicNum):
        
    # create new grid cells object
    gridCells = GridCells()

    # set the list of points, frame, and cell size
    gridCells.cells =  listOfPoints
    gridCells.header.frame_id = "/map"

    gridCells.cell_width = mapRes
    gridCells.cell_height = mapRes

    # publish the grid to the correct topic
    if topicNum == 1:
        gridCellPub.publish(gridCells)
    elif topicNum == 2:
        exploredCellsPub.publish(gridCells)
    elif topicNum == 3:
        frontierCellsPub.publish(gridCells)
    elif topicNum == 4:
        wayPointsPub.publish(gridCells)
    elif topicNum == 5:
        pathPub.publish(gridCells)
    elif topicNum == 6:
        goalCellPub.publish(gridCells)

def publishCells(grid, num):

    global frontierPub
    global clearPub
    global wallPub
    global xOffset
    global yOffset
    #print "publishing"
    k=0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = 0.2 # edit for grid size .3 for simple map
    cells.cell_height = 0.2 # edit for grid size

    for square in grid: #height should be set to hieght of grid
            #print k # used for debugging
            point=Point()
            point.x=square.x*cells.cell_width+cells.cell_width+xOffset # edit for grid size
            point.y=square.y*cells.cell_height+cells.cell_width+yOffset # edit for grid size
            point.z=0
            cells.cells.append(point)

    #print cells # used for debugging
    if(num == 0):    
        frontierPub.publish(cells)
    elif(num == 1):    
        clearPub.publish(cells)
    elif(num == 2):    
        wallPub.publish(cells)

	def merge(self, other):
		overlap_counter = 0
		self.f_ids.sort()
		other.f_ids.sort()

		# merge the cells
		for new_cell in other.cells:
			if new_cell not in self.cells:
				self.add_cell(new_cell.x, new_cell.y)
			else:
				overlap_counter += 1

		# add the sizes
		self.size += other.size - overlap_counter

		# when in debug mode, merge the gridcells and publish
		if DEBUG:
			for gridcell in other.gridcells.cells:
				if gridcell not in self.gridcells.cells:
					self.gridcells.cells.append(gridcell)
			# copy over the publisher
			if self.f_ids[0] > other.f_ids[0]:
				dummy_cells = GridCells()
				dummy_cells.header.frame_id = 'map'
				dummy_cells.cell_width = resolution
				dummy_cells.cell_height = resolution
				self.publisher.publish(dummy_cells)
				self.publisher = other.publisher
				self.publish()

		# merge the frontier ids
		for f_id in other.f_ids:
			if f_id not in self.f_ids:
				self.f_ids.append(f_id)
				self.f_ids.sort()

def publishGridCells(listOfPoints, topicNum):
        
    # create new grid cells object
    gridCells = GridCells()

    # set the list of points, frame, and cell size
    gridCells.cells =  listOfPoints
    gridCells.header.frame_id = "/map"

    gridCells.cell_width = mapRes
    gridCells.cell_height = mapRes

    # publish the grid to the correct topic
    if topicNum == 7:
        frontierPub.publish(gridCells)
    else:
        time.sleep(0.5)
        if topicNum == 8:
            smallFrontPub0.publish(gridCells)
        elif topicNum == 9:
            smallFrontPub1.publish(gridCells)
        elif topicNum == 10:
            smallFrontPub2.publish(gridCells)
        elif topicNum == 11:
            smallFrontPub3.publish(gridCells)
        elif topicNum == 12:
            smallFrontPub4.publish(gridCells)

def publishClosedCellsReduce(map2D):
    global resolution
    global scale

    global reducedHeight
    global reducedWidth
    global x0
    global x1
    global y0
    global y1

    gridCells = GridCells()
    gridCells.header.frame_id = "/map"
    gridCells.header.stamp = rospy.Time.now()
    gridCells.cell_width = resolution*scale
    gridCells.cell_height = resolution*scale
    xyscale = 1.0/(resolution*scale)
    pointList = []


    for x in range(reducedWidth/scale):
        for y in range(reducedHeight/scale):
            if(map2D[y][x] > obsThresh):
                p = Point()
                p.x = float((x+x0/scale)/xyscale)+1/(2*xyscale) + originx
                p.y = float((y+y0/scale)/xyscale)+1/(2*xyscale) + originy
                p.z = 0
                pointList.append(p)
    gridCells.cells = pointList
    closedPub.publish(gridCells)

def publishGridCellList(lst,typ):
    global resolution
    global scale
    gridCells = GridCells()
    gridCells.header.frame_id = "/map"
    gridCells.header.stamp = rospy.Time.now()
    gridCells.cell_width = resolution*scale
    gridCells.cell_height = resolution*scale
    xyscale = 1.0/(resolution*scale)
    
    pntList=[]
    for pnt in lst:
        p = Point()
        # p.x= float(pnt.x/xyscale)+1/(2*xyscale)
        # p.y= float(pnt.y/xyscale)+1/(2*xyscale)
        p.x = float((pnt.x+x0/scale)/xyscale)+1/(2*xyscale) + originx
        p.y = float((pnt.y+y0/scale)/xyscale)+1/(2*xyscale) + originy
        p.z=0
        pntList.append(p)

    gridCells.cells=pntList
    if(typ==0):
        openPub.publish(gridCells)
    if(typ==1):
        closedPub.publish(gridCells)
    if(typ==2):
        pathVizPub.publish(gridCells)
    if(typ==3):
        astarVizPub.publish(gridCells)

def publishGrid (nodeList, rez, where):
    pub = rospy.Publisher(where, GridCells, queue_size=1)
    gridCell = GridCells()
    gridCell.cell_width = rez
    gridCell.cell_height = rez
    gridCell.header.frame_id = "map"
    gridCell.cells = nodeList
    pub.publish(gridCell)