;; A simple beam 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 beam (start finish width &optional
                       (queue (list (list start))))
     (setf queue (butlast queue (max (- (length queue) width) 0)))
     (cond ((endp queue) nil)
	   ((equal finish (first (first queue)))
	       (reverse (first queue)))
	   (t (beam start finish width
                      (sort (apply #'append (mapcar #'extend 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))))