From 0a8a08e05f9d04680d884a0b125af4732c18f982 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Sun, 24 May 2020 15:59:51 -0700
Subject: [PATCH] Revert "Simplify moving average code"

This reverts commit 4da60e75794d79153c7fca2b0f667ca70fe4d403.
---
 .../car/tesla/speed_utils/movingaverage.py    | 57 ++++++++++++++-----
 1 file changed, 44 insertions(+), 13 deletions(-)

diff --git a/selfdrive/car/tesla/speed_utils/movingaverage.py b/selfdrive/car/tesla/speed_utils/movingaverage.py
index 75c1e6923bd53f..07ba16f463a116 100644
--- a/selfdrive/car/tesla/speed_utils/movingaverage.py
+++ b/selfdrive/car/tesla/speed_utils/movingaverage.py
@@ -1,18 +1,49 @@
-import queue
-
 class MovingAverage():
   def __init__(self, length):
+    self.position = 0
     self.length = length
-    self.reset()
-    
+    self.sum = 0.
+    self.no_items = 0
+    self.values = [0.] * length
+
   def reset(self):
-    self.queue = queue.Queue(maxsize=self.length)
-    self.sum = 0
+    self.position = 0
+    self.sum = 0.
+    self.no_items = 0
+    self.values = [0.] * self.length 
+
+  def add(self,element):
+    if self.no_items == self.length:
+      self.no_items -= 1
+      self.sum -= self.values[self.position]
+    self.values[self.position] = element
+    self.sum += self.values[self.position]
+    self.no_items += 1
+    self.position += 1
+    if self.sum == 0.:
+      #all empty so initialize
+      self.position = 0
+      self.sum = 0.
+      self.no_items = 0
+      return 0.
+    self.position = self.position % self.length
+    return self.sum/self.no_items
 
-  def add(self, sample):
-    if self.queue.full():
-      oldest_sample = q.get_nowait()
-      self.sum -= oldest_sample
-    self.queue.put_nowait(sample)
-    self.sum += sample
-    return self.sum / self.queue.qsize()
+  def dele(self):
+    if self.no_items == 0:
+      return 0.
+    if self.no_items > 0:
+      self.no_items -= 1
+      self.sum -= self.values[self.position]
+      self.values[self.position] = 0.
+      self.position -= 1
+      if self.position < 0:
+        self.position = self.length-1
+    if self.sum == 0. or self.no_items == 0.:
+      #all empty so initialize
+      self.position = 0
+      self.sum = 0.
+      self.no_items = 0
+      return 0.
+    self.position = self.position % self.length
+    return self.sum/self.no_items