;;; **********************************************************************
;;; This code was written as part of the Spice Lisp project at
;;; Carnegie-Mellon University, and has been placed in the public domain.
;;; Spice Lisp is currently incomplete and under active development.
;;; If you want to use this code or any part of Spice Lisp, please contact
;;; Scott Fahlman (FAHLMAN@CMUC). 
;;; **********************************************************************
;;;
;;; Sort functions for Spice lisp
;;;   these functions are part of the standard spice lisp environment.
;;;
;;; Written by Jim Large
;;; Hacked on and maintained by Skef Wholey
;;;
;;; *******************************************************************

;;; Apply-key applies the key to elt, or if key is (), then returns elt.
(eval-when (compile)
(defmacro apply-key (key elt)
  `(if ,key
       (funcall ,key ,elt)
       ,elt))
)

;; apply-pred applies the predicate and the key functions to two arguments. if
;;   the key function is () then apply-pred calls only the predicate function.
(eval-when (compile)
(defmacro apply-pred (one two pred key)
  "Internal Macro"
  `(if ,key
       (funcall ,pred (funcall ,key ,one)
		      (funcall ,key  ,two))
       (funcall ,pred ,one ,two)))
)
;;; Sort

;;; sorts a sequence destructively using a predicate which must be a
;;;  of two arguments which returns non-() only if the first argument is
;;;  strictly less than the second.  The keyfun (if present) must be a
;;;  function of one argument.  The predicate is applied to keyfun of the
;;;  sequence elements, or directly to the elements if the keyfun is not
;;;  given.

;;; Sort dispatches to type specific sorting routines.

(defun sort (sequence predicate &rest keywords)
  "Destructively sorts sequence.  Predicate should returns non-Nil if
  Arg1 is to precede Arg2."
  (with-keywords keywords
		 ((:key keyfun ()))
    (if (slisp-array-p sequence)
	(sort-vector sequence predicate keyfun)
	(if (slisp-vector-p sequence)
	    (sort-simple-vector sequence predicate keyfun)
	    (if (listp sequence)
		(sort-list sequence predicate keyfun)
		(error "~S is not a sequence." sequence))))))
;;;    Sort-Simple-Vector

;;; Sort-simple-vector sorts vector using the Quicksort algorithm.  
;;;
;;; Subranges (from bottom through top inclusive) of vector are partitioned
;;;  by selecting the first element of the subrange as a pivot, and then
;;;  rearranging the elements of the subrange so that those less than the pivot
;;;  come before it, and the others come after.
;;;
;;; First, the whole vector is partitioned, then each of the two partitions is
;;;  partitioned.  When a parition is trivial (0 or 1 elt) we don't bother to 
;;;  do it.
;;;
;;; Pending partitions are remembered on a stack.  When the stack becomes 
;;;  empty, then the array has been sorted.

(defun sort-simple-vector (vector pred key)
  "This is an internal function.  Use SORT instead."

  (prog* ((stack (list (1- (length vector)) ;stack of pending top/bottom pairs
		       0))		    ; initial pair for whole vector.
	  pivot			     ;The pivot element for a partition pass.
	  pivkey		     ;The extracted key for pivot.
	  top bottom		     ;The range being partitioned (inclusive).
	  TT BB)		     ;Working indices.

   START-PARTITION
    (setq TT (setq top (pop stack)))
    (setq BB (setq bottom (pop stack)))
    (setq pivkey (apply-key key (setq pivot (%sp-saref1 vector bottom))))
    

   DOWN
    (when (= BB TT) (go END-PARTITION))
    (let ((top-elt (%sp-saref1 vector TT)))
      (cond ((funcall pred (apply-key key top-elt) pivkey)
	     (setf (%sp-saref1 vector BB) top-elt)
	     (setq BB (1+ BB))
	     (go UP))
	    (T (setq TT (1- TT))
	       (go DOWN))))
    

    UP
    (when (= BB TT) (go END-PARTITION))
    (let ((bot-elt (%sp-saref1 vector BB)))
      (cond ((funcall pred pivkey (apply-key key bot-elt))
	     (setf (%sp-saref1 vector TT) bot-elt)
	     (setq TT (1- TT))
	     (go DOWN))
	    (T (setq BB (1+ BB))
	       (go UP))))


   END-PARTITION
    (setf (%sp-saref1 vector BB) pivot)
    (when (< bottom (1- BB))
      (push bottom stack)
      (push (1- BB) stack))
    (when (> top (1+ TT))
      (push (1+ TT) stack)
      (push top stack))
    (when (null stack) (return vector))
    (go START-PARTITION)
    ))
;;;    Sort-Vector

;;; Sort-Vector is the same as sort-simple-vector except that vector is 
;;;  a "complex" array instead of a "simple" array.


(defun sort-vector (vector pred key)
  "This is an internal function.  Use SORT instead."

  (prog* ((stack (list (1- (length vector)) ;stack of pending top/bottom pairs
		       0))		    ; initial pair for whole vector.
	  pivot			     ;The pivot element for a partition pass.
	  pivkey		     ;The extracted key for pivot.
	  top bottom		     ;The range being partitioned (inclusive).
	  TT BB)		     ;Working indices.

   START-PARTITION
    (setq TT (setq top (pop stack)))
    (setq BB (setq bottom (pop stack)))
    (setq pivkey (apply-key key (setq pivot (%sp-caref1 vector bottom))))
    

   DOWN
    (when (= BB TT) (go END-PARTITION))
    (let ((top-elt (%sp-caref1 vector TT)))
      (cond ((funcall pred (apply-key key top-elt) pivkey)
	     (setf (%sp-caref1 vector BB) top-elt)
	     (setq BB (1+ BB))
	     (go UP))
	    (T (setq TT (1- TT))
	       (go DOWN))))
    

    UP
    (when (= BB TT) (go END-PARTITION))
    (let ((bot-elt (%sp-caref1 vector BB)))
      (cond ((funcall pred pivkey (apply-key key bot-elt))
	     (setf (%sp-caref1 vector TT) bot-elt)
	     (setq TT (1- TT))
	     (go DOWN))
	    (T (setq BB (1+ BB))
	       (go UP))))


   END-PARTITION
    (setf (%sp-caref1 vector BB) pivot)
    (when (< bottom (1- BB))
      (push bottom stack)
      (push (1- BB) stack))
    (when (> top (1+ TT))
      (push (1+ TT) stack)
      (push top stack))
    (when (null stack) (return vector))
    (go START-PARTITION)
    ))
;;;    Sort-List

;;; Sort-prefix could be recursively defined as follows.

;(defun sort-prefix (height)
;  (cond ((null list) ())
;	 ((< height 1) (rplacd (prog1 list (setq list (cdr list))) nil))
;	 (T (merge-lists* (sort-prefix (1- n)) (sort-prefix (1- n)) pred key))))

;;; The slightly more complicated version which follows eliminates the function
;;;  call overhead, and eliminates the need to make LIST a special variable


(eval-when (compile)
  (defmacro sort-prefix ()
    '(prog ((stack ())
	    (res ()))

      CALL
      (when (null list)
	(setq res ())
	(go RETURN))
      (when (< height 1)
	(setq res (rplacd (prog1 list (setq list (cdr list))) nil))
	(go RETURN))

      (push height stack)
      (setq height (1- height))
      (push 's1 stack)
      (go CALL)

      S1
      (setq height (1- (car stack)))
      (push res stack)
      (push 's2 stack)
      (go CALL)

      S2
      (setq res (merge-lists* (pop stack) res pred key))
      (setq height (pop stack))
      (go RETURN)

      RETURN
      (let ((flag (pop stack)))
	(case flag
	  (s1 (go S1))
	  (s2 (go S2))
	  (T (return res))))
       )
    ))



;;; Sort-List returns a list containing the elements of LIST in sort.  The 
;;;  original list is destroyed.  Based on an algorithm described as:
;;;  `a "traditional" list merge sort' by Guy Steele in AI memo 587 (aug '80).
;;;
;;; This sort is stable.

(defun sort-list (list pred key)
  (do ((height 0 (1+ height))
       (result () (merge-lists* result (sort-prefix) pred key)))
      ((null list) result)))

;;; Stable-Sort

;;; Stable sort is the same as sort, but it guarantees that equal elements will
;;;  not change places.
;;;
;;; For lists, use the normal sort-list function, but vectors must use a less
;;;  efficient algorithm. 

(defun stable-sort (sequence predicate &rest keywords)
  "Destructively sorts Sequence.  Predicate should return non-Nil if
  Arg1 is to precede Arg2."
  (with-keywords keywords
		 ((:key keyfun ()))
    (if (slisp-array-p sequence)
	(stable-sort-simple-vector sequence predicate keyfun)
	(if (slisp-vector-p sequence)
	    (stable-sort-vector sequence predicate keyfun)
	    (if (listp sequence)
		(sort-list sequence predicate keyfun)
		(error "~S is not a sequence."))))))
;;;    Stable-Sort-Simple-Vector

;;; Stable sorting arrays is hard.  Knuth seems to think that finding an 
;;;  algorithm which can stably sort a vector in  n log n  time without using
;;;  gobs of extra storage is a 47 point problem.  
;;;
;;; We handle the problem by coercing the vector into a list, sorting that, 
;;;  and then copying the list back into the original vector.

(defun stable-sort-simple-vector (vector pred key)
  "This is an internal function.  Use STABLE-SORT instead."

  (let* ((header (cons 'header ()))
	 (length (length vector))
	 (list (do ((I 0 (1+ I))
		    (tail header (cdr (rplacd tail (cons (%sp-saref1 vector I)
							 ())))))
		   ((= I length) (cdr header)))))
    (do ((sorted-list (sort-list list pred key) (cdr sorted-list))
	 (I 0 (1+ I)))
	((null sorted-list) vector)
      (setf (%sp-saref1 vector I) (car sorted-list)))))
;;;    Stable-Sort-Vector

;;; Stable-sort-vector is the same as stable-sort-simple-vector except that 
;;;  the vector is a slisp array instead of a slisp vector.

(defun stable-sort-vector (vector pred key)
  "This is an internal function.  Use STABLE-SORT instead."

  (let* ((header (cons 'header ()))
	 (length (length vector))
	 (list (do ((I 0 (1+ I))
		    (tail header (cdr (rplacd tail (cons (%sp-saref1 vector I)
							 ())))))
		   ((= I length) (cdr header)))))
    (do ((sorted-list (sort-list list pred key) (cdr sorted-list))
	 (I 0 (1+ I)))
	((null sorted-list) vector)
      (setf (%sp-saref1 vector I) (car sorted-list)))))

;;; Merge:

(defun merge (result-type sequence1 sequence2 predicate &rest keywords)
  "The sequences Sequence1 and Sequence2 are destructively merged into
  a sequence of type Result-Type using the Predicate to order the elements."
  (with-keywords keywords
      ((:key key #'identity))
    (case (type-specifier result-type)
      (list
       (typecase sequence1
	 (list (typecase sequence2
		 (list (merge-lists* sequence1 sequence2 predicate key))
		 (array (merge-lists* sequence1 (vector-to-list* sequence2)
				      predicate key))
		 (t (error "~S is not a sequence." sequence2))))
	 (array (typecase sequence2
		  (list (merge-lists* (vector-to-list* sequence1) sequence2
				      predicate key))
		  (array (merge-lists* (vector-to-list* sequence1)
				       (vector-to-list* sequence2)
				       predicate key))
		  (t (error "~S is not a sequence." sequence2))))
	 (t (error "~S is not a sequence." sequence1))))
      ((vector array string)
       (typecase sequence1
	 (list (typecase sequence2
		 (list (merge-vectors* (list-to-vector* sequence1 'vector)
				       (list-to-vector* sequence2 'vector)
				       predicate key))
		 (array (merge-vectors* (list-to-vector* sequence1 'vector)
					sequence2 predicate key))
		 (t (error "~S is not a sequence." sequence2))))
	 (array (typecase sequence2
		  (list (merge-vectors* sequence1
					(list-to-vector* sequence2 'vector)
					predicate key))
		  (array (merge-vectors* sequence1 sequence2
					 predicate key))
		  (t (error "~S is not a sequence." sequence2))))
	 (t (error "~S is not a sequence." sequence1))))
      (t (error "~S is not a subtype of SEQUENCE." result-type)))))
;;;    Merge-Lists*

;;; Merge-Lists* destructively merges list-1 with list-2.  In the resulting
;;;  list, elements of list-2 are guaranteed to come after equal elements
;;;  of list-1.

(defun merge-lists* (list-1 list-2 pred key)
  (do* ((result (list 'header))
	(P result))                   ; P = pointer to last cell of result

       ((or (null list-1) (null list-2))       ; done when either list used up	
	(if (null list-1)                      ; in which case, append the
	    (rplacd p list-2)                  ;   other list
	    (rplacd p list-1))
	(cdr result))                          ; return the result sans header

    (cond ((apply-pred (car list-2) (car list-1) pred key)
	   (rplacd p list-2)           ; append the lesser list to last cell of
	   (setq p (cdr p))            ;   result.  Note: test must bo done for
	   (pop list-2))               ;   list-2 < list-1 so merge will be
	  (T (rplacd p list-1)         ;   stable for list-1
	     (setq p (cdr p))
	     (pop list-1)))))

;;;    Merge-Vectors*

;;; Merge-Vectors* dispatches to either Merge-Simple-Vectors or
;;;  Merge-Non-Simple-Vectors.

(defun merge-vectors* (vector1 vector2 pred key)
  (if (or (slisp-array-p vector1) (slisp-array-p vector2))
      (merge-non-simple-vectors vector1 vector2 pred key)
      (merge-simple-vectors vector1 vector2 pred key)))

;;;    Merge-Simple-Vectors

;;; Merge-simple-vectors returns a new vector which contains an interleaving
;;;  of the elements of vector-1 and vector-2.  Elements from vector-2 are
;;;  chosen only if they are strictly less than elements of vector-1,
;;;  (pred elt-2 elt-1), as specified in the manual.

(defun merge-simple-vectors (vector-1 vector-2 pred key)
  (declare (simple-vector vector-1 vector-2))
  "Internal function.  Use MERGE instead."

  (do* ((length-1 (length vector-1))
	(length-2 (length vector-2))
	(result-length (+ length-1 length-2))
	(result (make-vector
		 result-length
		 :element-type (array-element-type vector-1)))
	(fill 0 (1+ fill))			      ;index into result vector
	(I 0)					      ;index into vector-1
	(J 0))					      ;index into vector-2
	((>= fill result-length) result)

	(cond
	 ((= I length-1)
	  (do () ((= fill result-length) result)
	    (setf (%sp-saref1 result fill) (%sp-saref1 vector-2 J))
	    (setq fill (1+ fill))
	    (setq J (1+ J))))

	 ((= J length-2)
	  (do () ((= fill result-length) result)
	    (setf (%sp-saref1 result fill) (%sp-saref1 vector-1 I))
	    (setq fill (1+ fill))
	    (setq I (1+ I))))

	 ((apply-pred (%sp-saref1 vector-2 J) (%sp-saref1 vector-1 I) pred key)
	  (setf (%sp-saref1 result fill) (%sp-saref1 vector-2 J))
	  (setq J (1+ J)))

	 (T (setf (%sp-saref1 result fill) (%sp-saref1 vector-1 I))
	    (setq I (1+ I))))))
;;;    Merge-Non-Simple-Vectors

;;; Merge-non-simple-vectors is like merge-simple-vectors except that the
;;;  vectors are either slisp arrays or slisp vectors.


(defun merge-non-simple-vectors (vector-1 vector-2 pred key)
  "Internal function.  Use MERGE instead."

  (do* ((length-1 (length vector-1))
	(length-2 (length vector-2))
	(result-length (+ (length vector-1) (length vector-2)))
	(result (make-array
		 result-length
		 :element-type (array-element-type vector-1)))
	(fill 0 (1+ fill))			      ;index into result vector
	(I 0)					      ;index into vector-1
	(J 0))					      ;index into vector-2
	((>= fill result-length) result)
	(declare (vector result))

	(cond
	 ((= I length-1)
	  (do () ((= fill result-length) result)
	    (setf (aref result fill) (aref vector-2 J))
	    (setq fill (1+ fill))
	    (setq J (1+ J))))

	 ((= J length-2)
	  (do () ((= fill result-length) result)
	    (setf (aref result fill) (aref vector-1 I))
	    (setq fill (1+ fill))
	    (setq I (1+ I))))

	 ((apply-pred (aref vector-2 J) (aref vector-1 I) pred key)
	  (setf (aref result fill) (aref vector-2 J))
	  (setq J (1+ J)))

	 (T (setf (aref result fill) (aref vector-1 I))
	    (setq I (1+ I))))))