ConflictDetector.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.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Collectors;

public final class ConflictDetector {
  private ConflictDetector() {
    // This is a utility class!
  }

  public record Conflict(Command a, Command b, Set<Mechanism> sharedRequirements) {
    public String description() {
      var shared =
          sharedRequirements.stream()
              .map(Mechanism::getName)
              .sorted()
              .collect(Collectors.joining(", "));
      return "%s and %s both require %s".formatted(a.name(), b.name(), shared);
    }
  }

  public static void throwIfConflicts(Collection<? extends Command> commands) {
    requireNonNullParam(commands, "commands", "ConflictDetector.throwIfConflicts");
    if (commands.size() <= 1) {
      // can't have conflicts with only one command
      return;
    }
 
    var conflicts = findAllConflicts(commands);
    if (conflicts.isEmpty()) {
      // No conflicts, all good
      return;
    }
 
    StringBuilder message =
        new StringBuilder("Commands running in parallel cannot share requirements: ");
    for (int i = 0; i < conflicts.size(); i++) {
      Conflict conflict = conflicts.get(i);
      message.append(conflict.description());
      if (i < conflicts.size() - 1) {
        message.append("; ");
      }
    }
 
    throw new IllegalArgumentException(message.toString());
  }

  @SuppressWarnings("PMD.CompareObjectsWithEquals")
  public static List<Conflict> findAllConflicts(Collection<? extends Command> commands) {
    requireNonNullParam(commands, "commands", "ConflictDetector.findAllConflicts");
 
    List<Conflict> conflicts = new ArrayList<>();
    if (commands.size() <= 1) {
      return conflicts;
    }
 
    int i = 0;
    for (Command command : commands) {
      i++;
      int j = 0;
      for (Command other : commands) {
        j++;
        if (j <= i) {
          // Skip all elements in 0..i so the inner loop only checks elements from i + 1 onward.
          // Ordering of the elements in the pair doesn't matter, and this prevents finding every
          // pair twice eg (A, B) and (B, A).
          continue;
        }
 
        if (command == other) {
          // Commands cannot conflict with themselves, so just in case the input collection happens
          // to have duplicate elements we just skip any pairs of a command with itself.
          continue;
        }
 
        if (command.conflictsWith(other)) {
          conflicts.add(findConflict(command, other));
        }
      }
    }
 
    return conflicts;
  }
 
  private static Conflict findConflict(Command a, Command b) {
    Set<Mechanism> sharedRequirements = new HashSet<>(a.requirements());
    sharedRequirements.retainAll(b.requirements());
    return new Conflict(a, b, Set.copyOf(sharedRequirements));
  }
}