Adding more efficient heap sort which has a heapify and re-heapify which are different