#include <stdio.h>
#include <string.h>
#include "mex.h"
#include "matrix.h"
/*
 * fft_mult_im.c
 * author:    Nicholas Kottenstette
 * date:      12/14/2004
 * copyright: 2004 by Nicholas Kottenstette
 */

/* SCALAR can not be complex, ARRAY is just a real double array
   COMPLEX is always a complex array 
*/
enum in_types {SCALAR, ARRAY, COMPLEX};
#define SMAX 16
#define CN ((1<<SMAX)-1)
#define error(a) mexErrMsgIdAndTxt a
#ifdef DEBUG
#define dprintf(a) mexPrintf a
#else
#define dprintf(a)
#endif

static int
m_type(int t, const mxArray *p)
{
        int res = 0;
        int d, m, n, c;
        d = mxIsDouble(p);
        c = mxIsComplex(p);
        m = mxGetM(p);
        n = mxGetN(p);
        switch(t){
        case SCALAR:
                if(d && !c && m*n == 1)
                        res = 1;
                break;
        case ARRAY:
                if(d && !c && m*n != 1)
                        res = 1;
                break;
        case COMPLEX:
                if(d && c && m*n >= 1)
                        res = 1;
                break;
        }
        return res;
}

static void
com_vect_emult(double *xr, double *xi, double *par, double *pai,
               double *pbr, double *pbi, int len)
{
        int i;
        for(i = 0; i<len; i++){
                double ar, br, ai, bi;
                dprintf(("com_vect_emult\n"));
                ar = par[i];
                dprintf(("ar = %f\n",ar));
                ai = pai[i];
                dprintf(("ai = %f\n",ai));               
                br = pbr[i];
                dprintf(("br = %f\n",br));
                bi = pbi[i];
                dprintf(("bi = %f\n",bi));
                xr[i] = ar*br - ai*bi;
                dprintf(("xr[%d] = %f\n",i,xr[i]));
                xi[i] = ai*br + ar*bi;
                dprintf(("xi[%d] = %f\n",i,xi[i]));
        }
        return;
}

static void
com_vect_add(double *xr, double *xi, double *ar, double *ai,
               double *br, double *bi, int len)
{
        int i;
        for(i = 0; i < len; i++){
                xr[i] = ar[i] + br[i];
                xi[i] = ai[i] + bi[i];
        }
        return;
}

static void
com_vect_sub(double *xr, double *xi, double *ar, double *ai,
               double *br, double *bi, int len)
{
        int i;
        for(i = 0; i < len; i++){
                xr[i] = ar[i] - br[i];
                xi[i] = ai[i] - bi[i];
        }
        return;
}

/*
 * mxfft( Xi, Xr, xi, xr, ci, cr, lx, Smax )
 * c -version of mfft in dsp_fft.m
 * inputs:
 * Xi -> imaginary array to store resulting fft result
 * Xr -> real array to store resulting fft result
 * xi -> imaginary arrray of input vector x (if imaginary)
 * xr -> real array of input vector x
 * ci -> imaginary array of pre-calculated fft coeficients
 * cr -> real array of pre-calculated fft coeficients
 * lx -> length of x input vector
 * Smax -> number of stages to iterate through
 */
static void 
mxfft(double *Xi, double *Xr, double *pxi, double *pxr, double *ci, double *cr, int lx, int Smax )
{
        int s,n,i,j,imax,nmax, cis;
        int fns1, fne1, fns2, fne2, len;
        double *coxr, *coxi, *xi, *xr;
        /* allocate worst case memory up-front */
        coxr = (double*)mxCalloc(lx,sizeof(double));
        coxi = (double*)mxCalloc(lx,sizeof(double));
        xr = (double*)mxCalloc(lx,sizeof(double));
        xi = (double*)mxCalloc(lx,sizeof(double));
        memcpy(xr,pxr,sizeof(double)*lx);
        if( pxi != NULL )
                memcpy(xi,pxi,sizeof(double)*lx);
        
        for(s=1;s<=Smax;s++){
                n = 1 << s;
                cis = (n >> 1) - 1;
                imax = 1 << (Smax-s);
                for(i=1;i<=imax;i++){
                        fns1 = n*(i-1);
                        fne1 = fns1 + cis;
                        fns2 = fne1 + 1;
                        fne2 = fns2 + cis;
                        len = cis+1;
                        dprintf(("fns1=%d,fne1=%d,fns2=%d,fne2=%d\n",fns1,fne1,fns2,fne2));
                        dprintf(("len = %d\n",len));
                        com_vect_emult(coxr,coxi,cr+cis,ci+cis,
                                       xr+fns2,xi+fns2,len);
                        com_vect_add(Xr+fns1,Xi+fns1,xr+fns1,xi+fns1,
                                     coxr,coxi,len);
                        com_vect_sub(Xr+fns2,Xi+fns2,xr+fns1,xi+fns1,
                                     coxr,coxi,len);
#ifdef DEBUG
                        for(j=fns1;j<=fne2;j++){
                                mexPrintf("Xr(%d) = %f\n",j,Xr[j]);
                        }
#endif
                }
                memcpy(xr,Xr,sizeof(double)*lx);
                memcpy(xi,Xi,sizeof(double)*lx);
                dprintf(("x =\n"));
#ifdef DEBUG
                for(j=1;j<sizeof(double)*lx;j++)
                        mexPrintf("%f + %fi\n",xr[j],xi[j]);
#endif
        }
        mxFree(coxr);
        mxFree(coxi);
        mxFree(xr);
        mxFree(xi);
}
/* mexFunction is the gateway routine for the MEX-file.
   inputs:
    c    -> appropriate pre-calculated fft coeficients real and imag comp
    x    -> shuffled input data real and imag comp
    Smax -> number of stages required for calculation
   outputs:
   Xt -> appropriate complex pre-allocated memory to put final result in
          passed in by matlab
*/
void
mexFunction( int nlhs, mxArray *plhs[],
             int nrhs, const mxArray *prhs[] )
{
        double *cr, *ci, *xr, *xi, *Xr, *Xi, Smax;
        int mc, nc, mx, nx, mn, iSmax;
        char emsg[256];

        if (nrhs != 3)
                error(("fft_mult_im:inputs","3 inputs required!"));
        if (nlhs != 1)
                error(("fft_mult_im:outputs","1 output required!\nusage:X = fft_mult_im(c,x,Smax)"));

        if(m_type(COMPLEX, prhs[0])){
                mc = mxGetM(prhs[0]);
                nc = mxGetN(prhs[0]);
                cr = mxGetPr(prhs[0]);
                ci = mxGetPi(prhs[0]);
                mn = mc*nc;
                if( (mn != mc) & (mn != nc) )
                        error(("fft_mult_im:inputs","c must be a vector! mc = %d, nc = %d\n",mc,nc));
                if( mn != CN){
                        error(("fft_mult_im:inputs","length(c) = %d does not equal %d points!\n",mn,CN));
                }
        }else
                error(("fft_mult_im:inputs","c must be a complex vector!"));
        
        if(m_type(COMPLEX, prhs[1] ) | m_type(ARRAY, prhs[1]) ){
                mx = mxGetM(prhs[1]);
                nx = mxGetN(prhs[1]);
                xr = mxGetPr(prhs[1]);
                if(m_type(COMPLEX, prhs[1] ))
                        xi = mxGetPi(prhs[1]);
                else
                        xi = NULL;
                mn = mx*nx;
                if( (mn != mx) & (mn != nx) )
                        error(("fft_mult_im:inputs","x must be a vector! mx = %d, nx = %d\n",mx,nx));
        }else
                error(("fft_mult_im:inputs","x must be a vector!"));
        if(m_type(SCALAR, prhs[2]))
                Smax = mxGetScalar(prhs[2]);
        else
                error(("fft_mult_im:inputs","Smax must be a scalar!"));

        if(Smax > SMAX) 
                error(("fft_mult_im:inputs","Smax = %d, SMAX = %d\nSmax is to large.",Smax,SMAX));
        
        iSmax = (int)Smax;
        if( mn != (1<<iSmax))
                error(("fft_mult_im:inputs","Length of x: %d, does not equal length of 2^Smax: %d\n", mn, (1<<iSmax)));
        
        plhs[0] = mxCreateDoubleMatrix(mx, nx, mxCOMPLEX);
        Xr = mxGetPr(plhs[0]);
        Xi = mxGetPi(plhs[0]);
        mxfft(Xi,Xr,xi,xr,ci,cr,mx*nx,iSmax);
        dprintf(("Successfully accessed data"));
        return;
}

