A* Algorithm  (* heuristic search *)
   (* Search a graph or state space, depending on the problem definition. *)
   (* S is the start node, T is the goal node. *)
   (* Open is an ordered list of nodes (ordered by lowest F value; see below),
      also called a priority queue. Closed is a set of nodes (order doesn't
      matter). In general, nodes that need to be searched are put on Open (at
      the proper position). As they are searched, they are removed from Open
      and put in Closed. Occasionally a newer, better route will be found to a
      node after it has already been searched, in which case we remove it from
      Closed and put it back on Open to be reconsidered. *)
   (* G[x] is the distance already traveled to get from S to node x, and is
      known exactly. H(x) is a function (heuristic) which returns an estimate
      of the distance from node x to T. F[x] is the estimated distance from S
      to T by going through node x, and is computed by F[x] = G[x] + H(x).
      H(x) can be calculated for any node, but F[x] and G[x] only become
      defined when node x is visited. *)
   (* Pred is defined for each node, and is a list of "came from" indications,
      so when we finally reach T, we traverse Pred to construct a path to
      S. *)
   (* Distance(x,y) is a function for calculating the distance between two
      neighboring nodes. *)
1  Open <- {S}  (* a list of one element *)
   Closed <- {}  (* the empty set *)
   G[S] <- 0,  F[S] <- 0,  Pred[S] <- NULL,  found <- FALSE
   WHILE Open <> {} and not found DO
5     x <- the first node on Open  (* node with smallest F value *)
      Open <- Open - {x}  (* remove x from Open *)
      Closed <- Closed + {x}  (* put x in Closed *)
      IF x = T THEN found <- TRUE  (* we're done *)
      ELSE  (* continue search through node x *)
10       let R be the set of neighboring nodes of x
         FOR each y in R DO
            IF y is not on Open or in Closed THEN
               G[y] <- G[x] + Distance(x,y)
               F[y] <- G[y] + H(y)  (* estimate solution path length *)
15             Pred[y] <- x  (* remember where we came from *)
               Open <- Open + {y}  (* put y on Open *)
            ELSE  (* y is on Open or in Closed *)
               IF (G[x] + Distance(x,y)) < G[y] THEN
                  (* we've found a better route to y *)
20                G[y] <- G[x] + Distance(x,y)
                  F[y] <- G[y] + H(y)
                  Pred[y] <- x  (* remember where we came from *)
                  IF y is on Open THEN
                     reposition y according to F[y]
25                ELSE  (* y is in Closed *)
                     Closed <- Closed - {y}  (* remove y from Closed *)
                     Open <- Open + {y}  (* put y on Open *)
   IF found THEN
      use Pred[T] to find Pred[Pred[T]] and so on until S is reached
30    (* this traces out the solution path in reverse *)
   ELSE T cannot be reached from S
