New implementation of heapify