001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj2.command;
006
007import java.util.Collections;
008import java.util.LinkedHashMap;
009import java.util.Map;
010
011/**
012 * A command composition that runs a set of commands in parallel, ending when the last command ends.
013 *
014 * <p>The rules for command compositions apply: command instances that are passed to it cannot be
015 * added to any other composition or scheduled individually, and the composition requires all
016 * subsystems its components require.
017 *
018 * <p>This class is provided by the NewCommands VendorDep
019 */
020public class ParallelCommandGroup extends Command {
021  // maps commands in this composition to whether they are still running
022  // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that
023  // changing the value associated with a command does NOT change the order)
024  private final Map<Command, Boolean> m_commands = new LinkedHashMap<>();
025  private boolean m_runWhenDisabled = true;
026  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
027
028  /**
029   * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The
030   * command composition will finish when the last command finishes. If the composition is
031   * interrupted, only the commands that are still running will be interrupted.
032   *
033   * @param commands the commands to include in this composition.
034   */
035  @SuppressWarnings("this-escape")
036  public ParallelCommandGroup(Command... commands) {
037    addCommands(commands);
038  }
039
040  /**
041   * Adds the given commands to the group.
042   *
043   * @param commands Commands to add to the group.
044   */
045  public final void addCommands(Command... commands) {
046    if (m_commands.containsValue(true)) {
047      throw new IllegalStateException(
048          "Commands cannot be added to a composition while it's running");
049    }
050
051    CommandScheduler.getInstance().registerComposedCommands(commands);
052
053    for (Command command : commands) {
054      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
055        throw new IllegalArgumentException(
056            "Multiple commands in a parallel composition cannot require the same subsystems");
057      }
058      m_commands.put(command, false);
059      addRequirements(command.getRequirements());
060      m_runWhenDisabled &= command.runsWhenDisabled();
061      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
062        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
063      }
064    }
065  }
066
067  @Override
068  public final void initialize() {
069    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
070      commandRunning.getKey().initialize();
071      commandRunning.setValue(true);
072    }
073  }
074
075  @Override
076  public final void execute() {
077    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
078      if (!commandRunning.getValue()) {
079        continue;
080      }
081      commandRunning.getKey().execute();
082      if (commandRunning.getKey().isFinished()) {
083        commandRunning.getKey().end(false);
084        commandRunning.setValue(false);
085      }
086    }
087  }
088
089  @Override
090  public final void end(boolean interrupted) {
091    if (interrupted) {
092      for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
093        if (commandRunning.getValue()) {
094          commandRunning.getKey().end(true);
095        }
096      }
097    }
098  }
099
100  @Override
101  public final boolean isFinished() {
102    return !m_commands.containsValue(true);
103  }
104
105  @Override
106  public boolean runsWhenDisabled() {
107    return m_runWhenDisabled;
108  }
109
110  @Override
111  public InterruptionBehavior getInterruptionBehavior() {
112    return m_interruptBehavior;
113  }
114}