/*-------------------------------------------------------------------------
 * 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;
  REALBUF *pMu;            /* Temporaerer Zeiger fuer Bufferinit.*/

#ifdef _DEBUG
  UINT32 memMu, memPX, memY;
  UINT32 nBufMu, nBufPX, nBufY;
#endif
      
  /* 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;

#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 = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
    AvZeroMem(pObj->pBufMu[i].pData,pObj->C*sizeof(avfloat_t));
    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(avfloat_t)
         + 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 = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
  AvZeroMem(pObj->pBufPX,pObj->C*sizeof(avfloat_t));

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

  printf("PX:\n");
  printf("Anzahl reelle Buffer : %d\n", nBufPX);
  printf("Gesamt Buffergroesse : %d bytes\n", memPX);
#endif                                      
                  
  /* Ergebnis 'Y' der Faltung (komplex) */
  pObj->pTemp = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX));
  pObj->pTemp->pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
  pObj->pTemp->pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
  AvZeroMem(pObj->pTemp->pReal,pObj->C*sizeof(avfloat_t));
  AvZeroMem(pObj->pTemp->pImag,pObj->C*sizeof(avfloat_t));
  
  #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                                      

  /* 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 = (avfloat_t)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                          */           
avfloat_t *pWTD)             /* N Filterkoeffizienten im Zeitbereich       */ 
{
  return PfftFilterInit(pObj->pFilter, pWTD, pObj->pBufWS);
}
  
/*-----------------------------------------------------------------*/
/* Partitioned FLMS
/*-----------------------------------------------------------------*/
AVERR Pflms(
PFLMS *pObj,               /* Zeiger auf Objekt                    */
avfloat_t *pInTDx,           /* Ein: Daten x[0..L-1]                 */
avfloat_t *pInTDd,           /* Ein: Referenzsignal d[0..L-1]        */
avfloat_t *pOutTDy,          /* Aus: Filterausgang y[0..L-1]         */
avfloat_t *pOutTDe,          /* Aus: Fehlersignal e[0..L-1]          */ 
avfloat_t alpha)
{

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

  /* 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(avfloat_t));
  
  /* Re{Y[0..C-L-1]} = {0}                                           */
  AvZeroMem(pTemp->pReal,C_L*sizeof(avfloat_t));

  /* Re{Y[C-L..C-1]} = e[0..L-1]                                   */
  AvMemCpy(&pTemp->pReal[C_L],
            pOutTDe,pObj->L*sizeof(avfloat_t));
 
  /* 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 = (avfloat_t)(alpha*pObj->gamma/pObj->P);

  for (i=0; i < pObj->C; i++)
  {
    /* Schaetzung der mittleren Eingangsleistung PX                */
    pObj->pBufPX[i] = (avfloat_t)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++)
    {
      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(avfloat_t));
  AvZeroMem(&pPj->cmpxData.pReal[NP],(pObj->C-NP)*sizeof(avfloat_t));
  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;  

}

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