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 */
023public class ParallelDeadlineGroup extends Command {
024  // maps commands in this composition to whether they are still running
025  private final Map<Command, Boolean> m_commands = new HashMap<>();
026  private boolean m_runWhenDisabled = true;
027  private boolean m_finished = true;
028  private Command m_deadline;
029  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
030
031  /**
032   * Creates a new ParallelDeadlineGroup. The given commands, including the deadline, will be
033   * executed simultaneously. The composition will finish when the deadline finishes, interrupting
034   * all other still-running commands. If the composition is interrupted, only the commands still
035   * running will be interrupted.
036   *
037   * @param deadline the command that determines when the composition ends
038   * @param otherCommands the other commands to be executed
039   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
040   */
041  public ParallelDeadlineGroup(Command deadline, Command... otherCommands) {
042    addCommands(otherCommands);
043    setDeadline(deadline);
044  }
045
046  /**
047   * Sets the deadline to the given command. The deadline is added to the group if it is not already
048   * contained.
049   *
050   * @param deadline the command that determines when the group ends
051   * @throws IllegalArgumentException if the deadline command is already in the composition
052   */
053  public final void setDeadline(Command deadline) {
054    @SuppressWarnings("PMD.CompareObjectsWithEquals")
055    boolean isAlreadyDeadline = deadline == m_deadline;
056    if (isAlreadyDeadline) {
057      return;
058    }
059    if (m_commands.containsKey(deadline)) {
060      throw new IllegalArgumentException(
061          "The deadline command cannot also be in the other commands!");
062    }
063    addCommands(deadline);
064    m_deadline = deadline;
065  }
066
067  /**
068   * Adds the given commands to the group.
069   *
070   * @param commands Commands to add to the group.
071   */
072  public final void addCommands(Command... commands) {
073    if (!m_finished) {
074      throw new IllegalStateException(
075          "Commands cannot be added to a composition while it's running");
076    }
077
078    CommandScheduler.getInstance().registerComposedCommands(commands);
079
080    for (Command command : commands) {
081      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
082        throw new IllegalArgumentException(
083            "Multiple commands in a parallel group cannot require the same subsystems");
084      }
085      m_commands.put(command, false);
086      m_requirements.addAll(command.getRequirements());
087      m_runWhenDisabled &= command.runsWhenDisabled();
088      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
089        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
090      }
091    }
092  }
093
094  @Override
095  public final void initialize() {
096    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
097      commandRunning.getKey().initialize();
098      commandRunning.setValue(true);
099    }
100    m_finished = false;
101  }
102
103  @Override
104  public final void execute() {
105    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
106      if (!commandRunning.getValue()) {
107        continue;
108      }
109      commandRunning.getKey().execute();
110      if (commandRunning.getKey().isFinished()) {
111        commandRunning.getKey().end(false);
112        commandRunning.setValue(false);
113        if (commandRunning.getKey().equals(m_deadline)) {
114          m_finished = true;
115        }
116      }
117    }
118  }
119
120  @Override
121  public final void end(boolean interrupted) {
122    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
123      if (commandRunning.getValue()) {
124        commandRunning.getKey().end(true);
125      }
126    }
127  }
128
129  @Override
130  public final boolean isFinished() {
131    return m_finished;
132  }
133
134  @Override
135  public boolean runsWhenDisabled() {
136    return m_runWhenDisabled;
137  }
138
139  @Override
140  public InterruptionBehavior getInterruptionBehavior() {
141    return m_interruptBehavior;
142  }
143
144  @Override
145  public void initSendable(SendableBuilder builder) {
146    super.initSendable(builder);
147
148    builder.addStringProperty("deadline", m_deadline::getName, null);
149  }
150}