/*
 * SUPFIR A SIMULINK interpolating FIR filter block.
 *        DSP Blockset S-Function for the efficient polyphase
 *        implementation of upsampling followed by FIR filtering.
 *
 *    Syntax:  [sys, x0] = sfirdn(t,x,u,flag, filt,L,sampleTime)
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.12 $  $Date: 2000/06/14 14:28:02 $
 */

#define S_FUNCTION_NAME supfir

#include "dsp_sim.h"

/*
 * Defines for easy access of the input parameters
 */
#define NUM_ARGS       3
#define FILT_ARG       ssGetArg(S,0)
#define INTERP_ARG     ssGetArg(S,1)
#define SAMPLETIME_ARG ssGetArg(S,2)

/* PWork indices */
#define CURR_INBUFF_PTR 0
#define FILTEND_PTR     1
#define NUM_PWORKS      2

 /* IWork indices */
#define INTERP_IDX 0
#define K_IDX      1
#define PHASE_IDX  2
#define NUM_IWORKS 3


#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    const char *msg = NULL;
    int L;

    if ((mxGetM(SAMPLETIME_ARG) != 1) ||
        (mxGetN(SAMPLETIME_ARG) == 0) ||
        (mxGetN(SAMPLETIME_ARG) > 2)   ) {
        msg = "The sample time must be either a scalar or 2-element row vector";
        goto ERROR_EXIT;
    }

    if ((mxGetM(INTERP_ARG) != 1) || (mxGetN(INTERP_ARG) != 1)) {
        msg = "The upsample factor must be a scalar value";
        goto ERROR_EXIT;
    }
    L = (int)(mxGetPr(INTERP_ARG)[0]);
    if ((L < 1)) {
        msg = "The upsample factor must be >= 1";
        goto ERROR_EXIT;
    }

ERROR_EXIT:
    if (msg != NULL) {
        ssSetErrorStatus(S,msg);
    }
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_ARGS);

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Simulink will report a parameter mismatch error */
    }
#endif

    ssSetNumContStates(    S, 0);
    ssSetNumDiscStates(    S, 0);
    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumIWork(         S, NUM_IWORKS);
    ssSetNumPWork(         S, NUM_PWORKS);
    ssSetNumModes(         S, 0);
    ssSetNumNonsampledZCs( S, 0);
    ssSetOptions(          S, (SS_OPTION_USING_ssGetUPtrs |
                               SS_OPTION_EXCEPTION_FREE_CODE));
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    int    L      = (int)(mxGetPr(INTERP_ARG)[0]);
    real_T *pr    = mxGetPr(SAMPLETIME_ARG);
    real_T Tsi    = pr[0];
    real_T offset = (mxGetN(SAMPLETIME_ARG) > 1) ? pr[1] : 0.0;

    ssSetSampleTimeEvent(S, 0, Tsi/L);  /* Run at high rate */
    ssSetOffsetTimeEvent(S, 0, offset);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    int    filt_len  = mxGetM(FILT_ARG)*mxGetN(FILT_ARG);
    int    L         = (int)(mxGetPr(INTERP_ARG)[0]);
    int    K         = filt_len/L;
    /*
     * There are L polyphase filters, each of length
     * K = ceil(filt_len/L).  Note that there may need
     * to be some zeros padded to the end of filt.
     */
    if (K*L != filt_len) {
        K++;
    }

    /* Store values */
    ssSetIWorkValue(S, INTERP_IDX, L);
    ssSetIWorkValue(S, K_IDX,      K);
    ssSetIWorkValue(S, PHASE_IDX,  0);

    /*
     * Holds "current" input buffer pointer for first channel.
     * All other channels are offset from this by chan*KL
     */
    ssSetPWorkValue(S, CURR_INBUFF_PTR, ssGetRWork(S));
    ssSetPWorkValue(S, FILTEND_PTR, mxGetPr(FILT_ARG) + filt_len);  /* past end of filter */
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    const int_T   num_chans = ssGetNumInputs(S);
    const int_T   L         = ssGetIWorkValue(S, INTERP_IDX);
    const int_T   K         = ssGetIWorkValue(S, K_IDX);
    const int_T   KL        = K*L;
    const real_T *in_buff   = ssGetRWork(S);
    const real_T *filt_end  = ssGetPWorkValue(S, FILTEND_PTR);

    int_T        *phase     = ssGetIWork(S) + PHASE_IDX;
    real_T       *in_curr   = ssGetPWorkValue(S, CURR_INBUFF_PTR);
    int_T         i;

    if (*phase == 0) {
        /* Bump input delay line pointer: */
        UPtrsType uptr = ssGetUPtrs(S);

        if (++in_curr == in_buff + KL) {
            in_curr = (real_T *)in_buff;  /* reset to start of buffer alloc */
        }
        ssSetPWorkValue(S, CURR_INBUFF_PTR, in_curr);

        /* Store new input samples: */
        for(i=num_chans; i--;) {
            *in_curr = **(uptr++);
            in_curr += KL;
        }
        in_curr = ssGetPWorkValue(S, CURR_INBUFF_PTR);  /* Reset */
    }

    /* Compute filter outputs: */
    {
        const real_T *filt_tmp = mxGetPr(FILT_ARG) + *phase;
        int j;

        for (i=num_chans; i-- != 0; ) {
            *y++ = (real_T)0.0;
        }

        for(j=K; j-- != 0; ) {  /* Loop over all taps in filter phase */
            real_T *in_tmp = in_curr;

            y -= num_chans;           /* restore pointer */
            for (i=num_chans; i-- != 0; ) {
                *y++ += *filt_tmp * *in_tmp;
                in_tmp += KL;
            }

            if (--in_curr < in_buff) {
                in_curr += KL;
            }

            filt_tmp += L;
            if (filt_tmp >= filt_end) break;
        }
    }

    if (++(*phase) == L) {
        *phase = 0;
    }
}


static void mdlUpdate(real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
}


static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
                           SimStruct *S, int_T tid)
{
}


static void mdlTerminate(SimStruct *S)
{
}

/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *
 */
#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    int_T num_chans = ssGetNumInputs(S);
    int_T filt_len  = mxGetM(FILT_ARG)*mxGetN(FILT_ARG);
    int_T L         = (int_T)(mxGetPr(INTERP_ARG)[0]);
    int_T K         = filt_len/L;

    /*
     * There are L polyphase filters, each of length
     * K = ceil(filt_len/L).  Note that there may need
     * to be some zeros padded to the end of filt.
     */
    if (K*L != filt_len) {
        K++;
    }
    
    /*
     * Allocate space for input sample storage:
     */
    ssSetNumRWork(S, num_chans*K*L);
}
#endif


#include "dsp_trailer.c"
 
/* [EOF]: supfir.c */