cannam@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
cannam@0: 
cannam@0: /*
cannam@0:     QM DSP Library
cannam@0: 
cannam@0:     Centre for Digital Music, Queen Mary, University of London.
Chris@84: 
Chris@84:     This program is free software; you can redistribute it and/or
Chris@84:     modify it under the terms of the GNU General Public License as
Chris@84:     published by the Free Software Foundation; either version 2 of the
Chris@84:     License, or (at your option) any later version.  See the file
Chris@84:     COPYING included with this distribution for more information.
cannam@0: */
cannam@0: 
cannam@0: #include "Filter.h"
cannam@0: 
Chris@193: #include <stdexcept>
cannam@0: 
Chris@193: using namespace std;
Chris@193: 
Chris@193: Filter::Filter(Parameters params)
cannam@0: {
Chris@193:     if (params.a.empty()) {
Chris@193:         m_fir = true;
Chris@193:         if (params.b.empty()) {
Chris@193:             throw logic_error("Filter must have at least one pair of coefficients");
Chris@193:         }
Chris@193:     } else {
Chris@193:         m_fir = false;
Chris@193:         if (params.a.size() != params.b.size()) {
Chris@193:             throw logic_error("Inconsistent numbers of filter coefficients");
Chris@193:         }
Chris@193:     }
Chris@193:     
Chris@193:     m_sz = int(params.b.size());
Chris@193:     m_order = m_sz - 1;
cannam@0: 
Chris@193:     m_a = params.a;
Chris@193:     m_b = params.b;
Chris@193:     
Chris@193:     // We keep some empty space at the start of the buffer, and
Chris@193:     // encroach gradually into it as we add individual sample
Chris@193:     // calculations at the start. Then when we run out of space, we
Chris@193:     // move the buffer back to the end and begin again. This is
Chris@193:     // significantly faster than moving the whole buffer along in
Chris@193:     // 1-sample steps every time.
Chris@193: 
Chris@193:     m_offmax = 20;
Chris@193:     m_offa = m_offmax;
Chris@193:     m_offb = m_offmax;
Chris@193: 
Chris@193:     if (!m_fir) {
Chris@193:         m_bufa.resize(m_order + m_offmax);
Chris@193:     }
Chris@193: 
Chris@193:     m_bufb.resize(m_sz + m_offmax);
cannam@0: }
cannam@0: 
cannam@0: Filter::~Filter()
cannam@0: {
cannam@0: }
cannam@0: 
Chris@193: void
Chris@193: Filter::reset()
cannam@0: {
Chris@193:     m_offb = m_offmax;
Chris@193:     m_offa = m_offmax;
cannam@0: 
Chris@193:     if (!m_fir) {
Chris@193:         m_bufa.assign(m_bufa.size(), 0.0);
Chris@193:     }
cannam@0: 
Chris@193:     m_bufb.assign(m_bufb.size(), 0.0);
cannam@0: }
cannam@0: 
Chris@193: void
Chris@193: Filter::process(const double *const __restrict__ in,
Chris@193: 		double *const __restrict__ out,
Chris@193: 		const int n)
cannam@0: {
Chris@193:     for (int s = 0; s < n; ++s) {
Chris@193: 
Chris@193:         if (m_offb > 0) --m_offb;
Chris@193:         else {
Chris@193:             for (int i = m_sz - 2; i >= 0; --i) {
Chris@193:                 m_bufb[i + m_offmax + 1] = m_bufb[i];
Chris@193:             }
Chris@193:             m_offb = m_offmax;
Chris@193:         }
Chris@193:         m_bufb[m_offb] = in[s];
Chris@193: 
Chris@193:         double b_sum = 0.0;
Chris@193:         for (int i = 0; i < m_sz; ++i) {
Chris@193:             b_sum += m_b[i] * m_bufb[i + m_offb];
Chris@193:         }
Chris@193: 
Chris@193:         double outval;
Chris@193: 
Chris@193:         if (m_fir) {
Chris@193: 
Chris@193:             outval = b_sum;
Chris@193: 
Chris@193:         } else {
Chris@193: 
Chris@193:             double a_sum = 0.0;
Chris@193:             for (int i = 0; i < m_order; ++i) {
Chris@193:                 a_sum += m_a[i + 1] * m_bufa[i + m_offa];
Chris@193:             }
Chris@193: 
Chris@193:             outval = b_sum - a_sum;
Chris@193: 
Chris@193:             if (m_offa > 0) --m_offa;
Chris@193:             else {
Chris@193:                 for (int i = m_order - 2; i >= 0; --i) {
Chris@193:                     m_bufa[i + m_offmax + 1] = m_bufa[i];
Chris@193:                 }
Chris@193:                 m_offa = m_offmax;
Chris@193:             }
Chris@193:             m_bufa[m_offa] = outval;
Chris@193:         }
Chris@193:         
Chris@193: 	out[s] = outval;
Chris@193:     }
cannam@0: }
cannam@0: