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.ArrayList;
008import java.util.List;
009
010/**
011 * A CommandGroups that runs a list of commands in sequence.
012 *
013 * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
014 */
015public class SequentialCommandGroup extends CommandGroupBase {
016  private final List<Command> m_commands = new ArrayList<>();
017  private int m_currentCommandIndex = -1;
018  private boolean m_runWhenDisabled = true;
019
020  /**
021   * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the
022   * CommandGroup finishing when the last command finishes.
023   *
024   * @param commands the commands to include in this group.
025   */
026  public SequentialCommandGroup(Command... commands) {
027    addCommands(commands);
028  }
029
030  @Override
031  public final void addCommands(Command... commands) {
032    requireUngrouped(commands);
033
034    if (m_currentCommandIndex != -1) {
035      throw new IllegalStateException(
036          "Commands cannot be added to a CommandGroup while the group is running");
037    }
038
039    registerGroupedCommands(commands);
040
041    for (Command command : commands) {
042      m_commands.add(command);
043      m_requirements.addAll(command.getRequirements());
044      m_runWhenDisabled &= command.runsWhenDisabled();
045    }
046  }
047
048  @Override
049  public void initialize() {
050    m_currentCommandIndex = 0;
051
052    if (!m_commands.isEmpty()) {
053      m_commands.get(0).initialize();
054    }
055  }
056
057  @Override
058  public void execute() {
059    if (m_commands.isEmpty()) {
060      return;
061    }
062
063    Command currentCommand = m_commands.get(m_currentCommandIndex);
064
065    currentCommand.execute();
066    if (currentCommand.isFinished()) {
067      currentCommand.end(false);
068      m_currentCommandIndex++;
069      if (m_currentCommandIndex < m_commands.size()) {
070        m_commands.get(m_currentCommandIndex).initialize();
071      }
072    }
073  }
074
075  @Override
076  public void end(boolean interrupted) {
077    if (interrupted
078        && !m_commands.isEmpty()
079        && m_currentCommandIndex > -1
080        && m_currentCommandIndex < m_commands.size()) {
081      m_commands.get(m_currentCommandIndex).end(true);
082    }
083    m_currentCommandIndex = -1;
084  }
085
086  @Override
087  public boolean isFinished() {
088    return m_currentCommandIndex == m_commands.size();
089  }
090
091  @Override
092  public boolean runsWhenDisabled() {
093    return m_runWhenDisabled;
094  }
095
096  @Override
097  public SequentialCommandGroup beforeStarting(Command before) {
098    // store all the commands
099    var commands = new ArrayList<Command>();
100    commands.add(before);
101    commands.addAll(m_commands);
102
103    // reset current state
104    commands.forEach(CommandGroupBase::clearGroupedCommand);
105    m_commands.clear();
106    m_requirements.clear();
107    m_runWhenDisabled = true;
108
109    // add them back
110    addCommands(commands.toArray(Command[]::new));
111    return this;
112  }
113
114  @Override
115  public SequentialCommandGroup andThen(Command... next) {
116    addCommands(next);
117    return this;
118  }
119}