#define AVNEEDFLOAT
#include "avtypes.h"
#include "averror.h"
#include "avrtl.h"
#include "math.h"
#include "aeclib.h"

#define LAMBDA 0.6
#define pTHIS pObj

/*************************************************************************/
/* Average Magnitude Estimation 
/* Rekursive Schaetzung der mittleren Amplitude
/*************************************************************************/
void AECame(
FLOAT32 *pOut,		/* Ausgabevektor */
FLOAT32 *pIn,		/* Eingabevektor */
INT32 len,			/* Laenge der Vektoren */   
FLOAT32 a_rf,		/* Anstiegs-und Abfallzeitkonstante */ 
FLOAT32 *ic)		/* Initial Condition Anfangswert */
{
	INT32 i;
	register FLOAT32 arf2;
	
	arf2 = (FLOAT32)(1.0-a_rf);

	pOut[0] = a_rf*(FLOAT32)fabs(pIn[0]) + arf2* *ic;

	for (i=1; i < len; i++)
		pOut[i] = a_rf*(FLOAT32)fabs(pIn[i]) + arf2*pOut[i-1];

	*ic = pOut[len-1];
}

/*************************************************************************/
/* Average Magnitude Estimation 
/* Rekursive Schaetzung der mittleren Amplitude
/*************************************************************************/
void AECame2(FLOAT32 *pOut, FLOAT32 *pIn, INT32 len, FLOAT32 a_r, FLOAT32 a_f, FLOAT32 *ic)
{
	INT32 i;
	register FLOAT32 ar2, af2;
	
	ar2 = (FLOAT32)(1.0-a_r);
	af2 = (FLOAT32)(1.0-a_f);

	if (pIn[0] > *ic)
		pOut[0] = a_r*(FLOAT32)fabs(pIn[0]) + ar2* *ic;
	else
		pOut[0] = a_f*(FLOAT32)fabs(pIn[0]) + af2* *ic;

	for (i=1; i < len; i++)
	{
		if (pIn[i] > pOut[i-1])
			pOut[i] = a_r*(FLOAT32)fabs(pIn[i]) + ar2 * pOut[i-1];
		else
			pOut[i] = a_f*(FLOAT32)fabs(pIn[i]) + af2 * pOut[i-1];
	}

	*ic = pOut[len-1];
}

/*************************************************************************/
/* Average Background Noise Estimation 
/* Rekursive Schaetzung des Hintergrundgeraeuschs (entferntes Ende)
/*************************************************************************/
void AECXnoise_PH(
FLOAT32 *pOut,
FLOAT32 *pTalk,
FLOAT32 *pIn,
FLOAT32 *pInAme,
INT32 len,
FLOAT32 a_r,
FLOAT32 kth,
FLOAT32 *ic)
{
	INT32 i;
	register FLOAT32 ar2;
	
	ar2 = (FLOAT32)(1.0-a_r);
	
	if ((pTalk[0]=(FLOAT32)(pInAme[0] > (kth* *ic))))
		pOut[0] = *ic;
	else
		pOut[0] = a_r*(FLOAT32)fabs(pIn[0]) + ar2* *ic;

	for (i=1; i < len; i++)
	{
		if ((pTalk[i]=(FLOAT32)(pInAme[i] > (kth*pOut[i-1]))))
			pOut[i] = pOut[i-1];
		else
			pOut[i] = a_r*(FLOAT32)fabs(pIn[i]) + ar2 * pOut[i-1];
	}
	*ic = pOut[len-1];
}

/*************************************************************************/
/* Average Background Noise Estimation 
/* Rekursive Schaetzung des Hintergrundgeraeuschs (nahes Ende)
/*************************************************************************/
void AECDnoise_PH(
FLOAT32 *pNL,
FLOAT32 *pTalk,
FLOAT32 *pE,
FLOAT32 *pXS,
FLOAT32 *pXL,
FLOAT32 *pES,
FLOAT32 *pEL,
INT32 len,
FLOAT32 a_f,
FLOAT32 PL,
FLOAT32 *ic)
{
	INT32 i;
	register FLOAT32 af2;
	
	af2 = (FLOAT32)(1.0-a_f);
	
	if ((pTalk[0]=(FLOAT32)((pXS[0] > (PL*pXL[0])) && (pES[0] > (PL*pEL[0])))))
		pNL[0] = *ic;
	else
		pNL[0] = a_f*(FLOAT32)fabs(pE[0]) + af2* *ic;

	for (i=1; i < len; i++)
	{
		if ((pTalk[i]=(FLOAT32)((pXS[i] > (PL*pXL[i])) && (pES[i] > (PL*pEL[i])))))
			pNL[i] = pNL[i-1];
		else
			pNL[i] = a_f*(FLOAT32)fabs(pE[i]) + af2 * pNL[i-1];
	}
	*ic = pNL[len-1];
}


/*************************************************************************/
/* Average Power Estimation 
/* Rekursive Schaetzung der mittleren Leistung
/*************************************************************************/
void AECape(FLOAT32 *pOut, FLOAT32 *pIn, INT32 len, FLOAT32 a_rf, FLOAT32 *ic)
{
	INT32 i;
	register FLOAT32 arf2;
	
	arf2 = (FLOAT32)(1.0-a_rf);

	pOut[0] = a_rf*pIn[0]*pIn[0] + arf2* *ic;

	for (i=1; i < len; i++)
		pOut[i] = a_rf*(FLOAT32)pIn[i]*pIn[i] + arf2*pOut[i-1];

	*ic = pOut[len-1];
}

/*************************************************************************/
/* Normalized Correlation 
/* Rekursive Schaetzung der mittleren Leistung
/* Variante nach Peter Heitkämper
/*************************************************************************/
void AECnormXcorr_PH(FLOAT32 *pOut, FLOAT32 *pInX, FLOAT32 *pInY, UINT32 nLags, UINT32 len)
{
	FLOAT32 sumMag;
	FLOAT32 sum;
	UINT32 i, j;

	for (i=0; i < nLags; i++)
	{
		sum = 0;
		sumMag = 0;
		for (j=nLags; j < len; j++)
		{
			sum += pInX[j-i]*pInY[j];
			sumMag += (FLOAT32)fabs(pInX[j-i]*pInY[j]);
		}
		pOut[i] = (FLOAT32)(fabs(sum)/(sumMag+0.001));
	}
}

/*************************************************************************/
/* Normalized Correlation 
/* Rekursive Schaetzung der mittleren Leistung
/* Variante aus der Statistik (nach Papula)
/*************************************************************************/
void AECnormXcorr_LP(FLOAT32 *pOut, FLOAT32 *pInX, FLOAT32 *pInY, UINT32 nLags, UINT32 len)
{
	FLOAT32 sum;
	FLOAT32 sumX, sumY;
	UINT32 i, j;

	for (i=0; i < nLags; i++)
	{
		sum = 0;
		sumX = 0;
		sumY = 0;
		for (j=nLags; j < len; j++)
		{
			sumX += pInX[j-i]*pInX[j-i];
			sumY += pInY[j]*pInY[j];
			sum += (FLOAT32)(pInX[j-i]*pInY[j]);
		}
		pOut[i] = (FLOAT32)((sum*sum)/(sumX*sumY+0.001));
	}
}

/*************************************************************************/
/* AEC
/* 
/*************************************************************************/
AVERR AECInit(
AEC *pObj,               /* Zeiger auf Objekt                          */
UINT32 N,                  /* Filterlaenge N                             */
UINT32 P,                  /* Anzahl Filterpartitionen                   */
UINT32 S,                  /* Anzahl Filtersegmente pro Partition        */
UINT32 L,                  /* Laenge der Eingangsdaten (Blocklaenge      */
UINT32 C                   /* FFT-Laenge                                 */ 
)  
{  
  UINT32 i, s, NP, C_L;
  REALBUF *pMu;            /* Temporaerer Zeiger fuer Bufferinit.*/

  /* PFFT initialisieren */
  pObj->pFilter = (PFFT*)AvMemAlloc(sizeof(PFFT));
  PfftInit(pObj->pFilter, N, P, S, L, C);
  
  /* Objekt initialisieren */
  pObj->L = pObj->pFilter->L;
  pObj->N = pObj->pFilter->N;
  pObj->P = pObj->pFilter->P;
  pObj->S = pObj->pFilter->S;
  pObj->C = pObj->pFilter->C;
  NP  = pObj->S * pObj->L;
  C_L = pObj->C - pObj->L;

  /*-------------------------------------------------------*/
  /* Speicher allokieren */
  /*-------------------------------------------------------*/
  /* Speicher fuer Mu[P*S][C] (reell) */
  pObj->pBufMu = (REALBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(REALBUF));
 
  for (i=0; i < pObj->P*pObj->S; i++)
  {
    pObj->pBufMu[i].pData = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
    AvZeroMem(pObj->pBufMu[i].pData,pObj->C*sizeof(FLOAT32));
    pObj->pBufMu[i].pNext = &pObj->pBufMu[i+1];
    pObj->pBufMu[i].pLast = &pObj->pBufMu[i-1];
    pObj->pBufMu[i].pLastPS = NULL;
    pObj->pBufMu[i].user  = i; 
  }
  pObj->pBufMu[i-1].pNext = &pObj->pBufMu[0];
  pObj->pBufMu[0].pLast   = &pObj->pBufMu[i-1];

  /* Zeiger auf Mu[k-p*S] */
  for (i=0; i < pObj->P*pObj->S; i++)
  {
    pMu = pObj->pBufMu[i].pLast;
	for (s=1; s < pObj->S; s++)
	  pMu = pMu->pLast;

    pObj->pBufMu[i].pLastPS = pMu;
  }

  /* Speicher fuer PX[C] (reell) */
  pObj->pBufPX = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
  AvZeroMem(pObj->pBufPX,pObj->C*sizeof(FLOAT32));

  /* Ergebnis 'Y' der Faltung (komplex) */
  pObj->pTemp = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX));
  pObj->pTemp->pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
  pObj->pTemp->pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
  AvZeroMem(pObj->pTemp->pReal,pObj->C*sizeof(FLOAT32));
  AvZeroMem(pObj->pTemp->pImag,pObj->C*sizeof(FLOAT32));
  
  /* Speicher fuer AGC-Gain allokieren */
  pObj->pAgcGain = (FLOAT32*)AvMemAlloc(pObj->L*sizeof(FLOAT32));

  /* FFT initialisieren */
  pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT));
  FFTinit(pObj->pFFT, pObj->C);

  /* Speicher fuer WS[P][C] (komplex) zuweisen */
  PfftFilterAlloc(pObj->pFilter, &pObj->pBufWS);

  /* Arbeitszeiger initialisieren */
  pObj->pX  = pObj->pFilter->pBufX;
  pObj->pPj = pObj->pBufWS;
  pObj->pMu = pObj->pBufMu;
  
  /* Sicherheitskonstannte vermeidet Division durch Null */
  pObj->gamma = (FLOAT32)1.0/pObj->C;

  return AV_E_OK;
}

/*-----------------------------------------------------------------------*/
/* Partitioned FLMS Filterinitialisierung
/* 1. Partitionierung der Filterstartwerte in P-Partitionen 
/* 2. Transformation der P Teil-Filter in den Frequenzbereich
/*    Element pBufWS wird veraendert
/*-----------------------------------------------------------------------*/
AVERR AECInitFilter(
AEC *pObj,               /* Zeiger auf Objekt                          */           
FLOAT32 *pWTD)             /* N Filterkoeffizienten im Zeitbereich       */ 
{
  return PfftFilterInit(pObj->pFilter, pWTD, pObj->pBufWS);
}
  
/*-----------------------------------------------------------------*/
/* Partitioned FLMS
/*-----------------------------------------------------------------*/
AVERR AECcancel(
AEC *pObj,                 /* Zeiger auf Objekt                    */
AGC *pAGCobj,              /* Zeiger auf AGC Objekt                */
FLOAT32 *pInTDx,           /* Ein: Daten x[0..L-1]                 */
FLOAT32 *pInTDd,           /* Ein: Referenzsignal d[0..L-1]        */
FLOAT32 *pOutTDy,          /* Aus: Filterausgang y[0..L-1]         */
FLOAT32 *pOutTDe,          /* Aus: Fehlersignal e[0..L-1]          */ 
FLOAT32 alpha)
{

  UINT32 i, p, NP, C_L;
  
  CMPXBUF *pX, *pWS, *pPj;
  REALBUF *pMu;
  COMPLEX *pTemp;
  FLOAT32 muNom, gain;

  /* Arbeitszeiger */
  pX  = pObj->pX;          /* Aktueller Zeiger X[k]                */
  pWS = pObj->pBufWS;      /* Aktueller Zeiger WS[0]               */
  pPj = pObj->pPj;         /* Aktueller Zeiger Pj[]                */
  pMu = pObj->pMu;         /* Aktueller Zeiger Mu[k]               */
  pTemp = pObj->pTemp;     /* 'Y' wird nach der Filterung als
                               temporaerer Speicher benutzt        */

  NP = pObj->S*pObj->L;    /* N/P = S*L                            */
  C_L = pObj->C - pObj->L; /* C-L                                  */

  /*---------------------------------------------------------------*/
  /* Partitioned FFT-Filterung
  /*---------------------------------------------------------------*/
  /* TODO: PFFT Kommentar */
  PfftFilter(pObj->pFilter, pObj->pBufWS, pInTDx, pOutTDy);
  
  /*---------------------------------------------------------------*/
  /* Fehlersignal 'e = d - y' berechnen                            */
  /* Transformation von 'e' in den Frequenzbereich                 */         
  /*---------------------------------------------------------------*/
  for (i=0; i < pObj->L; i++)
    pOutTDe[i] = pInTDd[i] - pOutTDy[i]; 

  /* Variable Y wird fuer E missbraucht, da nicht mehr gebraucht   */
  /* Imaginaerteil von 'Y' auf Null setzen Im{Y[0..C-1]} = {0}     */

  AvZeroMem(pTemp->pImag,pObj->C*sizeof(FLOAT32));
  
  /* Re{Y[0..C-L-1]} = {0}                                           */
  AvZeroMem(pTemp->pReal,C_L*sizeof(FLOAT32));

  /* Re{Y[C-L..C-1]} = e[0..L-1]                                   */
  AvMemCpy(&pTemp->pReal[C_L],
            pOutTDe,pObj->L*sizeof(FLOAT32));
 
  if (pAGCobj != NULL)
  {
	  (*pAGCobj->pAGCfunc)(pAGCobj, pInTDx, pInTDd, pOutTDy, pOutTDe,
	                          pObj->pAgcGain, pObj->L);
  
  /* Gewichtung des Fehlersignal mit AGC-Gain */
  for (i=0; i < pObj->L; i++)
    pTemp->pReal[C_L+i] *= pObj->pAgcGain[i]; 
  
  }

  /* E = 1/C*fft{e[0[0..C-L-1],e[0..L]}                            */
  ffts(pObj->pFFT, pTemp->pReal, pTemp->pImag);

  /*---------------------------------------------------------------*/
  /* Berechnung PX und Mu
  /*---------------------------------------------------------------*/
  /* Zaehler Alpha*Gamma/P                                         */
  muNom = (FLOAT32)(alpha*pObj->gamma/pObj->P);

  for (i=0; i < pObj->C; i++)
  {
    /* Schaetzung der mittleren Eingangsleistung PX                */
    pObj->pBufPX[i] = (FLOAT32)fabs((1.0-LAMBDA) * pObj->C 
		            * (pX->cmpxData.pReal[i]*pX->cmpxData.pReal[i]
		            +  pX->cmpxData.pImag[i]*pX->cmpxData.pImag[i])
					+  LAMBDA*pObj->pBufPX[i]);

    /* Berechnung der variablen Schrittweite Mu                    */
	/* mu[k] = (Alpha*Gamma) / (P*(PX+Gamma))                      */
	pObj->pMu->pData[i] = muNom / (pObj->pBufPX[i] + pObj->gamma);
  }
  
  /* Aktualisierung der Filterkoeffizienten                        */
  for (p=0; p < pObj->P; p++)
  {
	for (i=0; i < pObj->C; i++)
    {
      gain = pMu->pData[i] * pObj->C;
	  pWS->cmpxData.pReal[i] += ((pX->cmpxData.pReal[i] * pTemp->pReal[i]
	                          +   pX->cmpxData.pImag[i] * pTemp->pImag[i])
						      *   gain);
      pWS->cmpxData.pImag[i] += ((pX->cmpxData.pReal[i] * pTemp->pImag[i]
	                          -   pX->cmpxData.pImag[i] * pTemp->pReal[i])
						      *   gain);
	}
    pWS = pWS->pNext;
	pX = pX->pLastPS;
	pMu= pMu->pLastPS;
  }

  /* Effiziente Projektion der Filterkoeffizienten                 */
  ifft(pObj->pFFT, pPj->cmpxData.pReal, pPj->cmpxData.pImag);
  AvZeroMem(pPj->cmpxData.pImag, pObj->C*sizeof(FLOAT32));
  AvZeroMem(&pPj->cmpxData.pReal[NP],(pObj->C-NP)*sizeof(FLOAT32));
  ffts(pObj->pFFT, pPj->cmpxData.pReal, pPj->cmpxData.pImag);

  /* Fuer den naechsten Aufruf Zeiger aktualisieren                */
  /* Naechstes X[k] ist:                                           */
  pObj->pX  = pObj->pX->pNext;

  /* Naechstes Mu[k] ist:                                          */
  pObj->pMu = pObj->pMu->pNext;

  /* Naechstes Teilfilter fuer Projektion ist:                     */
  pObj->pPj = pObj->pPj->pNext;

  return AV_E_OK;  

}