Update equalsWithIdMap

The method now accounts for the existence of multiple activities that are identical sans ids

Author: Mythicaeda

scheduler-driver/src/main/java/gov/nasa/jpl/aerie/scheduler/simulation/SimulationFacade.java

@@ -9,14 +9,15 @@
 import gov.nasa.jpl.aerie.types.ActivityDirective;
 import gov.nasa.jpl.aerie.types.ActivityDirectiveId;
 
+import java.util.ArrayList;
 import java.util.Collection;
 import java.util.HashMap;
 import java.util.HashSet;
+import java.util.List;
 import java.util.Map;
 import java.util.Optional;
 import java.util.Set;
 import java.util.function.Supplier;
-import java.util.stream.Collectors;
 
 public interface SimulationFacade {
   void setInitialSimResults(SimulationData simulationData);
@@ -82,28 +83,68 @@ public boolean equals(Object other){
      * `map` contains a mapping between *only the IDs that are different* between the two plans.
      */
     public Optional<Map<ActivityDirectiveId, ActivityDirectiveId>> equalsWithIdMap(PlanSimCorrespondence other) {
+      // If the simulations have a different amount of directives, they must not be equal
+      if(directiveIdActivityDirectiveMap.size() != other.directiveIdActivityDirectiveMap.size()) {
+        return Optional.empty();
+      }
+
+      // Build the maps of directives -> ids from the ids -> directives map
+      // Because multiple activities on a plan can be identical sans id, the inverted maps to a list of ids
+      final var thisInverted = new HashMap<ActivityDirective, List<ActivityDirectiveId>>(directiveIdActivityDirectiveMap.size());
+      directiveIdActivityDirectiveMap.forEach(
+          (key, value) -> {
+            thisInverted.putIfAbsent(value, new ArrayList<>());
+            thisInverted.get(value).add(key);
+          });
+
+      final var otherInverted = new HashMap<ActivityDirective, List<ActivityDirectiveId>>(other.directiveIdActivityDirectiveMap.size());
+      other.directiveIdActivityDirectiveMap.forEach(
+          (key, value) -> {
+            otherInverted.putIfAbsent(value, new ArrayList<>());
+            otherInverted.get(value).add(key);
+          });
+
+      // If these maps have different sizes, then the simulations must not be equal
+      if(thisInverted.size() != otherInverted.size()) {
+        return Optional.empty();
+      }
+
+      // Check every entry for equality while building up the return mapping
       final var result = new HashMap<ActivityDirectiveId, ActivityDirectiveId>();
-      final var thisInverse = directiveIdActivityDirectiveMap.entrySet()
-                                 .stream()
-                                 .collect(Collectors.toMap(Map.Entry::getValue, Map.Entry::getKey));
-      final var otherInverse = other.directiveIdActivityDirectiveMap.entrySet()
-                                                             .stream()
-                                                             .collect(Collectors.toMap(Map.Entry::getValue, Map.Entry::getKey));
-      for (var entry : thisInverse.entrySet()) {
-        final var otherId = otherInverse.remove(entry.getKey());
-        if (otherId == null) {
+      for (final var entry : thisInverted.entrySet()) {
+        // Check that at least one instance of this directive is on the other plan
+        if(!otherInverted.containsKey(entry.getKey())){
+          return Optional.empty();
+        }
+
+        final var thisIds = entry.getValue();
+        final var otherIds = otherInverted.get(entry.getKey());
+
+        // Check that there is the same number of instances of this directive on the other plan
+        if (thisIds.size() != otherIds.size()) {
           return Optional.empty();
         }
-        if (!otherId.equals(entry.getValue())) {
-          result.put(entry.getValue(), otherId);
+
+        // If the directive is unique (the most common case), then the two ids can be directly compared
+        if(thisIds.size() == 1) {
+          if(!thisIds.getFirst().equals(otherIds.getFirst())) {
+            result.put(thisIds.getFirst(), otherIds.getFirst());
+          }
+          continue;
         }
-      }
 
-      if (otherInverse.isEmpty()) {
-        return Optional.of(result);
-      } else {
-        return Optional.empty();
+        // Filter out the ids that are on both lists, as this method does not provide a mapping for ids that have not changed
+        final List<ActivityDirectiveId> toRemove = thisIds.stream().filter(otherIds::contains).toList();
+        thisIds.removeAll(toRemove);
+        otherIds.removeAll(toRemove);
+
+        // Add the remaining mappings to the result
+        for(int i = 0; i < thisIds.size(); ++i) {
+          result.put(thisIds.get(i), otherIds.get(i));
+        }
       }
+
+      return Optional.of(result);
     }
   }
 }