/*
 *  TunerUnit.cpp
 *
 *
 *  Created by Rémi Thébault on 26/11/07.
 *  Copyright 2007 Eclipse Audio. All rights reserved.
 *
 */

#include <math.h>
#include <cstdlib>
#include <string>
#include <fftw3.h>

#include <ladspa.h>

#define TU_PITCH	0
#define TU_INPUT	1
#define TU_OUTPUT	2

/*Internal Statics Parameters*/

#define TU_A_Freq 		440.0f			// A frequency in Hz
#define	TU_FirstFreqRes	50.0f			// Mininmal resolution for the first frequency estimation in Hz
#define TU_MinFreq  	55.0f		    // Minimal Frequency that the Tuner can read in Hz
										// This will determine the Window size

typedef struct
{
	LADSPA_Data mSampleRate;
	LADSPA_Data mTwoPiOverSampleRate;

	// Internal plugin data

	LADSPA_Data*    mWindow;
	float			mFreqEstimation;

	float*			mFFTWindow;
	fftwf_complex*	mFFTSpectrum;

	fftwf_plan		mPlan;


	// Some state variables

	unsigned long	mWindowSize;
	unsigned long	mIndex;
	unsigned long	mFFTSize;
	unsigned long	mFFTIndex;

	bool			mFrequencyEstimated;


  /* Member functions */

    void	findPitch();
	void	estimateFrequency();
  /* Ports:
     ------ */

	LADSPA_Data* mPitch;
	LADSPA_Data* mInput;
	LADSPA_Data* mOutput;

} TunerUnit;



LADSPA_Handle
instantiateTunerUnit(	const LADSPA_Descriptor * Descriptor,
						unsigned long             SampleRate	)
{
	TunerUnit* psTuner;

	psTuner = (TunerUnit *)malloc(sizeof(TunerUnit));

	if (psTuner) {

		// Calculation of the FFT Buffer Size
		unsigned int FFTSize = 2;
		while (SampleRate/(float)FFTSize >= TU_FirstFreqRes/2.0f)
			FFTSize *= 2;

		// Calculation of the Window Size from the Minimal Frequency
		unsigned int WS, minWS;
		minWS = (unsigned int) (2 * (1/(float)TU_MinFreq) * SampleRate + 1);

		WS = 2;
		while ((WS < minWS) || (WS < FFTSize))
			WS *= 2;


		psTuner->mWindowSize	= WS;
		psTuner->mFFTSize		= FFTSize;
		psTuner->mIndex	= 0;
		psTuner->mFFTIndex = 0;
		psTuner->mSampleRate	= (LADSPA_Data)SampleRate;
		//psTuner->mTwoPiOverSampleRate = (2 * M_PI) / (LADSPA_Data)SampleRate;
		psTuner->mFrequencyEstimated = false;

        psTuner->mWindow		= (LADSPA_Data *) malloc (WS * sizeof(LADSPA_Data));

		psTuner->mFFTWindow		= (float *) malloc (FFTSize * sizeof(float));
		psTuner->mFFTSpectrum	= (fftwf_complex *) malloc (FFTSize * sizeof(fftwf_complex));
		psTuner->mPlan			= fftwf_plan_dft_r2c_1d	(	FFTSize,
															psTuner->mFFTWindow,
															psTuner->mFFTSpectrum,
															FFTW_ESTIMATE
														) ;
	}

	return psTuner;
}



void
activateTunerUnit(LADSPA_Handle Instance)
{
	TunerUnit * psTuner;
	psTuner = (TunerUnit *)Instance;

	if (psTuner)
	{
		psTuner->mIndex = 0;
		psTuner->mFFTIndex = 0;
		psTuner->mFrequencyEstimated = false;

	}
}

void
connectPortToTunerUnit	(	LADSPA_Handle Instance,
							unsigned long Port,
							LADSPA_Data * DataLocation
						)
{
	TunerUnit * psTuner;

	psTuner = (TunerUnit *)Instance;

	switch (Port) {
	case TU_PITCH:
		psTuner->mPitch = DataLocation;
		break;
	case TU_INPUT:
		psTuner->mInput = DataLocation;
		break;
	case TU_OUTPUT:
		psTuner->mOutput = DataLocation;
		break;
  }
}

void
TunerUnit::findPitch ()
{

	float* Corr;			// AutoCorrelation function
    float* Dt;				// time domain derivation of the Autocorrelated function


	unsigned int halfWin;
	unsigned int begin, end;


	begin	= (unsigned int) (mSampleRate /
                (mFreqEstimation + (mSampleRate / (LADSPA_Data)mFFTSize)) );
	end		= (unsigned int) (mSampleRate /
                (mFreqEstimation - (mSampleRate / (LADSPA_Data)mFFTSize)) );

	Corr	= (float*) malloc ((end-begin) * sizeof(float));
	Dt		= (float*) malloc ((end-begin) * sizeof(float));

	Dt[0] = 0.f;

	unsigned int ti;		// Time domain index

	for (int tau = begin; tau <= end; tau++)
	{
        int i = tau - begin;

		for (int t = 0; t < halfWin; t++)
			Corr[i] += (float)mWindow[t] * (float)mWindow[t+tau];

		if (i>0)
		{
			Dt[i]=Corr[i]-Corr[i-1];
			if( (Dt[i] < 0) && (Dt[i-1] >= 0) )
			{
				ti = tau;
				break;
			}
		}
	}

    free(Corr);
    free(Dt);

	// Calcul de la fréquence liée à l'indice

	*mPitch = mSampleRate / (LADSPA_Data)ti;


	/* Extraction du Pitch et de NoteID
	NoteID est la partie réelle de noteNPitch
	Pitch est sa partie decimale
	noteNPitch = NoteID + pitch		avec -0.5 < pitch <= 0.5  */

	/*while(freq < TU_A_Freq)
		freq *= 2;

	while(freq >= TU_A_Freq*2)								// transposition of the real note frequency between 440 and 880
	//	freq /= 2.0;

/*	float noteNPitch = 12 * log(freq) / (log(TU_A_Freq) + log(TU_A_Freq*2));

	if (noteNPitch > 11.5) noteNPitch -= 12;

	unsigned short note = 0;

	while ( noteNPitch - note > 0.5)
		note++ ;

	LADSPA_Data pitch = noteNPitch - note;

	/* Affectation to output controls */
	/*mPitch = &pitch;
	mNoteID = (LADSPA_Data*) &note;*/
}


void
TunerUnit::estimateFrequency()
{
	float re, im, max, module2;
	unsigned int ind;

	max = 0.f;
	ind = 0;

	for (unsigned int i = 0; i < mFFTSize; i++)
	{
		re = mFFTSpectrum[i][0];
		im = mFFTSpectrum[i][1];
		module2 = re*re + im*im;
		if (max < module2)
		{
			max = module2;
			ind = i;
		}
	}

	mFreqEstimation = ind * mSampleRate / (float)mFFTSize;

}

void
runTunerUnit (	LADSPA_Handle Instance,
				unsigned long SampleCount	)
{
	LADSPA_Data * pfInput;
	LADSPA_Data * pfOutput;

	TunerUnit * psTuner;
	unsigned long lSampleIndex;

	psTuner = (TunerUnit*) Instance;


	pfInput = psTuner->mInput;
	pfOutput = psTuner->mOutput;


	for(	lSampleIndex = 0;
			lSampleIndex < SampleCount;
			lSampleIndex++)
	{
		unsigned long windowIndex = lSampleIndex + psTuner->mIndex;
		unsigned long FFTIndex = lSampleIndex + psTuner->mFFTIndex;

		if ( windowIndex < psTuner->mWindowSize)
			psTuner->mWindow[windowIndex] = *pfInput;

		if ( (!psTuner->mFrequencyEstimated) && (FFTIndex < psTuner->mFFTSize) )
			psTuner->mFFTWindow[FFTIndex] = *pfInput;

		*(pfOutput++) = *(pfInput++);
	}


	if (! psTuner->mFrequencyEstimated)
	{
		psTuner->mFFTIndex += SampleCount;
		if(psTuner->mFFTIndex > psTuner->mFFTSize)
		{
			fftwf_execute(psTuner->mPlan);
			psTuner->estimateFrequency();
			psTuner->mFrequencyEstimated = true;
		}
	}

	psTuner->mIndex += SampleCount ;

	if ( (psTuner->mIndex >= psTuner->mWindowSize) && psTuner->mFrequencyEstimated)
	{
	    psTuner->findPitch();
		psTuner->mIndex = 0;
		psTuner->mFrequencyEstimated = false;
	}

}

void
cleanupTunerUnit(LADSPA_Handle Instance)
{
	{
		TunerUnit* psTuner;
		psTuner = (TunerUnit*)Instance;
		free(psTuner->mWindow);
		free(psTuner->mFFTWindow);
	}

	free(Instance);
}


LADSPA_Descriptor * g_psTUDescriptor = NULL;

void
_init()
{
	char ** pcPortNames;
	LADSPA_PortDescriptor * piPortDescriptors;
	LADSPA_PortRangeHint * psPortRangeHints;

	g_psTUDescriptor
		= (LADSPA_Descriptor *)malloc(sizeof(LADSPA_Descriptor));

	if (g_psTUDescriptor != NULL)
	{
		g_psTUDescriptor->UniqueID
			= 5077;
		g_psTUDescriptor->Label
			= strdup("tner");
		g_psTUDescriptor->Properties
			= LADSPA_PROPERTY_HARD_RT_CAPABLE;
		g_psTUDescriptor->Name
			= strdup("Universal Digital Tuner Unit");
		g_psTUDescriptor->Maker
			= strdup("Remi Thebault");
		g_psTUDescriptor->Copyright
			= strdup("None");

		g_psTUDescriptor->PortCount
			= 3;
		piPortDescriptors
			= (LADSPA_PortDescriptor *) calloc (3, sizeof(LADSPA_PortDescriptor));
		g_psTUDescriptor->PortDescriptors
			= (const LADSPA_PortDescriptor *)piPortDescriptors;
		piPortDescriptors[TU_PITCH]
			= LADSPA_PORT_OUTPUT | LADSPA_PORT_CONTROL;
		piPortDescriptors[TU_INPUT]
			= LADSPA_PORT_INPUT | LADSPA_PORT_AUDIO;
		piPortDescriptors[TU_OUTPUT]
			= LADSPA_PORT_OUTPUT | LADSPA_PORT_AUDIO;

		pcPortNames
			= (char **)calloc(3, sizeof(char *));
		g_psTUDescriptor->PortNames
			= (const char **)pcPortNames;
		pcPortNames[TU_PITCH]
			= strdup("Pitch");
		pcPortNames[TU_INPUT]
			= strdup("Input");
		pcPortNames[TU_OUTPUT]
			= strdup("Output");

		psPortRangeHints = ((LADSPA_PortRangeHint *)
				calloc(3, sizeof(LADSPA_PortRangeHint)));
		g_psTUDescriptor->PortRangeHints
			= (const LADSPA_PortRangeHint *)psPortRangeHints;
		psPortRangeHints[TU_PITCH].HintDescriptor
			= (LADSPA_HINT_BOUNDED_BELOW
			| LADSPA_HINT_BOUNDED_ABOVE);
		psPortRangeHints[TU_PITCH].LowerBound
			= 0.0;
		psPortRangeHints[TU_PITCH].UpperBound
			= 1700.0;
		psPortRangeHints[TU_INPUT].HintDescriptor
			= 0;
		psPortRangeHints[TU_OUTPUT].HintDescriptor
			= 0;

		g_psTUDescriptor->instantiate
			= instantiateTunerUnit;
		g_psTUDescriptor->connect_port
			= connectPortToTunerUnit;
		g_psTUDescriptor->activate
			= activateTunerUnit;
		g_psTUDescriptor->run
			= runTunerUnit;
		g_psTUDescriptor->run_adding
			= NULL;
		g_psTUDescriptor->set_run_adding_gain
			= NULL;
		g_psTUDescriptor->deactivate
			= NULL;
		g_psTUDescriptor->cleanup
			= cleanupTunerUnit;
  }

}

void
deleteDescriptor(LADSPA_Descriptor * psDescriptor)
{
	unsigned long lIndex;
	if (psDescriptor) {
		free((char *)psDescriptor->Label);
		free((char *)psDescriptor->Name);
		free((char *)psDescriptor->Maker);
		free((char *)psDescriptor->Copyright);
		free((LADSPA_PortDescriptor *)psDescriptor->PortDescriptors);
		for (lIndex = 0; lIndex < psDescriptor->PortCount; lIndex++)
			free((char *)(psDescriptor->PortNames[lIndex]));
		free((char **)psDescriptor->PortNames);
		free((LADSPA_PortRangeHint *)psDescriptor->PortRangeHints);
		free(psDescriptor);
  }
}

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

/* _fini() is called automatically when the library is unloaded. */
void
_fini() {
  deleteDescriptor(g_psTUDescriptor);
}

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

/* Return a descriptor of the requested plugin type. There is one
   plugin type available in this library. */
const LADSPA_Descriptor *
ladspa_descriptor(unsigned long Index) {
  /* Return the requested descriptor or null if the index is out of
     range. */
  switch (Index) {
  case 0:
    return g_psTUDescriptor;
  default:
    return NULL;
  }
}