Heap implementation in C++