/**************************************************************
* Program to demonstrate the Fast Fourier Transform Procedure *
*      (frequency analysis of a discrete signal F(t))         *
*                                                             *
*                         C++ version by J-P Moreau, Paris    *
*                                (www.jpmoreau.fr)            *
*                                                             *
*     To be used with basi_r.cpp and wmblock.cpp found in     *
*     MATRICES directory.                                     *
* ----------------------------------------------------------- *
* SAMPLE RUN:                                                 *
*                                                             *
* Input data file (tfft.dat):                                 *
*                                                             *
* (The test signal contains 3 frequencies: 50, 250, 500 hz)   *
*                                                             * 
* 1024                                                        *
* 0.00000000000000E+0000   0.00000000000000E+0000             *
* 1.95503421309917E-0004   3.53914399999776E+0001             *
* 3.91006842619834E-0004   5.95684899999760E+0001             *
* 5.86510263929974E-0004   6.54621699999552E+0001             *
* 7.82013685239669E-0004   5.24038399999845E+0001             *
* ...                      ...                                *
* 1.98826979472187E-0001   2.77372500000183E-0001             *
* 1.99022482893497E-0001  -2.43361500000174E+0000             *
* 1.99217986314807E-0001  -4.84236799999780E+0000             *
* 1.99413489736116E-0001  -6.02247899999929E+0000             *
* 1.99608993157426E-0001  -5.45615399999951E+0000             *
* 1.99804496578736E-0001  -3.22824200000105E+0000             *
* 2.00000000000045E-0001  -2.96010699999982E-0003             *
*                                                             *
* Output file (tfft.lst):                                     *
*                                                             *
*   Frequency     Value                                       *
* ------------------------                                    *
*      0.00      0.271690                                     *
*      5.00      0.546336                                     *
*      9.99      0.555560                                     *
*     14.99      0.572242                                     *
*     19.98      0.598828                                     *
*     24.98      0.640061                                     *
*   ...          ...                                          *
*   2522.53      0.020520                                     *
*   2527.53      0.020518                                     *
*   2532.52      0.020516                                     *
*   2537.52      0.020513                                     *
*   2542.51      0.020512                                     *
*   2547.51      0.020511                                     *
*   2552.50      0.020511                                     *
*                                                             *
* ----------------------------------------------------------- *
* To link with files basis_r.cpp and vmblock.cpp (see direc-  *
* tory: C++/Matrices).                                        *                                         
**************************************************************/
#include <stdio.h>
#include <math.h>
#include <basis.h>     // ABS, MACH_EPS, REAL, WriteEnd(), WriteHead()   
#include <vmblock.h>   // dynamic allocations


FILE  *f_in,*f_out;
int   i,ip,n1,ndata;
REAL  df,dt,f,temp,tbegin,tend,y;
REAL  *signal;         // pointer to real vector
REAL  **A;             // pointer to matrix (complex vector)  
void  *vmblock;

 //complex numbers utility functions

 //returns real part of a complex number z
 double Re(double z[2])
 {
   return z[0];
 }

 //returns imaginary part of a complex number z
 double Im(double z[2])
 {
   return z[1];
 }

 //adds up to complex numbers (z1+z2)
 void Sum(double z1[2],double z2[2],double z1_plus_z2[2])
 {
   z1_plus_z2[0]=z1[0]+z2[0];
   z1_plus_z2[1]=z1[1]+z2[1];
 }

 //z1-z2
 void InvSum(double z1[2],double z2[2],double z1_z2[2])
 {
   z1_z2[0]=z1[0]-z2[0];
   z1_z2[1]=z1[1]-z2[1];
 }

 //z1*z2
 void Prod(double z1[2],double z2[2],double z1xz2[2])
 {
   double a1,a2,b1,b2;
   a1= Re(z1);
   b1= Im(z1);
   a2= Re(z2);
   b2= Im(z2);
   z1xz2[0]=a1*a2-b1*b2;
   z1xz2[1]=a1*b2+a2*b1;
 }

 /**************************************************************
 *             FAST FOURIER TRANSFORM PROCEDURE                *
 * ----------------------------------------------------------- *
 * This procedure calculates the fast Fourier transform of a   *
 * real function sampled in N points 0,1,....,N-1. N must be a *
 * power of two (2 power M).                                   *
 *  T being the sampling duration, the maximum frequency in    *
 * the signal can't be greater than fc = 1/(2T). The resulting *
 * specter H(k) is discreet and contains the frequencies:      *
 *         fn = k/(NT) with k = -N/2,.. -1,0,1,2,..,N/2.       *
 *         H(0) corresponds to null fréquency.                 *
 *         H(N/2) corresponds to fc frequency.                 * 
 * ----------------------------------------------------------- *
 * INPUTS:                                                     *
 *                                                             *
 *     A[i][2]  complex vector of size N, the real part of     *
 *              which contains the N sampled points of real    *
 *              signal to analyse (time spacing is constant).  *
 *                                                             *
 *          M   integer such as N=2^M                          *
 *                                                             *
 * OUTPUTS:                                                    *
 *                                                             *
 *     A[i][2]  complex vector of size N, the vector modulus   *
 *              contain the frequencies of input signal, the   *
 *              vector angles contain the corresponding phases *
 *              (not used here).                               *
 *                                                             *
 *                                     J-P Moreau/J-P Dumont   *
 **************************************************************/
void FFT(REAL **A, int M)  {
    
    const double pi=3.1415926535;
    double U[2],W[2],T[2];            //complex numbers
    int N,NV2,NM1,J,I,IP,K,L,LE,LE1;
    double Phi;

    N=(2 << (M-1));
    NV2=(N >> 1);
    NM1=N-1;
    J=1;
    for (I=1;I<=NM1;I++) {
      if (I<J) {
	    T[0]=A[J-1][0]; 
	    T[1]=A[J-1][1];
	    A[J-1][0]=A[I-1][0]; 
	    A[J-1][1]=A[I-1][1];
	    A[I-1][0]=T[0]; 
	    A[I-1][1]=T[1];
      }
      K=NV2;
      while (K<J) {
	    J=J-K;
	    K=(K >> 1);
      }
      J=J+K;
    }

    LE=1;
    for (L=1;L<=M;L++) {
      LE1=LE;
      LE=(LE << 1);
      U[0]=1.0;
      U[1]=0.0;
      Phi= pi/LE1;
      W[0] = cos(Phi);
      W[1] = sin(Phi);

      for (J=1;J<=LE1;J++)  {
	    I=J-LE;
	    while (I< N-LE) {
	      I=I+LE;
	      IP=I+LE1;
	      Prod(A[IP-1],U,T);
	      InvSum(A[I-1],T,A[IP-1]);
	      Sum(A[I-1],T,A[I-1]);
	    }
	    Prod(U,W,U);
      }
    }
    for (I=0;I<N;I++)  {
      A[I][0]=A[I][0]/N;
      A[I][1]=A[I][1]/N;
    }

} //FFT()


void main()  {
  // open input and output file
  f_in=fopen("tfft.dat","r");
  f_out=fopen("tfft.lst","w");
  //read number of input signal points in input file
  fscanf(f_in,"%d",&ndata);
  //take nearest power of two
  if (ndata > 2048) {ndata=2048; ip=11;}
  if (ndata < 2048 && ndata>1023) {ndata=1024; ip=10;}
  if (ndata < 1024 && ndata>511) {ndata=512; ip=9;}
  if (ndata <  512 && ndata>255) {ndata=256; ip=8;}
  if (ndata <  256 && ndata>127) {ndata=128; ip=7;}
  if (ndata <  128 && ndata> 63) {ndata=64; ip=6;}
  if (ndata <   64 && ndata> 31) {ndata=32; ip=5;}
  if (ndata <   32) {
    fprintf(f_out," Error: number of points too small (<32) !\n");
    fclose(f_out);
    printf(" Results in file tfft.lst (error).\n");
  }
  //Allocate memory space for A[ndata][2] and signal[ndata]
  vmblock = vminit();
  A      = (REAL **)vmalloc(vmblock, MATRIX, ndata, 2);
  signal = (REAL *) vmalloc(vmblock, VEKTOR, ndata, 0);
  if (! vmcomplete(vmblock)) {
    LogError ("Memory full!", 0, __FILE__, __LINE__);
    return;
  }	  
  
  //read ndata couples T(i), Y(i) in input data file
  for (i=0; i<ndata; i++) {
    fscanf(f_in,"%lf %lf",&temp,&y);
    signal[i] = y;
    if (i==0)  tbegin=temp;
    if (i==ndata-1)  tend=temp;
  }
  // Put input signal in real part of complex vector A
  for (i=0; i<ndata; i++) {
    A[i][0]=signal[i];
    A[i][1]=0.0;
  }
  // call FFT procedure
  FFT(A,ip);
  // get frequencies in signal real vector
  n1=ndata/2;
  for (i=0; i<n1; i++) {
    temp= sqrt(A[i][0]*A[i][0]+A[i][1]*A[i][1]);
    if (i>0)  temp=2*temp;
    signal[i]=temp;
  }

  // calculate sampling time range dt
  dt=(tend-tbegin)/(ndata-1);
  // calculate frequency step df
  df= 1.0/(ndata*dt);
  // print frequency specter to output file
  f=0.0;
  fprintf(f_out,"   Frequency     Value   \n");
  fprintf(f_out," ------------------------\n");
  for (i=0; i<n1; i++) {
    fprintf(f_out,"  %8.2f  %12.6f\n",f,signal[i]);
    f+=df;
  }
  fclose(f_out);
  free(vmblock);
  printf("\n Results in file tfft.lst.\n");

}

// End of file tfft.cpp