;; A simple best-first search example adapted from Winston & Horn

(setf (get 's 'neighbors) '(a d)
      (get 'a 'neighbors) '(s b d)
      (get 'b 'neighbors) '(a c e)
      (get 'c 'neighbors) '(b)
      (get 'd 'neighbors) '(s a e)
      (get 'e 'neighbors) '(b d f)
      (get 'f 'neighbors) '(e))

(setf (get 's 'coordinates) '(0 3)
      (get 'a 'coordinates) '(4 6)
      (get 'b 'coordinates) '(7 6)
      (get 'c 'coordinates) '(11 6)
      (get 'd 'coordinates) '(3 0)
      (get 'e 'coordinates) '(6 0)
      (get 'f 'coordinates) '(11 3))

(defun straight-line-distance (node-1 node-2)
	(let ((coordinates-1 (get node-1 'coordinates))
              (coordinates-2 (get node-2 'coordinates)))
           (sqrt (+ (expt (- (first coordinates-1)
                             (first coordinates-2))
                           2)
                     (expt (- (second coordinates-1)
                              (second coordinates-2))
                           2)))))

(defun closerp (path-1 path-2 target-node)
	(< (straight-line-distance (first path-1) target-node)
           (straight-line-distance (first path-2) target-node)))

(defun best-first (start finish &optional
                       (queue (list (list start))))
     (cond ((endp queue) nil)
	   ((equal finish (first (first queue)))
	       (reverse (first queue)))
	   (t (best-first start finish
                      (sort (append (extend (first queue)) (rest queue))
                             #'(lambda (p1 p2) (closerp p1 p2 finish)))))))

(defun extend (path)
	(print (reverse path))
	(mapcar #'(lambda (new-node) (cons new-node path))
             (remove-if #'(lambda (neighbor) (member neighbor path))
                        (get (first path) 'neighbors))))