SequentialGroupBuilder.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.command3;
 
import static org.wpilib.util.ErrorMessages.requireNonNullParam;
 
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.stream.Collectors;
import org.wpilib.annotation.NoDiscard;

@NoDiscard
public class SequentialGroupBuilder {
  private final List<Command> m_steps = new ArrayList<>();
  private BooleanSupplier m_endCondition;

  public SequentialGroupBuilder() {}

  public SequentialGroupBuilder andThen(Command next) {
    requireNonNullParam(next, "next", "SequentialGroupBuilder.andThen");
 
    m_steps.add(next);
    return this;
  }

  public SequentialGroupBuilder andThen(Command... nextCommands) {
    requireNonNullParam(nextCommands, "nextCommands", "SequentialGroupBuilder.andThen");
    for (int i = 0; i < nextCommands.length; i++) {
      requireNonNullParam(
          nextCommands[i], "nextCommands[" + i + "]", "SequentialGroupBuilder.andThen");
    }
 
    m_steps.addAll(Arrays.asList(nextCommands));
    return this;
  }

  public SequentialGroupBuilder until(BooleanSupplier endCondition) {
    m_endCondition = endCondition;
    return this;
  }

  public Command named(String name) {
    return named(name, false);
  }
 
  private Command named(String name, boolean useAutomaticName) {
    var seq = new SequentialGroup(name, m_steps);
    if (m_endCondition == null) {
      // No custom end condition, return the group as is
      return seq;
    }
 
    var out =
        new ParallelGroupBuilder()
            .optional(seq, Command.waitUntil(m_endCondition).named("Until Condition"));
    // We have a custom end condition, so we need to wrap the group in a race
    return useAutomaticName ? out.withAutomaticName() : out.named(name);
  }

  public Command withAutomaticName() {
    return named(m_steps.stream().map(Command::name).collect(Collectors.joining(" -> ")), true);
  }
}