/*
 * MLFFT.C - Fast Fourier Transform routines for PML
 *
 * Source Version: 2.0
 * Software Release #92-0043
 *
 */

#include "cpyright.h"

#include "pml.h"

#define SWAP(a, b) tempr = (a);(a) = (b);(b) = tempr

void
 SC_DECLARE(PM_even_space,
            (REAL *x, int n, double xmin, double xmax)),
 SC_DECLARE(PM_even_space_y, 
            (REAL *x, REAL *y, REAL *ax, REAL *ay, int n)),
 SC_DECLARE(_PM_fft_fin,
         (complex *y, REAL *x, int nh,
          double xmn, double xmx, int ordr));

complex
 SC_DECLARE(*PM_fft_sc_real, (REAL *x, int n, int flag));

static int
 SC_DECLARE(_PM_fft_sc_real_hsp, (REAL *f, int n, int sgn));

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_EVEN_SPACE - compute evenly spaced x values for the given x array */

void PM_even_space(x, n, xmin, xmax)
   REAL *x;
   int n;
   double xmin, xmax;
   {REAL step;
    int i;

    step = (xmax - xmin)/(n - 1);
    for (i = 0; i < n; i++)
        x[i] = xmin + step*i;

    x[n-1] = xmax;

    return;}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_EVEN_SPACE_Y - compute evenspaced y values for the given (x, y) pairs
 *                 - x,y    input x and y values
 *                 - ax,ay  ouput evenspaced x and y values
 *                 - n      number of points in evenspaced x array
 */

void PM_even_space_y(x, y, ax, ay, n)
   REAL *x, *y, *ax, *ay;
   int   n;
   {int i;
    REAL xta, xtb, xtc, yta, ytb;

    for (i = 0; i < n; i++)
        {while (ax[i] > x[1])
            {x++;
             y++;};
         PM_interp(ay[i], ax[i], *x, *y, x[1], y[1]);};
 
    return;}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_NEXT_EXP_TWO - return the largest M such that 2^M <= N  */

int PM_next_exp_two(n)
   int n;
   {int i, m;

    for (m = 0, i = n; i > 1; i >>= 1, m++);

    return(m);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_NEXT_POWER_TWO - return the smallest integer, P,  such that
 *                   - |P| >= |N| and |P| is an integer power of 2
 */

int PM_next_power_two(n)
   int n;
   {int sgn, p;

    sgn = 1;
    if (n != 0)
       {sgn  = abs(n)/n;
	n   *= sgn;};

    for (p = 1; n > p; p <<= 1);

    return(sgn*p);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_NEAR_POWER - return the number nearest N which is a power of A */

int PM_near_power(n, a)
   int n, a;
   {int i, d;

    i = PM_round(log((double) n)/log((double) a));
    d = (int) (POW((double) a, (double) i) + .999);

    return(d);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_FFT_SC_COMPLEX - perform an FFT on a scalar set of complex data
 *                   - DATA is an array of N complex values
 *                   - if FLAG > 0 do FFT and do inv(FFT) otherwise
 *                   - return TRUE iff successful
 */

int PM_fft_sc_complex(x, n, flag)
   complex *x;
   int n, flag;
   {int m, mmax, istep;
    complex temp, w, dw;
    unsigned int i, j;
    REAL nrm, dth;

    m = PM_next_exp_two(n);

/* permute series */
    for (i = 0; i < n; i++)
        {j = SC_bit_reverse(i, m);
         if (i < j)
            {temp = x[j];
             x[j] = x[i];
             x[i] = temp;};};
        
/* transform series */
    mmax = 1;
    while (mmax < n)
       {istep = mmax << 1;
        dth   = PI*flag/mmax;
        dw    = PM_COMPLEX(cos(dth), sin(dth));
        w     = PM_COMPLEX(1.0, 0.0);
        for (m = 1; m <= mmax; m++)
            {for (i = m-1; i < n; i += istep)
                 {j = i + mmax;
                  temp = PM_TIMES_CC(w, x[j]);
                  x[j] = PM_MINUS_CC(x[i], temp);
                  x[i] = PM_PLUS_CC(x[i], temp);};
             w = PM_TIMES_CC(w, dw);};
        mmax = istep;};

/* fix normalization */
    if (flag == 1)
       nrm = 2.0/((double) n);
    else
       nrm = 0.5;

    for (i = 0; i < n; i++)
        x[i] = PM_TIMES_RC(nrm, x[i]);

    return(TRUE);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_FFT_SC_REAL - perform an FFT on a scalar set of real data
 *                - X is an array of N (N must power of 2) evenly spaced
 *                - real values (use PM_fft_sc_real_data for standard data
 *                - sets encountered in practice)
 *                - if FLAG > 0 do FFT and do inv(FFT) otherwise
 *                - return array of complex values iff successful
 *                - return NULL otherwise
 *                - formerly in ULTRA
 */

complex *PM_fft_sc_real(x, n, flag)
   REAL *x;
   int n, flag;
   {int i;
    complex *cx;

    cx = FMAKE_N(complex, n + 1, "PM_FFT_SC_REAL:cx");
    if (cx == NULL)
       {sprintf(PM_error, "CAN'T ALLOCATE SPACE - PM_FFT_SC_REAL");
        return(NULL);};

    for (i = 0; i <= n; i++)
        cx[i] = PM_COMPLEX(x[i], 0.0);

    PM_fft_sc_complex(cx, n, flag);

    return(cx);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_FFT_SC_REAL_DATA - do FFT on set of real data which is not necessarily
 *                     - evenly spaced
 *                     - returned via the argument list are the frequency
 *                     - values from most negative to most positive
 *                     - and the complex transform values can be in the same
 *                     - order as the frequency array (this is counter to the
 *                     - standard FFT practice but is the way one thinks of
 *                     - the transform data (this is controlled by ORDR)
 *                     - return the number of points in the transform arrays
 *                     - if successful and 0 otherwise
 */

int PM_fft_sc_real_data(pcy, px, ipx, ipy, n, xmn, xmx, ordr)
   complex **pcy;
   REAL **px, *ipx, *ipy;
   int n;
   double xmn, xmx;
   int ordr;
   {int nh, np;
    REAL *x, *y;
    complex *cy;

    n  = PM_near_power(n, 2);
    np = n + 1;
    nh = n >> 1;

/* allocate space for arrays */
    x = FMAKE_N(REAL, np, "PM_FFT_SC_REAL_DATA:x");
    y = FMAKE_N(REAL, np, "PM_FFT_SC_REAL_DATA:y");
    if ((x == NULL) || (y == NULL))
       {sprintf(PM_error, "CAN`T ALLOCATE MEMORY - PM_FFT_SC_REAL_DATA");
        return(0);}
        
/* generate even spaced x values for interpolation */
    PM_even_space(x, n, xmn, xmx);

/* interpolate to find even spaced y's */
    PM_even_space_y(ipx, ipy, x, y, n);

    cy = PM_fft_sc_real(y, n, 1);

    _PM_fft_fin(cy, x, nh, xmn, xmx, ordr);

    *pcy = cy;
    *px  = x;

/* shouldn't y be freed now? */
    SFREE(y);

    return(np);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_FFT_SC_COMPLEX_DATA - do FFT on set of complex data which is not
 *                        - necessarily evenly spaced
 *                        - FLAG is 1 for FFT and -1 for inv(FFT)
 *                        - returned via the argument list are the frequency
 *                        - values from most negative to most positive
 *                        - and the complex transform values can be in the
 *                        - same order as the frequency array (this is
 *                        - counter to the standard FFT practice but is
 *                        - the way one thinks of the transform data
 *                        - (this is controlled by ORDR)
 *                        - return the number of points in the transform
 *                        - arrays if successful and 0 otherwise
 */

int PM_fft_sc_complex_data(pcy, px, ipx, ipy, n, xmn, xmx, flag, ordr)
   complex **pcy;
   REAL **px, *ipx;
   complex *ipy;
   int n;
   double xmn, xmx;
   int flag, ordr;
   {int i, nh, np, no, nr;
    REAL dt;
    REAL *x, *ya, *yb;
    complex *cy;

    no = n;
    n  = PM_near_power(n, 2);
    np = n + 1;
    nh = n >> 1;

/* allocate space for arrays */
    ya = FMAKE_N(REAL, no, "PM_FFT_SC_COMPLEX_DATA:ya");
    yb = FMAKE_N(REAL, np, "PM_FFT_SC_COMPLEX_DATA:yb");
    x  = FMAKE_N(REAL, np, "PM_FFT_SC_COMPLEX_DATA:x");
    cy = FMAKE_N(complex, np, "PM_FFT_SC_COMPLEX_DATA:cy");
    if ((x == NULL) || (cy == NULL) ||
        (ya == NULL) || (yb == NULL))
       {sprintf(PM_error, "CAN`T ALLOCATE MEMORY - PM_FFT_SC_COMPLEX_DATA");
        return(0);};

    *px  = x;
    *pcy = cy;

/* generate evenly spaced x values for interpolation */
    if (flag == 1)
       PM_even_space(x, n, xmn, xmx);
    else
       PM_even_space(x, np, xmn, xmx);

/* interpolate the real part of y */
    for (i = 0; i < no; i++)
        ya[i] = PM_REAL_C(ipy[i]);  

    if (flag == 1)
       {PM_even_space_y(ipx, ya, x, yb, n);

        for (i = 0; i < n; i++)
            PM_REAL_C(cy[i]) = yb[i];}

    else
       {PM_even_space_y(ipx, ya, x, yb, np);

        for (i = 0; i <= nh; i++)
            {PM_REAL_C(cy[i])   = yb[nh+i];           /* this one needs to be first */
             PM_REAL_C(cy[n-i]) = yb[nh-i];};};

/* interpolate the imaginary part of y */
    for (i = 0; i < no; i++)
        ya[i] = PM_IMAGINARY_C(ipy[i]);

    if (flag == 1)
       {PM_even_space_y(ipx, ya, x, yb, n);

        for (i = 0; i < n; i++)
            PM_IMAGINARY_C(cy[i]) = yb[i];}

    else
       {PM_even_space_y(ipx, ya, x, yb, np);

        for (i = 0; i <= nh; i++)
            {PM_IMAGINARY_C(cy[i])   = yb[nh+i];           /* this one needs to be first */
             PM_IMAGINARY_C(cy[n-i]) = yb[nh-i];};};

    SFREE(ya);
    SFREE(yb);

    PM_fft_sc_complex(cy, n, flag);
        
    if (flag == -1)
       {dt = 1.0/(x[n-1] - x[0]);
        for (i = 0; i < n; i++)
            x[i] = dt*i;

        nr = n;}

    else
       {_PM_fft_fin(cy, x, nh, xmn, xmx, ordr);
        nr = np;};

    return(nr);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* _PM_FFT_FIN - complete an FFT including such things as:
 *             -   normalizing the frequency axis
 *             -   reordering the transform into intuitive order
 *             -   introducing a phase shift to get IFFT to match original
 */

void _PM_fft_fin(y, x, nh, xmn, xmx, ordr)
   complex *y;
   REAL *x;
   int nh;
   double xmn, xmx;
   int ordr;
   {int i;
    double dt;

    dt = 1.0/(xmx - xmn);
    for (i = 0; i <= nh; i++)
        {x[nh+i] =  dt*i;
         x[nh-i] = -dt*i;};

/* if the transform was requested in intuitive order, reorder the array */
    if (ordr)
       {for (i = 0; i < nh; i++)
            {PM_COMPLEX_SWAP(y[i], y[nh+i]);};
        y[2*nh] = y[0];};

    return;}

/*--------------------------------------------------------------------------*/

/*                        FFT CONVOLUTION ROUTINES                          */

/*--------------------------------------------------------------------------*/

/* PM_CONVOLVE - convolve two data sets regardless of their coordinate
 *             - spacings
 *             - for some users H is the response function
 *             - and G is the signal function
 *             - contributed by Charles McMillan 8/94
 */

int PM_convolve(gx, gy, gn, hx, hy, hn, dt, pxr, pyr, pnr)
   REAL *gx, *gy;
   int gn;
   REAL *hx, *hy;
   int hn;
   double dt;
   REAL **pxr, **pyr;
   int *pnr;
   {int i, j, gtn, hin;
    REAL *gty, *hty, *xret, *yret;
    REAL *hix, *hiy;
    REAL *gdy, *hdy;
    double hdx, hxmn, hxmx;
    double gxmn, gxmx;
    double cxmn, cxmx;
    double igy, ihy, init_y, init_x, vx, nrm;
  
    gdy = FMAKE_N(REAL, gn, "PM_CONVOLVE:gdy");
    if (gdy == NULL)
       return(FALSE);
    _PM_spline(gx, gy, gn, HUGE, HUGE, gdy);

    hdy = FMAKE_N(REAL, hn, "PM_CONVOLVE:hdy");
    if (hdy == NULL) 
       return(FALSE);
    _PM_spline(hx, hy, hn, HUGE, HUGE, hdy);

/* setup limits, find number of points */
    gxmn = gx[0];
    gxmx = gx[gn-1];
    hxmn = hx[0];
    hxmx = hx[hn-1];
    hdx  = hxmx - hxmn;

    cxmn = gxmn - hdx;
    cxmx = gxmx + hdx;

    hin = hdx/dt;
    gtn = (cxmx - cxmn + 2*hdx)/dt;
    gtn = PM_next_power_two(gtn);

    xret = FMAKE_N(REAL, gtn, "PM_CONVOLVE:xret");
    gty  = FMAKE_N(REAL, gtn, "PM_CONVOLVE:gty");
    hty  = FMAKE_N(REAL, gtn, "PM_CONVOLVE:hty");
    hix  = FMAKE_N(REAL, hin, "PM_CONVOLVE:hix");
    hiy  = FMAKE_N(REAL, hin, "PM_CONVOLVE:hiy");

/* interpolate signal */
    init_y = gy[0];
    for (i = 0; i < gtn; i++)
        {vx = cxmn + i*dt;
         xret[i] = vx;
	 if ((gxmn <= vx) && (vx <= gxmx))
	    {PM_cubic_spline_int(gx, gy, gdy, gn, vx, &gty[i]);
	     gty[i] -= init_y;}

/* pad the ends of the array */
	 else
	    gty[i] = 0;};

/* interpolate response */
    for (i = 0; i < hin; i++)
        {vx = hxmn + i*dt;
         hix[i] = vx;
	 PM_cubic_spline_int(hx, hy, hdy, hn, vx, &hiy[i]);};

/* reorder response for fft convolution */
    init_x = 0;
    for (i = 0; hix[i] < 0; i++);
    j = i;
    if (j == 0)
       init_x = hxmn;
    if (j == hin)
       init_x = hxmx;

/* arrays are born zero but its cheap insurance */
    for (i = 0; i < gtn; i++)
        hty[i] = 0;

    for (i = j; i < hin; i++)
        hty[i-j] = hiy[i];

    for (i = 0; i < j; i++)
        hty[gtn-j+i] = hiy[i];

/* calculate the integrals for normalization */
    igy = 0.0;
    ihy = 0.0;
    for (i = 0; i < gtn; i++)
	{igy += gty[i];
	 ihy += hty[i];};
    nrm = min(igy, ihy);
    nrm = 1.0/nrm;

/* call fft convolution */
    yret = FMAKE_N(REAL, 2*gtn, "PM_CONVOLVE:yret");
    PM_convolve_logical(gty, gtn, hty, gtn, 1, yret);
  
    SFREE(hix);
    SFREE(hiy);
    SFREE(gty);
    SFREE(hty);

/* replace initial value */
    for (i = 0; xret[i] < cxmx; i++)
        {yret[i]  = nrm*yret[i] + init_y;
	 xret[i] += init_x;};

    *pnr = i;
    *pxr = xret;
    *pyr = yret;

    return(TRUE);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_CONVOLVE_LOGICAL - compute and return the convolution of G and H in CNV
 *                     - this is done on an implied grid of uniform spacing
 *                     - where the G and H are defined on possibly different
 *                     - parts of the same grid
 *                     - adapted from Numerical Recipes in C
 */

int PM_convolve_logical(g, n, h, m, sgn, cnv)
   REAL *g, *h, *cnv;
   int n, m, sgn;
   {int i, nh, mh;
    double t, mg, in, a, b;
    REAL *ft;

    if (sgn*sgn != 1)
       return(FALSE);

    ft = FMAKE_N(REAL, 2*n, "PM_CONVOLVE_LOGICAL:ft");

/* pad h in the middle */
    mh = (m - 1)/2;
    for (i = 0; i < mh; i++)
        h[n+1-i] = h[m+1-i];

    for (i = mh + 1; i < n - mh; i++)
        h[i] = 0.0;

    PM_fft_sc_real_simul(g, h, ft, cnv, n);

    nh = n/2;
    in = 1.0/nh;
    for (i = 1; i < n+2; i += 2)
        {t = cnv[i-1];
         if (sgn == 1)
	    {mg = in;
	     cnv[i-1] = (ft[i-1]*t - ft[i]*cnv[i])*mg;
	     cnv[i]   = (ft[i]*t + ft[i-1]*cnv[i])*mg;}
         else
	    {a = cnv[i];
             b = cnv[i-1];
	     mg = a*a + b*b;
	     if (mg == 0.0)
	        return(FALSE);
	     mg = in/mg;

	     cnv[i-1] = (ft[i-1]*t + ft[i]*cnv[i])*mg;
	     cnv[i]   = (ft[i]*t - ft[i-1]*cnv[i])*mg;};};

    cnv[1] = cnv[n];

    _PM_fft_sc_real_hsp(cnv, nh, -1);

    SFREE(ft);

    return(TRUE);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* PM_FFT_SC_REAL_SIMUL - do simultaneous FFT's on two real functions
 *                      - adapted from Numerical Recipes in C
 */

int PM_fft_sc_real_simul(fx1, fx2, fw1, fw2, n)
   REAL *fx1, *fx2, *fw1, *fw2;
   int n;
   {int i, j, nb, na;
    double rep, rem, aip, aim, nrm;

    for (j = 0, i = 1; j < n; j++, i += 2)
        {fw1[i-1] = fx1[j];
	 fw1[i]   = fx2[j];};

    PM_fft_sc_complex((complex *) fw1, n, 1);

    na = n << 1;
    nb = na + 1;

/* fix normalization */
    nrm = n/2.0;

    fw2[0]  = nrm*fw1[1];
    fw1[0] *= nrm;
    fw1[1] = fw2[1] = 0.0;

    nrm *= 0.5;
    for (j = 2; j < n+1; j += 2)
        {rep = nrm*(fw1[j] + fw1[na-j]);
	 rem = nrm*(fw1[j] - fw1[na-j]);
	 aip = nrm*(fw1[j+1] + fw1[nb-j]);
	 aim = nrm*(fw1[j+1] - fw1[nb-j]);

	 fw1[j]    = rep;
	 fw1[j+1]  = aim;
	 fw1[na-j] = rep;
	 fw1[nb-j] = -aim;

	 fw2[j]    = aip;
	 fw2[j+1]  = -rem;
	 fw2[na-j] = aip;
	 fw2[nb-j] = rem;};

    return(TRUE);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/

/* _PM_FFT_SC_REAL_HSP - perform an FFT on a scalar set of real data in
 *                     - Half the SPace required by PM_fft_sc_real
 *                     - used as helper for convolution routine
 *                     - unlikely to be useful in it own right since the
 *                     - ordering of the transforms is NOT user friendly
 *                     - adapted from Numerical Recipes in C
 */

static int _PM_fft_sc_real_hsp(f, n, sgn)
   REAL *f;
   int n, sgn;
   {int i, i1, i2, i3, i4, na, nh;
    double c1, c2, h1r, h1i, h2r, h2i;
    double wr, wi, dwr, dwi, wt, dth;

    dth = PI/((double) n);
    if (sgn == 1)
       {PM_fft_sc_complex((complex *) f, n, sgn);
	c1 = 0.5;
	c2 = -0.25*n;}
    else
       {c1  = 1.0;
        c2  = 1.0;
	dth = -dth;};

    wr  = cos(dth);
    wi  = sin(dth);

    dwr = wr - 1.0;
    dwi = wi;

    na  = 2*n + 1;
    nh  = n >> 1;
    for (i = 1; i < nh; i++)
        {i1 = 2*i;
         i2 = i1 + 1;
         i3 = na - i2;
         i4 = i3 + 1;

	 h1r =  c1*(f[i1] + f[i3]);
	 h1i =  c1*(f[i2] - f[i4]);
	 h2r = -c2*(f[i2] + f[i4]);
	 h2i =  c2*(f[i1] - f[i3]);

	 f[i1] =  h1r + wr*h2r - wi*h2i;
	 f[i2] =  h1i + wr*h2i + wi*h2r;
	 f[i3] =  h1r - wr*h2r + wi*h2i;
	 f[i4] = -h1i + wr*h2i + wi*h2r;

	 wt = wr;
	 wr = wt*dwr - wi*dwi + wr;
	 wi = wi*dwr + wt*dwi + wi;};

    if (sgn == 1)
       {h1r  = f[0];
        c2   = 0.5*n;
        f[0] = c2*(h1r + f[1]);
	f[1] = c2*(h1r - f[1]);}

    else
       {h1r  = f[0];
        f[0] = h1r + f[1];
	f[1] = h1r - f[1];

	PM_fft_sc_complex((complex *) f, n, sgn);};

    return(TRUE);}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/