Implement working heapsort implementation