// qtractorTimeStretch.cpp
//
/****************************************************************************
   Copyright (C) 2005-2008, rncbc aka Rui Nuno Capela. All rights reserved.

   Adapted and refactored from the SoundTouch library (L)GPL,
   Copyright (C) 2001-2006, Olli Parviainen.

   This program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public License
   as published by the Free Software Foundation; either version 2
   of the License, or (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License along
   with this program; if not, write to the Free Software Foundation, Inc.,
   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

*****************************************************************************/

#include "qtractorTimeStretch.h"


// Cross-correlation value calculation over the overlap period.
//

#if defined(__SSE__)

#include <xmmintrin.h>

// SSE detection.
static inline bool sse_enabled (void)
{
	bool bSSE = false;
#if defined(__GNUC__)
#if defined(__i386__) || defined(__x86_64__)
	unsigned int eax, ebx, ecx, edx;
	__asm__ __volatile__ (
		"movl %%ebx, %%esi\n\t" \
		"cpuid\n\t" \
		"xchgl %%ebx, %%esi" \
		: "=a" (eax), "=S" (ebx), "=c" (ecx), "=d" (edx) \
		: "0" (1));
#endif
#endif
	return bSSE;
}


// SSE enabled version.
static inline double sse_cross_corr (
	const float *pV1, const float *pV2, unsigned int iOverlapLength )
{
	unsigned int i;
	__m128 vSum, *pVec2;

	// Note. It means a major slow-down if the routine needs to tolerate 
	// unaligned __m128 memory accesses. It's way faster if we can skip 
	// unaligned slots and use _mm_load_ps instruction instead of _mm_loadu_ps.
	// This can mean up to ~ 10-fold difference (incl. part of which is
	// due to skipping every second round for stereo sound though).
	//
	// Little cheating allowed, return valid correlation only for 
	// aligned locations, meaning every second round for stereo sound.
	if (((unsigned long) pV1) & 15) return -1e50; // Skip unaligned locations.

	// Ensure overlapLength is divisible by 8
	// assert((m_iOverlapLength % 8) == 0);
	iOverlapLength >>= 3;

	// Calculates the cross-correlation value between 'pV1' and 'pV2' vectors
	// Note: pV2 _must_ be aligned to 16-bit boundary, pV1 need not.
	pVec2 = (__m128 *) pV2;
	vSum = _mm_setzero_ps();

	// Unroll the loop by factor of 4 * 4 operations
	for (i = 0; i < iOverlapLength; ++i) {
		// vSum += pV1[0..3] * pV2[0..3]
		vSum = _mm_add_ps(vSum, _mm_mul_ps(_mm_load_ps(pV1), pVec2[0]));
		// vSum += pV1[4..7] * pV2[4..7]
		vSum = _mm_add_ps(vSum, _mm_mul_ps(_mm_load_ps(pV1 + 4), pVec2[1]));
		// vSum += pV1[8..11] * pV2[8..11]
		vSum = _mm_add_ps(vSum, _mm_mul_ps(_mm_load_ps(pV1 + 8), pVec2[2]));
		// vSum += pV1[12..15] * pV2[12..15]
		vSum = _mm_add_ps(vSum, _mm_mul_ps(_mm_load_ps(pV1 + 12), pVec2[3]));
		pV1 += 16;
		pVec2 += 4;
	}

	// return value = vSum[0] + vSum[1] + vSum[2] + vSum[3]
	float *pvSum = (float *) &vSum;
	return (double) (pvSum[0] + pvSum[1] + pvSum[2] + pvSum[3]);
}

#endif


// Standard (slow) version.
static inline double std_cross_corr (
	const float *pV1, const float *pV2, unsigned int iOverlapLength )
{
	double dCorr = 0.0;

	for (unsigned int i = 1; i < iOverlapLength; ++i)
		dCorr += pV1[i] * pV2[i];

	return dCorr;
}


//---------------------------------------------------------------------------
// qtractorTimeStretch - Time-stretch (tempo change) effect for processed sound.
//

// Constructor.
qtractorTimeStretch::qtractorTimeStretch (
	unsigned short iChannels, unsigned int iSampleRate )
{
	m_iChannels = iChannels;

	m_fTempo = 1.0f;
	m_bQuickSeek = false;

	m_bMidBufferDirty = false;
	m_ppMidBuffer = NULL;
	m_ppRefMidBuffer = NULL;
	m_ppRefMidBufferUnaligned = NULL;
	m_ppFrames = NULL;

	m_iOverlapLength = 0;

#if defined(__SSE__)
	if (sse_enabled())
		m_pfnCrossCorr = sse_cross_corr;
	else
#endif
	m_pfnCrossCorr = std_cross_corr;

	setParameters(iSampleRate);
}


// Destructor.
qtractorTimeStretch::~qtractorTimeStretch (void)
{
	if (m_ppFrames) {
		for (unsigned short i = 0; i < m_iChannels; ++i) {
			delete [] m_ppMidBuffer[i];
			delete [] m_ppRefMidBufferUnaligned[i];
		}
		delete [] m_ppMidBuffer;
		delete [] m_ppRefMidBufferUnaligned;
		delete [] m_ppRefMidBuffer;
		delete [] m_ppFrames;
	}
}



// Sets the number of channels, 1=mono, 2=stereo.
void qtractorTimeStretch::setChannels ( unsigned short iChannels )
{
	if (m_iChannels == iChannels)
		return;

	m_iChannels = iChannels;
	m_inputBuffer.setChannels(m_iChannels);
	m_outputBuffer.setChannels(m_iChannels);
}


// Get the assigne number of channels, 1=mono, 2=stereo.
unsigned short qtractorTimeStretch::channels (void) const
{
	return m_iChannels;
}


// Sets new target tempo; less than 1.0 values represent
// slower tempo, greater than 1.0 represents faster tempo.
void qtractorTimeStretch::setTempo ( float fTempo )
{
	// Set new is tempo scaling.
	m_fTempo = fTempo;

	// Calculate ideal skip length (according to tempo value) 
	m_fNominalSkip = m_fTempo * (m_iSeekWindowLength - m_iOverlapLength);
	m_fSkipFract = 0;

	// Calculate how many samples are needed in the input buffer 
	// to process another batch of samples.
	m_iFramesReq = (unsigned int) (m_fNominalSkip + 0.5f) + m_iOverlapLength;
	if (m_iFramesReq < m_iSeekWindowLength)
		m_iFramesReq = m_iSeekWindowLength;
	m_iFramesReq += m_iSeekLength;

	clear();

	// These will be enough for most purposes, and
	// shoudl avoid in-the-fly buffer re-allocations...
	m_inputBuffer.ensureCapacity(m_iFramesReq);
	m_outputBuffer.ensureCapacity(m_iFramesReq);
}

// Get assigned target tempo.
float qtractorTimeStretch::tempo (void) const
{
	return m_fTempo;
}


// Set quick-seek mode (hierachical search).
void qtractorTimeStretch::setQuickSeek ( bool bQuickSeek )
{
	m_bQuickSeek = bQuickSeek;
}

// Get quick-seek mode.
bool qtractorTimeStretch::isQuickSeek (void) const
{
	return m_bQuickSeek;
}


// Sets routine control parameters.
// These control are certain time constants defining
// how the sound is stretched to the desired duration.
//
// iSampleRate = sample rate of the sound.
// iSequenceMs = one processing sequence length in milliseconds.
// iSeekWindowMs = seeking window length for scanning the best
//      overlapping position.
// iOverlapMs = overlapping length.
void qtractorTimeStretch::setParameters (
	unsigned int iSampleRate,
	unsigned int iSequenceMs,
	unsigned int iSeekWindowMs,
	unsigned int iOverlapMs )
{
	m_iSampleRate = iSampleRate;
	m_iSequenceMs = iSequenceMs;
	m_iSeekWindowMs = iSeekWindowMs;
	m_iOverlapMs = iOverlapMs;

	m_iSeekLength = (m_iSampleRate * m_iSeekWindowMs) / 1000;
	m_iSeekWindowLength = (m_iSampleRate * m_iSequenceMs) / 1000;

	calcOverlapLength();

	// Set tempo to recalculate required frames...
	setTempo(m_fTempo);
}



// Get routine control parameters, see setParameters() function.
// Any of the parameters to this function can be NULL, in such case
// corresponding parameter value isn't returned.
void qtractorTimeStretch::getParameters (
	unsigned int *piSampleRate,
	unsigned int *piSequenceMs,
	unsigned int *piSeekWindowMs,
	unsigned int *piOverlapMs )
{
	if (piSampleRate)
		*piSampleRate = m_iSampleRate;

	if (piSequenceMs)
		*piSequenceMs = m_iSequenceMs;

	if (piSeekWindowMs)
		*piSeekWindowMs = m_iSeekWindowMs;

	if (piOverlapMs)
		*piOverlapMs = m_iOverlapMs;
}


// Clears mid sample frame buffer.
void qtractorTimeStretch::clearMidBuffer (void)
{
	if (m_bMidBufferDirty) {
		for (unsigned short i = 0; i < m_iChannels; ++i)
			::memset(m_ppMidBuffer[i], 0, 2 * m_iOverlapLength * sizeof(float));
		m_bMidBufferDirty = false;
	}
}


// Clears input and mid sample frame buffers.
void qtractorTimeStretch::clearInput (void)
{
	m_inputBuffer.clear();
	clearMidBuffer();
}


// Clears all sample frame buffers.
void qtractorTimeStretch::clear (void)
{
	m_outputBuffer.clear();
	m_inputBuffer.clear();

	clearMidBuffer();
}



// Seeks for the optimal overlap-mixing position.
//
// The best position is determined as the position where
// the two overlapped sample sequences are 'most alike',
// in terms of the highest cross-correlation value over
// the overlapping period.
unsigned int qtractorTimeStretch::seekBestOverlapPosition (void) 
{
	double dBestCorr, dCorr;
	unsigned int iBestOffs, iPrevBestOffs;
	unsigned short i, iStep;
	int iOffs, j, k;
	
	// Slopes the amplitude of the 'midBuffer' samples
	calcCrossCorrReference();

	dBestCorr = -1e50; // A reasonable lower limit.

	// Scans for the best correlation value by testing each
	// possible position over the permitted range.
	if (m_bQuickSeek) {
		// Hierachical search...
		iPrevBestOffs = (m_iSeekLength + 1) >> 1;
		iOffs = iBestOffs = iPrevBestOffs; 
		for (iStep = 64; iStep > 0; iStep >>= 2) {
			for (k = -1; k <= 1; k += 2) {
				for (j = 1; j < 4 || iStep == 64; ++j) {
					iOffs = iPrevBestOffs + k * j * iStep;
					if (iOffs < 0 || iOffs >= (int) m_iSeekLength)
						break;
					for (i = 0; i < m_iChannels; ++i) {
						// Calculates correlation value for the mixing
						// position corresponding to iOffs.
						dCorr = (*m_pfnCrossCorr)(
							m_inputBuffer.ptrBegin(i) + iOffs,
							m_ppRefMidBuffer[i],
							m_iOverlapLength);
						// Checks for the highest correlation value.
						if (dCorr > dBestCorr) {
							dBestCorr = dCorr;
							iBestOffs = iOffs;
						}
					}
				}
			}
			iPrevBestOffs = iBestOffs;
		}
	} else {
		// Linear search...
		iBestOffs = 0;
		for (iOffs = 0; iOffs < (int) m_iSeekLength; ++iOffs) {
			for (i = 0; i < m_iChannels; ++i) {
				// Calculates correlation value for the mixing
				// position corresponding to iOffs.
				dCorr = (*m_pfnCrossCorr)(
					m_inputBuffer.ptrBegin(i) + iOffs,
					m_ppRefMidBuffer[i], m_iOverlapLength);
				// Checks for the highest correlation value.
				if (dCorr > dBestCorr) {
					dBestCorr = dCorr;
					iBestOffs = iOffs;
				}
			}
		}
	}

	return iBestOffs;
}


// Processes as many processing frames of the samples
// from input-buffer, store the result into output-buffer.
void qtractorTimeStretch::processFrames (void)
{
	unsigned short i;
	unsigned int j, k;
	float *pInput, *pOutput;
	unsigned int iSkip, iOffset;
	int iTemp;

	// If mid-buffer is empty, move the first
	// frames of the input stream into it...
	if (!m_bMidBufferDirty) {
		// Wait until we've got overlapLength samples
		if (m_inputBuffer.frames() < m_iOverlapLength)
			return;
		m_inputBuffer.receiveFrames(m_ppMidBuffer, m_iOverlapLength);
		m_bMidBufferDirty = true;
	}

	// Process frames as long as there are enough in
	// input-buffer to form a processing block.
	while (m_inputBuffer.frames() >= m_iFramesReq) {
	
		// If tempo differs from the nominal,
		// scan for the best overlapping position...
		iOffset = seekBestOverlapPosition();

		// Mix the frames in the input-buffer at position of iOffset
		// with the samples in mid-buffer using sliding overlapping;
		// first partially overlap with the end of the previous
		// sequence (that's in the mid-buffer).
		m_outputBuffer.ensureCapacity(m_iOverlapLength);
		// Overlap...
		for (i = 0; i < m_iChannels; ++i) {
			pInput = m_inputBuffer.ptrBegin(i);
			pOutput = m_outputBuffer.ptrEnd(i);
			for (j = 0; j < m_iOverlapLength ; ++j) {
				k = m_iOverlapLength - j;
				pOutput[j] = (pInput[j + iOffset] * j
					+ m_ppMidBuffer[i][j] * k) / m_iOverlapLength;
			}
		}
		// Commit...
		m_outputBuffer.putFrames(m_iOverlapLength);

		// Then copy sequence samples from input-buffer to output...
		iTemp = (m_iSeekWindowLength - 2 * m_iOverlapLength);
		if (iTemp > 0) {
			// Temporary mapping...
			for (i = 0; i < m_iChannels; ++i) {
				m_ppFrames[i] = m_inputBuffer.ptrBegin(i)
					+ (iOffset + m_iOverlapLength);
			}
			m_outputBuffer.putFrames(m_ppFrames, iTemp);
		}

		// Copies the end of the current sequence from input-buffer to 
		// mid-buffer for being mixed with the beginning of the next 
		// processing sequence and so on
		// assert(iOffset + m_iSeekWindowLength <= m_inputBuffer.frames());
		m_inputBuffer.readFrames(m_ppMidBuffer, m_iOverlapLength,
			(iOffset + m_iSeekWindowLength - m_iOverlapLength));
		m_bMidBufferDirty = true;

		// Remove the processed samples from the input-buffer. Update
		// the difference between integer & nominal skip step to skip-fract
		// in order to prevent the error from accumulating over time.
		m_fSkipFract += m_fNominalSkip; // real skip size
		iSkip = (int) m_fSkipFract;     // rounded to integer skip
		// Maintain the fraction part, i.e. real vs. integer skip
		m_fSkipFract -= iSkip;       
		m_inputBuffer.receiveFrames(iSkip);
	}
}


// Adds frames of samples into the input of the object.
void qtractorTimeStretch::putFrames ( float **ppFrames, unsigned int iFrames )
{
	// Add the frames into the input buffer.
	m_inputBuffer.putFrames(ppFrames, iFrames);
	// Process the samples in input buffer.
	processFrames();
}


// Output frames from beginning of the sample buffer.
// Copies requested frames output buffer and removes them
// from the sample buffer. If there are less than frames()
// samples in the buffer, returns all that available.
unsigned int qtractorTimeStretch::receiveFrames ( float **ppFrames, unsigned int iFrames )
{
	return m_outputBuffer.receiveFrames(ppFrames, iFrames);
}


// Returns number of frames currently available.
unsigned int qtractorTimeStretch::frames() const
{
	return m_outputBuffer.frames();
}


// Flush any last samples that are hiding in the internal processing pipeline.
void qtractorTimeStretch::flushInput (void)
{
	if (m_bMidBufferDirty) {
		// Prepare a dummy empty buffer...
		unsigned short i;
		float dummy[256];
		::memset(&dummy[0], 0, sizeof(dummy));
		for (i = 0; i < m_iChannels; ++i)
			m_ppFrames[i] = &dummy[0];
		// Push the last active frames out from the pipeline
		// by feeding blank samples into processing until
		// new samples appear in the output...
		unsigned int iFrames = frames();
		for (i = 0; i < 128; ++i) {
			putFrames(m_ppFrames, 256);
			// Any new samples appeared in the output?
			if (frames() > iFrames)
				break;
		}
	}

	clearInput();
}


//---------------------------------------------------------------------------
// Floating point arithmetics specific algorithm implementations.
//

// Slopes the amplitude of the mid-buffer samples
// so that cross correlation is faster to calculate
void qtractorTimeStretch::calcCrossCorrReference (void)
{
	for (unsigned int j = 0 ; j < m_iOverlapLength ; ++j) {
		float fTemp = (float) j * (float) (m_iOverlapLength - j);
		for (unsigned short i = 0; i < m_iChannels; ++i)
			m_ppRefMidBuffer[i][j] = (float) (m_ppMidBuffer[i][j] * fTemp);
	}
}


// Calculates overlap period length in frames and reallocate ref-mid-buffer.
void qtractorTimeStretch::calcOverlapLength (void)
{
	// Must be divisible by 8...
	unsigned int iNewOverlapLength = (m_iSampleRate * m_iOverlapMs) / 1000;
	if (iNewOverlapLength < 16)
		iNewOverlapLength = 16;
	iNewOverlapLength -= (iNewOverlapLength % 8);

	unsigned int iOldOverlapLength = m_iOverlapLength;
	m_iOverlapLength = iNewOverlapLength;
	if (m_iOverlapLength > iOldOverlapLength) {
		unsigned short i;
		if (m_ppFrames) {
			for (i = 0; i < m_iChannels; ++i) {
				delete [] m_ppMidBuffer;
				delete [] m_ppRefMidBufferUnaligned;
			}
			delete [] m_ppMidBuffer;
			delete [] m_ppRefMidBufferUnaligned;
			delete [] m_ppRefMidBuffer;
			delete [] m_ppFrames;
		}
		m_ppFrames = new float * [m_iChannels];
		m_ppMidBuffer = new float * [m_iChannels];
		m_ppRefMidBufferUnaligned = new float * [m_iChannels];
		m_ppRefMidBuffer = new float * [m_iChannels];
		for (i = 0; i < m_iChannels; ++i) {
			m_ppMidBuffer[i] = new float [2 * m_iOverlapLength];
			m_ppRefMidBufferUnaligned[i]
				= new float[2 * m_iOverlapLength + 16 / sizeof(float)];
			// Ensure that ref-mid-buffer is aligned
			// to 16 byte boundary for efficiency
			m_ppRefMidBuffer[i] = (float *)
				((((unsigned long) m_ppRefMidBufferUnaligned[i]) + 15) & -16);
		}
		m_bMidBufferDirty = true;
		clearMidBuffer();
	}
}


// end of qtractorTimeStretch.cpp
