heapify tested and working