Fix Heap indexing into the array, add some priority queue tests