ParallelGroupBuilder.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.Arrays;
import java.util.LinkedHashSet;
import java.util.Set;
import java.util.function.BooleanSupplier;
import java.util.stream.Collectors;
import org.wpilib.annotation.NoDiscard;

@NoDiscard
public class ParallelGroupBuilder {
  private final Set<Command> m_optionalCommands = new LinkedHashSet<>();
  private final Set<Command> m_requiredCommands = new LinkedHashSet<>();
  private BooleanSupplier m_endCondition;

  public ParallelGroupBuilder() {}

  public ParallelGroupBuilder optional(Command... commands) {
    requireNonNullParam(commands, "commands", "ParallelGroupBuilder.optional");
    for (int i = 0; i < commands.length; i++) {
      requireNonNullParam(commands[i], "commands[" + i + "]", "ParallelGroupBuilder.optional");
    }
 
    m_optionalCommands.addAll(Arrays.asList(commands));
    return this;
  }

  public ParallelGroupBuilder requiring(Command... commands) {
    requireNonNullParam(commands, "commands", "ParallelGroupBuilder.requiring");
    for (int i = 0; i < commands.length; i++) {
      requireNonNullParam(commands[i], "commands[" + i + "]", "ParallelGroupBuilder.requiring");
    }
 
    m_requiredCommands.addAll(Arrays.asList(commands));
    return this;
  }

  // Note: this primarily exists so users can cleanly chain .alongWith calls to build a
  //       parallel group, eg `fooCommand().alongWith(barCommand()).alongWith(bazCommand())`
  public ParallelGroupBuilder alongWith(Command command) {
    return requiring(command);
  }

  public ParallelGroupBuilder until(BooleanSupplier condition) {
    m_endCondition = condition;
    return this;
  }

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

  public ParallelGroup withAutomaticName() {
    // eg "(CommandA & CommandB & CommandC)"
    String required =
        m_requiredCommands.stream().map(Command::name).collect(Collectors.joining(" & ", "(", ")"));
 
    // eg "(CommandA | CommandB | CommandC)"
    String optional =
        m_optionalCommands.stream().map(Command::name).collect(Collectors.joining(" | ", "(", ")"));
 
    if (m_requiredCommands.isEmpty()) {
      // No required commands, pure race
      return named(optional, true);
    } else if (m_optionalCommands.isEmpty()) {
      // Everything required
      return named(required, true);
    } else {
      // Some form of deadline
      // eg "[(CommandA & CommandB) * (CommandX | CommandY | ...)]"
      String name = "[%s * %s]".formatted(required, optional);
      return named(name, true);
    }
  }
}