/*
 *
 *                             COPYRIGHT 2007
 *
 * Mika Seppa
 * Low Temperature Laboratory
 * Helsinki University of Technology
 * SF-02150 Espoo
 * FINLAND
 *
 * $Log$
 *
*/

/*
#ifndef __lint
static char rcsid[] = "$Header$";
#endif
*/

#include <math.h>
#include <stdlib.h>

#ifndef M_TWO_PI
#define M_TWO_PI 6.28318530717959
#endif

#define REAL float

typedef struct _fsm {
  unsigned int  n;       /* Original array size  (must be power of two) */
  unsigned int  interp;  /* Interpolation factor (must be power of two) */
  double       *cosines; /* Table of cosines                            */
  double       *sines;   /* Table of sines                              */
  REAL         *imuls_r; /* Real multipliers, n floats   OR             */
  REAL         *imuls_c; /* Complex multipliers, 2*n floats             */
} FSMRec, *FSM;

void fsmi(const FSMRec *fsm, REAL *data) {
  unsigned int n, mmax, m, j, istep, i, nn, interp, li, i2;
  REAL         tempr, tempi, *fp, *gp, *gpp;
  /* Double precision for the trigonometric recurrences. */
  double       wtemp, wr, wpr, wpi, wi, sgn; 

  /* Multiply the complex or real multipliers */
  if (fsm->imuls_c!=NULL) {
    for(i=fsm->n,fp=fsm->imuls_c,gp=data;i-->0;) {
      tempr = *gp; tempi = fp[1];
      *gp = tempr*fp[0] - tempi*gp[1]; gp++;
      *gp = tempr*tempi + fp[0]*gp[0]; gp++;
      fp += 2;
    }
  } else if (fsm->imuls_r!=NULL) {
    for(i=fsm->n,fp=fsm->imuls_r,gp=data;i-->0;) {
      *gp++ *= *fp;
      *gp++ *= *fp++;
    }
  }
    
  sgn = M_TWO_PI;
  n = (nn=fsm->n)*(interp=fsm->interp);
  gp=&data[n<<1];
  fp=&data[nn<<1]-2;

  /* Log of interpolation factor */
  for(i=interp,li=0;(i>>=1);li++);

  i2 = interp<<1;

  if (interp>1) {
    wpr = fsm->cosines[li];
    wpi = -fsm->sines[li];
  
    /*
     * Extend and pad the array. Shortcut through the first
     * FFT iterations
     */
    for(i=(nn>>1);i-->0;) {

      /*
       * Negative frequency, must be multiplied with
       * the exponential term
       */
      gp -= i2;
      gp[1] = fp[1]; gp[0] = fp[0];
      wr  = (double)gp[0];
      wi  = (double)gp[1];
      for(j=interp-1,gpp=gp+2;j-->0;) {
	/* Trigonometric recurrence. */
	wr = (wtemp=wr)*wpr - wi*wpi;
	wi = wi*wpr + wtemp*wpi;
	*gpp++ = (REAL)wr;
	*gpp++ = (REAL)wi;
      }
      fp -= 2;
      
      /* Positive frequency, just copy */
      for(j=interp;j-->0;) { *--gp = fp[1]; *--gp = fp[0]; }
      fp -= 2;
    }
    
    /* Correction for the aliased sample */
    gp = &data[i2];
    if (fsm->imuls_c!=NULL) { /* Complex multipliers? */
      REAL tr, ttr, tti, ti, s;
      
      /*
       * Find the result as if multiplied by the complex
       * conjugate (for the positive freq.)
       */
      ttr = fsm->imuls_c[2]*(REAL)fsm->n;
      tti = fsm->imuls_c[3]*(REAL)fsm->n;
      tempr = ttr*ttr;
      tempi = tti*tti;
      if ((s=tempr+tempi)<1e-6) { tempr = gp[0]; tempi = gp[1]; }
      else {
	s = 1.0/s;
	tr = s*(tempr-tempi);
	ti = -2.0*s*ttr*tti;
	tempr = gp[0]*tr - gp[1]*ti;
	tempi = gp[0]*ti + gp[1]*tr;
	gp[0] = 0.5*(gp[0] + tempr);
	gp[1] = 0.5*(gp[1] + tempi);
      }
    } else { tempr = gp[0]; tempi = gp[1]; }
    for(j=interp-1,gp+=2;j-->0;) {
      *gp = 0.5*(*gp + tempr); gp++;
      *gp = 0.5*(*gp + tempi); gp++;
    }
  }

  /*
   * Here begins the actual Danielson-Lanczos section
   * of the routine.
   */
  nn = n;
  n <<= 1;
  mmax = i2;
  while(n > mmax) { /* Outer loop */
    li++;
    istep = mmax << 1;
    /* Initialize the trigonometric recurrence.*/
    wtemp = fsm->sines[li+1];
    wpr = -2.0*wtemp*wtemp;
    wpi = fsm->sines[li];
    wr  = 1.0;
    wi  = 0.0;

    /* The two nested inner loops. */
    for(m=0;m<mmax;m+=2) {
      for(i=m;i<n;i+=istep) {
	j = i+mmax;
	/* This is the Danielson-Lanczos formula: */
	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;
      }
      /* Trigonometric recurrence. */
      wr += (wtemp=wr)*wpr - wi*wpi;
      wi += wi*wpr + wtemp*wpi;
    }
    mmax = istep;
  }
}