c@225: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
c@225: /*
c@225:     QM DSP Library
c@225: 
c@225:     Centre for Digital Music, Queen Mary, University of London.
c@309:     This file 2005-2006 Christian Landone.
c@309: 
c@309:     This program is free software; you can redistribute it and/or
c@309:     modify it under the terms of the GNU General Public License as
c@309:     published by the Free Software Foundation; either version 2 of the
c@309:     License, or (at your option) any later version.  See the file
c@309:     COPYING included with this distribution for more information.
c@225: */
c@225: 
c@225: #include "ConstantQ.h"
c@225: #include "dsp/transforms/FFT.h"
c@225: 
c@245: #include <iostream>
c@245: 
c@298: #ifdef NOT_DEFINED
c@298: // see note in CQprecalc
c@298: 
c@276: #include "CQprecalc.cpp"
c@276: 
c@276: static bool push_precalculated(int uk, int fftlength,
c@276:                                std::vector<unsigned> &is,
c@276:                                std::vector<unsigned> &js,
c@276:                                std::vector<double> &real,
c@276:                                std::vector<double> &imag)
c@276: {
c@276:     if (uk == 76 && fftlength == 16384) {
c@276:         push_76_16384(is, js, real, imag);
c@276:         return true;
c@276:     }
c@276:     if (uk == 144 && fftlength == 4096) {
c@276:         push_144_4096(is, js, real, imag);
c@276:         return true;
c@276:     }
c@276:     if (uk == 65 && fftlength == 2048) {
c@276:         push_65_2048(is, js, real, imag);
c@276:         return true;
c@276:     }
c@276:     if (uk == 84 && fftlength == 65536) {
c@276:         push_84_65536(is, js, real, imag);
c@276:         return true;
c@276:     }
c@276:     return false;
c@276: }
c@298: #endif
c@276: 
c@225: //---------------------------------------------------------------------------
c@225: // nextpow2 returns the smallest integer n such that 2^n >= x.
c@225: static double nextpow2(double x) {
c@225:     double y = ceil(log(x)/log(2.0));
c@225:     return(y);
c@225: }
c@225: 
c@225: static double squaredModule(const double & xx, const double & yy) {
c@225:     return xx*xx + yy*yy;
c@225: }
c@225: 
c@225: //----------------------------------------------------------------------------
c@225: 
c@276: ConstantQ::ConstantQ( CQConfig Config ) :
c@276:     m_sparseKernel(0)
c@225: {
c@225:     initialise( Config );
c@225: }
c@225: 
c@225: ConstantQ::~ConstantQ()
c@225: {
c@225:     deInitialise();
c@225: }
c@225: 
c@225: //----------------------------------------------------------------------------
c@225: void ConstantQ::sparsekernel()
c@225: {
c@276: //    std::cerr << "ConstantQ: initialising sparse kernel, uK = " << m_uK << ", FFTLength = " << m_FFTLength << "...";
c@276: 
c@276:     SparseKernel *sk = new SparseKernel();
c@276: 
c@298: #ifdef NOT_DEFINED
c@276:     if (push_precalculated(m_uK, m_FFTLength,
c@276:                            sk->is, sk->js, sk->real, sk->imag)) {
c@298: //        std::cerr << "using precalculated kernel" << std::endl;
c@276:         m_sparseKernel = sk;
c@276:         return;
c@276:     }
c@298: #endif
c@298: 
c@225:     //generates spectral kernel matrix (upside down?)
c@225:     // initialise temporal kernel with zeros, twice length to deal w. complex numbers
c@225: 
c@225:     double* hammingWindowRe = new double [ m_FFTLength ];
c@225:     double* hammingWindowIm = new double [ m_FFTLength ];
c@225:     double* transfHammingWindowRe = new double [ m_FFTLength ];
c@225:     double* transfHammingWindowIm = new double [ m_FFTLength ];
c@225: 
c@225:     for (unsigned u=0; u < m_FFTLength; u++) 
c@225:     {
c@225: 	hammingWindowRe[u] = 0;
c@225: 	hammingWindowIm[u] = 0;
c@225:     }
c@225: 
c@225:     // Here, fftleng*2 is a guess of the number of sparse cells in the matrix
c@225:     // The matrix K x fftlength but the non-zero cells are an antialiased
c@225:     // square root function. So mostly is a line, with some grey point.
c@276:     sk->is.reserve( m_FFTLength*2 );
c@276:     sk->js.reserve( m_FFTLength*2 );
c@276:     sk->real.reserve( m_FFTLength*2 );
c@276:     sk->imag.reserve( m_FFTLength*2 );
c@225: 	
c@225:     // for each bin value K, calculate temporal kernel, take its fft to
c@225:     //calculate the spectral kernel then threshold it to make it sparse and 
c@225:     //add it to the sparse kernels matrix
c@225:     double squareThreshold = m_CQThresh * m_CQThresh;
c@225: 
c@289:     FFT m_FFT(m_FFTLength);
c@225: 	
c@225:     for (unsigned k = m_uK; k--; ) 
c@225:     {
c@228:         for (unsigned u=0; u < m_FFTLength; u++) 
c@228:         {
c@228:             hammingWindowRe[u] = 0;
c@228:             hammingWindowIm[u] = 0;
c@228:         }
c@228:         
c@225: 	// Computing a hamming window
c@225: 	const unsigned hammingLength = (int) ceil( m_dQ * m_FS / ( m_FMin * pow(2,((double)(k))/(double)m_BPO)));
c@228: 
cannam@469: //        cerr << "k = " << k << ", q = " << m_dQ << ", m_FMin = " << m_FMin << ", hammingLength = " << hammingLength << " (rounded up from " << (m_dQ * m_FS / ( m_FMin * pow(2,((double)(k))/(double)m_BPO))) << ")" << endl;
cannam@469:         
cannam@469: 
c@228:         unsigned origin = m_FFTLength/2 - hammingLength/2;
c@228: 
c@225: 	for (unsigned i=0; i<hammingLength; i++) 
c@225: 	{
c@225: 	    const double angle = 2*PI*m_dQ*i/hammingLength;
c@225: 	    const double real = cos(angle);
c@225: 	    const double imag = sin(angle);
c@225: 	    const double absol = hamming(hammingLength, i)/hammingLength;
c@228: 	    hammingWindowRe[ origin + i ] = absol*real;
c@228: 	    hammingWindowIm[ origin + i ] = absol*imag;
c@225: 	}
c@225: 
c@228:         for (unsigned i = 0; i < m_FFTLength/2; ++i) {
c@228:             double temp = hammingWindowRe[i];
c@228:             hammingWindowRe[i] = hammingWindowRe[i + m_FFTLength/2];
c@228:             hammingWindowRe[i + m_FFTLength/2] = temp;
c@228:             temp = hammingWindowIm[i];
c@228:             hammingWindowIm[i] = hammingWindowIm[i + m_FFTLength/2];
c@228:             hammingWindowIm[i + m_FFTLength/2] = temp;
c@228:         }
c@228:     
c@225: 	//do fft of hammingWindow
c@289: 	m_FFT.process( 0, hammingWindowRe, hammingWindowIm, transfHammingWindowRe, transfHammingWindowIm );
c@225: 
c@225: 		
c@225: 	for (unsigned j=0; j<( m_FFTLength ); j++) 
c@225: 	{
c@225: 	    // perform thresholding
c@225: 	    const double squaredBin = squaredModule( transfHammingWindowRe[ j ], transfHammingWindowIm[ j ]);
c@225: 	    if (squaredBin <= squareThreshold) continue;
c@225: 		
cannam@469: 	    // Insert non-zero position indexes
c@276: 	    sk->is.push_back(j);
c@276: 	    sk->js.push_back(k);
c@225: 
c@225: 	    // take conjugate, normalise and add to array sparkernel
c@276: 	    sk->real.push_back( transfHammingWindowRe[ j ]/m_FFTLength);
c@276: 	    sk->imag.push_back(-transfHammingWindowIm[ j ]/m_FFTLength);
c@225: 	}
c@225: 
c@225:     }
c@225: 
c@225:     delete [] hammingWindowRe;
c@225:     delete [] hammingWindowIm;
c@225:     delete [] transfHammingWindowRe;
c@225:     delete [] transfHammingWindowIm;
c@225: 
c@276: /*
c@276:     using std::cout;
c@276:     using std::endl;
c@276: 
c@276:     cout.precision(28);
c@276: 
c@276:     int n = sk->is.size();
c@276:     int w = 8;
c@276:     cout << "static unsigned int sk_i_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
c@276:     for (int i = 0; i < n; ++i) {
c@276:         if (i % w == 0) cout << "    ";
c@276:         cout << sk->is[i];
c@276:         if (i + 1 < n) cout << ", ";
c@276:         if (i % w == w-1) cout << endl;
c@276:     };
c@276:     if (n % w != 0) cout << endl;
c@276:     cout << "};" << endl;
c@276: 
c@276:     n = sk->js.size();
c@276:     cout << "static unsigned int sk_j_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
c@276:     for (int i = 0; i < n; ++i) {
c@276:         if (i % w == 0) cout << "    ";
c@276:         cout << sk->js[i];
c@276:         if (i + 1 < n) cout << ", ";
c@276:         if (i % w == w-1) cout << endl;
c@276:     };
c@276:     if (n % w != 0) cout << endl;
c@276:     cout << "};" << endl;
c@276: 
c@276:     w = 2;
c@276:     n = sk->real.size();
c@276:     cout << "static double sk_real_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
c@276:     for (int i = 0; i < n; ++i) {
c@276:         if (i % w == 0) cout << "    ";
c@276:         cout << sk->real[i];
c@276:         if (i + 1 < n) cout << ", ";
c@276:         if (i % w == w-1) cout << endl;
c@276:     };
c@276:     if (n % w != 0) cout << endl;
c@276:     cout << "};" << endl;
c@276: 
c@276:     n = sk->imag.size();
c@276:     cout << "static double sk_imag_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
c@276:     for (int i = 0; i < n; ++i) {
c@276:         if (i % w == 0) cout << "    ";
c@276:         cout << sk->imag[i];
c@276:         if (i + 1 < n) cout << ", ";
c@276:         if (i % w == w-1) cout << endl;
c@276:     };
c@276:     if (n % w != 0) cout << endl;
c@276:     cout << "};" << endl;
c@276: 
c@276:     cout << "static void push_" << m_uK << "_" << m_FFTLength << "(vector<unsigned int> &is, vector<unsigned int> &js, vector<double> &real, vector<double> &imag)" << endl;
c@276:     cout << "{\n    is.reserve(" << n << ");\n";
c@276:     cout << "    js.reserve(" << n << ");\n";
c@276:     cout << "    real.reserve(" << n << ");\n";
c@276:     cout << "    imag.reserve(" << n << ");\n";
c@276:     cout << "    for (int i = 0; i < " << n << "; ++i) {" << endl;
c@276:     cout << "        is.push_back(sk_i_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
c@276:     cout << "        js.push_back(sk_j_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
c@276:     cout << "        real.push_back(sk_real_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
c@276:     cout << "        imag.push_back(sk_imag_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
c@276:     cout << "    }" << endl;
c@276:     cout << "}" << endl;
c@276: */
c@276: //    std::cerr << "done\n -> is: " << sk->is.size() << ", js: " << sk->js.size() << ", reals: " << sk->real.size() << ", imags: " << sk->imag.size() << std::endl;
c@276:     
c@276:     m_sparseKernel = sk;
c@276:     return;
c@225: }
c@225: 
c@225: //-----------------------------------------------------------------------------
c@257: double* ConstantQ::process( const double* fftdata )
c@225: {
c@276:     if (!m_sparseKernel) {
c@276:         std::cerr << "ERROR: ConstantQ::process: Sparse kernel has not been initialised" << std::endl;
c@276:         return m_CQdata;
c@276:     }
c@276: 
c@276:     SparseKernel *sk = m_sparseKernel;
c@276: 
c@225:     for (unsigned row=0; row<2*m_uK; row++) 
c@225:     {
c@225: 	m_CQdata[ row ] = 0;
c@225: 	m_CQdata[ row+1 ] = 0;
c@225:     }
c@276:     const unsigned *fftbin = &(sk->is[0]);
c@276:     const unsigned *cqbin  = &(sk->js[0]);
c@276:     const double   *real   = &(sk->real[0]);
c@276:     const double   *imag   = &(sk->imag[0]);
c@276:     const unsigned int sparseCells = sk->real.size();
c@225: 	
c@225:     for (unsigned i = 0; i<sparseCells; i++)
c@225:     {
c@225: 	const unsigned row = cqbin[i];
c@225: 	const unsigned col = fftbin[i];
cannam@469:         if (col == 0) continue;
c@225: 	const double & r1  = real[i];
c@225: 	const double & i1  = imag[i];
c@263: 	const double & r2  = fftdata[ (2*m_FFTLength) - 2*col - 2 ];
c@263: 	const double & i2  = fftdata[ (2*m_FFTLength) - 2*col - 2 + 1 ];
c@225: 	// add the multiplication
c@225: 	m_CQdata[ 2*row  ] += (r1*r2 - i1*i2);
c@225: 	m_CQdata[ 2*row+1] += (r1*i2 + i1*r2);
c@225:     }
c@225: 
c@225:     return m_CQdata;
c@225: }
c@225: 
c@225: 
c@225: void ConstantQ::initialise( CQConfig Config )
c@225: {
c@225:     m_FS = Config.FS;
c@225:     m_FMin = Config.min;		// min freq
c@225:     m_FMax = Config.max;		// max freq
c@225:     m_BPO = Config.BPO;		// bins per octave
c@225:     m_CQThresh = Config.CQThresh;// ConstantQ threshold for kernel generation
c@225: 
c@225:     m_dQ = 1/(pow(2,(1/(double)m_BPO))-1);	// Work out Q value for Filter bank
cannam@468:     m_uK = (unsigned int) ceil(m_BPO * log(m_FMax/m_FMin)/log(2.0));	// No. of constant Q bins
c@225: 
c@249: //    std::cerr << "ConstantQ::initialise: rate = " << m_FS << ", fmin = " << m_FMin << ", fmax = " << m_FMax << ", bpo = " << m_BPO << ", K = " << m_uK << ", Q = " << m_dQ << std::endl;
c@245: 
c@225:     // work out length of fft required for this constant Q Filter bank
c@225:     m_FFTLength = (int) pow(2, nextpow2(ceil( m_dQ*m_FS/m_FMin )));
c@225: 
cannam@468:     m_hop = m_FFTLength/8;
c@225: 
c@249: //    std::cerr << "ConstantQ::initialise: -> fft length = " << m_FFTLength << ", hop = " << m_hop << std::endl;
c@245: 
c@225:     // allocate memory for cqdata
c@225:     m_CQdata = new double [2*m_uK];
c@225: }
c@225: 
c@225: void ConstantQ::deInitialise()
c@225: {
c@225:     delete [] m_CQdata;
c@276:     delete m_sparseKernel;
c@225: }
c@225: 
c@257: void ConstantQ::process(const double *FFTRe, const double* FFTIm,
c@257:                         double *CQRe, double *CQIm)
c@225: {
c@276:     if (!m_sparseKernel) {
c@276:         std::cerr << "ERROR: ConstantQ::process: Sparse kernel has not been initialised" << std::endl;
c@276:         return;
c@276:     }
c@276: 
c@276:     SparseKernel *sk = m_sparseKernel;
c@276: 
c@225:     for (unsigned row=0; row<m_uK; row++) 
c@225:     {
c@225: 	CQRe[ row ] = 0;
c@225: 	CQIm[ row ] = 0;
c@225:     }
c@225: 
c@276:     const unsigned *fftbin = &(sk->is[0]);
c@276:     const unsigned *cqbin  = &(sk->js[0]);
c@276:     const double   *real   = &(sk->real[0]);
c@276:     const double   *imag   = &(sk->imag[0]);
c@276:     const unsigned int sparseCells = sk->real.size();
c@225: 	
c@225:     for (unsigned i = 0; i<sparseCells; i++)
c@225:     {
c@225: 	const unsigned row = cqbin[i];
c@225: 	const unsigned col = fftbin[i];
cannam@469:         if (col == 0) continue;
c@225: 	const double & r1  = real[i];
c@225: 	const double & i1  = imag[i];
cannam@469: 	const double & r2  = FFTRe[ m_FFTLength - col ];
cannam@469: 	const double & i2  = FFTIm[ m_FFTLength - col ];
c@225: 	// add the multiplication
c@225: 	CQRe[ row ] += (r1*r2 - i1*i2);
c@225: 	CQIm[ row ] += (r1*i2 + i1*r2);
c@225:     }
c@225: }