Chris@366: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@366: /*
Chris@366:     Constant-Q library
Chris@366:     Copyright (c) 2013-2014 Queen Mary, University of London
Chris@366: 
Chris@366:     Permission is hereby granted, free of charge, to any person
Chris@366:     obtaining a copy of this software and associated documentation
Chris@366:     files (the "Software"), to deal in the Software without
Chris@366:     restriction, including without limitation the rights to use, copy,
Chris@366:     modify, merge, publish, distribute, sublicense, and/or sell copies
Chris@366:     of the Software, and to permit persons to whom the Software is
Chris@366:     furnished to do so, subject to the following conditions:
Chris@366: 
Chris@366:     The above copyright notice and this permission notice shall be
Chris@366:     included in all copies or substantial portions of the Software.
Chris@366: 
Chris@366:     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
Chris@366:     EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
Chris@366:     MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
Chris@366:     NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
Chris@366:     CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
Chris@366:     CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
Chris@366:     WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Chris@366: 
Chris@366:     Except as contained in this notice, the names of the Centre for
Chris@366:     Digital Music; Queen Mary, University of London; and Chris Cannam
Chris@366:     shall not be used in advertising or otherwise to promote the sale,
Chris@366:     use or other dealings in this Software without prior written
Chris@366:     authorization.
Chris@366: */
Chris@366: 
Chris@366: #include "ConstantQ.h"
Chris@366: 
Chris@366: #include "CQKernel.h"
Chris@366: 
Chris@366: #include "dsp/Resampler.h"
Chris@366: #include "dsp/MathUtilities.h"
Chris@366: #include "dsp/FFT.h"
Chris@366: 
Chris@366: #include <algorithm>
Chris@366: #include <iostream>
Chris@366: #include <stdexcept>
Chris@366: 
Chris@366: #include <cmath>
Chris@366: 
Chris@366: using std::vector;
Chris@366: using std::cerr;
Chris@366: using std::endl;
Chris@366: 
Chris@366: //#define DEBUG_CQ 1
Chris@366: 
Chris@366: ConstantQ::ConstantQ(CQParameters params) :
Chris@366:     m_inparams(params),
Chris@366:     m_sampleRate(params.sampleRate),
Chris@366:     m_maxFrequency(params.maxFrequency),
Chris@366:     m_minFrequency(params.minFrequency),
Chris@366:     m_binsPerOctave(params.binsPerOctave),
Chris@366:     m_kernel(0),
Chris@366:     m_fft(0)
Chris@366: {
Chris@366:     if (m_minFrequency <= 0.0 || m_maxFrequency <= 0.0) {
Chris@366:         throw std::invalid_argument("Frequency extents must be positive");
Chris@366:     }
Chris@366: 
Chris@366:     initialise();
Chris@366: }
Chris@366: 
Chris@366: ConstantQ::~ConstantQ()
Chris@366: {
Chris@366:     delete m_fft;
Chris@366:     for (int i = 0; i < (int)m_decimators.size(); ++i) {
Chris@366:         delete m_decimators[i];
Chris@366:     }
Chris@366:     delete m_kernel;
Chris@366: }
Chris@366: 
Chris@366: double
Chris@366: ConstantQ::getMinFrequency() const
Chris@366: {
Chris@366:     return m_p.minFrequency / pow(2.0, m_octaves - 1);
Chris@366: }
Chris@366: 
Chris@366: double
Chris@366: ConstantQ::getBinFrequency(double bin) const
Chris@366: {
Chris@366:     // our bins are returned in high->low order
Chris@366:     bin = (getBinsPerOctave() * getOctaves()) - bin - 1;
Chris@366:     return getMinFrequency() * pow(2, (bin / getBinsPerOctave()));
Chris@366: }
Chris@366: 
Chris@366: void
Chris@366: ConstantQ::initialise()
Chris@366: {
Chris@366:     m_octaves = int(ceil(log(m_maxFrequency / m_minFrequency) / log(2)));
Chris@366: 
Chris@366:     if (m_octaves < 1) {
Chris@366:         m_kernel = 0; // incidentally causing isValid() to return false
Chris@366:         return;
Chris@366:     }
Chris@366: 
Chris@366:     m_kernel = new CQKernel(m_inparams);
Chris@366:     m_p = m_kernel->getProperties();
Chris@366:     
Chris@366:     if (!m_kernel->isValid()) {
Chris@366:         return;
Chris@366:     }
Chris@366: 
Chris@366:     // Use exact powers of two for resampling rates. They don't have
Chris@366:     // to be related to our actual samplerate: the resampler only
Chris@366:     // cares about the ratio, but it only accepts integer source and
Chris@366:     // target rates, and if we start from the actual samplerate we
Chris@366:     // risk getting non-integer rates for lower octaves
Chris@366: 
Chris@366:     int sourceRate = pow(2, m_octaves);
Chris@366:     vector<int> latencies;
Chris@366: 
Chris@366:     // top octave, no resampling
Chris@366:     latencies.push_back(0);
Chris@366:     m_decimators.push_back(0);
Chris@366: 
Chris@366:     for (int i = 1; i < m_octaves; ++i) {
Chris@366: 
Chris@366:         int factor = pow(2, i);
Chris@366: 
Chris@366:         Resampler *r;
Chris@366: 
Chris@366:         if (m_inparams.decimator == CQParameters::BetterDecimator) {
Chris@366:             r = new Resampler
Chris@366:                 (sourceRate, sourceRate / factor, 50, 0.05);
Chris@366:         } else {
Chris@366:             r = new Resampler
Chris@366:                 (sourceRate, sourceRate / factor, 25, 0.3);
Chris@366:         }                
Chris@366: 
Chris@366: #ifdef DEBUG_CQ
Chris@366:         cerr << "forward: octave " << i << ": resample from " << sourceRate << " to " << sourceRate / factor << endl;
Chris@366: #endif
Chris@366: 
Chris@366:         // We need to adapt the latencies so as to get the first input
Chris@366:         // sample to be aligned, in time, at the decimator output
Chris@366:         // across all octaves.
Chris@366:         // 
Chris@366:         // Our decimator uses a linear phase filter, but being causal
Chris@366:         // it is not zero phase: it has a latency that depends on the
Chris@366:         // decimation factor. Those latencies have been calculated
Chris@366:         // per-octave and are available to us in the latencies
Chris@366:         // array. Left to its own devices, the first input sample will
Chris@366:         // appear at output sample 0 in the highest octave (where no
Chris@366:         // decimation is needed), sample number latencies[1] in the
Chris@366:         // next octave down, latencies[2] in the next one, etc. We get
Chris@366:         // to apply some artificial per-octave latency after the
Chris@366:         // decimator in the processing chain, in order to compensate
Chris@366:         // for the differing latencies associated with different
Chris@366:         // decimation factors. How much should we insert?
Chris@366:         //
Chris@366:         // The outputs of the decimators are at different rates (in
Chris@366:         // terms of the relation between clock time and samples) and
Chris@366:         // we want them aligned in terms of time. So, for example, a
Chris@366:         // latency of 10 samples with a decimation factor of 2 is
Chris@366:         // equivalent to a latency of 20 with no decimation -- they
Chris@366:         // both result in the first output sample happening at the
Chris@366:         // same equivalent time in milliseconds.
Chris@366: 	// 
Chris@366: 	// So here we record the latency added by the decimator, in
Chris@366: 	// terms of the sample rate of the undecimated signal. Then we
Chris@366: 	// use that to compensate in a moment, when we've discovered
Chris@366: 	// what the longest latency across all octaves is.
Chris@366: 
Chris@366:         latencies.push_back(r->getLatency() * factor);
Chris@366:         m_decimators.push_back(r);
Chris@366:     }
Chris@366: 
Chris@366:     m_bigBlockSize = m_p.fftSize * pow(2, m_octaves - 1);
Chris@366: 
Chris@366:     // Now add in the extra padding and compensate for hops that must
Chris@366:     // be dropped in order to align the atom centres across
Chris@366:     // octaves. Again this is a bit trickier because we are doing it
Chris@366:     // at input rather than output and so must work in per-octave
Chris@366:     // sample rates rather than output blocks
Chris@366: 
Chris@366:     int emptyHops = m_p.firstCentre / m_p.atomSpacing;
Chris@366: 
Chris@366:     vector<int> drops;
Chris@366:     for (int i = 0; i < m_octaves; ++i) {
Chris@366: 	int factor = pow(2, i);
Chris@366: 	int dropHops = emptyHops * pow(2, m_octaves - i - 1) - emptyHops;
Chris@366: 	int drop = ((dropHops * m_p.fftHop) * factor) / m_p.atomsPerFrame;
Chris@366: 	drops.push_back(drop);
Chris@366:     }
Chris@366: 
Chris@366:     int maxLatPlusDrop = 0;
Chris@366:     for (int i = 0; i < m_octaves; ++i) {
Chris@366: 	int latPlusDrop = latencies[i] + drops[i];
Chris@366: 	if (latPlusDrop > maxLatPlusDrop) maxLatPlusDrop = latPlusDrop;
Chris@366:     }
Chris@366: 
Chris@366:     int totalLatency = maxLatPlusDrop;
Chris@366: 
Chris@366:     int lat0 = totalLatency - latencies[0] - drops[0];
Chris@366:     totalLatency = ceil(double(lat0 / m_p.fftHop) * m_p.fftHop)
Chris@366: 	+ latencies[0] + drops[0];
Chris@366: 
Chris@366:     // We want (totalLatency - latencies[i]) to be a multiple of 2^i
Chris@366:     // for each octave i, so that we do not end up with fractional
Chris@366:     // octave latencies below. In theory this is hard, in practice if
Chris@366:     // we ensure it for the last octave we should be OK.
Chris@366:     double finalOctLat = latencies[m_octaves-1];
Chris@366:     double finalOctFact = pow(2, m_octaves-1);
Chris@366:     totalLatency =
Chris@366:         int(finalOctLat +
Chris@366:             finalOctFact *
Chris@366:             ceil((totalLatency - finalOctLat) / finalOctFact) + .5);
Chris@366: 
Chris@366: #ifdef DEBUG_CQ
Chris@366:     cerr << "total latency = " << totalLatency << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     // Padding as in the reference (will be introduced with the
Chris@366:     // latency compensation in the loop below)
Chris@366:     m_outputLatency = totalLatency + m_bigBlockSize
Chris@366: 	- m_p.firstCentre * pow(2, m_octaves-1);
Chris@366: 
Chris@366: #ifdef DEBUG_CQ
Chris@366:     cerr << "m_bigBlockSize = " << m_bigBlockSize << ", firstCentre = "
Chris@366: 	 << m_p.firstCentre << ", m_octaves = " << m_octaves
Chris@366:          << ", so m_outputLatency = " << m_outputLatency << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     for (int i = 0; i < m_octaves; ++i) {
Chris@366: 
Chris@366: 	double factor = pow(2, i);
Chris@366: 
Chris@366: 	// Calculate the difference between the total latency applied
Chris@366: 	// across all octaves, and the existing latency due to the
Chris@366: 	// decimator for this octave, and then convert it back into
Chris@366: 	// the sample rate appropriate for the output latency of this
Chris@366: 	// decimator -- including one additional big block of padding
Chris@366: 	// (as in the reference).
Chris@366: 
Chris@366: 	double octaveLatency =
Chris@366: 	    double(totalLatency - latencies[i] - drops[i]
Chris@366: 		   + m_bigBlockSize) / factor;
Chris@366: 
Chris@366: #ifdef DEBUG_CQ
Chris@366:         cerr << "octave " << i << ": resampler latency = " << latencies[i]
Chris@366:              << ", drop " << drops[i] << " (/factor = " << drops[i]/factor
Chris@366:              << "), octaveLatency = " << octaveLatency << " -> "
Chris@366:              << int(round(octaveLatency)) << " (diff * factor = "
Chris@366:              << (octaveLatency - round(octaveLatency)) << " * "
Chris@366:              << factor << " = "
Chris@366:              << (octaveLatency - round(octaveLatency)) * factor << ")" << endl;
Chris@366: 
Chris@366:         cerr << "double(" << totalLatency << " - " 
Chris@366:              << latencies[i] << " - " << drops[i] << " + " 
Chris@366:              << m_bigBlockSize << ") / " << factor << " = " 
Chris@366:              << octaveLatency << endl;
Chris@366: #endif
Chris@366: 
Chris@366:         m_buffers.push_back
Chris@366:             (RealSequence(int(octaveLatency + 0.5), 0.0));
Chris@366:     }
Chris@366: 
Chris@366:     m_fft = new FFTReal(m_p.fftSize);
Chris@366: }
Chris@366: 
Chris@366: ConstantQ::ComplexBlock
Chris@366: ConstantQ::process(const RealSequence &td)
Chris@366: {
Chris@366:     m_buffers[0].insert(m_buffers[0].end(), td.begin(), td.end());
Chris@366: 
Chris@366:     for (int i = 1; i < m_octaves; ++i) {
Chris@366:         RealSequence dec = m_decimators[i]->process(td.data(), td.size());
Chris@366:         m_buffers[i].insert(m_buffers[i].end(), dec.begin(), dec.end());
Chris@366:     }
Chris@366: 
Chris@366:     ComplexBlock out;
Chris@366: 
Chris@366:     while (true) {
Chris@366: 
Chris@366: 	// We could have quite different remaining sample counts in
Chris@366: 	// different octaves, because (apart from the predictable
Chris@366: 	// added counts for decimator output on each block) we also
Chris@366: 	// have variable additional latency per octave
Chris@366: 	bool enough = true;
Chris@366: 	for (int i = 0; i < m_octaves; ++i) {
Chris@366: 	    int required = m_p.fftSize * pow(2, m_octaves - i - 1);
Chris@366: 	    if ((int)m_buffers[i].size() < required) {
Chris@366: 		enough = false;
Chris@366: 	    }
Chris@366: 	}
Chris@366: 	if (!enough) break;
Chris@366: 
Chris@366:         int base = out.size();
Chris@366:         int totalColumns = pow(2, m_octaves - 1) * m_p.atomsPerFrame;
Chris@366:         for (int i = 0; i < totalColumns; ++i) {
Chris@366:             out.push_back(ComplexColumn());
Chris@366:         }
Chris@366: 
Chris@366:         for (int octave = 0; octave < m_octaves; ++octave) {
Chris@366: 
Chris@366:             int blocksThisOctave = pow(2, (m_octaves - octave - 1));
Chris@366: 
Chris@366:             for (int b = 0; b < blocksThisOctave; ++b) {
Chris@366:                 ComplexBlock block = processOctaveBlock(octave);
Chris@366:                 
Chris@366:                 for (int j = 0; j < m_p.atomsPerFrame; ++j) {
Chris@366: 
Chris@366:                     int target = base +
Chris@366: 			    (b * (totalColumns / blocksThisOctave) + 
Chris@366: 			     (j * ((totalColumns / blocksThisOctave) /
Chris@366: 				   m_p.atomsPerFrame)));
Chris@366: 
Chris@366:                     while (int(out[target].size()) < 
Chris@366:                            m_p.binsPerOctave * (octave + 1)) {
Chris@366:                         out[target].push_back(Complex());
Chris@366:                     }
Chris@366:                     
Chris@366:                     for (int i = 0; i < m_p.binsPerOctave; ++i) {
Chris@366:                         out[target][m_p.binsPerOctave * octave + i] = 
Chris@366:                             block[j][m_p.binsPerOctave - i - 1];
Chris@366:                     }
Chris@366:                 }
Chris@366:             }
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     return out;
Chris@366: }
Chris@366: 
Chris@366: ConstantQ::ComplexBlock
Chris@366: ConstantQ::getRemainingOutput()
Chris@366: {
Chris@366:     // Same as padding added at start, though rounded up
Chris@366:     int pad = ceil(double(m_outputLatency) / m_bigBlockSize) * m_bigBlockSize;
Chris@366:     RealSequence zeros(pad, 0.0);
Chris@366:     return process(zeros);
Chris@366: }
Chris@366: 
Chris@366: ConstantQ::ComplexBlock
Chris@366: ConstantQ::processOctaveBlock(int octave)
Chris@366: {
Chris@366:     RealSequence ro(m_p.fftSize, 0.0);
Chris@366:     RealSequence io(m_p.fftSize, 0.0);
Chris@366: 
Chris@366:     m_fft->forward(m_buffers[octave].data(), ro.data(), io.data());
Chris@366: 
Chris@366:     m_buffers[octave] = RealSequence(m_buffers[octave].begin() + m_p.fftHop,
Chris@366:                                      m_buffers[octave].end());
Chris@366: 
Chris@366:     ComplexSequence cv(m_p.fftSize);
Chris@366:     for (int i = 0; i < m_p.fftSize; ++i) {
Chris@366:         cv[i] = Complex(ro[i], io[i]);
Chris@366:     }
Chris@366: 
Chris@366:     ComplexSequence cqrowvec = m_kernel->processForward(cv);
Chris@366: 
Chris@366:     // Reform into a column matrix
Chris@366:     ComplexBlock cqblock;
Chris@366:     for (int j = 0; j < m_p.atomsPerFrame; ++j) {
Chris@366:         cqblock.push_back(ComplexColumn());
Chris@366:         for (int i = 0; i < m_p.binsPerOctave; ++i) {
Chris@366:             cqblock[j].push_back(cqrowvec[i * m_p.atomsPerFrame + j]);
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     return cqblock;
Chris@366: }
Chris@366: 
Chris@366: