Heap almost done. Small bugs