From 2e6f7dbae4e5fae5f67154227c61db3610b34a9d Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sat, 23 Nov 2019 08:22:20 -0800
Subject: [PATCH] Average out the FPS of vision process

Still need to do somethign with it
---
 .../classabstraction/VisionProcess.java       | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
index d60c986ec..5e07b26cd 100644
--- a/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
+++ b/Main/src/main/java/com/chameleonvision/classabstraction/VisionProcess.java
@@ -14,6 +14,7 @@ import edu.wpi.first.networktables.EntryListenerFlags;
 import edu.wpi.first.networktables.EntryNotification;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpiutil.CircularBuffer;
 import org.opencv.core.Mat;
 
 import java.util.ArrayList;
@@ -325,11 +326,13 @@ public class VisionProcess {
     private class VisionProcessRunnable implements Runnable {
 
         public Double fps = 0.0; // TODO: (HIGH) update or average or something
+        private CircularBuffer fpsAveragingBuffer = new CircularBuffer(7);
         private CVPipelineResult result;
         private Mat streamBuffer = new Mat();
 
         @Override
         public void run() {
+            var lastUpdateTimeNanos = System.nanoTime();
             while(!Thread.interrupted()) {
                 System.out.println("running vision process");
 
@@ -354,9 +357,25 @@ public class VisionProcess {
                 } else {
 //                    System.err.println("Bad streambuffer mat");
                 }
+
+                var deltaTimeNanos = lastUpdateTimeNanos - System.nanoTime();
+                fpsAveragingBuffer.addFirst(1.0 / (deltaTimeNanos * 1E-09));
+                lastUpdateTimeNanos = System.nanoTime();
+                fps = getAverageFPS();
+
                 // TODO: (HIGH) do something with the result
             }
         }
+
+        public double getAverageFPS() {
+            var temp = 0.0;
+            for(int i = 0; i < 7; i++) {
+                temp += fpsAveragingBuffer.get(i);
+            }
+            temp /= 7.0;
+            return temp;
+        }
+
     }
 
     private class CameraStreamerRunnable extends LoopingRunnable {