;; "state-sort" takes a list of states and an estimator-value-function as
;; arguments.  It calls "aux-sort" on the new list of valued states of
;; the form ((value state) (value state) ... ).
;; NOTE: the value-fn must return a list of one binding.  The reason for
;; the mapcan rather than a more logical mapcar is so that the value-fn
;; can return nil when it examines an ilegal state.  This makes the
;; queue more efficient.

(defun state-sort (states value-fn)
  (aux-sort (remove nil (mapcar value-fn states))))

;; "aux-sort" is given a list of list of lists, the car of each list being
;; a number.  It outputs a new list sorted from least to greatest of
;; that number by going down the list keeping track of the smallest numbered
;; list it finds.  It then puts that list on the new list and removes it
;; from the old list and recurses.

(defun aux-sort (valued-lists)
  (cond ((null valued-lists) nil)
	(t (let ((least (car valued-lists)))
	     (mapc (function (lambda (list)
			       (cond ((lessp (car list) (car least))
				      (setq least list)))))
		   (cdr valued-lists))
	(cons least (aux-sort (remove least valued-lists)))))))
	   
	
;; "state-merge" takes two sorted lists of valued states as arguments
;; and outputs a merge of the two lists.  It checks the value of the car
;; of each list to see which is smaller.  It creates a new list who's
;; car is the smaller valued list and who's cdr is the merge of the
;; cdr of the list who's car was smallest with the other list.  Got that?
;; If one list is nil the other list is returned.

(defun state-merge (new-states queued-states)
  (let ((new-el (car new-states)) (queued-el (car queued-states)))
    (cond ((null new-states) queued-states)
	  ((null queued-states) new-states)
	  ((lessp (car new-el)  (car queued-el))
	   (cons new-el (state-merge (cdr new-states) queued-states)))
	  (t (cons queued-el (state-merge new-states (cdr queued-states)))))))