diff --git a/lib/src/main/java/com/team2813/lib2813/feature/FeatureIdentifier.java b/lib/src/main/java/com/team2813/lib2813/feature/FeatureIdentifier.java new file mode 100644 index 0000000..8a806eb --- /dev/null +++ b/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); + } +} diff --git a/lib/src/main/java/com/team2813/lib2813/feature/FeatureRegistry.java b/lib/src/main/java/com/team2813/lib2813/feature/FeatureRegistry.java new file mode 100644 index 0000000..6162a87 --- /dev/null +++ b/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 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 & FeatureIdentifier> BooleanSupplier asSupplier(T first, T... rest) { + if (first == null || rest == null || Stream.of(rest).anyMatch(Objects::isNull)) { + return () -> false; + } + + List 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); + } + + & FeatureIdentifier> BooleanSupplier asSupplier(Collection featureIdentifiers) { + if (featureIdentifiers == null || featureIdentifiers.stream().anyMatch(Objects::isNull)) { + return () -> false; + } + + List features = featureIdentifiers.stream().map(this::getFeature).collect(Collectors.toList()); + + return () -> features.stream().allMatch(Feature::enabled); + } + + @SafeVarargs + final & 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 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); + } + } + } +} diff --git a/lib/src/main/java/com/team2813/lib2813/feature/Features.java b/lib/src/main/java/com/team2813/lib2813/feature/Features.java new file mode 100644 index 0000000..c7659c1 --- /dev/null +++ b/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 & 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 features = new ArrayList<>(rest.length + 1); + features.add(first); + features.addAll(Arrays.asList(rest)); + return new FeatureControlledCommand<>(command, features); + } + + private static class FeatureControlledCommand & FeatureIdentifier> extends ConditionalCommand { + final List features; + + FeatureControlledCommand(Command command, List 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. + * + *

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 & FeatureIdentifier> boolean allEnabled(T first, T... rest) { + return FeatureRegistry.getInstance().allEnabled(first, rest); + } +}