;;;;
;;;; Bidirectional search to find the shortest paths
;;;; between two nodes v,w in the graph G = (V,E).
;;;;
;;;; Dan Cross <net!gajendra!cross>
;;;;
(defun next-frontier (graph f visited thunk)
  (let ((nvs ()))
    (dolist (v f)
      (dolist (v1 (cdr (assoc v graph)))
	(unless (member v1 visited)
	  (pushnew v1 nvs)
	  (funcall thunk v1 v))))
    nvs))

(defun traverse (graph Q visited thunk)
  (unless (null Q)
    (let ((nq (next-frontier graph Q visited thunk)))
      (traverse
       graph
       nq
     (union visited nq)
     thunk))))

(defmacro add-edge (graph a b)
  `(if (null (assoc ,a ,graph))
       (push (cons ,a (cons ,b nil)) ,graph)
       (pushnew ,b (cdr (assoc ,a ,graph)))))

(defun reconstruct-paths (I p1 p2)
  (let ((R nil))
    (traverse p1 I I #'(lambda (a b) (add-edge R a b)))
    (traverse p2 I I #'(lambda (a b) (add-edge R b a)))
    R))

(defun bidirectional-search-rec (graph f1 v1 p1 f2 v2 p2)
  (let ((I (intersection f1 f2)))
    (if (and (null I) (consp f1) (consp f2))
	(if (< (length f1) (length f2))
	    (let ((nf1 (next-frontier graph f1 v1 #'(lambda (a b) (add-edge p1 a b)))))
	      (bidirectional-search-rec graph nf1 (union v1 nf1) p1 f2 v2 p2))
	    (let ((nf2 (next-frontier graph f2 v2 #'(lambda (a b) (add-edge p2 a b)))))
	      (bidirectional-search-rec graph f1 v1 p1 nf2 (union v2 nf2) p2)))
	(reconstruct-paths I p1 p2))))

(defun bidirectional-search (graph v w)
  (bidirectional-search-rec graph (list v) (list v) nil (list w) (list w) nil))

(defun visit-digraph-paths (graph start path thunk)
  (let ((adj (sort (copy-list (cdr (assoc start graph))) #'<)))
    (if (null adj)
	(funcall thunk (reverse path))
	(dolist (v adj)
	  (visit-digraph-paths graph v (cons v path) thunk)))))

(defun print-paths (graph start)
  (visit-digraph-paths graph start (list start)
		       #'(lambda (p) (format t "~A~%" p))))

(defun save-paths (graph start)
  (let ((ps nil))
    (visit-digraph-paths graph start (list start) #'(lambda (p) (push p ps)))
    (nreverse ps)))