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

#define S_FUNCTION_NAME sfirdn

#include "dsp_sim.h"

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

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

/* IWork indices */
#define PHASE_IDX   0
#define NUM_IWORKS  1


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

    /* Check filter: */
    if (!mxIsDouble(FILT_ARG) || mxIsComplex(FILT_ARG)) {
        msg = "Decimation filter must be a real double matrix";
        goto ERROR_EXIT;
    }
    if (mxIsEmpty(FILT_ARG)) {
        msg = "Decimation filter must not be empty";
        goto ERROR_EXIT;
    }

    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;
    }

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)
{
    real_T *pr = mxGetPr(SAMPLETIME_ARG);
    ssSetSampleTimeEvent(S, 0, pr[0]);
    ssSetOffsetTimeEvent(S, 0, (mxGetN(SAMPLETIME_ARG) > 1) ? pr[1] : 0.0);
}


/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *
 */
#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    /*
     * Allocate space for input sample storage:
     * num_chans locations for filter summation results (one per channel)
     * num_chans * KD locations for input sample storage (KD per channel)
     */
    const int_T num_chans = ssGetNumInputs(S);
    const int_T KD        = mxGetNumberOfElements(FILT_ARG);
    ssSetNumRWork(S, num_chans*(1+KD));
}
#endif


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    /*
     * First num_chans locations hold the output summation points.
     * Remaining length(filt)*num_chans locations hold input samples.
     */
    const int_T num_chans = ssGetNumInputs(S);
    ssSetPWorkValue(S, CURR_INBUFF_PTR, ssGetRWork(S) + num_chans);
    ssSetIWorkValue(S, PHASE_IDX, 0);
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    const int_T   K         = mxGetM(FILT_ARG);
    const int_T   D         = mxGetN(FILT_ARG);
    const int_T   KD        = K*D;
    int_T         num_chans = ssGetNumInputs(S);
    int_T        *phase     = ssGetIWork(S) + PHASE_IDX;
    real_T       *sum_buff  = ssGetRWork(S);
    real_T       *in_buff   = sum_buff + num_chans;  /* i'th chan buffer  */
    real_T       *in_curr   = ssGetPWorkValue(S, CURR_INBUFF_PTR);

    /*
     * Bump input delay line pointers: 
     */
    if (++in_curr == in_buff + KD) {
        in_curr = in_buff;    /* reset to start of buffer */
    }
    ssSetPWorkValue(S, CURR_INBUFF_PTR, in_curr);

    /*
     * Update filter sums:
     */
    {
        /* k1,k2:
         *  Number of contiguous input samples before buffer wraps,
         *  k1 samples in first contig segment, k2 in second segment
         *  k2 might be 0.
         */
        const int k1 = (in_curr - in_buff)/D + 1;
        const int k2 = K - k1;

        /* Point to correct column of filter matrix (correct filter phase) */
        const real_T *filt_phase = mxGetPr(FILT_ARG) + *phase * K;

        UPtrsType uptr = ssGetUPtrs(S);
        int_T     i;

        for (i=0; i<num_chans; i++) {
            const real_T *h       = filt_phase;      /* Reset curr filter phase */
            real_T       *in_tmp  = in_curr + i*KD;  /* Reset curr pos in i'th chan */
            int_T         kk      = k1;              /* Contig elements in first segment */

            *in_tmp = **(uptr++);  /* Store next input for channel */

            while(kk-- != 0) {  /* 1st segment of contiguous inputs */
                *sum_buff += *h++ * *in_tmp;
                in_tmp -= D;
            }

            in_tmp += KD;
            kk = k2;
            while(kk-- != 0) {  /* 2nd segment of contiguous inputs */
                *sum_buff += *h++ * *in_tmp;
                in_tmp -= D;
            }

            sum_buff++;    /* next channel sum location */
        }
    }

    /*
     * Output decimator result:
     */
    if (*phase == 0) {
        sum_buff = ssGetRWork(S);
        while(num_chans-- != 0) {       /* Destroys num_chans     */
            *y++        = *sum_buff;    /* Output filter result   */
            *sum_buff++ = (real_T)0.0;  /* Reset summation        */
        }
        *phase = D-1;                   /* Wrap filter phase      */
    } else {
        (*phase)--;                     /* Decrement filter phase */
    }
}


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)
{
}


#ifdef	MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-File interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif