From 3ec88eda017982319d45c3ea88b71f599d8c2bef Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sat, 16 Nov 2019 10:17:33 -0800
Subject: [PATCH] Add streaming to VisionProcess and run the threads

---
 .../classabstraction/VisionProcess.java       | 108 ++++++++++++++++--
 1 file changed, 97 insertions(+), 11 deletions(-)

diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
index ded690973..1d742b463 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
@@ -1,6 +1,7 @@
 package com.chameleonvision.classabstraction;
 
 import com.chameleonvision.classabstraction.camera.CameraProcess;
+import com.chameleonvision.classabstraction.camera.CameraStreamer;
 import com.chameleonvision.classabstraction.pipeline.CVPipeline;
 import com.chameleonvision.classabstraction.pipeline.CVPipelineResult;
 import com.chameleonvision.classabstraction.pipeline.CVPipelineSettings;
@@ -14,21 +15,34 @@ public class VisionProcess {
 
     private final CameraProcess cameraProcess;
     private final List<CVPipeline> pipelines = new ArrayList<>();
+    private final CameraFrameRunnable cameraRunnable;
+    private final CameraStramerRunnable streamRunnable;
+    private final VisionProcessRunnable visionRunnable;
     private CVPipeline currentPipeline;
 
-    private final CameraFrameRunnable cameraFrameRunnable;
-
     private final CVPipelineSettings driverVisionSettings = new CVPipelineSettings();
 
-    public VisionProcess(CameraProcess cameraProcess) {
+    public VisionProcess(CameraProcess cameraProcess, String name, Long streamTimeMs, Long cameraUpdateTimeMs) {
         this.cameraProcess = cameraProcess;
 
         pipelines.add(new DriverVisionPipeline(() -> driverVisionSettings));
         setPipeline(pipelines.get(0));
 
-        cameraFrameRunnable = new CameraFrameRunnable();
+        // Thread to grab frames from the camera
+        this.cameraRunnable = new CameraFrameRunnable(cameraUpdateTimeMs) ;
+        new Thread(cameraRunnable).start();
+
+        // Thread to put frames on the dashboard
+        this.streamRunnable = new CameraStramerRunnable(streamTimeMs, new CameraStreamer(cameraProcess, name))
+        new Thread(streamRunnable).start();
+
+        // Thread to process vision data
+        this.visionRunnable = new VisionProcessRunnable();
+        new Thread(visionRunnable).start();
+
     }
 
+
     public void setPipeline(int pipelineIndex) {
         CVPipeline newPipeline = pipelines.get(pipelineIndex);
         if (newPipeline != null) {
@@ -45,15 +59,30 @@ public class VisionProcess {
         return currentPipeline;
     }
 
-    protected class CameraFrameRunnable implements Runnable {
+    /**
+     * CameraFrameRunnable grabs images from the cameraProcess
+     * at a specified loopTime
+     */
+    protected class CameraFrameRunnable extends LoopingRunnable {
         private Mat cameraFrame = new Mat();
         private long timestampMicros;
 
         private final Object frameLock = new Object();
 
+        /**
+         * CameraFrameRunnable grabs images from the cameraProcess
+         * at a specified loopTime
+         * @param loopTimeMs how often to grab frames at in ms
+         */
+        CameraFrameRunnable(Long loopTimeMs) {
+            super(loopTimeMs);
+        }
+
         @Override
-        public void run() {
-            while(Thread.interrupted()) {
+        public void process() {
+            while(!Thread.interrupted()) {
+
+                // Grab camera frames
                 var camData = cameraProcess.getFrame();
                 synchronized (frameLock) {
                     cameraFrame = camData.getLeft();
@@ -62,8 +91,11 @@ public class VisionProcess {
             }
         }
 
-        public Mat getFrame() {
-            return cameraFrame;
+        public Mat getFrame(Mat dst) {
+            synchronized (frameLock) {
+                cameraFrame.copyTo(dst);
+                return dst;
+            }
         }
 
         public long getTimestampMicros() {
@@ -71,15 +103,69 @@ public class VisionProcess {
         }
     }
 
-    private class VisionThread implements Runnable {
+    /**
+     * VisionProcessRunnable will process images as quickly as possible
+     */
+    private class VisionProcessRunnable implements Runnable {
 
         private CVPipelineResult result;
+        private Mat streamBuffer = new Mat();
 
         @Override
         public void run() {
             while(!Thread.interrupted()) {
-                result = currentPipeline.runPipeline(cameraFrameRunnable.getFrame());
+                result = currentPipeline.runPipeline(cameraRunnable.getFrame(streamBuffer));
+                // TODO do something with the result
             }
         }
     }
+
+    private class CameraStramerRunnable extends LoopingRunnable {
+
+        private final CameraStreamer streamer;
+        private Mat streamBuffer = new Mat();
+
+        private CameraStramerRunnable(Long loopTimeMs, CameraStreamer streamer) {
+            super(loopTimeMs);
+            this.streamer = streamer;
+        }
+
+        @Override
+        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) {}
+                }
+            }
+        }
+    }
+
 }