Added HeapSort

* Also minor updates to percolate_up, percolate_down, PriorityQueue