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