Corrected Heap Sort