From b9e35e65589f139b456ad14359b8df9e5ef82dea Mon Sep 17 00:00:00 2001
From: Ryzerth <whatsthetgeek@gmail.com>
Date: Tue, 27 Jul 2021 23:51:06 +0200
Subject: [PATCH] Added IQ correction

---
 core/src/dsp/correction.h | 76 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 76 insertions(+)
 create mode 100644 core/src/dsp/correction.h

diff --git a/core/src/dsp/correction.h b/core/src/dsp/correction.h
new file mode 100644
index 00000000..3c50458c
--- /dev/null
+++ b/core/src/dsp/correction.h
@@ -0,0 +1,76 @@
+#pragma once
+#include <dsp/block.h>
+#include <dsp/stream.h>
+#include <dsp/types.h>
+#include <dsp/window.h>
+
+namespace dsp {
+    class IQCorrector : public generic_block<IQCorrector> {
+    public:
+        IQCorrector() {}
+
+        IQCorrector(stream<complex_t>* in, float rate) { init(in, rate); }
+
+        void init(stream<complex_t>* in, float rate) {
+            _in = in;
+            correctionRate = rate;
+            offset.re = 0;
+            offset.im = 0;
+            generic_block<IQCorrector>::registerInput(_in);
+            generic_block<IQCorrector>::registerOutput(&out);
+            generic_block<IQCorrector>::_block_init = true;
+        }
+
+        void setInput(stream<complex_t>* in) {
+            assert(generic_block<IQCorrector>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<IQCorrector>::ctrlMtx);
+            generic_block<IQCorrector>::tempStop();
+            generic_block<IQCorrector>::unregisterInput(_in);
+            _in = in;
+            generic_block<IQCorrector>::registerInput(_in);
+            generic_block<IQCorrector>::tempStart();
+        }
+
+        void setCorrectionRate(float rate) {
+            correctionRate = rate;
+        }
+
+        int run() {
+            int count = _in->read();
+            if (count < 0) { return -1; }
+
+            if (bypass) {
+                memcpy(out.writeBuf, _in->readBuf, count * sizeof(complex_t));
+
+                _in->flush();
+
+                if (!out.swap(count)) { return -1; }
+
+                return count;
+            }
+
+            for (int i = 0; i < count; i++) {
+                out.writeBuf[i] = _in->readBuf[i] - offset;
+                offset = offset + (out.writeBuf[i] * correctionRate);
+            }
+
+            _in->flush();
+
+            if (!out.swap(count)) { return -1; }
+
+            return count;
+        }
+
+        stream<complex_t> out;
+
+        // TEMPORARY FOR DEBUG PURPOSES
+        bool bypass = true;
+        complex_t offset;
+
+    private:
+        stream<complex_t>* _in;
+        float correctionRate = 0.00001;
+        
+
+    };
+}
\ No newline at end of file