Fix vision module stream index logic (#295)

Previous logic could cause stream index with multiple cameras to run away

photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java

@@ -19,7 +19,6 @@ package org.photonvision.vision.processes;
 
 import java.util.*;
 import java.util.stream.Collectors;
-import org.photonvision.common.configuration.CameraConfiguration;
 
 /** VisionModuleManager has many VisionModules, and provides camera configuration data to them. */
 public class VisionModuleManager {
@@ -54,9 +53,9 @@ public class VisionModuleManager {
     public List<VisionModule> addSources(List<VisionSource> visionSources) {
         var addedModules = new HashMap<Integer, VisionModule>();
 
+        assignCameraIndex(visionSources);
         for (var visionSource : visionSources) {
             var pipelineManager = new PipelineManager(visionSource.getCameraConfiguration());
-            assignCameraIndex(visionSource.getCameraConfiguration());
 
             var module = new VisionModule(pipelineManager, visionSource, visionModules.size());
             visionModules.add(module);
@@ -72,16 +71,30 @@ public class VisionModuleManager {
         return sortedModulesList;
     }
 
-    private void assignCameraIndex(CameraConfiguration config) {
-        var max =
-                visionModules.stream()
-                        .mapToInt(it -> it.visionSource.getCameraConfiguration().streamIndex)
-                        .max()
-                        .orElse(-1);
+    private void assignCameraIndex(List<VisionSource> config) {
+        // We won't necessarily have already added all of the cameras we need to at this point
+        // But by operating on the list, we have a fairly good idea of which we need to change
+        // but it's not guaranteed that we change the correct one
+        // The best we can do is try to avoid a case where the stream index runs away to infinity
+        // since we can only stream 5 cameras at once
 
-        // If the current stream index is reserved, increase by 1
-        if (config.streamIndex <= max) {
-            config.streamIndex = max + 1;
+        for (var v : config) {
+            var listNoV = new ArrayList<>(config);
+            listNoV.remove(v);
+            if (listNoV.stream()
+                    .anyMatch(
+                            it ->
+                                    it.getCameraConfiguration().streamIndex
+                                            == v.getCameraConfiguration().streamIndex)) {
+                int idx = 0;
+                while (listNoV.stream()
+                        .map(it -> it.getCameraConfiguration().streamIndex)
+                        .collect(Collectors.toList())
+                        .contains(idx)) {
+                    idx++;
+                }
+                v.getCameraConfiguration().streamIndex = idx;
+            }
         }
     }
 }

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -17,10 +17,14 @@
 
 package org.photonvision.vision.processes;
 
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
 import edu.wpi.cscore.VideoMode;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
+import java.util.stream.Collectors;
 import org.junit.jupiter.api.*;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
@@ -131,6 +135,52 @@ public class VisionModuleManagerTest {
         printTestResults(module0DataConsumer.result);
     }
 
+    @Test
+    public void testMultipleStreamIndex() {
+        ConfigManager.getInstance().load();
+
+        var conf = new CameraConfiguration("Foo", "Bar");
+        conf.streamIndex = 1;
+        var ffp =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false),
+                        TestUtils.WPI2019Image.FOV);
+        var testSource = new TestSource(ffp, conf);
+
+        var conf2 = new CameraConfiguration("Foo2", "Bar");
+        conf2.streamIndex = 0;
+        var ffp2 =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false),
+                        TestUtils.WPI2019Image.FOV);
+        var testSource2 = new TestSource(ffp2, conf2);
+
+        var conf3 = new CameraConfiguration("Foo3", "Bar");
+        conf3.streamIndex = 0;
+        var ffp3 =
+                new FileFrameProvider(
+                        TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false),
+                        TestUtils.WPI2019Image.FOV);
+        var testSource3 = new TestSource(ffp3, conf3);
+
+        var modules =
+                VisionModuleManager.getInstance().addSources(List.of(testSource, testSource2, testSource3));
+
+        System.out.println(
+                Arrays.toString(
+                        modules.stream()
+                                .map(it -> it.visionSource.getCameraConfiguration().streamIndex)
+                                .collect(Collectors.toList())
+                                .toArray()));
+        var idxs =
+                modules.stream()
+                        .map(it -> it.visionSource.getCameraConfiguration().streamIndex)
+                        .collect(Collectors.toList());
+        assertTrue(idxs.contains(0));
+        assertTrue(idxs.contains(1));
+        assertTrue(idxs.contains(2));
+    }
+
     private static void printTestResults(CVPipelineResult pipelineResult) {
         double fps = 1000 / pipelineResult.getLatencyMillis();
         System.out.print(