/*-------------------------------------------------------------------------
 * pfft.c Partioned Fast Fourier Transform
 * $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"


/*************************************************************************/
AVERR PfftInit(
PFFT *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.*/

  /* 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)));
  else
	pObj->C = C;

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

  /*-------------------------------------------------------*/
  /* Speicher allokieren */
  /*-------------------------------------------------------*/

  /* 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 = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
	pObj->pBufX[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
    AvZeroMem(pObj->pBufX[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t));
    AvZeroMem(pObj->pBufX[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t));
    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;
  }

  /* Ergebnis 'Y' der Faltung (komplex) */
  pObj->pY = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX));
  pObj->pY->pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
  pObj->pY->pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t));
  AvZeroMem(pObj->pY->pReal,pObj->C*sizeof(avfloat_t));
  AvZeroMem(pObj->pY->pImag,pObj->C*sizeof(avfloat_t));
  
  /* Overlap-Save 'xs' (reell) */
  pObj->pBufXsave = (avfloat_t*)AvMemAlloc(C_L*sizeof(avfloat_t));
  AvZeroMem(pObj->pBufXsave, C_L*sizeof(avfloat_t));

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

  /* Arbeitszeiger initialisieren */
  pObj->pX  = pObj->pBufX;
  
  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 PfftFilterInit(
PFFT *pObj,               /* Zeiger auf Objekt                          */           
avfloat_t *pWTD,
CMPXBUF *pWS)             /* 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(pWS[p].cmpxData.pReal,
	        &pWTD[p*NP], NP*sizeof(avfloat_t));

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

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

   /* In den Frequenzbereich transformieren 
      WS[p][0..C-1] = 1/C*FFT{wi[p*N/P..(p+1)*N/P-1]}                    */ 
    ffts(pObj->pFFT, pWS[p].cmpxData.pReal, pWS[p].cmpxData.pImag);
  }
  return AV_E_OK;
}

/*---------------------------------------------------------------*/
/* Speicher zuweisen fuer Koeffizienten H
/*---------------------------------------------------------------*/
AVERR PfftFilterAlloc(PFFT *pObj, CMPXBUF **ppH)
{

	UINT32 i;

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

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

  return AV_E_OK;
}

/*---------------------------------------------------------------*/
/* Partitioned FFT-Filterung
/*---------------------------------------------------------------*/
AVERR PfftFilter(PFFT *pObj, CMPXBUF *pH, avfloat_t *px, avfloat_t *py)
{
  UINT32 p, NP, C_L;
  
  CMPXBUF *pX;
  COMPLEX *pY;

  /* Arbeitszeiger */
  pX  = pObj->pX;          /* Aktueller Zeiger X[k]                */
  pY  = pObj->pY;       /* '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],px, 
                         pObj->L*sizeof(avfloat_t));
  
  /* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1]        */
  AvMemCpy(pX->cmpxData.pReal,pObj->pBufXsave,
                              C_L*sizeof(avfloat_t));

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

  /* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = {0}     */
  AvZeroMem(pX->cmpxData.pImag,pObj->C*sizeof(avfloat_t));
  
  /* 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, &pH->cmpxData, 
               pY, pObj->C);

  /* 2. Partition bis P-te Partition                               */
  for (p=1; p < pObj->P; p++)
  {
    pH = pH->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, &pH->cmpxData, 
	             pY, pObj->C);
  }

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

  /* Abspeichern der letzten L Daten ys[0..L-1] = Re{Y[C-L..C-1]   */
  AvMemCpy(py, &pY->pReal[C_L], pObj->L*sizeof(avfloat_t));
 
  /* Fuer den naechsten Aufruf Zeiger aktualisieren                */
  /* Naechstes X[k] ist:                                           */
  pObj->pX  = pObj->pX->pNext;

  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->pReal[i] = pA->pReal[i]*pB->pReal[i] 
	               - pA->pImag[i]*pB->pImag[i];
     pAB->pImag[i] = pA->pReal[i]*pB->pImag[i]
	               + pA->pImag[i]*pB->pReal[i];
  }
}

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

  for (i=0; i < len; i++)
  {
     pAB->pReal[i] = pA->pReal[i] + pB->pReal[i];
     pAB->pImag[i] = pA->pImag[i] + pB->pImag[i];
  }
}

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

  for (i=0; i < len; i++)
  {
	 pAB->pReal[i] += pA->pReal[i]*pB->pReal[i]
	                - pA->pImag[i]*pB->pImag[i];
     pAB->pImag[i] += pA->pReal[i]*pB->pImag[i] 
	                + pA->pImag[i]*pB->pReal[i];
  }
}

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

  for (i=0; i < len; i++)
  {
	 pAB->pReal[i] = (pA->pReal[i]*pB->pReal[i] 
	               -  pA->pImag[i]*pB->pImag[i])*len;
     pAB->pImag[i] = (pA->pReal[i]*pB->pImag[i]
	               +  pA->pImag[i]*pB->pReal[i])*len;
  }
}

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

  for (i=0; i < len; i++)
  {
	 pAB->pReal[i] += (pA->pReal[i]*pB->pReal[i]
	                -  pA->pImag[i]*pB->pImag[i])*len;
     pAB->pImag[i] += (pA->pReal[i]*pB->pImag[i] 
	                +  pA->pImag[i]*pB->pReal[i])*len;
  }
}

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