Trigger.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.command3;
 
import static org.wpilib.units.Units.Seconds;
import static org.wpilib.util.ErrorMessages.requireNonNullParam;
 
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Deque;
import java.util.EnumMap;
import java.util.List;
import java.util.Map;
import java.util.function.BooleanSupplier;
import org.wpilib.event.EventLoop;
import org.wpilib.math.filter.Debouncer;
import org.wpilib.system.Timer;
import org.wpilib.units.measure.Time;

public class Trigger implements BooleanSupplier {
  private final BooleanSupplier m_condition;
  private final EventLoop m_loop;
  private final Scheduler m_scheduler;
  private final BindingScope m_lifetimeScope;

  private Signal m_previousSignal;

  private Signal m_cachedSignal;
 
  private final Map<BindingType, List<Binding>> m_bindings = new EnumMap<>(BindingType.class);
  private final Runnable m_eventLoopCallback = this::poll;

  enum Signal {
    HIGH,
    LOW
  }

  public Trigger(Scheduler scheduler, BooleanSupplier condition) {
    this(
        requireNonNullParam(scheduler, "scheduler", "Trigger"),
        scheduler.getDefaultEventLoop(),
        requireNonNullParam(condition, "condition", "Trigger"));
  }

  public Trigger(BooleanSupplier condition) {
    this(Scheduler.getDefault(), condition);
  }

  @SuppressWarnings("this-escape")
  public Trigger(Scheduler scheduler, EventLoop loop, BooleanSupplier condition) {
    m_scheduler = requireNonNullParam(scheduler, "scheduler", "Trigger");
    m_loop = requireNonNullParam(loop, "loop", "Trigger");
    m_condition = requireNonNullParam(condition, "condition", "Trigger");
 
    // Treat robot mode scopes as globals for the purposes of trigger object lifetimes.
    // Any bindings made to the trigger will still use the appropriate scopes, but this prevents
    // bugs where a trigger created in eg `autonomousInit()` and cached, then later rebound in
    // `teleopInit()`, would get removed from the scheduler _after_ the new binding was added in
    // `teleopInit()` due to init functions running _before_ `robotPeriodic()`.
    // This does open the possibility for these triggers to never be cleaned up, but robot mode
    // changes are so infrequent that it's not a big deal.
    m_lifetimeScope =
        switch (BindingScope.createNarrowestScope(m_scheduler)) {
          case BindingScope.RobotModeScope _ -> BindingScope.Global.INSTANCE;
          case BindingScope useMe -> useMe;
        };
 
    m_scheduler.addBoundTrigger(this);
    m_loop.bindWeak(m_eventLoopCallback);
  }

  public Trigger onTrue(Command command) {
    requireNonNullParam(command, "command", "onTrue");
    addBinding(BindingType.SCHEDULE_ON_RISING_EDGE, command);
    return this;
  }

  public Trigger ifTrue(Command command) {
    requireNonNullParam(command, "command", "ifTrue");
    addBinding(BindingType.SCHEDULE_WHILE_HIGH, command);
    return this;
  }

  public Trigger onFalse(Command command) {
    requireNonNullParam(command, "command", "onFalse");
    addBinding(BindingType.SCHEDULE_ON_FALLING_EDGE, command);
    return this;
  }

  public Trigger whileTrue(Command command) {
    requireNonNullParam(command, "command", "whileTrue");
    addBinding(BindingType.RUN_WHILE_HIGH, command);
    return this;
  }

  public Trigger whileFalse(Command command) {
    requireNonNullParam(command, "command", "whileFalse");
    addBinding(BindingType.RUN_WHILE_LOW, command);
    return this;
  }

  public Trigger retryWhileTrue(Command command) {
    requireNonNullParam(command, "command", "retryWhileTrue");
    addBinding(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH, command);
    return this;
  }

  public Trigger retryWhileFalse(Command command) {
    requireNonNullParam(command, "command", "retryWhileFalse");
    addBinding(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_LOW, command);
    return this;
  }

  public Trigger toggleOnTrue(Command command) {
    requireNonNullParam(command, "command", "toggleOnTrue");
    addBinding(BindingType.TOGGLE_ON_RISING_EDGE, command);
    return this;
  }

  public Trigger toggleOnFalse(Command command) {
    requireNonNullParam(command, "command", "toggleOnFalse");
    addBinding(BindingType.TOGGLE_ON_FALLING_EDGE, command);
    return this;
  }

  @Override
  public boolean getAsBoolean() {
    return m_cachedSignal == Signal.HIGH;
  }

  public Trigger and(BooleanSupplier trigger) {
    return new Trigger(m_scheduler, m_loop, () -> getAsBoolean() && trigger.getAsBoolean());
  }

  public Trigger or(BooleanSupplier trigger) {
    return new Trigger(m_scheduler, m_loop, () -> getAsBoolean() || trigger.getAsBoolean());
  }

  public Trigger negate() {
    return new Trigger(m_scheduler, m_loop, () -> !getAsBoolean());
  }

  public Trigger debounce(Time duration) {
    return debounce(duration, Debouncer.DebounceType.RISING);
  }

  public Trigger debounce(Time duration, Debouncer.DebounceType type) {
    var debouncer = new Debouncer(duration.in(Seconds), type);
    return new Trigger(m_scheduler, m_loop, () -> debouncer.calculate(getAsBoolean()));
  }

  public Trigger risingEdge() {
    return new Trigger(
        m_scheduler, m_loop, () -> m_cachedSignal == Signal.HIGH && m_previousSignal == Signal.LOW);
  }

  public Trigger fallingEdge() {
    return new Trigger(
        m_scheduler, m_loop, () -> m_cachedSignal == Signal.LOW && m_previousSignal == Signal.HIGH);
  }

  public Trigger multiPress(int pressCount, Time duration) {
    requireNonNullParam(duration, "duration", "multiTap");
 
    // Short circuits to avoid unnecessary state tracking and object allocations
    if (duration.baseUnitMagnitude() <= 0) {
      // A nonpositive window size can never be met
      return new Trigger(m_scheduler, m_loop, () -> false);
    } else if (pressCount <= 0) {
      // A nonpositive press count is always met
      return new Trigger(m_scheduler, m_loop, () -> true);
    }
 
    final double durationSeconds = duration.in(Seconds);
 
    return new Trigger(
        m_scheduler,
        m_loop,
        new BooleanSupplier() {
          // Note: unlike EdgeCounterFilter, this implementation tracks timestamps directly
          // and remains high even if the signal is low, just as long as the number of rising edges
          // meets the criteria. EdgeCounterFilter is only high when the requisite number of edges
          // have been seen _and_ the signal is high.
          private final Deque<Double> m_timestamps = new ArrayDeque<>();
          private boolean m_risingEdgeOccurred = false;
 
          @Override
          public boolean getAsBoolean() {
            if (m_cachedSignal == Signal.HIGH && m_previousSignal != Signal.HIGH) {
              if (!m_risingEdgeOccurred) {
                m_timestamps.addLast(Timer.getTimestamp());
                m_risingEdgeOccurred = true;
              }
            } else if (m_cachedSignal != Signal.HIGH) {
              m_risingEdgeOccurred = false;
            }
 
            double currentTime = Timer.getTimestamp();
            while (!m_timestamps.isEmpty()
                && currentTime - m_timestamps.peekFirst() > durationSeconds + 1e-9) {
              m_timestamps.removeFirst();
            }
 
            return m_timestamps.size() >= pressCount;
          }
        });
  }
 
  private void poll() {
    // Clear bindings that no longer need to run
    // This should always be checked, regardless of signal change, since bindings may be scoped
    // and those scopes may become inactive.
    clearStaleBindings();
 
    m_previousSignal = m_cachedSignal;
    m_cachedSignal = readSignal();
 
    // Always attempt to schedule bindings based on the current signal
    if (m_cachedSignal == Signal.HIGH) {
      scheduleBindings(BindingType.SCHEDULE_WHILE_HIGH);
      scheduleBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH);
    } else if (m_cachedSignal == Signal.LOW) {
      scheduleBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_LOW);
    }
 
    if (m_cachedSignal == m_previousSignal) {
      // No change in the signal. Nothing to do
      return;
    }
 
    if (m_cachedSignal == Signal.HIGH) {
      // Signal is now high when it wasn't before - a rising edge
      scheduleBindings(BindingType.SCHEDULE_ON_RISING_EDGE);
      scheduleBindings(BindingType.RUN_WHILE_HIGH);
      cancelBindings(BindingType.RUN_WHILE_LOW);
      cancelBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_LOW);
      toggleBindings(BindingType.TOGGLE_ON_RISING_EDGE);
    }
 
    if (m_cachedSignal == Signal.LOW) {
      // Signal is now low when it wasn't before - a falling edge
      scheduleBindings(BindingType.SCHEDULE_ON_FALLING_EDGE);
      scheduleBindings(BindingType.RUN_WHILE_LOW);
      cancelBindings(BindingType.RUN_WHILE_HIGH);
      cancelBindings(BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH);
      toggleBindings(BindingType.TOGGLE_ON_FALLING_EDGE);
    }
  }
 
  private Signal readSignal() {
    if (m_condition.getAsBoolean()) {
      return Signal.HIGH;
    } else {
      return Signal.LOW;
    }
  }

  private void clearStaleBindings() {
    m_bindings.forEach(
        (_bindingType, bindings) -> {
          for (var iterator = bindings.iterator(); iterator.hasNext(); ) {
            var binding = iterator.next();
            if (binding.scope().active()) {
              // This binding's scope is still active, leave it running
              continue;
            }
 
            // The scope is no longer active. Remove the binding and immediately cancel its command.
            iterator.remove();
            m_scheduler.cancel(binding.command());
          }
        });
  }

  private void scheduleBindings(BindingType bindingType) {
    m_bindings.getOrDefault(bindingType, List.of()).forEach(m_scheduler::schedule);
  }

  private void cancelBindings(BindingType bindingType) {
    m_bindings
        .getOrDefault(bindingType, List.of())
        .forEach(binding -> m_scheduler.cancel(binding.command()));
  }

  private void toggleBindings(BindingType bindingType) {
    m_bindings
        .getOrDefault(bindingType, List.of())
        .forEach(
            binding -> {
              var command = binding.command();
              if (m_scheduler.isScheduledOrRunning(command)) {
                m_scheduler.cancel(command);
              } else {
                m_scheduler.schedule(binding);
              }
            });
  }

  Signal getCachedSignal() {
    return m_cachedSignal;
  }

  Signal getPreviousSignal() {
    return m_previousSignal;
  }
 
  // package-private for Scheduler
  boolean isScopeActive() {
    return m_lifetimeScope.active();
  }
 
  // package-private for testing
  void addBinding(BindingScope scope, BindingType bindingType, Command command) {
    // Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
    //       stack frame filtering and modification.
    m_bindings
        .computeIfAbsent(bindingType, _k -> new ArrayList<>())
        .add(new Binding(scope, bindingType, command, new Throwable()));
  }
 
  private void addBinding(BindingType bindingType, Command command) {
    var scope = BindingScope.createNarrowestScope(m_scheduler);
    addBinding(scope, bindingType, command);
  }
}