Refactor so features are evaluated when commands are returned

Evalating the features inside the command was problematic, since
replacing a Command expression with an expression that could
return a feature-controlled flag would change the requirements of
the resulting Command.

lib/src/main/java/com/team2813/lib2813/feature/FeatureControlledCommand.java

@@ -0,0 +1,72 @@
+package com.team2813.lib2813.feature;
+
+import static java.util.Objects.requireNonNull;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WrapperCommand;
+
+import java.util.*;
+import java.util.function.Function;
+import java.util.function.Supplier;
+import java.util.stream.Collectors;
+
+/** A command that is commandSelected via one or more features. */
+public final class FeatureControlledCommand extends WrapperCommand {
+    private final boolean commandSelected;
+    private final String expression;
+
+    /** If the features are all disabled, use the provided command. */
+    public Command otherwise(Supplier<Command> commandSupplier) {
+        if (commandSelected) {
+            return this;
+        }
+        return new FeatureControlledCommand(commandSupplier.get(), String.format("not (%s)", expression));
+    }
+
+    /** If the features are disabled, and the following feature is commandSelected, use the provided command. */
+    public <T extends Enum<T> & FeatureIdentifier>  FeatureControlledCommand elseIfEnabled(T feature, Supplier<Command> commandSupplier) {
+        if (commandSelected) {
+            return this;
+        }
+        return ifAllEnabled(commandSupplier, Collections.singleton(feature),
+                s -> String.format("not %s and (%s)", expression, s));
+    }
+
+    String getExpression() {
+        return expression;
+    }
+
+    static FeatureControlledCommand ifAllEnabled(
+            Supplier<Command> commandSupplier, Collection<FeatureIdentifier> features) {
+        return ifAllEnabled(commandSupplier, features, s -> s);
+    }
+
+    private static FeatureControlledCommand ifAllEnabled(
+            Supplier<Command> commandSupplier, Collection<FeatureIdentifier> features, Function<String, String> expressionBuilder) {
+        requireNonNull(commandSupplier, "commandSupplier cannot be null");
+        requireNonNull(features, "features cannot be null");
+
+        String expression = expressionBuilder.apply(
+                features.stream().map(FeatureIdentifier::name).collect(Collectors.joining(", ")));
+
+        if (features.stream().noneMatch(Objects::isNull) &&
+                features.stream().allMatch(FeatureIdentifier::enabled)) {
+            return new FeatureControlledCommand(commandSupplier.get(), expression);
+        }
+        return new FeatureControlledCommand(null, "not " + expression);
+    }
+
+    private FeatureControlledCommand(Command command, String expression) {
+        super(command == null ? Commands.none() : command);
+        this.commandSelected = (command != null);
+        this.expression = expression;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        super.initSendable(builder);
+        builder.publishConstString("expression", expression);
+    }
+}

lib/src/main/java/com/team2813/lib2813/feature/FeatureIdentifier.java

@@ -1,5 +1,10 @@
 package com.team2813.lib2813.feature;
 
+import java.util.Collections;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
 /** Mix-in interface for features. This should be implemented by an enum. */
 public interface FeatureIdentifier {
 
@@ -22,4 +27,14 @@ enum FeatureBehavior {
     default boolean enabled() {
         return FeatureRegistry.getInstance().enabled(this);
     }
+
+    /**
+     * Decorates the command if this feature is enabled.
+     *
+     * <p>If the feature is disabled, returns a do-nothing command.
+     */
+    default FeatureControlledCommand ifEnabled(Supplier<Command> commandSupplier) {
+        return FeatureControlledCommand.ifAllEnabled(
+                commandSupplier, Collections.singleton(this));
+    }
 }

lib/src/main/java/com/team2813/lib2813/feature/FeatureRegistry.java

@@ -1,14 +1,8 @@
 package com.team2813.lib2813.feature;
 
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collection;
-import java.util.List;
-import java.util.Map;
-import java.util.Objects;
+import java.util.*;
 import java.util.concurrent.ConcurrentHashMap;
-import java.util.function.BooleanSupplier;
-import java.util.stream.Stream;
+import java.util.stream.Collectors;
 
 import com.team2813.lib2813.feature.FeatureIdentifier.FeatureBehavior;
 
@@ -20,58 +14,19 @@
 /** Container for features that can be enabled at runtime. */
 final class FeatureRegistry {
     private final Map<FeatureIdentifier, Feature> registeredFeatures = new ConcurrentHashMap<>();
+    private final ShuffleboardTab shuffleboardTab;
 
     /** Package-scope constructor (for testing) */
-    FeatureRegistry() {}
+    FeatureRegistry(String shuffleboardTabName) {
+        this.shuffleboardTab = Shuffleboard.getTab(shuffleboardTabName);
+    }
 
     public static FeatureRegistry getInstance() {
         return SingletonHolder.instance;
     }
 
     private static class SingletonHolder {
-        static final FeatureRegistry instance = new FeatureRegistry();
-    }
-
-    /**
-     * Creates a {@link BooleanSupplier} that returns {@code true} iff all the given features are enabled.
-     *
-     * @param first A feature identifier (if {@code null}, the returned supplier will always return {@code false}).
-     * @param rest  Zero or more additional feature identifiers (if {@code null} or contains {@code null} values, the
-     *              returned supplier will always return {@code false}).
-     */
-    @SafeVarargs
-    final <T extends Enum<T> & FeatureIdentifier> BooleanSupplier asSupplier(T first, T... rest) {
-        if (first == null || rest == null) {
-            return () -> false;
-        }
-
-        List<T> featureIdentifiers = new ArrayList<>(rest.length + 1);
-        featureIdentifiers.add(first);
-        featureIdentifiers.addAll(Arrays.asList(rest));
-        return asSupplier(featureIdentifiers);
-    }
-
-    <T extends Enum<T> & FeatureIdentifier> BooleanSupplier asSupplier(Collection<T> featureIdentifiers) {
-        if (featureIdentifiers == null || featureIdentifiers.stream().anyMatch(Objects::isNull)) {
-            return () -> false;
-        }
-
-        List<Feature> features = featureIdentifiers.stream().map(this::getFeature).toList();
-
-        return () -> features.stream().allMatch(Feature::enabled);
-    }
-
-    @SafeVarargs
-    final <T extends Enum<T> & FeatureIdentifier> boolean allEnabled(T first, T... rest) {
-        if (first == null || rest == null || Stream.of(rest).anyMatch(Objects::isNull)) {
-            return false;
-        }
-
-        if (!getFeature(first).enabled()) {
-            return false;
-        }
-
-        return Stream.of(rest).map(this::getFeature).allMatch(Feature::enabled);
+        static final FeatureRegistry instance = new FeatureRegistry("Features");
     }
 
     boolean enabled(FeatureIdentifier id) {
@@ -82,9 +37,16 @@ Feature getFeature(FeatureIdentifier id) {
         return registeredFeatures.computeIfAbsent(id, Feature::new);
     }
 
-    static final class Feature {
+    Object getState() {
+        return registeredFeatures.entrySet().stream()
+                .filter(entry -> entry.getValue().changed())
+                .map(Map.Entry::getKey)
+                .collect(Collectors.toSet());
+    }
+
+    final class Feature {
         final SimpleWidget widget;
-        private static final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Features");
+        final boolean initiallyEnabled;
 
         Feature(FeatureIdentifier id) {
             String name = String.format("%s.%s", id.getClass().getName(), id.name());
@@ -93,19 +55,23 @@ static final class Feature {
             }
 
             FeatureBehavior behavior = id.behavior();
-            boolean enabled = (behavior == FeatureBehavior.INITIALLY_ENABLED);
+            initiallyEnabled = (behavior == FeatureBehavior.INITIALLY_ENABLED);
             boolean alwaysDisabled = (
                     behavior == null || behavior == FeatureBehavior.ALWAYS_DISABLED);
             if (alwaysDisabled) {
                 shuffleboardTab.addBoolean(name, () -> false).withWidget(BuiltInWidgets.kBooleanBox);
                 widget = null;
             } else {
-                widget = shuffleboardTab.add(name, enabled).withWidget(BuiltInWidgets.kToggleSwitch);
+                widget = shuffleboardTab.add(name, initiallyEnabled).withWidget(BuiltInWidgets.kToggleSwitch);
             }
         }
 
         boolean enabled() {
             return widget != null && widget.getEntry().getBoolean(false);
         }
+
+        boolean changed() {
+            return enabled() != initiallyEnabled;
+        }
     }
 }