Fixed a silly bug in heapify
Fixed some more out by one bugs