C++ heapsort