fix a* implementation by adding decrease-key operation to heap implementations

src/test/java/baritone/bot/pathing/calc/openset/OpenSetsTest.java

@@ -5,6 +5,8 @@ import baritone.bot.pathing.goals.GoalBlock;
 import net.minecraft.util.math.BlockPos;
 import org.junit.Test;
 
+import java.util.*;
+
 import static org.junit.Assert.*;
 
 public class OpenSetsTest {
@@ -19,6 +21,29 @@ public class OpenSetsTest {
         }
     }
 
+    public void removeAndTest(int amount, IOpenSet[] test, Optional<Collection<PathNode>> mustContain) {
+        double[][] results = new double[test.length][amount];
+        for (int i = 0; i < test.length; i++) {
+            long before = System.currentTimeMillis();
+            for (int j = 0; j < amount; j++) {
+                PathNode pn = test[i].removeLowest();
+                if (mustContain.isPresent() && !mustContain.get().contains(pn)) {
+                    throw new IllegalStateException(mustContain.get() + " " + pn);
+                }
+                results[i][j] = pn.combinedCost;
+            }
+            System.out.println(test[i].getClass() + " " + (System.currentTimeMillis() - before));
+        }
+        for (int j = 0; j < amount; j++) {
+            for (int i = 1; i < test.length; i++) {
+                assertEquals(results[i][j], results[0][j], 0);
+            }
+        }
+        for (int i = 0; i < amount - 1; i++) {
+            assertTrue(results[0][i] < results[0][i + 1]);
+        }
+    }
+
     public void testSize(int size) {
         System.out.println("Testing size " + size);
         // Include LinkedListOpenSet even though it's not performant because I absolutely trust that it behaves properly
@@ -27,13 +52,25 @@ public class OpenSetsTest {
         for (IOpenSet set : test) {
             assertTrue(set.isEmpty());
         }
+
+        // generate the pathnodes that we'll be testing the sets on
         PathNode[] toInsert = new PathNode[size];
         for (int i = 0; i < size; i++) {
             PathNode pn = new PathNode(new BlockPos(0, 0, 0), new GoalBlock(new BlockPos(0, 0, 0)));
             pn.combinedCost = Math.random();
             toInsert[i] = pn;
-
         }
+
+        // create a list of what the first removals should be
+        ArrayList<PathNode> copy = new ArrayList<>(Arrays.asList(toInsert));
+        copy.sort(Comparator.comparingDouble(pn -> pn.combinedCost));
+        Set<PathNode> lowestQuarter = new HashSet<>(copy.subList(0, size / 4));
+
+        // all opensets should be empty; nothing has been inserted yet
+        for (IOpenSet set : test) {
+            assertTrue(set.isEmpty());
+        }
+
         System.out.println("Insertion");
         for (IOpenSet set : test) {
             long before = System.currentTimeMillis();
@@ -43,23 +80,46 @@ public class OpenSetsTest {
             //all three take either 0 or 1ms to insert up to 10,000 nodes
             //linkedlist takes 0ms most often (because there's no array resizing or allocation there, just pointer shuffling)
         }
+
+        // all opensets should now be full
         for (IOpenSet set : test) {
             assertFalse(set.isEmpty());
         }
-        System.out.println("Removal");
-        double[][] results = new double[test.length][size];
-        for (int i = 0; i < test.length; i++) {
-            long before = System.currentTimeMillis();
-            for (int j = 0; j < size; j++) {
-                results[i][j] = test[i].removeLowest().combinedCost;
-            }
-            System.out.println(test[i].getClass() + " " + (System.currentTimeMillis() - before));
+
+        System.out.println("Removal round 1");
+        // remove a quarter of the nodes and verify that they are indeed the size/4 lowest ones
+        removeAndTest(size / 4, test, Optional.of(lowestQuarter));
+
+        // none of them should be empty (sanity check)
+        for (IOpenSet set : test) {
+            assertFalse(set.isEmpty());
         }
-        for (int j = 0; j < size; j++) {
-            for (int i = 1; i < test.length; i++) {
-                assertEquals(results[i][j], results[0][j], 0);
+        int cnt = 0;
+        for (int i = 0; cnt < size / 2 && i < size; i++) {
+            if (lowestQuarter.contains(toInsert[i])) { // these were already removed and can't be updated to test
+                continue;
             }
+            toInsert[i].combinedCost *= Math.random();
+            // multiplying it by a random number between 0 and 1 is guaranteed to decrease it
+            for (IOpenSet set : test) {
+                // it's difficult to benchmark these individually because if you modify all at once then update then
+                // it breaks the internal consistency of the heaps.
+                // you have to call update every time you modify a node.
+                set.update(toInsert[i]);
+            }
+            cnt++;
         }
+
+        //still shouldn't be empty
+        for (IOpenSet set : test) {
+            assertFalse(set.isEmpty());
+        }
+
+        System.out.println("Removal round 2");
+        // remove the remaining 3/4
+        removeAndTest(size - size / 4, test, Optional.empty());
+
+        // every set should now be empty
         for (IOpenSet set : test) {
             assertTrue(set.isEmpty());
         }