Chris@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@0: 
Chris@0: #include "Resampler.h"
Chris@0: 
Chris@0: #include "qm-dsp/maths/MathUtilities.h"
Chris@0: #include "qm-dsp/base/KaiserWindow.h"
Chris@0: #include "qm-dsp/base/SincWindow.h"
Chris@0: 
Chris@0: #include <iostream>
Chris@0: 
Chris@0: Resampler::Resampler(int sourceRate, int targetRate) :
Chris@0:     m_sourceRate(sourceRate),
Chris@0:     m_targetRate(targetRate)
Chris@0: {
Chris@0:     initialise();
Chris@0: }
Chris@0: 
Chris@0: Resampler::~Resampler()
Chris@0: {
Chris@0:     delete[] m_buffer;
Chris@0:     delete[] m_phaseData;
Chris@0: }
Chris@0: 
Chris@0: void
Chris@0: Resampler::initialise()
Chris@0: {
Chris@0:     int higher = std::max(m_sourceRate, m_targetRate);
Chris@0:     int lower = std::min(m_sourceRate, m_targetRate);
Chris@0: 
Chris@0:     m_gcd = MathUtilities::gcd(lower, higher);
Chris@0: 
Chris@0:     int peakToPole = higher / m_gcd;
Chris@0: 
Chris@0:     KaiserWindow::Parameters params =
Chris@0: 	KaiserWindow::parametersForBandwidth(100, 0.02, peakToPole);
Chris@0: 
Chris@0:     params.length =
Chris@0: 	(params.length % 2 == 0 ? params.length + 1 : params.length);
Chris@0:     
Chris@0:     m_filterLength = params.length;
Chris@0:     
Chris@0:     KaiserWindow kw(params);
Chris@0:     SincWindow sw(m_filterLength, peakToPole * 2);
Chris@0: 
Chris@0:     double *filter = new double[m_filterLength];
Chris@0:     for (int i = 0; i < m_filterLength; ++i) filter[i] = 1.0;
Chris@0:     sw.cut(filter);
Chris@0:     kw.cut(filter);
Chris@0: 
Chris@0:     int inputSpacing = m_targetRate / m_gcd;
Chris@0:     int outputSpacing = m_sourceRate / m_gcd;
Chris@0: 
Chris@0:     m_latency = int((m_filterLength / 2) / outputSpacing);
Chris@0: 
Chris@0:     m_bufferLength = 0;
Chris@0: 
Chris@0:     m_phaseData = new Phase[inputSpacing];
Chris@0: 
Chris@0:     for (int phase = 0; phase < inputSpacing; ++phase) {
Chris@0: 
Chris@0: 	Phase p;
Chris@0: 
Chris@0: 	p.nextPhase = phase - outputSpacing;
Chris@0: 	while (p.nextPhase < 0) p.nextPhase += inputSpacing;
Chris@0: 	p.nextPhase %= inputSpacing;
Chris@0: 	
Chris@0: 	p.drop = int(ceil(std::max(0, outputSpacing - phase) / inputSpacing));
Chris@0: 	p.take = int((outputSpacing +
Chris@0: 		      ((m_filterLength - 1 - phase) % inputSpacing))
Chris@0: 		     / outputSpacing);
Chris@0: 
Chris@0: 	int filtZipLength = int(ceil((m_filterLength - phase) / inputSpacing));
Chris@0: 	if (filtZipLength > m_bufferLength) {
Chris@0: 	    m_bufferLength = filtZipLength;
Chris@0: 	}
Chris@0: 	
Chris@0: 	for (int i = 0; i < filtZipLength; ++i) {
Chris@0: 	    p.filter.push_back(filter[i * inputSpacing + phase]);
Chris@0: 	}
Chris@0: 
Chris@0: 	m_phaseData[phase] = p;
Chris@0:     }
Chris@0: 
Chris@0:     delete[] filter;
Chris@0: 
Chris@0:     // The May implementation of this uses a pull model -- we ask the
Chris@0:     // resampler for a certain number of output samples, and it asks
Chris@0:     // its source stream for as many as it needs to calculate
Chris@0:     // those. This means (among other things) that the source stream
Chris@0:     // can be asked for enough samples up-front to fill the buffer
Chris@0:     // before the first output sample is generated.
Chris@0:     // 
Chris@0:     // In this implementation we're using a push model in which a
Chris@0:     // certain number of source samples is provided and we're asked
Chris@0:     // for as many output samples as that makes available. But we
Chris@0:     // can't return any samples from the beginning until half the
Chris@0:     // filter length has been provided as input. This means we must
Chris@0:     // either return a very variable number of samples (none at all
Chris@0:     // until the filter fills, then half the filter length at once) or
Chris@0:     // else have a lengthy declared latency on the output. We do the
Chris@0:     // latter. (What do other implementations do?)
Chris@0: 
Chris@0:     m_phase = m_filterLength % inputSpacing;
Chris@0:     m_buffer = new double[m_bufferLength];
Chris@0:     for (int i = 0; i < m_bufferLength; ++i) m_buffer[i] = 0.0;
Chris@0: }
Chris@0: 
Chris@0: double
Chris@0: Resampler::reconstructOne(const double **srcptr)
Chris@0: {
Chris@0:     Phase &pd = m_phaseData[m_phase];
Chris@0:     double *filt = pd.filter.data();
Chris@0:     int n = pd.filter.size();
Chris@0:     double v = 0.0;
Chris@0:     for (int i = 0; i < n; ++i) {
Chris@0: 	v += m_buffer[i] * filt[i];
Chris@0:     }
Chris@0:     for (int i = pd.drop; i < n; ++i) {
Chris@0: 	m_buffer[i - pd.drop] = m_buffer[i];
Chris@0:     }
Chris@0:     for (int i = 0; i < pd.take; ++i) {
Chris@0: 	m_buffer[n - pd.drop + i] = **srcptr;
Chris@0: 	++ *srcptr;
Chris@0:     }
Chris@0:     m_phase = pd.nextPhase;
Chris@0:     return v;
Chris@0: }
Chris@0: 
Chris@0: int
Chris@0: Resampler::process(const double *src, double *dst, int n)
Chris@0: {
Chris@0:     int m = 0;
Chris@0:     const double *srcptr = src;
Chris@0: 
Chris@0:     while (n > m_phaseData[m_phase].take) {
Chris@0: 	std::cerr << "n = " << n << ", m = " << m << ", take = " << m_phaseData[m_phase].take << std::endl;
Chris@0: 	n -= m_phaseData[m_phase].take;
Chris@0: 	dst[m] = reconstructOne(&srcptr);
Chris@0: 	std::cerr << "n -> " << n << std::endl;
Chris@0: 	++m;
Chris@0:     }
Chris@0: 
Chris@0:     //!!! save any excess 
Chris@0: 
Chris@0:     return m;
Chris@0: }
Chris@0: