From 164a02159ec27ba60e463e3de4c1716a5d0b0c60 Mon Sep 17 00:00:00 2001
From: Banks Troutman <bankst@roswellrobotics.com>
Date: Sat, 16 Nov 2019 14:30:44 -0500
Subject: [PATCH] Move LoopingRunnable, begin work on main and VisionManager

---
 Main/chameleon-vision.iml                     |  6 +-
 Main/pom.xml                                  | 10 ++--
 .../main/java/com/chameleonvision/Main.java   | 48 +++++++++++-----
 .../classabstraction/VisionManager.java       | 36 ++++++------
 .../classabstraction/VisionProcess.java       | 55 +++++--------------
 .../pipeline/CVPipeline2d.java                |  6 +-
 .../pipeline/CVPipeline3d.java                |  4 +-
 .../pipeline/DriverVisionPipeline.java        |  6 +-
 .../util/LoopingRunnable.java                 | 33 +++++++++++
 9 files changed, 115 insertions(+), 89 deletions(-)
 create mode 100644 Main/src/main/java/com/chameleonvision/classabstraction/util/LoopingRunnable.java

diff --git a/Main/chameleon-vision.iml b/Main/chameleon-vision.iml
index d886581e1..c825b54de 100644
--- a/Main/chameleon-vision.iml
+++ b/Main/chameleon-vision.iml
@@ -41,9 +41,9 @@
     <orderEntry type="library" name="Maven: org.msgpack:msgpack-core:0.8.18" level="project" />
     <orderEntry type="library" name="Maven: org.msgpack:jackson-dataformat-msgpack:0.8.18" level="project" />
     <orderEntry type="library" name="Maven: org.apache.commons:commons-lang3:3.9" level="project" />
-    <orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-core:2.10.0" level="project" />
-    <orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-annotations:2.10.0" level="project" />
-    <orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-databind:2.10.0" level="project" />
+    <orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-core:2.10.1" level="project" />
+    <orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-annotations:2.10.1" level="project" />
+    <orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-databind:2.10.1" level="project" />
     <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-java:2019.4.1-213-g56d782b" level="project" />
     <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxaarch64bionic:2019.4.1-213-g56d782b" level="project" />
     <orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxraspbian:2019.4.1-213-g56d782b" level="project" />
diff --git a/Main/pom.xml b/Main/pom.xml
index 7264e4cf7..76af8f0a8 100644
--- a/Main/pom.xml
+++ b/Main/pom.xml
@@ -5,7 +5,7 @@
     <modelVersion>4.0.0</modelVersion>
     <groupId>org.chameleon-vision.main</groupId>
     <artifactId>chameleon-vision</artifactId>
-    <version>1.1.4-BETA</version>
+    <version>2.0-SNAPSHOT</version>
     <build>
         <plugins>
             <!--setup for java jdk 12-->
@@ -106,25 +106,25 @@
             <artifactId>commons-lang3</artifactId>
             <version>3.9</version>
         </dependency>
+
         <dependency>
             <groupId>com.fasterxml.jackson.core</groupId>
             <artifactId>jackson-core</artifactId>
-            <version>2.10.0</version>
+            <version>2.10.1</version>
         </dependency>
 
         <dependency>
             <groupId>com.fasterxml.jackson.core</groupId>
             <artifactId>jackson-annotations</artifactId>
-            <version>2.10.0</version>
+            <version>2.10.1</version>
         </dependency>
 
         <dependency>
             <groupId>com.fasterxml.jackson.core</groupId>
             <artifactId>jackson-databind</artifactId>
-            <version>2.10.0</version>
+            <version>2.10.1</version>
         </dependency>
 
-
         <!-- supported platforms for wpilib JNI classifiers
             linuxaarch64bionic
             linuxraspbian
diff --git a/Main/src/main/java/com/chameleonvision/Main.java b/Main/src/main/java/com/chameleonvision/Main.java
index 06b65b327..d52eff63a 100644
--- a/Main/src/main/java/com/chameleonvision/Main.java
+++ b/Main/src/main/java/com/chameleonvision/Main.java
@@ -1,5 +1,6 @@
 package com.chameleonvision;
 
+import com.chameleonvision.classabstraction.VisionManager;
 import com.chameleonvision.network.NetworkManager;
 import com.chameleonvision.settings.Platform;
 import com.chameleonvision.settings.SettingsManager;
@@ -133,28 +134,47 @@ public class Main {
 
         if (testMode) {
             // todo: boot in to the new classabstraction stuff
+            boolean visionSourcesOk = VisionManager.initializeSources();
+            if (!visionSourcesOk) {
+                System.out.println("No cameras connected!");
+                return;
+            }
+
+            NetworkManager.initialize(manageNetwork);
+
+            boolean visionProcessesOk = VisionManager.initializeProcesses();
+            if (!visionProcessesOk) {
+                System.err.println("shit");
+                return;
+            }
+
+            runServer();
         } else {
             if (CameraManager.initializeCameras()) {
                 SettingsManager.initialize();
                 NetworkManager.initialize(manageNetwork);
                 CameraManager.initializeThreads();
-                if (ntServerMode) {
-                    System.out.println("Starting NT Server");
-                    NetworkTableInstance.getDefault().startServer();
-                } else {
-                    NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
-                    if (ntClientModeServer != null) {
-                        NetworkTableInstance.getDefault().startClient(ntClientModeServer);
-                    } else {
-                        NetworkTableInstance.getDefault().startClientTeam(SettingsManager.generalSettings.teamNumber);
-                    }
-                }
-
-                System.out.printf("Starting Webserver at port %d\n", DEFAULT_PORT);
-                Server.main(DEFAULT_PORT);
+                runServer();
             } else {
                 System.err.println("No cameras connected!");
             }
         }
     }
+
+    private static void runServer() {
+        if (ntServerMode) {
+            System.out.println("Starting NT Server");
+            NetworkTableInstance.getDefault().startServer();
+        } else {
+            NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
+            if (ntClientModeServer != null) {
+                NetworkTableInstance.getDefault().startClient(ntClientModeServer);
+            } else {
+                NetworkTableInstance.getDefault().startClientTeam(SettingsManager.generalSettings.teamNumber);
+            }
+        }
+
+        System.out.printf("Starting Webserver at port %d\n", DEFAULT_PORT);
+        Server.main(DEFAULT_PORT);
+    }
 }
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/VisionManager.java b/Main/src/main/java/com/chameleonvision/classabstraction/VisionManager.java
index 33ab13bc9..5f5a161a3 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/VisionManager.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/VisionManager.java
@@ -1,27 +1,21 @@
 package com.chameleonvision.classabstraction;
 
-import com.chameleonvision.classabstraction.camera.USBCameraProcess;
-import com.chameleonvision.classabstraction.pipeline.CVPipelineSettings;
 import com.chameleonvision.settings.SettingsManager;
 import com.chameleonvision.util.FileHelper;
-import com.chameleonvision.vision.camera.CameraDeserializer;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import com.google.gson.Gson;
-import com.google.gson.GsonBuilder;
 import edu.wpi.cscore.UsbCamera;
 import edu.wpi.cscore.UsbCameraInfo;
 import org.opencv.videoio.VideoCapture;
 
 import java.io.File;
-import java.io.IOException;
+import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
-import java.util.Arrays;
 import java.util.LinkedHashMap;
-import java.util.List;
 
 public class VisionManager {
 
+    private VisionManager() {}
+
     private static final Path CamConfigPath = Paths.get(SettingsManager.SettingsPath.toString(), "cameras");
 
     public static final LinkedHashMap<String, UsbCameraInfo> UsbCameraInfosByCameraName = new LinkedHashMap<>();
@@ -46,6 +40,20 @@ public class VisionManager {
             return false;
         }
 
+        FileHelper.CheckPath(CamConfigPath);
+
+        UsbCameraInfosByCameraName.forEach((cameraName, cameraInfo) -> {
+            Path cameraConfigFolder = Paths.get(CamConfigPath.toString(), String.format("%s\\", cameraName));
+            Path cameraConfigPath = Paths.get(cameraConfigFolder.toString(), String.format("%s.json", cameraName));
+
+            if (Files.exists(cameraConfigFolder)) {
+                if (Files.exists(cameraConfigPath)) {
+                    File cameraConfigFile = new File(cameraConfigPath.toString());
+
+                }
+            }
+        });
+
 //        FileHelper.CheckPath(CamConfigPath);
 //        UsbCameraInfosByCameraName.forEach((cameraName, cameraInfo) -> {
 //            Path cameraConfigPath = Paths.get(CamConfigPath.toString(), String.format("%s.json", cameraName));
@@ -60,14 +68,8 @@ public class VisionManager {
         return true;
     }
 
-    public void PipelineSerializer(List<CVPipelineSettings> list, Path path) throws IOException {
-        File pipelineFile = new File(path.toString());
-        ObjectMapper objectMapper = new ObjectMapper().enableDefaultTyping();
-        objectMapper.writeValue(pipelineFile,list);
-    }
-
-
-    public static void initializeProcesses() {
+    public static boolean initializeProcesses() {
 
+        return true;
     }
 }
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
index 9390e9282..7e801930f 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
@@ -6,6 +6,7 @@ import com.chameleonvision.classabstraction.pipeline.CVPipeline;
 import com.chameleonvision.classabstraction.pipeline.CVPipelineResult;
 import com.chameleonvision.classabstraction.pipeline.CVPipelineSettings;
 import com.chameleonvision.classabstraction.pipeline.DriverVisionPipeline;
+import com.chameleonvision.classabstraction.util.LoopingRunnable;
 import edu.wpi.cscore.VideoMode;
 import org.opencv.core.Mat;
 
@@ -17,7 +18,7 @@ public class VisionProcess {
     private final CameraProcess cameraProcess;
     private final List<CVPipeline> pipelines = new ArrayList<>();
     private final CameraFrameRunnable cameraRunnable;
-    private final CameraStramerRunnable streamRunnable;
+    private final CameraStreamerRunnable streamRunnable;
     private final VisionProcessRunnable visionRunnable;
     private final CameraStreamer cameraStreamer;
     private CVPipeline currentPipeline;
@@ -31,7 +32,8 @@ public class VisionProcess {
         setPipeline(pipelines.get(0));
 
         // Thread to grab frames from the camera
-        this.cameraRunnable = new CameraFrameRunnable(cameraUpdateTimeMs) ;
+        // TODO: fix video modes!!!
+        this.cameraRunnable = new CameraFrameRunnable(cameraProcess.getProperties().videoModes.get(0).fps);
         new Thread(cameraRunnable).start();
 
         // Thread to put frames on the dashboard
@@ -78,11 +80,13 @@ public class VisionProcess {
 
         /**
          * CameraFrameRunnable grabs images from the cameraProcess
-         * at a specified loopTime
-         * @param loopTimeMs how often to grab frames at in ms
+         * at a specified framerate
+         * @param cameraFPS FPS of camera
          */
-        CameraFrameRunnable(Long loopTimeMs) {
-            super(loopTimeMs);
+        CameraFrameRunnable(int cameraFPS) {
+            // add 2 FPS to allow for a bit of overhead
+            // TODO: test the affect of this
+            super(1000L/(cameraFPS + 2));
         }
 
         @Override
@@ -127,52 +131,19 @@ public class VisionProcess {
         }
     }
 
-    private class CameraStramerRunnable extends LoopingRunnable {
+    private class CameraStreamerRunnable extends LoopingRunnable {
 
         private final CameraStreamer streamer;
         private Mat streamBuffer = new Mat();
 
-        private CameraStramerRunnable(Long loopTimeMs, CameraStreamer streamer) {
+        private CameraStreamerRunnable(Long loopTimeMs, CameraStreamer streamer) {
             super(loopTimeMs);
             this.streamer = streamer;
         }
 
         @Override
-        void process() {
+        protected void process() {
             streamer.runStream(cameraRunnable.getFrame(streamBuffer));
         }
     }
-
-    /**
-     * A thread that tries to run at a specified loop time
-     */
-    private static abstract class LoopingRunnable implements Runnable {
-        private final Long loopTimeMs;
-
-        abstract void process();
-
-        private LoopingRunnable(Long loopTimeMs) {
-            this.loopTimeMs = loopTimeMs;
-        }
-
-        @Override
-        public void run() {
-            while(!Thread.interrupted()) {
-                var now = System.currentTimeMillis();
-
-                // Do the thing
-                process();
-
-                // sleep for the remaining time
-                var timeElapsed = System.currentTimeMillis() - now;
-                var delta = loopTimeMs - timeElapsed;
-                if(delta > 0.0) {
-                    try {
-                        Thread.sleep(delta, 0);
-                    } catch (Exception ignored) {}
-                }
-            }
-        }
-    }
-
 }
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java
index 8851a5422..7a25b2c91 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline2d.java
@@ -116,12 +116,12 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
         System.out.println(procTimeStringBuilder.toString());
         System.out.printf("Pipeline ran in %.3fms\n", totalProcessTimeNanos / 1000.0);
 
-        return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dContoursResult.getLeft());
+        return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dContoursResult.getLeft(), totalProcessTimeNanos / 1000);
     }
 
     public static class CVPipeline2dResult extends CVPipelineResult<Target> {
-        public CVPipeline2dResult(List<Target> targets, Mat outputMat) {
-            super(targets, outputMat);
+        public CVPipeline2dResult(List<Target> targets, Mat outputMat, long processTime) {
+            super(targets, outputMat, processTime);
         }
     }
 
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline3d.java b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline3d.java
index 66029cfe0..b56d23938 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline3d.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/CVPipeline3d.java
@@ -21,8 +21,8 @@ public class CVPipeline3d extends CVPipeline<CVPipeline3dResult, CVPipeline3dSet
 
 
     public static class CVPipeline3dResult extends CVPipelineResult<Target3d> {
-        public CVPipeline3dResult(List<Target3d> targets, Mat outputMat) {
-            super(targets, outputMat);
+        public CVPipeline3dResult(List<Target3d> targets, Mat outputMat, long processTime) {
+            super(targets, outputMat, processTime);
         }
     }
 
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/DriverVisionPipeline.java b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/DriverVisionPipeline.java
index c59a5ea83..394e95edf 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/DriverVisionPipeline.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/pipeline/DriverVisionPipeline.java
@@ -25,12 +25,12 @@ public class DriverVisionPipeline extends CVPipeline<DriverPipelineResult, CVPip
 
         inputMat.copyTo(outputMat);
 
-        return new DriverPipelineResult(null, inputMat);
+        return new DriverPipelineResult(null, inputMat, 0);
     }
 
     public static class DriverPipelineResult extends CVPipelineResult<Void> {
-        public DriverPipelineResult(List<Void> targets, Mat outputMat) {
-            super(targets, outputMat);
+        public DriverPipelineResult(List<Void> targets, Mat outputMat, long processTime) {
+            super(targets, outputMat, processTime);
         }
     }
 }
diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/util/LoopingRunnable.java b/Main/src/main/java/com/chameleonvision/classabstraction/util/LoopingRunnable.java
new file mode 100644
index 000000000..c20bb27f8
--- /dev/null
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/util/LoopingRunnable.java
@@ -0,0 +1,33 @@
+package com.chameleonvision.classabstraction.util;
+
+/**
+ * A thread that tries to run at a specified loop time
+ */
+public abstract class LoopingRunnable implements Runnable {
+    private final Long loopTimeMs;
+
+    protected abstract void process();
+
+    public LoopingRunnable(Long loopTimeMs) {
+        this.loopTimeMs = loopTimeMs;
+    }
+
+    @Override
+    public void run() {
+        while(!Thread.interrupted()) {
+            var now = System.currentTimeMillis();
+
+            // Do the thing
+            process();
+
+            // sleep for the remaining time
+            var timeElapsed = System.currentTimeMillis() - now;
+            var delta = loopTimeMs - timeElapsed;
+            if(delta > 0.0) {
+                try {
+                    Thread.sleep(delta, 0);
+                } catch (Exception ignored) {}
+            }
+        }
+    }
+}