From be600231e57728fd70ffaaea8d3b1c71c34cb506 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Sat, 26 Sep 2020 20:34:20 +0800
Subject: [PATCH] add LinearPhaseFilter and Delay

---
 noptica.py | 35 +++++++++++++++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/noptica.py b/noptica.py
index 474fa97..06a513b 100644
--- a/noptica.py
+++ b/noptica.py
@@ -4,7 +4,7 @@ import threading
 
 import SoapySDR
 import numpy as np
-from scipy.signal import blackmanharris
+from scipy import signal
 
 
 class BufferedSDR:
@@ -127,7 +127,7 @@ class Stabilizer:
         self.wiggle_amplitude = 0.15
 
     def input(self, samples):
-        spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples))))
+        spectrum = np.abs(np.fft.fft(samples*signal.blackmanharris(len(samples))))
         i = np.argmax(spectrum)
         amplitude = spectrum[i]
 
@@ -189,3 +189,34 @@ class PositionTracker:
         self.last_phase = phase[-1]
         self.last_position = position[-1]
         return position
+
+
+class LinearPhaseFilter:
+    def __init__(self, numtaps, *args, **kwargs):
+        self.numtaps = numtaps
+        self.coef = signal.firwin(numtaps, *args, **kwargs)
+        self.state = np.zeros(self.numtaps - 1)
+
+    def input(self, block):
+        output, self.state = signal.lfilter(self.coef, [1.0], block, zi=self.state)
+        return output
+
+    def delay(self):
+        assert(self.numptaps % 2 == 1)
+        return (self.numptaps - 1)//2
+
+
+class Delay:
+    def __init__(self, block_size, delay_amount):
+        self.block_size = block_size
+        self.delay_amount = delay_amount
+        self.output = np.zeros(block_size)
+        self.next = np.zeros(delay_amount)
+
+    def input(self, block):
+        assert(len(block) == self.block_size)
+        split = self.block_size - self.delay_amount
+        self.output[:self.delay_amount] = self.next
+        self.output[self.delay_amount:] = block[:split]
+        self.next = block[split:]
+        return self.output