# HG changeset patch
# User Chris Cannam
# Date 1444210569 -3600
# Node ID ca658c7215a97a28f3d74e444ed3d1f4a032cc05
# Parent  3780b91297eaf950ddd17448ea89f85148a82324
Faster filter implementation with explicit FIR support

diff -r 3780b91297ea -r ca658c7215a9 dsp/signalconditioning/DFProcess.cpp
--- a/dsp/signalconditioning/DFProcess.cpp	Wed Oct 07 10:07:30 2015 +0100
+++ b/dsp/signalconditioning/DFProcess.cpp	Wed Oct 07 10:36:09 2015 +0100
@@ -59,13 +59,11 @@
     filtSrc = new double[ m_length ];
     filtDst = new double[ m_length ];
 
-	
-    //Low Pass Smoothing Filter Config
-    m_FilterConfigParams.ord = Config.LPOrd;
-    m_FilterConfigParams.ACoeffs = Config.LPACoeffs;
-    m_FilterConfigParams.BCoeffs = Config.LPBCoeffs;
-	
-    m_FiltFilt = new FiltFilt( m_FilterConfigParams );
+    Filter::Parameters params;
+    params.a = std::vector<double>(Config.LPACoeffs, Config.LPACoeffs + Config.LPOrd + 1);
+    params.b = std::vector<double>(Config.LPBCoeffs, Config.LPBCoeffs + Config.LPOrd + 1);
+    
+    m_FiltFilt = new FiltFilt(params);
 	
     //add delta threshold
     m_delta = Config.delta;
diff -r 3780b91297ea -r ca658c7215a9 dsp/signalconditioning/DFProcess.h
--- a/dsp/signalconditioning/DFProcess.h	Wed Oct 07 10:07:30 2015 +0100
+++ b/dsp/signalconditioning/DFProcess.h	Wed Oct 07 10:36:09 2015 +0100
@@ -81,8 +81,6 @@
     double* m_filtScratchIn;
     double* m_filtScratchOut;
 
-    FilterConfig m_FilterConfigParams;
-
     FiltFilt* m_FiltFilt;
 
     bool m_isMedianPositive;
diff -r 3780b91297ea -r ca658c7215a9 dsp/signalconditioning/FiltFilt.cpp
--- a/dsp/signalconditioning/FiltFilt.cpp	Wed Oct 07 10:07:30 2015 +0100
+++ b/dsp/signalconditioning/FiltFilt.cpp	Wed Oct 07 10:36:09 2015 +0100
@@ -19,36 +19,16 @@
 // Construction/Destruction
 //////////////////////////////////////////////////////////////////////
 
-FiltFilt::FiltFilt( FilterConfig Config )
+FiltFilt::FiltFilt(Filter::Parameters parameters) :
+    m_filter(parameters)
 {
-    m_filtScratchIn = NULL;
-    m_filtScratchOut = NULL;
-    m_ord = 0;
-	
-    initialise( Config );
+    m_ord = m_filter.getOrder();
 }
 
 FiltFilt::~FiltFilt()
 {
-    deInitialise();
 }
 
-void FiltFilt::initialise( FilterConfig Config )
-{
-    m_ord = Config.ord;
-    m_filterConfig.ord = Config.ord;
-    m_filterConfig.ACoeffs = Config.ACoeffs;
-    m_filterConfig.BCoeffs = Config.BCoeffs;
-	
-    m_filter = new Filter( m_filterConfig );
-}
-
-void FiltFilt::deInitialise()
-{
-    delete m_filter;
-}
-
-
 void FiltFilt::process(double *src, double *dst, unsigned int length)
 {	
     unsigned int i;
@@ -59,14 +39,13 @@
     unsigned int nFact = 3 * ( nFilt - 1);
     unsigned int nExt	= length + 2 * nFact;
 
-    m_filtScratchIn = new double[ nExt ];
-    m_filtScratchOut = new double[ nExt ];
-
+    double *filtScratchIn = new double[ nExt ];
+    double *filtScratchOut = new double[ nExt ];
 	
     for( i = 0; i< nExt; i++ ) 
     {
-	m_filtScratchIn[ i ] = 0.0;
-	m_filtScratchOut[ i ] = 0.0;
+	filtScratchIn[ i ] = 0.0;
+	filtScratchOut[ i ] = 0.0;
     }
 
     // Edge transients reflection
@@ -76,51 +55,51 @@
     unsigned int index = 0;
     for( i = nFact; i > 0; i-- )
     {
-	m_filtScratchIn[ index++ ] = sample0 - src[ i ];
+	filtScratchIn[ index++ ] = sample0 - src[ i ];
     }
     index = 0;
     for( i = 0; i < nFact; i++ )
     {
-	m_filtScratchIn[ (nExt - nFact) + index++ ] = sampleN - src[ (length - 2) - i ];
+	filtScratchIn[ (nExt - nFact) + index++ ] = sampleN - src[ (length - 2) - i ];
     }
 
     index = 0;
     for( i = 0; i < length; i++ )
     {
-	m_filtScratchIn[ i + nFact ] = src[ i ];
+	filtScratchIn[ i + nFact ] = src[ i ];
     }
 	
     ////////////////////////////////
     // Do  0Ph filtering
-    m_filter->process( m_filtScratchIn, m_filtScratchOut, nExt);
+    m_filter.process( filtScratchIn, filtScratchOut, nExt);
 	
     // reverse the series for FILTFILT 
     for ( i = 0; i < nExt; i++)
     { 
-	m_filtScratchIn[ i ] = m_filtScratchOut[ nExt - i - 1];
+	filtScratchIn[ i ] = filtScratchOut[ nExt - i - 1];
     }
 
     // do FILTER again 
-    m_filter->process( m_filtScratchIn, m_filtScratchOut, nExt);
+    m_filter.process( filtScratchIn, filtScratchOut, nExt);
 	
     // reverse the series back 
     for ( i = 0; i < nExt; i++)
     {
-	m_filtScratchIn[ i ] = m_filtScratchOut[ nExt - i - 1 ];
+	filtScratchIn[ i ] = filtScratchOut[ nExt - i - 1 ];
     }
     for ( i = 0;i < nExt; i++)
     {
-	m_filtScratchOut[ i ] = m_filtScratchIn[ i ];
+	filtScratchOut[ i ] = filtScratchIn[ i ];
     }
 
     index = 0;
     for( i = 0; i < length; i++ )
     {
-	dst[ index++ ] = m_filtScratchOut[ i + nFact ];
+	dst[ index++ ] = filtScratchOut[ i + nFact ];
     }	
 
-    delete [] m_filtScratchIn;
-    delete [] m_filtScratchOut;
+    delete [] filtScratchIn;
+    delete [] filtScratchOut;
 
 }
 
diff -r 3780b91297ea -r ca658c7215a9 dsp/signalconditioning/FiltFilt.h
--- a/dsp/signalconditioning/FiltFilt.h	Wed Oct 07 10:07:30 2015 +0100
+++ b/dsp/signalconditioning/FiltFilt.h	Wed Oct 07 10:36:09 2015 +0100
@@ -20,30 +20,21 @@
 
 /**
  * Zero-phase digital filter, implemented by processing the data
- * through a filter specified by the given FilterConfig structure (see
+ * through a filter specified by the given filter parameters (see
  * Filter) and then processing it again in reverse.
  */
 class FiltFilt  
 {
 public:
-    FiltFilt( FilterConfig Config );
+    FiltFilt(Filter::Parameters);
     virtual ~FiltFilt();
 
     void reset();
     void process( double* src, double* dst, unsigned int length );
 
 private:
-    void initialise( FilterConfig Config );
-    void deInitialise();
-
-    unsigned int m_ord;
-
-    Filter* m_filter;
-
-    double* m_filtScratchIn;
-    double* m_filtScratchOut;
-
-    FilterConfig m_filterConfig;
+    Filter m_filter;
+    int m_ord;
 };
 
 #endif
diff -r 3780b91297ea -r ca658c7215a9 dsp/signalconditioning/Filter.cpp
--- a/dsp/signalconditioning/Filter.cpp	Wed Oct 07 10:07:30 2015 +0100
+++ b/dsp/signalconditioning/Filter.cpp	Wed Oct 07 10:36:09 2015 +0100
@@ -4,7 +4,6 @@
     QM DSP Library
 
     Centre for Digital Music, Queen Mary, University of London.
-    This file 2005-2006 Christian Landone.
 
     This program is free software; you can redistribute it and/or
     modify it under the terms of the GNU General Public License as
@@ -15,73 +14,112 @@
 
 #include "Filter.h"
 
-//////////////////////////////////////////////////////////////////////
-// Construction/Destruction
-//////////////////////////////////////////////////////////////////////
+#include <stdexcept>
 
-Filter::Filter( FilterConfig Config )
+using namespace std;
+
+Filter::Filter(Parameters params)
 {
-    m_ord = 0;
-    m_outBuffer = NULL;
-    m_inBuffer = NULL;
+    if (params.a.empty()) {
+        m_fir = true;
+        if (params.b.empty()) {
+            throw logic_error("Filter must have at least one pair of coefficients");
+        }
+    } else {
+        m_fir = false;
+        if (params.a.size() != params.b.size()) {
+            throw logic_error("Inconsistent numbers of filter coefficients");
+        }
+    }
+    
+    m_sz = int(params.b.size());
+    m_order = m_sz - 1;
 
-    initialise( Config );
+    m_a = params.a;
+    m_b = params.b;
+    
+    // We keep some empty space at the start of the buffer, and
+    // encroach gradually into it as we add individual sample
+    // calculations at the start. Then when we run out of space, we
+    // move the buffer back to the end and begin again. This is
+    // significantly faster than moving the whole buffer along in
+    // 1-sample steps every time.
+
+    m_offmax = 20;
+    m_offa = m_offmax;
+    m_offb = m_offmax;
+
+    if (!m_fir) {
+        m_bufa.resize(m_order + m_offmax);
+    }
+
+    m_bufb.resize(m_sz + m_offmax);
 }
 
 Filter::~Filter()
 {
-    deInitialise();
 }
 
-void Filter::initialise( FilterConfig Config )
+void
+Filter::reset()
 {
-    m_ord = Config.ord;
-    m_ACoeffs = Config.ACoeffs;
-    m_BCoeffs = Config.BCoeffs;
+    m_offb = m_offmax;
+    m_offa = m_offmax;
 
-    m_inBuffer = new double[ m_ord + 1 ];
-    m_outBuffer = new double[ m_ord + 1 ];
+    if (!m_fir) {
+        m_bufa.assign(m_bufa.size(), 0.0);
+    }
 
-    reset();
+    m_bufb.assign(m_bufb.size(), 0.0);
 }
 
-void Filter::deInitialise()
+void
+Filter::process(const double *const __restrict__ in,
+		double *const __restrict__ out,
+		const int n)
 {
-    delete[] m_inBuffer;
-    delete[] m_outBuffer;
+    for (int s = 0; s < n; ++s) {
+
+        if (m_offb > 0) --m_offb;
+        else {
+            for (int i = m_sz - 2; i >= 0; --i) {
+                m_bufb[i + m_offmax + 1] = m_bufb[i];
+            }
+            m_offb = m_offmax;
+        }
+        m_bufb[m_offb] = in[s];
+
+        double b_sum = 0.0;
+        for (int i = 0; i < m_sz; ++i) {
+            b_sum += m_b[i] * m_bufb[i + m_offb];
+        }
+
+        double outval;
+
+        if (m_fir) {
+
+            outval = b_sum;
+
+        } else {
+
+            double a_sum = 0.0;
+            for (int i = 0; i < m_order; ++i) {
+                a_sum += m_a[i + 1] * m_bufa[i + m_offa];
+            }
+
+            outval = b_sum - a_sum;
+
+            if (m_offa > 0) --m_offa;
+            else {
+                for (int i = m_order - 2; i >= 0; --i) {
+                    m_bufa[i + m_offmax + 1] = m_bufa[i];
+                }
+                m_offa = m_offmax;
+            }
+            m_bufa[m_offa] = outval;
+        }
+        
+	out[s] = outval;
+    }
 }
 
-void Filter::reset()
-{
-    for( unsigned int i = 0; i < m_ord+1; i++ ){ m_inBuffer[ i ] = 0.0; }
-    for(unsigned int  i = 0; i < m_ord+1; i++ ){ m_outBuffer[ i ] = 0.0; }
-}
-
-void Filter::process( double *src, double *dst, unsigned int length )
-{
-    unsigned int SP,i,j;
-
-    double xin,xout;
-
-    for (SP=0;SP<length;SP++)
-    {
-        xin=src[SP];
-        /* move buffer */
-        for ( i = 0; i < m_ord; i++) {m_inBuffer[ m_ord - i ]=m_inBuffer[ m_ord - i - 1 ];}
-        m_inBuffer[0]=xin;
-
-        xout=0.0;
-        for (j=0;j< m_ord + 1; j++)
-	    xout = xout + m_BCoeffs[ j ] * m_inBuffer[ j ];
-        for (j = 0; j < m_ord; j++)
-	    xout= xout - m_ACoeffs[ j + 1 ] * m_outBuffer[ j ];
-
-        dst[ SP ] = xout;
-        for ( i = 0; i < m_ord - 1; i++ ) { m_outBuffer[ m_ord - i - 1 ] = m_outBuffer[ m_ord - i - 2 ];}
-        m_outBuffer[0]=xout;
-
-    } /* end of SP loop */
-}
-
-
-
diff -r 3780b91297ea -r ca658c7215a9 dsp/signalconditioning/Filter.h
--- a/dsp/signalconditioning/Filter.h	Wed Oct 07 10:07:30 2015 +0100
+++ b/dsp/signalconditioning/Filter.h	Wed Oct 07 10:36:09 2015 +0100
@@ -4,7 +4,6 @@
     QM DSP Library
 
     Centre for Digital Music, Queen Mary, University of London.
-    This file 2005-2006 Christian Landone.
 
     This program is free software; you can redistribute it and/or
     modify it under the terms of the GNU General Public License as
@@ -16,46 +15,53 @@
 #ifndef FILTER_H
 #define FILTER_H
 
-#ifndef NULL
-#define NULL 0
-#endif
+#include <vector>
 
-/**
- * Filter specification. For a filter of order ord, the ACoeffs and
- * BCoeffs arrays must point to ord+1 values each. ACoeffs provides
- * the denominator and BCoeffs the numerator coefficients of the
- * filter.
- */
-struct FilterConfig{
-    unsigned int ord;
-    double* ACoeffs;
-    double* BCoeffs;
-};
-
-/**
- * Digital filter specified through FilterConfig structure.
- */
-class Filter  
+class Filter
 {
 public:
-    Filter( FilterConfig Config );
-    virtual ~Filter();
+    struct Parameters {
+        std::vector<double> a;
+        std::vector<double> b;
+    };
+
+    /**
+     * Construct an IIR filter with numerators b and denominators
+     * a. The filter will have order b.size()-1. To make an FIR
+     * filter, leave the vector a in the param struct empty.
+     * Otherwise, a and b must have the same number of values.
+     */
+    Filter(Parameters params);
+    
+    ~Filter();
 
     void reset();
 
-    void process( double *src, double *dst, unsigned int length );
+    /**
+     * Filter the input sequence \arg in of length \arg n samples, and
+     * write the resulting \arg n samples into \arg out. There must be
+     * enough room in \arg out for \arg n samples to be written.
+     */
+    void process(const double *const __restrict__ in,
+                 double *const __restrict__ out,
+                 const int n);
 
+    int getOrder() const { return m_order; }
+    
 private:
-    void initialise( FilterConfig Config );
-    void deInitialise();
+    int m_order;
+    int m_sz;
+    std::vector<double> m_a;
+    std::vector<double> m_b;
+    std::vector<double> m_bufa;
+    std::vector<double> m_bufb;
+    int m_offa;
+    int m_offb;
+    int m_offmax;
+    bool m_fir;
 
-    unsigned int m_ord;
-
-    double* m_inBuffer;
-    double* m_outBuffer;
-
-    double* m_ACoeffs;
-    double* m_BCoeffs;
+    Filter(const Filter &); // not supplied
+    Filter &operator=(const Filter &); // not supplied
 };
-
+    
 #endif
diff -r 3780b91297ea -r ca658c7215a9 tests/TestFilter.cpp
--- a/tests/TestFilter.cpp	Wed Oct 07 10:07:30 2015 +0100
+++ b/tests/TestFilter.cpp	Wed Oct 07 10:36:09 2015 +0100
@@ -28,8 +28,7 @@
     vector<double> b(iir_b);
     vector<double> expected(iir_expected);
 
-    FilterConfig config { a.size()-1, a.data(), b.data() };
-    Filter f(config);
+    Filter f({ a, b });
     
     int n = expected.size();
     vector<double> out(n, 0.0);
@@ -49,8 +48,7 @@
     vector<double> b(iir_b);
     vector<double> expected(iir_expected);
 
-    FilterConfig config { a.size()-1, a.data(), b.data() };
-    Filter f(config);
+    Filter f({ a, b });
     
     int n = expected.size();
     vector<double> out(n, 0.0);
@@ -77,12 +75,10 @@
 
 BOOST_AUTO_TEST_CASE(fir)
 {
-    vector<double> a(fir_b.size(), 0.0); //!!!
     vector<double> b(fir_b);
     vector<double> expected(fir_expected);
 
-    FilterConfig config { b.size()-1, a.data(), b.data() };
-    Filter f(config);
+    Filter f({ {}, b });
     
     int n = expected.size();
     vector<double> out(n, 0.0);