;; -*- Lisp -*-
;;;
;;; $Header: /home/gene/library/website/docsrc/lisp-heap/RCS/heap.lisp,v 395.1 2008/04/20 17:25:55 gene Exp $
;;;
;;; (parts) Copyright (c) 2002, 2003 Gene Michael Stover., GPL 
;;; modified 7/25/08 by RJF.  copy obtained on that day from
;;; http://cybertiggyr.com/lisp-heap/ changed order to 2, always

(eval-when (compile load) (declaim (optimize (speed 3)(safety 0))))

(defstruct heap a )

;; just for doing heaps of numbers. no data other than index
(defmacro hlessfun (a b)
  `(< ,a ,b))

(defun percolate-down (heap hole x)
  "Private. Move the HOLE down until it's in a location suitable for X.
Return the new index of the hole."
  (declare (optimize(speed 3)(safety 0)))
  (do ((a (heap-a heap))
       (child (lesser-child heap hole) (lesser-child heap hole)))
      ((or (>= child (fill-pointer a)) (hlessfun x (aref a child)))
       hole)
    (declare (fixnum child hole))
      (setf (aref a hole) (aref a child)
	    hole child)))

(defun percolate-up (heap hole x)
  "Private.  Moves the HOLE until it's in a location suitable for holding
X.  Does not actually bind X to the HOLE.  Returns the new
index of the HOLE.  The hole itself percolates down; it's the X
that percolates up."
  (let ((a (heap-a heap)))
    (setf (aref a 0) x)
    (do ((i hole parent)
	 (parent (ash hole -1);;(floor (/ hole 2))
		 (ash parent -1);;(floor (/ parent 2))
		 ))
	;; potential to speed up line below by declaration if a, x are fixnum,
	((not (hlessfun x (aref a parent))) i)
      (declare (fixnum hole parent))
      (setf (aref a i) (aref a parent)))))

(defun heap-init (heap  &key  (initial-contents nil) (initial-size 2))
  "Initialize the indicated heap.  If INITIAL-CONTENTS is a non-empty
list, the heap's contents are initialized to the values in that
list which are ordered according to hlessfun.  INITIAL-CONTENTS must
be a list or NIL."
  (setf	(heap-a heap) 
	(make-array initial-size :initial-element nil
		    :adjustable t :fill-pointer 1)	)
  (when initial-contents
    (dolist (i initial-contents) (vector-push-extend i (heap-a heap)))
    (loop for i from (floor (/ (length (heap-a heap)) 2)) downto 1
	  do (let* ((tmp (aref (heap-a heap) i))
		    (hole (percolate-down heap i tmp)))
	       (setf (aref (heap-a heap) hole) tmp))) )
  heap)

(defun heap-init-ordered
  (heap &key (initial-contents nil)(element-type t))
  ;; used if initial-contents is already sorted list
  ;; could be a fixnum array ;;rjf
  
  "Initialize the indicated heap.  If INITIAL-CONTENTS is a non-empty
list, the heap's contents are initialized to the values in that
list; they are assumed already ordered according to hlessfun.  INITIAL-CONTENTS must
be a list or NIL."
  (let ((n (length initial-contents)))	;array or list
    (setf
     (heap-a heap)
     (make-array n
		 :initial-contents initial-contents
		  :adjustable t  ; try not adjustable for speed
		 :fill-pointer n
		 :element-type element-type) )
    heap))

(defun create-heap (&key (initial-contents nil)(initial-size 2))
  (heap-init (make-heap)     :initial-contents initial-contents
	     :initial-size initial-size))

(defun create-heap-ordered (initial-contents &key (element-type t))
  ;; important: initial-contents must have the first element duplicated.
  ;; e.g.  (3 5 8)  should be (3 3 5 8)
  (heap-init-ordered (make-heap) 
	     :initial-contents initial-contents :element-type element-type))

(defun heap-clear (heap)
  "Remove all elements from the heap, leaving it empty.  Faster
(& more convenient) than calling HEAP-REMOVE until the heap is
empty."
  (setf (fill-pointer (heap-a heap)) 1)
  nil)

(defun heap-count (heap)
  (1- (fill-pointer (heap-a heap))))

(defun heap-empty-p (heap)
  "Returns non-NIL if & only if the heap contains no items."
  (= (fill-pointer (heap-a heap)) 1))
(defun heap-insert (heap x)
  "Insert a new element into the heap.  Returns the heap." ;; rjf
  (let ((a (heap-a heap)))
    ;; Append a hole for the new element.
     (vector-push-extend nil a) 
    ;; assume enough room...
    ;;(vector-push nil a)

    ;; Move the hole from the end towards the front of the
    ;; queue until it is in the right position for the new
    ;; element.
    (setf (aref a (percolate-up heap (1- (fill-pointer a)) x)) x)))

(defun heap-find-idx (heap fnp)
  "Return the index of the element which satisfies the predicate FNP.
If there is no such element, return the fill pointer of HEAP's array A."
  (do* ((a (heap-a heap))
	(fp (fill-pointer a))
	(i  1  (1+ i)))
       ((or (>= i fp) (funcall fnp heap (aref a i)))
	i)))

(defun heap-remove (heap &optional (fn #'(lambda (h x)(declare (ignore h x)) t)))
  "Remove the minimum (first) element in the heap & return it.  It's
an error if the heap is already empty.  (Should that be an error?)"
  (let ((a (heap-a heap))
	(i (heap-find-idx heap fn)))
    (cond ((< i (fill-pointer a));; We found an element to remove.
	   (let ((x (aref a i))
		 (last-object (vector-pop a)))
	     (setf (aref a (percolate-down heap i last-object)) last-object)
	     x))
	  (t nil))));; Nothing to remove

(defun heap-remove-fast(heap) 
  ;; assumes non-empty!! if empty next answer is bogus.
  ;; answer after that is an error (fill pointer can't pop)
  (let* ((a (heap-a heap))
	 (x (aref a 1))
	 (last-object (vector-pop a)))
    ;; could declare array a element-type if we knew it if was fixnum
    ;; here and elsewhere.
 ;   (declare(type (simple-array fixnum (*)) a)(optimize (speed 3)(safety 0)))
    (setf (aref a (percolate-down heap 1 last-object)) last-object)
	     x))

(defun heap-peek (heap)
  "Return the first element in the heap, but don't remove it.  It'll
be Erroneous if the heap is empty.   (Should that be an error?)"
  (aref (heap-a heap) 1))

(defun lesser-child (heap parent)
  "Return the index of the lesser child.  If there's one child,
 return its index.  If there are no children, return 
 (FILL-POINTER (HEAP-A HEAP))."
  (declare (optimize(speed 3)(safety 0)) (fixnum parent) )
  (let* ((a (heap-a heap))
	 (left (ash parent 1)) ;;(* parent 2 )
         (right (1+ left))
         (fp (fill-pointer a)))
    (declare (fixnum left fp right)
	    ;;(type (simple-array t (*)) a)
	     )
    (cond ((>= left fp) fp)
          ((= right fp) left)
          ((hlessfun (aref a left) (aref a right)) left)
          (t right))))

(provide "heap")

(defparameter data (loop for i from 1 to 1000 collect  (random 1000)))
(defparameter h (create-heap))
(defun test-in(heap data)(dolist (i data)(heap-insert heap i)))
(defun test-out(heap)(loop (if (heap-empty-p heap)(return t)(heap-remove-fast heap))))


(defun msort(data);; merge sort of list of integers.
  (let ((h (create-heap)))
    (test-in h data)
    (loop while (not (heap-empty-p h)) collect (heap-remove-fast h))))