Add heapify function