;; -*- Lisp -*-
;;; ASSUMING many items to be fixnums
;;; REMOVING assumptions about data being fixnums. Just fixnum array indexes
;;; see heap.lisp for docs.
;;; here we make array big enough so adjustable is not needed.

(eval-when (compile load) (declaim (special fill-pointer) (fixnum fill-pointer)
				   (optimize (speed 3)(safety 0))))

(defmacro hlessfun (r s)
  
  `(let ((cr (car ,r))(cs (car ,s)))
     (declare (integer cr cs)) ;gheap
  (cond ((< cr cs) t)
	((= cr cs) (< (the integer (caaadr ,r))(the integer (caaadr ,s))))
	)))

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

(defun percolate-up (a 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."
  (declare (fixnum hole)  (type (simple-array t (*)) a)
	   (optimize (speed 3)(safety 0)))
    (setf (aref a 0) x)
    (do ((i hole parent)
	 (parent (ash hole -1) (ash parent -1) ))
	((not (hlessfun x (aref a parent))) i)
      (declare (fixnum i parent))
      (setf (aref a i) (aref a parent))))

(defun create-heap-ordered
  (initial-contents)
  ;; used only if initial-contents is already sorted list.
  ;; important: initial-contents must have the first element duplicated.
  ;; e.g.  (3 5 8)  should be (3 3 5 8)
 ;; (format t "*")
  
  "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))
	 (heap
	  (make-array  n
		       :initial-contents initial-contents
		       :element-type t)))
    (setf fill-pointer n)		;global value for this heap
    heap))

(defun heap-count (heap)
   (declare (ignore heap))
  (1- fill-pointer))

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

    (declare (type (simple-array t (*)) a)(fixnum fill-pointer)
	     (optimize (speed 3)(safety 0)))
         ;;(format t ".")
    (setf (aref a fill-pointer) nil)
    (incf fill-pointer)
    ;; 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 a (1- fill-pointer) x)) x))

(defun heap-remove-fast(a) 
  ;; assumes non-empty!! if empty next answer is bogus.
  ;; answer after that is an error (fill pointer can't pop)
 
  (declare(type (simple-array t(*)) a)(optimize (speed 3)(safety 0)))
     ;;(format t "-")
  (let* ((x (aref a 1))
	 (last-object		
	  (progn (decf fill-pointer)(aref a fill-pointer) ) ))
    (setf (aref a (percolate-down a 1 last-object)) last-object)
    x))

(defun lesser-child (a 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 (type (simple-array t (*)) a)
	   (optimize(speed 3)(safety 0)) (fixnum parent) )
  (let* ((left (ash parent 1)) ;;(* parent 2 )
         (right (1+ left))
         (fp fill-pointer))
    (declare (fixnum left fp right))
    (cond ((>= left fp) fp)
          ((= right fp) left)
          ((hlessfun (aref a left) (aref a right)) left)
          (t right))))
;;; --- end of heap file ---