Fixed heapsort bug; cleaned the code a bit