def pathfind_to(self, target_location):
        print nx.astar_path(self.map, self.location, target_location)[1:]

        try:
            return nx.astar_path(self.map, self.location, target_location)[1:]
        except nx.exception.NetworkXNoPath:
            return None

def heuristic(isDropNode, package, state):
    driver = state.getVehicleList()
    #a backwards array of the capacity to award points
    points = [i for i in range(0, driver.getCapacity())]
    points.reverse()
    #drop off
    if isDropNode:
        star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeEndLocation()))
        # don't penalize for dropping off
        totalVal = star
    #pick up
    else:
        star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeStartLocation()))
        # the lower the difference between capacity and num packages you have, the greater the penalty
        totalVal = star + points[(driver.getCapacity() - len(driver.getPackageList()))]

    return totalVal

# def heuristic1(isDropNode, package, state):
#     driver = state.getVehicleList()
#     #drop off
#     if isDropNode:
#         star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeEndLocation()))
#
#     #pick up
#     else:
#         star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeStartLocation()))
#
#     return totalVal

    def LabelFeature(self, graph):
        # for each graph
        # pick a random source and a random target
        # run each of the networkx src tgt shortest path algorithms one by one
        # time how long they each take
        # repeat for N different srcs/tgts
        # find the average time for each algorithm
        # make the label for that graph the one with the shortest time
        # feature key: 0 = dijkstra, 1 = bidijkstra 2 = astar
        numIters = 10
        n = networkx.number_of_nodes(graph)
        dijkstraTimes = np.zeros(numIters)
        biDijkstraTimes = np.zeros(numIters)
        aStarTimes = np.zeros(numIters)
        for i in xrange(numIters):
            # pick a random source and target
            src = np.random.randint(0, n) + 1
            tgt = np.random.randint(0, n) + 1
            while tgt == src:
                tgt = np.random.randint(0, n) + 1

            dijkstraTime = time.time()
            try:
                networkx.dijkstra_path(graph, src, tgt)
            except:
                # no path found
                i -= 1
                continue

            dijkstraTime = time.time() - dijkstraTime
            dijkstraTimes[i] = dijkstraTime

            biDijkstraTime = time.time()
            networkx.bidirectional_dijkstra(graph, src, tgt)
            biDijkstraTime = time.time() - biDijkstraTime
            biDijkstraTimes[i] = biDijkstraTime

            aStarTime = time.time()
            networkx.astar_path(graph, src, tgt)
            aStarTime = time.time() - aStarTime
            aStarTimes[i] = aStarTime

        meanDijkstra = np.mean(dijkstraTimes)
        meanBiDijkstra = np.mean(biDijkstraTimes)
        meanAStar = np.mean(aStarTimes)

        minTime = min(meanDijkstra, meanBiDijkstra, meanAStar)
        if meanDijkstra == minTime:
            label = 0
        elif meanBiDijkstra == minTime:
            label = 1
        else:
            label = 2

        return label

    def findPathScattered(self,source,destination):
	    path = nx.astar_path(self.G,source,destination,self.heuristics1)
	    if len(path)< MAX_NODE_LOOKUP:
		#print "recompute the destination as one of the random 4 corners "
		destination = self.corners[random.randrange(0,4)]
		#print "destination", destination
	    	path = nx.astar_path(self.G,source,destination,self.heuristics1)
	    if(len(path)>1):
	        return path[1]
	    else: 
	        return path[0]

 def pickFarthestPackageAwayPlusDistanceToGarage(self, state, farthestReachablePackage):
     driverHomeLocation = state.getVehicleList().getHomeLocation()
     driverCurrLocation = state.getVehicleList().getCurrLocation()
     #print("Driver's Curr location: {0}" .format(driverCurrLocation))
     packageLocation = farthestReachablePackage.getNodeStartLocation()
     #print("Package's location: {0}" .format(packageLocation))
     driverToPackageDistance = len(nx.astar_path(Problem2.graph, driverCurrLocation, packageLocation))
     packageToHomeDistance = len(nx.astar_path(Problem2.graph, packageLocation, driverHomeLocation))
     # over lapping value so minus 1
     projectedDistace = driverToPackageDistance + packageToHomeDistance - 1
     return projectedDistace

    def plotPath(self, startLocation, endLocation):
        # TODO
        # Make this more efficient
        startX = startLocation.getX()
        startY = startLocation.getY()
        endX = endLocation.getX()
        endY = endLocation.getY()
        nodes = self.graph.nodes()
        closestStartNode = nodes[0]
        closestStartX = closestStartNode[0]
        closestStartY = closestStartNode[1]
        closestEndNode = nodes[0]
        closestEndX = closestEndNode[0]
        closestEndY = closestEndNode[1]
        closestStartDist = self.getDistance(startX, startY, closestStartX, closestStartY)
        closestEndDist = self.getDistance(endX, endY, closestEndX, closestEndY)
        for node in nodes:
            startDist = self.getDistance(node[0], node[1], closestStartX, closestStartY)
            if startDist < closestStartDist:
                closestStartNode = node
                closestStartDist = startDist
            endDist = self.getDistance(node[0], node[1], closestEndX, closestEndY)
            if endDist < closestEndDist:
                closestEndNode = node
                closestEndDist = endDist

        
        
        path = networkx.astar_path(self.graph, closestStartNode, closestEndNode, self.nodeDistance)

        # TODO: need to smooth out path
        # return a list of locations
        return [location.Location(x,y) for (x,y) in path]

def find_shortest_path(graph):
    """
    Take networkx graph and calculate the shortest path.

    args:
    graph - a networkx graph
    returns:
    List, that contains shortest path
    """
    G1 = graph
    G = create_graph2(G1.graph['labyrinth1'])

    list_of_nodes = nx.nodes(G)

    if len(list_of_nodes) == 1:
        return list_of_nodes

    for i in list_of_nodes:
        if i.count("A0") == 1:
            start_node = i
        if G.node[i]['lastnode'] == 1:
            end_node = i

    length = nx.astar_path(G, start_node, end_node)
    return length

def searchRouteFromNode(sumo_net, nx_net, origin_node_id, dest_node_id):
    def distFromEdge(edge_A_id, edge_B_id):
        s = sumo_net.getEdge(edge_A_id).getToNode().getCoord()
        t = sumo_net.getEdge(edge_B_id).getToNode().getCoord()
        return ((s[0] - t[0]) ** 2 + (s[1] - t[1]) ** 2) ** 0.5

    new_route = []
    min_travel_time = float('inf')
    origin_node = sumo_net.getNode(origin_node_id)
    dest_node = sumo_net.getNode(dest_node_id)

    for origin_edge in origin_node.getOutgoing():
        for dest_edge in dest_node.getIncoming():
            origin_edge_id = origin_edge.getID().encode('utf-8')
            dest_edge_id = dest_edge.getID().encode('utf-8')
            if origin_edge_id == "-" + dest_edge_id or "-" + origin_edge_id == dest_edge_id:
                continue
            if not origin_edge_id in nx_net.nodes() or not dest_edge_id in nx_net.nodes():
                continue

            try:
                candidate = nx.astar_path(nx_net, origin_edge_id, dest_edge_id, distFromEdge)
                sum_travel_time = sum([freeFlowTravelTime(sumo_net, edge_id) for edge_id in candidate])
                if min_travel_time > sum_travel_time:
                    min_travel_time = sum_travel_time
                    new_route = candidate
            except nx.NetworkXNoPath:
                continue
    return new_route

def routepath(G,lat1,lon1,lat2,lon2,tolerance=0.02,factor=1000):
    #print "G",G
    startnode = getnode(G,lat1,lon1)[0]
    # print "startnode:",startnode
    endnode = getnode(G,lat2,lon2)[0]
    # print "endnode:",endnode

###
#    for n in G:
#        print n
#    print "path nodes:"
    start_time=time.time()
    pathnodes =networkx.astar_path(G, startnode,endnode)
    duration=time.time()-start_time
    print "pathing time:",duration
    #print "pathnodes",pathnodes
    path = zip([G.node[n]['data'].lat for n in pathnodes], [G.node[n]['data'].lon for n in pathnodes])
    # print path

    # fig, ax = plt.subplots()
    # ax.plot([G.node[n]['data'].lat for n in pathnodes], [G.node[n]['data'].lon for n in pathnodes], '-')
    # ax.scatter([G.node[n]['data'].lat for n in G], [G.node[n]['data'].lon for n in G], s=50)
    # ax.set_aspect('equal')
    # plt.show()
    #print "path",path
    polyline=PolylineCodec().encode(path)
    # print polyline
    return  polyline#ordered list of nodes to be visited in order, forming a path

def test_weights_planning():
    plot_map()

    start_pos = [ 2650, 2650 ]

    L, c = grid_graph(start_pos, dim=10, width=1000)

    filename = os.path.join(root, 'flash', 'fft2', 'processed', 'map.png')

    img_data = imread(filename)

    custom_labels = add_weights(L, img_data)

    astar_path = nx.astar_path(L, (5, 5), (0, 4))

    H = L.subgraph(astar_path)

    h_pos = nx.get_node_attributes(H, 'pos')

    pos = nx.get_node_attributes(L,'pos')
    nx.draw(L, pos, node_size=5)

    edge_weight=dict([((u,v,),int(d['weight'])) for u,v,d in L.edges(data=True)])

    nx.draw_networkx_edge_labels(L,pos,edge_labels=edge_weight)
    nx.draw_networkx_nodes(L,pos, node_size=0)
    nx.draw_networkx_edges(L,pos)
    nx.draw_networkx_labels(L,pos, labels=custom_labels)

    nx.draw(H,h_pos, node_size=5, edge_color='r')


    plt.show()

    def test_unorderable_nodes(self):
        """Tests that A* accomodates nodes that are not orderable.

        For more information, see issue #554.

        """
        # TODO In Python 3, instances of the `object` class are
        # unorderable by default, so we wouldn't need to define our own
        # class here, we could just instantiate an instance of the
        # `object` class. However, we still support Python 2; when
        # support for Python 2 is dropped, this test can be simplified
        # by replacing `Unorderable()` by `object()`.
        class Unorderable(object):

            def __le__(self):
                raise NotImplemented

            def __ge__(self):
                raise NotImplemented

        # Create the cycle graph on four nodes, with nodes represented
        # as (unorderable) Python objects.
        nodes = [Unorderable() for n in range(4)]
        G = nx.Graph()
        G.add_edges_from(pairwise(nodes, cyclic=True))
        path = nx.astar_path(G, nodes[0], nodes[2])
        assert_equal(len(path), 3)

def artist_path2(start_aid, end_aid):
    start = time.time()
    aids = nx.astar_path(G, start_aid, end_aid)
    end = time.time()
    path = get_path(aids)
    print len(path['links']), 'path, calculated in', end - start, 'secs'
    return path

 def test_astar_undirected2(self):
     XG3 = nx.Graph()
     edges = [(0, 1, 2), (1, 2, 12), (2, 3, 1), (3, 4, 5), (4, 5, 1),
              (5, 0, 10)]
     XG3.add_weighted_edges_from(edges)
     assert_equal(nx.astar_path(XG3, 0, 3), [0, 1, 2, 3])
     assert_equal(nx.astar_path_length(XG3, 0, 3), 15)

 def test_astar_undirected3(self):
     XG4 = nx.Graph()
     edges = [(0, 1, 2), (1, 2, 2), (2, 3, 1), (3, 4, 1), (4, 5, 1),
              (5, 6, 1), (6, 7, 1), (7, 0, 1)]
     XG4.add_weighted_edges_from(edges)
     assert_equal(nx.astar_path(XG4, 0, 2), [0, 1, 2])
     assert_equal(nx.astar_path_length(XG4, 0, 2), 4)

def search_match(graph, seekingList, offeringList, seeking_product, offering_product):
    seekingIdxOccurrence = find_index_matches_in_seeking(seeking_product, seekingList)
    if len(seekingIdxOccurrence) == 0:
        return "noSeek"

    offeringIdxOccurrence = find_index_matches_in_offering(offering_product, offeringList)
    if len(offeringIdxOccurrence) == 0:
        return "noHave"

    paths = []
    for idx_offer in offeringIdxOccurrence:
        for idx_seek in seekingIdxOccurrence:
            try:
                paths.append(nx.astar_path(graph, offeringIdxOccurrence[0],
                                           seekingIdxOccurrence[0]))
            except nx.NetworkXNoPath:
                pass
    items_list = []
    url_list = []
    if len(paths) != 0:
        min(paths)
        for node in min(paths):
            items_list.append(graph.node[node]["Offering"])
            url_list.append(graph.node[node]["URL"])
        return {"items": items_list, "link": url_list}
    else:
        return "noPath"

    def gen_tab(self):
        """
        Generate array of [string, fret]


        """
        self.graph = self._gen_graph()

        # run the A* algorithm
        self.path = nx.astar_path(self.graph, 1, self.graph.number_of_nodes())
        # remove start and end nodes
        del self.path[0], self.path[-1]

        strums = []
        for n in self.path:
            n = self.graph.node[n]
            guitar_event = n['guitar_event']
            score_event = n['score_event']

            plucks = []
            if isinstance(guitar_event, Pluck):
                plucks.append((score_event.pname, score_event.oct, guitar_event))
            else:
                for pluck, note in zip(guitar_event.plucks, score_event.notes):
                    plucks.append((note.id, pluck))
            strums.append(plucks)

        fingering = np.empty([0,2], dtype=int)
        for s in strums:            
            for ss in s: 
                fingering = np.append(fingering,[[ss[2].string+1, ss[2].fret+1]], axis=0)

        return fingering

def plot_paths(graph, start_pts_list, exit_pt, ows, settings):
    """
    Provides an option to do pathfinding and plot results. Primarily useful for debugging,
    """

    # This should by definition be impossible, since the floor @ exit exists
    assert exit_pt in graph, "Specified exit point not in walkable path"

    plt.imshow(ows, cmap="gray_r")
    ax = plt.gca()
    ax.autoscale(False)

    for p_start in start_pts_list:
        msg = "Specified starting point is not walkable; ignore soldier @ {} "
        assert p_start in graph, msg.format(p_start)

        path_to_exit = nx.astar_path(graph,
                                     source=p_start,
                                     target=exit_pt,
                                     weight='weight',
                                     heuristic=a_star_heuristic)
        # Don't plot the final exit point; that's for internal bookkeeping. We care more about
        # what edge the soldier stepped on right before exiting.
        coord_lists = zip(*path_to_exit)
        
        ax.plot(coord_lists[1], coord_lists[0], linewidth=3, alpha=0.3)
        ax.plot(exit_pt[1], exit_pt[0], marker="^", color='r')

    save_folder = os.path.join("results", settings["run_id"], str(settings["voxel_size"]))
    plt.savefig(os.path.join(save_folder, "troop_exit_paths.png"))

    def getAnticipatoryStigmergyRoute(self, veh_id):
        route = traci.vehicle.getRoute(veh_id)
        reroute = []

        if traci.vehicle.getRoadID(veh_id) != "":
            current_road_id = traci.vehicle.getRoadID(veh_id)
            origin_node_id = self.sumo_net.getEdge(current_road_id).getToNode().getID().encode('utf-8')
        else:
            current_road_id = self.sumo_net.getEdge(route[0]).getID().encode('utf-8')
            origin_node_id = self.sumo_net.getEdge(current_road_id).getFromNode().getID().encode('utf-8')

        dest_node_id = self.sumo_net.getEdge(route[-1]).getToNode().getID().encode('utf-8')

        if self.conf.dce == True:
            if self.dce_stigmergy[origin_node_id]['anticipatory'] is None:
                network = self.calcDCE(origin_node_id, self.anticipatory_stigmergy_network)
                self.dce_stigmergy[origin_node_id]['anticipatory'] = network
            else:
                network = self.dce_stigmergy[origin_node_id]['anticipatory']
        else:
            network = self.anticipatory_stigmergy_network

        if self.conf.real_net == True:
            new_route = netutil.searchRouteFromNode(self.sumo_net, network, origin_node_id, dest_node_id)
        else:
            new_nodes = nx.astar_path(network, origin_node_id, dest_node_id, self.distFromNode)
            new_route = netutil.nodes2Route(self.original_network, new_nodes)

        reroute = route[:route.index(current_road_id)] + new_route
        return route, reroute

 def test_astar_w1(self):
     G = nx.DiGraph()
     G.add_edges_from([('s', 'u'), ('s', 'x'), ('u', 'v'), ('u', 'x'),
                       ('v', 'y'), ('x', 'u'), ('x', 'w'), ('w', 'v'),
                       ('x', 'y'), ('y', 's'), ('y', 'v')])
     assert_equal(nx.astar_path(G, 's', 'v'), ['s', 'u', 'v'])
     assert_equal(nx.astar_path_length(G, 's', 'v'), 2)

def plan_path(start_pos, goal_pos):
    """ Actual path planneer that integrates local/global graphs and finds path
    """

    #for now, just hard code this
    filename = os.path.join(root, 'flash', 'fft2', 'processed', 'map.png')
    img_data = imread(filename)

    #make local unconstrained motion graph
    #create unconstrained local graph at the start
    start_local_graph, start_center = grid_graph(start_pos, 
                                                 dim=LOCAL_GRAPH_DIM,
                                                 width=LOCAL_GRAPH_WIDTH)

    add_weights(start_local_graph, img_data)

    #create unconstrained local graph at the goal
    goal_local_graph, goal_center = grid_graph(goal_pos, 
                                               dim=LOCAL_GRAPH_DIM, 
                                               width=LOCAL_GRAPH_WIDTH)

    add_weights(goal_local_graph, img_data)

    #make global graph based on waypoints
    filename = os.path.join(root, 'flash', 'fft2', 
                            'export', 'binaryData', '910.bin')

    global_graph = graph_from_waypoints(filename)

    #make kd-tree from the global graph
    pos = nx.get_node_attributes(global_graph, 'pos')

    #sorted by keys
    d_x = OrderedDict(sorted(pos.items(), key=lambda t: t[0])).values()

    c_x = numpy.array(d_x)

    global_tree = scipy.spatial.cKDTree(c_x)

    #stitch together unconstrained local with global 
    u_graph = stitch(start_local_graph, global_graph, global_tree, 100, start_center, 'S-')

    u_graph = stitch(goal_local_graph, u_graph, global_tree, 100, goal_center, 'G-')

    astar_path = nx.astar_path(u_graph, 'S-' + str(start_center), 'G-' + str(goal_center))


    #rename node labels from '0' to final node, i.e. '35'
    count = 0
    mapping = {}
    for node in astar_path:
        mapping[node] = count
        count += 1

    planned_path = u_graph.subgraph(astar_path)

    planned_path = nx.relabel_nodes(planned_path, mapping)

    return planned_path