Added Heapsort and tests