Corrected heapsort