Binary Heap implementation, works now.