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 org.wpilib.commands3;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import java.util.ArrayList;
010import java.util.Collection;
011import java.util.HashSet;
012import java.util.List;
013import java.util.Set;
014import java.util.stream.Collectors;
015
016/** Utility class for helping with detecting conflicts between commands. */
017public final class ConflictDetector {
018  private ConflictDetector() {
019    // This is a utility class!
020  }
021
022  /**
023   * A conflict between two commands.
024   *
025   * @param a The first conflicting command.
026   * @param b The second conflicting command.
027   * @param sharedRequirements The set of mechanisms required by both commands. This set is
028   *     read-only
029   */
030  public record Conflict(Command a, Command b, Set<Mechanism> sharedRequirements) {
031    /**
032     * Gets a descriptive message for the conflict. The description includes the names of the
033     * conflicting commands and the names of all mechanisms required by both commands.
034     *
035     * @return A description of the conflict.
036     */
037    public String description() {
038      var shared =
039          sharedRequirements.stream()
040              .map(Mechanism::getName)
041              .sorted()
042              .collect(Collectors.joining(", "));
043      return "%s and %s both require %s".formatted(a.name(), b.name(), shared);
044    }
045  }
046
047  /**
048   * Validates that a set of commands have no internal requirement conflicts. An error is thrown if
049   * a conflict is detected.
050   *
051   * @param commands The commands to validate
052   * @throws IllegalArgumentException If at least one pair of commands is found in the input where
053   *     both commands have at least one required mechanism in common
054   */
055  public static void throwIfConflicts(Collection<? extends Command> commands) {
056    requireNonNullParam(commands, "commands", "ConflictDetector.throwIfConflicts");
057
058    var conflicts = findAllConflicts(commands);
059    if (conflicts.isEmpty()) {
060      // No conflicts, all good
061      return;
062    }
063
064    StringBuilder message =
065        new StringBuilder("Commands running in parallel cannot share requirements: ");
066    for (int i = 0; i < conflicts.size(); i++) {
067      Conflict conflict = conflicts.get(i);
068      message.append(conflict.description());
069      if (i < conflicts.size() - 1) {
070        message.append("; ");
071      }
072    }
073
074    throw new IllegalArgumentException(message.toString());
075  }
076
077  /**
078   * Finds all conflicting pairs of commands in the input collection.
079   *
080   * @param commands The commands to check.
081   * @return All detected conflicts. The returned list is empty if no conflicts were found.
082   */
083  @SuppressWarnings("PMD.CompareObjectsWithEquals")
084  public static List<Conflict> findAllConflicts(Collection<? extends Command> commands) {
085    requireNonNullParam(commands, "commands", "ConflictDetector.findAllConflicts");
086
087    List<Conflict> conflicts = new ArrayList<>();
088
089    int i = 0;
090    for (Command command : commands) {
091      i++;
092      int j = 0;
093      for (Command other : commands) {
094        j++;
095        if (j <= i) {
096          // Skip all elements in 0..i so the inner loop only checks elements from i + 1 onward.
097          // Ordering of the elements in the pair doesn't matter, and this prevents finding every
098          // pair twice eg (A, B) and (B, A).
099          continue;
100        }
101
102        if (command == other) {
103          // Commands cannot conflict with themselves, so just in case the input collection happens
104          // to have duplicate elements we just skip any pairs of a command with itself.
105          continue;
106        }
107
108        if (command.conflictsWith(other)) {
109          conflicts.add(findConflict(command, other));
110        }
111      }
112    }
113
114    return conflicts;
115  }
116
117  private static Conflict findConflict(Command a, Command b) {
118    Set<Mechanism> sharedRequirements = new HashSet<>(a.requirements());
119    sharedRequirements.retainAll(b.requirements());
120    return new Conflict(a, b, Set.copyOf(sharedRequirements));
121  }
122}