Refactor Heapsort