
 #include <math.h>
 #include <fft.h>
 #include <fftw.h>
 #include <mathfunctions.h>
 #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
 
 void FFT::fft_nd(float* data,int* nn,int ndim,int isign) const
 {
   cout << "WARNING -- not using FFTW !\n";
   int i,i1,i2,i3,i2rev,i3rev,ip1,ip2,ip3,ifp1,ifp2;
   int ibit,idim,k1,k2,n,nprev,nrem,ntot;
   float tempi,tempr;
   double theta,wi,wpi,wpr,wr,wtemp;
   
   ntot=1;
   for (idim=1;idim<=ndim;idim++) {
         ntot *=nn[idim];
   }
   nprev=1;
   for (idim=ndim;idim>=1;idim--) {
     n=nn[idim];
     nrem=ntot/(n*nprev);
     ip1=nprev << 1;
     ip2=ip1*n;
     ip3=ip2*nrem;
     i2rev=1;
     for (i2=1;i2<=ip2;i2+=ip1) {
       if (i2<i2rev) {
         for (i1=i2;i1<=i2+ip1-2;i1+=2) {
           for (i3=i1;i3<=ip3;i3+=ip2) {
             i3rev=i2rev+i3-i2;
             SWAP(data[i3],data[i3rev]);
             SWAP(data[i3+1],data[i3rev+1]);
           }
         }
       }
       ibit=ip2 >> 1;
       while (ibit >= ip1 && i2rev > ibit) {
         i2rev-=ibit;
         ibit >>=1;
       }
       i2rev+=ibit;
     }
     ifp1=ip1;
 
     while (ifp1 < ip2) {
       ifp2=ifp1 << 1;
       theta=isign*6.28318530717959/(ifp2/ip1);
       wtemp=sin(0.5*theta);
       wpr=-2.0*wtemp*wtemp;
       wpi=sin(theta);
       wr=1.0;
       wi=0.0;
       for (i3=1;i3<=ifp1;i3+=ip1) {
         for (i1=i3;i1<=i3+ip1-2;i1+=2) {
           for (i2=i1;i2<=ip3;i2+=ifp2) {
             k1=i2;
             k2=k1+ifp1;
             tempr=wr*data[k2]-wi*data[k2+1];
             tempi=wr*data[k2+1]+wi*data[k2];
             data[k2]=data[k1]-tempr;
             data[k2+1]=data[k1+1]-tempi;
             data[k1]+=tempr;
             data[k1+1]+=tempi;
           }
         }
         wr=(wtemp=wr)*wpr-wi*wpi+wr;
         wi=wi*wpr+wtemp*wpi+wi;
       }
       ifp1=ifp2;
     }
     nprev*=n;
   }
 }
 
 int FFT::FFTComplexMap(ComplexMap& cmap,const int& inverse) const
 {
   return (FFTMaps(cmap.GetRe(),cmap.GetIm(),inverse));
 }
 
 int FFT::FFTMaps(SMPMap& mapr,SMPMap& mapc,const int& inverse) const
 {
   double dum;
   int i,j;
   int* dimarray;
   float* cdata;
   int nx=mapr.GetXPixelSize();
   int ny=mapr.GetYPixelSize();
   if (nx<2 || ny<2) return bad_map;
   int tnx=nx,tny=ny;
   while (fmod(tnx,2.0)==0.0) tnx/=2;
   if (tnx>1) return bad_map;
   if (inverse==-1) {
     mapr.ShiftMap(-int(nx/2),-int(ny/2));
     mapc.ShiftMap(-int(nx/2),-int(ny/2));
   }
   cdata=new float[2*nx*ny+1];
   cdata[0]=0.0;
   dimarray=new int[3];
   dimarray[1]=nx;
   dimarray[2]=ny;
   for (i=0,j=1;i<nx*ny;i++,j+=2) { 
     cdata[j]=mapr.GetPixel(i); // note: getPixel may mix up rows
     cdata[j+1]=mapc.GetPixel(i); // note: get Pixel may mix up rows...
   }
   fft_nd(cdata,dimarray,2,inverse);
   for (i=0;i<nx*ny;i++) {
     dum=cdata[2*i+1];
     if (inverse==-1) dum/=nx*ny;
     mapr.SetPixel(i,dum);
     dum=cdata[2*i+2];
     if (inverse==-1) dum/=nx*ny;
     mapc.SetPixel(i,dum);
   }
   //if (inverse==-1) {
   //mapr.ShiftMap(int(nx/2),int(ny/2));
   //mapc.ShiftMap(int(nx/2),int(ny/2));
   //}
   delete[] dimarray;
   delete[] cdata;
   return ok;
 }
 
 int FFT::FFTMaps(const SMPMap& mapin,SMPMap** mapr,SMPMap** mapc) const
 {
   int i,j;
   int* dimarray;
   float* cdata;
   int nx=mapin.GetXPixelSize();
   int ny=mapin.GetYPixelSize();
   if (nx<2 || ny<2) return bad_map;
   int tnx=nx,tny=ny;
   while (fmod(tnx,2.0)==0.0) tnx/=2;
   if (tnx>1) return bad_map;
 
   *mapr=new SMPMap(mapin);
   *mapc=new SMPMap(mapin);
   cdata=new float[2*nx*ny+1];
   cdata[0]=0.0;
   dimarray=new int[3];
   dimarray[1]=nx;
   dimarray[2]=ny;
   for (i=0,j=1;i<nx*ny;i++,j+=2) {
     cdata[j]=mapin.GetPixel(i); 
     cdata[j+1]=0.0;
   }
   fft_nd(cdata,dimarray,2,1);
   for (i=0;i<nx*ny;i++) {
     (*mapr)->SetPixel(i,cdata[2*i+1]);
     (*mapc)->SetPixel(i,cdata[2*i+2]);
   }
   (*mapr)->ShiftMap(int(nx/2),int(ny/2));
   (*mapc)->ShiftMap(int(nx/2),int(ny/2));
   delete[] dimarray;
   delete[] cdata;
   return ok;
 }
 
 
 int FFT::PowerMap(SMPMap& mapin,const int& inverse) const
 {
   int i,j;
   double dum;
   int* dimarray;
   float* cdata;
   int nx=mapin.GetXPixelSize();
   int ny=mapin.GetYPixelSize();
   if (nx<2 || ny<2) return bad_map;
   int tnx=nx,tny=ny;
   while (fmod(tnx,2.0)==0.0) tnx/=2;
   if (tnx>1) return bad_map;
  
   cdata=new float[2*nx*ny+1];
   cdata[0]=0.0;
   dimarray=new int[3];
   dimarray[1]=nx;
   dimarray[2]=ny;
   for (i=0,j=1;i<nx*ny;i++,j+=2) {
     cdata[j]=mapin.GetPixel(i);
     cdata[j+1]=0.0;
   }
   fft_nd(cdata,dimarray,2,inverse);
   for (i=0;i<nx*ny;i++) {
     dum=pow(cdata[2*i+1],2)+pow(cdata[2*i+2],2);
     if (inverse==-1) dum/=nx*ny;
     mapin.SetPixel(i,dum);
   }
   delete[] dimarray;
   delete[] cdata;
   return ok;
 }
 
 SMPMap FFT::PowerMap(const SMPMap& map1) const {
         SMPMap* fftmap1_r,*fftmap1_i;
         FFTMaps(map1,&fftmap1_r,&fftmap1_i);
         SMPMap powermap=(*fftmap1_r)*(*fftmap1_r)+(*fftmap1_i)*(*fftmap1_i);
         delete fftmap1_i; 
         delete fftmap1_r;
         return powermap; 
 } 
 int FFT::FFTWMaps(const SMPMap& mapin,SMPMap** mapr,SMPMap** mapc) const
 {
         int x,y;
         int nx=mapin.GetXPixelSize();
   int ny=mapin.GetYPixelSize();
         fftw_complex in[nx][ny],out[nx][ny];
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         in[x][y].re=mapin.GetPixel(x,y);
                         in[x][y].im=0.0;
                         out[x][y].re=0.0;
                         out[x][y].im=0.0;
                 }
         }
         fftwnd_plan fftplan=fftw2d_create_plan(nx,ny,FFTW_FORWARD,FFTW_ESTIMATE);
         fftwnd_one(fftplan,&in[0][0],&out[0][0]);
   *mapr=new SMPMap(mapin);
   *mapc=new SMPMap(mapin);
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         (*mapr)->SetPixel(x,y,out[x][y].re);
                         (*mapc)->SetPixel(x,y,out[x][y].im);
                 }
         }
         fftwnd_destroy_plan(fftplan);
 }
 int FFT::FFTWMaps(SMPMap& mapr,SMPMap& mapc,const int& inverse) const
 {
         int x,y;
         int nx=mapr.GetXPixelSize();
   int ny=mapr.GetYPixelSize();
         fftw_complex* m=NULL;
         m=(fftw_complex*)calloc(nx*ny,sizeof(fftw_complex));
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         m[y*nx+x].re=mapr[y*nx+x];
                         m[y*nx+x].im=mapc[y*nx+x];
                 }
         }
         fftwnd_plan fftplan;
         if (inverse>0) fftplan=fftw2d_create_plan(nx,ny,FFTW_FORWARD,FFTW_ESTIMATE | FFTW_IN_PLACE);
         else fftplan=fftw2d_create_plan(nx,ny,FFTW_BACKWARD,FFTW_ESTIMATE | FFTW_IN_PLACE);
         fftwnd_one(fftplan,&m[0],NULL);
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         mapr[y*nx+x]=m[y*nx+x].re;
                         mapc[y*nx+x]=m[y*nx+x].im;
                 }
         }
         fftwnd_destroy_plan(fftplan);
         free(m);
 }
 SMPMap FFT::PowerMap(const SMPMap& map1,const SMPMap& map2) const {
   SMPMap* fftmap1_r,*fftmap1_i;
   SMPMap* fftmap2_r,*fftmap2_i;
   FFTMaps(map1,&fftmap1_r,&fftmap1_i);
   FFTMaps(map2,&fftmap2_r,&fftmap2_i);
   SMPMap powermap=(*fftmap1_r)*(*fftmap2_r)+(*fftmap1_i)*(*fftmap2_i);
   delete fftmap1_i;
   delete fftmap1_r;
   delete fftmap2_i;
   delete fftmap2_r;
   return powermap;
 }
 SMPMap FFT::TwoPointCorrelation(const SMPMap& map1) const {
   SMPMap pmap=PowerMap(map1);
   SMPMap dummy=pmap;
   dummy.Fill(0.0);
   FFTMaps(pmap,dummy,-1);
   pmap.SetMiddle(vect(0.0,0.0));
   pmap.ShiftMap(pmap.GetXPixelSize()/2,pmap.GetYPixelSize()/2);
   return pmap;
 }
 SMPMap FFT::TwoPointCorrelation(const SMPMap& map1,const SMPMap& map2) const {
   SMPMap pmap=PowerMap(map1,map2);
   SMPMap dummy=pmap;
   dummy.Fill(0.0);
   FFTMaps(pmap,dummy,-1);
   pmap.SetMiddle(vect(0.0,0.0));
   pmap.ShiftMap(pmap.GetXPixelSize()/2,pmap.GetYPixelSize()/2);
   return pmap;
 }
 
 SMPMap FFT::ConvolveMaps(SMPMap& map1,SMPMap& mapi) const {
         SMPMap map2=mapi;
         double sum=Abs(map2).Sum();
         map2.ShiftMap(-map2.GetXPixelSize()/2,-map2.GetYPixelSize()/2);
         //map2=map2/sum;
         int nx=map1.GetXPixelSize();
         int ny=map1.GetYPixelSize();
         fftwnd_plan forw,backw; 
         int x,y;
         if (nx!=map2.GetXPixelSize() || ny!=map2.GetYPixelSize()) {
                 cerr << "Error -- maps not the same size in FFT !\n";
                 if (nx<map2.GetXPixelSize()) {nx=map2.GetXPixelSize();}
                 if (ny<map2.GetXPixelSize()) {ny=map2.GetYPixelSize();}
         }
         float* fm1=NULL;
         float* fm2=NULL;
         fftw_complex* m=NULL;
         m=(fftw_complex*)calloc(nx*ny,sizeof(fftw_complex));
         fm1=(float*)calloc(nx*ny,sizeof(float));
         fm2=(float*)calloc(nx*ny,sizeof(float));
         if (!fm1 || !fm2 || !m) {
                 cerr << "out of memory in fft \n";
                 exit(0);
         }
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         m[y*nx+x].re=map1.GetPixel(x,y);
                         m[y*nx+x].im=0.0;
                 }
         }
         forw = fftw2d_create_plan(ny, nx, FFTW_FORWARD,FFTW_ESTIMATE | FFTW_IN_PLACE);
         backw = fftw2d_create_plan(ny, nx, FFTW_BACKWARD,FFTW_ESTIMATE | FFTW_IN_PLACE);
         fftwnd_one(forw, &m[0], NULL);
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         fm1[y*nx+x]=m[y*nx+x].re;                       
                         fm2[y*nx+x]=m[y*nx+x].im;                       
                         //m[x][y].re=fm2[x][y];//map2.GetPixel(x,y);
                         m[y*nx+x].re=map2.GetPixel(x,y);
                         m[y*nx+x].im=0.0;
                 }
         }
         fftwnd_one(forw, &m[0], NULL);
 
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                         double dum=m[y*nx+x].re;
                         m[y*nx+x].re=fm1[y*nx+x]*m[y*nx+x].re-fm2[y*nx+x]*m[y*nx+x].im;
                         m[y*nx+x].im=fm1[y*nx+x]*m[y*nx+x].im+dum*fm2[y*nx+x];
                 }
         }
                                                                                               
         fftwnd_one(backw, &m[0], NULL);
         SMPMap result(map1);
         for (x=0;x<nx;x++) {
                 for (y=0;y<ny;y++) {
                     result.SetPixel(x,y,m[y*nx+x].re/(nx*ny));// over nx*ny ?
                 }
         }         
         fftwnd_destroy_plan(backw);
         fftwnd_destroy_plan(forw);      
         //result.ShiftMap(nx/2,ny/2);
         free(m);
         free(fm1);
         free(fm2);      
         return result;
 }
 
 /*SMPMap FFT::ConvolveMaps(const SMPMap& map1,const SMPMap& map2) const {
   SMPMap* fftmap1_r,*fftmap1_i;
   SMPMap* fftmap2_r,*fftmap2_i;
   FFTMaps(map1,&fftmap1_r,&fftmap1_i);
   FFTMaps(map2,&fftmap2_r,&fftmap2_i);
   SMPMap invfftmap_r=(*fftmap1_r)*(*fftmap2_r)-(*fftmap1_i)*(*fftmap2_i);
   SMPMap invfftmap_i=(*fftmap1_r)*(*fftmap2_i)+(*fftmap1_i)*(*fftmap2_r);
   FFTMaps(invfftmap_r,invfftmap_i,-1);
   delete fftmap1_i;
   delete fftmap1_r;
   delete fftmap2_i;
   delete fftmap2_r;
   invfftmap_r.ShiftMap(invfftmap_r.GetXPixelSize()/2,invfftmap_r.GetYPixelSize()/2);
   return invfftmap_r;
 }*/
