Finished the heapify function without verification