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.HashMap;
009import java.util.Map;
010
011/**
012 * A CommandGroup that runs a set of commands in parallel, ending when the last command ends.
013 *
014 * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
015 */
016public class ParallelCommandGroup extends CommandGroupBase {
017  // maps commands in this group to whether they are still running
018  private final Map<Command, Boolean> m_commands = new HashMap<>();
019  private boolean m_runWhenDisabled = true;
020
021  /**
022   * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The
023   * command group will finish when the last command finishes. If the CommandGroup is interrupted,
024   * only the commands that are still running will be interrupted.
025   *
026   * @param commands the commands to include in this group.
027   */
028  public ParallelCommandGroup(Command... commands) {
029    addCommands(commands);
030  }
031
032  @Override
033  public final void addCommands(Command... commands) {
034    requireUngrouped(commands);
035
036    if (m_commands.containsValue(true)) {
037      throw new IllegalStateException(
038          "Commands cannot be added to a CommandGroup while the group is running");
039    }
040
041    registerGroupedCommands(commands);
042
043    for (Command command : commands) {
044      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
045        throw new IllegalArgumentException(
046            "Multiple commands in a parallel group cannot" + "require the same subsystems");
047      }
048      m_commands.put(command, false);
049      m_requirements.addAll(command.getRequirements());
050      m_runWhenDisabled &= command.runsWhenDisabled();
051    }
052  }
053
054  @Override
055  public void initialize() {
056    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
057      commandRunning.getKey().initialize();
058      commandRunning.setValue(true);
059    }
060  }
061
062  @Override
063  public void execute() {
064    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
065      if (!commandRunning.getValue()) {
066        continue;
067      }
068      commandRunning.getKey().execute();
069      if (commandRunning.getKey().isFinished()) {
070        commandRunning.getKey().end(false);
071        commandRunning.setValue(false);
072      }
073    }
074  }
075
076  @Override
077  public void end(boolean interrupted) {
078    if (interrupted) {
079      for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
080        if (commandRunning.getValue()) {
081          commandRunning.getKey().end(true);
082        }
083      }
084    }
085  }
086
087  @Override
088  public boolean isFinished() {
089    return !m_commands.containsValue(true);
090  }
091
092  @Override
093  public boolean runsWhenDisabled() {
094    return m_runWhenDisabled;
095  }
096
097  @Override
098  public ParallelCommandGroup alongWith(Command... parallel) {
099    addCommands(parallel);
100    return this;
101  }
102}