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

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


  ;; #;(define A*-pure
  ;;   (lambda* (start
  ;;             target
  ;;             nodes
  ;;             get-neighbors
  ;;             get-neighbor-distance
  ;;             cost-heuristic
  ;;             node<
  ;;             distance<)
  ;;     (let ([fringe (psq-set (make-psq node< <)
  ;;                            ;; Initially put the start node in the open-set,
  ;;                            ;; since we need at least some node to go on from.
  ;;                            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
  ;;            (bbtree-set (make-bbtree node<) start start)]
  ;;           ;; 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
  ;;            (bbtree-set (make-bbtree node<) start 0)]
  ;;           ;; 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-score-estimate(node) := current-best-score(node) + heuristic(node).
  ;;           [via-node-score-estimate
  ;;            ;; Set cost estimate for start node.
  ;;            (psq-set
  ;;             ;; Set initial estimates for all nodes.
  ;;             (fold (λ (node queue) (psq-set queue node +inf.0))
  ;;                   (make-psq node< <)
  ;;                   nodes)
  ;;             start
  ;;             (cost-heuristic start))])
  ;;       (let iter ([current-node°
  ;;                   ;; Initially the current-node° 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.
  ;;                   #;(psq-min via-node-score-estimate)
  ;;                   ;; Perhaps one can simply put start here.
  ;;                   start]
  ;;                  [fringe° fringe]
  ;;                  [routes° routes]
  ;;                  [cheapest-path-costs° cheapest-path-costs]
  ;;                  [via-node-score-estimate° via-node-score-estimate])
  ;;         (cond
  ;;          ;; If the current node is the target node, return the routes.
  ;;          [(eq? current-node° target) routes°]
  ;;          [else
  ;;           ;; TODO: do not forget to remove the current node from the fringe, since we visited it
  ;;           (let ([all-neighbors (get-neighbors current-node°)])
  ;;             ;; per neighbor node update the following:
  ;;             ;; routes, cheapest path cost, via node path cost estimate

  ;;             (iter (psq-min via-node-score-estimate)
  ;;                   ;; Add all neighbors to the fringe, if they are not yet in
  ;;                   ;; the fringe. Otherwise update their priority values.
  ;;                   (fold (λ (node queue) (psq-set queue node ... #|cheapest known path cost of node|#))
  ;;                         fringe°
  ;;                         all-neighbors)
  ;;                   #;(pretty-print (bbtree->alist (bbtree-set (bbtree-set
  ;;                   (bbtree-set (make-bbtree (lambda (k1 k2) (string<?
  ;;                   (symbol->string k1) (symbol->string k2)))) 'a 4) 'b 3)
  ;;                   'c 5)))
  ;;                   ;; --> ((a . 4) (b . 3) (c . 5))
  ;;                   )
  ;;             (let* ([distance (get-neighbor-distance current-node° neighbor)]
  ;;                    ;; 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°)
  ;;                        distance)])
  ;;               (cond
  ;;                ;; If we have found a cheaper path to the neighbor ...
  ;;                [(< tentative-score (hash-table-ref cheapest-path-costs° neighbor))
  ;;                 ;; if tentative_gScore < gScore[neighbor]
  ;;                 ;;     // This path to neighbor is better than any previous one. Record it!
  ;;                 ;;     cameFrom[neighbor] := current
  ;;                 ;;     gScore[neighbor] := tentative_gScore
  ;;                 ;;     fScore[neighbor] := tentative_gScore + h(neighbor)
  ;;                 ;;     if neighbor not in openSet
  ;;                 ;;         openSet.add(neighbor)

  ;;                 ;; ... update the preceding node on the path to the neighbor
  ;;                 ;; in the routes° map ...
  ;;                 (hash-table-set! routes° neighbor current-node°)
  ;;                 ;; ... and update the cheapest path costs for the neighbor
  ;;                 ;; ... (->1)
  ;;                 (hash-table-set! cheapest-path-costs° tentative-score)
  ;;                 ;; ... and update the estimates of the cost of a path from
  ;;                 ;; the start node through the neighbor to the target node.
  ;;                 #|TODO|#]
  ;;                [else ...])))
  ;;           ...])))))

  ;;     ;; // Open set is empty but goal was never reached
  ;;     ;; return failure
  )