ヒープによる優先度付きキュー（priority queue）

ちょっと思うところあって、優先度付きキューをヒープで書いてみた。ただし実体はクロージャ。引数なしで呼ぶとデキュー、引数を受け取るとエンキューするという単純なもの。
キューを作るためには、初期要素となる配列（未整列で良い）と順序関数を与える。ヒープソートしたあとエンキュー・デキュー用のクロージャが返ってくる。内部的にはキューと言っても配列上のヒープで、添字は先頭が１。添字 i の子は 2*i と 2*i + 1 で、親は i/2 となる。局所関数 downheap は、添字 i を受け取って、そこを頂点にヒープを再構成する。
【追記】なぜかエンキュー時にまで downheap していたのを修正。

コード：

(defun make-priority-queue (array order
                            &key default (start 0) (end (length array)))
  (let ((len (- end start)))
    (check-type len (integer 0))
    (let* ((q-end (1+ len))
           (pq (make-array q-end
                           :element-type (array-element-type array)
                           :adjustable t
                           :fill-pointer q-end)))
      (replace pq array :start1 1 :end1 q-end :start2 start :end2 end)
      (flet ((downheap (i)
               (let ((x (aref pq i)))
                 (do ((j (* 2 i)))
                     ((>= j (fill-pointer pq)))
                   (when (and (< (1+ j) (fill-pointer pq))
                              (funcall order
                                       (aref pq (1+ j)) (aref pq j)))
                     (incf j))
                   (if (funcall order x (aref pq j))
                       (return)
                       (setf (aref pq i) (aref pq j)
                             i j
                             j (* 2 j))))
                 (setf (aref pq i) x))))
        (do ((i (floor len 2) (1- i)))
            ((zerop i))
          (downheap i))
        #'(lambda (&optional (newval nil supplied))
            (if supplied
                (do* ((child (vector-push-extend newval pq) parent)
                      (parent (floor child 2) (floor parent 2)))
                     ((zerop parent) newval)
                  (if (funcall order (aref pq parent) (aref pq child))
                      (return newval)
                      (rotatef (aref pq parent) (aref pq child))))
                (if (= (fill-pointer pq) 1)
                    default
                    (prog1 (aref pq 1)
                      (setf (aref pq 1) (vector-pop pq))
                      (downheap 1)))))))))

使用例：

CL-USER> (setq a #(3 1 4))
#(3 1 4)
CL-USER> (setf (symbol-function 'pq)
               (make-priority-queue a #'<=))
#<Closure (:INTERNAL MAKE-PRIORITY-QUEUE 0) @ #x206c1222>
CL-USER> (pq)
1
CL-USER> (pq)
3
CL-USER> (pq -1)
-1
CL-USER> (pq 5)
5
CL-USER> (pq)
-1
CL-USER> (pq)
4
CL-USER> (pq)
5
CL-USER> (pq)
NIL