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 "CQInverse.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: CQInverse::CQInverse(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@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: CQInverse::~CQInverse()
c@116: {
c@116:     delete m_fft;
c@116:     for (int i = 0; i < (int)m_upsamplers.size(); ++i) {
c@116:         delete m_upsamplers[i];
c@116:     }
c@116:     delete m_kernel;
c@116: }
c@116: 
c@116: double
c@116: CQInverse::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: CQInverse::getBinFrequency(double bin) const
c@116: {
c@145:     // our bins are returned in high->low order
c@145:     bin = (getBinsPerOctave() * getOctaves()) - bin - 1;
c@145:     return getMinFrequency() * pow(2, (bin / getBinsPerOctave()));
c@116: }
c@116: 
c@116: void
c@116: CQInverse::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@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_upsamplers.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@116:         Resampler *r = new Resampler
c@116:             (sourceRate / factor, sourceRate, 50, 0.05);
c@116: 
c@116: #ifdef DEBUG_CQ
c@116:         cerr << "inverse: octave " << i << ": resample from " << sourceRate/factor << " to " << sourceRate << endl;
c@116: #endif
c@116: 
c@116: 	// See ConstantQ.cpp for discussion on latency -- output
c@116: 	// latency here is at target rate which, this way around, is
c@116: 	// what we want
c@116: 
c@116:         latencies.push_back(r->getLatency());
c@116:         m_upsamplers.push_back(r);
c@116:     }
c@116: 
c@116:     // additionally we will have fftHop latency at individual octave
c@116:     // rate (before upsampling) for the overlap-add in each octave
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116:         latencies[i] += m_p.fftHop * pow(2, i);
c@116:     }
c@116: 
c@116:     // Now reverse the drop adjustment made in ConstantQ to align the
c@116:     // atom centres across different octaves (but this time at output
c@116:     // sample rate)
c@116: 
c@116:     int emptyHops = m_p.firstCentre / m_p.atomSpacing;
c@116: 
c@116:     vector<int> pushes;
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116: 	int factor = pow(2, i);
c@116: 	int pushHops = emptyHops * pow(2, m_octaves - i - 1) - emptyHops;
c@116: 	int push = ((pushHops * m_p.fftHop) * factor) / m_p.atomsPerFrame;
c@116: 	pushes.push_back(push);
c@116:     }
c@116: 
c@116:     int maxLatLessPush = 0;
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116: 	int latLessPush = latencies[i] - pushes[i];
c@116: 	if (latLessPush > maxLatLessPush) maxLatLessPush = latLessPush;
c@116:     }
c@116: 
c@116:     int totalLatency = maxLatLessPush + 10;
c@116:     if (totalLatency < 0) totalLatency = 0;
c@116: 
c@116:     m_outputLatency = totalLatency + m_p.firstCentre * pow(2, m_octaves-1);
c@116: 
c@116: #ifdef DEBUG_CQ
c@116:     cerr << "totalLatency = " << totalLatency << ", m_outputLatency = " << m_outputLatency << endl;
c@116: #endif
c@116: 
c@116:     for (int i = 0; i < m_octaves; ++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: 	// upsampler for this octave.
c@116: 
c@116:         int latencyPadding = totalLatency - latencies[i] + pushes[i];
c@116: 
c@116: #ifdef DEBUG_CQ
c@116:         cerr << "octave " << i << ": push " << pushes[i] << ", resampler latency inc overlap space " << latencies[i] << ", latencyPadding = " << latencyPadding << " (/factor = " << latencyPadding / pow(2, i) << ")" << endl;
c@116: #endif
c@116: 
c@116:         m_buffers.push_back(RealSequence(latencyPadding, 0.0));
c@116:     }
c@116: 
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116:         // Fixed-size buffer for IFFT overlap-add
c@116:         m_olaBufs.push_back(RealSequence(m_p.fftSize, 0.0));
c@116:     }
c@116: 
c@116:     m_fft = new FFTReal(m_p.fftSize);
c@116: }
c@116: 
c@116: CQInverse::RealSequence
c@116: CQInverse::process(const ComplexBlock &block)
c@116: {
c@116:     // The input data is of the form produced by ConstantQ::process --
c@116:     // an unknown number N of columns of varying height. We assert
c@116:     // that N is a multiple of atomsPerFrame * 2^(octaves-1), as must
c@116:     // be the case for data that came directly from our ConstantQ
c@116:     // implementation.
c@116: 
c@116:     int widthProvided = block.size();
c@116: 
c@116:     if (widthProvided == 0) {
c@116:         return drawFromBuffers();
c@116:     }
c@116: 
c@116:     int blockWidth = m_p.atomsPerFrame * int(pow(2, m_octaves - 1));
c@116: 
c@116:     if (widthProvided % blockWidth != 0) {
c@116:         cerr << "ERROR: CQInverse::process: Input block size ("
c@116:              << widthProvided
c@116:              << ") must be a multiple of processing block width "
c@116:              << "(atoms-per-frame * 2^(octaves-1) = "
c@116:              << m_p.atomsPerFrame << " * 2^(" << m_octaves << "-1) = "
c@116:              << blockWidth << ")" << endl;
c@116:         throw std::invalid_argument
c@116:             ("Input block size must be a multiple of processing block width");
c@116:     }
c@116: 
c@116:     // Procedure:
c@116:     // 
c@116:     // 1. Slice the list of columns into a set of lists of columns,
c@116:     // one per octave, each of width N / (2^octave-1) and height
c@116:     // binsPerOctave, containing the values present in that octave
c@116:     //
c@116:     // 2. Group each octave list by atomsPerFrame columns at a time,
c@116:     // and stack these so as to achieve a list, for each octave, of
c@116:     // taller columns of height binsPerOctave * atomsPerFrame
c@116:     //
c@116:     // 3. For each taller column, take the product with the inverse CQ
c@116:     // kernel (which is the conjugate of the forward kernel) and
c@116:     // perform an inverse FFT
c@116:     //
c@116:     // 4. Overlap-add each octave's resynthesised blocks (unwindowed)
c@116:     //
c@116:     // 5. Resample each octave's overlap-add stream to the original
c@116:     // rate
c@116:     //
c@116:     // 6. Sum the resampled streams and return
c@116:     
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116:         
c@116:         // Step 1
c@116: 
c@116:         ComplexBlock oct;
c@116: 
c@116:         for (int j = 0; j < widthProvided; ++j) {
c@116:             int h = block[j].size();
c@116:             if (h < m_binsPerOctave * (i+1)) {
c@116:                 continue;
c@116:             }
c@116:             ComplexColumn col(block[j].begin() + m_binsPerOctave * i,
c@116:                               block[j].begin() + m_binsPerOctave * (i+1));
c@116:             oct.push_back(col);
c@116:         }
c@116: 
c@116:         // Steps 2, 3, 4, 5
c@116:         processOctave(i, oct);
c@116:     }
c@116:     
c@116:     // Step 6
c@116:     return drawFromBuffers();
c@116: }
c@116: 
c@116: CQInverse::RealSequence
c@116: CQInverse::drawFromBuffers()
c@116: {
c@116:     // 6. Sum the resampled streams and return
c@116: 
c@116:     int available = 0;
c@116: 
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116:         if (i == 0 || int(m_buffers[i].size()) < available) {
c@116:             available = m_buffers[i].size();
c@116:         }
c@116:     }
c@116: 
c@116:     RealSequence result(available, 0);
c@116: 
c@116:     if (available == 0) {
c@116:         return result;
c@116:     }
c@116: 
c@116:     for (int i = 0; i < m_octaves; ++i) {
c@116:         for (int j = 0; j < available; ++j) {
c@116:             result[j] += m_buffers[i][j];
c@116:         }
c@116:         m_buffers[i] = RealSequence(m_buffers[i].begin() + available,
c@116:                                     m_buffers[i].end());
c@116:     }
c@116: 
c@116:     return result;
c@116: }
c@116: 
c@116: CQInverse::RealSequence
c@116: CQInverse::getRemainingOutput()
c@116: {
c@116:     for (int j = 0; j < m_octaves; ++j) {
c@116:         int factor = pow(2, j);
c@116:         int latency = (j > 0 ? m_upsamplers[j]->getLatency() : 0) / factor;
c@116:         for (int i = 0; i < (latency + m_p.fftSize) / m_p.fftHop; ++i) {
c@116:             overlapAddAndResample(j, RealSequence(m_olaBufs[j].size(), 0));
c@116:         }
c@116:     }
c@116: 
c@116:     return drawFromBuffers();
c@116: }
c@116: 
c@116: void
c@116: CQInverse::processOctave(int octave, const ComplexBlock &columns)
c@116: {
c@116:     // 2. Group each octave list by atomsPerFrame columns at a time,
c@116:     // and stack these so as to achieve a list, for each octave, of
c@116:     // taller columns of height binsPerOctave * atomsPerFrame
c@116: 
c@116:     int ncols = columns.size();
c@116: 
c@116:     if (ncols % m_p.atomsPerFrame != 0) {
c@116:         cerr << "ERROR: CQInverse::process: Number of columns ("
c@116:              << ncols
c@116:              << ") in octave " << octave
c@116:              << " must be a multiple of atoms-per-frame ("
c@116:              << m_p.atomsPerFrame << ")" << endl;
c@116:         throw std::invalid_argument
c@116:             ("Columns in octave must be a multiple of atoms per frame");
c@116:     }
c@116: 
c@116:     for (int i = 0; i < ncols; i += m_p.atomsPerFrame) {
c@116: 
c@116:         ComplexColumn tallcol;
c@116:         for (int b = 0; b < m_binsPerOctave; ++b) {
c@116:             for (int a = 0; a < m_p.atomsPerFrame; ++a) {
c@116:                 tallcol.push_back(columns[i + a][m_binsPerOctave - b - 1]);
c@116:             }
c@116:         }
c@116:         
c@116:         processOctaveColumn(octave, tallcol);
c@116:     }
c@116: }
c@116: 
c@116: void
c@116: CQInverse::processOctaveColumn(int octave, const ComplexColumn &column)
c@116: {
c@116:     // 3. For each taller column, take the product with the inverse CQ
c@116:     // kernel (which is the conjugate of the forward kernel) and
c@116:     // perform an inverse FFT
c@116: 
c@116:     if ((int)column.size() != m_p.atomsPerFrame * m_binsPerOctave) {
c@116:         cerr << "ERROR: CQInverse::processOctaveColumn: Height of column ("
c@116:              << column.size() << ") in octave " << octave
c@116:              << " must be atoms-per-frame * bins-per-octave ("
c@116:              << m_p.atomsPerFrame << " * " << m_binsPerOctave << " = "
c@116:              << m_p.atomsPerFrame * m_binsPerOctave << ")" << endl;
c@116:         throw std::invalid_argument
c@116:             ("Column height must match atoms-per-frame * bins-per-octave");
c@116:     }
c@116: 
c@116:     ComplexSequence transformed = m_kernel->processInverse(column);
c@116: 
c@116:     int halfLen = m_p.fftSize/2 + 1;
c@116: 
c@116:     RealSequence ri(halfLen, 0);
c@116:     RealSequence ii(halfLen, 0);
c@116: 
c@116:     for (int i = 0; i < halfLen; ++i) {
c@116:         ri[i] = transformed[i].real();
c@116:         ii[i] = transformed[i].imag();
c@116:     }
c@116: 
c@116:     RealSequence timeDomain(m_p.fftSize, 0);
c@116: 
c@116:     m_fft->inverse(ri.data(), ii.data(), timeDomain.data());
c@116: 
c@116:     overlapAddAndResample(octave, timeDomain);
c@116: }
c@116: 
c@116: void
c@116: CQInverse::overlapAddAndResample(int octave, const RealSequence &seq)
c@116: {
c@116:     // 4. Overlap-add each octave's resynthesised blocks (unwindowed)
c@116:     //
c@116:     // and
c@116:     //
c@116:     // 5. Resample each octave's overlap-add stream to the original
c@116:     // rate
c@116: 
c@116:     if (seq.size() != m_olaBufs[octave].size()) {
c@116:         cerr << "ERROR: CQInverse::overlapAdd: input sequence length ("
c@116:              << seq.size() << ") is expected to match OLA buffer size ("
c@116:              << m_olaBufs[octave].size() << ")" << endl;
c@116:         throw std::invalid_argument
c@116:             ("Input sequence length should match OLA buffer size");
c@116:     }
c@116: 
c@116:     RealSequence toResample(m_olaBufs[octave].begin(),
c@116:                             m_olaBufs[octave].begin() + m_p.fftHop);
c@116: 
c@116:     RealSequence resampled = 
c@116:         octave > 0 ?
c@116:         m_upsamplers[octave]->process(toResample.data(), toResample.size()) :
c@116:         toResample;
c@116: 
c@116:     m_buffers[octave].insert(m_buffers[octave].end(),
c@116:                              resampled.begin(),
c@116:                              resampled.end());
c@116:     
c@116:     m_olaBufs[octave] = RealSequence(m_olaBufs[octave].begin() + m_p.fftHop,
c@116:                                      m_olaBufs[octave].end());
c@116:     
c@116:     RealSequence pad(m_p.fftHop, 0);
c@116: 
c@116:     m_olaBufs[octave].insert(m_olaBufs[octave].end(),
c@116:                              pad.begin(),
c@116:                              pad.end());
c@116: 
c@116:     for (int i = 0; i < m_p.fftSize; ++i) {
c@116:         m_olaBufs[octave][i] += seq[i];
c@116:     }
c@116: }
c@116: