Corrected HeapSort