Heapsort implemented in C++