Heap sort code