#include <ITAVariableDelayLine.h>

#include <ITAInterpolation.h>
#include <ITAFastMath.h>
#include <ITANumericUtils.h>
#include <ITASampleBuffer.h>
#include <ITAStringUtils.h>
#include <ITAConstants.h>
#include <ITADebug.h>

#include <spline.h>

#include <cassert>
#include <cmath>
#include <iostream>


// Interpolation resampling factor range (otherwise crossfading is used)
#define MIN_RESAMPLING_FACTOR 0.01f // [0, inf) ... aber sinnvoll z.B. 0, 0.1, 0.5 (< 1)
#define MAX_RESAMPLING_FACTOR 25.0f // [0, inf) ... aber sinnvoll z.B. 1, 1.5, 2, 3 (> 1)


// --= VDL =--

CITAVariableDelayLine::CITAVariableDelayLine( const double dSamplerate, const int iBlocklength, const float fReservedMaxDelaySamples, const int iAlgorithm /* = cubic spline */ )
	: m_dSampleRate( dSamplerate )
	, m_iBlockLength( iBlocklength )
	, m_iSwitchingAlgorithm( iAlgorithm )
	, m_psbVDLBuffer( nullptr )
	, m_psbTemp( nullptr )
	, m_bFracDelays( false )
	, m_pInterpolationRoutine( nullptr )
{
	assert( dSamplerate > 0 );
	assert( iBlocklength > 0 );

	// Define size of temp buffer depending on maximum oversampling factor
	int iTempBufferBlockLength = ( ( int ) ceil( MAX_RESAMPLING_FACTOR ) + 1 )*m_iBlockLength;
	m_psbTemp = new ITASampleBuffer( iTempBufferBlockLength, true );

	ReserveMaximumDelaySamples( fReservedMaxDelaySamples );

	m_iFadeLength = std::min( m_iBlockLength, 32 );

	if( m_iSwitchingAlgorithm == LINEAR_INTERPOLATION )
		m_pInterpolationRoutine = new CITASampleLinearInterpolation();

	if( m_iSwitchingAlgorithm == CUBIC_SPLINE_INTERPOLATION )
		m_pInterpolationRoutine = new CITASampleCubicSplineInterpolation();

	if( m_iSwitchingAlgorithm == WINDOWED_SINC_INTERPOLATION )
		m_pInterpolationRoutine = new CITASampleWindowedSincInterpolation();

#if (ITA_DSP_VDL_DATA_LOG == 1)
	m_oDataLog.setOutputFile( "VDL.log" );
#endif

	Clear();
}

void CITAVariableDelayLine::Clear()
{
	m_psbVDLBuffer->Zero();
	m_psbTemp->Zero();

	m_iWriteCursor = 0;
	m_fCurrentDelay = 0;
	m_fNewDelay = 0;
	m_bStarted = false;

	m_swProcess.reset();
	m_swBufferSizeInc.reset();
	m_iNumberOfDropouts = 0;
}

CITAVariableDelayLine::~CITAVariableDelayLine()
{
	delete m_psbVDLBuffer;
	delete m_psbTemp;

	std::string sAddition = "";
	if( m_iSwitchingAlgorithm == SWITCH )
		sAddition = "(Switching)";
	else if( m_iSwitchingAlgorithm == CROSSFADE )
		sAddition = "(Crossfading)";
	else
		sAddition = "(" + m_pInterpolationRoutine->GetName() + ")";

	delete m_pInterpolationRoutine;

	if( m_swProcess.cycles() > 0 )
	{
		DEBUG_PRINTF( " * [VDL] Process time monitor: avg=%.2f us max=%.2f us stddev=%.2f us, dropouts=%i %s\n", m_swProcess.mean()*1e6, m_swProcess.maximum()*1e6, m_swProcess.std_deviation()*1e6, m_iNumberOfDropouts, sAddition.c_str() );
	}
}

int CITAVariableDelayLine::GetAlgorithm() const {
	return m_iSwitchingAlgorithm;
}

void CITAVariableDelayLine::SetAlgorithm( int iAlgorithm ) {
	m_iSwitchingAlgorithm = iAlgorithm;
}

float CITAVariableDelayLine::GetReservedMaximumDelaySamples() const {
	return ( float ) ( m_iVDLBufferSize - m_iBlockLength );
}

float CITAVariableDelayLine::GetReservedMaximumDelayTime() const {
	return GetReservedMaximumDelaySamples() / ( float ) m_dSampleRate;
}

int CITAVariableDelayLine::GetMinimumDelaySamples() const
{
	if( !m_pInterpolationRoutine )
		return 0;

	int iLeft, iRight;
	m_pInterpolationRoutine->GetOverlapSamples( iLeft, iRight );

	return ( iLeft + iRight );
}

float CITAVariableDelayLine::GetMinimumDelayTime() const
{
	return GetMinimumDelaySamples() / ( float ) m_dSampleRate;
}

void CITAVariableDelayLine::ReserveMaximumDelaySamples( float fMaxDelaySamples )
{
	// Festlegung: Die Methode darf nicht parallel betreten werden (non-reentrant)

	assert( fMaxDelaySamples >= 0 ); // Verzögerung immer positiv

	// Anzahl Pufferblöcke bestimmen
	int iNewBufferSize = uprmul( ( int ) ceil( fMaxDelaySamples ), m_iBlockLength );

	assert( iNewBufferSize > 0 ); // Puffer muss eine Größe haben

	if( !m_psbVDLBuffer )
	{

		/*
		 *  Erste Initalisierung des Puffers
		 *  Hinweis: Hier ist keine Synchronisierung erforderlich, da dieser Ast
		 *  nur durch den Konstruktor aufgerufen wird
		 */

		m_psbVDLBuffer = new ITASampleBuffer( iNewBufferSize, true );
		m_iVDLBufferSize = iNewBufferSize;
		m_iWriteCursor = 0;

	}
	else
	{

		// Puffer schon gross genug => Nichts tun...
		if( m_psbVDLBuffer->length() >= iNewBufferSize )
			return;

		m_swBufferSizeInc.start();

		assert( m_psbVDLBuffer->length() >= 0 ); // VDL-Pufferlänge muss eine Größe haben

		m_csBuffer.enter();


		// Alte Puffergröße sichern
		int iOldBufferSize = m_psbVDLBuffer->length();

		// Vorhandene Daten zyklisch Kopieren (aktuelle Schreibposition => Anfang abrollen)
		ITASampleBuffer* psbNewBuffer = new ITASampleBuffer( iNewBufferSize, true );
		psbNewBuffer->cyclic_write( m_psbVDLBuffer, iNewBufferSize, m_iWriteCursor, 0 );

		// Alten Puffer freigeben, neuen zuweisen
		delete m_psbVDLBuffer;
		m_psbVDLBuffer = psbNewBuffer;
		m_iVDLBufferSize = iNewBufferSize;
		m_iWriteCursor = iOldBufferSize; // Hinten anhängend weiterschreiben

		m_csBuffer.leave();

		double t = m_swBufferSizeInc.stop() * 1e6;
		std::cout << "VairableDelayLine: Buffer increment from " << iOldBufferSize << " samples to " << iNewBufferSize << " samples triggered, took " << timeToString( t * 1e-6 );
	}
}

void CITAVariableDelayLine::ReserveMaximumDelayTime( float fMaxDelaySecs )
{
	ReserveMaximumDelaySamples( fMaxDelaySecs * ( float ) m_dSampleRate );
}

bool CITAVariableDelayLine::GetFractionalDelaysEnabled() const {
	return m_bFracDelays;
}

void CITAVariableDelayLine::SetFractionalDelaysEnabled( bool bEnabled )
{
	m_bFracDelays = bEnabled;
}

float CITAVariableDelayLine::GetDelaySamples() const
{
	// Um Konsistenz zu wahren geben wir immer den
	// neuen Verzögerungswert zurück, da es sein kann,
	// dass Getter und Setter für den Delay mehrmals benutzt werden,
	// ohne dass im Process() Schritt der neue Wert als Aktueller
	// übernommen wurde.
	if( m_fCurrentDelay == m_fNewDelay )
		return m_fCurrentDelay;
	else
		return m_fNewDelay;
}

float CITAVariableDelayLine::GetDelaySamples( int& iIntegerDelay, float& fFractionalDelay ) const
{
	// Lokale Kopie der Verzögerung
	float fDelay = GetDelaySamples();

	iIntegerDelay = ( int ) floor( fDelay );
	fFractionalDelay = fDelay - ( float ) iIntegerDelay;
	return fDelay;
}

void CITAVariableDelayLine::SetDelaySamples( float fDelaySamples )
{
	assert( fDelaySamples >= 0 ); // Verzögerung immer positiv

	// Internen Puffer vergrößern
	/* Erfordert die neuen Verzögerung auch eine Puffer-Vergrößerung,
	 * ist es wahrscheinlich, dass bald erneut eine Vergrößerung nötig ist.
	 * Dies ist teuer, deshalb erzwingen wird hier direkt eine deutliche
	 * Vergrößerung.
	 */
	if( m_iVDLBufferSize - m_iBlockLength < ceil( fDelaySamples ) )
		ReserveMaximumDelaySamples( ceil( fDelaySamples ) * 2 );

	// Neuen Wert nicht einfach übernehmen, sondern der
	// Process()-Routine überlassen
	m_fNewDelay = fDelaySamples;

	//DEBUG_PRINTF(" * [VDL] Delay set to %.3f samples\n", fDelaySamples);

	// Falls das Streaming noch nicht gestartet ist sofort die
	// neue Verzögerung übernehmen, sonst wird im ersten Block
	// gleich von 0 auf fDelaySamples umgesetzt und bei
	// Crossfading und Interpolieren gibts unerwünschte Effekte
	if( !m_bStarted )
		m_fCurrentDelay.exchange( m_fNewDelay );
}

void CITAVariableDelayLine::SetDelayTime( float fDelaySecs )
{
	SetDelaySamples( fDelaySecs * ( float ) m_dSampleRate );
}

void CITAVariableDelayLine::Process( const ITASampleBuffer* psbInput, ITASampleBuffer* psbOutput )
{
	m_swProcess.start();

	// Start-Flag setzen
	if( !m_bStarted )
		m_bStarted = true;

	assert( m_iWriteCursor % m_iBlockLength == 0 ); // Schreibcursor immer Vielfaches der Blocklänge

	// Neue Daten in den VDL-Puffer schreiben
	m_psbVDLBuffer->write( psbInput, m_iBlockLength, 0, m_iWriteCursor );

	// Lokale Kopie des gewünschten Algorithmus (Atomare Membervariable)
	int iAlgorithm = m_iSwitchingAlgorithm;

	// Lokale Kopie der neuen Verzögerung
	float fCurrentDelay = m_fCurrentDelay;
	float fNewDelay = m_fNewDelay;

#if (ITA_DSP_VDL_DATA_LOG == 1)
	VDLLogData oLogDataItem;
	oLogDataItem.fCurrentDelay = fCurrentDelay;
	oLogDataItem.fNewDelay = m_fNewDelay;
	oLogDataItem.fResamplingFactor = 1;
	oLogDataItem.iTargetBlockSize = m_iBlockLength;
#endif

	// --= Keine Änderung der Verzögerung (für rasant schnelle statische Szenen) =--

	if( fNewDelay == fCurrentDelay )
	{
		// Keine Änderung der Verzögerung. Einfach Anfang der VDL in den Ausgang kopieren.
		int iReadCursor = ( m_iWriteCursor + m_iVDLBufferSize - ( int ) ceil( fCurrentDelay ) ) % m_iVDLBufferSize;
		psbOutput->cyclic_write( m_psbVDLBuffer, m_iBlockLength, iReadCursor, 0 );

		// Schreibzeiger um Blocklänge vergrößern (BlockPointerIncrement)
		m_iWriteCursor = ( m_iWriteCursor + m_iBlockLength ) % m_iVDLBufferSize;

#if (ITA_DSP_VDL_DATA_LOG == 1)
		oLogDataItem.fProcessingTime = ( float ) ( m_swProcess.stop()*1.0e6 );
		m_oDataLog.log( oLogDataItem );
#endif

		return;
	}


	// --= Änderung der Verzögerung =--

	// Zerlegen in Ganzzahl und Kommazahl
	int iCurrentIntDelay = ( int ) ceil( fCurrentDelay );
	float fCurrentFracDelay = ( float ) iCurrentIntDelay - fCurrentDelay;
	assert( fCurrentFracDelay >= 0.0f );  // Subsample darf nicht negativ sein
	int iNewIntDelay = ( int ) ceil( fNewDelay );
	float fNewFracDelay = ( float ) iNewIntDelay - fNewDelay;
	assert( fNewFracDelay >= 0.0f ); // Subsample darf nicht negativ sein
	int iDeltaDelay = iNewIntDelay - iCurrentIntDelay;

	// Falls Interpolation gewünscht ist, Grenzen prüfen
	if( ( iAlgorithm == LINEAR_INTERPOLATION ) ||
		( iAlgorithm == WINDOWED_SINC_INTERPOLATION ) ||
		( iAlgorithm == CUBIC_SPLINE_INTERPOLATION ) )
	{

		/*
		 * Folgendes Problem kann bei der Abstandsverkleinerung auftreten:
		 * Ist die Abstandsverkleinerung zu schnell, werden Frequenzen so stark gestaucht,
		 * dass sie über die Nyquist-Grenze der Soundkarte hinaus wandern (22.05 kHz) und
		 * bei der Abtastung zu Alias-Fehlern führen. Es muss hierfür das Eingangssignal
		 * entsprechend tiefpass gefiltert werden. Eine synchrone Abtastratenerhöhung
		 * kann auch sinnvoll sein. Diese muss nach der Interpolation erneut tp-gefiltert
		 * und entsprechend runtergesetzt werden.
		 *
		 * Folgendes Problem kann bei der Abstandsvergrößerung auftreten:
		 * Ist die Abstandsvergrößerung sehr schnell, werden im Grenzfall lediglich
		 * einige Stützsamples zur Interpolation vieler Ausgangssamples genutzt,
		 * aber ein systembedingte Grenze existiert bis zur Schallgeschwindigkeit
		 * nicht.
		 *
		 * Kann die Interpolation die entsprechenden Frequenzen nicht nachbilden,
		 * treten Verzerrungen auf, bei der Linearinterpolation nicht-lineare Ver-
		 * zerrungen. Diese Fehler Verstärken sich zunehmend.
		 *
		 */

		// Resamplingfaktor auf dem Eingangsstream bezogen auf eine Blocklänge berechnen
		float fResamplingFactor = 1 - iDeltaDelay / ( float ) m_iBlockLength;

#if (ITA_DSP_VDL_DATA_LOG == 1)
		oLogDataItem.fResamplingFactor = fResamplingFactor;
#endif

		// Wenn Voraussetzungen verletzt werden, für diesen Bearbeitungsschritt auf weiches Umschalten wechseln
		if( ( fResamplingFactor <= MIN_RESAMPLING_FACTOR ) || ( fResamplingFactor > MAX_RESAMPLING_FACTOR ) )
		{
			iAlgorithm = CROSSFADE;
			std::cout << "VariableDelayLine: Forced crossfading, because resampling factor is out of bounds: r=" << fResamplingFactor << "( min = " << MIN_RESAMPLING_FACTOR << ", max = " << MAX_RESAMPLING_FACTOR << " )" << std::endl;
		}
	}


	// --= Leseköpfe =--

	// VDL-Puffergröße wird nur addiert, damit
	int iReadCursorCurrent = ( m_iWriteCursor - iCurrentIntDelay + m_iVDLBufferSize ) % m_iVDLBufferSize;
	int iReadCursorNew = ( m_iWriteCursor - iNewIntDelay + m_iVDLBufferSize ) % m_iVDLBufferSize;
	assert( iReadCursorCurrent >= 0 ); // Cursor darf nicht negativ sein
	assert( iReadCursorNew >= 0 ); // Cursor darf nicht negativ sein


	// --= Umschaltverfahren =--

	// TODO:  - vermutlich sehen die Rümpfe für jede andere Interpolation ähnlich aus, sodass man mit getOverlap() vereinheitlichen kann. 
	//			(LinInterp schon fertig, aber erst Erfahrungen mit den anderen Verfahren sammeln...)

	int iSize;
	int iLeft, iRight; // Überlappung an den Grenzen
	switch( iAlgorithm )
	{

		// o Hartes Umschalten
	case SWITCH:
		// Direkt neue Verzögerung nehmen. Einfach kopieren. Keine Rücksicht nehmen.
		psbOutput->cyclic_write( m_psbVDLBuffer, m_iBlockLength, iReadCursorNew, 0 );

		break;

		// o Umschalten mittels Kreuzblende
	case CROSSFADE:
		// Kreuzblende mittels temporärem Puffer
		assert( m_iFadeLength <= m_psbTemp->length() ); // Zu große Blende für diesen Puffer
		m_psbTemp->cyclic_write( m_psbVDLBuffer, m_iFadeLength, iReadCursorCurrent, 0 );
		psbOutput->cyclic_write( m_psbVDLBuffer, m_iBlockLength, iReadCursorNew, 0 );

		psbOutput->Crossfade( m_psbTemp, 0, m_iFadeLength, ITABase::CrossfadeDirection::FROM_SOURCE, ITABase::FadingFunction::COSINE_SQUARE );

		break;


		// o Umschalten durch Stauchen oder Strecken: Lineare Interpolation, Kubische Spline-Interpolation oder gefensterte Sinc-Interpolation
	default:

		/* Zur Erklärung:

			Verringerung der Verzögerung => Samples stauchen
			(Size > Blocklength), "Zeit" muss eingeholt werden
			Resamplingfaktor > 1

			Vergrößern der Verzögerung => Samples strecken
			(Size < Blocklength), "Zeit" muss gedehnt werden
			Resamplingfaktor < 1
			*/

		// Überlappung an den Grenzen holen
		m_pInterpolationRoutine->GetOverlapSamples( iLeft, iRight );

		// Neuer Abstand
		if( iNewIntDelay < iLeft )
			iSize = m_iBlockLength; // Kausalität erzwingen, wenn der Abstand kleiner als linker Überlappbereich ist
		else
			iSize = iCurrentIntDelay - iNewIntDelay + m_iBlockLength;

#if (ITA_DSP_VDL_DATA_LOG == 1)
		oLogDataItem.iTargetBlockSize = iSize;
#endif

		// Sicherstellen, dass Daten des rechten Überlappungsbereichs vorhanden sind
		// (VDL Puffer sollte immer mindestens eine Blocklänge größer sein als die
		//  aktuelle Verzögerung)
		assert( iLeft < m_iBlockLength ); // Überlapp links bereits größer als Blocklänge
		assert( iRight < m_iBlockLength ); // Überlapp rechts bereits größer als Blocklänge
		assert( m_iVDLBufferSize > iNewIntDelay + iRight ); // Neue Verzögerung und rechter Überlapp größer als VDL Länge

		// "Size" Daten plus Überlappung aus dem Hauptpuffer vom momentanen Lesekopf an
		// den Anfang des Temporärpuffers kopieren
		assert( iLeft + iSize + iRight <= m_psbTemp->length() ); // iTempBufferBlockLength zu klein
		m_psbTemp->cyclic_write( m_psbVDLBuffer, iLeft + iSize + iRight, iReadCursorCurrent - iLeft, 0 );

		// Interpolieren: Temporärpuffer (Größe Size+Offset) --> Output (Größe Blocklänge)
		// Input Offset = iLeft -> effektive Inputlänge = iSize
		// iRight wird implizit durch die Interpolationsroutine verwendet, d.h.
		// bei iInputLength von iLeft+iSize wird auf weitere Daten zugegriffen, die 
		// rechts davon liegen und im Temporärpuffer vorhanden sein müssen!
		int iInputLength = iSize;
		int iInputStartOffset = iLeft;
		m_pInterpolationRoutine->Interpolate( m_psbTemp, iInputLength, iInputStartOffset, psbOutput, m_iBlockLength );

		for( int k = 0; k < psbOutput->GetLength(); k++ )
		{
			if( isnan( psbOutput->GetData()[ k ] ) )
			{
				std::stringstream ss;
				ss << "PANIC! VDL produced NAN value at output sample " << k << std::endl;
				ss << "\t" << "Switching from " + IntToString( iCurrentIntDelay ) + " to " + IntToString( iNewIntDelay ) << std::endl;
				ss << "\t" << "Write cursor: " << m_iWriteCursor << std::endl;
				ss << "\t" << "Read cursor current: " << iReadCursorCurrent << std::endl;
				ss << "\t" << "Read cursor new:     " << iReadCursorNew << std::endl;
				ss << "\t" << "Interpolation routine: " << m_pInterpolationRoutine->GetName() << std::endl;

				int iNANsInInputBuffer = 0;
				for( int k1 = 0; k1 < psbOutput->GetLength(); k1++ )
				{
					if( isnan( m_psbTemp->GetData()[ k ] ) )
						iNANsInInputBuffer++;
				}
				ss << "\t" << "NaN values in input buffer: " << iNANsInInputBuffer << std::endl;

				ITA_EXCEPT1( INVALID_PARAMETER, ss.str().c_str() );
			}
		}

		break;


	} // end case switch

	// Neue Verzögerung speichern
	m_fCurrentDelay = fNewDelay;

	// Schreibzeiger inkrementieren
	// (Hinweis: Der Schreibzeiger ist immer Vielfaches der Blocklänge)
	m_iWriteCursor = ( m_iWriteCursor + m_iBlockLength ) % m_iVDLBufferSize;

	// Zeitnahme
	double t = m_swProcess.stop();
	if( t > m_iBlockLength / m_dSampleRate )
	{
		DEBUG_PRINTF( " * [VDL] Overload, processing took %.2f ms\n", t*1e3 );
		m_iNumberOfDropouts++;
	}

#if (ITA_DSP_VDL_DATA_LOG == 1)
	oLogDataItem.fProcessingTime = ( float ) ( t*1e6 );
	m_oDataLog.log( oLogDataItem );
#endif

	return;
}

#if (ITA_DSP_VDL_DATA_LOG == 1)
std::ostream& CITAVariableDelayLine::VDLLogData::outputDesc( std::ostream& os )
{
	os	<< "Current delay" << "\t" 
		<< "New delay" << "\t" 
		<< "Resampling factor" << "\t" 
		<< "Target block size" << "\t" 
		<<  "Processing time [us]" <<  std::endl;
	return os; 
}

std::ostream& CITAVariableDelayLine::VDLLogData::outputData(std::ostream& os) const {
	os << fCurrentDelay << "\t" << fNewDelay << "\t" << fResamplingFactor << "\t" << iTargetBlockSize << "\t" << fProcessingTime << std::endl;
	return os; 
}
#endif

float CITAVariableDelayLine::GetDelayTime() const
{
	return m_fCurrentDelay;
}

float CITAVariableDelayLine::GetNewDelayTime() const
{
	return float( m_fNewDelay / m_dSampleRate );
}