/***************************************************************************
                          Fourier.cpp  -  description
                             -------------------
    begin                : Fre Mai 9 2003
    copyright            : (C) 2003 by klaus frieler
    email                : kgf@omniversum.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/
#include <cmath>
#include <iostream>
#include "Fourier.h"
#include "Types.h"

using namespace std;

float *Fourier::four1(float data[], unsigned long nn, int isign)
{
   unsigned long n, mmax, m, j, istep, i;
   float wtemp, wR, wI, wpR, wpI, theta;
   float tempR, tempI;
   n = nn<<1;
   j = 1;
   for (i = 1; i<n; i+= 2){
      if(j>i){
         //cerr<<"Swapping : "<<i<<" "<<j<<endl;
         tempR = data[j]; data[j] = data[i]; data[i] = tempR;
         tempR = data[j+1]; data[j+1] = data[i+1];  data[i+1]= tempR;
      }
      m = nn;
      while( m>=2 && j>m){
         j-=m;
         m>>=1;
      }
      j += m;
   }
   mmax = 2;
   while(n>mmax){
      istep = mmax<<1;
      theta = isign * (TWOPI/mmax);
      wtemp = sin(0.5*theta);
      wpR = -2.0 * wtemp*wtemp;
      wpI = sin(theta);
      wR = 1.0;
      wI = 0.0;
      //cerr<<"Outer loop: "<<n<<" "<<istep<<" "<<mmax<<endl;

      for(m=1; m<mmax; m+=2){
         //cerr<<"Inner loop: "<<m<<endl;
         for(i = m; i<=n; i+=istep){
            j = i + mmax;
            //cerr<<"Calculating: "<<i<<" "<<j<<endl;
            tempR = wR*data[j]    - wI * data[j+1];
            tempI = wR *data[j+1] + wI * data[j];
            data[j] = data[i]- tempR;
            data[j+1] = data[i+1]-tempI;
            data[i]  += tempR;
            data[i+1]+= tempI;
         }
         wR = (wtemp = wR ) * wpR - wI * wpI * wR;
         wI = wI * wpR + wtemp*wpI + wI;
      }
      mmax = istep;
   }
   //cerr<<"Done."<<endl;
   return data;
}

float *Fourier::fourier(float data[], unsigned long nn, int isign)
{
   float tmp;
   four1(data-1, nn, isign);
   for(uint i = 0; i<nn; ++i){
      //cerr<<"i: "<<2*i<<" Re: "<<data[2*i]<<" Im:"<<data[2*i+1]<<endl;
      tmp  = data[i];   data[i]  = data[2*i];   data[2*i]   = tmp;
      tmp  = data[i+1]; data[i+1]= data[2*i+1]; data[2*i+1] = tmp;
      //cerr<<"i: "<<2*i<<" Re: "<<data[2*i]<<" Im:"<<data[2*i+1]<<endl;
   }

   return data;
}

double * Fourier::four1(double data[], unsigned long nn, int isign)
{
   unsigned long n, mmax, m, j, istep, i;
   double wtemp, wR, wI, wpR, wpI, theta;
   double tempR, tempI;
   n = nn<<1;
   j = 1;
   for (i = 1; i<n; i+= 2){
      if(j>i){
         tempR = data[j]; data[j] = data[i]; data[i] = tempR;
         tempR = data[j+1]; data[j+1] = data[i+1];  data[i+1]= tempR;
      }
      m = nn;
      while( m>=2 && j>m){
         j-=m;
         m>>=1;
      }
      j += m;
   }
   mmax = 2;
   while(n>mmax){
      istep = mmax<<1;
      theta = isign * (TWOPI/mmax);
      wtemp = sin(0.5*theta);
      wpR = -2.0 * wtemp*wtemp;
      wpI = sin(theta);
      wR = 1.0;
      wI = 0.0;

      for(m=1; m<mmax; m+=2){
         //cerr<<"Inner loop: "<<m<<endl;
         for(i = m; i<=n; i+=istep){
            j = i + mmax;
            tempR = wR*data[j]    - wI * data[j+1];
            tempI = wR *data[j+1] + wI * data[j];
            data[j] = data[i]- tempR;
            data[j+1] = data[i+1]-tempI;
            data[i]  += tempR;
            data[i+1]+= tempI;
         }
         wR = (wtemp = wR ) * wpR - wI * wpI * wR;
         wI = wI * wpR + wtemp*wpI + wI;
      }
      mmax = istep;
   }
   return data;
}

double *Fourier::fourier(double data[], unsigned long nn, int isign)
{
   four1(data-1, nn, isign);
   return data;
}


float  *Fourier::powerspectrum(float  data[], unsigned long nn)
{
   unsigned long N = 2 * nn;
   //four1(data-1, nn, +1);

   data[0]  = data[0]*data[0] + data[1]*data[1];
   for(uint i = 1; i<nn/2+1; ++i ){
      uint ii = 2 * i; 
      data[i] = 0.5*(data[ii]*data[ii] + data[ii+1]*data[ii+1]+
                     data[N-ii]*data[N-ii] + data[N-ii +1]*data[N-ii+1]);
   }
   return data; 
}

double *Fourier::powerspectrum(double data[], unsigned long nn)
{
   unsigned long N = nn + nn;
   //dfour1(data-1, nn, +1);
   data[0]  = data[0]*data[0] + data[1]*data[1];
   for(uint i = 1; i<nn/2; ++i ){
      uint ii = i+i; 
      data[i] = 0.5*(data[ii]*data[ii] + data[ii+1]*data[ii+1]+
                     data[N-ii]*data[N-ii] + data[N-ii +1]*data[N-ii+1]);

   }
   return data; 

}

void Fourier::twoFFT(float data1[], float data2[], float fft1[], float fft2[], unsigned long n)
{
   unsigned long nn3,nn2,jj,j;
   float rep, rem, aip, aim;
   nn3 = 1 + ( nn2 = 2 + n + n);
   for(j = 1, jj = 2; j <= n; j++, jj += 2){
      fft1[jj-1] = data1[j];
      fft1[jj]    = data2[j];
   }

   Fourier::four1(fft1,n,1);
   fft2[1] = fft1[2];
   fft1[2] = fft2[2] = 0.0;
   for ( j = 3; j<=n+1;j+=2){
      rep = 0.5 *(fft1[j] + fft1[nn2-j]);
      rem = 0.5 *(fft1[j] - fft1[nn2-j]);
      aip = 0.5 *(fft1[j+1]+fft1[nn3-j]);
      aim = 0.5 *(fft1[j+1]-fft1[nn3-j]);
      fft1[j]     = rep;
      fft1[j+1]   = aim;
      fft1[nn2-j] = rep;
      fft1[nn3-j] = -aim;
      fft2[j]     = aip;
      fft2[j+1]   = -rem;
      fft2[nn2-j] = aip;
      fft2[nn3-j] = rem;

   }
}

void Fourier::twoFFT(double data1[], double data2[], double fft1[], double fft2[], unsigned long n)
{
   unsigned long nn3,nn2,jj,j;
   double rep, rem, aip, aim;
   nn3 = 1 +(nn2=2+n+n);
   for(j=1, jj=2; j<=n; j++, jj+=2){
      fft1[jj-1] = data1[j];
      fft1[jj]    = data2[j];
   }

   Fourier::four1(fft1,n,1);
   fft2[1] = fft1[2];
   fft1[2] = fft2[2] = 0.0;
   for ( j = 3; j<=n+1;j+=2){
      rep = 0.5 *(fft1[j] + fft1[nn2-j]);
      rem = 0.5 *(fft1[j] - fft1[nn2-j]);
      aip = 0.5 *(fft1[j+1]+fft1[nn3-j]);
      aim = 0.5 *(fft1[j+1]-fft1[nn3-j]);
      fft1[j]     = rep;
      fft1[j+1]   = aim;
      fft1[nn2-j] = rep;
      fft1[nn3-j] = -aim;
      fft2[j]     = aip;
      fft2[j+1]   = -rem;
      fft2[nn2-j] = aip;
      fft2[nn3-j] = rem;

   }
}

float *Fourier::realfft(float data[], unsigned long n, int isign)
{

   unsigned long i, i1, i2, i3, i4, np3;
   float c1 = 0.5, c2, h1r, h1i, h2r, h2i;
   double wr, wi, wpr, wpi, wtemp, theta;
   theta = PI/(double) (n>>1);
   if( isign == 1){
      c2 = -0.5;
      four1(data, n>>1,1);
   }
   else{
      c2 = 0.5;
      theta = -theta;
   }

   wtemp = sin(0.5*theta);
   wpr   = -2.0 * wtemp * wtemp;
   wpi   = sin(theta);
   wr = 1.0 + wpr;
   wi = wpi;
   np3 = n + 3;
   for(i = 2; i<=(n>>2);++i){
      i4 = i + (i3 = np3 -(i2 = 1 + (i1 = i + i -1 )));
      h1r =  c1 * (data[i1] + data[i3]);
      h1i =  c1 * (data[i2] + data[i4]);
      h2r = -c2 * (data[i2] - data[i4]);
      h2i =  c2 * (data[i1] - data[i3]);
      data[i1] =  h1r + wr * h2r - wi * h2i;
      data[i2] =  h1i + wr * h2i + wi * h2r;
      data[i3] =  h1r - wr * h2r + wi * h2i;
      data[i4] = -h1i + wr * h2i + wi * h2r;
      wr = (wtemp = wr) * wpr -   wi  * wpi + wr;
      wi =           wi * wpr + wtemp * wpi + wi;
   }
   if(isign == 1){
      data[1] = (h1r = data[1]) +data[2];
      data[2] = h1r - data[2];
   }
   else{
      data[1] = c1 * ( (h1r = data[1]) +data[2]);
      data[2] = c1 * ( h1r -data [2]);
      four1(data, n>>1, -1);
   }

   return data;
}

float *Fourier::realFFT(float data[], unsigned long n, int isign)
{
   return realfft(data-1, n, isign);
}

double *Fourier::realfft(double data[], unsigned long n, int isign)
{
   unsigned long i, i1, i2, i3, i4, np3;
   double c1 = 0.5, c2, h1r, h1i, h2r, h2i;
   double wr, wi, wpr, wpi, wtemp, theta;
   theta = PI/(double) (n>>1);
   if( isign == 1){
      c2 = -0.5;
      four1(data, n>>1,1);
   }
   else{
      c2 = 0.5;
      theta = -theta;
   }

   wtemp = sin(0.5*theta);
   wpr   = -2.0 * wtemp * wtemp;
   wpi   = sin(theta);
   wr = 1.0 + wpr;
   wi = wpi;
   np3 = n + 3;
   for(i = 2; i<=(n>>2);++i){
      i4 = 1 + (i3 = np3 -(i2 = 1 + (i1 = i + i -1 )));
      h1r =  c1 * (data[i1] + data[i3]);
      h1i =  c1 * (data[i2] + data[i4]);
      h2r = -c2 * (data[i2] - data[i4]);
      h2i =  c2 * (data[i1] - data[i3]);
      data[i1] =  h1r + wr * h2r - wi * h2i;
      data[i2] =  h1i + wr * h2i + wi * h2r;
      data[i3] =  h1r - wr * h2r + wi * h2i;
      data[i4] = -h1i + wr * h2i + wi * h2r;
      wr = (wtemp = wr) * wpr -   wi  * wpi + wr;
      wi =           wi * wpr + wtemp * wpi + wi;
   }
   if(isign == 1){
      data[1] = (h1r = data[1]) +data[2];
      data[2] = h1r - data[2];
   }
   else{
      data[1] = c1 * ((h1r = data[1]) +data[2]);
      data[2] = c1 * ( h1r -data [2]);
      four1(data, n>>1, -1);
   }
   return data;

}

double *Fourier::realFFT(double data[], unsigned long n, int isign)
{
   return realfft(data-1, n, isign);
}


float  *Fourier::realPS(float  data[], float ps[], unsigned long n )
{
   ps[0] = fabs(data[0]);
   ps[n] = fabs(data[1]);
   for(uint i = 1; i<n; ++i){
      ps[i] = fabs(data[i]);
   }

   return ps;
}

double *Fourier::realPS(double data[], double ps[], unsigned long n )
{
   ps[0] = fabs(data[0]);
   ps[n] = fabs(data[1]);
   for(uint i = 1; i<n; ++i){
      ps[i] = fabs(data[i]);
   }

   return ps;
}

//zero based!
float *Fourier::slowRealFour(float data[], unsigned long nn, int isign, float ps[])
{
   unsigned long i = 0;
   float *tmp = new float[nn];
   float hr = 0, hi = 0;
   if( nn<2|| !tmp) return data;
   
   for(i = 0; i<=nn>>1; ++i){
      hr = 0; hi = 0;
      for(uint j = 0; j<nn; ++i){
         hr += data[j] * cos(isign*TWOPI*i*j/nn);
         hi += data[j] * sin(isign*TWOPI*i*j/nn);         
      }
      if( i == 0) {
         tmp[0] = hr;
         ps[0] = fabs(hr);
      }
      else if(i == nn>>1){
         tmp[1] = hr;
         ps[nn>>2] = fabs(hr);
      }
      else{
         tmp[2*i] = hr;
         tmp[2*i+1] = hi;
         ps[i] = 2 * sqrt( hr*hr + hi*hi);
      }
   }
   for(i = 0; i<nn; ++i){
      data[i] = tmp[i];
   }
   return data;
}

double  *Fourier::slowRealFour(double data[], unsigned long nn, int isign, double ps[])
{
   unsigned long i = 0;
   double *tmp = new double[nn];
   float hr = 0, hi = 0;
   
   if( nn<2|| !tmp|| !data) return 0;
   for(i = 0; i<=nn>>1; ++i){
      hr = 0; hi = 0;
      for(uint j = 0; j<nn; ++j){
         hr += data[j] * cos(isign*TWOPI*i*j/nn);
         hi += data[j] * sin(isign*TWOPI*i*j/nn);
         //cerr<<"i = "<<i<<" j = "<<j<<" hr = "<<hr<<" hi = "<<hi<<endl;
      }
      //cerr<<"i = "<<i<<" hr = "<<hr<<" hi = "<<hi<<endl;
      if( i == 0) {
         tmp[0] = hr;
         ps[0]  = fabs(hr);
      }
      else if(i == nn>>1){
         tmp[1] = hr;
         ps[nn>>1] = 2*fabs(hr);
      }
      else{
         tmp[2*i] = hr;
         tmp[2*i+1] = hi;
         ps[i] = 2 * sqrt( hr*hr + hi*hi);
      }
   }
   for(i = 0; i<nn; ++i){
      data[i] = tmp[i];
   }
   delete [] tmp;
   return data;
}
