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.HashSet;
009import java.util.Set;
010
011/**
012 * A CommandGroup that runs a set of commands in parallel, ending when any one of the commands ends
013 * and interrupting all the others.
014 *
015 * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
016 */
017public class ParallelRaceGroup extends CommandGroupBase {
018  private final Set<Command> m_commands = new HashSet<>();
019  private boolean m_runWhenDisabled = true;
020  private boolean m_finished = true;
021
022  /**
023   * Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will
024   * "race to the finish" - the first command to finish ends the entire command, with all other
025   * commands being interrupted.
026   *
027   * @param commands the commands to include in this group.
028   */
029  public ParallelRaceGroup(Command... commands) {
030    addCommands(commands);
031  }
032
033  @Override
034  public final void addCommands(Command... commands) {
035    requireUngrouped(commands);
036
037    if (!m_finished) {
038      throw new IllegalStateException(
039          "Commands cannot be added to a CommandGroup while the group is running");
040    }
041
042    registerGroupedCommands(commands);
043
044    for (Command command : commands) {
045      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
046        throw new IllegalArgumentException(
047            "Multiple commands in a parallel group cannot" + " require the same subsystems");
048      }
049      m_commands.add(command);
050      m_requirements.addAll(command.getRequirements());
051      m_runWhenDisabled &= command.runsWhenDisabled();
052    }
053  }
054
055  @Override
056  public void initialize() {
057    m_finished = false;
058    for (Command command : m_commands) {
059      command.initialize();
060    }
061  }
062
063  @Override
064  public void execute() {
065    for (Command command : m_commands) {
066      command.execute();
067      if (command.isFinished()) {
068        m_finished = true;
069      }
070    }
071  }
072
073  @Override
074  public void end(boolean interrupted) {
075    for (Command command : m_commands) {
076      command.end(!command.isFinished());
077    }
078  }
079
080  @Override
081  public boolean isFinished() {
082    return m_finished;
083  }
084
085  @Override
086  public boolean runsWhenDisabled() {
087    return m_runWhenDisabled;
088  }
089
090  @Override
091  public ParallelRaceGroup raceWith(Command... parallel) {
092    addCommands(parallel);
093    return this;
094  }
095}