From 355a6352da51bbb9b782fba4d68d4882cf6fa0a6 Mon Sep 17 00:00:00 2001
From: AlexandreRouma <alexandre.rouma@gmail.com>
Date: Tue, 7 Dec 2021 02:13:23 +0100
Subject: [PATCH] New radio logic system

---
 core/src/dsp/chain.h                         | 208 +++++++++++++++++++
 core/src/dsp/noise_reduction.h               | 122 +++++++++++
 decoder_modules/new_radio/src/radio_module.h | 196 +++++++++--------
 3 files changed, 438 insertions(+), 88 deletions(-)
 create mode 100644 core/src/dsp/chain.h

diff --git a/core/src/dsp/chain.h b/core/src/dsp/chain.h
new file mode 100644
index 00000000..1045d506
--- /dev/null
+++ b/core/src/dsp/chain.h
@@ -0,0 +1,208 @@
+#pragma once
+#include <dsp/stream.h>
+#include <type_traits>
+#include <vector>
+#include <utils/event.h>
+
+namespace dsp {
+    template <class T>
+    class ChainLinkAny {
+    public:
+        virtual ~ChainLinkAny() {}
+        virtual void setInput(stream<T>* stream) = 0;
+        virtual stream<T>* getOutput() = 0;
+        virtual void start() = 0;
+        virtual void stop() = 0;
+        bool enabled = false;
+    };
+
+    template <class BLOCK, class T>
+    class ChainLink : public ChainLinkAny<T> {
+    public:
+        ~ChainLink() {}
+        
+        void setInput(stream<T>* stream) {
+            block.setInput(stream);
+        }
+        
+        stream<T>* getOutput() {
+            return &block.out;
+        }
+
+        void start() {
+            block.start();
+        }
+
+        void stop() {
+            block.stop();
+        }
+
+        BLOCK block;
+    };
+
+    template <class T>
+    class Chain {
+    public:
+        Chain() {}
+
+        Chain(stream<T>* input, EventHandler<stream<T>*>* outputChangedHandler) {
+            init(input, outputChangedHandler);
+        }
+
+        void init(stream<T>* input, EventHandler<stream<T>*>* outputChangedHandler) {
+            _input = input;
+            onOutputChanged.bindHandler(outputChangedHandler);
+        }
+
+        void add(ChainLinkAny<T>* link) {
+            // Check that link exists
+            if (std::find(links.begin(), links.end(), link) != links.end()) {
+                spdlog::error("Could not add new link to the chain, link already in the chain");
+                return;
+            }
+            
+            // Assert that the link is stopped and disabled
+            link->stop();
+            link->enabled = false;
+
+            // Add new link to the list
+            links.push_back(link);
+        }
+
+        void enable(ChainLinkAny<T>* link) {
+            // Check that link exists and locate it
+            auto lnit = std::find(links.begin(), links.end(), link);
+            if (lnit == links.end()) {
+                spdlog::error("Could not enable link");
+                return;
+            }
+            // Enable the link
+            link->enabled = true;
+
+            // Find input
+            stream<T>* input = _input;
+            for (auto i = links.begin(); i < lnit; i++) {
+                if (!(*i)->enabled) { continue; }
+                input = (*i)->getOutput();
+            }
+            
+            // Find next block
+            ChainLinkAny<T>* nextLink = NULL;
+            for (auto i = ++lnit; i < links.end(); i++) {
+                if (!(*i)->enabled) { continue; }
+                nextLink = *i;
+            }
+
+            if (nextLink) {
+                // If a next block exists, change its input
+                nextLink->setInput(link->getOutput());
+            }
+            else {
+                // If there are no next blocks, change output of outside reader
+                onOutputChanged.emit(link->getOutput());
+            }
+
+            // Set input of newly enabled link
+            link->setInput(input);
+
+            // If running, start everything
+            if (running) { start(); }
+        }
+
+        void disable(ChainLinkAny<T>* link) {
+            // Check that link exists and locate it
+            auto lnit = std::find(links.begin(), links.end(), link);
+            if (lnit == links.end()) {
+                spdlog::error("Could not disable link");
+                return;
+            }
+
+            // Stop and disable link
+            link->stop();
+            link->enabled = false;
+
+            // Find its input
+            stream<T>* input = _input;
+            for (auto i = links.begin(); i < lnit; i++) {
+                if (!(*i)->enabled) { continue; }
+                input = (*i)->getOutput();
+            }
+            
+            // Find next block
+            ChainLinkAny<T>* nextLink = NULL;
+            for (auto i = ++lnit; i < links.end(); i++) {
+                if (!(*i)->enabled) { continue; }
+                nextLink = *i;
+            }
+
+            if (nextLink) {
+                // If a next block exists, change its input
+                nextLink->setInput(input);
+            }
+            else {
+                // If there are no next blocks, change output of outside reader
+                onOutputChanged.emit(input);
+            }
+        }
+
+        void disableAll() {
+            for (auto& ln : links) {
+                disable(ln);
+            }
+        }
+
+        void setState(ChainLinkAny<T>* link, bool enabled) {
+            enabled ? enable(link) : disable(link);
+        }
+
+        void setInput(stream<T>* input) {
+            _input = input;
+            
+            // Set input of first enabled link
+            for (auto& ln : links) {
+                if (!ln->enabled) { continue; }
+                ln->setInput(_input);
+                return;
+            }
+
+            // No block found, this means nothing is enabled
+            onOutputChanged.emit(_input);
+        }
+
+        stream<T>* getOutput() {
+            stream<T>* lastOutput = _input;
+            for (auto& ln : links) {
+                if (!ln->enabled) { continue; }
+                lastOutput = ln->getOutput();
+            }
+            return lastOutput;
+        }
+
+        void start() {
+            running = true;
+            for (auto& ln : links) {
+                if (ln->enabled) {
+                    ln->start();
+                }
+                else {
+                    ln->stop();
+                }
+            }
+        }
+
+        void stop() {
+            running = false;
+            for (auto& ln : links) {
+                ln->stop();
+            }
+        }
+
+        Event<stream<T>*> onOutputChanged;
+
+    private:
+        stream<T>* _input;
+        std::vector<ChainLinkAny<T>*> links;
+        bool running = false;
+
+    };
+}
\ No newline at end of file
diff --git a/core/src/dsp/noise_reduction.h b/core/src/dsp/noise_reduction.h
index 52334e6d..b1eb8021 100644
--- a/core/src/dsp/noise_reduction.h
+++ b/core/src/dsp/noise_reduction.h
@@ -128,4 +128,126 @@ namespace dsp {
         float* fft_fout;
 
     };
+
+    class NoiseBlanker : public generic_block<NoiseBlanker> {
+    public:
+        NoiseBlanker() {}
+
+        NoiseBlanker(stream<complex_t>* in, float attack, float decay, float threshold, float level, float sampleRate) { init(in, attack, decay, threshold, level, sampleRate); }
+
+        ~NoiseBlanker() {
+            if (!generic_block<NoiseBlanker>::_block_init) { return; }
+            generic_block<NoiseBlanker>::stop();
+            volk_free(ampBuf);
+        }
+
+        void init(stream<complex_t>* in, float attack, float decay, float threshold, float level, float sampleRate) {
+            _in = in;
+            _attack = attack;
+            _decay = decay;
+            _threshold = powf(10.0f, threshold / 10.0f);
+            _level = level;
+            _sampleRate = sampleRate;
+
+            _inv_attack = 1.0f - _attack;
+            _inv_decay = 1.0f - _decay;
+
+            ampBuf = (float*)volk_malloc(STREAM_BUFFER_SIZE*sizeof(float), volk_get_alignment());
+
+            generic_block<NoiseBlanker>::registerInput(_in);
+            generic_block<NoiseBlanker>::registerOutput(&out);
+            generic_block<NoiseBlanker>::_block_init = true;
+        }
+
+        void setAttack(float attack) {
+            _attack = attack;
+            _inv_attack = 1.0f - _attack;
+        }
+
+        void setDecay(float decay) {
+            _decay = decay;
+            _inv_decay = 1.0f - _decay;
+        }
+
+        void setThreshold(float threshold) {
+            _threshold = powf(10.0f, threshold / 10.0f);
+            spdlog::warn("Threshold {0}", _threshold);
+        }
+
+        void setLevel(float level) {
+            _level = level;
+        }
+
+        void setSampleRate(float sampleRate) {
+            _sampleRate = sampleRate;
+            // TODO: Change parameters if the algo needs it
+        }
+
+        void setInput(stream<complex_t>* in) {
+            assert(generic_block<NoiseBlanker>::_block_init);
+            std::lock_guard<std::mutex> lck(generic_block<NoiseBlanker>::ctrlMtx);
+            generic_block<NoiseBlanker>::tempStop();
+            generic_block<NoiseBlanker>::unregisterInput(_in);
+            _in = in;
+            generic_block<NoiseBlanker>::registerInput(_in);
+            generic_block<NoiseBlanker>::tempStart();
+        }
+
+        int run() {
+            int count = _in->read();
+            if (count < 0) { return -1; }
+
+            // Get amplitudes
+            volk_32fc_magnitude_32f(ampBuf, (lv_32fc_t*)_in->readBuf, count);
+
+            // Apply filtering and threshold
+            float val;
+            for (int i = 0; i < count; i++) {
+                // Filter using attack/threshold methode
+                val = ampBuf[i];
+                if (val > lastValue) {
+                    lastValue = (_inv_attack*lastValue) + (_attack*val);
+                }
+                else {
+                    lastValue = (_inv_decay*lastValue) + (_decay*val);
+                }
+
+                // Apply threshold and invert
+                if (lastValue > _threshold) {
+                    ampBuf[i] = _threshold / (lastValue * _level);
+                    if (ampBuf[i] == 0) {
+                        spdlog::warn("WTF???");
+                    }
+                }
+                else {
+                    ampBuf[i] = 1.0f;
+                }
+            }
+
+            // Multiply
+            volk_32fc_32f_multiply_32fc((lv_32fc_t*)out.writeBuf, (lv_32fc_t*)_in->readBuf, ampBuf, count);
+
+            _in->flush();
+            if (!out.swap(count)) { return -1; }
+            return count;
+        }
+
+        stream<complex_t> out;
+
+    private:
+        float* ampBuf;
+
+        float _attack;
+        float _decay;
+        float _inv_attack;
+        float _inv_decay;
+        float _threshold;
+        float _level;
+        float _sampleRate;
+
+        float lastValue = 0.0f;
+        
+        stream<complex_t>* _in;
+
+    };
 }
\ No newline at end of file
diff --git a/decoder_modules/new_radio/src/radio_module.h b/decoder_modules/new_radio/src/radio_module.h
index 4c525024..509ceef7 100644
--- a/decoder_modules/new_radio/src/radio_module.h
+++ b/decoder_modules/new_radio/src/radio_module.h
@@ -5,6 +5,8 @@
 #include <gui/style.h>
 #include <signal_path/signal_path.h>
 #include <config.h>
+#include <dsp/chain.h>
+#include <dsp/noise_reduction.h>
 
 ConfigManager config;
 
@@ -52,13 +54,16 @@ public:
         onUserChangedBandwidthHandler.ctx = this;
         vfo->wtfVFO->onUserChangedBandwidth.bindHandler(&onUserChangedBandwidthHandler);
 
-        // Initialize the sink
-        srChangeHandler.ctx = this;
-        srChangeHandler.handler = sampleRateChangeHandler;
-        stream.init(&deemp.out, &srChangeHandler, audioSampleRate);
-        sigpath::sinkManager.registerStream(name, &stream);
+        // Initialize IF DSP chain
+        ifChainOutputChanged.ctx = this;
+        ifChainOutputChanged.handler = ifChainOutputChangeHandler;
+        ifChain.init(vfo->output, &ifChainOutputChanged);
 
-        // Load configuration for all demodulators
+        squelch.block.init(NULL, MIN_SQUELCH);
+
+        ifChain.add(&squelch);
+
+        // Load configuration for and enabled all demodulators
         EventHandler<dsp::stream<dsp::stereo_t>*> _demodOutputChangeHandler;
         _demodOutputChangeHandler.handler = demodOutputChangeHandler;
         _demodOutputChangeHandler.ctx = this;
@@ -77,19 +82,37 @@ public:
             bw = std::clamp<double>(bw, demod->getMinBandwidth(), demod->getMaxBandwidth());
             
             // Initialize
-            demod->init(name, &config, &squelch.out, bw, _demodOutputChangeHandler, stream.getSampleRate());
+            demod->init(name, &config, ifChain.getOutput(), bw, _demodOutputChangeHandler, stream.getSampleRate());
         }
+        
+        // Initialize audio DSP chain
+        afChainOutputChanged.ctx = this;
+        afChainOutputChanged.handler = afChainOutputChangeHandler;
+        afChain.init(&dummyAudioStream, &afChainOutputChanged);
 
-        // Initialize DSP
-        squelch.init(vfo->output, MIN_SQUELCH);
         win.init(24000, 24000, 48000);
-        resamp.init(NULL, &win, 250000, 48000);
-        deemp.init(&resamp.out, 48000, 50e-6);
-        deemp.bypass = false;
+        resamp.block.init(NULL, &win, 250000, 48000);
+        deemp.block.init(NULL, 48000, 50e-6);
+        deemp.block.bypass = false;
+
+        afChain.add(&resamp);
+        afChain.add(&deemp);
+
+        // Initialize the sink
+        srChangeHandler.ctx = this;
+        srChangeHandler.handler = sampleRateChangeHandler;
+        stream.init(afChain.getOutput(), &srChangeHandler, audioSampleRate);
+        sigpath::sinkManager.registerStream(name, &stream);
 
         // Select the demodulator
         selectDemodByID((DemodID)selectedDemodID);
 
+        // Start IF chain
+        ifChain.start();
+
+        // Start AF chain
+        afChain.start();
+
         // Start stream, the rest was started when selecting the demodulator
         stream.start();
 
@@ -100,11 +123,7 @@ public:
         gui::menu.removeEntry(name);
         stream.stop();
         if (enabled) {
-            squelch.stop();
-            if (selectedDemod) { selectedDemod->stop(); }
-            resamp.stop();
-            deemp.stop();
-            if (vfo) { sigpath::vfoManager.deleteVFO(vfo); }
+            disable();
         }
         sigpath::sinkManager.unregisterStream(name);
     }
@@ -122,10 +141,9 @@ public:
 
     void disable() {
         enabled = false;
-        squelch.stop();
+        ifChain.stop();
         if (selectedDemod) { selectedDemod->stop(); }
-        resamp.stop();
-        deemp.stop();
+        afChain.stop();
         if (vfo) { sigpath::vfoManager.deleteVFO(vfo); }
         vfo = NULL;
     }
@@ -232,7 +250,7 @@ private:
         ImGui::SameLine();
         ImGui::SetNextItemWidth(menuWidth - ImGui::GetCursorPosX());
         if (ImGui::SliderFloat(("##_radio_sqelch_lvl_" + _this->name).c_str(), &_this->squelchLevel, _this->MIN_SQUELCH, _this->MAX_SQUELCH, "%.3fdB")) {
-            _this->squelch.setLevel(_this->squelchLevel);
+            _this->squelch.block.setLevel(_this->squelchLevel);
             config.acquire();
             config.conf[_this->name][_this->selectedDemod->getName()]["squelchLevel"] = _this->squelchLevel;
             config.release(true);
@@ -252,6 +270,7 @@ private:
         }
         selectedDemodID = id;
         selectDemod(demod);
+
         // Save config
         config.acquire();
         config.conf[name]["selectedDemodId"] = id;
@@ -266,6 +285,12 @@ private:
         // Give the demodulator the most recent audio SR
         selectedDemod->AFSampRateChanged(audioSampleRate);
 
+        // Set the demodulator's input
+        selectedDemod->setInput(ifChain.getOutput());
+
+        // Set AF chain's input
+        afChain.setInput(selectedDemod->getOutput());
+
         // Load config
         bandwidth = selectedDemod->getDefaultBandwidth();
         minBandwidth = selectedDemod->getMinBandwidth();
@@ -276,6 +301,7 @@ private:
         deempAllowed = selectedDemod->getDeempAllowed();
         deempMode = DEEMP_MODE_NONE;
         squelchEnabled = false;
+        postProcEnabled = selectedDemod->getPostProcEnabled();
         if (config.conf[name][selectedDemod->getName()].contains("snapInterval")) {
             bandwidth = config.conf[name][selectedDemod->getName()]["bandwidth"];
             bandwidth = std::clamp<double>(bandwidth, minBandwidth, maxBandwidth);
@@ -302,27 +328,24 @@ private:
             vfo->setSampleRate(selectedDemod->getIFSampleRate(), bandwidth);
         }
 
-        // Configure squelch
-        squelch.setLevel(squelchLevel);
+        // Configure IF chain
+        squelch.block.setLevel(squelchLevel);
         setSquelchEnabled(squelchEnabled);
 
-        // Enable or disable post processing entirely depending on the demodulator's options
-        setPostProcEnabled(selectedDemod->getPostProcEnabled());
-
+        // Configure AF chain
         if (postProcEnabled) {
             // Configure resampler
-            resamp.stop();
-            resamp.setInput(selectedDemod->getOutput());
-            resamp.setInSampleRate(selectedDemod->getAFSampleRate());
+            afChain.stop();
+            resamp.block.setInSampleRate(selectedDemod->getAFSampleRate());
             setAudioSampleRate(audioSampleRate);
+            afChain.enable(&resamp);
 
             // Configure deemphasis
-            if (deempAllowed) {
-                setDeemphasisMode(deempMode);
-            }
-            else {
-                setDeemphasisMode(DEEMP_MODE_NONE);
-            }
+            setDeemphasisMode(deempMode);
+        }
+        else {
+            // Disable everyting if post processing is disabled
+            afChain.disableAll();
         }
 
         // Start new demodulator
@@ -337,11 +360,14 @@ private:
         audioBW = std::min<float>(audioBW, audioSampleRate / 2.0);
         vfo->setBandwidth(bandwidth);
         selectedDemod->setBandwidth(bandwidth);
+
+        // Only bother with setting the resampling setting if we're actually post processing and dynamic bw is enabled
         if (selectedDemod->getDynamicAFBandwidth() && postProcEnabled) {
             win.setCutoff(audioBW);
             win.setTransWidth(audioBW);
-            resamp.updateWindow(&win);
+            resamp.block.updateWindow(&win);
         }
+
         config.acquire();
         config.conf[name][selectedDemod->getName()]["bandwidth"] = bandwidth;
         config.release(true);
@@ -352,6 +378,7 @@ private:
         if (!selectedDemod) { return; }
         selectedDemod->AFSampRateChanged(audioSampleRate);
         if (!postProcEnabled) {
+            // If postproc is disabled, IF SR = AF SR
             minBandwidth = selectedDemod->getMinBandwidth();
             maxBandwidth = selectedDemod->getMaxBandwidth();
             bandwidth = selectedDemod->getIFSampleRate();
@@ -361,59 +388,36 @@ private:
         }
         float audioBW = std::min<float>(selectedDemod->getMaxAFBandwidth(), selectedDemod->getAFBandwidth(bandwidth));
         audioBW = std::min<float>(audioBW, audioSampleRate / 2.0);
-        resamp.stop();
-        deemp.stop();
-        deemp.setSampleRate(audioSampleRate);
-        resamp.setOutSampleRate(audioSampleRate);
-        win.setSampleRate(selectedDemod->getAFSampleRate() * resamp.getInterpolation());
+
+        afChain.stop();
+
+        // Configure resampler
+        resamp.block.setOutSampleRate(audioSampleRate);
+        win.setSampleRate(selectedDemod->getAFSampleRate() * resamp.block.getInterpolation());
         win.setCutoff(audioBW);
         win.setTransWidth(audioBW);
-        resamp.updateWindow(&win);
-        resamp.start();
-        if (deempMode != DEEMP_MODE_NONE) { deemp.start(); }
-    }
+        resamp.block.updateWindow(&win);
 
-    void setPostProcEnabled(bool enable) {
-        postProcEnabled = enable;
-        if (!selectedDemod) { return; }
-        if (postProcEnabled) {
-            setDeemphasisMode(deempMode);
-        }
-        else {
-            resamp.stop();
-            deemp.stop();
-            stream.setInput(selectedDemod->getOutput());
-        }
+        // Configure deemphasis sample rate
+        deemp.block.setSampleRate(audioSampleRate);
+        
+        afChain.start();
     }
 
     void setDeemphasisMode(int mode) {
         deempMode = mode;
         if (!postProcEnabled) { return; }
-        if (deempMode != DEEMP_MODE_NONE) {
-            // TODO: Investigate why not stopping the deemp here causes the DSP to stall
-            deemp.stop();
-            stream.setInput(&deemp.out);
-            deemp.setTau(DeemphasisModes[deempMode]);
-            deemp.start();
-        }
-        else {
-            deemp.stop();
-            stream.setInput(&resamp.out);
+        bool deempEnabled = (deempMode != DEEMP_MODE_NONE);
+        if (deempEnabled) {
+            deemp.block.setTau(DeemphasisModes[deempMode]);
         }
+        afChain.setState(&deemp, deempEnabled);
     }
 
     void setSquelchEnabled(bool enable) {
         squelchEnabled = enable;
         if (!selectedDemod) { return; }
-        if (squelchEnabled) {
-            squelch.setInput(vfo->output);
-            selectedDemod->setInput(&squelch.out);
-            squelch.start();
-        }
-        else {
-            squelch.stop();
-            selectedDemod->setInput(vfo->output);
-        }
+        ifChain.setState(&squelch, squelchEnabled);
     }
 
     static void vfoUserChangedBandwidthHandler(double newBw, void* ctx) {
@@ -428,23 +432,39 @@ private:
 
     static void demodOutputChangeHandler(dsp::stream<dsp::stereo_t>* output, void* ctx) {
         NewRadioModule* _this = (NewRadioModule*)ctx;
-        if (_this->postProcEnabled) {
-            _this->resamp.setInput(output);
-        }
-        else {
-            _this->stream.setInput(output);
-        }
+        _this->afChain.setInput(output);
+    }
+
+    static void ifChainOutputChangeHandler(dsp::stream<dsp::complex_t>* output, void* ctx) {
+        NewRadioModule* _this = (NewRadioModule*)ctx;
+        if (!_this->selectedDemod) { return; }
+        _this->selectedDemod->setInput(output);
+    }
+
+    static void afChainOutputChangeHandler(dsp::stream<dsp::stereo_t>* output, void* ctx) {
+        NewRadioModule* _this = (NewRadioModule*)ctx;
+        _this->stream.setInput(output);
     }
     
+    // Handlers
     EventHandler<double> onUserChangedBandwidthHandler;
-    VFOManager::VFO* vfo = NULL;
-    dsp::Squelch squelch;
-
-    dsp::filter_window::BlackmanWindow win;
-    dsp::PolyphaseResampler<dsp::stereo_t> resamp;
-    dsp::BFMDeemp deemp;
-
     EventHandler<float> srChangeHandler;
+    EventHandler<dsp::stream<dsp::complex_t>*> ifChainOutputChanged;
+    EventHandler<dsp::stream<dsp::stereo_t>*> afChainOutputChanged;
+
+    VFOManager::VFO* vfo = NULL;
+
+    // IF chain
+    dsp::Chain<dsp::complex_t> ifChain;
+    dsp::ChainLink<dsp::Squelch, dsp::complex_t> squelch;
+
+    // Audio chain
+    dsp::stream<dsp::stereo_t> dummyAudioStream;
+    dsp::Chain<dsp::stereo_t> afChain;
+    dsp::filter_window::BlackmanWindow win;
+    dsp::ChainLink<dsp::PolyphaseResampler<dsp::stereo_t>, dsp::stereo_t> resamp;
+    dsp::ChainLink<dsp::BFMDeemp, dsp::stereo_t> deemp;
+
     SinkManager::Stream stream;
 
     std::array<demod::Demodulator*, _RADIO_DEMOD_COUNT> demods;