c@225: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
c@225: 
c@225: /*
c@225:     QM DSP Library
c@225: 
c@225:     Centre for Digital Music, Queen Mary, University of London.
c@340:     This file 2005-2006 Christian Landone, copyright 2013 QMUL.
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 "PhaseVocoder.h"
c@225: #include "dsp/transforms/FFT.h"
c@340: #include "maths/MathUtilities.h"
c@225: #include <math.h>
c@225: 
c@340: #include <iostream>
c@340: using std::cerr;
c@340: using std::endl;
c@225: 
c@340: PhaseVocoder::PhaseVocoder(int n, int hop) :
c@340:     m_n(n),
c@340:     m_hop(hop)
c@225: {
c@289:     m_fft = new FFTReal(m_n);
c@340:     m_real = new double[m_n];
c@340:     m_imag = new double[m_n];
c@340:     m_phase = new double[m_n/2 + 1];
c@340:     m_unwrapped = new double[m_n/2 + 1];
c@340: 
c@340:     for (int i = 0; i < m_n/2 + 1; ++i) {
c@340:         m_phase[i] = 0.0;
c@340:         m_unwrapped[i] = 0.0;
c@340:     }
c@340: 
c@340:     reset();
c@225: }
c@225: 
c@225: PhaseVocoder::~PhaseVocoder()
c@225: {
c@340:     delete [] m_unwrapped;
c@340:     delete [] m_phase;
c@340:     delete [] m_real;
c@340:     delete [] m_imag;
c@289:     delete m_fft;
c@225: }
c@225: 
c@340: void PhaseVocoder::FFTShift(double *src)
c@225: {
c@340:     const int hs = m_n/2;
c@280:     for (int i = 0; i < hs; ++i) {
c@280:         double tmp = src[i];
c@280:         src[i] = src[i + hs];
c@280:         src[i + hs] = tmp;
c@225:     }
c@225: }
c@225: 
c@340: void PhaseVocoder::process(double *src, double *mag, double *theta,
c@340:                            double *unwrapped)
c@225: {
c@340:     FFTShift(src);
c@289: 	
c@340:     m_fft->forward(src, m_real, m_imag);
c@225: 
c@340:     getMagnitudes(mag);
c@340:     getPhases(theta);
c@340:     unwrapPhases(theta, unwrapped);
c@225: }
c@225: 
c@340: void PhaseVocoder::reset()
c@340: {
c@340:     for (int i = 0; i < m_n/2 + 1; ++i) {
c@340:         // m_phase stores the "previous" phase, so set to one step
c@340:         // behind so that a signal with initial phase at zero matches
c@340:         // the expected values. This is completely unnecessary for any
c@340:         // analytical purpose, it's just tidier.
c@340:         double omega = (2 * M_PI * m_hop * i) / m_n;
c@340:         m_phase[i] = -omega;
c@340:         m_unwrapped[i] = -omega;
c@225:     }
c@225: }
c@225: 
c@340: void PhaseVocoder::getMagnitudes(double *mag)
c@340: {	
c@340:     for (int i = 0; i < m_n/2 + 1; i++) {
c@340: 	mag[i] = sqrt(m_real[i] * m_real[i] + m_imag[i] * m_imag[i]);
c@340:     }
c@340: }
c@340: 
c@340: void PhaseVocoder::getPhases(double *theta)
c@225: {
c@340:     for (int i = 0; i < m_n/2 + 1; i++) {
c@340: 	theta[i] = atan2(m_imag[i], m_real[i]);
c@225:     }	
c@225: }
c@340: 
c@340: void PhaseVocoder::unwrapPhases(double *theta, double *unwrapped)
c@340: {
c@340:     cerr << "PhaseVocoder::unwrapPhases" << endl;
c@340: 
c@340:     for (int i = 0; i < m_n/2 + 1; ++i) {
c@340: 
c@340:         double omega = (2 * M_PI * m_hop * i) / m_n;
c@340:         double expected = m_phase[i] + omega;
c@340:         double error = MathUtilities::princarg(theta[i] - expected);
c@340: 
c@340:         unwrapped[i] = m_unwrapped[i] + omega + error;
c@340: 
c@340:         cerr << "i = " << i << ", instantaneous phase = " << theta[i] << ", prev phase = " << m_phase[i] << ", omega = " << omega << ", expected = " << expected << ", error = " << error << ", unwrapped = " << unwrapped[i] << endl;
c@340: 
c@340:         m_phase[i] = theta[i];
c@340:         m_unwrapped[i] = unwrapped[i];
c@340:     }
c@340: }
c@340: