Scheduler.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.Milliseconds;
import static org.wpilib.units.Units.Nanoseconds;
 
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.HashSet;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
import java.util.SequencedMap;
import java.util.SequencedSet;
import java.util.Set;
import java.util.Stack;
import java.util.function.Consumer;
import java.util.stream.Collectors;
import org.wpilib.annotation.NoDiscard;
import org.wpilib.command3.Scheduler.ScheduleResult.AlreadyRunning;
import org.wpilib.command3.Scheduler.ScheduleResult.LowerPriorityThanQueuedCommand;
import org.wpilib.command3.Scheduler.ScheduleResult.LowerPriorityThanRunningCommand;
import org.wpilib.command3.Scheduler.ScheduleResult.RequiresUnsafeMechanisms;
import org.wpilib.command3.Scheduler.ScheduleResult.Success;
import org.wpilib.command3.button.CommandGenericHID;
import org.wpilib.command3.proto.SchedulerProto;
import org.wpilib.event.EventLoop;
import org.wpilib.framework.TimedRobot;
import org.wpilib.system.RobotController;
import org.wpilib.util.ErrorMessages;
import org.wpilib.util.protobuf.ProtobufSerializable;

public final class Scheduler implements ProtobufSerializable {
  private final Map<Mechanism, List<Binding>> m_defaultCommandBindings = new LinkedHashMap<>();

  private final Collection<Binding> m_activeBindings = new ArrayList<>();

  private final Set<Trigger> m_boundTriggers = new HashSet<>();

  private final SequencedSet<CommandState> m_queuedToRun = new LinkedHashSet<>();

  private final SequencedMap<Command, CommandState> m_runningCommands = new LinkedHashMap<>();

  private final Stack<CommandState> m_currentCommandAncestry = new Stack<>();

  private final List<PeriodicCallback> m_periodicCallbacks = new ArrayList<>();

  private final EventLoop m_eventLoop = new EventLoop();

  private final ContinuationScope m_scope = new ContinuationScope("coroutine commands");

  private record PeriodicCallback(BindingScope scope, Coroutine coroutine) {}
 
  // Telemetry
  public static final SchedulerProto proto = new SchedulerProto();
 
  private double m_lastRunTimeMs = -1;
 
  private final Set<Consumer<? super SchedulerEvent>> m_eventListeners = new LinkedHashSet<>();

  private static final Scheduler s_defaultScheduler = new Scheduler();

  @NoDiscard
  public static Scheduler getDefault() {
    return s_defaultScheduler;
  }

  @NoDiscard
  public static Scheduler createIndependentScheduler() {
    return new Scheduler();
  }

  private Scheduler() {}

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  public void setDefaultCommand(Mechanism mechanism, Command defaultCommand) {
    if (!defaultCommand.requires(mechanism)) {
      throw new IllegalArgumentException(
          "A mechanism's default command must require that mechanism");
    }
 
    if (defaultCommand.requirements().size() > 1) {
      throw new IllegalArgumentException(
          "A mechanism's default command cannot require other mechanisms");
    }
 
    var currentCommand = currentCommand();
    BindingScope scope = BindingScope.createNarrowestScope(this);
 
    var binding =
        new Binding(
            scope, BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH, defaultCommand, new Throwable());
 
    var currentDefaultCommand = getDefaultCommandFor(mechanism);
    m_defaultCommandBindings.computeIfAbsent(mechanism, k -> new ArrayList<>()).add(binding);
 
    if (currentCommand != null && currentCommand != currentDefaultCommand) {
      // User called `setDefaultCommand` inside another command.
      // Immediately reprocess the default commands for this mechanism to ensure it's in sync with
      // the rest of the commands in the scheduler. This is required because we normally schedule
      // the default commands at the start of the scheduler `run()` method, so the new default
      // command wouldn't be handled until the next run (ie, the previous default command would
      // still be active for the current iteration)
      //
      // Note that we cannot do this if the current default command is the caller because commands
      // cannot be canceled while mounted.
      processDefaultCommands(mechanism);
    }
  }

  public Command getDefaultCommandFor(Mechanism mechanism) {
    var bindings = m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
    if (bindings.isEmpty()) {
      return null;
    }
 
    return bindings.getLast().command();
  }
 
  // package-private helper for unit test access
  List<Binding> getDefaultCommandBindingsFor(Mechanism mechanism) {
    return m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
  }

  public void sideload(Consumer<Coroutine> callback) {
    var coroutine = new Coroutine(this, m_scope, callback);
    // Sideloaded functions shouldn't be canceled if a command can't be forked. The default behavior
    // should allow user code the opportunity to recover from a failure. This differs from
    // coroutines issued to commands, which should fail by default to prevent unexpected robot state
    coroutine.setCancelOnForkFailure(false);
 
    var scope = BindingScope.createNarrowestScope(this);
    m_periodicCallbacks.add(new PeriodicCallback(scope, coroutine));
  }

  public void addPeriodic(Runnable callback) {
    sideload(
        coroutine -> {
          while (true) {
            callback.run();
            coroutine.yield();
          }
        });
  }

  public sealed interface ScheduleResult {
    Command command();

    boolean successful();

    sealed interface Successful extends ScheduleResult {
      @Override
      default boolean successful() {
        return true;
      }

      int runId();
    }

    sealed interface Failure extends ScheduleResult {
      @Override
      default boolean successful() {
        return false;
      }
    }

    record Success(Command command, int runId) implements Successful {}

    record AlreadyRunning(Command command, int runId) implements Successful {
      @Override
      public int runId() {
        return runId;
      }
    }

    record LowerPriorityThanRunningCommand(Command command, Command alreadyRunning)
        implements Failure {}

    record LowerPriorityThanQueuedCommand(Command command, Command queuedCommand)
        implements Failure {}

    record RequiresUnsafeMechanisms(Command command, Collection<Mechanism> unsafeMechanisms)
        implements Failure {}
  }

  @NoDiscard
  public ScheduleResult isSchedulable(Command command) {
    ErrorMessages.requireNonNullParam(command, "command", "isSchedulable");
 
    int id = runId(command);
    if (id != 0) {
      return new AlreadyRunning(command, id);
    }
 
    if (!RobotStateFetcher.getFetcher().isEnabled()) {
      var uncontrollables =
          command.requirements().stream()
              .filter(mechanism -> !mechanism.controllableDuringDisabled())
              .collect(Collectors.toSet());
 
      if (!uncontrollables.isEmpty()) {
        return new RequiresUnsafeMechanisms(command, uncontrollables);
      }
    }
 
    Set<Command> ancestry = new HashSet<>();
    for (var state = currentState(); state != null; state = m_runningCommands.get(state.parent())) {
      ancestry.add(state.command());
    }
 
    return isSchedulableFromPriority(command, ancestry);
  }
 
  // similar to isSchedulable(Command), but used internally for processing bindings
  // that may have command scopes.
  private ScheduleResult isSchedulable(Binding binding) {
    var command = binding.command();
 
    int id = runId(command);
    if (id != 0) {
      return new AlreadyRunning(command, id);
    }
 
    if (!RobotStateFetcher.getFetcher().isEnabled()) {
      var uncontrollables =
          command.requirements().stream()
              .filter(mechanism -> !mechanism.controllableDuringDisabled())
              .collect(Collectors.toSet());
 
      if (!uncontrollables.isEmpty()) {
        return new RequiresUnsafeMechanisms(command, uncontrollables);
      }
    }
 
    Set<Command> ancestry = new HashSet<>();
    if (binding.scope() instanceof BindingScope.ForCommand(Scheduler _, Command parent)) {
      for (var state = m_runningCommands.get(parent);
          state != null;
          state = m_runningCommands.get(state.parent())) {
        ancestry.add(state.command());
      }
    } else {
      for (var state = currentState();
          state != null;
          state = m_runningCommands.get(state.parent())) {
        ancestry.add(state.command());
      }
    }
 
    return isSchedulableFromPriority(command, ancestry);
  }
 
  private Scheduler.ScheduleResult isSchedulableFromPriority(
      Command command, Set<Command> ancestry) {
    var running = m_runningCommands.values();
    Command conflict = lowerPriorityThanConflictingCommands(command, ancestry, running);
    if (conflict != null) {
      return new LowerPriorityThanRunningCommand(command, conflict);
    }
 
    conflict = lowerPriorityThanConflictingCommands(command, ancestry, m_queuedToRun);
    if (conflict != null) {
      return new LowerPriorityThanQueuedCommand(command, conflict);
    }
 
    return new Success(command, 0);
  }

  // Implementation detail: a child command will immediately start running when scheduled by a
  // parent command, skipping the queue entirely. This avoids dead loop cycles where a parent
  // schedules a child, appending it to the queue, then waits for the next cycle to pick it up and
  // start it. With deeply nested commands, dead loops could quickly to add up and cause the
  // innermost commands that actually _do_ something to start running hundreds of milliseconds after
  // their root ancestor was scheduled.
  public ScheduleResult schedule(Command command) {
    // Get the narrowest binding scope.
    // This prevents commands from outliving the opmodes that scheduled them, or from outliving
    // their parents (eg if someone writes a command that manually calls schedule(Command) instead
    // of using triggers to do so).
    BindingScope scope = BindingScope.createNarrowestScope(this);
 
    // Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
    //       stack frame filtering and modification.
    var binding = new Binding(scope, BindingType.IMMEDIATE, command, new Throwable());
 
    return schedule(binding);
  }
 
  // Package-private for use by Trigger
  ScheduleResult schedule(Binding binding) {
    var command = binding.command();
 
    var result = isSchedulable(binding);
    if (!(result instanceof Success)) {
      // We check specifically for Success, instead of `successful()`, because only a Success
      // indicates that the command can actually go through the scheduling process. AlreadyRunning
      // means what it says on the tin, and it would be incorrect to run the command back through
      // the scheduling process.
      return result;
    }
 
    if (!(binding.scope() instanceof BindingScope.ForCommand)) {
      // Track this binding so we can disable it when it's out of scope.
      // Note that, even though triggers can clean themselves up, commands that are manually
      // scheduled cannot do the same, so we have to track them in the scheduler.
 
      // We don't bother tracking command-scoped bindings; the bound command will already be
      // cleaned up in the same cycle that the parent command exits, so this would attempt to
      // double-cancel a child command across two cycles (once when the parent exits, and then in
      // the next cycle in cancelStaleBindings). That would cause problems if the command object
      // is immediately rescheduled as cancelStaleBindings would immediately cancel the new run
      m_activeBindings.add(binding);
    }
 
    // Evict conflicting on-deck commands
    // We check above if the input command is lower priority than any of these,
    // so at this point we're guaranteed to be >= priority than anything already on deck
    evictConflictingOnDeckCommands(command);
 
    // If the binding is scoped to a particular command, that command is the parent. If we're in the
    // middle of a run cycle and running commands, the parent is whatever command is currently
    // running. Otherwise, there is no parent command.
    var parentCommand =
        binding.scope() instanceof BindingScope.ForCommand scope
            ? scope.command()
            : currentCommand();
    var state = new CommandState(command, parentCommand, buildCoroutine(command), binding);
 
    emitScheduledEvent(command);
 
    if (currentState() != null) {
      // Scheduling a child command while running. Start it immediately instead of waiting a full
      // cycle. This prevents issues with deeply nested command groups taking many scheduler cycles
      // to start running the commands that actually _do_ things
      evictConflictingRunningCommands(state);
      m_runningCommands.put(command, state);
      runCommand(state);
    } else {
      // Scheduling outside a command, add it to the pending set. If it's not overridden by another
      // conflicting command being scheduled in the same scheduler loop, it'll be promoted and
      // start to run when #runCommands() is called
      m_queuedToRun.add(state);
    }
 
    return new Success(command, state.id());
  }

  private Command lowerPriorityThanConflictingCommands(
      Command command, Collection<Command> ancestry, Collection<CommandState> checkAgainst) {
    int maxAncestorPriority = command.priority();
    for (Command ancestor : ancestry) {
      maxAncestorPriority = Math.max(maxAncestorPriority, ancestor.priority());
    }
 
    // Check for conflicts with the commands that are already running
    for (var state : checkAgainst) {
      if (ancestry.contains(state.command())) {
        // Can't conflict with an ancestor command
        continue;
      }
 
      var c = state.command();
      if (c.conflictsWith(command) && maxAncestorPriority < effectivePriority(state)) {
        return c;
      }
    }
 
    return null;
  }

  private int effectivePriority(CommandState state) {
    int max = state.command().priority();
    for (var parent = state.parent(); parent != null; parent = state.parent()) {
      state = m_runningCommands.get(parent);
      max = Math.max(max, parent.priority());
    }
    return max;
  }
 
  private void evictConflictingOnDeckCommands(Command command) {
    for (var iterator = m_queuedToRun.iterator(); iterator.hasNext(); ) {
      var scheduledState = iterator.next();
      var scheduledCommand = scheduledState.command();
      if (scheduledCommand.conflictsWith(command)) {
        // Remove the lower priority conflicting command from the on deck commands.
        // We don't need to call removeOrphanedChildren here because it hasn't started yet,
        // meaning it hasn't had a chance to schedule any children
        iterator.remove();
        emitInterruptedEvent(scheduledCommand, command);
        emitCanceledEvent(scheduledCommand);
      }
    }
  }

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  private void evictConflictingRunningCommands(CommandState incomingState) {
    // The set of root states with which the incoming state conflicts but does not inherit from
    Set<CommandState> conflictingRootStates =
        m_runningCommands.values().stream()
            .filter(state -> incomingState.command().conflictsWith(state.command()))
            .filter(state -> !isAncestorOf(state.command(), incomingState))
            .map(
                state -> {
                  // Find the highest level ancestor of the conflicting command from which the
                  // incoming state does _not_ inherit. If they're totally unrelated, this will
                  // get the very root ancestor; otherwise, it'll return a direct sibling of the
                  // incoming command
                  CommandState root = state;
                  while (root.parent() != null && root.parent() != incomingState.parent()) {
                    root = m_runningCommands.get(root.parent());
                  }
                  return root;
                })
            .collect(Collectors.toSet());
 
    // Cancel the root commands
    for (var conflictingState : conflictingRootStates) {
      emitInterruptedEvent(conflictingState.command(), incomingState.command());
      cancel(conflictingState.command());
    }
  }

  @SuppressWarnings({"PMD.CompareObjectsWithEquals", "PMD.SimplifyBooleanReturns"})
  private boolean isAncestorOf(Command ancestor, CommandState state) {
    if (state.parent() == null) {
      // No parent, cannot inherit
      return false;
    }
    if (!m_runningCommands.containsKey(ancestor)) {
      // The given ancestor isn't running
      return false;
    }
    if (state.parent() == ancestor) {
      // Direct child
      return true;
    }
    // Check if the command's parent inherits from the given ancestor
    return m_runningCommands.values().stream()
        .filter(s -> state.parent() == s.command())
        .anyMatch(s -> isAncestorOf(ancestor, s));
  }

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  public void cancel(Command command) {
    var currentState = currentState();
    if (currentState != null && command == currentState.command()) {
      currentState.coroutine().requestCancellation(); // yields internally
      return;
    }
 
    boolean running = isRunning(command);
 
    // Evict the command. The next call to run() will schedule the default command for all its
    // required mechanisms, unless another command requiring those mechanisms is scheduled between
    // calling cancel() and calling run()
    m_runningCommands.remove(command);
    boolean queued = m_queuedToRun.removeIf(state -> state.command() == command);
 
    if (running) {
      // Only run the hooks if the command was running. If it was on deck or not
      // even in the scheduler at the time, then there's nothing to do
      // Always run onExit first, in case cancellation has special behavior that overrides
      // standard exit logic.
      command.onExit();
      command.onCancel();
    }
 
    if (running || queued) {
      // Emit a cancellation event only if the given command was in the scheduler
      emitCanceledEvent(command);
    }
 
    // Clean up any orphaned child commands; their lifespan must not exceed the parent's
    removeOrphanedChildren(command);
  }

  public void run() {
    final long startNanos = RobotController.getTime();
 
    // Cancel any commands with stale binding scopes
    cancelStaleBindings();
 
    // Unbind any triggers with stale creation scopes.
    // This allows triggers that can never be used again to be garbage collected to reduce
    // memory usage and avoid potential OOMs from poorly written user code.
    unbindStaleTriggers();
 
    // If the robot is disabled, cancel any commands that require uncontrollable mechanisms
    cancelCommandsThatCannotRunInDisabled();
 
    // Sideloads may change some state that affects triggers. Run them first.
    runPeriodicSideloads();
 
    // Poll triggers next to schedule and cancel commands
    m_eventLoop.poll();
 
    // Schedule default commands for any mechanisms that don't have a running command and didn't
    // have a new command scheduled by a sideload function or a trigger
    scheduleDefaultCommands();
 
    // Move all scheduled commands to the running set
    promoteScheduledCommands();
 
    // Run every command in order until they call Coroutine.yield() or exit
    runCommands();
 
    final long endNanos = RobotController.getTime();
    m_lastRunTimeMs = Milliseconds.convertFrom(endNanos - startNanos, Nanoseconds);
  }
 
  private void cancelStaleBindings() {
    for (var iterator = m_activeBindings.iterator(); iterator.hasNext(); ) {
      var binding = iterator.next();
      if (binding.scope().active()) {
        continue;
      }
      cancel(binding.command());
      iterator.remove();
    }
  }
 
  private void unbindStaleTriggers() {
    // Remove strong references to any triggers that have gone stale. This allows triggers to be
    // garbage collected if they're not referenced outside the scope that created them. We don't
    // clear any command bindings or unbind it from the event loop; otherwise, cached triggers
    // would stop being updated until new command bindings are added.
    for (var iterator = m_boundTriggers.iterator(); iterator.hasNext(); ) {
      var trigger = iterator.next();
      if (!trigger.isScopeActive()) {
        iterator.remove();
      }
    }
  }
 
  private void cancelCommandsThatCannotRunInDisabled() {
    if (RobotStateFetcher.getFetcher().isEnabled()) {
      // Nothing to do if the robot is enabled
      return;
    }
 
    List<Command> commandsToCancel = new ArrayList<>();
 
    for (var runningState : m_runningCommands.values()) {
      var command = runningState.command();
      boolean canRun = true;
      for (var mechanism : command.requirements()) {
        if (!mechanism.controllableDuringDisabled()) {
          canRun = false;
          break;
        }
      }
 
      if (canRun) {
        continue;
      }
 
      commandsToCancel.add(getRoot(command));
    }
 
    for (var queuedState : m_queuedToRun) {
      var command = queuedState.command();
 
      boolean canRun = true;
      for (var mechanism : command.requirements()) {
        if (!mechanism.controllableDuringDisabled()) {
          canRun = false;
          break;
        }
      }
 
      if (canRun) {
        continue;
      }
 
      commandsToCancel.add(command);
    }
 
    for (var command : commandsToCancel) {
      cancel(command);
    }
  }

  // package-private for Trigger
  void addBoundTrigger(Trigger trigger) {
    m_boundTriggers.add(trigger);
  }

  // package-private for Trigger
  void removeBoundTrigger(Trigger trigger) {
    m_boundTriggers.remove(trigger);
  }
 
  private void promoteScheduledCommands() {
    // Clear any commands that conflict with the scheduled set
    for (var queuedState : m_queuedToRun) {
      evictConflictingRunningCommands(queuedState);
    }
 
    // Move any scheduled commands to the running set
    for (var queuedState : m_queuedToRun) {
      m_runningCommands.put(queuedState.command(), queuedState);
    }
 
    // Clear the set of on-deck commands,
    // since we just put them all into the set of running commands
    m_queuedToRun.clear();
  }
 
  private void runPeriodicSideloads() {
    for (var iterator = m_periodicCallbacks.iterator(); iterator.hasNext(); ) {
      PeriodicCallback callback = iterator.next();
      if (!callback.scope().active()) {
        // The callback's enclosing scope exited - remove without running it and move on
        iterator.remove();
        continue;
      }
 
      // Update periodic callbacks
      Coroutine coroutine = callback.coroutine();
      if (coroutine.inUnmountableState()) {
        // Nothing to do
        continue;
      }
 
      coroutine.mount();
      try {
        coroutine.runToYieldPoint();
      } finally {
        Continuation.mountContinuation(null);
      }
 
      if (coroutine.isDone()
          || coroutine.isInterruptRequested()
          || coroutine.isCancellationRequested()) {
        // Callback finished or requested early termination - remove it from the list
        iterator.remove();
      }
    }
  }
 
  private void runCommands() {
    // TODO: Track command roots and run the commands in each root in reverse order, but run the
    //       roots in insertion order
    // Tick every command that hasn't been completed yet
    // Run in reverse so parent commands can resume in the same loop cycle an awaited child command
    // completes. Otherwise, parents could only resume on the next loop cycle, introducing a delay
    // at every layer of nesting.
    for (var state : List.copyOf(m_runningCommands.values()).reversed()) {
      runCommand(state);
    }
  }

  @SuppressWarnings("PMD.AvoidCatchingGenericException")
  private void runCommand(CommandState state) {
    final var command = state.command();
    final var coroutine = state.coroutine();
 
    if (coroutine.inUnmountableState()) {
      // Nothing to do
      return;
    }
 
    if (!m_runningCommands.containsKey(command)) {
      // Probably canceled by an owning composition, do not run
      return;
    }
 
    var previousState = currentState();
 
    m_currentCommandAncestry.push(state);
    long startNanos = RobotController.getTime();
    emitMountedEvent(command);
    coroutine.mount();
    try {
      coroutine.runToYieldPoint();
    } catch (RuntimeException e) {
      // Command encountered an uncaught exception.
      handleCommandException(state, e);
    } finally {
      long endNanos = RobotController.getTime();
      double elapsedMs = Milliseconds.convertFrom(endNanos - startNanos, Nanoseconds);
      state.setLastRuntimeMs(elapsedMs);
 
      if (state.equals(currentState())) {
        // Remove the command we just ran from the top of the stack
        m_currentCommandAncestry.pop();
      }
 
      if (previousState != null) {
        // Remount the parent command, if there is one
        previousState.coroutine().mount();
      } else {
        Continuation.mountContinuation(null);
      }
    }
 
    if (coroutine.isCancellationRequested()) {
      cancel(command);
    } else if (coroutine.isDone()) {
      handleCommandCompletion(command);
    } else if (coroutine.getForkResult() != null) {
      handleCoroutineIRQ(coroutine.getForkResult(), command);
    } else {
      // Yielded
      emitYieldedEvent(command);
    }
  }
 
  private void handleCommandCompletion(Command command) {
    command.onExit();
    emitCompletedEvent(command);
    m_runningCommands.remove(command);
    removeOrphanedChildren(command);
  }
 
  private void handleCoroutineIRQ(Coroutine.ForkResult failure, Command command) {
    Command root = getRoot(command); // capture the root command before modifying scheduler state
    m_currentCommandAncestry.clear();
    emitForkFailureEvent(command, failure.getFailedCommands());
    cancel(root);
    Continuation.mountContinuation(null);
  }

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  private void handleCommandException(CommandState state, RuntimeException e) {
    var command = state.command();
 
    // Fetch the root command
    // (needs to be done before removing the failed command from the running set)
    final Command root = getRoot(command);
 
    // Remove it from the running set.
    m_runningCommands.remove(command);
 
    // Intercept the exception, inject stack frames from the schedule site, and rethrow it
    var binding = state.binding();
    e.setStackTrace(
        CommandTraceHelper.modifyTrace(
            e.getStackTrace(), binding.stackTraceStore().getStackTrace()));
    emitCompletedWithErrorEvent(command, e);
 
    // Clean up child commands after emitting the event so child Canceled events are emitted
    // after the parent's CompletedWithError
    removeOrphanedChildren(command);
 
    // Bubble up to the root and cancel all commands between the root and this one
    // Note: Because we remove the command from the running set above, we still need to
    //       clean up all the failed command's children
    if (root != null && root != command) {
      cancel(root);
    }
 
    // Then rethrow the exception
    throw e;
  }

  private CommandState currentState() {
    if (m_currentCommandAncestry.isEmpty()) {
      // Avoid EmptyStackException
      return null;
    }
 
    return m_currentCommandAncestry.peek();
  }

  public Command currentCommand() {
    var state = currentState();
    if (state == null) {
      return null;
    }
 
    return state.command();
  }
 
  private void scheduleDefaultCommands() {
    m_defaultCommandBindings.keySet().forEach(this::processDefaultCommands);
  }
 
  private void processDefaultCommands(Mechanism mechanism) {
    var bindings = m_defaultCommandBindings.get(mechanism);
 
    // Remove default command bindings that are no longer active.
    // If a default command is running when its scope goes inactive, also be sure to cancel it.
    bindings.removeIf(
        b -> {
          if (!b.scope().active()) {
            cancel(b.command());
            return true;
          }
          return false;
        });
 
    if (bindings.isEmpty()) {
      // Nothing to do. No active bindings remain.
      return;
    }
 
    // Cancel any default command except the narrowest-scoped one (the last binding in the list)
    for (int i = 0; i < bindings.size() - 1; i++) {
      Command widerScopeDefaultCommand = bindings.get(i).command();
      cancel(widerScopeDefaultCommand);
    }
 
    if (!RobotStateFetcher.getFetcher().isEnabled() && !mechanism.controllableDuringDisabled()) {
      // Default commands can never be scheduled if the robot is disabled and the mechanism is not
      // controllable during disabled, so there's no point in attempting to queue the default
      // command
      return;
    }
 
    // Check if the mechanism is currently in use. We can queue the default command if it's not.
    for (Command runningCommand : m_runningCommands.keySet()) {
      if (runningCommand.requires(mechanism)) {
        return;
      }
    }
    for (CommandState queuedState : m_queuedToRun) {
      if (queuedState.command().requires(mechanism)) {
        return;
      }
    }
 
    // Nothing currently running or queued that needs this mechanism. Queue the default command.
    var defaultCommand = bindings.getLast();
    schedule(defaultCommand);
  }

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  private void removeOrphanedChildren(Command parent) {
    m_runningCommands.entrySet().stream()
        .filter(e -> e.getValue().parent() == parent)
        .toList() // copy to an intermediate list to avoid concurrent modification
        .forEach(e -> cancel(e.getKey()));
  }

  private Coroutine buildCoroutine(Command command) {
    return new Coroutine(this, m_scope, command::run);
  }

  public boolean isRunning(Command command) {
    return m_runningCommands.containsKey(command);
  }

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  public boolean isScheduled(Command command) {
    return m_queuedToRun.stream().anyMatch(state -> state.command() == command);
  }

  public boolean isScheduledOrRunning(Command command) {
    return isScheduled(command) || isRunning(command);
  }

  public Collection<Command> getRunningCommands() {
    return List.copyOf(m_runningCommands.keySet());
  }

  public List<Command> getRunningCommandsFor(Mechanism mechanism) {
    return m_runningCommands.keySet().stream()
        .filter(command -> command.requires(mechanism))
        .toList();
  }

  public void cancelAll() {
    for (var onDeckIter = m_queuedToRun.iterator(); onDeckIter.hasNext(); ) {
      var state = onDeckIter.next();
      onDeckIter.remove();
      emitCanceledEvent(state.command());
    }
 
    for (var liveIter = m_runningCommands.entrySet().iterator(); liveIter.hasNext(); ) {
      var entry = liveIter.next();
      liveIter.remove();
      Command canceledCommand = entry.getKey();
      // Always run onExit first, in case cancellation has special behavior that overrides
      // standard exit logic.
      canceledCommand.onExit();
      canceledCommand.onCancel();
      emitCanceledEvent(canceledCommand);
    }
  }

  @NoDiscard
  public EventLoop getDefaultEventLoop() {
    return m_eventLoop;
  }

  @NoDiscard
  public Collection<Command> getQueuedCommands() {
    return m_queuedToRun.stream().map(CommandState::command).toList();
  }
 
  private Command getRoot(Command command) {
    Command root = command;
    Command parent = getParentOf(command);
    while (parent != null) {
      root = parent;
      parent = getParentOf(parent);
    }
 
    return root;
  }

  public Command getParentOf(Command command) {
    var state = m_runningCommands.get(command);
    if (state == null) {
      return null;
    }
    return state.parent();
  }

  public double lastCommandRuntimeMs(Command command) {
    if (m_runningCommands.containsKey(command)) {
      return m_runningCommands.get(command).lastRuntimeMs();
    } else {
      return -1;
    }
  }

  public double totalRuntimeMs(Command command) {
    if (m_runningCommands.containsKey(command)) {
      return m_runningCommands.get(command).totalRuntimeMs();
    } else {
      // Not running; no data
      return -1;
    }
  }

  @NoDiscard
  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  public int runId(Command command) {
    if (m_runningCommands.containsKey(command)) {
      return m_runningCommands.get(command).id();
    }
 
    // Check scheduled commands
    for (var scheduled : m_queuedToRun) {
      if (scheduled.command() == command) {
        return scheduled.id();
      }
    }
 
    return 0;
  }

  @NoDiscard
  public double lastRuntimeMs() {
    return m_lastRunTimeMs;
  }
 
  // Event-base telemetry and helpers. The static factories are for convenience to automatically
  // set the timestamp instead of littering RobotController.getTime() everywhere.
 
  private void emitScheduledEvent(Command command) {
    var event = new SchedulerEvent.Scheduled(command, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitMountedEvent(Command command) {
    var event = new SchedulerEvent.Mounted(command, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitYieldedEvent(Command command) {
    var event = new SchedulerEvent.Yielded(command, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitCompletedEvent(Command command) {
    var event = new SchedulerEvent.Completed(command, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitCompletedWithErrorEvent(Command command, Throwable error) {
    var event = new SchedulerEvent.CompletedWithError(command, error, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitCanceledEvent(Command command) {
    var event = new SchedulerEvent.Canceled(command, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitInterruptedEvent(Command command, Command interrupter) {
    var event = new SchedulerEvent.Interrupted(command, interrupter, RobotController.getTime());
    emitEvent(event);
  }
 
  private void emitForkFailureEvent(Command command, List<ScheduleResult.Failure> failures) {
    var event = new SchedulerEvent.ForkFailure(command, failures, RobotController.getTime());
    emitEvent(event);
  }

  public void addEventListener(Consumer<? super SchedulerEvent> listener) {
    ErrorMessages.requireNonNullParam(listener, "listener", "addEventListener");
 
    m_eventListeners.add(listener);
  }
 
  private void emitEvent(SchedulerEvent event) {
    // TODO: Prevent listeners from interacting with the scheduler.
    //       Scheduling or canceling commands while the scheduler is processing will probably cause
    //       bugs in user code or even a program crash.
    for (var listener : m_eventListeners) {
      listener.accept(event);
    }
  }
}