#include "stdio.h"
#include "stdlib.h"
#include "math.h"

#define TRUE  1
#define FALSE 0
#define TWOPI 6.283185307179586476925287

int WriteSeries(char *,double *,double *,double *,int);
int FFT(int,int,double *,double *);

/*
   Test-bed for Fourier digital filter design
   Illustrative code only....not a "product".
*/

#define N  1024       /* Number of samples       */
#define NP 10         /* Power of 2, 2^NP = N    */
#define FS 500.0      /* Sampling frequency      */
#define F1 50.0       /* Cutoff frequency        */
#define W  40         /* Desired filter width    */
#define RECT 0
#define HANN 1
#define WINDOW 1

int main(int argc,char **argv)
{
   int i;
   double re[N],im[N],freq[N],time[N],window[N],mag[N],pha[N];
   int floorMid,mid,ceilMid;
   double tmp[N];

   /*
      Fill in the time and frequency indices
      These are only used to give more informative file output
      although they are useful to ensure you understand the data arrangement
   */
   for (i=0;i<N;i++) {
      time[i] = i / FS;
      if (i <= N/2)                     /* Positive frequencies */
         freq[i] = i * FS / N;
      else                              /* Negative frequencies */
         freq[i] = i * FS / N - FS;
   }

   /* 
      Create the impulse response of the filter in the frequency domain 
      This is an example of a low pass filter, cutoff at F1 Hz
      Note that since I am using a full complex FFT it is necessary
      to correctly fill in the negative frequencies as well.
      Zero phase is assumed at this stage but this will become linear
      phase when the filter is shifted in time to make it causal.
   */
   for (i=0;i<N;i++) {
      re[i] = 0;
      if (i <= (F1 * N / FS) || i >= (N - F1 * N / FS))
         re[i] = 1;
      im[i] = 0;
   }
   WriteSeries("impulse1.f",freq,re,im,N);

   /* 
      Inverse FFT the signal into the time domain
      If the filter is created properly then the imaginary
      components should be zero....baring numerical error.
   */
   FFT(-1,NP,re,im);
   WriteSeries("impulse.t",time,re,im,N);

   /* 
      Create the window so to make the series finite duration
   */
   for (i=0;i<N;i++) {
      window[i] = 0;
      if (i <= W / 2) {
         if (WINDOW == RECT)
            window[i] = 1;
         else
            window[i] = (0.5 + 0.5 * cos(i * TWOPI / (W+1)));
      } else if (i >= N - W/2) {
         if (WINDOW == RECT)
            window[i] = 1;
         else
            window[i] = (0.5 + 0.5 * cos((N - i) * TWOPI / (W+1)));
      }
   }
   WriteSeries("window.t",time,window,im,N);

   /* 
      Apply the window
   */
   for (i=0;i<N;i++) {
      re[i] *= window[i];
      im[i] = 0;                 /* Not necessary */
   }
   WriteSeries("windowed.t",time,re,im,N);

   /*
      Shift the filter to make it causal
   for (i=W-1;i>=N/2;i--)
      re[i] = re[i - W/2];
   for (i=0;i<W/2;i++) {
      re[i] = re[N - 1 - W/2 + i];
      re[N - 1 - W/2 + i] = 0;
   }
	*/
   WriteSeries("causal.t",time,re,im,N);

   /* shift the filter, suggested by Henry Drew W. Abbot */
   mid = (W+1)/2;
   floorMid = (size_t)floor(mid);
   ceilMid = (size_t)ceil(mid);
   tmp = (double *)malloc(sz);
   for (i=0; i<sz; i++)
      tmp[i] = re[i];
   for (i=0; i<floorMid; i++)
      re[i] = tmp[N - floorMid + i];
   for (i=0; i<ceilMid; i++)
      re[i + floorMid] = tmp[i];

   // apply the window. 
   for (i=0; i<=W; i++) {
      re[i] *= window[i];
      im[i] = 0;  
   }
   for (i=W;i<N;i++) {
      re[i] = 0;
      im[i] = 0;
   }

   /* Write the filter coefficients to standard out */
   printf("%d\n",W);
   for (i=0;i<=W;i++)
      printf("%g\n",re[i]);

   /*
      Transform back into the frequency domain mostly as a check
   */
   FFT(1,NP,re,im);
   for (i=0;i<N;i++) {
      mag[i] = sqrt(re[i]*re[i] + im[i]*im[i]);
      pha[i] = atan2(im[i],re[i]);
   }
   WriteSeries("impulse2.f",freq,re,im,N);
   WriteSeries("impulse3.f",freq,mag,pha,N);

}

/*-------------------------------------------------------------------------
   General routine for writing the vectors
*/
int WriteSeries(char *fname,double *x,double *y1,double *y2,int n)
{
   int i;
   FILE *fptr;

   if ((fptr = fopen(fname,"w")) == NULL)
      return(FALSE);
   for (i=0;i<n;i++)
      fprintf(fptr,"%i %g %g %g\n",i,x[i],y1[i],y2[i]); 
   fclose(fptr);
   
   return(TRUE);
}

/*-------------------------------------------------------------------------
   This computes an in-place complex-to-complex FFT
   x and y are the real and imaginary arrays of 2^m points.
   dir =  1 gives forward transform
   dir = -1 gives reverse transform

     Formula: forward
                  N-1
                  ---
              1   \          - j k 2 pi n / N
      X(n) = ---   >   x(k) e                    = forward transform
              N   /                                n=0..N-1
                  ---
                  k=0

      Formula: reverse
                  N-1
                  ---
                  \          j k 2 pi n / N
      X(n) =       >   x(k) e                    = forward transform
                  /                                n=0..N-1
                  ---
                  k=0
*/
int FFT(int dir,int m,double *x,double *y)
{
   long nn,i,i1,j,k,i2,l,l1,l2;
   double c1,c2,tx,ty,t1,t2,u1,u2,z;

   /* Calculate the number of points */
   nn = 1;
   for (i=0;i<m;i++)
      nn *= 2;

   /* Do the bit reversal */
   i2 = nn >> 1;
   j = 0;
   for (i=0;i<nn-1;i++) {
      if (i < j) {
         tx = x[i];
         ty = y[i];
         x[i] = x[j];
         y[i] = y[j];
         x[j] = tx;
         y[j] = ty;
      }
      k = i2;
      while (k <= j) {
         j -= k;
         k >>= 1;
      }
      j += k;
   }

   /* Compute the FFT */
   c1 = -1.0;
   c2 = 0.0;
   l2 = 1;
   for (l=0;l<m;l++) {
      l1 = l2;
      l2 <<= 1;
      u1 = 1.0;
      u2 = 0.0;
      for (j=0;j<l1;j++) {
         for (i=j;i<nn;i+=l2) {
            i1 = i + l1;
            t1 = u1 * x[i1] - u2 * y[i1];
            t2 = u1 * y[i1] + u2 * x[i1];
            x[i1] = x[i] - t1;
            y[i1] = y[i] - t2;
            x[i] += t1;
            y[i] += t2;
         }
         z =  u1 * c1 - u2 * c2;
         u2 = u1 * c2 + u2 * c1;
         u1 = z;
      }
      c2 = sqrt((1.0 - c1) / 2.0);
      if (dir == 1)
         c2 = -c2;
      c1 = sqrt((1.0 + c1) / 2.0);
   }

   /* Scaling for forward transform */
   if (dir == 1) {
      for (i=0;i<nn;i++) {
         x[i] /= (double)nn;
         y[i] /= (double)nn;
      }
   }

   return(TRUE);
}