From 6662357bcf326eca6c58f2d57b01972b4719a06d Mon Sep 17 00:00:00 2001 From: f4exb Date: Mon, 29 Jul 2024 08:57:02 +0200 Subject: [PATCH] WDSP: ANF, ANR, EMNR: use vectors instead of C arrays and disable copy constructor --- plugins/channelrx/wdsprx/wdsprxsink.cpp | 20 +- wdsp/RXA.cpp | 72 +- wdsp/RXA.hpp | 4 +- wdsp/amd.hpp | 1 + wdsp/amsq.hpp | 1 + wdsp/anb.hpp | 1 + wdsp/anf.cpp | 201 ++-- wdsp/anf.hpp | 36 +- wdsp/anr.cpp | 210 ++-- wdsp/anr.hpp | 33 +- wdsp/bpsnba.hpp | 1 + wdsp/emnr.cpp | 1348 ++++++++++++----------- wdsp/emnr.hpp | 167 ++- wdsp/eqp.hpp | 1 + wdsp/fmd.hpp | 1 + wdsp/fmsq.hpp | 1 + wdsp/nbp.hpp | 2 + wdsp/nob.hpp | 1 + wdsp/resample.hpp | 1 + 19 files changed, 1143 insertions(+), 959 deletions(-) diff --git a/plugins/channelrx/wdsprx/wdsprxsink.cpp b/plugins/channelrx/wdsprx/wdsprxsink.cpp index d03c380b1..f72df0145 100644 --- a/plugins/channelrx/wdsprx/wdsprxsink.cpp +++ b/plugins/channelrx/wdsprx/wdsprxsink.cpp @@ -510,12 +510,12 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force) switch (settings.m_nrPosition) { case WDSPRxProfile::NRPositionPreAGC: - WDSP::ANR::SetANRPosition(*m_rxa, 0); - WDSP::EMNR::SetEMNRPosition(*m_rxa, 0); + WDSP::RXA::SetANRPosition(*m_rxa, 0); + WDSP::RXA::SetEMNRPosition(*m_rxa, 0); break; case WDSPRxProfile::NRPositionPostAGC: - WDSP::ANR::SetANRPosition(*m_rxa, 1); - WDSP::EMNR::SetEMNRPosition(*m_rxa, 1); + WDSP::RXA::SetANRPosition(*m_rxa, 1); + WDSP::RXA::SetEMNRPosition(*m_rxa, 1); break; default: break; @@ -527,13 +527,13 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force) switch (settings.m_nr2Gain) { case WDSPRxProfile::NR2GainLinear: - WDSP::EMNR::SetEMNRgainMethod(*m_rxa, 0); + m_rxa->emnr->setGainMethod(0); break; case WDSPRxProfile::NR2GainLog: - WDSP::EMNR::SetEMNRgainMethod(*m_rxa, 1); + m_rxa->emnr->setGainMethod(1); break; case WDSPRxProfile::NR2GainGamma: - WDSP::EMNR::SetEMNRgainMethod(*m_rxa, 2); + m_rxa->emnr->setGainMethod(2); break; default: break; @@ -545,10 +545,10 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force) switch (settings.m_nr2NPE) { case WDSPRxProfile::NR2NPEOSMS: - WDSP::EMNR::SetEMNRnpeMethod(*m_rxa, 0); + m_rxa->emnr->setNpeMethod(0); break; case WDSPRxProfile::NR2NPEMMSE: - WDSP::EMNR::SetEMNRnpeMethod(*m_rxa, 1); + m_rxa->emnr->setNpeMethod(1); break; default: break; @@ -556,7 +556,7 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force) } if ((m_settings.m_nr2ArtifactReduction != settings.m_nr2ArtifactReduction) || force) { - WDSP::EMNR::SetEMNRaeRun(*m_rxa, settings.m_nr2ArtifactReduction ? 1 : 0); + m_rxa->emnr->setAeRun(settings.m_nr2ArtifactReduction ? 1 : 0); } if ((m_settings.m_anf != settings.m_anf) || force) { diff --git a/wdsp/RXA.cpp b/wdsp/RXA.cpp index 4c1d700d1..bae905898 100644 --- a/wdsp/RXA.cpp +++ b/wdsp/RXA.cpp @@ -347,7 +347,7 @@ RXA* RXA::create_rxa ( } // Auto notch filter - rxa->anf = ANF::create_anf ( + rxa->anf = new ANF( 0, // run - OFF by default 0, // position rxa->dsp_size, // buffer size @@ -367,7 +367,7 @@ RXA* RXA::create_rxa ( 3.0); // ldecr // LMS noise reduction (ANR or "NR") - rxa->anr = ANR::create_anr ( + rxa->anr = new ANR( 0, // run - OFF by default 0, // position rxa->dsp_size, // buffer size @@ -387,7 +387,7 @@ RXA* RXA::create_rxa ( 3.0); // ldecr // Spectral noise reduyction (EMNR or "NR2") - rxa->emnr = EMNR::create_emnr ( + rxa->emnr = new EMNR( 0, // run 0, // position rxa->dsp_size, // buffer size @@ -573,9 +573,9 @@ void RXA::destroy_rxa (RXA *rxa) BANDPASS::destroy_bandpass (rxa->bp1); delete (rxa->agcmeter); WCPAGC::destroy_wcpagc (rxa->agc); - EMNR::destroy_emnr (rxa->emnr); - ANR::destroy_anr (rxa->anr); - ANF::destroy_anf (rxa->anf); + delete (rxa->emnr); + delete (rxa->anr); + delete (rxa->anf); delete (rxa->eqp); delete (rxa->snba); delete (rxa->fmsq); @@ -618,9 +618,9 @@ void RXA::flush_rxa (RXA *rxa) rxa->fmsq->flush(); rxa->snba->flush(); rxa->eqp->flush(); - ANF::flush_anf (rxa->anf); - ANR::flush_anr (rxa->anr); - EMNR::flush_emnr (rxa->emnr); + rxa->anf->flush(); + rxa->anr->flush(); + rxa->emnr->flush(); WCPAGC::flush_wcpagc (rxa->agc); rxa->agcmeter->flush(); BANDPASS::flush_bandpass (rxa->bp1); @@ -653,14 +653,14 @@ void RXA::xrxa (RXA *rxa) rxa->bpsnba->exec_out(1); rxa->snba->execute(); rxa->eqp->execute(); - ANF::xanf (rxa->anf, 0); - ANR::xanr (rxa->anr, 0); - EMNR::xemnr (rxa->emnr, 0); + rxa->anf->execute(0); + rxa->anr->ANR::execute(0); + rxa->emnr->execute(0); BANDPASS::xbandpass (rxa->bp1, 0); WCPAGC::xwcpagc (rxa->agc); - ANF::xanf (rxa->anf, 1); - ANR::xanr (rxa->anr, 1); - EMNR::xemnr (rxa->emnr, 1); + rxa->anf->execute(1); + rxa->anr->execute(1); + rxa->emnr->execute(1); BANDPASS::xbandpass (rxa->bp1, 1); rxa->agcmeter->execute(); SIPHON::xsiphon (rxa->sip1, 0); @@ -764,9 +764,9 @@ void RXA::setDSPSamplerate (RXA *rxa, int dsp_rate) rxa->fmsq->setSamplerate(rxa->dsp_rate); // rxa->snba->setSamplerate(rxa->dsp_rate); SMBA removed rxa->eqp->setSamplerate(rxa->dsp_rate); - ANF::setSamplerate_anf (rxa->anf, rxa->dsp_rate); - ANR::setSamplerate_anr (rxa->anr, rxa->dsp_rate); - EMNR::setSamplerate_emnr (rxa->emnr, rxa->dsp_rate); + rxa->anf->setSamplerate(rxa->dsp_rate); + rxa->anr->setSamplerate(rxa->dsp_rate); + rxa->emnr->setSamplerate(rxa->dsp_rate); BANDPASS::setSamplerate_bandpass (rxa->bp1, rxa->dsp_rate); WCPAGC::setSamplerate_wcpagc (rxa->agc, rxa->dsp_rate); rxa->agcmeter->setSamplerate(rxa->dsp_rate); @@ -837,12 +837,12 @@ void RXA::setDSPBuffsize (RXA *rxa, int dsp_size) rxa->snba->setSize(rxa->dsp_size); rxa->eqp->setBuffers(rxa->midbuff, rxa->midbuff); rxa->eqp->setSize(rxa->dsp_size); - ANF::setBuffers_anf (rxa->anf, rxa->midbuff, rxa->midbuff); - ANF::setSize_anf (rxa->anf, rxa->dsp_size); - ANR::setBuffers_anr (rxa->anr, rxa->midbuff, rxa->midbuff); - ANR::setSize_anr (rxa->anr, rxa->dsp_size); - EMNR::setBuffers_emnr (rxa->emnr, rxa->midbuff, rxa->midbuff); - EMNR::setSize_emnr (rxa->emnr, rxa->dsp_size); + rxa->anf->setBuffers(rxa->midbuff, rxa->midbuff); + rxa->anf->setSize(rxa->dsp_size); + rxa->anr->setBuffers(rxa->midbuff, rxa->midbuff); + rxa->anr->setSize(rxa->dsp_size); + rxa->emnr->setBuffers(rxa->midbuff, rxa->midbuff); + rxa->emnr->setSize(rxa->dsp_size); BANDPASS::setBuffers_bandpass (rxa->bp1, rxa->midbuff, rxa->midbuff); BANDPASS::setSize_bandpass (rxa->bp1, rxa->dsp_size); WCPAGC::setBuffers_wcpagc (rxa->agc, rxa->midbuff, rxa->midbuff); @@ -1280,10 +1280,17 @@ void RXA::SetANFRun (RXA& rxa, int run) ); a->run = run; RXA::bp1Set (rxa); - ANF::flush_anf (a); + a->flush(); } } +void RXA::SetANFPosition (RXA& rxa, int position) +{ + rxa.anf->position = position; + rxa.bp1->position = position; + rxa.anf->flush(); +} + void RXA::SetANRRun (RXA& rxa, int run) { ANR *a = rxa.anr; @@ -1300,10 +1307,17 @@ void RXA::SetANRRun (RXA& rxa, int run) ); a->run = run; RXA::bp1Set (rxa); - ANR::flush_anr (a); + a->flush(); } } +void RXA::SetANRPosition (RXA& rxa, int position) +{ + rxa.anr->position = position; + rxa.bp1->position = position; + rxa.anr->flush(); +} + void RXA::SetEMNRRun (RXA& rxa, int run) { EMNR *a = rxa.emnr; @@ -1323,6 +1337,12 @@ void RXA::SetEMNRRun (RXA& rxa, int run) } } +void RXA::SetEMNRPosition (RXA& rxa, int position) +{ + rxa.emnr->position = position; + rxa.bp1->position = position; +} + /******************************************************************************************************** * * * Collectives * diff --git a/wdsp/RXA.hpp b/wdsp/RXA.hpp index 26eb6167e..ac8e1b861 100644 --- a/wdsp/RXA.hpp +++ b/wdsp/RXA.hpp @@ -173,11 +173,13 @@ public: static void SetSNBARun (RXA& rxa, int run); // ANF static void SetANFRun (RXA& rxa, int run); + static void SetANFPosition (RXA& rxa, int position); // ANR static void SetANRRun (RXA& rxa, int run); + static void SetANRPosition (RXA& rxa, int position); // EMNR static void SetEMNRRun (RXA& rxa, int run); - + static void SetEMNRPosition (RXA& rxa, int position); // Collectives static void SetPassband (RXA& rxa, float f_low, float f_high); static void SetNC (RXA& rxa, int nc); diff --git a/wdsp/amd.hpp b/wdsp/amd.hpp index 6b955587c..3fb7a91a2 100644 --- a/wdsp/amd.hpp +++ b/wdsp/amd.hpp @@ -100,6 +100,7 @@ public: double tauI ); AMD(const AMD&) = delete; + AMD& operator=(const AMD& other) = delete; ~AMD() = default; void init(); diff --git a/wdsp/amsq.hpp b/wdsp/amsq.hpp index 5e48303b9..02819475d 100644 --- a/wdsp/amsq.hpp +++ b/wdsp/amsq.hpp @@ -81,6 +81,7 @@ public: double _muted_gain ); AMSQ(const AMSQ&) = delete; + AMSQ& operator=(const AMSQ& other) = delete; ~AMSQ() = default; void flush(); diff --git a/wdsp/anb.hpp b/wdsp/anb.hpp index 2a349e19f..e05998289 100644 --- a/wdsp/anb.hpp +++ b/wdsp/anb.hpp @@ -80,6 +80,7 @@ public: double threshold ); ANB(const ANB&) = delete; + ANB& operator=(const ANB& other) = delete; ~ANB() = default; void flush(); diff --git a/wdsp/anf.cpp b/wdsp/anf.cpp index f4560f51f..81edd9578 100644 --- a/wdsp/anf.cpp +++ b/wdsp/anf.cpp @@ -36,144 +36,136 @@ warren@wpratt.com namespace WDSP { -ANF* ANF::create_anf( - int run, - int position, - int buff_size, - float *in_buff, - float *out_buff, - int dline_size, - int n_taps, - int delay, - double two_mu, - double gamma, - double lidx, - double lidx_min, - double lidx_max, - double ngamma, - double den_mult, - double lincr, - double ldecr +ANF::ANF( + int _run, + int _position, + int _buff_size, + float *_in_buff, + float *_out_buff, + int _dline_size, + int _n_taps, + int _delay, + double _two_mu, + double _gamma, + double _lidx, + double _lidx_min, + double _lidx_max, + double _ngamma, + double _den_mult, + double _lincr, + double _ldecr ) { - ANF *a = new ANF; - a->run = run; - a->position = position; - a->buff_size = buff_size; - a->in_buff = in_buff; - a->out_buff = out_buff; - a->dline_size = dline_size; - a->mask = dline_size - 1; - a->n_taps = n_taps; - a->delay = delay; - a->two_mu = two_mu; - a->gamma = gamma; - a->in_idx = 0; - a->lidx = lidx; - a->lidx_min = lidx_min; - a->lidx_max = lidx_max; - a->ngamma = ngamma; - a->den_mult = den_mult; - a->lincr = lincr; - a->ldecr = ldecr; + run = _run; + position = _position; + buff_size = _buff_size; + in_buff = _in_buff; + out_buff = _out_buff; + dline_size = _dline_size; + mask = _dline_size - 1; + n_taps = _n_taps; + delay = _delay; + two_mu = _two_mu; + gamma = _gamma; + in_idx = 0; + lidx = _lidx; + lidx_min = _lidx_min; + lidx_max = _lidx_max; + ngamma = _ngamma; + den_mult = _den_mult; + lincr = _lincr; + ldecr = _ldecr; - memset (a->d, 0, sizeof(double) * ANF_DLINE_SIZE); - memset (a->w, 0, sizeof(double) * ANF_DLINE_SIZE); - - return a; + std::fill(d.begin(), d.end(), 0); + std::fill(w.begin(), w.end(), 0); } -void ANF::destroy_anf (ANF *a) -{ - delete a; -} - -void ANF::xanf(ANF *a, int position) +void ANF::execute(int _position) { int i, j, idx; double c0, c1; double y, error, sigma, inv_sigp; double nel, nev; - if (a->run && (a->position == position)) + if (run && (position == _position)) { - for (i = 0; i < a->buff_size; i++) + for (i = 0; i < buff_size; i++) { - a->d[a->in_idx] = a->in_buff[2 * i + 0]; + d[in_idx] = in_buff[2 * i + 0]; y = 0; sigma = 0; - for (j = 0; j < a->n_taps; j++) + for (j = 0; j < n_taps; j++) { - idx = (a->in_idx + j + a->delay) & a->mask; - y += a->w[j] * a->d[idx]; - sigma += a->d[idx] * a->d[idx]; + idx = (in_idx + j + delay) & mask; + y += w[j] * d[idx]; + sigma += d[idx] * d[idx]; } inv_sigp = 1.0 / (sigma + 1e-10); - error = a->d[a->in_idx] - y; + error = d[in_idx] - y; - a->out_buff[2 * i + 0] = error; - a->out_buff[2 * i + 1] = 0.0; + out_buff[2 * i + 0] = error; + out_buff[2 * i + 1] = 0.0; - if ((nel = error * (1.0 - a->two_mu * sigma * inv_sigp)) < 0.0) + if ((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0) nel = -nel; - if ((nev = a->d[a->in_idx] - (1.0 - a->two_mu * a->ngamma) * y - a->two_mu * error * sigma * inv_sigp) < 0.0) + if ((nev = d[in_idx] - (1.0 - two_mu * ngamma) * y - two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev; if (nev < nel) { - if ((a->lidx += a->lincr) > a->lidx_max) a->lidx = a->lidx_max; + if ((lidx += lincr) > lidx_max) lidx = lidx_max; } else { - if ((a->lidx -= a->ldecr) < a->lidx_min) a->lidx = a->lidx_min; + if ((lidx -= ldecr) < lidx_min) lidx = lidx_min; } - a->ngamma = a->gamma * (a->lidx * a->lidx) * (a->lidx * a->lidx) * a->den_mult; + ngamma = gamma * (lidx * lidx) * (lidx * lidx) * den_mult; - c0 = 1.0 - a->two_mu * a->ngamma; - c1 = a->two_mu * error * inv_sigp; + c0 = 1.0 - two_mu * ngamma; + c1 = two_mu * error * inv_sigp; - for (j = 0; j < a->n_taps; j++) + for (j = 0; j < n_taps; j++) { - idx = (a->in_idx + j + a->delay) & a->mask; - a->w[j] = c0 * a->w[j] + c1 * a->d[idx]; + idx = (in_idx + j + delay) & mask; + w[j] = c0 * w[j] + c1 * d[idx]; } - a->in_idx = (a->in_idx + a->mask) & a->mask; + in_idx = (in_idx + mask) & mask; } } - else if (a->in_buff != a->out_buff) + else if (in_buff != out_buff) { - std::copy(a->in_buff, a->in_buff + a->buff_size * 2, a->out_buff); + std::copy(in_buff, in_buff + buff_size * 2, out_buff); } } -void ANF::flush_anf (ANF *a) +void ANF::flush() { - memset (a->d, 0, sizeof(double) * ANF_DLINE_SIZE); - memset (a->w, 0, sizeof(double) * ANF_DLINE_SIZE); - a->in_idx = 0; + std::fill(d.begin(), d.end(), 0); + std::fill(w.begin(), w.end(), 0); + in_idx = 0; } -void ANF::setBuffers_anf (ANF *a, float* in, float* out) +void ANF::setBuffers(float* _in, float* _out) { - a->in_buff = in; - a->out_buff = out; + in_buff = _in; + out_buff = _out; } -void ANF::setSamplerate_anf (ANF *a, int) +void ANF::setSamplerate(int) { - flush_anf (a); + flush(); } -void ANF::setSize_anf (ANF *a, int size) +void ANF::setSize(int _size) { - a->buff_size = size; - flush_anf (a); + buff_size = _size; + flush(); } /******************************************************************************************************** @@ -182,44 +174,37 @@ void ANF::setSize_anf (ANF *a, int size) * * ********************************************************************************************************/ -void ANF::SetANFVals (RXA& rxa, int taps, int delay, double gain, double leakage) +void ANF::setVals(int _taps, int _delay, double _gain, double _leakage) { - rxa.anf->n_taps = taps; - rxa.anf->delay = delay; - rxa.anf->two_mu = gain; //try two_mu = 1e-4 - rxa.anf->gamma = leakage; //try gamma = 0.10 - flush_anf (rxa.anf); + n_taps = _taps; + delay = _delay; + two_mu = _gain; //try two_mu = 1e-4 + gamma = _leakage; //try gamma = 0.10 + flush(); } -void ANF::SetANFTaps (RXA& rxa, int taps) +void ANF::setTaps(int _taps) { - rxa.anf->n_taps = taps; - flush_anf (rxa.anf); + n_taps = _taps; + flush(); } -void ANF::SetANFDelay (RXA& rxa, int delay) +void ANF::setDelay(int _delay) { - rxa.anf->delay = delay; - flush_anf (rxa.anf); + delay = _delay; + flush(); } -void ANF::SetANFGain (RXA& rxa, double gain) +void ANF::setGain(double _gain) { - rxa.anf->two_mu = gain; - flush_anf (rxa.anf); + two_mu = _gain; + flush(); } -void ANF::SetANFLeakage (RXA& rxa, double leakage) +void ANF::setLeakage(double _leakage) { - rxa.anf->gamma = leakage; - flush_anf (rxa.anf); -} - -void ANF::SetANFPosition (RXA& rxa, int position) -{ - rxa.anf->position = position; - rxa.bp1->position = position; - flush_anf (rxa.anf); + gamma = _leakage; + flush(); } } // namespace WDSP diff --git a/wdsp/anf.hpp b/wdsp/anf.hpp index 987495225..73fc1f052 100644 --- a/wdsp/anf.hpp +++ b/wdsp/anf.hpp @@ -28,6 +28,8 @@ warren@wpratt.com #ifndef wdsp_anf_h #define wdsp_anf_h +#include + #include "export.h" #define ANF_DLINE_SIZE 2048 @@ -50,8 +52,8 @@ public: int delay; double two_mu; double gamma; - double d [ANF_DLINE_SIZE]; - double w [ANF_DLINE_SIZE]; + std::array d; + std::array w; int in_idx; double lidx; double lidx_min; @@ -61,7 +63,7 @@ public: double lincr; double ldecr; - static ANF* create_anf( + ANF( int run, int position, int buff_size, @@ -80,19 +82,21 @@ public: double lincr, double ldecr ); - static void destroy_anf (ANF *a); - static void flush_anf (ANF *a); - static void xanf (ANF *a, int position); - static void setBuffers_anf (ANF *a, float* in, float* out); - static void setSamplerate_anf (ANF *a, int rate); - static void setSize_anf (ANF *a, int size); - // RXA Properties - static void SetANFVals (RXA& rxa, int taps, int delay, double gain, double leakage); - static void SetANFTaps (RXA& rxa, int taps); - static void SetANFDelay (RXA& rxa, int delay); - static void SetANFGain (RXA& rxa, double gain); - static void SetANFLeakage (RXA& rxa, double leakage); - static void SetANFPosition (RXA& rxa, int position); + ANF(const ANF&) = delete; + ANF& operator=(const ANF& other) = delete; + ~ANF() = default; + + void flush(); + void execute(int position); + void setBuffers(float* in, float* out); + void setSamplerate(int rate); + void setSize(int size); + // Public Properties + void setVals(int taps, int delay, double gain, double leakage); + void setTaps(int taps); + void setDelay(int delay); + void setGain(double gain); + void setLeakage(double leakage); }; } // namespace WDSP diff --git a/wdsp/anr.cpp b/wdsp/anr.cpp index 8fe3d5b5b..de998264a 100644 --- a/wdsp/anr.cpp +++ b/wdsp/anr.cpp @@ -36,190 +36,174 @@ warren@wpratt.com namespace WDSP { -ANR* ANR::create_anr ( - int run, - int position, - int buff_size, - float *in_buff, - float *out_buff, - int dline_size, - int n_taps, - int delay, - double two_mu, - double gamma, - double lidx, - double lidx_min, - double lidx_max, - double ngamma, - double den_mult, - double lincr, - double ldecr -) +ANR::ANR( + int _run, + int _position, + int _buff_size, + float *_in_buff, + float *_out_buff, + int _dline_size, + int _n_taps, + int _delay, + double _two_mu, + double _gamma, + double _lidx, + double _lidx_min, + double _lidx_max, + double _ngamma, + double _den_mult, + double _lincr, + double _ldecr +) : + run(_run), + position(_position), + buff_size(_buff_size), + in_buff(_in_buff), + out_buff(_out_buff), + dline_size(_dline_size), + mask(_dline_size - 1), + n_taps(_n_taps), + delay(_delay), + two_mu(_two_mu), + gamma(_gamma), + in_idx(0), + lidx(_lidx), + lidx_min(_lidx_min), + lidx_max(_lidx_max), + ngamma(_ngamma), + den_mult(_den_mult), + lincr(_lincr), + ldecr(_ldecr) { - ANR *a = new ANR; - a->run = run; - a->position = position; - a->buff_size = buff_size; - a->in_buff = in_buff; - a->out_buff = out_buff; - a->dline_size = dline_size; - a->mask = dline_size - 1; - a->n_taps = n_taps; - a->delay = delay; - a->two_mu = two_mu; - a->gamma = gamma; - a->in_idx = 0; - a->lidx = lidx; - a->lidx_min = lidx_min; - a->lidx_max = lidx_max; - a->ngamma = ngamma; - a->den_mult = den_mult; - a->lincr = lincr; - a->ldecr = ldecr; - - memset (a->d, 0, sizeof(double) * ANR_DLINE_SIZE); - memset (a->w, 0, sizeof(double) * ANR_DLINE_SIZE); - - return a; + std::fill(d.begin(), d.end(), 0); + std::fill(w.begin(), w.end(), 0); } -void ANR::destroy_anr (ANR *a) -{ - delete a; -} - -void ANR::xanr (ANR *a, int position) +void ANR::execute(int _position) { int i, j, idx; double c0, c1; double y, error, sigma, inv_sigp; double nel, nev; - if (a->run && (a->position == position)) + if (run && (position == _position)) { - for (i = 0; i < a->buff_size; i++) + for (i = 0; i < buff_size; i++) { - a->d[a->in_idx] = a->in_buff[2 * i + 0]; + d[in_idx] = in_buff[2 * i + 0]; y = 0; sigma = 0; - for (j = 0; j < a->n_taps; j++) + for (j = 0; j < n_taps; j++) { - idx = (a->in_idx + j + a->delay) & a->mask; - y += a->w[j] * a->d[idx]; - sigma += a->d[idx] * a->d[idx]; + idx = (in_idx + j + delay) & mask; + y += w[j] * d[idx]; + sigma += d[idx] * d[idx]; } inv_sigp = 1.0 / (sigma + 1e-10); - error = a->d[a->in_idx] - y; + error = d[in_idx] - y; - a->out_buff[2 * i + 0] = y; - a->out_buff[2 * i + 1] = 0.0; + out_buff[2 * i + 0] = y; + out_buff[2 * i + 1] = 0.0; - if ((nel = error * (1.0 - a->two_mu * sigma * inv_sigp)) < 0.0) + if ((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0) nel = -nel; - if ((nev = a->d[a->in_idx] - (1.0 - a->two_mu * a->ngamma) * y - a->two_mu * error * sigma * inv_sigp) < 0.0) + if ((nev = d[in_idx] - (1.0 - two_mu * ngamma) * y - two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev; if (nev < nel) { - if ((a->lidx += a->lincr) > a->lidx_max) - a->lidx = a->lidx_max; + if ((lidx += lincr) > lidx_max) + lidx = lidx_max; } else { - if ((a->lidx -= a->ldecr) < a->lidx_min) - a->lidx = a->lidx_min; + if ((lidx -= ldecr) < lidx_min) + lidx = lidx_min; } - a->ngamma = a->gamma * (a->lidx * a->lidx) * (a->lidx * a->lidx) * a->den_mult; - c0 = 1.0 - a->two_mu * a->ngamma; - c1 = a->two_mu * error * inv_sigp; + ngamma = gamma * (lidx * lidx) * (lidx * lidx) * den_mult; + c0 = 1.0 - two_mu * ngamma; + c1 = two_mu * error * inv_sigp; - for (j = 0; j < a->n_taps; j++) + for (j = 0; j < n_taps; j++) { - idx = (a->in_idx + j + a->delay) & a->mask; - a->w[j] = c0 * a->w[j] + c1 * a->d[idx]; + idx = (in_idx + j + delay) & mask; + w[j] = c0 * w[j] + c1 * d[idx]; } - a->in_idx = (a->in_idx + a->mask) & a->mask; + in_idx = (in_idx + mask) & mask; } } - else if (a->in_buff != a->out_buff) + else if (in_buff != out_buff) { - std::copy(a->in_buff, a->in_buff + a->buff_size * 2, a->out_buff); + std::copy(in_buff, in_buff + buff_size * 2, out_buff); } } -void ANR::flush_anr (ANR *a) +void ANR::flush() { - memset (a->d, 0, sizeof(double) * ANR_DLINE_SIZE); - memset (a->w, 0, sizeof(double) * ANR_DLINE_SIZE); - a->in_idx = 0; + std::fill(d.begin(), d.end(), 0); + std::fill(w.begin(), w.end(), 0); + in_idx = 0; } -void ANR::setBuffers_anr (ANR *a, float* in, float* out) +void ANR::setBuffers(float* _in, float* _out) { - a->in_buff = in; - a->out_buff = out; + in_buff = _in; + out_buff = _out; } -void ANR::setSamplerate_anr (ANR *a, int) +void ANR::setSamplerate(int) { - flush_anr(a); + flush(); } -void ANR::setSize_anr (ANR *a, int size) +void ANR::setSize(int _size) { - a->buff_size = size; - flush_anr(a); + buff_size = _size; + flush(); } /******************************************************************************************************** * * -* RXA Properties * +* Public Properties * * * ********************************************************************************************************/ -void ANR::SetANRVals (RXA& rxa, int taps, int delay, double gain, double leakage) +void ANR::setVals(int _taps, int _delay, double _gain, double _leakage) { - rxa.anr->n_taps = taps; - rxa.anr->delay = delay; - rxa.anr->two_mu = gain; - rxa.anr->gamma = leakage; - flush_anr (rxa.anr); + n_taps = _taps; + delay = _delay; + two_mu = _gain; + gamma = _leakage; + flush(); } -void ANR::SetANRTaps (RXA& rxa, int taps) +void ANR::setTaps(int _taps) { - rxa.anr->n_taps = taps; - flush_anr (rxa.anr); + n_taps = _taps; + flush(); } -void ANR::SetANRDelay (RXA& rxa, int delay) +void ANR::setDelay(int _delay) { - rxa.anr->delay = delay; - flush_anr (rxa.anr); + delay = _delay; + flush(); } -void ANR::SetANRGain (RXA& rxa, double gain) +void ANR::setGain(double _gain) { - rxa.anr->two_mu = gain; - flush_anr (rxa.anr); + two_mu = _gain; + flush(); } -void ANR::SetANRLeakage (RXA& rxa, double leakage) +void ANR::setLeakage(double _leakage) { - rxa.anr->gamma = leakage; - flush_anr (rxa.anr); -} - -void ANR::SetANRPosition (RXA& rxa, int position) -{ - rxa.anr->position = position; - rxa.bp1->position = position; - flush_anr (rxa.anr); + gamma = _leakage; + flush(); } } // namespace WDSP diff --git a/wdsp/anr.hpp b/wdsp/anr.hpp index c0640d00f..8adeebc16 100644 --- a/wdsp/anr.hpp +++ b/wdsp/anr.hpp @@ -28,6 +28,8 @@ warren@wpratt.com #ifndef wdsp_anr_h #define wdsp_anr_h +#include + #include "export.h" #define ANR_DLINE_SIZE 2048 @@ -50,8 +52,8 @@ public: int delay; double two_mu; double gamma; - double d [ANR_DLINE_SIZE]; - double w [ANR_DLINE_SIZE]; + std::array d; + std::array w; int in_idx; double lidx; @@ -62,7 +64,7 @@ public: double lincr; double ldecr; - static ANR* create_anr ( + ANR( int run, int position, int buff_size, @@ -81,20 +83,21 @@ public: double lincr, double ldecr ); + ANR(const ANR&) = delete; + ANR& operator=(const ANR& other) = delete; + ~ANR() = default; - static void destroy_anr (ANR *a); - static void flush_anr (ANR *a); - static void xanr (ANR *a, int position); - static void setBuffers_anr (ANR *a, float* in, float* out); - static void setSamplerate_anr (ANR *a, int rate); - static void setSize_anr (ANR *a, int size); + void flush(); + void execute(int position); + void setBuffers(float* in, float* out); + void setSamplerate(int rate); + void setSize(int size); // RXA Properties - static void SetANRVals (RXA& rxa, int taps, int delay, double gain, double leakage); - static void SetANRTaps (RXA& rxa, int taps); - static void SetANRDelay (RXA& rxa, int delay); - static void SetANRGain (RXA& rxa, double gain); - static void SetANRLeakage (RXA& rxa, double leakage); - static void SetANRPosition (RXA& rxa, int position); + void setVals(int taps, int delay, double gain, double leakage); + void setTaps(int taps); + void setDelay(int delay); + void setGain(double gain); + void setLeakage(double leakage); }; } // namespace WDSP diff --git a/wdsp/bpsnba.hpp b/wdsp/bpsnba.hpp index a5b320afa..28221eb63 100644 --- a/wdsp/bpsnba.hpp +++ b/wdsp/bpsnba.hpp @@ -80,6 +80,7 @@ public: NOTCHDB* notchdb ); BPSNBA(const BPSNBA&) = delete; + BPSNBA& operator=(BPSNBA& other) = delete; ~BPSNBA(); void flush(); diff --git a/wdsp/emnr.cpp b/wdsp/emnr.cpp index 20091f0be..35909775b 100644 --- a/wdsp/emnr.cpp +++ b/wdsp/emnr.cpp @@ -38,6 +38,490 @@ warren@wpratt.com namespace WDSP { +EMNR::AE::AE( + int _msize, + double* _lambda_y, + double _zetaThresh, + double _psi +) : + msize(_msize), + lambda_y(_lambda_y), + zetaThresh(_zetaThresh), + psi(_psi) +{ + nmask = new double[msize]; +} + +EMNR::AE::~AE() +{ + delete[] nmask; +} + +EMNR::NPS::NPS( + int _incr, + double _rate, + int _msize, + double* _lambda_y, + double* _lambda_d, + + double _alpha_pow, + double _alpha_Pbar, + double _epsH1 +) : + incr(_incr), + rate(_rate), + msize(_msize), + lambda_y(_lambda_y), + lambda_d(_lambda_d), + alpha_pow(_alpha_pow), + alpha_Pbar(_alpha_Pbar), + epsH1(_epsH1) +{ + epsH1r = epsH1 / (1.0 + epsH1); + sigma2N = new double[msize]; // (float *)malloc0(nps.msize * sizeof(float)); + PH1y = new double[msize]; // (float *)malloc0(nps.msize * sizeof(float)); + Pbar = new double[msize]; // (float *)malloc0(nps.msize * sizeof(float)); + EN2y = new double[msize]; // (float *)malloc0(nps.msize * sizeof(float)); + + for (int i = 0; i < msize; i++) + { + sigma2N[i] = 0.5; + Pbar[i] = 0.5; + } +} + +EMNR::NPS::~NPS() +{ + delete[] sigma2N; + delete[] PH1y; + delete[] Pbar; + delete[] EN2y; +} + +void EMNR::NPS::LambdaDs() +{ + int k; + + for (k = 0; k < msize; k++) + { + PH1y[k] = 1.0 / (1.0 + (1.0 + epsH1) * exp (- epsH1r * lambda_y[k] / sigma2N[k])); + Pbar[k] = alpha_Pbar * Pbar[k] + (1.0 - alpha_Pbar) * PH1y[k]; + + if (Pbar[k] > 0.99) + PH1y[k] = std::min (PH1y[k], 0.99); + + EN2y[k] = (1.0 - PH1y[k]) * lambda_y[k] + PH1y[k] * sigma2N[k]; + sigma2N[k] = alpha_pow * sigma2N[k] + (1.0 - alpha_pow) * EN2y[k]; + } + + std::copy(sigma2N, sigma2N + msize, lambda_d); +} + +const std::array EMNR::NP::DVals = { 1.0, 2.0, 5.0, 8.0, 10.0, 15.0, 20.0, 30.0, 40.0, + 60.0, 80.0, 120.0, 140.0, 160.0, 180.0, 220.0, 260.0, 300.0 }; +const std::array EMNR::NP::MVals = { 0.000, 0.260, 0.480, 0.580, 0.610, 0.668, 0.705, 0.762, 0.800, + 0.841, 0.865, 0.890, 0.900, 0.910, 0.920, 0.930, 0.935, 0.940 }; + +EMNR::NP::NP( + int _incr, + double _rate, + int _msize, + double* _lambda_y, + double* _lambda_d +) +{ + incr = _incr; + rate = _rate; + msize = _msize; + lambda_y = _lambda_y; + lambda_d = _lambda_d; + + { + double tau = -128.0 / 8000.0 / log(0.7); + alphaCsmooth = exp(-incr / rate / tau); + } + + { + double tau = -128.0 / 8000.0 / log(0.96); + alphaMax = exp(-incr / rate / tau); + } + + { + double tau = -128.0 / 8000.0 / log(0.7); + alphaCmin = exp(-incr / rate / tau); + } + + { + double tau = -128.0 / 8000.0 / log(0.3); + alphaMin_max_value = exp(-incr / rate / tau); + } + + snrq = -incr / (0.064 * rate); + + double tau4 = -128.0 / 8000.0 / log(0.8); + betamax = exp(-incr / rate / tau4); + + invQeqMax = 0.5; + av = 2.12; + Dtime = 8.0 * 12.0 * 128.0 / 8000.0; + U = 8; + V = (int)(0.5 + (Dtime * rate / (U * incr))); + + if (V < 4) + V = 4; + + if ((U = (int)(0.5 + (Dtime * rate / (V * incr)))) < 1) + U = 1; + + D = U * V; + interpM(&MofD, D, 18, DVals, MVals); + interpM(&MofV, V, 18, DVals, MVals); + + invQbar_points[0] = 0.03; + invQbar_points[1] = 0.05; + invQbar_points[2] = 0.06; + invQbar_points[3] = std::numeric_limits::max();; + + { + double db; + db = 10.0 * log10(8.0) / (12.0 * 128 / 8000); + nsmax[0] = pow(10.0, db / 10.0 * V * incr / rate); + db = 10.0 * log10(4.0) / (12.0 * 128 / 8000); + nsmax[1] = pow(10.0, db / 10.0 * V * incr / rate); + db = 10.0 * log10(2.0) / (12.0 * 128 / 8000); + nsmax[2] = pow(10.0, db / 10.0 * V * incr / rate); + db = 10.0 * log10(1.2) / (12.0 * 128 / 8000); + nsmax[3] = pow(10.0, db / 10.0 * V * incr / rate); + } + + p = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + alphaOptHat = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + alphaHat = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + sigma2N = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + pbar = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + p2bar = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + Qeq = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + bmin = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + bmin_sub = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + k_mod = new int[msize]; // (int *)malloc0(np.msize * sizeof(int)); + actmin = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + actmin_sub = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + lmin_flag = new int[msize]; // (int *)malloc0(np.msize * sizeof(int)); + pmin_u = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + actminbuff = new double*[U]; // (float**)malloc0(np.U * sizeof(float*)); + + for (int i = 0; i < U; i++) { + actminbuff[i] = new double[msize]; // (float *)malloc0(np.msize * sizeof(float)); + } + + { + int k, ku; + alphaC = 1.0; + subwc = V; + amb_idx = 0; + + for (k = 0; k < msize; k++) { + lambda_y[k] = 0.5; + } + + std::copy(lambda_y, lambda_y + msize, p); + std::copy(lambda_y, lambda_y + msize, sigma2N); + std::copy(lambda_y, lambda_y + msize, pbar); + std::copy(lambda_y, lambda_y + msize, pmin_u); + + for (k = 0; k < msize; k++) + { + p2bar[k] = lambda_y[k] * lambda_y[k]; + actmin[k] = std::numeric_limits::max(); + actmin_sub[k] = std::numeric_limits::max();; + + for (ku = 0; ku < U; ku++) { + actminbuff[ku][k] = std::numeric_limits::max();; + } + } + + std::fill(lmin_flag, lmin_flag + msize, 0); + } +} + +EMNR::NP::~NP() +{ + for (int i = 0; i < U; i++) + delete[] (actminbuff[i]); + + delete[] (actminbuff); + delete[] (pmin_u); + delete[] (lmin_flag); + delete[] (actmin_sub); + delete[] (actmin); + delete[] (k_mod); + delete[] (bmin_sub); + delete[] (bmin); + delete[] (Qeq); + delete[] (p2bar); + delete[] (pbar); + delete[] (sigma2N); + delete[] (alphaHat); + delete[] (alphaOptHat); + delete[] (p); +} + +void EMNR::NP::interpM ( + double* res, + double x, + int nvals, + const std::array& xvals, + const std::array& yvals +) +{ + if (x <= xvals[0]) + { + *res = yvals[0]; + } + else if (x >= xvals[nvals - 1]) + { + *res = yvals[nvals - 1]; + } + else + { + int idx = 1; + double xllow, xlhigh, frac; + + while ((x >= xvals[idx]) && (idx < nvals - 1)) + idx++; + + xllow = log10 (xvals[idx - 1]); + xlhigh = log10(xvals[idx]); + frac = (log10 (x) - xllow) / (xlhigh - xllow); + *res = yvals[idx - 1] + frac * (yvals[idx] - yvals[idx - 1]); + } +} + +void EMNR::NP::LambdaD() +{ + int k; + double f0, f1, f2, f3; + double sum_prev_p; + double sum_lambda_y; + double alphaCtilda; + double sum_prev_sigma2N; + double alphaMin, SNR; + double beta, varHat, invQeq; + double invQbar; + double bc; + double QeqTilda, QeqTildaSub; + double noise_slope_max; + + sum_prev_p = 0.0; + sum_lambda_y = 0.0; + sum_prev_sigma2N = 0.0; + + for (k = 0; k < msize; k++) + { + sum_prev_p += p[k]; + sum_lambda_y += lambda_y[k]; + sum_prev_sigma2N += sigma2N[k]; + } + + for (k = 0; k < msize; k++) + { + f0 = p[k] / sigma2N[k] - 1.0; + alphaOptHat[k] = 1.0 / (1.0 + f0 * f0); + } + + SNR = sum_prev_p / sum_prev_sigma2N; + alphaMin = std::min (alphaMin_max_value, pow (SNR, snrq)); + + for (k = 0; k < msize; k++) + if (alphaOptHat[k] < alphaMin) alphaOptHat[k] = alphaMin; + + f1 = sum_prev_p / sum_lambda_y - 1.0; + alphaCtilda = 1.0 / (1.0 + f1 * f1); + alphaC = alphaCsmooth * alphaC + (1.0 - alphaCsmooth) * std::max (alphaCtilda, alphaCmin); + f2 = alphaMax * alphaC; + + for (k = 0; k < msize; k++) + alphaHat[k] = f2 * alphaOptHat[k]; + + for (k = 0; k < msize; k++) + p[k] = alphaHat[k] * p[k] + (1.0 - alphaHat[k]) * lambda_y[k]; + + invQbar = 0.0; + + for (k = 0; k < msize; k++) + { + beta = std::min (betamax, alphaHat[k] * alphaHat[k]); + pbar[k] = beta * pbar[k] + (1.0 - beta) * p[k]; + p2bar[k] = beta * p2bar[k] + (1.0 - beta) * p[k] * p[k]; + varHat = p2bar[k] - pbar[k] * pbar[k]; + invQeq = varHat / (2.0 * sigma2N[k] * sigma2N[k]); + if (invQeq > invQeqMax) + invQeq = invQeqMax; + Qeq[k] = 1.0 / invQeq; + invQbar += invQeq; + } + invQbar /= (double) msize; + bc = 1.0 + av * sqrt (invQbar); + + for (k = 0; k < msize; k++) + { + QeqTilda = (Qeq[k] - 2.0 * MofD) / (1.0 - MofD); + QeqTildaSub = (Qeq[k] - 2.0 * MofV) / (1.0 - MofV); + bmin[k] = 1.0 + 2.0 * (D - 1.0) / QeqTilda; + bmin_sub[k] = 1.0 + 2.0 * (V - 1.0) / QeqTildaSub; + } + + std::fill(k_mod, k_mod + msize, 0); + + for (k = 0; k < msize; k++) + { + f3 = p[k] * bmin[k] * bc; + + if (f3 < actmin[k]) + { + actmin[k] = f3; + actmin_sub[k] = p[k] * bmin_sub[k] * bc; + k_mod[k] = 1; + } + } + + if (subwc == V) + { + if (invQbar < invQbar_points[0]) + noise_slope_max = nsmax[0]; + else if (invQbar < invQbar_points[1]) + noise_slope_max = nsmax[1]; + else if (invQbar < invQbar_points[2]) + noise_slope_max = nsmax[2]; + else + noise_slope_max = nsmax[3]; + + for (k = 0; k < msize; k++) + { + int ku; + double min; + + if (k_mod[k]) + lmin_flag[k] = 0; + + actminbuff[amb_idx][k] = actmin[k]; + min = std::numeric_limits::max();; + + for (ku = 0; ku < U; ku++) + { + if (actminbuff[ku][k] < min) + min = actminbuff[ku][k]; + } + + pmin_u[k] = min; + + if ((lmin_flag[k] == 1) + && (actmin_sub[k] < noise_slope_max * pmin_u[k]) + && (actmin_sub[k] > pmin_u[k])) + { + pmin_u[k] = actmin_sub[k]; + for (ku = 0; ku < U; ku++) + actminbuff[ku][k] = actmin_sub[k]; + } + + lmin_flag[k] = 0; + actmin[k] = std::numeric_limits::max();; + actmin_sub[k] = std::numeric_limits::max();; + } + + if (++amb_idx == U) + amb_idx = 0; + + subwc = 1; + } + else + { + if (subwc > 1) + { + for (k = 0; k < msize; k++) + { + if (k_mod[k]) + { + lmin_flag[k] = 1; + sigma2N[k] = std::min (actmin_sub[k], pmin_u[k]); + pmin_u[k] = sigma2N[k]; + } + } + } + + ++subwc; + } + + std::copy(sigma2N, sigma2N + msize, lambda_d); +} + +EMNR::G::G( + int _incr, + double _rate, + int _msize, + double* _mask, + float* _y +) +{ + incr = _incr; + rate = _rate; + msize = _msize; + mask = _mask; + y = _y; + + lambda_y = new double[msize]; // (float *)malloc0(msize * sizeof(float)); + lambda_d = new double[msize]; // (float *)malloc0(msize * sizeof(float)); + prev_gamma = new double[msize]; // (float *)malloc0(msize * sizeof(float)); + prev_mask = new double[msize]; // (float *)malloc0(msize * sizeof(float)); + + gf1p5 = sqrt(PI) / 2.0; + + { + double tau = -128.0 / 8000.0 / log(0.98); + alpha = exp(-incr / rate / tau); + } + + eps_floor = std::numeric_limits::min(); + gamma_max = 1000.0; + q = 0.2; + + std::fill(prev_mask, prev_mask + msize, 1.0); + std::fill(prev_gamma, prev_gamma + msize, 1.0); + + gmax = 10000.0; + // + GG = new double[241 * 241]; // (float *)malloc0(241 * 241 * sizeof(float)); + GGS = new double[241 * 241]; // (float *)malloc0(241 * 241 * sizeof(float)); + + if ((fileb = fopen("calculus", "rb"))) + { + std::size_t lgg = fread(GG, sizeof(float), 241 * 241, fileb); + if (lgg != 241 * 241) { + fprintf(stderr, "GG file has an invalid size\n"); + } + std::size_t lggs =fread(GGS, sizeof(float), 241 * 241, fileb); + if (lggs != 241 * 241) { + fprintf(stderr, "GGS file has an invalid size\n"); + } + fclose(fileb); + } + else + { + std::copy(Calculus::GG, Calculus::GG + (241 * 241), GG); + std::copy(Calculus::GGS, Calculus::GGS + (241 * 241), GGS); + } +} + +EMNR::G::~G() +{ + delete[] (GGS); + delete[] (GG); + delete[] (prev_mask); + delete[] (prev_gamma); + delete[] (lambda_d); + delete[] (lambda_y); +} + /******************************************************************************************************** * * * Special Functions * @@ -192,27 +676,27 @@ double EMNR::e1xb (double x) * * ********************************************************************************************************/ -void EMNR::calc_window (EMNR *a) +void EMNR::calc_window() { int i; double arg, sum, inv_coherent_gain; - switch (a->wintype) + switch (wintype) { case 0: - arg = 2.0 * PI / (double) a->fsize; + arg = 2.0 * PI / (double) fsize; sum = 0.0; - for (i = 0; i < a->fsize; i++) + for (i = 0; i < fsize; i++) { - a->window[i] = sqrt (0.54 - 0.46 * cos((float)i * arg)); - sum += a->window[i]; + window[i] = sqrt (0.54 - 0.46 * cos((float)i * arg)); + sum += window[i]; } - inv_coherent_gain = (double) a->fsize / sum; + inv_coherent_gain = (double) fsize / sum; - for (i = 0; i < a->fsize; i++) - a->window[i] *= inv_coherent_gain; + for (i = 0; i < fsize; i++) + window[i] *= inv_coherent_gain; break; } @@ -243,549 +727,185 @@ void EMNR::interpM (double* res, double x, int nvals, double* xvals, double* yva } } -void EMNR::calc_emnr(EMNR *a) +void EMNR::calc() { - double Dvals[18] = { 1.0, 2.0, 5.0, 8.0, 10.0, 15.0, 20.0, 30.0, 40.0, - 60.0, 80.0, 120.0, 140.0, 160.0, 180.0, 220.0, 260.0, 300.0 }; - double Mvals[18] = { 0.000, 0.260, 0.480, 0.580, 0.610, 0.668, 0.705, 0.762, 0.800, - 0.841, 0.865, 0.890, 0.900, 0.910, 0.920, 0.930, 0.935, 0.940 }; // float Hvals[18] = { 0.000, 0.150, 0.480, 0.780, 0.980, 1.550, 2.000, 2.300, 2.520, // 3.100, 3.380, 4.150, 4.350, 4.250, 3.900, 4.100, 4.700, 5.000 }; - a->incr = a->fsize / a->ovrlp; - a->gain = a->ogain / a->fsize / (float)a->ovrlp; + incr = fsize / ovrlp; + gain = ogain / fsize / (float)ovrlp; - if (a->fsize > a->bsize) - a->iasize = a->fsize; + if (fsize > bsize) + iasize = fsize; else - a->iasize = a->bsize + a->fsize - a->incr; + iasize = bsize + fsize - incr; - a->iainidx = 0; - a->iaoutidx = 0; + iainidx = 0; + iaoutidx = 0; - if (a->fsize > a->bsize) + if (fsize > bsize) { - if (a->bsize > a->incr) - a->oasize = a->bsize; + if (bsize > incr) + oasize = bsize; else - a->oasize = a->incr; + oasize = incr; - a->oainidx = (a->fsize - a->bsize - a->incr) % a->oasize; + oainidx = (fsize - bsize - incr) % oasize; } else { - a->oasize = a->bsize; - a->oainidx = a->fsize - a->incr; + oasize = bsize; + oainidx = fsize - incr; } - a->init_oainidx = a->oainidx; - a->oaoutidx = 0; - a->msize = a->fsize / 2 + 1; - a->window = new float[a->fsize]; // (float *)malloc0(a->fsize * sizeof(float)); - a->inaccum = new float[a->iasize]; // (float *)malloc0(a->iasize * sizeof(float)); - a->forfftin = new float[a->fsize]; // (float *)malloc0(a->fsize * sizeof(float)); - a->forfftout = new float[a->msize * 2]; // (float *)malloc0(a->msize * sizeof(complex)); - a->mask = new double[a->msize]; // (float *)malloc0(a->msize * sizeof(float)); - std::fill(a->mask, a->mask + a->msize, 1.0); - a->revfftin = new float[a->msize * 2]; // (float *)malloc0(a->msize * sizeof(complex)); - a->revfftout = new float[a->fsize]; // (float *)malloc0(a->fsize * sizeof(float)); - a->save = new float*[a->ovrlp]; // (float **)malloc0(a->ovrlp * sizeof(float *)); + init_oainidx = oainidx; + oaoutidx = 0; + msize = fsize / 2 + 1; + window.resize(fsize); // (float *)malloc0(fsize * sizeof(float)); + inaccum.resize(iasize); // (float *)malloc0(iasize * sizeof(float)); + forfftin.resize(fsize); // (float *)malloc0(fsize * sizeof(float)); + forfftout.resize(msize * 2); // (float *)malloc0(msize * sizeof(complex)); + mask.resize(msize); // (float *)malloc0(msize * sizeof(float)); + std::fill(mask.begin(), mask.end(), 1.0); + revfftin.resize(msize * 2); // (float *)malloc0(msize * sizeof(complex)); + revfftout.resize(fsize); // (float *)malloc0(fsize * sizeof(float)); + save.resize(ovrlp); // (float **)malloc0(ovrlp * sizeof(float *)); - for (int i = 0; i < a->ovrlp; i++) - a->save[i] = new float[a->fsize]; // (float *)malloc0(a->fsize * sizeof(float)); + for (int i = 0; i < ovrlp; i++) + save[i].resize(fsize); // (float *)malloc0(fsize * sizeof(float)); - a->outaccum = new float[a->oasize]; // (float *)malloc0(a->oasize * sizeof(float)); - a->nsamps = 0; - a->saveidx = 0; - a->Rfor = fftwf_plan_dft_r2c_1d( - a->fsize, - a->forfftin, - (fftwf_complex *)a->forfftout, + outaccum.resize(oasize); // (float *)malloc0(oasize * sizeof(float)); + nsamps = 0; + saveidx = 0; + Rfor = fftwf_plan_dft_r2c_1d( + fsize, + forfftin.data(), + (fftwf_complex *)forfftout.data(), FFTW_ESTIMATE ); - a->Rrev = fftwf_plan_dft_c2r_1d( - a->fsize, - (fftwf_complex *)a->revfftin, - a->revfftout, + Rrev = fftwf_plan_dft_c2r_1d( + fsize, + (fftwf_complex *)revfftin.data(), + revfftout.data(), FFTW_ESTIMATE ); - calc_window(a); + calc_window(); - a->g.msize = a->msize; - a->g.mask = a->mask; - a->g.y = a->forfftout; - a->g.lambda_y = new double[a->msize]; // (float *)malloc0(a->msize * sizeof(float)); - a->g.lambda_d = new double[a->msize]; // (float *)malloc0(a->msize * sizeof(float)); - a->g.prev_gamma = new double[a->msize]; // (float *)malloc0(a->msize * sizeof(float)); - a->g.prev_mask = new double[a->msize]; // (float *)malloc0(a->msize * sizeof(float)); + // G - a->g.gf1p5 = sqrt(PI) / 2.0; - { - double tau = -128.0 / 8000.0 / log(0.98); - a->g.alpha = exp(-a->incr / a->rate / tau); - } - a->g.eps_floor = std::numeric_limits::min(); - a->g.gamma_max = 1000.0; - a->g.q = 0.2; + g = new G( + incr, + rate, + msize, + mask.data(), + forfftout.data() + ); - std::fill(a->g.prev_mask, a->g.prev_mask + a->msize, 1.0); - std::fill(a->g.prev_gamma, a->g.prev_gamma + a->msize, 1.0); + // NP - a->g.gmax = 10000.0; - // - a->g.GG = new double[241 * 241]; // (float *)malloc0(241 * 241 * sizeof(float)); - a->g.GGS = new double[241 * 241]; // (float *)malloc0(241 * 241 * sizeof(float)); + np = new NP( + incr, + rate, + msize, + g->lambda_y, + g->lambda_d + ); - if ((a->g.fileb = fopen("calculus", "rb"))) - { - std::size_t lgg = fread(a->g.GG, sizeof(float), 241 * 241, a->g.fileb); - if (lgg != 241 * 241) { - fprintf(stderr, "GG file has an invalid size\n"); - } - std::size_t lggs =fread(a->g.GGS, sizeof(float), 241 * 241, a->g.fileb); - if (lggs != 241 * 241) { - fprintf(stderr, "GGS file has an invalid size\n"); - } - fclose(a->g.fileb); - } - else - { - std::copy(Calculus::GG, Calculus::GG + (241 * 241), a->g.GG); - std::copy(Calculus::GGS, Calculus::GGS + (241 * 241), a->g.GGS); - } - // + // NPS - a->np.incr = a->incr; - a->np.rate = a->rate; - a->np.msize = a->msize; - a->np.lambda_y = a->g.lambda_y; - a->np.lambda_d = a->g.lambda_d; + double tauNPS0 = -128.0 / 8000.0 / log(0.8); + double alpha_pow = exp(-incr / rate / tauNPS0); - { - double tau = -128.0 / 8000.0 / log(0.7); - a->np.alphaCsmooth = exp(-a->np.incr / a->np.rate / tau); - } - { - double tau = -128.0 / 8000.0 / log(0.96); - a->np.alphaMax = exp(-a->np.incr / a->np.rate / tau); - } - { - double tau = -128.0 / 8000.0 / log(0.7); - a->np.alphaCmin = exp(-a->np.incr / a->np.rate / tau); - } - { - double tau = -128.0 / 8000.0 / log(0.3); - a->np.alphaMin_max_value = exp(-a->np.incr / a->np.rate / tau); - } - a->np.snrq = -a->np.incr / (0.064 * a->np.rate); - { - double tau = -128.0 / 8000.0 / log(0.8); - a->np.betamax = exp(-a->np.incr / a->np.rate / tau); - } - a->np.invQeqMax = 0.5; - a->np.av = 2.12; - a->np.Dtime = 8.0 * 12.0 * 128.0 / 8000.0; - a->np.U = 8; - a->np.V = (int)(0.5 + (a->np.Dtime * a->np.rate / (a->np.U * a->np.incr))); + double tauNPS1 = -128.0 / 8000.0 / log(0.9); + double alpha_Pbar = exp(-incr / rate / tauNPS1); - if (a->np.V < 4) - a->np.V = 4; + nps = new NPS( + incr, + rate, + msize, + g->lambda_y, + g->lambda_d, + alpha_pow, + alpha_Pbar, + pow(10.0, 15.0 / 10.0) // epsH1 + ); - if ((a->np.U = (int)(0.5 + (a->np.Dtime * a->np.rate / (a->np.V * a->np.incr)))) < 1) - a->np.U = 1; + // AE - a->np.D = a->np.U * a->np.V; - interpM(&a->np.MofD, a->np.D, 18, Dvals, Mvals); - interpM(&a->np.MofV, a->np.V, 18, Dvals, Mvals); - a->np.invQbar_points[0] = 0.03; - a->np.invQbar_points[1] = 0.05; - a->np.invQbar_points[2] = 0.06; - a->np.invQbar_points[3] = std::numeric_limits::max();; - { - double db; - db = 10.0 * log10(8.0) / (12.0 * 128 / 8000); - a->np.nsmax[0] = pow(10.0, db / 10.0 * a->np.V * a->np.incr / a->np.rate); - db = 10.0 * log10(4.0) / (12.0 * 128 / 8000); - a->np.nsmax[1] = pow(10.0, db / 10.0 * a->np.V * a->np.incr / a->np.rate); - db = 10.0 * log10(2.0) / (12.0 * 128 / 8000); - a->np.nsmax[2] = pow(10.0, db / 10.0 * a->np.V * a->np.incr / a->np.rate); - db = 10.0 * log10(1.2) / (12.0 * 128 / 8000); - a->np.nsmax[3] = pow(10.0, db / 10.0 * a->np.V * a->np.incr / a->np.rate); - } - - a->np.p = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.alphaOptHat = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.alphaHat = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.sigma2N = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.pbar = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.p2bar = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.Qeq = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.bmin = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.bmin_sub = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.k_mod = new int[a->np.msize]; // (int *)malloc0(a->np.msize * sizeof(int)); - a->np.actmin = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.actmin_sub = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.lmin_flag = new int[a->np.msize]; // (int *)malloc0(a->np.msize * sizeof(int)); - a->np.pmin_u = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - a->np.actminbuff = new double*[a->np.U]; // (float**)malloc0(a->np.U * sizeof(float*)); - - for (int i = 0; i < a->np.U; i++) - a->np.actminbuff[i] = new double[a->np.msize]; // (float *)malloc0(a->np.msize * sizeof(float)); - - { - int k, ku; - a->np.alphaC = 1.0; - a->np.subwc = a->np.V; - a->np.amb_idx = 0; - - for (k = 0; k < a->np.msize; k++) - a->np.lambda_y[k] = 0.5; - - std::copy(a->np.lambda_y, a->np.lambda_y + a->np.msize, a->np.p); - std::copy(a->np.lambda_y, a->np.lambda_y + a->np.msize, a->np.sigma2N); - std::copy(a->np.lambda_y, a->np.lambda_y + a->np.msize, a->np.pbar); - std::copy(a->np.lambda_y, a->np.lambda_y + a->np.msize, a->np.pmin_u); - - for (k = 0; k < a->np.msize; k++) - { - a->np.p2bar[k] = a->np.lambda_y[k] * a->np.lambda_y[k]; - a->np.actmin[k] = std::numeric_limits::max(); - a->np.actmin_sub[k] = std::numeric_limits::max();; - - for (ku = 0; ku < a->np.U; ku++) - a->np.actminbuff[ku][k] = std::numeric_limits::max();; - } - - std::fill(a->np.lmin_flag, a->np.lmin_flag + a->np.msize, 0); - } - - a->nps.incr = a->incr; - a->nps.rate = a->rate; - a->nps.msize = a->msize; - a->nps.lambda_y = a->g.lambda_y; - a->nps.lambda_d = a->g.lambda_d; - - { - double tau = -128.0 / 8000.0 / log(0.8); - a->nps.alpha_pow = exp(-a->nps.incr / a->nps.rate / tau); - } - { - double tau = -128.0 / 8000.0 / log(0.9); - a->nps.alpha_Pbar = exp(-a->nps.incr / a->nps.rate / tau); - } - - a->nps.epsH1 = pow(10.0, 15.0 / 10.0); - a->nps.epsH1r = a->nps.epsH1 / (1.0 + a->nps.epsH1); - - a->nps.sigma2N = new double[a->nps.msize]; // (float *)malloc0(a->nps.msize * sizeof(float)); - a->nps.PH1y = new double[a->nps.msize]; // (float *)malloc0(a->nps.msize * sizeof(float)); - a->nps.Pbar = new double[a->nps.msize]; // (float *)malloc0(a->nps.msize * sizeof(float)); - a->nps.EN2y = new double[a->nps.msize]; // (float *)malloc0(a->nps.msize * sizeof(float)); - - for (int i = 0; i < a->nps.msize; i++) - { - a->nps.sigma2N[i] = 0.5; - a->nps.Pbar[i] = 0.5; - } - - a->ae.msize = a->msize; - a->ae.lambda_y = a->g.lambda_y; - - a->ae.zetaThresh = 0.75; - a->ae.psi = 10.0; - - a->ae.nmask = new double[a->ae.msize]; // (float *)malloc0(a->ae.msize * sizeof(float)); + ae = new AE( + msize, + g->lambda_y, + 0.75, // zetaThresh + 10.0 // psi + ); } -void EMNR::decalc_emnr(EMNR *a) +void EMNR::decalc() { - int i; - delete[] (a->ae.nmask); + delete (ae); + delete (nps); + delete (np); + delete (g); - delete[] (a->nps.EN2y); - delete[] (a->nps.Pbar); - delete[] (a->nps.PH1y); - delete[] (a->nps.sigma2N); - - for (i = 0; i < a->np.U; i++) - delete[] (a->np.actminbuff[i]); - - delete[] (a->np.actminbuff); - delete[] (a->np.pmin_u); - delete[] (a->np.lmin_flag); - delete[] (a->np.actmin_sub); - delete[] (a->np.actmin); - delete[] (a->np.k_mod); - delete[] (a->np.bmin_sub); - delete[] (a->np.bmin); - delete[] (a->np.Qeq); - delete[] (a->np.p2bar); - delete[] (a->np.pbar); - delete[] (a->np.sigma2N); - delete[] (a->np.alphaHat); - delete[] (a->np.alphaOptHat); - delete[] (a->np.p); - - delete[] (a->g.GGS); - delete[] (a->g.GG); - delete[] (a->g.prev_mask); - delete[] (a->g.prev_gamma); - delete[] (a->g.lambda_d); - delete[] (a->g.lambda_y); - - fftwf_destroy_plan(a->Rrev); - fftwf_destroy_plan(a->Rfor); - - delete[] (a->outaccum); - - for (i = 0; i < a->ovrlp; i++) - delete[] (a->save[i]); - - delete[] (a->save); - delete[] (a->revfftout); - delete[] (a->revfftin); - delete[] (a->mask); - delete[] (a->forfftout); - delete[] (a->forfftin); - delete[] (a->inaccum); - delete[] (a->window); + fftwf_destroy_plan(Rrev); + fftwf_destroy_plan(Rfor); } -EMNR* EMNR::create_emnr ( - int run, - int position, - int size, - float* in, - float* out, - int fsize, - int ovrlp, - int rate, - int wintype, - double gain, - int gain_method, - int npe_method, - int ae_run +EMNR::EMNR( + int _run, + int _position, + int _size, + float* _in, + float* _out, + int _fsize, + int _ovrlp, + int _rate, + int _wintype, + double _gain, + int _gain_method, + int _npe_method, + int _ae_run ) { - EMNR *a = new EMNR; - - a->run = run; - a->position = position; - a->bsize = size; - a->in = in; - a->out = out; - a->fsize = fsize; - a->ovrlp = ovrlp; - a->rate = rate; - a->wintype = wintype; - a->ogain = gain; - a->g.gain_method = gain_method; - a->g.npe_method = npe_method; - a->g.ae_run = ae_run; - calc_emnr (a); - return a; + run = _run; + position = _position; + bsize = _size; + in = _in; + out = _out; + fsize = _fsize; + ovrlp = _ovrlp; + rate = _rate; + wintype = _wintype; + ogain = _gain; + calc(); + g->gain_method = _gain_method; + g->npe_method = _npe_method; + g->ae_run = _ae_run; } -void EMNR::flush_emnr (EMNR *a) +void EMNR::flush() { int i; - std::fill(a->inaccum, a->inaccum + a->iasize, 0); + std::fill(inaccum.begin(), inaccum.end(), 0); - for (i = 0; i < a->ovrlp; i++) - std::fill(a->save[i], a->save[i] + a->fsize, 0); + for (i = 0; i < ovrlp; i++) + std::fill(save[i].begin(), save[i].end(), 0); - std::fill(a->outaccum, a->outaccum + a->oasize, 0); - a->nsamps = 0; - a->iainidx = 0; - a->iaoutidx = 0; - a->oainidx = a->init_oainidx; - a->oaoutidx = 0; - a->saveidx = 0; + std::fill(outaccum.begin(), outaccum.end(), 0); + nsamps = 0; + iainidx = 0; + iaoutidx = 0; + oainidx = init_oainidx; + oaoutidx = 0; + saveidx = 0; } -void EMNR::destroy_emnr (EMNR *a) +EMNR::~EMNR() { - decalc_emnr (a); - delete[] (a); + decalc(); } -void EMNR::LambdaD(EMNR *a) -{ - int k; - double f0, f1, f2, f3; - double sum_prev_p; - double sum_lambda_y; - double alphaCtilda; - double sum_prev_sigma2N; - double alphaMin, SNR; - double beta, varHat, invQeq; - double invQbar; - double bc; - double QeqTilda, QeqTildaSub; - double noise_slope_max; - - sum_prev_p = 0.0; - sum_lambda_y = 0.0; - sum_prev_sigma2N = 0.0; - - for (k = 0; k < a->np.msize; k++) - { - sum_prev_p += a->np.p[k]; - sum_lambda_y += a->np.lambda_y[k]; - sum_prev_sigma2N += a->np.sigma2N[k]; - } - - for (k = 0; k < a->np.msize; k++) - { - f0 = a->np.p[k] / a->np.sigma2N[k] - 1.0; - a->np.alphaOptHat[k] = 1.0 / (1.0 + f0 * f0); - } - - SNR = sum_prev_p / sum_prev_sigma2N; - alphaMin = std::min (a->np.alphaMin_max_value, pow (SNR, a->np.snrq)); - - for (k = 0; k < a->np.msize; k++) - if (a->np.alphaOptHat[k] < alphaMin) a->np.alphaOptHat[k] = alphaMin; - - f1 = sum_prev_p / sum_lambda_y - 1.0; - alphaCtilda = 1.0 / (1.0 + f1 * f1); - a->np.alphaC = a->np.alphaCsmooth * a->np.alphaC + (1.0 - a->np.alphaCsmooth) * std::max (alphaCtilda, a->np.alphaCmin); - f2 = a->np.alphaMax * a->np.alphaC; - - for (k = 0; k < a->np.msize; k++) - a->np.alphaHat[k] = f2 * a->np.alphaOptHat[k]; - - for (k = 0; k < a->np.msize; k++) - a->np.p[k] = a->np.alphaHat[k] * a->np.p[k] + (1.0 - a->np.alphaHat[k]) * a->np.lambda_y[k]; - - invQbar = 0.0; - - for (k = 0; k < a->np.msize; k++) - { - beta = std::min (a->np.betamax, a->np.alphaHat[k] * a->np.alphaHat[k]); - a->np.pbar[k] = beta * a->np.pbar[k] + (1.0 - beta) * a->np.p[k]; - a->np.p2bar[k] = beta * a->np.p2bar[k] + (1.0 - beta) * a->np.p[k] * a->np.p[k]; - varHat = a->np.p2bar[k] - a->np.pbar[k] * a->np.pbar[k]; - invQeq = varHat / (2.0 * a->np.sigma2N[k] * a->np.sigma2N[k]); - if (invQeq > a->np.invQeqMax) invQeq = a->np.invQeqMax; - a->np.Qeq[k] = 1.0 / invQeq; - invQbar += invQeq; - } - invQbar /= (double) a->np.msize; - bc = 1.0 + a->np.av * sqrt (invQbar); - - for (k = 0; k < a->np.msize; k++) - { - QeqTilda = (a->np.Qeq[k] - 2.0 * a->np.MofD) / (1.0 - a->np.MofD); - QeqTildaSub = (a->np.Qeq[k] - 2.0 * a->np.MofV) / (1.0 - a->np.MofV); - a->np.bmin[k] = 1.0 + 2.0 * (a->np.D - 1.0) / QeqTilda; - a->np.bmin_sub[k] = 1.0 + 2.0 * (a->np.V - 1.0) / QeqTildaSub; - } - - std::fill(a->np.k_mod, a->np.k_mod + a->np.msize, 0); - - for (k = 0; k < a->np.msize; k++) - { - f3 = a->np.p[k] * a->np.bmin[k] * bc; - - if (f3 < a->np.actmin[k]) - { - a->np.actmin[k] = f3; - a->np.actmin_sub[k] = a->np.p[k] * a->np.bmin_sub[k] * bc; - a->np.k_mod[k] = 1; - } - } - - if (a->np.subwc == a->np.V) - { - if (invQbar < a->np.invQbar_points[0]) - noise_slope_max = a->np.nsmax[0]; - else if (invQbar < a->np.invQbar_points[1]) - noise_slope_max = a->np.nsmax[1]; - else if (invQbar < a->np.invQbar_points[2]) - noise_slope_max = a->np.nsmax[2]; - else - noise_slope_max = a->np.nsmax[3]; - - for (k = 0; k < a->np.msize; k++) - { - int ku; - double min; - - if (a->np.k_mod[k]) - a->np.lmin_flag[k] = 0; - - a->np.actminbuff[a->np.amb_idx][k] = a->np.actmin[k]; - min = std::numeric_limits::max();; - - for (ku = 0; ku < a->np.U; ku++) - { - if (a->np.actminbuff[ku][k] < min) - min = a->np.actminbuff[ku][k]; - } - - a->np.pmin_u[k] = min; - - if ((a->np.lmin_flag[k] == 1) - && (a->np.actmin_sub[k] < noise_slope_max * a->np.pmin_u[k]) - && (a->np.actmin_sub[k] > a->np.pmin_u[k])) - { - a->np.pmin_u[k] = a->np.actmin_sub[k]; - for (ku = 0; ku < a->np.U; ku++) - a->np.actminbuff[ku][k] = a->np.actmin_sub[k]; - } - - a->np.lmin_flag[k] = 0; - a->np.actmin[k] = std::numeric_limits::max();; - a->np.actmin_sub[k] = std::numeric_limits::max();; - } - - if (++a->np.amb_idx == a->np.U) - a->np.amb_idx = 0; - - a->np.subwc = 1; - } - else - { - if (a->np.subwc > 1) - { - for (k = 0; k < a->np.msize; k++) - { - if (a->np.k_mod[k]) - { - a->np.lmin_flag[k] = 1; - a->np.sigma2N[k] = std::min (a->np.actmin_sub[k], a->np.pmin_u[k]); - a->np.pmin_u[k] = a->np.sigma2N[k]; - } - } - } - - ++a->np.subwc; - } - - std::copy(a->np.sigma2N, a->np.sigma2N + a->np.msize, a->np.lambda_d); -} - -void EMNR::LambdaDs (EMNR *a) -{ - int k; - - for (k = 0; k < a->nps.msize; k++) - { - a->nps.PH1y[k] = 1.0 / (1.0 + (1.0 + a->nps.epsH1) * exp (- a->nps.epsH1r * a->nps.lambda_y[k] / a->nps.sigma2N[k])); - a->nps.Pbar[k] = a->nps.alpha_Pbar * a->nps.Pbar[k] + (1.0 - a->nps.alpha_Pbar) * a->nps.PH1y[k]; - - if (a->nps.Pbar[k] > 0.99) - a->nps.PH1y[k] = std::min (a->nps.PH1y[k], 0.99); - - a->nps.EN2y[k] = (1.0 - a->nps.PH1y[k]) * a->nps.lambda_y[k] + a->nps.PH1y[k] * a->nps.sigma2N[k]; - a->nps.sigma2N[k] = a->nps.alpha_pow * a->nps.sigma2N[k] + (1.0 - a->nps.alpha_pow) * a->nps.EN2y[k]; - } - - std::copy(a->nps.sigma2N, a->nps.sigma2N + a->nps.msize, a->nps.lambda_d); -} - -void EMNR::aepf(EMNR *a) +void EMNR::aepf() { int k, m; int N, n; @@ -793,15 +913,15 @@ void EMNR::aepf(EMNR *a) sumPre = 0.0; sumPost = 0.0; - for (k = 0; k < a->ae.msize; k++) + for (k = 0; k < ae->msize; k++) { - sumPre += a->ae.lambda_y[k]; - sumPost += a->mask[k] * a->mask[k] * a->ae.lambda_y[k]; + sumPre += ae->lambda_y[k]; + sumPost += mask[k] * mask[k] * ae->lambda_y[k]; } zeta = sumPost / sumPre; - if (zeta >= a->ae.zetaThresh) + if (zeta >= ae->zetaThresh) zetaT = 1.0; else zetaT = zeta; @@ -809,19 +929,19 @@ void EMNR::aepf(EMNR *a) if (zetaT == 1.0) N = 1; else - N = 1 + 2 * (int)(0.5 + a->ae.psi * (1.0 - zetaT / a->ae.zetaThresh)); + N = 1 + 2 * (int)(0.5 + ae->psi * (1.0 - zetaT / ae->zetaThresh)); n = N / 2; - for (k = n; k < (a->ae.msize - n); k++) + for (k = n; k < (ae->msize - n); k++) { - a->ae.nmask[k] = 0.0; + ae->nmask[k] = 0.0; for (m = k - n; m <= (k + n); m++) - a->ae.nmask[k] += a->mask[m]; - a->ae.nmask[k] /= (float)N; + ae->nmask[k] += mask[m]; + ae->nmask[k] /= (float)N; } - std::copy(a->ae.nmask, a->ae.nmask + (a->ae.msize - 2 * n), a->mask + n); + std::copy(ae->nmask, ae->nmask + (ae->msize - 2 * n), &mask[n]); } double EMNR::getKey(double* type, double gamma, double xi) @@ -873,57 +993,57 @@ double EMNR::getKey(double* type, double gamma, double xi) + dg * dx * type[241 * nxi2 + ngamma2]; } -void EMNR::calc_gain (EMNR *a) +void EMNR::calc_gain() { int k; - for (k = 0; k < a->msize; k++) + for (k = 0; k < msize; k++) { - double y0 = a->g.y[2 * k + 0]; - double y1 = a->g.y[2 * k + 1]; - a->g.lambda_y[k] = y0 * y0 + y1 * y1; + double y0 = g->y[2 * k + 0]; + double y1 = g->y[2 * k + 1]; + g->lambda_y[k] = y0 * y0 + y1 * y1; } - switch (a->g.npe_method) + switch (g->npe_method) { case 0: - LambdaD(a); + np->LambdaD(); break; case 1: - LambdaDs(a); + nps->LambdaDs(); break; } - switch (a->g.gain_method) + switch (g->gain_method) { case 0: { double gamma, eps_hat, v; - for (k = 0; k < a->msize; k++) + for (k = 0; k < msize; k++) { - gamma = std::min (a->g.lambda_y[k] / a->g.lambda_d[k], a->g.gamma_max); - eps_hat = a->g.alpha * a->g.prev_mask[k] * a->g.prev_mask[k] * a->g.prev_gamma[k] - + (1.0 - a->g.alpha) * std::max (gamma - 1.0f, a->g.eps_floor); + gamma = std::min (g->lambda_y[k] / g->lambda_d[k], g->gamma_max); + eps_hat = g->alpha * g->prev_mask[k] * g->prev_mask[k] * g->prev_gamma[k] + + (1.0 - g->alpha) * std::max (gamma - 1.0f, g->eps_floor); v = (eps_hat / (1.0 + eps_hat)) * gamma; - a->g.mask[k] = a->g.gf1p5 * sqrt (v) / gamma * exp (- 0.5 * v) + g->mask[k] = g->gf1p5 * sqrt (v) / gamma * exp (- 0.5 * v) * ((1.0 + v) * bessI0 (0.5 * v) + v * bessI1 (0.5 * v)); { double v2 = std::min (v, 700.0); - double eta = a->g.mask[k] * a->g.mask[k] * a->g.lambda_y[k] / a->g.lambda_d[k]; - double eps = eta / (1.0 - a->g.q); - double witchHat = (1.0 - a->g.q) / a->g.q * exp (v2) / (1.0 + eps); - a->g.mask[k] *= witchHat / (1.0 + witchHat); + double eta = g->mask[k] * g->mask[k] * g->lambda_y[k] / g->lambda_d[k]; + double eps = eta / (1.0 - g->q); + double witchHat = (1.0 - g->q) / g->q * exp (v2) / (1.0 + eps); + g->mask[k] *= witchHat / (1.0 + witchHat); } - if (a->g.mask[k] > a->g.gmax) - a->g.mask[k] = a->g.gmax; + if (g->mask[k] > g->gmax) + g->mask[k] = g->gmax; - if (a->g.mask[k] != a->g.mask[k]) - a->g.mask[k] = 0.01; + if (g->mask[k] != g->mask[k]) + g->mask[k] = 0.01; - a->g.prev_gamma[k] = gamma; - a->g.prev_mask[k] = a->g.mask[k]; + g->prev_gamma[k] = gamma; + g->prev_mask[k] = g->mask[k]; } break; @@ -933,22 +1053,22 @@ void EMNR::calc_gain (EMNR *a) { double gamma, eps_hat, v, ehr; - for (k = 0; k < a->msize; k++) + for (k = 0; k < msize; k++) { - gamma = std::min (a->g.lambda_y[k] / a->g.lambda_d[k], a->g.gamma_max); - eps_hat = a->g.alpha * a->g.prev_mask[k] * a->g.prev_mask[k] * a->g.prev_gamma[k] - + (1.0 - a->g.alpha) * std::max (gamma - 1.0f, a->g.eps_floor); + gamma = std::min (g->lambda_y[k] / g->lambda_d[k], g->gamma_max); + eps_hat = g->alpha * g->prev_mask[k] * g->prev_mask[k] * g->prev_gamma[k] + + (1.0 - g->alpha) * std::max (gamma - 1.0f, g->eps_floor); ehr = eps_hat / (1.0 + eps_hat); v = ehr * gamma; - if ((a->g.mask[k] = ehr * exp (std::min (700.0, 0.5 * e1xb(v)))) > a->g.gmax) - a->g.mask[k] = a->g.gmax; + if ((g->mask[k] = ehr * exp (std::min (700.0, 0.5 * e1xb(v)))) > g->gmax) + g->mask[k] = g->gmax; - if (a->g.mask[k] != a->g.mask[k]) - a->g.mask[k] = 0.01; + if (g->mask[k] != g->mask[k]) + g->mask[k] = 0.01; - a->g.prev_gamma[k] = gamma; - a->g.prev_mask[k] = a->g.mask[k]; + g->prev_gamma[k] = gamma; + g->prev_mask[k] = g->mask[k]; } break; @@ -958,111 +1078,111 @@ void EMNR::calc_gain (EMNR *a) { double gamma, eps_hat, eps_p; - for (k = 0; k < a->msize; k++) + for (k = 0; k < msize; k++) { - gamma = std::min(a->g.lambda_y[k] / a->g.lambda_d[k], a->g.gamma_max); - eps_hat = a->g.alpha * a->g.prev_mask[k] * a->g.prev_mask[k] * a->g.prev_gamma[k] - + (1.0 - a->g.alpha) * std::max(gamma - 1.0f, a->g.eps_floor); - eps_p = eps_hat / (1.0 - a->g.q); - a->g.mask[k] = getKey(a->g.GG, gamma, eps_hat) * getKey(a->g.GGS, gamma, eps_p); - a->g.prev_gamma[k] = gamma; - a->g.prev_mask[k] = a->g.mask[k]; + gamma = std::min(g->lambda_y[k] / g->lambda_d[k], g->gamma_max); + eps_hat = g->alpha * g->prev_mask[k] * g->prev_mask[k] * g->prev_gamma[k] + + (1.0 - g->alpha) * std::max(gamma - 1.0f, g->eps_floor); + eps_p = eps_hat / (1.0 - g->q); + g->mask[k] = getKey(g->GG, gamma, eps_hat) * getKey(g->GGS, gamma, eps_p); + g->prev_gamma[k] = gamma; + g->prev_mask[k] = g->mask[k]; } break; } } - if (a->g.ae_run) - aepf(a); + if (g->ae_run) + aepf(); } -void EMNR::xemnr (EMNR *a, int pos) +void EMNR::execute(int _pos) { - if (a->run && pos == a->position) + if (run && _pos == position) { int i, j, k, sbuff, sbegin; double g1; - for (i = 0; i < 2 * a->bsize; i += 2) + for (i = 0; i < 2 * bsize; i += 2) { - a->inaccum[a->iainidx] = a->in[i]; - a->iainidx = (a->iainidx + 1) % a->iasize; + inaccum[iainidx] = in[i]; + iainidx = (iainidx + 1) % iasize; } - a->nsamps += a->bsize; + nsamps += bsize; - while (a->nsamps >= a->fsize) + while (nsamps >= fsize) { - for (i = 0, j = a->iaoutidx; i < a->fsize; i++, j = (j + 1) % a->iasize) - a->forfftin[i] = a->window[i] * a->inaccum[j]; + for (i = 0, j = iaoutidx; i < fsize; i++, j = (j + 1) % iasize) + forfftin[i] = window[i] * inaccum[j]; - a->iaoutidx = (a->iaoutidx + a->incr) % a->iasize; - a->nsamps -= a->incr; - fftwf_execute (a->Rfor); - calc_gain(a); + iaoutidx = (iaoutidx + incr) % iasize; + nsamps -= incr; + fftwf_execute (Rfor); + calc_gain(); - for (i = 0; i < a->msize; i++) + for (i = 0; i < msize; i++) { - g1 = a->gain * a->mask[i]; - a->revfftin[2 * i + 0] = g1 * a->forfftout[2 * i + 0]; - a->revfftin[2 * i + 1] = g1 * a->forfftout[2 * i + 1]; + g1 = gain * mask[i]; + revfftin[2 * i + 0] = g1 * forfftout[2 * i + 0]; + revfftin[2 * i + 1] = g1 * forfftout[2 * i + 1]; } - fftwf_execute (a->Rrev); + fftwf_execute (Rrev); - for (i = 0; i < a->fsize; i++) - a->save[a->saveidx][i] = a->window[i] * a->revfftout[i]; + for (i = 0; i < fsize; i++) + save[saveidx][i] = window[i] * revfftout[i]; - for (i = a->ovrlp; i > 0; i--) + for (i = ovrlp; i > 0; i--) { - sbuff = (a->saveidx + i) % a->ovrlp; - sbegin = a->incr * (a->ovrlp - i); + sbuff = (saveidx + i) % ovrlp; + sbegin = incr * (ovrlp - i); - for (j = sbegin, k = a->oainidx; j < a->incr + sbegin; j++, k = (k + 1) % a->oasize) + for (j = sbegin, k = oainidx; j < incr + sbegin; j++, k = (k + 1) % oasize) { - if ( i == a->ovrlp) - a->outaccum[k] = a->save[sbuff][j]; + if ( i == ovrlp) + outaccum[k] = save[sbuff][j]; else - a->outaccum[k] += a->save[sbuff][j]; + outaccum[k] += save[sbuff][j]; } } - a->saveidx = (a->saveidx + 1) % a->ovrlp; - a->oainidx = (a->oainidx + a->incr) % a->oasize; + saveidx = (saveidx + 1) % ovrlp; + oainidx = (oainidx + incr) % oasize; } - for (i = 0; i < a->bsize; i++) + for (i = 0; i < bsize; i++) { - a->out[2 * i + 0] = a->outaccum[a->oaoutidx]; - a->out[2 * i + 1] = 0.0; - a->oaoutidx = (a->oaoutidx + 1) % a->oasize; + out[2 * i + 0] = outaccum[oaoutidx]; + out[2 * i + 1] = 0.0; + oaoutidx = (oaoutidx + 1) % oasize; } } - else if (a->out != a->in) + else if (out != in) { - std::copy(a->in, a->in + a->bsize * 2, a->out); + std::copy(in, in + bsize * 2, out); } } -void EMNR::setBuffers_emnr (EMNR *a, float* in, float* out) +void EMNR::setBuffers(float* _in, float* _out) { - a->in = in; - a->out = out; + in = _in; + out = _out; } -void EMNR::setSamplerate_emnr (EMNR *a, int rate) +void EMNR::setSamplerate(int _rate) { - decalc_emnr (a); - a->rate = rate; - calc_emnr (a); + decalc(); + rate = _rate; + calc(); } -void EMNR::setSize_emnr (EMNR *a, int size) +void EMNR::setSize(int _size) { - decalc_emnr (a); - a->bsize = size; - calc_emnr (a); + decalc(); + bsize = _size; + calc(); } /******************************************************************************************************** @@ -1071,35 +1191,29 @@ void EMNR::setSize_emnr (EMNR *a, int size) * * ********************************************************************************************************/ -void EMNR::SetEMNRgainMethod (RXA& rxa, int method) +void EMNR::setGainMethod(int _method) { - rxa.emnr->g.gain_method = method; + g->gain_method = _method; } -void EMNR::SetEMNRnpeMethod (RXA& rxa, int method) +void EMNR::setNpeMethod(int _method) { - rxa.emnr->g.npe_method = method; + g->npe_method = _method; } -void EMNR::SetEMNRaeRun (RXA& rxa, int run) +void EMNR::setAeRun(int _run) { - rxa.emnr->g.ae_run = run; + g->ae_run = _run; } -void EMNR::SetEMNRPosition (RXA& rxa, int position) +void EMNR::setAeZetaThresh(double _zetathresh) { - rxa.emnr->position = position; - rxa.bp1->position = position; + ae->zetaThresh = _zetathresh; } -void EMNR::SetEMNRaeZetaThresh (RXA& rxa, double zetathresh) +void EMNR::setAePsi(double _psi) { - rxa.emnr->ae.zetaThresh = zetathresh; -} - -void EMNR::SetEMNRaePsi (RXA& rxa, double psi) -{ - rxa.emnr->ae.psi = psi; + ae->psi = _psi; } } // namespace WDSP diff --git a/wdsp/emnr.hpp b/wdsp/emnr.hpp index dadeb0176..fc4a6efb9 100644 --- a/wdsp/emnr.hpp +++ b/wdsp/emnr.hpp @@ -28,6 +28,9 @@ warren@wpratt.com #ifndef wdsp_emnr_h #define wdsp_emnr_h +#include +#include + #include "fftw3.h" #include "export.h" @@ -46,18 +49,18 @@ public: int fsize; int ovrlp; int incr; - float* window; + std::vector window; int iasize; - float* inaccum; - float* forfftin; - float* forfftout; + std::vector inaccum; + std::vector forfftin; + std::vector forfftout; int msize; - double* mask; - float* revfftin; - float* revfftout; - float** save; + std::vector mask; + std::vector revfftin; + std::vector revfftout; + std::vector> save; int oasize; - float* outaccum; + std::vector outaccum; double rate; int wintype; double ogain; @@ -71,14 +74,16 @@ public: int saveidx; fftwf_plan Rfor; fftwf_plan Rrev; - struct _g + struct G { + int incr; + double rate; + int msize; + double* mask; + float* y; int gain_method; int npe_method; int ae_run; - double msize; - double* mask; - float* y; double* lambda_y; double* lambda_d; double* prev_mask; @@ -93,36 +98,48 @@ public: double* GG; double* GGS; FILE* fileb; - } g; - struct _npest + G( + int incr, + double rate, + int msize, + double* mask, + float* y + ); + G(const G&) = delete; + G& operator=(const G& other) = delete; + ~G(); + } *g; + struct NP { int incr; double rate; int msize; double* lambda_y; double* lambda_d; - double* p; - double* alphaOptHat; - double alphaC; double alphaCsmooth; - double alphaCmin; - double* alphaHat; double alphaMax; - double* sigma2N; + double alphaCmin; double alphaMin_max_value; double snrq; double betamax; - double* pbar; - double* p2bar; double invQeqMax; double av; - double* Qeq; - int U; double Dtime; + int U; int V; int D; + double* p; + double* alphaOptHat; + double alphaC; + double* alphaHat; + double* sigma2N; + double* pbar; + double* p2bar; + double* Qeq; double MofD; double MofV; + std::array invQbar_points; + std::array nsmax; double* bmin; double* bmin_sub; int* k_mod; @@ -131,12 +148,32 @@ public: int subwc; int* lmin_flag; double* pmin_u; - double invQbar_points[4]; - double nsmax[4]; double** actminbuff; int amb_idx; - } np; - struct _npests + NP( + int incr, + double rate, + int msize, + double* lambda_y, + double* lambda_d + ); + NP(const NP&) = delete; + NP& operator=(const NP& other) = delete; + ~NP(); + void LambdaD(); + + private: + static const std::array DVals; + static const std::array MVals; + static void interpM ( + double* res, + double x, + int nvals, + const std::array& xvals, + const std::array& yvals + ); + } *np; + struct NPS { int incr; double rate; @@ -153,17 +190,41 @@ public: double* PH1y; double* Pbar; double* EN2y; - } nps; - struct _ae + NPS( + int incr, + double rate, + int msize, + double* lambda_y, + double* lambda_d, + + double alpha_pow, + double alpha_Pbar, + double epsH1 + ); + NPS(const NPS&) = delete; + NPS& operator=(const NPS& other) = delete; + ~NPS(); + void LambdaDs(); + } *nps; + struct AE { int msize; double* lambda_y; double zetaThresh; double psi; double* nmask; - } ae; + AE( + int msize, + double* lambda_y, + double zetaThresh, + double psi + ); + AE(const AE&) = delete; + AE& operator=(const AE& other) = delete; + ~AE(); + } *ae; - static EMNR* create_emnr ( + EMNR( int run, int position, int size, @@ -178,33 +239,33 @@ public: int npe_method, int ae_run ); - static void destroy_emnr (EMNR *a); - static void flush_emnr (EMNR *a); - static void xemnr (EMNR *a, int pos); - static void setBuffers_emnr (EMNR *a, float* in, float* out); - static void setSamplerate_emnr (EMNR *a, int rate); - static void setSize_emnr (EMNR *a, int size); - // RXA Properties - static void SetEMNRgainMethod (RXA& rxa, int method); - static void SetEMNRnpeMethod (RXA& rxa, int method); - static void SetEMNRaeRun (RXA& rxa, int run); - static void SetEMNRPosition (RXA& rxa, int position); - static void SetEMNRaeZetaThresh (RXA& rxa, double zetathresh); - static void SetEMNRaePsi (RXA& rxa, double psi); + EMNR(const EMNR&) = delete; + EMNR& operator=(const EMNR& other) = delete; + ~EMNR(); + + void flush(); + void execute(int pos); + void setBuffers(float* in, float* out); + void setSamplerate(int rate); + void setSize(int size); + // Public Properties + void setGainMethod(int method); + void setNpeMethod(int method); + void setAeRun(int run); + void setAeZetaThresh(double zetathresh); + void setAePsi(double psi); private: static double bessI0 (double x); static double bessI1 (double x); static double e1xb (double x); - static void calc_window (EMNR *a); static void interpM (double* res, double x, int nvals, double* xvals, double* yvals); - static void calc_emnr(EMNR *a); - static void decalc_emnr(EMNR *a); - static void LambdaD(EMNR *a); - static void LambdaDs (EMNR *a); - static void aepf(EMNR *a); static double getKey(double* type, double gamma, double xi); - static void calc_gain (EMNR *a); + void calc_window(); + void calc(); + void decalc(); + void aepf(); + void calc_gain(); }; } // namespace WDSP diff --git a/wdsp/eqp.hpp b/wdsp/eqp.hpp index 086bd42ab..32423d0c7 100644 --- a/wdsp/eqp.hpp +++ b/wdsp/eqp.hpp @@ -76,6 +76,7 @@ public: int samplerate ); EQP(const EQP&) = delete; + EQP& operator=(const EQP& other) = delete; ~EQP(); void flush(); diff --git a/wdsp/fmd.hpp b/wdsp/fmd.hpp index f8e37be77..204bd5d64 100644 --- a/wdsp/fmd.hpp +++ b/wdsp/fmd.hpp @@ -111,6 +111,7 @@ public: int mp_aud ); FMD(const FMD&) = delete; + FMD& operator=(const FMD& other) = delete; ~FMD(); void flush(); diff --git a/wdsp/fmsq.hpp b/wdsp/fmsq.hpp index 02d2b13df..1d5b8f614 100644 --- a/wdsp/fmsq.hpp +++ b/wdsp/fmsq.hpp @@ -101,6 +101,7 @@ public: int _mp ); FMSQ(const FMSQ&) = delete; + FMSQ& operator=(const FMSQ& other) = delete; ~FMSQ(); void flush(); diff --git a/wdsp/nbp.hpp b/wdsp/nbp.hpp index 97b52c5b3..4bb04429f 100644 --- a/wdsp/nbp.hpp +++ b/wdsp/nbp.hpp @@ -52,6 +52,7 @@ public: NOTCHDB(int master_run, int maxnotches); NOTCHDB(const NOTCHDB&) = delete; + NOTCHDB& operator=(const NOTCHDB& other) = delete; ~NOTCHDB() = default; int addNotch (int notch, double fcenter, double fwidth, int active); @@ -108,6 +109,7 @@ public: NOTCHDB* notchdb ); NBP(const NBP&) = delete; + NBP& operator=(const NBP& other) = delete; ~NBP(); void flush(); diff --git a/wdsp/nob.hpp b/wdsp/nob.hpp index adfc525d9..6264d00be 100644 --- a/wdsp/nob.hpp +++ b/wdsp/nob.hpp @@ -99,6 +99,7 @@ public: double threshold ); NOB(const NOB&) = delete; + NOB& operator=(const NOB& other) = delete; ~NOB() = default; //////////// legacy interface - remove void flush(); diff --git a/wdsp/resample.hpp b/wdsp/resample.hpp index d442039fe..5b4a12357 100644 --- a/wdsp/resample.hpp +++ b/wdsp/resample.hpp @@ -76,6 +76,7 @@ public: double gain ); RESAMPLE(const RESAMPLE&) = delete; + RESAMPLE& operator=(const RESAMPLE& other) = delete; ~RESAMPLE() = default; void flush();