Cancel lift when using Y

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TeleOp.java

@@ -298,7 +298,8 @@ public void runOpMode() {
             double slideTicks = (robot.liftRight().getCurrentPosition() + robot.liftLeft().getCurrentPosition()) / 2.0;
 
             if (gamepad1.y) {
-                // FIXME this class is not being used elsewhere
+                // Cancel any lift related tasks, including WAITING tasks, in favor of this one
+                scheduler.filteredStop((task) -> task.requirements().contains(robot.getLiftLock()), true, true);
                 dumper.autoReset();
                 targetLeft.set(hangTarget);
                 targetRight.set(hangTarget);

multitask/src/main/java/dev/aether/collaborative_multitasking/Extensions.kt

@@ -0,0 +1,20 @@
+package dev.aether.collaborative_multitasking
+
+fun <K, V> MutableMap<K, V>.filterInPlace(predicate: (Map.Entry<K, V>) -> Boolean): List<K> {
+    val collected: MutableList<K> = mutableListOf()
+    for (pair in this.entries) {
+        if (!predicate(pair)) {
+            collected.add(pair.key)
+        }
+    }
+    val deleted: MutableList<K> = mutableListOf()
+    for (delete in collected) {
+        val del = this.remove(delete)
+        if (del != null) deleted.add(delete)
+    }
+    return deleted
+}
+
+fun <K, V> MutableMap<K, V>.filterInPlace(predicate: (K, V) -> Boolean) = filterInPlace { pair ->
+    predicate(pair.key, pair.value)
+}

multitask/src/main/java/dev/aether/collaborative_multitasking/MultitaskScheduler.kt

@@ -241,14 +241,33 @@ class MultitaskScheduler
      * Resources owned by matching tasks are guaranteed to be released after this call.
      */
 
-    override fun filteredStop(predicate: (Task) -> Boolean, cancel: Boolean) {
+    override fun filteredStop(
+        predicate: (Task) -> Boolean,
+        cancel: Boolean,
+        dropNonStarted: Boolean
+    ) {
         tasks.values
             .filter { it.state != Task.State.Finished && it.state != Task.State.NotStarted && it.state != Task.State.Cancelled }
             .filter(predicate)
             .forEach {
                 release(it, cancel)
             }
+        if (dropNonStarted) {
+            val dropped = tasks.filterInPlace { k, v ->
+                v.state == Task.State.NotStarted && predicate(v)
+            }
+            println("dropped ${dropped.size} tasks: ${dropped.joinToString(", ")}")
+        }
     }
 
-    override fun filteredStop(predicate: (Task) -> Boolean) = filteredStop(predicate, true)
+    override fun filteredStop(predicate: (Task) -> Boolean, cancel: Boolean) = filteredStop(
+        predicate,
+        cancel = cancel, dropNonStarted = false
+    )
+
+    override fun filteredStop(predicate: (Task) -> Boolean) = filteredStop(
+        predicate,
+        cancel = true,
+        dropNonStarted = false
+    )
 }

multitask/src/main/java/dev/aether/collaborative_multitasking/Scheduler.kt

@@ -13,4 +13,9 @@ abstract class Scheduler {
 
     abstract fun filteredStop(predicate: (Task) -> Boolean, cancel: Boolean)
     abstract fun filteredStop(predicate: (Task) -> Boolean)
+    abstract fun filteredStop(
+        predicate: (Task) -> Boolean,
+        cancel: Boolean,
+        dropNonStarted: Boolean
+    )
 }