Heapify works. Still has some problems. Error found by Valgrind