/* 
 *  fgcore.c
 *   Filtered Gaussian source (core C-code).
 *   Shared by MATLAB C-MEX function, SIMULINK C-MEX S-Function, and TLC.
 *
 *  Copyright 1996-2005 The MathWorks, Inc.
 *  $Revision: 1.1.6.1 $ $Date: 2004/12/10 19:23:11 $ 
 */

#include <math.h>
#include "fgcore.h"

/* 
The following Gaussian random number generator has been adapted from the 
DSP run-time library function MWDSP_RandSrc_GZ_D.
It produces the same output as the randn function in MATLAB.
Note that state is now declared as real for compatibility with MATLAB. 
*/
void randn(real64_T *outPtr,     /* output signal */ 
           int_T nRows,          /* number of rows */
           int_T nCols,          /* number of columns */
           real_T *state)        /* state vector */
{
    static const real64_T aa = 12.37586,   b = 0.4878992, c = 12.67706;
    static const real64_T c1 = 0.9689279,  c2 = 1.301198;
    static const real64_T pc = 0.01958303, xn = 2.776994;
    static const real64_T tpm31 = 4.656612873077393e-10;
    static const real64_T tpm32 = 2.328306436538696e-10;
    static const real64_T vt[] = {
	0.3409450, 0.4573146, 0.5397793, 0.6062427, 0.6631691, 0.7136975,
	0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948, 0.9490791,
	0.9820005, 1.0138492, 1.0447810, 1.0749254, 1.1043917, 1.1332738,
	1.1616530, 1.1896010, 1.2171815, 1.2444516, 1.2714635, 1.2982650,
	1.3249008, 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915,
	1.4834527, 1.5100122, 1.5367061, 1.5635712, 1.5906454, 1.6179680,
	1.6455802, 1.6735255, 1.7018503, 1.7306045, 1.7598422, 1.7896223,
	1.8200099, 1.8510770, 1.8829044, 1.9155831, 1.9492166, 1.9839239,
	2.0198431, 2.0571356, 2.0959930, 2.1366450, 2.1793713, 2.2245175,
	2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
	2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942};

    int32_T i;
    int_T j;
    real64_T r,x,s,y;
    real64_T val;
    
    while (nCols--) {
        uint32_T icng = (uint32_T) state[0];
        uint32_T jsr = (uint32_T) state[1];
        int_T samps = nRows;
        while (samps--) {
            icng = 69069*icng + 1234567;
            jsr ^= (jsr<<13); jsr ^= (jsr>>17); jsr ^= (jsr<<5);
            i = icng + jsr;
            j = i&0x3f;
            r = (real64_T)i*tpm31*vt[j+1];
            if (fabs(r) <= vt[j]) {
                val = r;
            } else {
                x = (fabs(r)-vt[j])/(vt[j+1]-vt[j]);
                icng = 69069*icng + 1234567;
                jsr ^= (jsr<<13); jsr ^= (jsr>>17); jsr ^= (jsr<<5);
                i = icng+jsr;
                y = 0.5 + i*tpm32;
                s = x + y;
                if (s > c2) {
                    val = (r < 0 ? b*x-b : b-b*x);
                } else if (s <= c1) {
                    val = r;
                } else {
                    x = b - b*x;
                    if (y > c-aa*exp(-0.5*x*x)) {
                        val = (r < 0 ? -x : x);
                    } else if (exp(-0.5*vt[j+1]*vt[j+1])+y*pc/vt[j+1] <= 
                               exp(-.5*r*r)) {
                        val = r;
                    } else {
                        do {
                            icng = 69069*icng + 1234567;
                            jsr ^= (jsr<<13); jsr ^= (jsr>>17); jsr ^= (jsr<<5);
                            i = icng+jsr;
                            x = log(0.5+i*tpm32)/xn;
                            icng = 69069*icng+1234567;
                            jsr ^= (jsr<<13); jsr ^= (jsr>>17); jsr ^= (jsr<<5);
                            i = icng+jsr;
                        } while (-2.*log(0.5+i*tpm32) <= x*x);
                        val = (r < 0 ? x-xn : xn-x);
                    }
                }
            }
            *outPtr++ = val;
        }
        state[0] = icng;  
        state[1] = jsr;
    }
}

void corefiltgaussian(
    cArray     y,           /* Output */
    int_T      Nout,        /* Number of samples to output per channel */
    real_T    *h,           /* Filter impulse response */
    cArray     u,           /* State matrix */
    real_T    *WGNState,    /* White Gaussian noise generator state */
    cArray     lastOutputs, /* Last two outputs */
    cArray     w,           /* WGN (allocated storage) */
    real64_T  *w2,          /* WGN (temp. alloc. storage) */
    int_T      NS,          /* Number of samples for y (per channel) */
    int_T      NC,          /* Number of channels */
    int_T      lengthIR     /* Length of impulse response */
    )
{
    int_T m, n; /* Loop indices */
             
    /* Generate multi-channel white Gaussian signal.
       Note that these matrices have each channel in a row, *NOT* col. */
    randn(w2, 2*NC, Nout, WGNState);
    for (m=0; m<Nout; m++) {
        for (n=0; n<NC; n++) {
            int_T i1 = m*NC + n;
            int_T i2 = m*(2*NC) + n;
            Re(w, i1) = w2[i2] / sqrt(2); 
            Im(w, i1) = w2[i2+NC] / sqrt(2);          
        }
    } 
    
    /* Compute output for each channel.
       Note that M and N have swapped order compared with above. */
    for (n=0; n<NC; n++) {  /* Channel (cols of y, h, u; rows of w) */
        
        int_T nN = n*NS;
        
        /* Filter WGN signal. */
        for (m=0; m<Nout; m++) {  
            /* m is sample idx (rows of y, h, u; cols of w) */
            int_T iy = nN+m, k;
            Re(y, iy) = Im(y, iy) = 0.0;
            for (k=0; k<=m && k<=lengthIR-1; k++) {
                int_T iw = (m-k)*NC + n;
                Re(y, iy) += h[k] * Re(w, iw);
                Im(y, iy) += h[k] * Im(w, iw);
            }
            for (k=m+1; k<=lengthIR-1; k++) {
                int_T iu = (lengthIR+m-k-1)*NC + n;
                Re(y, iy) += h[k] * Re(u, iu);
                Im(y, iy) += h[k] * Im(u, iu);
            }
        }
     
        /* Update state. */
        {
            int_T k, i1 ,i2, k1;
            int_T N = lengthIR - Nout;
            for (k=0; k<=N-2; k++) {
                i1 = k*NC + n;
                i2 = (k+Nout)*NC + n;
                Re(u, i1) = Re(u, i2);
                Im(u, i1) = Im(u, i2);
            }
            if (N<=0) k1=0; else k1=N-1;
            for (k=k1; k<=lengthIR-2; k++) {
                i1 = k*NC + n;
                i2 = (k-N+1)*NC + n;
                Re(u, i1) = Re(w, i2);
                Im(u, i1) = Im(w, i2);
            }
        }   
       
        /* Store last two outputs. */
        {
            int_T i2 = NC + n;
                        
            if (Nout==1)
            {
                Re(lastOutputs, n)  = Re(lastOutputs, i2);
                Im(lastOutputs, n)  = Im(lastOutputs, i2);
                Re(lastOutputs, i2) = Re(y, n);
                Im(lastOutputs, i2) = Im(y, n);
            }
            else
            {
                int_T iy2 = nN + (Nout - 2);
                int_T iy1 = nN + (Nout - 1);
                Re(lastOutputs, n)  = Re(y, iy2);
                Im(lastOutputs, n)  = Im(y, iy2);
                Re(lastOutputs, i2) = Re(y, iy1);
                Im(lastOutputs, i2) = Im(y, iy1);
            }
        }
    }   
}

/* [EOF] */