// * DCT modifiée pour calculer toutes les matrices utilisables pour le calcul
// * puis, choix de la bonne matrice en fonction de la taille des données d'entrée
// * Malheureusement on ne peut obtenir sans calcul qu'une matrice de taille n à partir
// * d'une autre de taille n², or ici les différentes tailles nécessaires ne sont pas dans ce cas
// * d'une relation de puissance 2 entre elles. D'où le calcul de toutes les matrices.
// * Résultat : Passage du temps de calcul d'environ 15 ms à environ 3 ou 4 ms (en Release)

// * Pour l'instant, il y a un large choix de matrice (10), utilisées ainsi:
// * Les matrices 0 à 5 permettent de calculer la DCT pour des temps de 50 à 100 ms, pour le mode Release
// * Les matrices 4 à 9 permettent de calculer la DCT pour des temps de 90ms au max (environ 157 ms), pour le mode Debug

// * Attention à la valeur de variables de type 'Somme des coefficients' qui vont probablement avoir des
// * valeurs suffisamment différentes pour fausser la détection silence/parole/souffle.

#include "CDct.h"

const double PI = atan(1.0)*4;
#define SND_VALUE_NEUTRAL 0 // ??


//*******************************************************************************
//* Constructeur
//*******************************************************************************
CDct::CDct()
{
	m_bCoefficientIsAccessible	= false;
	isIdentity					= true;
	isFullProcess				= false;
}

//*******************************************************************************
//* Destructeur
//*******************************************************************************
CDct::~CDct()
{
}

//*******************************************************************************
bool CDct::Liberations()
{
	m_bCoefficientIsAccessible = false;
	return true;
}

//*******************************************************************************
//* Initialisations de la table des bandes de fréquences, des coefficients
//* et de la matrice de la DCT
//*******************************************************************************
bool CDct::Initialisation()
{

	// Par défaut, on considère la taille maximale. Pour l'instant, c'est la taille maximale de ce qui est demandé, soit ( ((SND_SOUNDDATA_SIZE_MAX - SND_HEADER_SIZE_OCTET_TOTAL) >> 2) - 18 )<<2)
	// Rq : on met ici la 'formule longue', plutôt que le #define correspondant : SND_DCT_SIZE_SAMPLE, au cas où on diminue ce SND_DCT_SIZE_SAMPLE, qui serait alors une valeur de départ, mais qui pourrait augmenter.
	memset( m_pfCoeffs,		0, sizeof(m_pfCoeffs) );
	memset( m_pfResultat,	0, sizeof(m_pfResultat) );
	memset( m_pfMatrice,	0, sizeof(m_pfMatrice) );
	memset(m_pfEffectMatrix,0, sizeof(m_pfEffectMatrix));
	*m_pfEffectMatrix = 1.0f; // never touch first element


	m_uNbFreq		= SND_DCT_SIZE_MATRIX_COEFS;
	m_uSampleSize	= SND_DCT_SIZE_MATRIX_COEFS;

	m_uMatrixSize	= ( m_uNbFreq ) * ( m_uSampleSize );
	// Calcul une fois pour toutes de la matrice utilisée pour la DCT
	m_fAlpha1		= sqrt( 1.0f / m_uNbFreq );
	m_fAlpha2		= sqrt( 2.0f / m_uNbFreq ); //sqrt( 1.0f / m_uNbFreq );

	ComputeMatrix( m_pfMatrice , m_uNbFreq, m_uSampleSize );
		// On met les variables globales en paramètres
		// en fait, c'est pour pouvoir utiliser ComputeMatrix avec d'autres valeurs
// Prévoir pour ne mettre que iNumMatrix en paramètre.
// Pour l'instant comme ça pour laisser une insertion sans trop de modifs (1ere étape).

	return true;
}

//*******************************************************************************
//* Pour le calcul de la DCT, en utilisant la formule :
//* C(u) = alpha(u) * sum( f(x)*cos( (PI(2x+1)u)/2N) ) )
//* avec u les fréquences et x les échantillons
//*******************************************************************************
float	CDct::Alpha( UINT uFreq )
{
	if ( uFreq == 0 )		return m_fAlpha1;
	else					return m_fAlpha2;
}

//* Calculer la matrice utilisée pour la DCT ************************************
void CDct::ComputeMatrix( float* pfMatrice, UINT m_uNbFreq, UINT uSampleSize )
{
	float	fValue;
	float*	pfTab = pfMatrice;

	for ( UINT uNumFreq = 0; uNumFreq < m_uNbFreq; uNumFreq++ )
	{
		for ( UINT uIndiceData = 0; uIndiceData < uSampleSize; uIndiceData++, pfTab++ )
		{
			fValue			=	Alpha( uNumFreq );
			float fProduit	=	1.0f;
			fProduit		=	fProduit * PI * (float)(uNumFreq) * ((float)(uIndiceData) + 0.5f);
			fProduit		=	fProduit / (float)uSampleSize;
			fValue			=	fValue * cos( fProduit );
			*pfTab = fValue;
		}
	}
}


UINT CDct::ComputeDCT( float* pfSource, UINT uSourceSize )
{
	float* pfTabSource;
	float* pfTabCoeff;
	float* pfTab;

	BOOL isEnd			= false;
	UINT iterations;

	for (iterations = 0; !isEnd && uSourceSize != 0; iterations++)
	{
		if (uSourceSize < m_uSampleSize)
		{
			// Si la source est plus petite que SND_DCT_SIZE_SAMPLE, on remplit de SND_VALUE_NEUTRAL
			pfTabSource	= pfSource + uSourceSize;
			for( UINT uData = uSourceSize; uData < m_uSampleSize; uData++, pfTabSource++ )
				*pfTabSource = (float)SND_VALUE_NEUTRAL;
			isEnd = true;
		}

		// Calcul des coefficients
		memset( m_pfCoeffs, 0, m_uNbFreq * sizeof(float));

		pfTabSource	= pfSource;
		pfTabCoeff	= m_pfCoeffs;
		pfTab		= m_pfMatrice;
		for ( UINT uNumFreq = 0; uNumFreq < m_uNbFreq; uNumFreq++, pfTabCoeff++ )
		{
			pfTabSource	= pfSource;
			for( UINT uIndiceData = 0; uIndiceData < m_uSampleSize; uIndiceData++, pfTab++, pfTabSource++ )
			{
				*pfTabCoeff +=	( *pfTab ) * ( *pfTabSource );
			}
		}

		memcpy(pfSource, m_pfCoeffs, m_uSampleSize*sizeof(float));
		pfSource	+= m_uSampleSize;
		uSourceSize -= m_uSampleSize;
	}

	m_bCoefficientIsAccessible = true;

	return iterations*m_uSampleSize;
}



//* Inverse de la DCT à partir de la matrice calculée dans les initialisations **************
//* On considère le tableau rempli des coefficients et normalement on a uDestinationSize = m_uNbFreq
UINT CDct::ComputeInverseDCT( float* pfDestination, UINT uDestinationSize )
{
	float* pfTabDestination;
	float* pfTabCoeff;
	float* pfTab;
	float* pfTabResultat;

	BOOL isEnd			= false;
	UINT iterations;

	// Si uDestinationSize est supérieur à m_uNbFreq, on ne tient pas compte du surplus
	// Si uDestinationSize est plus petit, on remplit la table des coefficients de 0

	for (iterations = 0; !isEnd && uDestinationSize != 0; iterations++)
	{
		if (uDestinationSize < m_uNbFreq)
		{
			memcpy(m_pfCoeffs, pfDestination, uDestinationSize*sizeof(float));
			memset(m_pfCoeffs + uDestinationSize, 0, (m_uNbFreq-uDestinationSize)*sizeof(float));
			isEnd = true;
		} else
		{
			memcpy(m_pfCoeffs, pfDestination, m_uNbFreq*sizeof(float));
		}
	
		// On récupère les coefficients stockés dans la destination
		// Calcul effectué en float
		memset( m_pfResultat, 0, m_uSampleSize*sizeof(float));

		pfTabCoeff			= m_pfCoeffs;
		pfTabDestination	= pfDestination;
		pfTab				= m_pfMatrice;
		for ( UINT uNumFreq = 0; uNumFreq < m_uNbFreq; uNumFreq++, pfTabCoeff++ )
		{
			pfTabResultat	= m_pfResultat;
			for( UINT uIndiceData = 0; uIndiceData < m_uSampleSize; uIndiceData++, pfTabResultat++, pfTab++ )
			{
				*pfTabResultat += ( *pfTab ) * ( *pfTabCoeff );
			}
		}

		memcpy(	pfDestination, m_pfResultat, m_uSampleSize*sizeof(float));

		pfDestination	+= m_uSampleSize;
		uDestinationSize-= m_uSampleSize;
	}

	return iterations*m_uSampleSize;
}



#define MIN_AMPLITUDE 0
#define HEAD_UNIT float


UINT CDct::ToBufferSample(unsigned char *dstBuffer, float *srcCoefs, UINT nbElmts)
{
	// first stock coef 0, sp coef ...
	*((HEAD_UNIT *)dstBuffer) = (HEAD_UNIT)(*(srcCoefs++));
	dstBuffer += sizeof(HEAD_UNIT);

	float min, max, amplitude;
	float *l_pfSource;

	l_pfSource = srcCoefs;
	min = max = *(l_pfSource++);
	for (UINT iBuff=2; iBuff<nbElmts; iBuff++,l_pfSource++)
	{
		if (*l_pfSource < min) min = *l_pfSource;
		if (*l_pfSource > max) max = *l_pfSource;
	}
	
	if (max < -min) max = -min;
	if (max < MIN_AMPLITUDE) max = MIN_AMPLITUDE;

	*((HEAD_UNIT *)dstBuffer) = (HEAD_UNIT)max;
	dstBuffer += sizeof(HEAD_UNIT);

	amplitude	= 2*max;
	l_pfSource	= srcCoefs;
	for (UINT iBuff=1; iBuff<nbElmts; iBuff++,l_pfSource++)
	{
		*(dstBuffer++) = (unsigned char)(255*(*l_pfSource + max)/amplitude);
	}
	return (nbElmts-1) + 2*sizeof(HEAD_UNIT);
}

UINT CDct::FromBufferSample(float *dstCoefs, unsigned char *srcBuffer, UINT nbElmts)
{
	if (nbElmts < 2*sizeof(HEAD_UNIT))
		return 0;

	float max, amplitude;

	*(dstCoefs++)	= (float)(*((HEAD_UNIT *)srcBuffer));		srcBuffer += sizeof(HEAD_UNIT);
	max				= (float)(*((HEAD_UNIT *)srcBuffer));		srcBuffer += sizeof(HEAD_UNIT);
	amplitude		= 2*max;

	nbElmts -= 2*sizeof(HEAD_UNIT);
	for (UINT iBuff=0; iBuff<nbElmts; iBuff++,srcBuffer++)
	{
		*(dstCoefs++) = amplitude**srcBuffer / 255.0f - max;
	}
	return (nbElmts+1);
}

UINT CDct::ToBuffer(unsigned char *pucSource, float *m_pfSource, UINT uResult)
{
	UINT uResultOut		= 0;
	UINT uResultLocal	= 0;
	UINT local;

	while (uResult)
	{
		if (uResult > m_uSampleSize)	local = m_uSampleSize;
		else							local = uResult;
		uResultLocal = ToBufferSample(pucSource, m_pfSource, local);
		uResultOut	+=uResultLocal;
		pucSource	+=uResultLocal;
		m_pfSource	+=local;
		uResult		-=local;
	}

	return uResultOut;
}

UINT CDct::FromBuffer(float *m_pfSource, unsigned char *pucSource, UINT uResult)
{
	UINT uResultOut		= 0;
	UINT uResultLocal	= 0;
	UINT local;

	while (uResult) // float ampli and float max
	{
		if (uResult > m_uSampleSize-1+2*sizeof(HEAD_UNIT))	local = m_uSampleSize-1+2*sizeof(HEAD_UNIT);
		else												local = uResult;
		uResultLocal = FromBufferSample(m_pfSource, pucSource, local);
		uResultOut	+=uResultLocal;
		m_pfSource	+=uResultLocal;
		pucSource	+=local;
		uResult		-=local;
	}
	return uResultOut;
}


void CDct::EffectInit()
{
	memset(m_pfEffectMatrix, 0, sizeof(m_pfEffectMatrix));
	*m_pfEffectMatrix = 1.0f;
	isIdentity		= true;
	isFullProcess	= false;
}

void CDct::EffectEnd()
{
	if (isIdentity)
	{
		isFullProcess	= false;
		return;
	}
	
	float m_tmpMatrix[ SND_DCT_SIZE_MATRIX ];
	float *m_tmpMatrixPtr = m_tmpMatrix;
	// multiply : INVDCT * EFFECTS * DCT and stock it in EFFECTS ...
	// step 1 EFFECTS * DCT -> TMP
	memset(m_tmpMatrix, 0, sizeof(m_tmpMatrix));

	for (int dstY=0; dstY<SND_DCT_SIZE_MATRIX_COEFS; dstY++)
	{
		for (int dstX=0; dstX<SND_DCT_SIZE_MATRIX_COEFS; dstX++, m_tmpMatrixPtr++)
		{
			for (int K=0; K<SND_DCT_SIZE_MATRIX_COEFS; K++)
			{
				*m_tmpMatrixPtr += m_pfEffectMatrix[K+dstY*SND_DCT_SIZE_MATRIX_COEFS]*
									m_pfMatrice[dstX+K*SND_DCT_SIZE_MATRIX_COEFS];
//				*m_tmpMatrixPtr += m_pfEffectMatrix[K+dstY*SND_DCT_SIZE_MATRIX_COEFS]*
//									m_pfCoeffs[K+dstX*SND_DCT_SIZE_MATRIX_COEFS];
			}
		}
	}
	// step 2 INVDCT * TMP -> EFFECTS
	m_tmpMatrixPtr = m_pfEffectMatrix;
	memset(m_pfEffectMatrix, 0, sizeof(m_pfEffectMatrix));

	for (int dstY=0; dstY<SND_DCT_SIZE_MATRIX_COEFS; dstY++)
	{
		for (int dstX=0; dstX<SND_DCT_SIZE_MATRIX_COEFS; dstX++, m_tmpMatrixPtr++)
		{
			for (int K=0; K<SND_DCT_SIZE_MATRIX_COEFS; K++)
			{
				*m_tmpMatrixPtr += m_pfMatrice[dstY+K*SND_DCT_SIZE_MATRIX_COEFS]*
									m_tmpMatrix[dstX+K*SND_DCT_SIZE_MATRIX_COEFS];
//				*m_tmpMatrixPtr += m_pfCoeffs[K+dstY*SND_DCT_SIZE_MATRIX_COEFS]*
//									m_tmpMatrix[dstX+K*SND_DCT_SIZE_MATRIX_COEFS];
			}
		}
	}
	isFullProcess	= true;
}

void CDct::EffectNormalize()
{
	float	sum;
	float	*m_pfEffectMatrixPtr, *m_pfEffectMatrixPtrIn;

	sum = 0.0f;
	m_pfEffectMatrixPtr = m_pfEffectMatrix + SND_DCT_SIZE_MATRIX_COEFS;
	for (int y=1; y<SND_DCT_SIZE_MATRIX_COEFS; y++, m_pfEffectMatrixPtr+=SND_DCT_SIZE_MATRIX_COEFS)
	{
		m_pfEffectMatrixPtrIn = m_pfEffectMatrixPtr + 1;
		for (int x=1; x<SND_DCT_SIZE_MATRIX_COEFS; x++, m_pfEffectMatrixPtrIn++)
		{
			sum += *m_pfEffectMatrixPtrIn;
		}
	}
	
	sum /= SND_DCT_SIZE_MATRIX_COEFS-1;

	m_pfEffectMatrixPtr = m_pfEffectMatrix + SND_DCT_SIZE_MATRIX_COEFS;
	if (sum != 0.0f && (abs(sum) < 0.99 || abs(sum) > 1.01))
	{
		for (int y=1; y<SND_DCT_SIZE_MATRIX_COEFS; y++, m_pfEffectMatrixPtr+=SND_DCT_SIZE_MATRIX_COEFS)
		{
			m_pfEffectMatrixPtrIn = m_pfEffectMatrixPtr + 1;
			for (int x=1; x<SND_DCT_SIZE_MATRIX_COEFS; x++, m_pfEffectMatrixPtrIn++)
			{
				*m_pfEffectMatrixPtrIn /= sum;
			}
		}
	}
}


float CDct::EffectMatrixXYGet(int x, int y)
{
	return m_pfEffectMatrix[x+(y<<SND_DCT_SIZE_MATRIX_POWER)];
}

bool CDct::EffectMatrixXYSet(int x, int y, float value)
{
	if (x < 0 || x > SND_DCT_SIZE_MATRIX_COEFS ||
		y < 0 || y > SND_DCT_SIZE_MATRIX_COEFS)
		return false;
	m_pfEffectMatrix[x+(y<<SND_DCT_SIZE_MATRIX_POWER)] = value;
	isIdentity = false;
	return true;
}

void CDct::EffectAddBase(float factor)
{
	for (int i=1; i<SND_DCT_SIZE_MATRIX_COEFS; i++)
		m_pfEffectMatrix[i+(i<<SND_DCT_SIZE_MATRIX_POWER)] += factor;

	isIdentity = false;
}

void CDct::EffectAddStretch(float factor)
{
	float	begin, end, width;
	float	current;
	float	*m_pfEffectMatrixPtr = m_pfEffectMatrix + SND_DCT_SIZE_MATRIX_COEFS;

	for (UINT line=1; line<SND_DCT_SIZE_MATRIX_COEFS; line++, m_pfEffectMatrixPtr+=SND_DCT_SIZE_MATRIX_COEFS)
	{
		begin	= (float) line   /factor;
		end		= (float)(line+1)/factor;

		if (end < 1.0f)							continue;
		if (begin < 1.0f)						begin = 1.0f;
		if (end > SND_DCT_SIZE_MATRIX_COEFS)
			end = SND_DCT_SIZE_MATRIX_COEFS;
			
		width = end - begin;
		if (width <= 0)		continue;

		current = begin;
		while (current < end)
		{
			if ((int)current != (int)end)
			{
				m_pfEffectMatrixPtr[(int)current] += ((int)(current+1) - current)/width;
				current  = (float)((int)(current+1));
			} else
			{
				m_pfEffectMatrixPtr[(int)current] += (end - current)/width;
				current = end;
			}
		}
	}		
	isIdentity = false;
}




void CDct::AlterSample(float *srcCoefs, UINT nbElmts, float neutralValue)
{
	float	*srcCoefsPtr = srcCoefs;
	float	*m_pfEffectMatrixPtr = m_pfEffectMatrix;
	float	*m_pfCoeffsPtr;
	UINT	x,y;

	memset(m_pfCoeffs, 0, sizeof(m_pfCoeffs));	
	
	m_pfCoeffsPtr	= m_pfCoeffs;
	for (y=0; y<m_uNbFreq; y++, m_pfCoeffsPtr++)
	{
		srcCoefsPtr		= srcCoefs;
		for (x=0; x<nbElmts; x++, srcCoefsPtr++, m_pfEffectMatrixPtr++)
		{
			*m_pfCoeffsPtr += *srcCoefsPtr * *m_pfEffectMatrixPtr;
		}
		for (; x<m_uNbFreq; x++, m_pfEffectMatrixPtr++)
		{
			*m_pfCoeffsPtr += neutralValue * *m_pfEffectMatrixPtr;
		}
	}

	memcpy(srcCoefs, m_pfCoeffs, nbElmts*sizeof(float));
}


void CDct::Alter(float *srcCoefs, UINT nbElmts, float neutralValue)
{
	if (isIdentity) return;
	UINT sample = m_uSampleSize;

	while (nbElmts)
	{
		if (sample > nbElmts)
			sample = nbElmts;
		AlterSample(srcCoefs, sample, neutralValue);
		srcCoefs+=sample;
		nbElmts	-=sample;
	}
}