Implemented heap functions, including heapsort