c@119: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
c@119: /*
c@119:     Constant-Q library
c@119:     Copyright (c) 2013-2014 Queen Mary, University of London
c@119: 
c@119:     Permission is hereby granted, free of charge, to any person
c@119:     obtaining a copy of this software and associated documentation
c@119:     files (the "Software"), to deal in the Software without
c@119:     restriction, including without limitation the rights to use, copy,
c@119:     modify, merge, publish, distribute, sublicense, and/or sell copies
c@119:     of the Software, and to permit persons to whom the Software is
c@119:     furnished to do so, subject to the following conditions:
c@119: 
c@119:     The above copyright notice and this permission notice shall be
c@119:     included in all copies or substantial portions of the Software.
c@119: 
c@119:     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
c@119:     EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
c@119:     MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
c@119:     NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
c@119:     CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
c@119:     CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
c@119:     WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
c@119: 
c@119:     Except as contained in this notice, the names of the Centre for
c@119:     Digital Music; Queen Mary, University of London; and Chris Cannam
c@119:     shall not be used in advertising or otherwise to promote the sale,
c@119:     use or other dealings in this Software without prior written
c@119:     authorization.
c@119: */
c@119: 
c@119: #include "Resampler.h"
c@119: 
c@122: #include "MathUtilities.h"
c@122: #include "KaiserWindow.h"
c@122: #include "SincWindow.h"
c@119: 
c@119: #include <iostream>
c@119: #include <vector>
c@119: #include <map>
c@119: #include <cassert>
c@184: #include <algorithm>
c@119: 
c@119: using std::vector;
c@119: using std::map;
c@119: using std::cerr;
c@119: using std::endl;
c@119: 
c@119: //#define DEBUG_RESAMPLER 1
c@119: //#define DEBUG_RESAMPLER_VERBOSE 1
c@119: 
c@119: Resampler::Resampler(int sourceRate, int targetRate) :
c@119:     m_sourceRate(sourceRate),
c@119:     m_targetRate(targetRate)
c@119: {
c@183: #ifdef DEBUG_RESAMPLER
c@183:     cerr << "Resampler::Resampler(" <<  sourceRate << "," << targetRate << ")" << endl;
c@183: #endif
c@119:     initialise(100, 0.02);
c@119: }
c@119: 
c@119: Resampler::Resampler(int sourceRate, int targetRate, 
c@119:                      double snr, double bandwidth) :
c@119:     m_sourceRate(sourceRate),
c@119:     m_targetRate(targetRate)
c@119: {
c@119:     initialise(snr, bandwidth);
c@119: }
c@119: 
c@119: Resampler::~Resampler()
c@119: {
c@119:     delete[] m_phaseData;
c@119: }
c@119: 
c@119: void
c@119: Resampler::initialise(double snr, double bandwidth)
c@119: {
c@119:     int higher = std::max(m_sourceRate, m_targetRate);
c@119:     int lower = std::min(m_sourceRate, m_targetRate);
c@119: 
c@119:     m_gcd = MathUtilities::gcd(lower, higher);
c@119:     m_peakToPole = higher / m_gcd;
c@119: 
c@119:     if (m_targetRate < m_sourceRate) {
c@119:         // antialiasing filter, should be slightly below nyquist
c@119:         m_peakToPole = m_peakToPole / (1.0 - bandwidth/2.0);
c@119:     }
c@119: 
c@119:     KaiserWindow::Parameters params =
c@119: 	KaiserWindow::parametersForBandwidth(snr, bandwidth, higher / m_gcd);
c@119: 
c@119:     params.length =
c@119: 	(params.length % 2 == 0 ? params.length + 1 : params.length);
c@119:     
c@119:     params.length =
c@119:         (params.length > 200001 ? 200001 : params.length);
c@119: 
c@119:     m_filterLength = params.length;
c@119: 
c@119:     vector<double> filter;
c@122: 
c@164:     KaiserWindow kw(params);
c@164:     SincWindow sw(m_filterLength, m_peakToPole * 2);
c@119: 
c@164:     filter = vector<double>(m_filterLength, 0.0);
c@164:     for (int i = 0; i < m_filterLength; ++i) filter[i] = 1.0;
c@164:     sw.cut(filter.data());
c@164:     kw.cut(filter.data());
c@122:     
c@119:     int inputSpacing = m_targetRate / m_gcd;
c@119:     int outputSpacing = m_sourceRate / m_gcd;
c@119: 
c@119: #ifdef DEBUG_RESAMPLER
c@119:     cerr << "resample " << m_sourceRate << " -> " << m_targetRate
c@119:          << ": inputSpacing " << inputSpacing << ", outputSpacing "
c@119:          << outputSpacing << ": filter length " << m_filterLength
c@119:          << endl;
c@119: #endif
c@119: 
c@119:     // Now we have a filter of (odd) length flen in which the lower
c@119:     // sample rate corresponds to every n'th point and the higher rate
c@119:     // to every m'th where n and m are higher and lower rates divided
c@119:     // by their gcd respectively. So if x coordinates are on the same
c@119:     // scale as our filter resolution, then source sample i is at i *
c@119:     // (targetRate / gcd) and target sample j is at j * (sourceRate /
c@119:     // gcd).
c@119: 
c@119:     // To reconstruct a single target sample, we want a buffer (real
c@119:     // or virtual) of flen values formed of source samples spaced at
c@119:     // intervals of (targetRate / gcd), in our example case 3.  This
c@119:     // is initially formed with the first sample at the filter peak.
c@119:     //
c@119:     // 0  0  0  0  a  0  0  b  0
c@119:     //
c@119:     // and of course we have our filter
c@119:     //
c@119:     // f1 f2 f3 f4 f5 f6 f7 f8 f9
c@119:     //
c@119:     // We take the sum of products of non-zero values from this buffer
c@119:     // with corresponding values in the filter
c@119:     //
c@119:     // a * f5 + b * f8
c@119:     //
c@119:     // Then we drop (sourceRate / gcd) values, in our example case 4,
c@119:     // from the start of the buffer and fill until it has flen values
c@119:     // again
c@119:     //
c@119:     // a  0  0  b  0  0  c  0  0
c@119:     //
c@119:     // repeat to reconstruct the next target sample
c@119:     //
c@119:     // a * f1 + b * f4 + c * f7
c@119:     //
c@119:     // and so on.
c@119:     //
c@119:     // Above I said the buffer could be "real or virtual" -- ours is
c@119:     // virtual. We don't actually store all the zero spacing values,
c@119:     // except for padding at the start; normally we store only the
c@119:     // values that actually came from the source stream, along with a
c@119:     // phase value that tells us how many virtual zeroes there are at
c@119:     // the start of the virtual buffer.  So the two examples above are
c@119:     //
c@119:     // 0 a b  [ with phase 1 ]
c@119:     // a b c  [ with phase 0 ]
c@119:     //
c@119:     // Having thus broken down the buffer so that only the elements we
c@119:     // need to multiply are present, we can also unzip the filter into
c@119:     // every-nth-element subsets at each phase, allowing us to do the
c@119:     // filter multiplication as a simply vector multiply. That is, rather
c@119:     // than store
c@119:     //
c@119:     // f1 f2 f3 f4 f5 f6 f7 f8 f9
c@119:     // 
c@119:     // we store separately
c@119:     //
c@119:     // f1 f4 f7
c@119:     // f2 f5 f8
c@119:     // f3 f6 f9
c@119:     //
c@119:     // Each time we complete a multiply-and-sum, we need to work out
c@119:     // how many (real) samples to drop from the start of our buffer,
c@119:     // and how many to add at the end of it for the next multiply.  We
c@119:     // know we want to drop enough real samples to move along by one
c@119:     // computed output sample, which is our outputSpacing number of
c@119:     // virtual buffer samples. Depending on the relationship between
c@119:     // input and output spacings, this may mean dropping several real
c@119:     // samples, one real sample, or none at all (and simply moving to
c@119:     // a different "phase").
c@119: 
c@119:     m_phaseData = new Phase[inputSpacing];
c@119: 
c@119:     for (int phase = 0; phase < inputSpacing; ++phase) {
c@119: 
c@119: 	Phase p;
c@119: 
c@119: 	p.nextPhase = phase - outputSpacing;
c@119: 	while (p.nextPhase < 0) p.nextPhase += inputSpacing;
c@119: 	p.nextPhase %= inputSpacing;
c@119: 	
c@119: 	p.drop = int(ceil(std::max(0.0, double(outputSpacing - phase))
c@119: 			  / inputSpacing));
c@119: 
c@119: 	int filtZipLength = int(ceil(double(m_filterLength - phase)
c@119: 				     / inputSpacing));
c@119: 
c@119: 	for (int i = 0; i < filtZipLength; ++i) {
c@119: 	    p.filter.push_back(filter[i * inputSpacing + phase]);
c@119: 	}
c@119: 
c@119: 	m_phaseData[phase] = p;
c@119:     }
c@119: 
c@119: #ifdef DEBUG_RESAMPLER
c@119:     int cp = 0;
c@119:     int totDrop = 0;
c@119:     for (int i = 0; i < inputSpacing; ++i) {
c@119:         cerr << "phase = " << cp << ", drop = " << m_phaseData[cp].drop
c@119:              << ", filter length = " << m_phaseData[cp].filter.size()
c@119:              << ", next phase = " << m_phaseData[cp].nextPhase << endl;
c@119:         totDrop += m_phaseData[cp].drop;
c@119:         cp = m_phaseData[cp].nextPhase;
c@119:     }
c@119:     cerr << "total drop = " << totDrop << endl;
c@119: #endif
c@119: 
c@119:     // The May implementation of this uses a pull model -- we ask the
c@119:     // resampler for a certain number of output samples, and it asks
c@119:     // its source stream for as many as it needs to calculate
c@119:     // those. This means (among other things) that the source stream
c@119:     // can be asked for enough samples up-front to fill the buffer
c@119:     // before the first output sample is generated.
c@119:     // 
c@119:     // In this implementation we're using a push model in which a
c@119:     // certain number of source samples is provided and we're asked
c@119:     // for as many output samples as that makes available. But we
c@119:     // can't return any samples from the beginning until half the
c@119:     // filter length has been provided as input. This means we must
c@119:     // either return a very variable number of samples (none at all
c@119:     // until the filter fills, then half the filter length at once) or
c@119:     // else have a lengthy declared latency on the output. We do the
c@119:     // latter. (What do other implementations do?)
c@119:     //
c@119:     // We want to make sure the first "real" sample will eventually be
c@119:     // aligned with the centre sample in the filter (it's tidier, and
c@119:     // easier to do diagnostic calculations that way). So we need to
c@119:     // pick the initial phase and buffer fill accordingly.
c@119:     // 
c@119:     // Example: if the inputSpacing is 2, outputSpacing is 3, and
c@119:     // filter length is 7,
c@119:     // 
c@119:     //    x     x     x     x     a     b     c ... input samples
c@119:     // 0  1  2  3  4  5  6  7  8  9 10 11 12 13 ... 
c@119:     //          i        j        k        l    ... output samples
c@119:     // [--------|--------] <- filter with centre mark
c@119:     //
c@119:     // Let h be the index of the centre mark, here 3 (generally
c@119:     // int(filterLength/2) for odd-length filters).
c@119:     //
c@119:     // The smallest n such that h + n * outputSpacing > filterLength
c@119:     // is 2 (that is, ceil((filterLength - h) / outputSpacing)), and
c@119:     // (h + 2 * outputSpacing) % inputSpacing == 1, so the initial
c@119:     // phase is 1.
c@119:     //
c@119:     // To achieve our n, we need to pre-fill the "virtual" buffer with
c@119:     // 4 zero samples: the x's above. This is int((h + n *
c@119:     // outputSpacing) / inputSpacing). It's the phase that makes this
c@119:     // buffer get dealt with in such a way as to give us an effective
c@119:     // index for sample a of 9 rather than 8 or 10 or whatever.
c@119:     //
c@119:     // This gives us output latency of 2 (== n), i.e. output samples i
c@119:     // and j will appear before the one in which input sample a is at
c@119:     // the centre of the filter.
c@119: 
c@119:     int h = int(m_filterLength / 2);
c@119:     int n = ceil(double(m_filterLength - h) / outputSpacing);
c@119:     
c@119:     m_phase = (h + n * outputSpacing) % inputSpacing;
c@119: 
c@119:     int fill = (h + n * outputSpacing) / inputSpacing;
c@119:     
c@119:     m_latency = n;
c@119: 
c@119:     m_buffer = vector<double>(fill, 0);
c@119:     m_bufferOrigin = 0;
c@119: 
c@119: #ifdef DEBUG_RESAMPLER
c@119:     cerr << "initial phase " << m_phase << " (as " << (m_filterLength/2) << " % " << inputSpacing << ")"
c@119: 	      << ", latency " << m_latency << endl;
c@119: #endif
c@119: }
c@119: 
c@119: double
c@119: Resampler::reconstructOne()
c@119: {
c@119:     Phase &pd = m_phaseData[m_phase];
c@119:     double v = 0.0;
c@119:     int n = pd.filter.size();
c@119: 
c@183:     if (n + m_bufferOrigin > (int)m_buffer.size()) {
c@183:         cerr << "ERROR: n + m_bufferOrigin > m_buffer.size() [" << n << " + "
c@183:              << m_bufferOrigin << " > " << m_buffer.size() << "]" << endl;
c@183:         throw std::logic_error("n + m_bufferOrigin > m_buffer.size()");
c@183:     }
c@119: 
c@164: #if defined(__MSVC__)
c@164: #define R__ __restrict
c@164: #elif defined(__GNUC__)
c@164: #define R__ __restrict__
c@164: #else
c@164: #define R__
c@164: #endif
c@164: 
c@164:     const double *const R__ buf(m_buffer.data() + m_bufferOrigin);
c@164:     const double *const R__ filt(pd.filter.data());
c@119: 
c@119:     for (int i = 0; i < n; ++i) {
c@119: 	// NB gcc can only vectorize this with -ffast-math
c@119: 	v += buf[i] * filt[i];
c@119:     }
c@119: 
c@119:     m_bufferOrigin += pd.drop;
c@119:     m_phase = pd.nextPhase;
c@119:     return v;
c@119: }
c@119: 
c@119: int
c@119: Resampler::process(const double *src, double *dst, int n)
c@119: {
c@176:     m_buffer.insert(m_buffer.end(), src, src + n);
c@119: 
c@119:     int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
c@119:     int outidx = 0;
c@119: 
c@119: #ifdef DEBUG_RESAMPLER
c@119:     cerr << "process: buf siz " << m_buffer.size() << " filt siz for phase " << m_phase << " " << m_phaseData[m_phase].filter.size() << endl;
c@119: #endif
c@119: 
c@119:     double scaleFactor = (double(m_targetRate) / m_gcd) / m_peakToPole;
c@119: 
c@119:     while (outidx < maxout &&
c@119: 	   m_buffer.size() >= m_phaseData[m_phase].filter.size() + m_bufferOrigin) {
c@119: 	dst[outidx] = scaleFactor * reconstructOne();
c@119: 	outidx++;
c@119:     }
c@119: 
c@183:     if (m_bufferOrigin > (int)m_buffer.size()) {
c@183:         cerr << "ERROR: m_bufferOrigin > m_buffer.size() [" 
c@183:              << m_bufferOrigin << " > " << m_buffer.size() << "]" << endl;
c@183:         throw std::logic_error("m_bufferOrigin > m_buffer.size()");
c@183:     }
c@183: 
c@119:     m_buffer = vector<double>(m_buffer.begin() + m_bufferOrigin, m_buffer.end());
c@119:     m_bufferOrigin = 0;
c@119:     
c@119:     return outidx;
c@119: }
c@119:     
c@119: vector<double>
c@119: Resampler::process(const double *src, int n)
c@119: {
c@119:     int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
c@119:     vector<double> out(maxout, 0.0);
c@119:     int got = process(src, out.data(), n);
c@119:     assert(got <= maxout);
c@119:     if (got < maxout) out.resize(got);
c@119:     return out;
c@119: }
c@119: 
c@119: vector<double>
c@119: Resampler::resample(int sourceRate, int targetRate, const double *data, int n)
c@119: {
c@119:     Resampler r(sourceRate, targetRate);
c@119: 
c@119:     int latency = r.getLatency();
c@119: 
c@119:     // latency is the output latency. We need to provide enough
c@119:     // padding input samples at the end of input to guarantee at
c@119:     // *least* the latency's worth of output samples. that is,
c@119: 
c@119:     int inputPad = int(ceil((double(latency) * sourceRate) / targetRate));
c@119: 
c@119:     // that means we are providing this much input in total:
c@119: 
c@119:     int n1 = n + inputPad;
c@119: 
c@119:     // and obtaining this much output in total:
c@119: 
c@119:     int m1 = int(ceil((double(n1) * targetRate) / sourceRate));
c@119: 
c@119:     // in order to return this much output to the user:
c@119: 
c@119:     int m = int(ceil((double(n) * targetRate) / sourceRate));
c@119:     
c@119: #ifdef DEBUG_RESAMPLER
c@119:     cerr << "n = " << n << ", sourceRate = " << sourceRate << ", targetRate = " << targetRate << ", m = " << m << ", latency = " << latency << ", inputPad = " << inputPad << ", m1 = " << m1 << ", n1 = " << n1 << ", n1 - n = " << n1 - n << endl;
c@119: #endif
c@119: 
c@119:     vector<double> pad(n1 - n, 0.0);
c@119:     vector<double> out(m1 + 1, 0.0);
c@119: 
c@119:     int gotData = r.process(data, out.data(), n);
c@119:     int gotPad = r.process(pad.data(), out.data() + gotData, pad.size());
c@119:     int got = gotData + gotPad;
c@119:     
c@119: #ifdef DEBUG_RESAMPLER
c@119:     cerr << "resample: " << n << " in, " << pad.size() << " padding, " << got << " out (" << gotData << " data, " << gotPad << " padding, latency = " << latency << ")" << endl;
c@119: #endif
c@119: #ifdef DEBUG_RESAMPLER_VERBOSE
c@119:     int printN = 50;
c@119:     cerr << "first " << printN << " in:" << endl;
c@119:     for (int i = 0; i < printN && i < n; ++i) {
c@119: 	if (i % 5 == 0) cerr << endl << i << "... ";
c@119:         cerr << data[i] << " ";
c@119:     }
c@119:     cerr << endl;
c@119: #endif
c@119: 
c@119:     int toReturn = got - latency;
c@119:     if (toReturn > m) toReturn = m;
c@119: 
c@119:     vector<double> sliced(out.begin() + latency, 
c@119: 			  out.begin() + latency + toReturn);
c@119: 
c@119: #ifdef DEBUG_RESAMPLER_VERBOSE
c@119:     cerr << "first " << printN << " out (after latency compensation), length " << sliced.size() << ":";
c@119:     for (int i = 0; i < printN && i < sliced.size(); ++i) {
c@119: 	if (i % 5 == 0) cerr << endl << i << "... ";
c@119: 	cerr << sliced[i] << " ";
c@119:     }
c@119:     cerr << endl;
c@119: #endif
c@119: 
c@119:     return sliced;
c@119: }
c@119: