A* search algorithm.
    
    A set of heuristics is available under C{graph.algorithms.heuristics}. User-created heuristics
    are allowed too.
    
    @type graph: graph, digraph
    @param graph: Graph
    
    @type start: node
    @param start: Start node
    
    @type goal: node
    @param goal: Goal node
    
    @type heuristic: function
    @param heuristic: Heuristic function
    
    @rtype: list
    @return: Optimized path from start to goal node

def heuristic_search(graph, start, goal, heuristic):
    """
    A* search algorithm.
    
    A set of heuristics is available under C{graph.algorithms.heuristics}. User-created heuristics
    are allowed too.
    
    @type graph: graph, digraph
    @param graph: Graph
    
    @type start: node
    @param start: Start node
    
    @type goal: node
    @param goal: Goal node
    
    @type heuristic: function
    @param heuristic: Heuristic function
    
    @rtype: list
    @return: Optimized path from start to goal node 
    """
    
    # The queue stores priority, node, cost to reach, and parent.
    queue = [ (0, start, 0, None) ]

    # This dictionary maps queued nodes to distance of discovered paths
    # and the computed heuristics to goal. We avoid to compute the heuristics
    # more than once and to insert too many times the node in the queue.
    g = {}

    # This maps explored nodes to parent closest to the start
    explored = {}
    
    while queue:
        _, current, dist, parent = heappop(queue)
        
        if current == goal:
            path = [current] + [ n for n in _reconstruct_path( parent, explored ) ]
            path.reverse()
            return path

        if current in explored:
            continue

        explored[current] = parent

        for neighbor in graph[current]:
            if neighbor in explored:
                continue
            
            ncost = dist + graph.edge_weight((current, neighbor))

            if neighbor in g:
                qcost, h = g[neighbor]
                if qcost <= ncost:
                    continue
                # if ncost < qcost, a longer path to neighbor remains
                # g. Removing it would need to filter the whole
                # queue, it's better just to leave it there and ignore
                # it when we visit the node a second time.
            else:
                h = heuristic(neighbor, goal)
            
            g[neighbor] = ncost, h
            heappush(queue, (ncost + h, neighbor, ncost, current))

    raise NodeUnreachable( start, goal )