Binary Heap - Fixed errors, added tests.