source: branches/f4grobner/priority-queue.lisp@ 54

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;
;; Priority queue stuff
;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defparameter *priority-queue-allocation-size* 16)

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

(defstruct (priority-queue (:constructor priority-queue-construct))
  (heap (priority-queue-make-heap))
  test)

(defun make-priority-queue (&key (element-type 'fixnum)
                            (test #'<=)
                            (element-key #'identity))
  (priority-queue-construct
   :heap (priority-queue-make-heap :element-type element-type)
   :test #'(lambda (x y) (funcall test (funcall element-key y) (funcall element-key x)))))
  
(defun priority-queue-insert (pq item)
  (priority-queue-heap-insert (priority-queue-heap pq) item (priority-queue-test pq)))

(defun priority-queue-remove (pq)
  (priority-queue-heap-remove (priority-queue-heap pq) (priority-queue-test pq)))

(defun priority-queue-empty-p (pq)
  (priority-queue-heap-empty-p (priority-queue-heap pq)))

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

(defun priority-queue-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 priority-queue-heap-insert (a item &optional (test #'<=))
  (vector-push-extend item a)
  (priority-queue-upheap a (1- (fill-pointer a)) test))

(defun priority-queue-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 priority-queue-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 priority-queue-heap-empty-p (a)
  (<= (fill-pointer a) 1))