Heapify for initialization + benchmarks