Introduce Features

Features can be initally disabled or disabled, and can be configured to be
enabled via Shuffleboard. Commands can be configured to be scheduled only
if all controlling features are enabled by using Features.whenAllEnabled().

This allows us to add functionality to the robot that might not be fully tested
by having the functionality disabled by default.

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

@@ -0,0 +1,25 @@
+package com.team2813.lib2813.feature;
+
+/** Mix-in interface for features. This should be implemented by an enum. */
+public interface FeatureIdentifier {
+
+    enum FeatureBehavior {
+        /** The feature is disabled but can be enabled via Shuffleboard. */
+        INITIALLY_DISABLED,
+
+        /** The feature is disabled but can be enabled via Shuffleboard. */
+        INITIALLY_ENABLED,
+
+        /** The feature is disabled and cannot be enabled via Shuffleboard. */
+        ALWAYS_DISABLED,
+    }
+
+    String name();
+
+    FeatureBehavior behavior();
+
+    /** Returns {@code true} iff this feature is enabled. */
+    default boolean enabled() {
+        return FeatureRegistry.getInstance().enabled(this);
+    }
+}

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

@@ -0,0 +1,162 @@
+package com.team2813.lib2813.feature;
+
+import com.team2813.lib2813.feature.FeatureIdentifier.FeatureBehavior;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.List;
+import java.util.Map;
+import java.util.Objects;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.stream.Collectors;
+import java.util.stream.Stream;
+
+/** Container for features that can be enabled at runtime. */
+final class FeatureRegistry {
+    private final Map<FeatureIdentifier, Feature> registeredFeatures = new ConcurrentHashMap<>();
+    private boolean initialized = false;
+
+    /** Package-scope constructor (for testing) */
+    FeatureRegistry() {}
+
+    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 || Stream.of(rest).anyMatch(Objects::isNull)) {
+            return () -> false;
+        }
+
+        List<Feature> features = new ArrayList<>(rest.length + 1);
+        features.add(getFeature(first));
+        Stream.of(rest).map(this::getFeature).forEach(features::add);
+
+        return () -> features.stream().allMatch(Feature::enabled);
+    }
+
+    <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).collect(Collectors.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);
+    }
+
+    boolean enabled(FeatureIdentifier id) {
+        return getFeature(id).enabled;
+    }
+
+    private Feature getFeature(FeatureIdentifier id) {
+        addToSmartDashboard();
+        return registeredFeatures.computeIfAbsent(id, Feature::new);
+    }
+
+    private synchronized void addToSmartDashboard() {
+        if (!initialized) {
+            initialized = true;
+            if (RobotBase.isSimulation()) {
+                System.out.println("Adding Features to SmartDashboard");
+                // The below should work, but I do not see it in SmartDashboard.
+                SmartDashboard.putData("Features", new SmartDashboardSendable());
+            }
+        }
+    }
+
+    private class SmartDashboardSendable implements Sendable {
+        @Override
+        public void initSendable(SendableBuilder builder) {
+            Map<FeatureIdentifier, Feature> features = FeatureRegistry.this.registeredFeatures;
+            builder.setSmartDashboardType("Feature List");
+            builder.publishConstInteger(".instance", 42);
+            builder.addStringArrayProperty(
+                    "options", () -> features.keySet().stream().map(FeatureIdentifier::name).toArray(String[]::new),
+                    null);
+        }
+    }
+
+    private static final class Feature implements Sendable {
+        private final String name;
+        private volatile boolean enabled;
+        private final boolean alwaysDisabled;
+
+        Feature(FeatureIdentifier id) {
+            String name = String.format("%s.%s", id.getClass().getName(), id.name());
+            if (name.startsWith("com.team2813.")) {
+                name = name.substring(13);
+            }
+            this.name = name;
+
+            FeatureBehavior behavior = id.behavior();
+            this.enabled = (behavior == FeatureBehavior.INITIALLY_ENABLED);
+            this.alwaysDisabled = (
+                    behavior == null || behavior == FeatureBehavior.ALWAYS_DISABLED);
+
+            System.out.printf("Adding feature %s to Shuffleboard%n", name);
+            Shuffleboard.getTab("Features").add(name, this);
+        }
+
+        boolean enabled() {
+            return enabled;
+        }
+
+        void enable(boolean enabled) {
+            if (DriverStation.isEnabled()) {
+                return; // Do not allow updating values while the Robot is enabled.
+            }
+            if (alwaysDisabled && enabled) {
+                // We shouldn't be able to get here since initSendable() called publishConstBoolean()
+                DriverStation.reportWarning(
+                        String.format("Attempt to enable feature %s which is configured as ALWAYS_DISABLED", name), false);
+                return;
+            }
+            this.enabled = enabled;
+        }
+
+        @Override
+        public void initSendable(SendableBuilder builder) {
+            builder.setSmartDashboardType("Feature");
+            builder.publishConstString(".name", name);
+            if (alwaysDisabled) {
+                builder.publishConstBoolean("enabled", false);
+            } else {
+                builder.addBooleanProperty("enabled", this::enabled, this::enable);
+            }
+        }
+    }
+}

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

@@ -0,0 +1,72 @@
+package com.team2813.lib2813.feature;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.*;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import static java.util.Objects.requireNonNull;
+
+public class Features {
+    private Features() {
+        throw new AssertionError("not instantiable");
+    }
+
+    /**
+     * Decorates the command to only run if all the provided features were enabled when the command is initialized.
+     *
+     * @param command The command to decorate
+     * @param first A feature identifier (if {@code null}, the command will not be run).
+     * @param rest Zero or more additional feature identifiers (if {@code null} or contains {@code null} values, the
+     *             command will not be run).
+     *
+     * @return the decorated command
+     * @throws NullPointerException if {@code command} is {@code null}
+     */
+    @SafeVarargs
+    public static <T extends Enum<T> & FeatureIdentifier> ConditionalCommand whenAllEnabled(
+            Command command, T first, T... rest) {
+        requireNonNull(command, "command cannot be null");
+        if (first == null || rest == null) {
+            return new ConditionalCommand(Commands.none(), Commands.none(), () -> false);
+        }
+        List<T> features = new ArrayList<>(rest.length + 1);
+        features.add(first);
+        features.addAll(Arrays.asList(rest));
+        return new FeatureControlledCommand<>(command, features);
+    }
+
+    private static class FeatureControlledCommand<T extends Enum<T> & FeatureIdentifier> extends ConditionalCommand {
+        final List<FeatureIdentifier> features;
+
+        FeatureControlledCommand(Command command, List<T> features) {
+            super(command, Commands.none(), FeatureRegistry.getInstance().asSupplier(features));
+            this.features = new ArrayList<>(features);
+        }
+
+        @Override
+        public void initSendable(SendableBuilder builder) {
+            super.initSendable(builder);
+            builder.addStringArrayProperty(
+                    "features",
+                    () -> features.stream().map(FeatureIdentifier::name).toArray(String[]::new),
+                    null);
+        }
+    }
+
+    /**
+     * Returns {@code True} iff all the given features are enabled.
+     *
+     * <p>To see determine if a single feature is enabled, use {@link FeatureIdentifier#enabled()}
+     *
+     * @param first A feature identifier (if {@code null}, the command will not be run).
+     * @param rest Zero or more additional feature identifiers (if {@code null} or contains {@code null} values, the
+     *             command will not be run).
+     */
+    @SafeVarargs
+    public static <T extends Enum<T> & FeatureIdentifier> boolean allEnabled(T first, T... rest) {
+        return FeatureRegistry.getInstance().allEnabled(first, rest);
+    }
+}