From cd2a616ccb73d1a9a3b42dbc98d86076f0bedda2 Mon Sep 17 00:00:00 2001
From: Dane Johnson <daneallenjohnson@protonmail.com>
Date: Mon, 11 Nov 2024 21:07:19 -0600
Subject: [PATCH] Implement the a* algorithm

---
 demo.scm     | 11 +++++++++--
 graphgif.scm | 42 ++++++++++++++++++++++++++++++------------
 2 files changed, 39 insertions(+), 14 deletions(-)

diff --git a/demo.scm b/demo.scm
index 7d2824a..abf0933 100644
--- a/demo.scm
+++ b/demo.scm
@@ -19,11 +19,18 @@
 		   (cdr pair))))
        graph))
 
-(define graph-generator
+(define djikstra-generator
   (make-generator
    (lambda (yield)
      (djikstra graph 90 9 (lambda (visited heap)
 			    (yield (color-graph graph visited heap))))
      #f)))
+(define a*-generator
+  (make-generator
+   (lambda (yield)
+     (a* graph 90 9 (lambda (visited heap)
+		      (yield (color-graph graph visited heap))))
+     #f)))
+(write-graphs-to-file "djikstra.webp" djikstra-generator)
+(write-graphs-to-file "astar.webp" a*-generator)
 
-(write-graphs-to-file "djikstra.webp" graph-generator)
diff --git a/graphgif.scm b/graphgif.scm
index f60dd0b..b5aad1b 100644
--- a/graphgif.scm
+++ b/graphgif.scm
@@ -4,6 +4,7 @@
 	     (d-)
 	     (srfi srfi-1)
 	     (srfi srfi-9 gnu)
+	     (srfi srfi-11)
 	     (srfi srfi-43))
 
 (re-export (cairo-pattern-create-rgb . create-color))
@@ -179,15 +180,6 @@
 ;; ALGORITHMS!!! ;;
 ;;;;;;;;;;;;;;;;;;;
 
-(define (lex< a b)
-  (cond
-   [(and (number? a) (number? b)) (< a b)]
-   [(and (null? a) (null? b)) #f]
-   [(and (pair? a) (null? b)) #f]
-   [(and (null? a) (pair? b)) #t]
-   [(= (car a) (car b)) (lex< (cdr a) (cdr b))]
-   [else (< (car a) (car b))]))
-
 ;; Heap algorithms
 (define-public (heap-insert priority item heap)
   (define (fix-up! i heap)
@@ -196,7 +188,7 @@
 	(let* ([parent (quotient (1- i) 2)]
 	       [i-priority (car (vector-ref heap i))]
 	       [parent-priority (car (vector-ref heap parent))])
-	  (when (lex< i-priority parent-priority)
+	  (when (< i-priority parent-priority)
 	    (vector-swap! heap i parent))
 	  (fix-up! parent heap))))
   (fix-up! (vector-length heap) (vector-append heap (vector (cons priority item)))))
@@ -211,8 +203,8 @@
 	   [right (1+ left)])
       (if (< left len)
 	  (let ([min (if (< right len)
-			 (argmin (compose car (partial vector-ref heap)) lex< i left right)
-			 (argmin (compose car (partial vector-ref heap)) lex< i left))])
+			 (argmin (compose car (partial vector-ref heap)) < i left right)
+			 (argmin (compose car (partial vector-ref heap)) < i left))])
 	    (if (= min i)
 		heap
 		(begin
@@ -246,6 +238,32 @@
 	    (cons idx visited))])))
 (export djikstra)
 
+(define* (a* graph source sink #:optional update)
+  "Uses the Chebychev distance as a heuristic"
+  (define (chebychev idx)
+    (let-values ([(x1 y1) (car+cdr (node-coords (assv-ref graph idx)))]
+		 [(x2 y2) (car+cdr (node-coords (assv-ref graph sink)))])
+      (max (abs (- x1 x2)) (abs (- y1 y2)))))
+  (define unvisited (list->vector (map (lambda (entry)
+					 (cons (inf) (car entry)))
+				       graph)))
+  (let iter ([heap (heap-insert (chebychev source) source unvisited)]
+	     [visited '()])
+    (define-values (dist idx) (car+cdr (heap-peek heap)))
+    (cond
+     [(eqv? sink idx) dist]
+     [(memv idx visited) (iter (heap-pop heap) visited)]
+     [(inf? dist) #f]
+     [else
+      (when update (update visited heap))
+      (iter (fold
+	     (lambda (edge heap)
+	       (heap-insert (+ dist (chebychev edge) 1) edge heap))
+	     (heap-pop heap)
+	     (node-edges (assv-ref graph idx)))
+	    (cons idx visited))])))
+(export a*)
+
 ;;;;;;;;;;;;
 ;; Output ;;
 ;;;;;;;;;;;;