/*-------------------------------------------------------------------------
 * pflms.c Partioned Frequency Least-Mean-Square adaptive algorithm
 * $Id: $
 *-------------------------------------------------------------------------
 *
 * Copyright (C) 2001  Algo Vision Systems GmbH
 *
 *-------------------------------------------------------------------------
 */
#define AVNEEDFLOAT

#ifdef _DEBUG
#include <stdio.h>
#endif
#include <math.h>
#include "avtypes.h"
#include "averror.h"
#include "avrtl.h"
#include "avfft.h"
#include "pfft.h"
#include "pflms.h"

/*-----------------------------------------------------------------------*/
#define LAMBDA 0.6
#define pTHIS pObj

AVERR PflmsInit(
PFLMS *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;
  CMPXBUF *pX;             /* Temporaerer Zeiger fuer Bufferinit.*/
  REALBUF *pMu;            /* Temporaerer Zeiger fuer Bufferinit.*/

#ifdef _DEBUG
  UINT32 memMu, memPX, memX, memWS, memY, memXs;
  UINT32 nBufMu, nBufPX, nBufX, nBufWS, nBufY, nBufXs;
#endif
      
  /* Auto-Param */
  if ((N*L)==0)
    return AV_E_FAIL;

  if (P==0)
  {
    if ((N%L) != 0)
	  return AV_E_FAIL;
    P = N/L;
  }

  /* Auto-guess FFT-Groesse C */
  if (C==0)
    pObj->C = (UINT32)pow(2,ceil(log(L+N/P-1)/log(2.0)));

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

#ifdef _DEBUG
  printf("PFLMS Init:\n");
  printf("N   = %d\n",pObj->N);
  printf("P   = %d\n",pObj->P);
  printf("S   = %d\n",pObj->S);
  printf("L   = %d\n",pObj->L);
  printf("NP  = %d\n",NP);
  printf("C   = %d\n",pObj->C);
  printf("C-L = %d\n",C_L);
#endif

  /*-------------------------------------------------------*/
  /* 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;
  }

#ifdef _DEBUG
  printf("* Speichernutzung *\n");

  /* Speichernutzung fuer Mu ausgeben */
  nBufMu =  pObj->P*pObj->S;
  memMu  = pObj->P*pObj->S*pObj->C*sizeof(FLOAT32)
         + pObj->P*pObj->S*sizeof(REALBUF);

  printf("Mu:\n");
  printf("Anzahl reelle Buffer : %d\n", nBufMu);
  printf("Gesamt Buffergroesse : %d bytes\n", memMu);
#endif                                      


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

#ifdef _DEBUG
  /* Speichernutzung fuer PX ausgeben */
  nBufPX =  1;
  memPX  =  pObj->C*sizeof(FLOAT32);

  printf("PX:\n");
  printf("Anzahl reelle Buffer : %d\n", nBufPX);
  printf("Gesamt Buffergroesse : %d bytes\n", memPX);
#endif                                      
                                      

  /* Speicher fuer X[P*S][C] (komplex) */
  pObj->pBufX = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF));

  for (i=0; i < pObj->P*pObj->S; i++)
  {
	pObj->pBufX[i].cmpxData.pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
	pObj->pBufX[i].cmpxData.pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
    AvZeroMem(pObj->pBufX[i].cmpxData.pReal,pObj->C*sizeof(FLOAT32));
    AvZeroMem(pObj->pBufX[i].cmpxData.pImag,pObj->C*sizeof(FLOAT32));
    pObj->pBufX[i].pNext = &pObj->pBufX[i+1];
    pObj->pBufX[i].pLast = &pObj->pBufX[i-1];
    pObj->pBufX[i].pLastPS = NULL;
    pObj->pBufX[i].user  = i; 
  }
  pObj->pBufX[i-1].pNext = &pObj->pBufX[0];
  pObj->pBufX[0].pLast   = &pObj->pBufX[i-1];

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

    pObj->pBufX[i].pLastPS = pX;
  }
  
#ifdef _DEBUG
  /* Speichernutzung fuer X ausgeben */
  nBufX =  pObj->P*pObj->S;
  memX  =  pObj->P*pObj->S*pObj->C*sizeof(CMPXBUF);

  printf("X:\n");
  printf("Anzahl complexe Buffer : %d\n", nBufX);
  printf("Gesamt Buffergroesse   : %d bytes\n", memX);
#endif                                      

  /* Speicher fuer WS[P][C] (komplex) */
  pObj->pBufWS = (CMPXBUF*)AvMemAlloc(P*S*sizeof(CMPXBUF));

  for (i=0; i < P*S; i++)
  {
	pObj->pBufWS[i].cmpxData.pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
	pObj->pBufWS[i].cmpxData.pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
    AvZeroMem(pObj->pBufWS[i].cmpxData.pReal,pObj->C*sizeof(FLOAT32));
    AvZeroMem(pObj->pBufWS[i].cmpxData.pImag,pObj->C*sizeof(FLOAT32));
    pObj->pBufWS[i].pNext = &pObj->pBufWS[i+1];
    pObj->pBufWS[i].pLast = &pObj->pBufWS[i-1];
    pObj->pBufWS[i].pLastPS = NULL;
    pObj->pBufWS[i].user  = i; 
  }
  pObj->pBufWS[i-1].pNext = &pObj->pBufWS[0];
  pObj->pBufWS[0].pLast   = &pObj->pBufWS[i-1];

#ifdef _DEBUG
  /* Speichernutzung fuer WS ausgeben */
  nBufWS =  pObj->P;
  memWS  =  pObj->P*pObj->C*sizeof(CMPXBUF);

  printf("WS:\n");
  printf("Anzahl complexe Buffer : %d\n", nBufWS);
  printf("Gesamt Buffergroesse   : %d bytes\n", memWS);
#endif                                      

  /* 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));
  
  #ifdef _DEBUG
  /* Speichernutzung fuer Y ausgeben */
  nBufY =  1;
  memY  =  pObj->C*sizeof(CMPXBUF);

  printf("Y:\n");
  printf("Anzahl complexe Buffer : %d\n", nBufY);
  printf("Gesamt Buffergroesse   : %d bytes\n", memY);
#endif                                      

  /* Overlap-Save 'xs' (reell) */
  pObj->pBufXsave = (FLOAT32*)AvMemAlloc(C_L*sizeof(FLOAT32));
  AvZeroMem(pObj->pBufXsave, C_L*sizeof(FLOAT32));

#ifdef _DEBUG
  /* Speichernutzung fuer Xsave ausgeben */
  nBufXs =  1;
  memXs  =  C_L*sizeof(FLOAT32);

  printf("Xsave:\n");
  printf("Anzahl reelle Buffer : %d\n", nBufXs);
  printf("Gesamt Buffergroesse   : %d bytes\n", memXs);
#endif                                      

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

  /* Arbeitszeiger initialisieren */
  pObj->pX  = pObj->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 PflmsInitFilter(
PFLMS *pObj,               /* Zeiger auf Objekt                          */           
FLOAT32 *pWTD)             /* N Filterkoeffizienten im Zeitbereich       */ 
{
  UINT32 p, NP, C_SL;

  NP   = pObj->S * pObj->L;
  C_SL = pObj->C - NP;

  for (p=0; p < pObj->P; p++)
  {
    /* Arbeitspuffer WS auffuellen
       Re{WS[p][0..C-L-1]}=wi[p*N/P..(p+1)*N/P-1]                        */
	AvMemCpy(pObj->pBufWS[p].cmpxData.pReal,
	        &pWTD[p*NP], NP*sizeof(FLOAT32));

   /* Re{WS[C-L..C]} = {0} (vorsichtshalber)                             */
	AvZeroMem(&pObj->pBufWS[p].cmpxData.pReal[NP],
	           C_SL *sizeof(FLOAT32)); 

   /* Im{WS[0..C-1]} = {0}                                               */
	AvZeroMem(pObj->pBufWS[p].cmpxData.pImag, pObj->C *sizeof(FLOAT32));

   /* In den Frequenzbereich transformieren 
      WS[p][0..C-1] = 1/C*FFT{wi[p*N/P..(p+1)*N/P-1]}                    */ 
    ffts(pObj->pFFT, pObj->pBufWS[p].cmpxData.pReal,
	                pObj->pBufWS[p].cmpxData.pImag);
  }
  return AV_E_OK;
}
  
/*-----------------------------------------------------------------*/
/* Partitioned FLMS
/*-----------------------------------------------------------------*/
AVERR Pflms(
PFLMS *pObj,               /* Zeiger auf 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;

  /*---------------------------------------------------------------*/
  /* Beginn: Partitioned FFT-Filterung
  /*---------------------------------------------------------------*/
  /* TODO: Standalone Version der Partitioned-FFT-Filterung */

  /* 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                                  */

  
  /* Arbeitspuffer 'X' auffuellen  Re{X[S*L..C-1]} = x[0..L-1]     */
  AvMemCpy(&pX->cmpxData.pReal[C_L],pInTDx, 
                         pObj->L*sizeof(FLOAT32));
  
  /* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1]        */
  AvMemCpy(pX->cmpxData.pReal,pObj->pBufXsave,
                              C_L*sizeof(FLOAT32));

  /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1]              */
  AvMemCpy(pObj->pBufXsave, &pX->cmpxData.pReal[pObj->L],
                            C_L*sizeof(FLOAT32));

  /* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = {0}     */
  AvZeroMem(pX->cmpxData.pImag,pObj->C*sizeof(FLOAT32));
  
  /* X = 1/C*FFT{x} */
  ffts(pObj->pFFT, pX->cmpxData.pReal, pX->cmpxData.pImag);

  /* 1. Partition Multiplikation im Frequenzbereich 
     Y = X[k][0..C-1] * H[0][0..C-1] * C                           */
  CmpxVectMulS(&pX->cmpxData, &pWS->cmpxData, 
               pTemp, pObj->C);

  /* 2. Partition bis P-te Partition                               */
  for (p=1; p < pObj->P; p++)
  {
    pWS = pWS->pNext;

    /* X[k-p*S] suchen */
    pX = pX->pLastPS;

    /* Y = X[k-p*S][0..C-1] * H[p][0..C-1] * C                     */
    CmpxVectMacS(&pX->cmpxData, &pWS->cmpxData, 
	             pTemp, pObj->C);
  }

  /* Zeiger wieder WS[0] setzten */
  pWS = pObj->pBufWS;

  /* Zeiger wieder auf aktuellen Block X[k] setzten */
  pX = pObj->pX;

  /* In den Zeitbereich transformieren, y = IFFT{Y}                */
  ifft(pObj->pFFT, pTemp->pReal, pTemp->pImag);

  /* Abspeichern der letzten L Daten ys[0..L-1] = Re{Y[C-L..C-1]   */
  AvMemCpy(pOutTDy, &pTemp->pReal[C_L],
                     pObj->L*sizeof(FLOAT32));
  
  /*---------------------------------------------------------------*/
  /* Ende: Partitioned FFT-Filterung
  /*---------------------------------------------------------------*/
  /* Die Variable 'Y' wird nun nicht mehr gebraucht.
  /* In den Nachfolgenden Abschnitten wird 'Y' als temporaerer
  /* Speicher benutzt.
   
  /*---------------------------------------------------------------*/
  /* 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));
 
  /* 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((pX->cmpxData.pReal[i]*pX->cmpxData.pReal[i]
		            +  pX->cmpxData.pImag[i]*pX->cmpxData.pImag[i])
					* (1.0-LAMBDA) * pObj->C + 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++)
    {
      pWS->cmpxData.pReal[i] += ((pX->cmpxData.pReal[i] * pTemp->pReal[i]
	                          +  pX->cmpxData.pImag[i] * pTemp->pImag[i])
						      *  pMu->pData[i] * pObj->C);
      pWS->cmpxData.pImag[i] += ((pX->cmpxData.pReal[i] * pTemp->pImag[i]
	                          -  pX->cmpxData.pImag[i] * pTemp->pReal[i])
						      *  pMu->pData[i] * pObj->C);
	}
    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;  

}

/*------------------------------------------------------------------*/