;;; -*-  Mode: Lisp -*- 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;                                                                              
;;;  Copyright (C) 1999, 2002, 2009, 2015 Marek Rychlik <rychlik@u.arizona.edu>		 
;;;  		       								 
;;;  This program is free software; you can redistribute it and/or modify	 
;;;  it under the terms of the GNU General Public License as published by	 
;;;  the Free Software Foundation; either version 2 of the License, or		 
;;;  (at your option) any later version.					 
;;; 		       								 
;;;  This program is distributed in the hope that it will be useful,		 
;;;  but WITHOUT ANY WARRANTY; without even the implied warranty of		 
;;;  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the		 
;;;  GNU General Public License for more details.				 
;;; 		       								 
;;;  You should have received a copy of the GNU General Public License		 
;;;  along with this program; if not, write to the Free Software 		 
;;;  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.	 
;;;										 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;
;; A conventional implementation of priority queues based on heaps
;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defpackage "HEAP"
  (:use :cl)
  (:export "+HEAP-ALLOCATION-SIZE+"
	   "MAKE-HEAP"
	   "HEAP-SIZE"
	   "HEAP-UPHEAP"
	   "HEAP-INSERT"
	   ))

(in-package :heap)
(defparameter +heap-allocation-size+ 16)

(defun make-heap (&key (element-type 'fixnum))
  (make-array +heap-allocation-size+ :element-type element-type :fill-pointer 1
	      :adjustable t))

(defun heap-size (pq)
  (fill-pointer (priority-queue-heap pq)))

(defun heap-upheap (a k
	       &optional
	       (test #'<=)
	       &aux  (v (aref a k)))
  (declare (fixnum k))
  (assert (< 0 k (fill-pointer a)))
  (loop
   (let ((parent (ash k -1)))
     (when (zerop parent) (return))
     (unless (funcall test (aref a parent) v)
       (return))
     (setf (aref a k) (aref a parent)
	   k parent)))
  (setf (aref a k) v)
  a)

    
(defun heap-insert (a item &optional (test #'<=))
  (vector-push-extend item a)
  (heap-upheap a (1- (fill-pointer a)) test))

(defun heap-downheap (a k
		 &optional
		 (test #'<=)
		 &aux  (v (aref a k)) (j 0) (n (fill-pointer a)))
  (declare (fixnum k n j))
  (loop
   (unless (<= k (ash n -1))
     (return))
   (setf j (ash k 1))
   (if (and (< j n) (not (funcall test (aref a (1+ j)) (aref a j))))
       (incf j))
   (when (funcall test (aref a j) v)
     (return))
   (setf (aref a k) (aref a j)
	 k j))
  (setf (aref a k) v)
  a)

(defun heap-remove (a &optional (test #'<=) &aux (v (aref a 1)))
  (when (<= (fill-pointer a) 1) (error "Empty queue."))
  (setf (aref a 1) (vector-pop a))
  (priority-queue-downheap a 1 test)
  (values v a))

(defun heap-empty-p (a)
  (<= (fill-pointer a) 1))