/* 
    Fft routines. Originally by J.G.G. Dobbe, modifed to speed up
    by Alexey V. Medvedev 2:5022/78.6
*/
/* PCFFT.C  -- by J.G.G. Dobbe -- Performs an FFT on two arrays (Re, Im) of
               type float (can be changed). This unit is written in C and
               doesn't call assembler routines.
*/
/* --------------------- Local variables -------------------------- */
static float CosArray[28] =
{ /* cos{-2pi/N} for N = 2, 4, 8, ... 16384 */
 -1.00000000000000,  0.00000000000000,  0.70710678118655,
  0.92387953251129,  0.98078528040323,  0.99518472667220,
  0.99879545620517,  0.99969881869620,  0.99992470183914,
  0.99998117528260,  0.99999529380958,  0.99999882345170,
  0.99999970586288,  0.99999992646572,
  /* cos{2pi/N} for N = 2, 4, 8, ... 16384 */
 -1.00000000000000,  0.00000000000000,  0.70710678118655,
  0.92387953251129,  0.98078528040323,  0.99518472667220,
  0.99879545620517,  0.99969881869620,  0.99992470183914,
  0.99998117528260,  0.99999529380958,  0.99999882345170,
  0.99999970586288,  0.99999992646572
};
static float SinArray[28] =
{ /* sin{-2pi/N} for N = 2, 4, 8, ... 16384 */
  0.00000000000000, -1.00000000000000, -0.70710678118655,
 -0.38268343236509, -0.19509032201613, -0.09801714032956,
 -0.04906767432742, -0.02454122852291, -0.01227153828572,
 -0.00613588464915, -0.00306795676297, -0.00153398018628,
 -0.00076699031874, -0.00038349518757,
  /* sin{2pi/N} for N = 2, 4, 8, ... 16384 */
  0.00000000000000,  1.00000000000000,  0.70710678118655,
  0.38268343236509,  0.19509032201613,  0.09801714032956,
  0.04906767432742,  0.02454122852291,  0.01227153828572,
  0.00613588464915,  0.00306795676297,  0.00153398018628,
  0.00076699031874,  0.00038349518757
};

/* --------------------- Function implementations ----------------- */
/* Another (faster) implementation of Shuffle2Arr                   */

#define SWAP(a,i,j) (temp=a[i], a[i]=a[j], a[j]=temp)
void shuffle_2_arr(float *real, float *imag, int n)
{ float temp;
  int i=0, j=0, i0,i1,i2,i3,i4,i5,i6,i7,i8,i9,i10,i11,i12,i13;

#define TWICE(k) for(i##k = 2; -- i##k >= 0; j ^= (1<<k) )
    switch(n) {                                                                           
    case 1:
        TWICE(0)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 2:
        TWICE(0) TWICE(1)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 3:
        TWICE(0) TWICE(1) TWICE(2)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 4:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 5:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 6:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 7:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 8:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 9:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 10:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 11:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 12:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10) TWICE(11)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 13:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10) TWICE(11) TWICE(12)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    case 14:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10) TWICE(11) TWICE(12) TWICE(13)
        {  if(j>i){ SWAP(real,i,j); SWAP(imag,i,j); }  i++;  }
        break;
    default: break;
    }
}
/* --------------------- Fft -------------------------------------- */
void Fft(float *Re, float *Im, int Pwr, int Dir)

/* Function     : Actual FFT algorithm. "Re" and "Im" point to start of real 
                 and imaginary arrays of numbers, "Pwr" holds the array sizes
                 as a power of 2 while "Dir" indicates whether an FFT (Dir>=1)
                 or an inverse FFT must be performed (Dir<=0).
  Return value : The transformed information is returned by "Re"
                 and "Im" (real and imaginary part respectively).
  Comment      : modifed to speed up by Alexey V. Medvedev
*/
{
  register float *pr1, *pi1, *pr2, *pi2;
  register float c, s;
  register float tempr, tempi;
  register float Qr, Qi;

  register int Section;
  register int AngleCounter;
  register int FlyCount;

  int N;


/*    Shuffle2Arr(Re, Im, Pwr);    original version: Shuffle before (i)FFT */
  shuffle_2_arr(Re,Im,Pwr);     /* new version, a bit faster */ 

  N = 1<<Pwr;

  if (Dir >= 1) AngleCounter = 0;                            /* FFT */
  else          AngleCounter = 14;                   /* Inverse FFT */

  Section = 1;
  while(Section<N)
  {
    c = CosArray[AngleCounter];
    s = SinArray[AngleCounter++];

    Qr = 1; Qi = 0;
    for (FlyCount = 0; FlyCount < Section; FlyCount++)
    {
      pr1 = Re + FlyCount;
      pi1 = Im + FlyCount;

      do {
        pr2 = pr1 + Section;
        pi2 = pi1 + Section;
                                           /* Perform 2-Point DFT  */
        tempr = Qr * (*pr2) - Qi * (*pi2);
        tempi = Qr * (*pi2) + Qi * (*pr2);

        *pr2 = *pr1 - tempr;             /* For Re-part */
        *pr1 = *pr1 + tempr;
        *pi2 = *pi1 - tempi;             /* For Im-part */
        *pi1 = *pi1 + tempi;

         pr1 = pr2 + Section;
         pi1 = pi2 + Section;

      } while (pr1-Re < N);

      tempr = Qr;
      Qr   = Qr*c - Qi*s;
      Qi   = Qi*c + tempr*s;
    }
    
    Section = Section * 2;
  }

  if (Dir <= 0)                                    /* Normalize for */
  {                                             /* inverse FFT only */
    tempr = 1.0/N;
    for (Section = 0; Section < N; Section++)
    {
      Re[Section] *= tempr;
      Im[Section] *= tempr;
    }
  }

}
/*==============================================================================================*/
/*
Complex version of fft-routines by Alexey V. Medvedev
Uses array of 'struct complex' instead of 2 arrays of float 
-- this is faster
*/

struct complex { float re; float im; };

#define comp_SWAP(a,i,j) (temp=a[i], a[i]=a[j], a[j]=temp)
void comp_shuffle_2_arr(struct complex *Cp, int n)
{ struct complex temp;
  int i=0, j=0, i0,i1,i2,i3,i4,i5,i6,i7,i8,i9,i10,i11,i12,i13;

    switch(n) {                                                                           
    case 1:
        TWICE(0)
        {  if(j>i){ comp_SWAP(Cp,i,j); }  i++;  }
        break;
    case 2:
        TWICE(0) TWICE(1)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 3:
        TWICE(0) TWICE(1) TWICE(2)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 4:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 5:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 6:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 7:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 8:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 9:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 10:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 11:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 12:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10) TWICE(11)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 13:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10) TWICE(11) TWICE(12)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    case 14:
        TWICE(0) TWICE(1) TWICE(2) TWICE(3) TWICE(4) TWICE(5) TWICE(6) TWICE(7)
        TWICE(8) TWICE(9) TWICE(10) TWICE(11) TWICE(12) TWICE(13)
        {  if(j>i){ comp_SWAP(Cp,i,j);  }  i++;  }
        break;
    default: break;
    }    
}
/* --------------------- Fft -------------------------------------- */
void comp_Fft(struct complex *Cp, int Pwr, int Dir)

/* Function     : Actual FFT algorithm. struct complex *Cp point to start of 
                 array of complex numbers, "Pwr" holds the array sizes
                 as a power of 2 while "Dir" indicates whether an FFT (Dir>=1)
                 or an inverse FFT must be performed (Dir<=0).
  Return value : The transformed information is returned by new values
                in Cp.
  Comment      : coded by Alexey V. Medvedev (based on Fft)
*/
{
  register struct complex *p1, *p2;
  register float c, s;
  register float tempr, tempi;
  register float Qr, Qi;

  register int Section;
  register int AngleCounter;
  register int FlyCount;

  int N;


  comp_shuffle_2_arr(Cp,Pwr);

  N = 1<<Pwr;

  if (Dir >= 1) AngleCounter = 0;                            /* FFT */
  else          AngleCounter = 14;                   /* Inverse FFT */

  Section = 1;
  while(Section<N)
  {
    c = CosArray[AngleCounter];
    s = SinArray[AngleCounter++];

    Qr = 1; Qi = 0;
    for (FlyCount = 0; FlyCount < Section; FlyCount++)
    {
      p1 = Cp + FlyCount;

      do {
        p2 = p1 + Section;
                                           /* Perform 2-Point DFT  */
        tempr = Qr * (p2->re) - Qi * (p2->im);
        tempi = Qr * (p2->im) + Qi * (p2->re);

        p2->re = p1->re - tempr;             /* For Re-part */
        p1->re = p1->re + tempr;
        p2->im = p1->im - tempi;             /* For Im-part */
        p1->im = p1->im + tempi;

        p1 = p2 + Section;

      } while (p1-Cp < N);

      tempr = Qr;
      Qr   = Qr*c - Qi*s;
      Qi   = Qi*c + tempr*s;
    }
    
    Section = Section * 2;
  }

  if (Dir <= 0)                                    /* Normalize for */
  {                                             /* inverse FFT only */
    tempr = 1.0/N;
    for (Section = 0; Section < N; Section++)
    {
      Cp[Section].re *= tempr;
      Cp[Section].im *= tempr;
    }
  }

}
/* ---------------------------------------------------------------- */

#if 1
/* 
   Very primitive test programm
*/
/* --------------------- Include directives ----------------------- */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>

/* --------------------- Constant definition ---------------------- */
#define SIZE 16384                     /* Size of data arrays (re, im) */
#define BASE 14
/* --------------------- Variable definitions --------------------- */
float re[SIZE];                  /* Array holding Real part of data */
float im[SIZE];             /* Array holding Imaginary part of data */
struct complex sig[SIZE];    /* complex variant */
//------------------------------------------------------------------
int main()
{
  int i; time_t t1, t2;

  for (i = 0; i < SIZE; i++)                   /* Clear data arrays */
  {
    sig[i].re = re[i] = (float)rand()/(float)RAND_MAX;  // random values
    sig[i].im = im[i] = (float)rand()/(float)RAND_MAX;

  }
  sig[0].re = re[0] = 100.0;                    /* Fill array with pulse signal */

#define   NUMC 20

  printf("Waiting 1 second..."); fflush(stdout);
  sleep(1);
  printf("\nBegin test.\n");

  t1 = time(0);
  for(i=1;i<NUMC;i++) {
      Fft(re, im, BASE, 1);                          /* FFT on data points */
      Fft(re, im, BASE, -1);                   /* Inverse FFT on freq data */
/*        comp_Fft(sig,BASE,1); */
/*        comp_Fft(sig,BASE,-1); */
  }
  t2 = time(0);
  printf("End. Time: %g ms\n",1000./2.*(t2-t1)/NUMC);

  return 0;
}
/* ---------------------------------------------------------------- */
#endif