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