A* Search in Clojure.clj

(defstruct path :state :previous :cost-so-far :total-cost)

(defn path-to-string
  [path]
  (format "Path to %s, cost %s" (:state path) (:total-cost path)))

(defn make-path
  "Create a new path object"
  [state previous cost-so-far total-cost]
  (struct path state previous cost-so-far total-cost))

(defn find-path
  "Find the path with this state amongst a list of paths"
  [state paths state-eq]
  (let [x (filter (fn [path] (state-eq (:state path) state)) paths)]
    (when-not (empty? x)
      (first x))))

(defn better-path?
  "Is path1 cheaper than path2?"
  [path1 path2]
  (< (:total-cost path1) (:total-cost path2)))

;; TODO a bit inefficient!
(defn insert-path
  [path paths]
  "Put path in the right position, sorted by total cost."
  (sort better-path? (cons path paths)))

(defn path-states
  "Collect the states along this path."
  [path]
  (when-not (nil? path)
    (lazy-seq 
      (cons 
       (:state path) 
       (path-states (:previous path))))))

(defn setf [atom val]
  (swap! atom (constantly val)))
  
(defn a*-search
  "Find a path whose state satisfies goal?.  Start with paths, and expand
   successors, exploring least cost first.  When there are duplicate states,
   keep the one with the lower cost and discard the other."
  ([paths goal? successors cost-fn cost-left-fn]
     (a*-search paths goal? successors cost-fn cost-left-fn = #{}))
  ([paths goal? successors cost-fn cost-left-fn state-eq]
     (a*-search paths goal? successors cost-fn cost-left-fn state-eq #{}))
  ([paths goal? successors cost-fn cost-left-fn state-eq old-paths]
     (dbg :search ";; Search: %s" paths)
     (cond
       (empty? paths) nil
       (goal? (:state (first paths))) (first paths)
       :else (let [path (first paths)
		   rest-paths (rest paths)
		   old-paths-a (atom (insert-path path old-paths)) ;; mutable wrappers
		   paths-a (atom rest-paths)
		   state (:state path)]
	       (doseq [state2 (successors state)]
		 (let [cost (+ (:cost-so-far path)
			       (cost-fn state state2))
		       cost2 (cost-left-fn state2)
		       path2 (make-path state2 path cost (+ cost cost2))
		       old-a (atom nil)]
		   (cond
		     (not (empty? (setf old-a (find-path state2 @paths-a state-eq))))
  		           (when (better-path? path2 @old-a)
			     (setf paths-a (insert-path path2 (remove (partial = @old-a) @paths-a))))
		     (not (empty? (setf old-a (find-path state2 @old-paths-a  state-eq))))
		           (when (better-path? path2 @old-a)
			     (setf paths-a (insert-path path2 @paths-a))
			     (setf old-paths-a (remove (partial = @old-a) @old-paths-a)))
		     :else (setf paths-a (insert-path path2 @paths-a)))))
	       (recur @paths-a goal? successors cost-fn cost-left-fn state-eq @old-paths-a)))))