Chris@137: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@137: 
Chris@137: #include "Resampler.h"
Chris@137: 
Chris@137: #include "qm-dsp/maths/MathUtilities.h"
Chris@137: #include "qm-dsp/base/KaiserWindow.h"
Chris@137: #include "qm-dsp/base/SincWindow.h"
Chris@137: 
Chris@137: #include <iostream>
Chris@138: #include <vector>
Chris@138: 
Chris@138: using std::vector;
Chris@137: 
Chris@141: //#define DEBUG_RESAMPLER 1
Chris@141: 
Chris@137: Resampler::Resampler(int sourceRate, int targetRate) :
Chris@137:     m_sourceRate(sourceRate),
Chris@137:     m_targetRate(targetRate)
Chris@137: {
Chris@137:     initialise();
Chris@137: }
Chris@137: 
Chris@137: Resampler::~Resampler()
Chris@137: {
Chris@137:     delete[] m_phaseData;
Chris@137: }
Chris@137: 
Chris@137: void
Chris@137: Resampler::initialise()
Chris@137: {
Chris@137:     int higher = std::max(m_sourceRate, m_targetRate);
Chris@137:     int lower = std::min(m_sourceRate, m_targetRate);
Chris@137: 
Chris@137:     m_gcd = MathUtilities::gcd(lower, higher);
Chris@137: 
Chris@137:     int peakToPole = higher / m_gcd;
Chris@137: 
Chris@137:     KaiserWindow::Parameters params =
Chris@137: 	KaiserWindow::parametersForBandwidth(100, 0.02, peakToPole);
Chris@137: 
Chris@137:     params.length =
Chris@137: 	(params.length % 2 == 0 ? params.length + 1 : params.length);
Chris@137:     
Chris@137:     m_filterLength = params.length;
Chris@137:     
Chris@143:     std::cerr << "making filter... ";
Chris@137:     KaiserWindow kw(params);
Chris@137:     SincWindow sw(m_filterLength, peakToPole * 2);
Chris@143:     std::cerr << "done" << std::endl;
Chris@137: 
Chris@137:     double *filter = new double[m_filterLength];
Chris@137:     for (int i = 0; i < m_filterLength; ++i) filter[i] = 1.0;
Chris@137:     sw.cut(filter);
Chris@137:     kw.cut(filter);
Chris@137: 
Chris@137:     int inputSpacing = m_targetRate / m_gcd;
Chris@137:     int outputSpacing = m_sourceRate / m_gcd;
Chris@137: 
Chris@141: #ifdef DEBUG_RESAMPLER
Chris@141:     std::cerr << "resample " << m_sourceRate << " -> " << m_targetRate
Chris@141: 	      << ": inputSpacing " << inputSpacing << ", outputSpacing "
Chris@141: 	      << outputSpacing << ": filter length " << m_filterLength
Chris@141: 	      << std::endl;
Chris@141: #endif
Chris@137: 
Chris@137:     m_phaseData = new Phase[inputSpacing];
Chris@137: 
Chris@137:     for (int phase = 0; phase < inputSpacing; ++phase) {
Chris@137: 
Chris@137: 	Phase p;
Chris@137: 
Chris@137: 	p.nextPhase = phase - outputSpacing;
Chris@137: 	while (p.nextPhase < 0) p.nextPhase += inputSpacing;
Chris@137: 	p.nextPhase %= inputSpacing;
Chris@137: 	
Chris@141: 	p.drop = int(ceil(std::max(0.0, double(outputSpacing - phase))
Chris@141: 			  / inputSpacing));
Chris@137: 
Chris@141: 	int filtZipLength = int(ceil(double(m_filterLength - phase)
Chris@141: 				     / inputSpacing));
Chris@137: 	for (int i = 0; i < filtZipLength; ++i) {
Chris@137: 	    p.filter.push_back(filter[i * inputSpacing + phase]);
Chris@137: 	}
Chris@137: 
Chris@137: 	m_phaseData[phase] = p;
Chris@137:     }
Chris@137: 
Chris@141: #ifdef DEBUG_RESAMPLER
Chris@141:     for (int phase = 0; phase < inputSpacing; ++phase) {
Chris@141: 	std::cerr << "filter for phase " << phase << " of " << inputSpacing << " (with length " << m_phaseData[phase].filter.size() << "):";
Chris@141: 	for (int i = 0; i < m_phaseData[phase].filter.size(); ++i) {
Chris@141: 	    if (i % 4 == 0) {
Chris@141: 		std::cerr << std::endl << i << ": ";
Chris@141: 	    }
Chris@141: 	    float v = m_phaseData[phase].filter[i];
Chris@141: 	    if (v == 1) {
Chris@141: 		std::cerr << " *** " << v << " ***  ";
Chris@141: 	    } else {
Chris@141: 		std::cerr << v << " ";
Chris@141: 	    }
Chris@141: 	}
Chris@141: 	std::cerr << std::endl;
Chris@141:     }
Chris@141: #endif
Chris@141: 
Chris@137:     delete[] filter;
Chris@137: 
Chris@137:     // The May implementation of this uses a pull model -- we ask the
Chris@137:     // resampler for a certain number of output samples, and it asks
Chris@137:     // its source stream for as many as it needs to calculate
Chris@137:     // those. This means (among other things) that the source stream
Chris@137:     // can be asked for enough samples up-front to fill the buffer
Chris@137:     // before the first output sample is generated.
Chris@137:     // 
Chris@137:     // In this implementation we're using a push model in which a
Chris@137:     // certain number of source samples is provided and we're asked
Chris@137:     // for as many output samples as that makes available. But we
Chris@137:     // can't return any samples from the beginning until half the
Chris@137:     // filter length has been provided as input. This means we must
Chris@137:     // either return a very variable number of samples (none at all
Chris@137:     // until the filter fills, then half the filter length at once) or
Chris@137:     // else have a lengthy declared latency on the output. We do the
Chris@137:     // latter. (What do other implementations do?)
Chris@137: 
Chris@141:     m_phase = (m_filterLength/2) % inputSpacing;
Chris@141:     
Chris@141:     m_buffer = vector<double>(m_phaseData[0].filter.size(), 0);
Chris@141: 
Chris@141:     m_latency =
Chris@141: 	((m_buffer.size() * inputSpacing) - (m_filterLength/2)) / outputSpacing
Chris@141: 	+ m_phase;
Chris@141: 
Chris@141: #ifdef DEBUG_RESAMPLER
Chris@141:     std::cerr << "initial phase " << m_phase << " (as " << (m_filterLength/2) << " % " << inputSpacing << ")"
Chris@141: 	      << ", latency " << m_latency << std::endl;
Chris@141: #endif
Chris@137: }
Chris@137: 
Chris@137: double
Chris@141: Resampler::reconstructOne()
Chris@137: {
Chris@137:     Phase &pd = m_phaseData[m_phase];
Chris@141:     double v = 0.0;
Chris@137:     int n = pd.filter.size();
Chris@143:     const double *buf = m_buffer.data();
Chris@143:     const double *filt = pd.filter.data();
Chris@137:     for (int i = 0; i < n; ++i) {
Chris@143: 	v += buf[i] * filt[i]; //!!! gcc can't vectorize: why?
Chris@137:     }
Chris@139:     m_buffer = vector<double>(m_buffer.begin() + pd.drop, m_buffer.end());
Chris@141:     m_phase = pd.nextPhase;
Chris@137:     return v;
Chris@137: }
Chris@137: 
Chris@137: int
Chris@141: Resampler::process(const double *src, double *dst, int n)
Chris@137: {
Chris@141:     for (int i = 0; i < n; ++i) {
Chris@141: 	m_buffer.push_back(src[i]);
Chris@137:     }
Chris@137: 
Chris@141:     int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
Chris@141:     int outidx = 0;
Chris@139: 
Chris@141: #ifdef DEBUG_RESAMPLER
Chris@141:     std::cerr << "process: buf siz " << m_buffer.size() << " filt siz for phase " << m_phase << " " << m_phaseData[m_phase].filter.size() << std::endl;
Chris@141: #endif
Chris@141: 
Chris@142:     double scaleFactor = 1.0;
Chris@142:     if (m_targetRate < m_sourceRate) {
Chris@142: 	scaleFactor = double(m_targetRate) / double(m_sourceRate);
Chris@142:     }
Chris@142: 
Chris@143:     std::cerr << "maxout = " << maxout << std::endl;
Chris@143: 
Chris@141:     while (outidx < maxout &&
Chris@141: 	   m_buffer.size() >= m_phaseData[m_phase].filter.size()) {
Chris@142: 	dst[outidx] = scaleFactor * reconstructOne();
Chris@141: 	outidx++;
Chris@139:     }
Chris@141:     
Chris@141:     return outidx;
Chris@137: }
Chris@141:     
Chris@138: std::vector<double>
Chris@138: Resampler::resample(int sourceRate, int targetRate, const double *data, int n)
Chris@138: {
Chris@138:     Resampler r(sourceRate, targetRate);
Chris@138: 
Chris@138:     int latency = r.getLatency();
Chris@138: 
Chris@143:     // latency is the output latency. We need to provide enough
Chris@143:     // padding input samples at the end of input to guarantee at
Chris@143:     // *least* the latency's worth of output samples. that is,
Chris@143: 
Chris@143:     int inputPad = int(ceil(double(latency * sourceRate) / targetRate));
Chris@143: 
Chris@143:     std::cerr << "latency = " << latency << ", inputPad = " << inputPad << std::endl;
Chris@143: 
Chris@143:     // that means we are providing this much input in total:
Chris@143: 
Chris@143:     int n1 = n + inputPad;
Chris@143: 
Chris@143:     // and obtaining this much output in total:
Chris@143: 
Chris@143:     int m1 = int(ceil(double(n1 * targetRate) / sourceRate));
Chris@143: 
Chris@143:     // in order to return this much output to the user:
Chris@143: 
Chris@141:     int m = int(ceil(double(n * targetRate) / sourceRate));
Chris@143:     
Chris@143:     std::cerr << "n = " << n << ", sourceRate = " << sourceRate << ", targetRate = " << targetRate << ", m = " << m << ", latency = " << latency << ", m1 = " << m1 << ", n1 = " << n1 << ", n1 - n = " << n1 - n << std::endl;
Chris@138: 
Chris@138:     vector<double> pad(n1 - n, 0.0);
Chris@143:     vector<double> out(m1 + 1, 0.0);
Chris@138: 
Chris@138:     int got = r.process(data, out.data(), n);
Chris@138:     got += r.process(pad.data(), out.data() + got, pad.size());
Chris@138: 
Chris@141: #ifdef DEBUG_RESAMPLER
Chris@141:     std::cerr << "resample: " << n << " in, " << got << " out" << std::endl;
Chris@141:     for (int i = 0; i < got; ++i) {
Chris@141: 	if (i % 5 == 0) std::cout << std::endl << i << "... ";
Chris@141: 	std::cout << (float) out[i] << " ";
Chris@141:     }
Chris@141:     std::cout << std::endl;
Chris@141: #endif
Chris@141: 
Chris@143:     int toReturn = got - latency;
Chris@143:     if (toReturn > m) toReturn = m;
Chris@143: 
Chris@143:     return vector<double>(out.begin() + latency, 
Chris@143: 			  out.begin() + latency + toReturn);
Chris@138: }
Chris@138: