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 edu.wpi.first.util.sendable.SendableBuilder;
008import java.util.Collections;
009import java.util.HashMap;
010import java.util.Map;
011
012/**
013 * A command composition that runs a set of commands in parallel, ending only when a specific
014 * command (the "deadline") ends, interrupting all other commands that are still running at that
015 * point.
016 *
017 * <p>The rules for command compositions apply: command instances that are passed to it cannot be
018 * added to any other composition or scheduled individually, and the composition requires all
019 * subsystems its components require.
020 *
021 * <p>This class is provided by the NewCommands VendorDep
022 */
023@SuppressWarnings("removal")
024public class ParallelDeadlineGroup extends CommandGroupBase {
025  // maps commands in this composition to whether they are still running
026  private final Map<Command, Boolean> m_commands = new HashMap<>();
027  private boolean m_runWhenDisabled = true;
028  private boolean m_finished = true;
029  private Command m_deadline;
030  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
031
032  /**
033   * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
034   * executed simultaneously. The composition will finish when the deadline finishes, interrupting
035   * all other still-running commands. If the composition is interrupted, only the commands still
036   * running will be interrupted.
037   *
038   * @param deadline the command that determines when the composition ends
039   * @param commands the commands to be executed
040   */
041  public ParallelDeadlineGroup(Command deadline, Command... commands) {
042    m_deadline = deadline;
043    addCommands(commands);
044    if (!m_commands.containsKey(deadline)) {
045      addCommands(deadline);
046    }
047  }
048
049  /**
050   * Sets the deadline to the given command. The deadline is added to the group if it is not already
051   * contained.
052   *
053   * @param deadline the command that determines when the group ends
054   */
055  public void setDeadline(Command deadline) {
056    if (!m_commands.containsKey(deadline)) {
057      addCommands(deadline);
058    }
059    m_deadline = deadline;
060  }
061
062  @Override
063  public final void addCommands(Command... commands) {
064    if (!m_finished) {
065      throw new IllegalStateException(
066          "Commands cannot be added to a composition while it's running");
067    }
068
069    CommandScheduler.getInstance().registerComposedCommands(commands);
070
071    for (Command command : commands) {
072      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
073        throw new IllegalArgumentException(
074            "Multiple commands in a parallel group cannot" + "require the same subsystems");
075      }
076      m_commands.put(command, false);
077      m_requirements.addAll(command.getRequirements());
078      m_runWhenDisabled &= command.runsWhenDisabled();
079      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
080        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
081      }
082    }
083  }
084
085  @Override
086  public final void initialize() {
087    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
088      commandRunning.getKey().initialize();
089      commandRunning.setValue(true);
090    }
091    m_finished = false;
092  }
093
094  @Override
095  public final void execute() {
096    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
097      if (!commandRunning.getValue()) {
098        continue;
099      }
100      commandRunning.getKey().execute();
101      if (commandRunning.getKey().isFinished()) {
102        commandRunning.getKey().end(false);
103        commandRunning.setValue(false);
104        if (commandRunning.getKey().equals(m_deadline)) {
105          m_finished = true;
106        }
107      }
108    }
109  }
110
111  @Override
112  public final void end(boolean interrupted) {
113    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
114      if (commandRunning.getValue()) {
115        commandRunning.getKey().end(true);
116      }
117    }
118  }
119
120  @Override
121  public final boolean isFinished() {
122    return m_finished;
123  }
124
125  @Override
126  public boolean runsWhenDisabled() {
127    return m_runWhenDisabled;
128  }
129
130  @Override
131  public InterruptionBehavior getInterruptionBehavior() {
132    return m_interruptBehavior;
133  }
134
135  @Override
136  public void initSendable(SendableBuilder builder) {
137    super.initSendable(builder);
138
139    builder.addStringProperty("deadline", m_deadline::getName, null);
140  }
141}