(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 with contp = t while (and (> (hash-table-count unknown) 0) contp)
	 do (multiple-value-bind (u d) (shortest-unseen)
	      (remhash u unknown)
	      (if (or (>= d most-positive-fixnum) (equalp u to))
		  (setf contp nil)
		  (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))))