guile-algorithms/graphs/lib/algorithms/a-star.scm

(library (a-star)
  (export A*)
  (import (except (rnrs base)
                  let-values
                  map)
          (only (guile)
                lambda* λ
                when
                unless
                simple-format
                string<?
                call-with-output-string)
          (ice-9 pretty-print)
          (srfi srfi-1)  ; lists
          (srfi srfi-69)  ; hash tables
          (srfi srfi-11)  ; let-values
          ;; Functional Sets
          (pfds sets)
          ;; Priority Search Queues
          (pfds psqs)
          (priority-search-queues)
          ;; Bounded Balance Trees
          (pfds bbtrees)
          (graph-model))


  (define A*
    (lambda* (start
              target
              node-names
              nodes-table
              get-neighbor-distance
              cost-heuristic
              distance<)
      (let ([fringe (psq-set (make-psq string<? <)
                             ;; Initially put the start node in the open-set,
                             ;; since we need at least some node to go on from.
                             (node-name start)
                             ;; The start node has priority 0, which is the
                             ;; highest priority. It does not really matter,
                             ;; since there is only one element in the open-set.
                             0)]
            ;; routes stores the node preceding any target node on the cheapest
            ;; path to that target node. Initially only the preceding node for
            ;; the start node is set. The start node itself.
            [routes (alist->hash-table (list (cons (node-name start)
                                                   (node-name start)))
                                       string=?)]
            ;; score stores the cost of the cheapest path from the start node to
            ;; other nodes as currently known. Initially it is only set for the
            ;; start node, as one does not know other costs yet.
            [cheapest-path-costs
             (alist->hash-table `((,(node-name start) . 0)) string=?)]
            ;; Also keep track of a best estimate (calculated using the
            ;; heuristic) of the cost from the start node to the target node via
            ;; a node. Initially we have not explored any other nodes than the
            ;; start node, so the cost for any path via them to the target node
            ;; is pessimistically estimated to be infinite. Update formula:
            ;; via-node-cost-estimate(node) := current-best-score(node) + heuristic(node).
            [via-node-cost-estimate (make-hash-table string=?)])

        ;; Set cost estimate for start node.
        (for-each (λ (name)
                    (hash-table-set! via-node-cost-estimate
                                     name
                                     +inf.0))
                  node-names)

        (for-each (λ (name)
                    (unless (string=? name (node-name start))
                      (hash-table-set! cheapest-path-costs
                                       name
                                       +inf.0)))
                  node-names)

        (hash-table-set! via-node-cost-estimate
                         (node-name start)
                         (cost-heuristic start target))

        (simple-format #t "search begins\n")

        (let iter
            (;; The current-node-name° is the one, that is estimated
             ;; to have the lowest cost, when a path to the target
             ;; contains it. Initially this should be the start node,
             ;; because there is only one node in the fringe.
             [current-node-name° (psq-min fringe)]
             [fringe° fringe]
             [visited° (set-insert (make-set string<?) (node-name start))])

          (simple-format #t "fringe: ~a\n" (psq->list fringe°))
          (simple-format #t "current node: ~a\n" current-node-name°)

          (cond
           ;; If the current node is the target node, return the routes.
           [(string=? current-node-name° (node-name target))
            (simple-format
             #t "reached the target, returning routes:\n~a\n"
             (call-with-output-string
               (λ (port)
                 (pretty-print (hash-table->alist routes) port))))
            routes]
           [else
            (let ([neighbor-names
                   (node-neighbors (hash-table-ref nodes-table current-node-name°))])

              ;; Per neighbor node update the following: routes, cheapest path
              ;; cost, via node path cost estimate
              (for-each (λ (neighbor-name)
                          ;; (cond
                          ;;  ;; [(set-member? visited° neighbor-name)
                          ;;  ;;  (simple-format
                          ;;  ;;   #t "node ~a is already visited ignoring it as a neighbor\n"
                          ;;  ;;   neighbor-name)
                          ;;  ;;  (let-values ([(_node-name-of-min fringe-without-current) (psq-pop fringe°)])
                          ;;  ;;    (iter fringe-without-current visited°))]
                          ;;  [else ...])
                          (let* ([distance
                                  (get-neighbor-distance current-node-name° neighbor-name)]
                                 ;; At first the tentative score is the distance
                                 ;; from the start to the neighbor going via the
                                 ;; current node.
                                 [tentative-score
                                  (+ (hash-table-ref cheapest-path-costs
                                                     current-node-name°)
                                     distance)])

                            ;; If we have found a cheaper path to the neighbor
                            ;; ...
                            (when (< tentative-score
                                     (hash-table-ref cheapest-path-costs
                                                     neighbor-name))

                              ;; ... update the preceding node on the path to
                              ;; the neighbor in the routes map ...
                              (hash-table-set! routes
                                               neighbor-name
                                               current-node-name°)
                              ;; ... and update the cheapest path costs for the
                              ;; neighbor ...
                              (hash-table-set! cheapest-path-costs
                                               neighbor-name
                                               tentative-score)
                              ;; ... and update the estimates of the cost of a
                              ;; path from the start node through the neighbor
                              ;; to the target node.
                              (hash-table-set! via-node-cost-estimate
                                               neighbor-name
                                               (+ tentative-score
                                                  (cost-heuristic
                                                   (hash-table-ref nodes-table current-node-name°)
                                                   (hash-table-ref nodes-table neighbor-name)))))))
                        neighbor-names)
              (let ([updated-fringe
                     (fold (λ (node-name fringe-acc)
                             ;; Avoid adding visited nodes back to the
                             ;; fringe, to avoid going in circles.
                             (if (set-member? visited° node-name)
                                 fringe-acc
                                 (psq-set fringe-acc
                                          node-name
                                          (hash-table-ref cheapest-path-costs
                                                          node-name))))
                           ;; Remove the current node from the fringe.
                           ;; -- It is now visited and should not be
                           ;; visited again.
                           (psq-delete fringe° current-node-name°)
                           ;; For all neighbors update the
                           ;; fringe. The fringe is a priority search
                           ;; queue, which will have the minimum item
                           ;; readily available at the beginning of
                           ;; the next iteration.
                           neighbor-names)])
                (cond
                 [(psq-empty? updated-fringe)
                  ;; TODO: add proper exception type
                  (error 'A*
                         (call-with-output-string
                           (λ (port)
                             (simple-format port "no path found from start node to target node")))
                         (node-name start)
                         (node-name target))]
                 [else
                  (iter (psq-min updated-fringe)
                        updated-fringe
                        (set-insert visited° current-node-name°))])))]))))))