Added heapsort algorithm and refactored some code