Improve heapify method