Revert "So there is a way to do prioq general node removal in log(n)"

This reverts commit c51b556894713d8154cdbef2d4ac9ffcccdaa930.

main.go

@@ -386,11 +386,6 @@ func (pq *prioq) Add(car *Car) {
 
 	// Rebalance tree to respect invariant
 	var i = len(pq.bintree) - 1
-	pq.percolateUp(i)
-}
-
-func (pq *prioq) percolateUp(k int) {
-	var i = k
 	var p = (i - 1) / 2
 	for p >= 0 && pq.bintree[p].greater(pq.bintree[i]) {
 		pq.bintree[p], pq.bintree[i] = pq.bintree[i], pq.bintree[p]
@@ -401,31 +396,25 @@ func (pq *prioq) percolateUp(k int) {
 	}
 }
 
-func (pq *prioq) RemoveAtIndex(k int) *Car {
+func (pq *prioq) Pop() *Car {
 	if len(pq.bintree) == 0 {
 		return nil
 	}
 
-	if k == len(pq.bintree)-1 {
-		elem := pq.bintree[k]
-		pq.bintree = pq.bintree[:k]
+	if len(pq.bintree) == 1 {
+		elem := pq.bintree[0]
+		pq.bintree = pq.bintree[:0]
 		return elem
 	}
 
-	pq.bintree[k].pqindex = -1
-	elem := pq.bintree[k]
-	// Put last element at hole
-	pq.bintree[k] = pq.bintree[len(pq.bintree)-1]
-	pq.bintree[k].pqindex = k
+	pq.bintree[0].pqindex = -1
+	elem := pq.bintree[0]
+	// Put last element at root
+	pq.bintree[0] = pq.bintree[len(pq.bintree)-1]
+	pq.bintree[0].pqindex = 0
 	// Remove last element
 	pq.bintree = pq.bintree[:len(pq.bintree)-1]
 
-	// Oops! might need to go up
-	if k != 0 && !pq.bintree[k].greater(pq.bintree[(k-1)/2]) {
-		pq.percolateUp(k)
-		return elem
-	}
-
 	//        1                  9
 	//    10     9	         10     12
 	//  11 12   13 14  ->  11 12   13 14
@@ -433,8 +422,7 @@ func (pq *prioq) RemoveAtIndex(k int) *Car {
 
 	// Rebalance tree to respect invariant
 	len := len(pq.bintree)
-	i := k
-	left, right := 0, 0
+	i, left, right := 0, 0, 0
 	for {
 		left = 2*i + 1
 		right = 2*i + 2
@@ -475,8 +463,18 @@ func (pq *prioq) RemoveAtIndex(k int) *Car {
 	return elem
 }
 
-func (pq *prioq) Pop() *Car {
-	return pq.RemoveAtIndex(0)
+func (pq *prioq) RemoveAtIndex(k int) *Car {
+	if k == 0 {
+		return pq.Pop()
+	}
+	pq.bintree[k].pqindex = -1
+	elem := pq.bintree[k]
+	reassign := pq.bintree[k+1:]
+	pq.bintree = pq.bintree[:k]
+	for _, c := range reassign {
+		pq.Add(c)
+	}
+	return elem
 }
 
 func (pq *prioq) empty() bool {