Implement heapsort