Priority queue works fine

Add some method to display the heap.