fac.c

linear time－frequency toolbox

#include "../config.h"
#ifdef HAVE_COMPLEX_H
#include <complex.h>
#endif
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <fftw3.h>

/* FFTW_ESTIMATE is the only one to work, as the other types destroys input */
#define FFTW_OPTITYPE FFTW_ESTIMATE

void print_z(const int N, fftw_complex *p)
{
  int i;
  
  for(i=0;i<N;i++)
  {
#ifdef HAVE_COMPLEX_H
    printf("%.16lf  %.16lf\n",creal(p[i]),cimag(p[i]));
#else
    printf("%.16lf  %.16lf\n",p[i][0],p[i][1]);
#endif
  }
  fflush(NULL);
  
}
/* Extended Euclid algorithm. */
int gcd (const int a, const int b, int *r, int *s )
{
  int a1 = a;
  int b1 = b;
  int a2 = 1;
  int b2 = 0;
  int a3 = 0;
  int b3 = 1;
  int c, d;
  while ( b1 != 0 ) 
  {
      d=a1/b1;
      c = a1;
      a1 = b1;
      b1 = c-d*b1;

      c = a2;
      a2 = b2;
      b2 = c-d*b2;
      
      c = a3;
      a3 = b3;
      b3 = c-d*b3;
      
  }
   
  *r=a2;
  *s=a3;
  return a1;
}


void wfac(fftw_complex *g, const int L, const int R, const int a, const int M,
	  fftw_complex *gf)
{
  
   int b, N, c, d, p, q, h_a, h_m;
   int *permutation, *P;
   
   int w, k, l, r, s, ld2, ld3;
   div_t domod;

   double sqrtM;

   fftw_plan p_before;   
   
   b=L/M;
   N=L/a;
   
   c=gcd(a, M,&h_a, &h_m);
   p=a/c;
   q=M/c;
   d=b/p;

   sqrtM=sqrt(M);
   
   /* for testing, set ldf=L. */
   const int ldf=L;
     
   /* Create plans. In-place. 
    *
    * THIS WILL NOT CORRECTLY HANDLE MULTIWINDOWS
    *
    */
   p_before = fftw_plan_many_dft(1, &d, c*p*q*R,
				 gf, NULL,
				 c*p*q*R, 1,
				 gf, NULL,
				 c*p*q*R, 1,
				 FFTW_FORWARD, FFTW_OPTITYPE);
   


   ld2=  p*q*R;
   ld3=c*p*q*R;
   for (w=0;w<R;w++)
   {
      for (s=0;s<d;s++)
      {
	for (l=0;l<q;l++)
	  {
	     for (k=0;k<p;k++)
	     {
		/*gf(k+1,l+1+q*w,r+1,s+1)=g(r+mod(k*M-l*a+s*p*M,L)+1,w+1);*/
		/*Add L to make sure it is positive */
		domod= div(k*M-l*a+s*p*M+L, L);
		for (r=0;r<c;r++)
		{
#ifdef HAVE_COMPLEX_H
		   gf[k+(l+q*w)*p+r*ld2+s*ld3]=sqrtM*g[r+domod.rem+L*w];
#else
		   gf[k+(l+q*w)*p+r*ld2+s*ld3][0]=sqrtM*g[r+domod.rem+L*w][0];
		   gf[k+(l+q*w)*p+r*ld2+s*ld3][1]=sqrtM*g[r+domod.rem+L*w][1];
#endif
		}
	     }
	  }
      }
   }

   fftw_execute(p_before);	  

}


void iwfac(fftw_complex *gf, const int L, const int R,
	   const int a, const int M, fftw_complex *g)
{
   
   int b, N, c, d, p, q, h_a, h_m, ld2, ld3;
   
   int l, k, r, s, w;
   div_t domod;
   
   double scaling;
   
   fftw_complex *tmp_gf;

   b=L/M;
   N=L/a;
   
   c=gcd(a, M,&h_a, &h_m);
   p=a/c;
   q=M/c;
   d=b/p;

   /* division by d is because of the way FFTW normalizes the transform. */
   scaling=1.0/sqrt(M)/d;
   
   fftw_plan p_before;
   
   /* for testing, set ldf=L. */
   const int ldf=L;

   /* Create plans. In-place.
    * We must copy data to a temporary variable in order not
    * to destroy the input.
    */
   tmp_gf = fftw_malloc(sizeof(fftw_complex) * L*R);

   /* This plan is not in-place. It copies to the work array. */
   p_before = fftw_plan_many_dft(1, &d, c*p*q*R,
				 gf, NULL,
				 c*p*q*R, 1,
				 tmp_gf, NULL,
				 c*p*q*R, 1,
				 FFTW_BACKWARD, FFTW_OPTITYPE);
   
  
  
   fftw_execute(p_before);	  

   ld2=p*q*R;
   ld3=c*p*q*R;

   for (w=0;w<R;w++)
   {
      for (s=0;s<d;s++)
      {
	 for (l=0;l<q;l++)
	 {
	    for (k=0;k<p;k++)
	    {
	       /*gf(k+1,l+1+q*w,r+1,s+1)=g(r+mod(k*M-l*a+s*p*M,L)+1,w+1);*/
	       /*Add L to make sure it is positive */
	       domod= div(k*M-l*a+s*p*M+L, L);
	       for (r=0;r<c;r++)
	       {	
#ifdef HAVE_COMPLEX_H	  
		  g[r+domod.rem+L*w]    = tmp_gf[k+(l+q*w)*p+r*ld2+s*ld3]*scaling;
#else
		  g[r+domod.rem+L*w][0] = tmp_gf[k+(l+q*w)*p+r*ld2+s*ld3][0]*scaling;
		  g[r+domod.rem+L*w][1] = tmp_gf[k+(l+q*w)*p+r*ld2+s*ld3][1]*scaling;
#endif
	       }
	    }
	 }
      }
   }           
   
   /* Clear the work-array. */
   fftw_free(tmp_gf);
}


void dgt_fw(fftw_complex *f, fftw_complex *gf, const int L, const int W,
	    const int R, const int a, const int M, fftw_complex *cout)
{

   /*  --------- initial declarations -------------- */

   int b, N, c, d, p, q, h_a, h_m;
   
   fftw_complex *gbase, *fbase, *cbase;

   int l, k, r, s, u, w, rw, nm, mm, km;
   int ld1, ld2, ld3;
   div_t domod;
   
   double scaling;

   /*  ----------- calculation of parameters and plans -------- */
   
   b=L/M;
   N=L/a;
   
   c=gcd(a, M,&h_a, &h_m);
   p=a/c;
   q=M/c;
   d=b/p;

   h_a=-h_a;

   /*printf("%i %i %i %i %i\n",c,d,p,q,W);*/

   fftw_plan p_before, p_after, p_veryend;

   fftw_complex *ff, *cf;

   ff = fftw_malloc(sizeof(fftw_complex) * L*W);
   cf = fftw_malloc(sizeof(fftw_complex) * c*d*q*q*W*R);

   /* Create plans. In-place. */
   
   p_before = fftw_plan_many_dft(1, &d, c*p*q*W,
				 ff, NULL,
				 c*p*q*W, 1,
				 ff, NULL,
				 c*p*q*W, 1,
				 FFTW_FORWARD, FFTW_OPTITYPE);


   p_after = fftw_plan_many_dft(1, &d, c*q*q*W*R,
				cf, NULL,
				c*q*q*W*R, 1,
				cf, NULL,
				c*q*q*W*R, 1,
				FFTW_BACKWARD, FFTW_OPTITYPE);
   

   /*  ---------- compute signal factorization ----------- */

   /* Leading dimensions of the 4dim array. */
   ld2=p*q*W;
   ld3=c*p*q*W;

   for (w=0;w<W;w++)
   {
      for (s=0;s<d;s++)
      {
	 for (l=0;l<q;l++)
	 {
	    for (k=0;k<p;k++)
	    {
	       /* Add L*M to make sure it is always positive */
	       domod = div(k*M+s*p*M+l*(c-h_m*M)+L*M, L);
	       
	       for (r=0;r<c;r++)
	       {		  
		  /* ff(k+1,l+1+w*q,r+1,s+1)=f(r+1+mod(k*M+s*p*M+l*(c-h_m*M),L),w+1);*/
#ifdef HAVE_COMPLEX_H
		  ff[k+(l+q*w)*p+r*ld2+s*ld3]=f[r+domod.rem+L*w];
#else
		  ff[k+(l+q*w)*p+r*ld2+s*ld3][0]=f[r+domod.rem+L*w][0];
		  ff[k+(l+q*w)*p+r*ld2+s*ld3][1]=f[r+domod.rem+L*w][1];
#endif
	       }
	    }
	 }
      }
   }           

   
   /* Do fft to complete signal factorization.*/
   fftw_execute(p_before);

   /* ----------- compute matrix multiplication ----------- */


   /* Do the matmul  */
   for (r=0;r<c;r++)
   {
      for (s=0;s<d;s++)
      {	
	 gbase=gf+(r+s*c)*p*q*R;
	 fbase=ff+(r+s*c)*p*q*W;
	 cbase=cf+(r+s*c)*q*q*W*R;

	 for (nm=0;nm<q*W;nm++)
	 {
	    for (mm=0;mm<q*R;mm++)
	    {
#ifdef HAVE_COMPLEX_H
	       cbase[mm+nm*q*R]=0.0;
	       for (km=0;km<p;km++)
	       {
		 cbase[mm+nm*q*R]+=conj(gbase[km+mm*p])*fbase[km+nm*p];
	       }
	       /* Scale because of FFTWs normalization. */
	       cbase[mm+nm*q*R]=cbase[mm+nm*q*R]/d;
#else
	       cbase[mm+nm*q*R][0]=0.0;
	       cbase[mm+nm*q*R][1]=0.0;
	       for (km=0;km<p;km++)
	       {
		  cbase[mm+nm*q*R][0]+=gbase[km+mm*p][0]*fbase[km+nm*p][0]+gbase[km+mm*p][1]*fbase[km+nm*p][1];
		  cbase[mm+nm*q*R][1]+=gbase[km+mm*p][0]*fbase[km+nm*p][1]-gbase[km+mm*p][1]*fbase[km+nm*p][0];
	       }
	       /* Scale because of FFTWs normalization. */
	       cbase[mm+nm*q*R][0]=cbase[mm+nm*q*R][0]/d;
	       cbase[mm+nm*q*R][1]=cbase[mm+nm*q*R][1]/d;

#endif
	    }		  
	 }	      	 
      }
   }

   /*  -------  compute inverse coefficient factorization ------- */

   /* Do inverse fft of length d */
   fftw_execute(p_after);

   /* Leading dimensions of cf */
   ld1=q*R;
   ld2=q*R*q*W;
   ld3=c*q*R*q*W;
         
   /* Complete inverse fac of coefficients */
   for (rw=0;rw<R;rw++)
   {
      for (w=0;w<W;w++)
      {
	 for (s=0;s<d;s++)
	 {
	    for (l=0;l<q;l++)
	    {
	       for (u=0;u<q;u++)
	       {	       
		  /*Add N to make sure it is positive */
		  domod= div(u+s*q-l*h_a+N*M,N);
		  for (r=0;r<c;r++)
		  {	
#ifdef HAVE_COMPLEX_H	  
		     cout[r+l*c+domod.rem*M+rw*M*N+w*M*N*R]=cf[u+rw*q+(l+q*w)*ld1+r*ld2+s*ld3];
#else
		     cout[r+l*c+domod.rem*M+rw*M*N+w*M*N*R][0]=cf[u+rw*q+(l+q*w)*ld1+r*ld2+s*ld3][0];
		     cout[r+l*c+domod.rem*M+rw*M*N+w*M*N*R][1]=cf[u+rw*q+(l+q*w)*ld1+r*ld2+s*ld3][1];
#endif
		  }
	       }
	    }
	 }
      }      
   }     

    /* ------------  Clean up  -------------------*/   
   fftw_free(ff);
   fftw_free(cf);
   
}




void idgt_fw(fftw_complex *cin, fftw_complex *gf, const int L, const int W,
	     const int R, const int a, const int M, fftw_complex *f)
{

   /*  --------- initial declarations -------------- */

   int b, N, c, d, p, q, h_a, h_m;
   
   fftw_complex *gbase, *fbase, *cbase;

   int l, k, r, s, u, w, rw, nm, mm, km;
   int ld1, ld2, ld3;

   div_t domod;
   
   double scaling;
   
   /*  ----------- calculation of parameters and plans -------- */

   b=L/M;
   N=L/a;
   
   c=gcd(a, M,&h_a, &h_m);
   p=a/c;
   q=M/c;
   d=b/p;

   h_a=-h_a;

   fftw_plan p_before, p_after, p_veryend;

   fftw_complex *ff, *cf;

   ff = fftw_malloc(sizeof(fftw_complex) * L*W);
   cf = fftw_malloc(sizeof(fftw_complex) * c*d*q*q*W*R);

   /* Create plans. In-place. */   
   p_before = fftw_plan_many_dft(1, &d, c*p*q*W,
				 ff, NULL,
				 c*p*q*W, 1,
				 ff, NULL,
				 c*p*q*W, 1,
				 FFTW_BACKWARD, FFTW_OPTITYPE);


   p_after = fftw_plan_many_dft(1, &d, c*q*q*W*R,
				cf, NULL,
				c*q*q*W*R, 1,
				cf, NULL,
				c*q*q*W*R, 1,
				FFTW_FORWARD, FFTW_OPTITYPE);
   


   /* -------- compute coefficient factorization ----------- */

   /* Leading dimensions of the 4dim array. */
   ld1=q*R;
   ld2=q*R*q*W;
   ld3=c*q*R*q*W;
   
   for (rw=0;rw<R;rw++)
   {
      for (w=0;w<W;w++)
      {
	 for (s=0;s<d;s++)
	 {
	    for (l=0;l<q;l++)
	    {
	       for (u=0;u<q;u++)
	       {	       
		  /*Add N to make sure it is positive */
		  domod= div(u+s*q-l*h_a+N*M,N);
		  for (r=0;r<c;r++)
		  {	
#ifdef HAVE_COMPLEX_H	  
		     cf[u+rw*q+(l+q*w)*ld1+r*ld2+s*ld3]    = cin[r+l*c+domod.rem*M+rw*M*N+w*M*N*R];
#else
		     cf[u+rw*q+(l+q*w)*ld1+r*ld2+s*ld3][0] = cin[r+l*c+domod.rem*M+rw*M*N+w*M*N*R][0];
		     cf[u+rw*q+(l+q*w)*ld1+r*ld2+s*ld3][1] = cin[r+l*c+domod.rem*M+rw*M*N+w*M*N*R][1];
#endif
		  }
	       }
	    }
	 }
      }           
   }

   /* Do fft of length d */
   fftw_execute(p_after);


   /* -------- compute matrix multiplication ---------- */
   

   /* Do the matmul  */
   for (r=0;r<c;r++)
   {
      for (s=0;s<d;s++)
      {	
	 gbase=gf+(r+s*c)*p*q*R;
	 fbase=ff+(r+s*c)*p*q*W;
	 cbase=cf+(r+s*c)*q*q*W*R;

	 for (nm=0;nm<q*W;nm++)
	 {
	    for (km=0;km<p;km++)
	    {
#ifdef HAVE_COMPLEX_H
	       fbase[km+nm*p]=0.0;
	       for (mm=0;mm<q*R;mm++)
	       {
		 fbase[km+nm*p]+=gbase[km+mm*p]*cbase[mm+nm*q*R];
	       }
	       /* Scale because of FFTWs normalization. */
	       fbase[km+nm*p]=fbase[km+nm*p]/d;
#else
	       fbase[km+nm*p][0]=0.0;
	       fbase[km+nm*p][1]=0.0;
	       for (mm=0;mm<q*R;mm++)
	       {
		 fbase[km+nm*p][0]+=gbase[km+mm*p][0]*cbase[mm+nm*q*R][0]-gbase[km+mm*p][1]*cbase[mm+nm*q*R][1];
		 fbase[km+nm*p][1]+=gbase[km+mm*p][0]*cbase[mm+nm*q*R][1]+gbase[km+mm*p][1]*cbase[mm+nm*q*R][0];
	       }
	       /* Scale because of FFTWs normalization. */
	       fbase[km+nm*p][0]=fbase[km+nm*p][0]/d;
	       fbase[km+nm*p][1]=fbase[km+nm*p][1]/d;
#endif
	    }		  
	 }	      	 
      }
   }

         


   /* ----------- compute inverse signal factorization ---------- */


   /* Do ifft to begin inverse signal factorization.*/
   fftw_execute(p_before);

   /* Leading dimensions of the 4dim array. */
   ld2=p*q*W;
   ld3=c*p*q*W;

   for (w=0;w<W;w++)
   {
      for (s=0;s<d;s++)
      {
	 for (l=0;l<q;l++)
	 {
	    for (k=0;k<p;k++)
	    {
	       /* Add L*M to make sure it is always positive */
	       domod = div(k*M+s*p*M+l*(c-h_m*M)+L*M, L);
	       
	       for (r=0;r<c;r++)
	       {		  
#ifdef HAVE_COMPLEX_H
		  f[r+domod.rem+L*w] = ff[k+(l+q*w)*p+r*ld2+s*ld3];
#else
		  f[r+domod.rem+L*w][0] = ff[k+(l+q*w)*p+r*ld2+s*ld3][0];
		  f[r+domod.rem+L*w][1] = ff[k+(l+q*w)*p+r*ld2+s*ld3][1];
#endif
	       }
	    }
	 }
      }
   }           

    /* -----------  Clean up ----------------- */   
   fftw_free(ff);
   fftw_free(cf);
   
}