Priority queue with binary heap

binary_heap.cc

/*
 * Priority queue with binary heap
 */

#include <iostream>
#include <vector>
#include <utility>
#include <cstdint>

using std::vector; using std::swap;
using uint32 = std::uint_least32_t;
  
template <class T, class Compare = std::less<T>> class PQueue {
  vector<T> storage;
  uint32 end;

public:
  PQueue (uint32 size) : end(1) {
    storage.assign(size+1, 0);
  }

  void push (T obj) {
    storage[end] = obj;
    push_heapify(end);
    end++;
  }

  T top() {
    return storage[1];
  }
  
  void pop () {
    end--;
    storage[1] = storage[end];
    pop_heapify(1);
  }

private:
  void push_heapify(uint32 pos) {
    if (pos != 1) {
      uint32 parent = pos / 2;
      if (Compare()(storage[pos], storage[parent])) {
        swap(storage[parent], storage[pos]);
        push_heapify(parent);
      }
    }
  }

  void pop_heapify(uint32 pos) {
    uint32 child1 = pos*2;
    uint32 child2 = pos*2 + 1;
    if (child1 < end) {
      if (child2 < end) {
        if (Compare()(storage[child1], storage[child2])) {
          if (Compare()(storage[child1], storage[pos])) {
            swap(storage[pos], storage[child1]);
            pop_heapify(child1);
          }
        } else {
          if (Compare()(storage[child2], storage[pos])) {
            swap(storage[pos], storage[child2]);
            pop_heapify(child2);
          }
        }
      } else {
        if (Compare()(storage[child1], storage[pos])) {
          swap(storage[pos], storage[child1]);
        }
      }
    }
  }
};

// test
int main () {
  PQueue<int, std::greater<int>> q(100);
  q.push(4);
  q.push(-1);
  q.push(4);
  q.push(5);
  q.push(0);
  for (int i = 0; i < 5; i++) {
    std::cout << q.top() << std::endl;
    q.pop();
  }
  return 0;
}