From 94c136c29c58ac8e8898ec25a8976ffa36e87ee3 Mon Sep 17 00:00:00 2001
From: y5nw <y5nw@localhost>
Date: Mon, 23 Aug 2021 14:50:20 +0200
Subject: [PATCH] Implement basic pathfinder

---
 dataserver.lisp      | 12 ++++++++++++
 helpers.lisp         |  3 +++
 tracks/conns.lisp    |  5 +++++
 tracks/database.lisp | 12 ++++++++++++
 tracks/package.lisp  |  4 +++-
 tracks/path.lisp     | 30 ++++++++++++++++++++++++++++++
 6 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/dataserver.lisp b/dataserver.lisp
index 63b2ece..b9b5311 100644
--- a/dataserver.lisp
+++ b/dataserver.lisp
@@ -113,6 +113,18 @@
    graph-tracks ("/graph/paths" :method :get) ()
    ("Return a simple graph of paths")
    (tracks:gvdump *trackdb*))
+  (ywsw:safe-text-route
+   find-path ("/find-path/:from-pos/:from-side/:to-pos/:to-side" :method :get)
+   (&path (from-pos 'string) (from-side 'integer)
+	  (to-pos 'string) (to-side 'integer))
+   ("Find path between two entries in the track database")
+   (multiple-value-bind (path dist)
+       (tracks:dijkstra *trackdb*
+			(tracks:trackside (aux:string-to-v3d from-pos) from-side)
+			(tracks:trackside (aux:string-to-v3d to-pos) to-side))
+     (if path (format nil "Distance: ~a~%~{~#[~;To~:;From~] ~a~@{~%~#[~;To~:;Via~] ~a~}~}"
+		      dist path)
+	 "No path")))
   (ywsw:defsafe
       docroute-master ("/doc" :method :get) ()
       (let ((entries (loop for i being the hash-keys of easy-routes::*routes*
diff --git a/helpers.lisp b/helpers.lisp
index 2aef819..e9002c4 100644
--- a/helpers.lisp
+++ b/helpers.lisp
@@ -27,6 +27,9 @@
   (y 0 :type lua-number)
   (z 0 :type lua-number))
 
+(defmacro v3d (x y z)
+  `(make-v3d :x ,x :y ,y :z ,z))
+
 (defmethod print-object ((obj v3d) stream)
   (with-accessors ((x v3d-x) (y v3d-y) (z v3d-z)) obj
     (print-unreadable-object (obj stream)
diff --git a/tracks/conns.lisp b/tracks/conns.lisp
index 083b101..29a78b5 100644
--- a/tracks/conns.lisp
+++ b/tracks/conns.lisp
@@ -4,6 +4,11 @@
   (:pos (error "no coordinates specified") :type aux:v3d)
   (:side (error "no side specified") :type (integer 0 15)))
 
+(defmacro trackside (pos side)
+  (alexandria:once-only
+   (pos side)
+   `(if (and ,pos ,side) (make-trackside :pos ,pos :side ,side))))
+
 (defmethod print-object ((obj trackside) s)
   (with-accessors ((p trackside-pos) (side trackside-side)) obj
     (print-unreadable-object (obj s)
diff --git a/tracks/database.lisp b/tracks/database.lisp
index ba3bbb3..d07a762 100644
--- a/tracks/database.lisp
+++ b/tracks/database.lisp
@@ -26,6 +26,18 @@
 	  (let ((,track (get-track ,trackdb ,pos)))
 	    (when ,track ,@body)))))))
 
+(defmacro direct-next (trackdb trackside &optional no-eol-p)
+  (alexandria:with-gensyms (dir track i c d connects)
+    (alexandria:once-only
+     (trackdb trackside no-eol-p)
+     `(with-track-at-ts (,trackdb ,trackside nil ,dir ,track)
+	(loop with ,connects = (track-connects ,track)
+	   for ,i in (cddr (aref ,connects ,dir))
+	   for ,c = (car (aref ,connects ,i))
+	   and ,d = (cadr (aref ,connects ,i))
+	   when (or (not ,no-eol-p) ,c)
+	   collect (cons ,c ,d))))))
+
 (defun optimize-track-database (tdb)
   (loop for pos in (loop for i being the hash-keys of tdb collect i)
      for track = (gethash pos tdb)
diff --git a/tracks/package.lisp b/tracks/package.lisp
index 91cc38b..99d4347 100644
--- a/tracks/package.lisp
+++ b/tracks/package.lisp
@@ -1,7 +1,9 @@
 (defpackage :tracks
   (:use :cl)
   (:export :init-tracks :import-data
-	   :dump-track-definitions :gvdump))
+	   :dump-track-definitions :gvdump
+	   :trackside
+	   :dijkstra))
 (in-package :tracks)
 
 (defparameter *hdiff*
diff --git a/tracks/path.lisp b/tracks/path.lisp
index e69de29..8f379a1 100644
--- a/tracks/path.lisp
+++ b/tracks/path.lisp
@@ -0,0 +1,30 @@
+(in-package :tracks)
+
+(defun dijkstra (tdb from to)
+  (let ((unknown (make-hash-table :test #'equalp))
+	(dist (make-hash-table :test #'equalp))
+	(prev (make-hash-table :test #'equalp)))
+    (loop for pos being the hash-keys of tdb using (hash-value track) do
+	 (loop for fdir from 0 to 15 for conns across (track-connects track) when conns do
+	      (let ((ts (trackside pos fdir)))
+		(setf (gethash ts unknown) t)
+		(setf (gethash ts dist) most-positive-fixnum)
+		(setf (gethash ts prev) nil))))
+    (setf (gethash to dist) most-positive-fixnum)
+    (setf (gethash from dist) 0)
+    (labels ((shortest-unseen ()
+	       (let ((trackside nil) (dst most-positive-fixnum))
+		 (loop for ts being the hash-keys of unknown
+		    for d = (gethash ts dist)
+		    when (<= d dst) do (psetf dst d trackside ts))
+		 (values trackside dst))))
+      (loop while (> (hash-table-count unknown) 0) do
+	   (multiple-value-bind (u d) (shortest-unseen)
+	     (remhash u unknown)
+	     (loop for (v . len) in (direct-next tdb u t) for alt = (+ d len)
+		when (< alt (gethash v dist)) do
+		  (setf (gethash v dist) alt)
+		  (setf (gethash v prev) u)))))
+    (let ((path nil) (distance (gethash to dist)))
+      (loop for i = to then (gethash i prev) while i do (push i path))
+      (values (if (equalp (car path) from) path nil) distance))))