code: def heuristic_search(graph, start, goal, heuristic):
      
      
      # 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 )
docs: 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