Chris@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@0: 
Chris@0: /*
Chris@0:     Sonic Visualiser
Chris@0:     An audio file viewer and annotation editor.
Chris@0:     Centre for Digital Music, Queen Mary, University of London.
Chris@0:     This file copyright 2006 Chris Cannam.
Chris@0:     
Chris@0:     This program is free software; you can redistribute it and/or
Chris@0:     modify it under the terms of the GNU General Public License as
Chris@0:     published by the Free Software Foundation; either version 2 of the
Chris@0:     License, or (at your option) any later version.  See the file
Chris@0:     COPYING included with this distribution for more information.
Chris@0: */
Chris@0: 
Chris@14: #include "PhaseVocoderTimeStretcher.h"
Chris@0: 
Chris@0: #include <iostream>
Chris@0: #include <cassert>
Chris@0: 
Chris@14: //#define DEBUG_PHASE_VOCODER_TIME_STRETCHER 1
Chris@0: 
Chris@14: PhaseVocoderTimeStretcher::PhaseVocoderTimeStretcher(float ratio,
Chris@0: 					   size_t maxProcessInputBlockSize,
Chris@0: 					   size_t inputIncrement,
Chris@0: 					   size_t windowSize,
Chris@0: 					   WindowType windowType) :
Chris@0:     m_ratio(ratio),
Chris@0:     m_n1(inputIncrement),
Chris@12:     m_n2(lrintf(m_n1 * ratio)),
Chris@0:     m_wlen(std::max(windowSize, m_n2 * 2)),
Chris@0:     m_inbuf(m_wlen),
Chris@12:     m_outbuf(maxProcessInputBlockSize * ratio + 1024) //!!!
Chris@0: {
Chris@0:     m_window = new Window<float>(windowType, m_wlen),
Chris@0: 
Chris@0:     m_time = (fftwf_complex *)fftwf_malloc(sizeof(fftwf_complex) * m_wlen);
Chris@0:     m_freq = (fftwf_complex *)fftwf_malloc(sizeof(fftwf_complex) * m_wlen);
Chris@0:     m_dbuf = (float *)fftwf_malloc(sizeof(float) * m_wlen);
Chris@12:     m_mashbuf = (float *)fftwf_malloc(sizeof(float) * m_wlen);
Chris@13:     m_modulationbuf = (float *)fftwf_malloc(sizeof(float) * m_wlen);
Chris@12:     m_prevPhase = (float *)fftwf_malloc(sizeof(float) * m_wlen);
Chris@12:     m_prevAdjustedPhase = (float *)fftwf_malloc(sizeof(float) * m_wlen);
Chris@0: 
Chris@0:     m_plan = fftwf_plan_dft_1d(m_wlen, m_time, m_freq, FFTW_FORWARD, FFTW_ESTIMATE);
Chris@0:     m_iplan = fftwf_plan_dft_c2r_1d(m_wlen, m_freq, m_dbuf, FFTW_ESTIMATE);
Chris@0: 
Chris@0:     for (int i = 0; i < m_wlen; ++i) {
Chris@0: 	m_mashbuf[i] = 0.0;
Chris@13: 	m_modulationbuf[i] = 0.0;
Chris@12:         m_prevPhase[i] = 0.0;
Chris@12:         m_prevAdjustedPhase[i] = 0.0;
Chris@0:     }
Chris@0: }
Chris@0: 
Chris@14: PhaseVocoderTimeStretcher::~PhaseVocoderTimeStretcher()
Chris@0: {
Chris@14:     std::cerr << "PhaseVocoderTimeStretcher::~PhaseVocoderTimeStretcher" << std::endl;
Chris@0: 
Chris@0:     fftwf_destroy_plan(m_plan);
Chris@0:     fftwf_destroy_plan(m_iplan);
Chris@0: 
Chris@0:     fftwf_free(m_time);
Chris@0:     fftwf_free(m_freq);
Chris@0:     fftwf_free(m_dbuf);
Chris@12:     fftwf_free(m_mashbuf);
Chris@13:     fftwf_free(m_modulationbuf);
Chris@12:     fftwf_free(m_prevPhase);
Chris@12:     fftwf_free(m_prevAdjustedPhase);
Chris@0: 
Chris@0:     delete m_window;
Chris@0: }	
Chris@0: 
Chris@0: size_t
Chris@14: PhaseVocoderTimeStretcher::getProcessingLatency() const
Chris@0: {
Chris@0:     return getWindowSize() - getInputIncrement();
Chris@0: }
Chris@0: 
Chris@0: void
Chris@14: PhaseVocoderTimeStretcher::process(float *input, float *output, size_t samples)
Chris@0: {
Chris@0:     // We need to add samples from input to our internal buffer.  When
Chris@0:     // we have m_windowSize samples in the buffer, we can process it,
Chris@0:     // move the samples back by m_n1 and write the output onto our
Chris@0:     // internal output buffer.  If we have (samples * ratio) samples
Chris@0:     // in that, we can write m_n2 of them back to output and return
Chris@0:     // (otherwise we have to write zeroes).
Chris@0: 
Chris@0:     // When we process, we write m_wlen to our fixed output buffer
Chris@0:     // (m_mashbuf).  We then pull out the first m_n2 samples from that
Chris@0:     // buffer, push them into the output ring buffer, and shift
Chris@0:     // m_mashbuf left by that amount.
Chris@0: 
Chris@0:     // The processing latency is then m_wlen - m_n2.
Chris@0: 
Chris@0:     size_t consumed = 0;
Chris@0: 
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@14:     std::cerr << "PhaseVocoderTimeStretcher::process(" << samples << ", consumed = " << consumed << "), writable " << m_inbuf.getWriteSpace() <<", readable "<< m_outbuf.getReadSpace() << std::endl;
Chris@0: #endif
Chris@0: 
Chris@0:     while (consumed < samples) {
Chris@0: 
Chris@0: 	size_t writable = m_inbuf.getWriteSpace();
Chris@0: 	writable = std::min(writable, samples - consumed);
Chris@0: 
Chris@0: 	if (writable == 0) {
Chris@0: 	    //!!! then what? I don't think this should happen, but
Chris@14: 	    std::cerr << "WARNING: PhaseVocoderTimeStretcher::process: writable == 0" << std::endl;
Chris@0: 	    break;
Chris@0: 	}
Chris@0: 
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@0: 	std::cerr << "writing " << writable << " from index " << consumed << " to inbuf, consumed will be " << consumed + writable << std::endl;
Chris@0: #endif
Chris@0: 	m_inbuf.write(input + consumed, writable);
Chris@0: 	consumed += writable;
Chris@0: 
Chris@0: 	while (m_inbuf.getReadSpace() >= m_wlen &&
Chris@0: 	       m_outbuf.getWriteSpace() >= m_n2) {
Chris@0: 
Chris@0: 	    // We know we have at least m_wlen samples available
Chris@0: 	    // in m_inbuf.  We need to peek m_wlen of them for
Chris@0: 	    // processing, and then read m_n1 to advance the read
Chris@0: 	    // pointer.
Chris@0: 
Chris@0: 	    size_t got = m_inbuf.peek(m_dbuf, m_wlen);
Chris@0: 	    assert(got == m_wlen);
Chris@0: 		
Chris@13: 	    processBlock(m_dbuf, m_mashbuf, m_modulationbuf);
Chris@0: 
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@0: 	    std::cerr << "writing first " << m_n2 << " from mashbuf, skipping " << m_n1 << " on inbuf " << std::endl;
Chris@0: #endif
Chris@0: 	    m_inbuf.skip(m_n1);
Chris@13: 
Chris@13:             for (size_t i = 0; i < m_n2; ++i) {
Chris@13:                 if (m_modulationbuf[i] > 0.f) {
Chris@13:                     m_mashbuf[i] /= m_modulationbuf[i];
Chris@13:                 }
Chris@13:             }
Chris@13: 
Chris@0: 	    m_outbuf.write(m_mashbuf, m_n2);
Chris@0: 
Chris@0: 	    for (size_t i = 0; i < m_wlen - m_n2; ++i) {
Chris@0: 		m_mashbuf[i] = m_mashbuf[i + m_n2];
Chris@13:                 m_modulationbuf[i] = m_modulationbuf[i + m_n2];
Chris@0: 	    }
Chris@13: 
Chris@0: 	    for (size_t i = m_wlen - m_n2; i < m_wlen; ++i) {
Chris@0: 		m_mashbuf[i] = 0.0f;
Chris@13:                 m_modulationbuf[i] = 0.0f;
Chris@0: 	    }
Chris@0: 	}
Chris@0: 
Chris@14: //	std::cerr << "WARNING: PhaseVocoderTimeStretcher::process: writespace not enough for output increment (" << m_outbuf.getWriteSpace() << " < " << m_n2 << ")" << std::endl;
Chris@0: //	}
Chris@0: 
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@0: 	std::cerr << "loop ended: inbuf read space " << m_inbuf.getReadSpace() << ", outbuf write space " << m_outbuf.getWriteSpace() << std::endl;
Chris@0: #endif
Chris@0:     }
Chris@0: 
Chris@12:     size_t toRead = lrintf(samples * m_ratio);
Chris@12: 
Chris@12:     if (m_outbuf.getReadSpace() < toRead) {
Chris@14: 	std::cerr << "WARNING: PhaseVocoderTimeStretcher::process: not enough data (yet?) (" << m_outbuf.getReadSpace() << " < " << toRead << ")" << std::endl;
Chris@12: 	size_t fill = toRead - m_outbuf.getReadSpace();
Chris@0: 	for (size_t i = 0; i < fill; ++i) {
Chris@0: 	    output[i] = 0.0;
Chris@0: 	}
Chris@0: 	m_outbuf.read(output + fill, m_outbuf.getReadSpace());
Chris@0:     } else {
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@12: 	std::cerr << "enough data - writing " << toRead << " from outbuf" << std::endl;
Chris@0: #endif
Chris@12: 	m_outbuf.read(output, toRead);
Chris@0:     }
Chris@0: 
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@14:     std::cerr << "PhaseVocoderTimeStretcher::process returning" << std::endl;
Chris@0: #endif
Chris@0: }
Chris@0: 
Chris@0: void
Chris@14: PhaseVocoderTimeStretcher::processBlock(float *buf, float *out, float *modulation)
Chris@0: {
Chris@0:     size_t i;
Chris@0: 
Chris@0:     // buf contains m_wlen samples; out contains enough space for
Chris@0:     // m_wlen * ratio samples (we mix into out, rather than replacing)
Chris@0: 
Chris@14: #ifdef DEBUG_PHASE_VOCODER_TIME_STRETCHER
Chris@14:     std::cerr << "PhaseVocoderTimeStretcher::processBlock" << std::endl;
Chris@0: #endif
Chris@0: 
Chris@0:     m_window->cut(buf);
Chris@0: 
Chris@0:     for (i = 0; i < m_wlen/2; ++i) {
Chris@0: 	float temp = buf[i];
Chris@0: 	buf[i] = buf[i + m_wlen/2];
Chris@0: 	buf[i + m_wlen/2] = temp;
Chris@0:     }
Chris@0:     
Chris@0:     for (i = 0; i < m_wlen; ++i) {
Chris@0: 	m_time[i][0] = buf[i];
Chris@0: 	m_time[i][1] = 0.0;
Chris@0:     }
Chris@0: 
Chris@0:     fftwf_execute(m_plan); // m_time -> m_freq
Chris@0: 
Chris@0:     for (i = 0; i < m_wlen; ++i) {
Chris@0: 	
Chris@0: 	float mag = sqrtf(m_freq[i][0] * m_freq[i][0] +
Chris@0: 			  m_freq[i][1] * m_freq[i][1]);
Chris@0: 		
Chris@12:         float phase = princargf(atan2f(m_freq[i][1], m_freq[i][0]));
Chris@12: 
Chris@12:         float omega = (2 * M_PI * m_n1 * i) / m_wlen;
Chris@0: 	
Chris@12:         float expectedPhase = m_prevPhase[i] + omega;
Chris@12: 
Chris@12:         float phaseError = princargf(phase - expectedPhase);
Chris@12: 
Chris@12:         float phaseIncrement = (omega + phaseError) / m_n1;
Chris@12: 
Chris@12:         float adjustedPhase = m_prevAdjustedPhase[i] + m_n2 * phaseIncrement;
Chris@0: 	
Chris@12: 	float real = mag * cosf(adjustedPhase);
Chris@12: 	float imag = mag * sinf(adjustedPhase);
Chris@0: 	m_freq[i][0] = real;
Chris@0: 	m_freq[i][1] = imag;
Chris@12: 
Chris@12:         m_prevPhase[i] = phase;
Chris@12:         m_prevAdjustedPhase[i] = adjustedPhase;
Chris@0:     }
Chris@0:     
Chris@0:     fftwf_execute(m_iplan); // m_freq -> in, inverse fft
Chris@0:     
Chris@0:     for (i = 0; i < m_wlen/2; ++i) {
Chris@0: 	float temp = buf[i] / m_wlen;
Chris@0: 	buf[i] = buf[i + m_wlen/2] / m_wlen;
Chris@0: 	buf[i + m_wlen/2] = temp;
Chris@0:     }
Chris@0:     
Chris@0:     m_window->cut(buf);
Chris@13: /*    
Chris@0:     int div = m_wlen / m_n2;
Chris@0:     if (div > 1) div /= 2;
Chris@0:     for (i = 0; i < m_wlen; ++i) {
Chris@0: 	buf[i] /= div;
Chris@0:     }
Chris@13: */
Chris@0:     for (i = 0; i < m_wlen; ++i) {
Chris@0: 	out[i] += buf[i];
Chris@13:         modulation[i] += m_window->getValue(i);
Chris@0:     }
Chris@0: }