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

;;; -*-  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 "PRIORITY-QUEUE"

  (:use :cl)

  (:export "*PRIORITY-QUEUE-ALLOCATION-SIZE*"

           "PRIORITY-QUEUE"

           "PRIORITY-QUEUE-HEAP"

           "PRIORITY-QUEUE-TEST"

           "MAKE-PRIORITY-QUEUE"

           "PRIORITY-QUEUE-INSERT"

           "PRIORITY-QUEUE-REMOVE"

           "PRIORITY-QUEUE-EMPTY-P"

           "PRIORITY-QUEUE-SIZE"

           ))



(in-package :priority-queue)

(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))