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

#include <math.h>
#include "../../../include/avtypes.h"
#include "../../../include/averror.h"
#include "../../../include/avrtl.h"
#include "../../../include/fft.h"
#include "pflms.h"

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

AVERR PflmsInit(
PFLMS *pObj,       /* Zeiger auf PFLMS-Objekt */
UINT32 N,
UINT32 P,
UINT32 S,
UINT32 L,
UINT32 C
)  
{  
  UINT32 i, NP;
    
  /* 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;

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

  /* Speicher fuer PX[C] (reell) */
  pObj->pPX = (REALBUF*)AvMemAlloc(sizeof(REALBUF));
  pObj->pPX->pData = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
  AvZeroMem(pObj->pPX->pData,pObj->C*sizeof(FLOAT32));
  pObj->pPX->pNext = pObj->pPX;
  pObj->pPX->pLast = pObj->pPX;
  pObj->pPX->user  = 0; 


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

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

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

  for (i=0; i < P*S; i++)
  {
	pObj->pWS[i].pData = (COMPLEX*)AvMemAlloc(pObj->C*sizeof(COMPLEX));
    AvZeroMem(pObj->pWS[i].pData,pObj->C*sizeof(COMPLEX));
    pObj->pWS[i].pNext = &pObj->pWS[i+1];
    pObj->pWS[i].pLast = &pObj->pWS[i-1];
    pObj->pWS[i].user  = i; 

  }
  pObj->pWS[i-1].pNext = &pObj->pWS[0];
  pObj->pWS[0].pLast   = &pObj->pWS[i-1];
  pObj->pPj = pObj->pWS;            /* aktueller Buffer fuer Projektion */

  /* Ergebnis 'Y' der Faltung (complex) */
  pObj->pY = (COMPLEX*)AvMemAlloc(pObj->C*sizeof(COMPLEX));
  AvZeroMem(pObj->pY,pObj->C*sizeof(COMPLEX));
  
  /* Overlap-Save 'xs' */
  pObj->pXsave = (FLOAT32*)AvMemAlloc(NP*sizeof(FLOAT32));
  AvZeroMem(pObj->pXsave,NP*sizeof(FLOAT32));

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

  return AV_E_OK;
}

/*-----------------------------------------------------------------------*/
/* Partitioned FFT
/*-----------------------------------------------------------------------*/
AVERR Pflms(PFLMS *pObj, FLOAT32 *pDataTD)
{

  UINT32 p, s, NP;
  
  CMPXBUF *pX, *pWS;
  NP = pObj->S*pObj->L;

  pX = pObj->pX;    /* Aktueller Zeiger *X[C]  */
  pWS = pObj->pWS;  /* Aktueller Zeiger *WS[C] */

  /* Arbeitspuffer 'X' auffuellen  Re{X[S*L..C-1]} = x[0..L] */
  AvMemCpy(&pX->pData[NP].real,pDataTD, pObj->L*sizeof(FLOAT32));
  
  /* letzten Saveblock 'xs' anfuegen Re{X[0..S*L-1]} = xs[0..S*L-1] */
  AvMemCpy(&pX->pData->real,pObj->pXsave, NP*sizeof(FLOAT32));

  /* Saveblock aktualisieren xs[0..S*L-1] = x[L..C-1] */
  AvMemCpy(pObj->pXsave, &pDataTD[pObj->L], NP*sizeof(FLOAT32));

  /* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = 0 */
  AvZeroMem(&pX->pData->imag,pObj->C*sizeof(FLOAT32));
  
  /* X = FFT{x} */
  fft(pObj->pFFT, &pX->pData->real, &pX->pData->imag);
  
  /* 1. Partition Faltung im Frequenzbereich Y = X * H */
  CmpxVectMul(pX->pData, pWS->pData, pObj->pY, pObj->C);

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

    for (s=0; s < pObj->S; s++)
	  pX = pX->pLast;

    CmpxVectMac(pX->pData, pWS->pData, pObj->pY, pObj->C);
  }

  /* y = IFFT{Y} */
  ifft(pObj->pFFT, &pObj->pY->real, &pObj->pY->imag);

  /* Abspeichern der letzten L Daten */
  AvMemCpy(pDataTD, &pObj->pY[NP].real, pObj->L*sizeof(FLOAT32));
     
  return AV_E_OK;  

}

/*-----------------------------------------------------------------------*/
/* Complex-Funktionen
/*-----------------------------------------------------------------------*/
void CmpxVectMul(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len)
{
  UINT32 i;

  for (i=0; i < len; i++)
  {
	 pAB[i].real = pA[i].real*pB[i].real - pA[i].imag*pB[i].imag;
     pAB[i].imag = pA[i].real*pB[i].imag + pA[i].imag*pB[i].real;
  }
}

void CmpxVectAdd(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len)
{
  UINT32 i;

  for (i=0; i < len; i++)
  {
     pAB[i].real = pA[i].real + pB[i].real;
     pAB[i].imag = pA[i].imag + pB[i].imag;
  }
}

void CmpxVectMac(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len)
{
  UINT32 i;

  for (i=0; i < len; i++)
  {
	 pAB[i].real += pA[i].real*pB[i].real - pA[i].imag*pB[i].imag;
     pAB[i].imag += pA[i].real*pB[i].imag + pA[i].imag*pB[i].real;
  }
}

void RealVectSquConj(struct _sCOMPLEX *pA, FLOAT32 *pAA, UINT32 len)
{
  UINT32 i;

  for (i=0; i < len; i++)
  {
     pAA[i] = pA[i].real*pA[i].real + pA[i].imag*pA[i].imag;
  }
}
  
/*-----------------------------------------------------------------------*/