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