#lang racket
(define heap-new
  (lambda (higher-priority?)
    (vector 0 (make-vector 31) higher-priority?)))
(define first
  (lambda (heap)
    (vector-ref (vector-ref heap 1) 0)))

(define add
  (lambda (heap value)
    (if (= (vector-ref (vector-ref heap 1) (vector-ref heap 0)) 0)
           (vector-set! (vector-ref heap 1) (vector-ref heap 0) value)
           (begin (vector-set! heap 1 (list->vector (append (vector->list (vector-ref heap 1)) (make-list (vector-length heap) 0))))
                  (vector-set! (vector-ref heap 1) (vector-ref heap 0) value)))
    (vector-set! heap 1 (add-helper (vector-ref heap 1) (+ (vector-ref heap 0) 1) (vector-ref heap 2)))
    (vector-set! heap 0 (+ (vector-ref heap 0) 1))))

(define add-helper
  (lambda (heap position higher-priority?)
    (if (= position 1)
        heap
        (letrec ([current (vector-ref heap (- position 1))]
             [parent-index (- (floor (/ position 2)) 1)]
             [parent (vector-ref heap parent-index)])
      (if (higher-priority? current parent)
          (begin (vector-set! heap (- position 1) parent)
                 (vector-set! heap parent-index current)
                 (add-helper heap parent-index higher-priority?))
          heap)))))

(define get
  (lambda (heap)
    (let ([ret (vector-ref (vector-ref heap 1) 0)])
      (vector-set! (vector-ref heap 1) 0 (vector-ref (vector-ref heap 1) (- (vector-ref heap 0) 1)))
      (vector-set! (vector-ref heap 1) (- (vector-ref heap 0) 1) 0)
      (vector-set! heap 1 (get-helper (vector-ref heap 1) 1 (vector-ref heap 2)))
      (vector-set! heap 0 (- (vector-ref heap 0) 1))
      ret)))

(define get-helper
  (lambda (heap position higher-priority?)
    (let ([leftChild (vector-ref heap (- (* position 2) 1))]
          [rightChild (vector-ref heap (* position 2))]
          [current (vector-ref heap (- position 1))])
      (if (higher-priority? leftChild rightChild)
          (if (higher-priority? leftChild (vector-ref heap (- position 1)))
              (begin (vector-set! heap (- position 1) leftChild)
                     (vector-set! heap (- (* position 2) 1) current)
                     (get-helper heap (- (* position 2) 1) higher-priority?))
              heap)
          (if (higher-priority? rightChild (vector-ref heap (- position 1)))
              (begin (vector-set! heap (- position 1) rightChild)
                     (vector-set! heap (* position 2) current)
                     (get-helper heap (* position 2) higher-priority?))
              heap)))))




(define heap
  (heap-new >))

(add heap 100)
(add heap 19)
(add heap 36)
(add heap 1)
(add heap 25)
(add heap 7)
(add heap 2)
(add heap 3)
(add heap 17)
(add heap 20)

(get heap)

(vector-ref heap 1)

(get heap)