Refactor heapsort