(defvar my-graph-search-debug nil
  "Print debug information during graphs search.")

(defun my-graph-search (frontier neighbors goalp add &optional seen)
  "Search a directed graph.

FRONTIER is a list of paths to be searched, NEIGHBORS is a function that
takes a path and returns all adjacent vertices to its `car', GOALP is a
function that takes a path and returns non-nil if the path reached the
goal, and ADD is a function that takes a frontier and a list of paths
and adds the paths to the frontier.

SEEN is a set of visited vertices, used internally.

In all paths, the most recently seen vertex is the first element."
  (if (not frontier) nil
    (let ((path (car frontier))
          (seen (or seen
                    (let ((seen (make-hash-table :test #'equal)))
                      (mapc (lambda (vertex) (puthash vertex t seen))
                            (mapcan #'identity frontier))
                      seen))))
      (when my-graph-search-debug
        (princ (format "Search: %s\n" path)))
      (if (funcall goalp path) path
        (my-graph-search
         (funcall add
                  (cdr frontier)
                  (mapcan (lambda (neighbor)
                            (if (gethash neighbor seen) nil
                              (puthash neighbor t seen)
                              (list (copy-sequence (cons neighbor path)))))
                          (funcall neighbors path)))
         neighbors goalp add seen)))))