Add heap and heapsort