upheap algorithm complete
1) use .at instead of [ since it does bounds check 2) add to q without asking is only one queue