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 only when a specific command (the
013 * "deadline") ends, interrupting all other commands that are still running at that point.
014 *
015 * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
016 */
017public class ParallelDeadlineGroup extends CommandGroupBase {
018  // maps commands in this group to whether they are still running
019  private final Map<Command, Boolean> m_commands = new HashMap<>();
020  private boolean m_runWhenDisabled = true;
021  private boolean m_finished = true;
022  private Command m_deadline;
023
024  /**
025   * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
026   * executed simultaneously. The CommandGroup will finish when the deadline finishes, interrupting
027   * all other still-running commands. If the CommandGroup is interrupted, only the commands still
028   * running will be interrupted.
029   *
030   * @param deadline the command that determines when the group ends
031   * @param commands the commands to be executed
032   */
033  public ParallelDeadlineGroup(Command deadline, Command... commands) {
034    m_deadline = deadline;
035    addCommands(commands);
036    if (!m_commands.containsKey(deadline)) {
037      addCommands(deadline);
038    }
039  }
040
041  /**
042   * Sets the deadline to the given command. The deadline is added to the group if it is not already
043   * contained.
044   *
045   * @param deadline the command that determines when the group ends
046   */
047  public void setDeadline(Command deadline) {
048    if (!m_commands.containsKey(deadline)) {
049      addCommands(deadline);
050    }
051    m_deadline = deadline;
052  }
053
054  @Override
055  public final void addCommands(Command... commands) {
056    requireUngrouped(commands);
057
058    if (!m_finished) {
059      throw new IllegalStateException(
060          "Commands cannot be added to a CommandGroup while the group is running");
061    }
062
063    registerGroupedCommands(commands);
064
065    for (Command command : commands) {
066      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
067        throw new IllegalArgumentException(
068            "Multiple commands in a parallel group cannot" + "require the same subsystems");
069      }
070      m_commands.put(command, false);
071      m_requirements.addAll(command.getRequirements());
072      m_runWhenDisabled &= command.runsWhenDisabled();
073    }
074  }
075
076  @Override
077  public void initialize() {
078    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
079      commandRunning.getKey().initialize();
080      commandRunning.setValue(true);
081    }
082    m_finished = false;
083  }
084
085  @Override
086  public void execute() {
087    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
088      if (!commandRunning.getValue()) {
089        continue;
090      }
091      commandRunning.getKey().execute();
092      if (commandRunning.getKey().isFinished()) {
093        commandRunning.getKey().end(false);
094        commandRunning.setValue(false);
095        if (commandRunning.getKey().equals(m_deadline)) {
096          m_finished = true;
097        }
098      }
099    }
100  }
101
102  @Override
103  public void end(boolean interrupted) {
104    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
105      if (commandRunning.getValue()) {
106        commandRunning.getKey().end(true);
107      }
108    }
109  }
110
111  @Override
112  public boolean isFinished() {
113    return m_finished;
114  }
115
116  @Override
117  public boolean runsWhenDisabled() {
118    return m_runWhenDisabled;
119  }
120
121  @Override
122  public ParallelDeadlineGroup deadlineWith(Command... parallel) {
123    addCommands(parallel);
124    return this;
125  }
126}