Added HeapSort

* Also minor updates to percolate_up, percolate_down, PriorityQueue
Merge pull request #3824 from kmsquire/heapsort

RFC: HeapSort