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: 
Chris@155: #include "DecimatorB.h"
Chris@155: 
Chris@155: #include "maths/MathUtilities.h"
cannam@0: 
cannam@22: #include <iostream>
cannam@22: 
Chris@155: using std::vector;
cannam@0: 
Chris@155: DecimatorB::DecimatorB(int inLength, int decFactor)
cannam@0: {
cannam@0:     m_inputLength = 0;
cannam@0:     m_outputLength = 0;
cannam@0:     m_decFactor = 1;
Chris@155:     m_aaBuffer = 0;
Chris@155:     m_tmpBuffer = 0;
cannam@0: 
Chris@155:     initialise(inLength, decFactor);
cannam@0: }
cannam@0: 
Chris@155: DecimatorB::~DecimatorB()
cannam@0: {
cannam@0:     deInitialise();
cannam@0: }
cannam@0: 
Chris@155: void DecimatorB::initialise(int inLength, int decFactor)
cannam@0: {
cannam@0:     m_inputLength = inLength;
cannam@0:     m_decFactor = decFactor;
cannam@0:     m_outputLength = m_inputLength / m_decFactor;
cannam@0: 
Chris@155:     if (m_decFactor < 2 || !MathUtilities::isPowerOfTwo(m_decFactor)) {
Chris@155:         std::cerr << "ERROR: DecimatorB::initialise: Decimation factor must be a power of 2 and at least 2 (was: " << m_decFactor << ")" << std::endl;
Chris@155:         m_decFactor = 0;
Chris@155:         return;
cannam@0:     }
cannam@0: 
Chris@155:     if (m_inputLength % m_decFactor != 0) {
Chris@155:         std::cerr << "ERROR: DecimatorB::initialise: inLength must be a multiple of decimation factor (was: " << m_inputLength << ", factor is " << m_decFactor << ")" << std::endl;
Chris@155:         m_decFactor = 0;
Chris@155:         return;
Chris@155:     }        
cannam@0: 
Chris@155:     m_aaBuffer = new double[m_inputLength];
Chris@155:     m_tmpBuffer = new double[m_inputLength];
cannam@0: 
Chris@155:     // Order 6 Butterworth lowpass filter
Chris@155:     // Calculated using e.g. MATLAB butter(6, 0.5, 'low')
cannam@0: 
Chris@155:     m_b[0] = 0.029588223638661;
Chris@155:     m_b[1] = 0.177529341831965;
Chris@155:     m_b[2] = 0.443823354579912;
Chris@155:     m_b[3] = 0.591764472773216;
Chris@155:     m_b[4] = 0.443823354579912;
Chris@155:     m_b[5] = 0.177529341831965;
Chris@155:     m_b[6] = 0.029588223638661;
cannam@0: 
Chris@155:     m_a[0] = 1.000000000000000;
Chris@155:     m_a[1] = 0.000000000000000;
Chris@155:     m_a[2] = 0.777695961855673;
Chris@155:     m_a[3] = 0.000000000000000;
Chris@155:     m_a[4] = 0.114199425062434;
Chris@155:     m_a[5] = 0.000000000000000;
Chris@155:     m_a[6] = 0.001750925956183;
cannam@0: 
Chris@155:     for (int factor = m_decFactor; factor > 1; factor /= 2) {
Chris@155:         m_o.push_back(vector<double>(6, 0.0));
cannam@0:     }
cannam@0: }
cannam@55: 
Chris@155: void DecimatorB::deInitialise()
cannam@55: {
Chris@155:     delete [] m_aaBuffer;
Chris@155:     delete [] m_tmpBuffer;
Chris@155: }
cannam@55: 
Chris@155: void DecimatorB::doAntiAlias(const double *src, double *dst, int length,
Chris@155:                              int filteridx)
Chris@155: {
Chris@155:     vector<double> &o = m_o[filteridx];
Chris@155: 
Chris@155:     for (int i = 0; i < length; i++) {
Chris@155: 
Chris@155: 	double input = src[i];
Chris@155: 	double output = input * m_b[0] + o[0];
Chris@155: 
Chris@155: 	o[0] = input * m_b[1] - output * m_a[1] + o[1];
Chris@155: 	o[1] = input * m_b[2] - output * m_a[2] + o[2];
Chris@155: 	o[2] = input * m_b[3] - output * m_a[3] + o[3];
Chris@155: 	o[3] = input * m_b[4] - output * m_a[4] + o[4];
Chris@155: 	o[4] = input * m_b[5] - output * m_a[5] + o[5];
Chris@155: 	o[5] = input * m_b[6] - output * m_a[6];
Chris@155: 
Chris@155: 	dst[i] = output;
cannam@55:     }
cannam@55: }
Chris@155: 
Chris@155: void DecimatorB::doProcess()
Chris@155: {
Chris@155:     int filteridx = 0;
Chris@155:     int factorDone = 1;
Chris@155:     int factorRemaining = m_decFactor;
Chris@155: 
Chris@155:     while (factorDone < m_decFactor) {
Chris@155: 
Chris@155:         doAntiAlias(m_tmpBuffer, m_aaBuffer,
Chris@155:                     m_inputLength / factorDone,
Chris@155:                     filteridx);
Chris@155: 
Chris@155:         filteridx ++;
Chris@155:         factorDone *= 2;
Chris@155: 
Chris@155:         for (int i = 0; i < m_inputLength / factorDone; ++i) {
Chris@155:             m_tmpBuffer[i] = m_aaBuffer[i * 2];
Chris@155:         }
Chris@155:     }
Chris@155: }
Chris@155: 
Chris@155: void DecimatorB::process(const double *src, double *dst)
Chris@155: {
Chris@155:     if (m_decFactor == 0) return;
Chris@155: 
Chris@155:     for (int i = 0; i < m_inputLength; ++i) {
Chris@155:         m_tmpBuffer[i] = src[i];
Chris@155:     }
Chris@155: 
Chris@155:     doProcess();
Chris@155:     
Chris@155:     for (int i = 0; i < m_outputLength; ++i) {
Chris@155:         dst[i] = m_tmpBuffer[i];
Chris@155:     }
Chris@155: }
Chris@155: 
Chris@155: void DecimatorB::process(const float *src, float *dst)
Chris@155: {
Chris@155:     if (m_decFactor == 0) return;
Chris@155: 
Chris@155:     for (int i = 0; i < m_inputLength; ++i) {
Chris@155:         m_tmpBuffer[i] = src[i];
Chris@155:     }
Chris@155: 
Chris@155:     doProcess();
Chris@155:     
Chris@155:     for (int i = 0; i < m_outputLength; ++i) {
Chris@155:         dst[i] = m_tmpBuffer[i];
Chris@155:     }
Chris@155: }