Modified Heap sort implementation