/*******************************************************************************
*  cfft.c   :   raix-2 complex fft lib function
*  author   :   duweitao
*  date     :   2008-June-24th
*  version  :   0.1
*  modify   :   initial version
*******************************************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <memory.h>

#include "cfft.h"
/*******************************************************************************
 * MEMALOCZ(), allocate memory then set the contents
 * input  : buffer size
 * output : buffer pointer
 ******************************************************************************/
void * MEMALOCZ(int size){
  void * ptr = NULL;
  ptr = malloc(size);
  memset(ptr, 0, size);
  return ptr; 
}
/*******************************************************************************
 * CMult(), complex data mult
 * input  : input data pointer
 * output : output pointer
 * Notes  : this function can perform in-place Mult
 ******************************************************************************/
void CMult(
    cfftdata_t *in0R, cfftdata_t *in0I,
    cfftdata_t *in1R, cfftdata_t *in1I,
    cfftdata_t *outR, cfftdata_t *outI)
{   cfftdata_t outTempR, outTempI = 0;
    outTempR = in0R[0] * in1R[0] - in0I[0]*in1I[0]; 
    outTempI = in0R[0] * in1I[0] + in1R[0]*in0I[0]; 
    outR[0]  = outTempR;
    outI[0]  = outTempI;
}
/*******************************************************************************
 * CAdd(), complex data add 
 * input  : input data pointer
 * output : output pointer 
 ******************************************************************************/
void CAdd(
    cfftdata_t *in0R, cfftdata_t *in0I,
    cfftdata_t *in1R, cfftdata_t *in1I,
    cfftdata_t *outR, cfftdata_t *outI)
{   outR[0] = in0R[0] + in1R[0];
    outI[0] = in0I[0] + in1I[0];
}
/*******************************************************************************
 * CSub(), complex data sub
 * input  : input data pointer
 * output : output pointer 
 ******************************************************************************/
void CSub(
    cfftdata_t *in0R, cfftdata_t *in0I,
    cfftdata_t *in1R, cfftdata_t *in1I,
    cfftdata_t *outR, cfftdata_t *outI)
{   outR[0] = in0R[0] - in1R[0];
    outI[0] = in0I[0] - in1I[0];
}
/*******************************************************************************
 * CfftClose(): free cfft memory with hanndle
 * input : cfft handle 
 * output: a ERROR code
 *         0: NO ERROR
 *         1: ERR
 * notes : this fucntion free the internal data used by CfftRun()
 ******************************************************************************/
int CfftClose( handleCfft_t hCfft){
  if(hCfft->errMsg){
    free(hCfft->errMsg);
    hCfft->errMsg = NULL;
  }
  if(hCfft->bitRevMap){
    free(hCfft->bitRevMap);
    hCfft->bitRevMap= NULL;
  }
  if(hCfft->twd){
    free(hCfft->twd);
    hCfft->twd= NULL;
  }

  return 0;
}

/*******************************************************************************
 * CfftInit(): init the cfft hanndle
 * input : handle pointer hCfftPtr
 *       : fft size N
 * output: a ERROR code
 *         0: NO ERROR
 *         1: ERR
 * notes : this fucntion init internal data needed by CfftRun()
 ******************************************************************************/
#define DBG_CFFTINIT 0
int CfftInit(int N, handleCfft_t *hCfftPtr){
  int  msgBufLen        = 256;
  handleCfft_t hCfft    = NULL;
  char  *errMsg     = NULL;
  u8_t  *bitRevMap  = NULL;
  cfftdata_t *twd   = NULL;
  cfftdata_t        FFTPI  = 0;

  u16_t temp    = 0;
  u16_t nLv     = 0;
  u16_t nBfL    = 0;
  u16_t nBfG    = 0;
  u16_t nGpL    = 0;

  int err               = 0;
  int idx1              = 0;
  int idx2              = 0;
  u8_t bitOrg, bitRev   = 0;
  cfftdata_t theta      = 0;
#if DBG_CFFTINIT
  FILE *fpDbg       = NULL;
  char *initDbg     = "fftInitdbg.txt";
  fpDbg             = fopen(initDbg, "w");
#endif
  
  errMsg    = (char*)       MEMALOCZ(msgBufLen * sizeof(char));
  if(N > 32768){
    sprintf(errMsg, "FFT_MAX_LEN_32768");
    err = 1;
    goto INITEXIT;
  }
  if(N <  2){
    sprintf(errMsg, "FFT_MIN_LEN_0");
    err = 1;
    goto INITEXIT;
  }
  nLv = 0;
  temp = (u16_t) N;
  while(temp != 1){
    temp = temp >> 1;
    nLv ++;
  }
// check N is 2's power
  if((1<<nLv)!= N){
    sprintf(errMsg, "FFT_LEN_MUST_BE_POWER_OF_2");
    err = 1;
    goto INITEXIT;
  }
  nBfL      = N/2;
  bitRevMap = (u8_t*)       MEMALOCZ(256*sizeof(u8_t));
  twd       = (cfftdata_t*) MEMALOCZ((N/4)*sizeof(cfftdata_t));
  FFTPI     = (cfftdata_t)4*atan(1);
  // init the twiddle factor, only save the imag part, -sin(x)
  theta = 2*FFTPI/N;
  for(idx1 = 0; idx1 < (N/4); idx1 ++){
    twd[idx1] = -sin(theta*idx1);
  }
#if DBG_CFFTINIT
  for(idx1 = 0; idx1 < (N/4); idx1 ++){
    fprintf(fpDbg, "twd[%6d] = %20.10f\n", idx1, twd[idx1]); 
  }
#endif
  // init the bit reverse map, for one byte 256 entries 
  for(idx1 = 0; idx1 < 256; idx1 ++){
    for(idx2 = 0; idx2 < 8; idx2 ++){
      bitOrg = idx1 &(1 << idx2);
      if(bitOrg)
        bitRev = 1<<(7-idx2);
      else
        bitRev = 0;
      bitRevMap[idx1] = bitRevMap[idx1] | bitRev;
    }
  }
#if DBG_CFFTINIT
  for(idx1 = 0; idx1 < 256; idx1 ++){
    fprintf(fpDbg, "bitRevMap[%4d] = %4d\n", idx1, bitRevMap[idx1]);
  }
#endif
  
  hCfft = (handleCfft_t) MEMALOCZ(sizeof(objCfft_t));
  hCfft->errMsg     = errMsg;
  hCfft->msgBufLen  = msgBufLen;
  hCfft->N      = (u16_t)N;
  hCfft->nBfL   = nBfL;
  hCfft->nLv    = nLv;
  hCfft->twd    = twd;
  hCfft->FFTPI  = FFTPI;
  hCfft->bitRevMap = bitRevMap;

  *hCfftPtr = hCfft;
#if DBG_CFFTINIT
  fclose(fpDbg);
#endif
INITEXIT:  
  return err;
}// CfftInit(int N, handleCfft_t *hCfftPtr)
/*******************************************************************************
 * CfftRun(): caculate the complex fft
 * input : input data pointer, in
 *       : output data pointer, out
 *       : cfft handle, hCfft
 * output: a ERROR code
 *         0: NO ERROR
 *         1: ERR
 * Notes : in and out must be size of 2*N 1-D array, for each
 *         complex data in the array , the format is real part 
 *         first imag part followed
 ******************************************************************************/
#define DBG_CFFTRUN 0
int CfftRun(cfftdata_t *in, cfftdata_t *out, handleCfft_t hCfft){
// ref vars
  u16_t       N          = hCfft->  N          ;           
  u16_t       nLv        = hCfft->  nLv        ;         
  u16_t       nBfL       = hCfft->  nBfL       ;        
  cfftdata_t  FFTPI      = hCfft->  FFTPI      ;       
  cfftdata_t  *twd       = hCfft->  twd        ;        
  u8_t        *bitRevMap = hCfft->  bitRevMap  ;  
  char        *errMsg    = hCfft->  errMsg     ;     
  int         msgBufLen  = hCfft->  msgBufLen  ;   
// local vars
  int   err        = 0;
  int   idxBfG, idxBfL, idxGpL, idxLv, idxWn, idxData = 0;
  u16_t addrRd0, addrRd1;      // read address, also the write address
  u16_t addrWnReal, addrWnImag;// real and imag part address
  u16_t addrReal0, addrReal1;  // address of real data
  u16_t addrImag0, addrImag1;  // address of imag data
  u16_t nBfG;  // number of butterfly per group
  u16_t nGpL;  // number of group per level 
  u16_t idxH, idxL, idxRH, idxRL; // high/low/orig/reverse idx
  u16_t idxRev; // reverse idx
  cfftdata_t twdReal, twdImag;    // real and imag part of twiddle factor
  cfftdata_t *src, bfOut0R, bfOut0I, bfOut1R, bfOut1I, dataSwpR, dataSwpI; 

#if DBG_CFFTRUN
  FILE *fpDbg       = NULL;
  char *runDbg      = "fftRundbg.txt";
  fpDbg             = fopen(runDbg, "w");
#endif
  // caculate by level and butterfly
  // if your compiler cannot optimize the indexing expression
  // substitue them with bit operations
  for(idxLv = 0; idxLv < nLv; idxLv ++){
    nBfG      = 1 << (nLv - idxLv - 1);
    nGpL      = 1 << idxLv;
    if(idxLv == 0) {
      src = in;
    }
    else {
      src = out;
    }
    for(idxBfL = 0; idxBfL < nBfL; idxBfL ++){
      idxGpL    = idxBfL >> (nLv-idxLv-1);
      idxBfG    = idxBfL & (nBfG - 1);
      addrRd0   = idxBfG+idxGpL*nBfG*2;
      addrRd1   = addrRd0 + nBfG;
      addrReal0 = addrRd0 << 1;
      addrImag0 = (addrRd0 << 1) + 1;
      addrReal1 = addrRd1 << 1;
      addrImag1 = (addrRd1 << 1) + 1;
      idxWn     = idxBfG << idxLv;
      // get the twiddle factor addrese
       if(idxWn == 0){
         twdImag = (cfftdata_t) 0;
         twdReal = (cfftdata_t) 1;
         goto TWDDONE;
       }
       if(idxWn < (N>>2)) { 
         addrWnImag = idxWn; 
         addrWnReal = (N>>2)-idxWn;
         twdImag    = twd[addrWnImag]; 
         twdReal    = -twd[addrWnReal];
         goto TWDDONE;
       }
       if (idxWn > (N>>2)){
         addrWnImag = (N>>1) - idxWn; 
         addrWnReal = idxWn & ((1<<(nLv-2))-1);
         twdImag    = twd[addrWnImag];
         twdReal    = twd[addrWnReal];
         goto TWDDONE;
       }
       {    // idxWn == N/4, imag = -1, real = 0;
         twdImag    = (cfftdata_t) -1.0;
         twdReal    = (cfftdata_t) 0;
       }
TWDDONE:
       // compute butterfly
       CAdd(&(src[addrReal0]), &(src[addrImag0]),
            &(src[addrReal1]), &(src[addrImag1]),
            &(bfOut0R)       , &(bfOut0I));
       CSub(&(src[addrReal0]), &(src[addrImag0]),
            &(src[addrReal1]), &(src[addrImag1]),
            &(bfOut1R)       , &(bfOut1I));

       CMult(&twdReal,   &twdImag,
             &(bfOut1R), &(bfOut1I),
             &(bfOut1R), &(bfOut1I));
       out[addrReal0] = bfOut0R; out[addrImag0] = bfOut0I;
       out[addrReal1] = bfOut1R; out[addrImag1] = bfOut1I;

#if DBG_CFFTRUN
      fprintf(fpDbg, "idxLv=%-4d idxBfL=%-4d", idxLv, idxBfL);
      fprintf(fpDbg, "nBfG=%-4d nGpL=%-4d",nBfG, nGpL);
      fprintf(fpDbg, "idxGpL%-4d idxBfG=%-4d", idxGpL, idxBfG);
      fprintf(fpDbg, "addrRd0=%-4d addrRd1=%-4d", addrRd0, addrRd1);
      fprintf(fpDbg, "idxWn=%-4d ", idxWn);
      fprintf(fpDbg, "addrWnImag=%-4d addrWnReal=%-4d", addrWnImag, addrWnReal);
      fprintf(fpDbg, "bf0In=%+10.4f%+10.4fj ", src[addrReal0], src[addrImag0]);
      fprintf(fpDbg, "bf1In=%+10.4f%+10.4fj ", src[addrReal1], src[addrImag1]);
      fprintf(fpDbg, "twd=%+4.3f%+4.3fj ", twdReal, twdImag);
      fprintf(fpDbg, "bf0Out=%+10.4f%+10.4fj ", bfOut0R, bfOut0I);
      fprintf(fpDbg, "bf1Out=%+10.4f%+10.4fj ", bfOut1R, bfOut1I);

      fprintf(fpDbg, "\n");
      if(idxBfL == nBfL-1) 
        fprintf(fpDbg, "\n");
#endif 
    } // for idxBfL
  } // for idxLv
  // reorder the output
  for(idxData = 0; idxData < N; idxData ++){
    idxH    = (idxData & 0xff00);
    idxH    = (idxH >> 8);
    // idxH    = ((idxData & 0xff00)>>8);
    idxL    = idxData & 0x00ff;
    idxRH   = bitRevMap[idxH];
    idxRL   = bitRevMap[idxL];
    idxRev  = (idxRL<<8)|idxRH;
    idxRev  = idxRev >> (16-nLv);
#if DBG_CFFTRUN
    fprintf(fpDbg, "idxData=%-6d idxRev=%-6d ", idxData, idxRev);
    fprintf(fpDbg, "idxH=%-6d idxL=%-6d ", idxH, idxL);
    fprintf(fpDbg, "idxRH=%-6d idxRL=%-6d", idxRH, idxRL);
    fprintf(fpDbg, "\n");
#endif
    if(idxRev > idxData){
    dataSwpR = out[idxData<<1];
    dataSwpI = out[(idxData<<1)+1];
    out[idxData<<1]     = out[idxRev<<1];
    out[(idxData<<1)+1] = out[(idxRev<<1)+1];
    out[idxRev<<1]      = dataSwpR; 
    out[(idxRev<<1)+1]  = dataSwpI; 
    }
  } // for idxData 
#if DBG_CFFTRUN
  for(idxData = 0; idxData < N; idxData ++){
    fprintf(fpDbg, "out[%6d]=%+10.4f%+10.4fj ", idxData, out[idxData<<1], out[(idxData<<1)+1]);
    fprintf(fpDbg, "\n");
  }
#endif

#if DBG_CFFTRUN
  fclose(fpDbg);
#endif
  return err;
}
