; conditional heap insertion

(define-syntax pq-rank (syntax-rules () ((_ pq) (vector-ref pq 0))))
(define-syntax pq-item (syntax-rules () ((_ pq) (vector-ref pq 1))))
(define-syntax pq-lkid (syntax-rules () ((_ pq) (vector-ref pq 2))))
(define-syntax pq-rkid (syntax-rules () ((_ pq) (vector-ref pq 3))))

(define pq-empty (vector 0 'pq-empty 'pq-empty 'pq-empty))
(define (pq-empty? pq) (eqv? pq pq-empty))

(define (pq-merge lt? p1 p2)
  (define (pq-swap item lkid rkid)
    (if (< (pq-rank rkid) (pq-rank lkid))
        (vector (+ (pq-rank rkid) 1) item lkid rkid)
        (vector (+ (pq-rank lkid) 1) item rkid lkid)))
  (cond ((pq-empty? p1) p2)
        ((pq-empty? p2) p1)
        ((lt? (pq-item p2) (pq-item p1))
          (pq-swap (pq-item p2) (pq-lkid p2)
                   (pq-merge lt? p1 (pq-rkid p2))))
        (else (pq-swap (pq-item p1) (pq-lkid p1)
                       (pq-merge lt? (pq-rkid p1) p2)))))

(define (pq-insert lt? x pq)
  (pq-merge lt? (vector 1 x pq-empty pq-empty) pq))

(define (pq-first pq)
  (if (pq-empty? pq)
      (error 'pq-first "empty priority queue")
      (pq-item pq)))

(define (pq-rest lt? pq)
  (if (pq-empty? pq)
      (error 'pq-rest "empty priority queue")
      (pq-merge lt? (pq-lkid pq) (pq-rkid pq))))

(define (pq-contains? lt? x pq)
  (if (pq-empty? pq) #f
    (if (lt? x (pq-item pq)) #f
      (if (not (lt? (pq-item pq) x)) #t
        (or (pq-contains? lt? x (pq-lkid pq))
            (pq-contains? lt? x (pq-rkid pq)))))))

(define (pq-conditional-insert lt? x pq)
  (if (pq-contains? lt? x pq) pq
    (pq-insert lt? x pq)))

(define pq
  (pq-insert < 3
  (pq-insert < 7
  (pq-insert < 8
  (pq-insert < 1
  (pq-insert < 2
  (pq-insert < 9
  (pq-insert < 6
  (pq-insert < 4
  (pq-insert < 5
  pq-empty))))))))))

(display pq) (newline)

(display (pq-contains? < 3 pq)) (newline)

(display (pq-contains? < 0 pq)) (newline)

(display (pq-conditional-insert < 0 pq))