/*
 * SVFDELAY A SIMULINK variable fractional delay block.
 *
 *    Syntax:  [sys, x0] = sfirdn(t,x,u,flag, h,maxDelay)
 *
 * h is a "polyphase filter matrix", with one filter phase per column
 * - Each filter phase occupies one column of the polyphase matrix
 * - The first column contains the "0 delay" interpolation filter
 * - The number of columns is the number of intersample interpolation points.
 * - The number of rows is the order of the interpolator function.
 * - If h is empty, linear interpolation is used.
 *
 * maxDelay is the maximum sample delay to support.  If the requested delay
 * exceeds maxDelay, the maximum delay is substituted.
 *
 * If the requested delay time is less than half the interpolation filter length,
 * i.e., a small delay time, the algorithm falls back to linear interpolation.
 *         d < size(filt,1)/2
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.10 $  $Date: 2000/06/14 14:28:02 $
 */

#define S_FUNCTION_NAME svfdelay

#include "dsp_sim.h"

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

 /* IWork indices */
#define BUF_LEN    0
#define BUF_OFFSET 1
#define NUM_IWORKS 2

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

    /* We must assume that the second input signal is a scalar.
     * Currently there is no "good" way to verify that assumption.
     */

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

    /* Check max delay: */
    if (mxGetNumberOfElements(DMAX_ARG) != 1) {
      msg = "Maximum delay must be a scalar integer";
      goto ERROR_EXIT;
    }
    d = mxGetPr(DMAX_ARG)[0];
    i = (int_T)d;
    if ((i != d) || (d<=0)) {
      msg = "Maximum delay must be a scalar integer > 0";
      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, 0);
    ssSetNumModes(         S, 0);
    ssSetNumNonsampledZCs( S, 0);
    ssSetOptions(          S, (SS_OPTION_USING_ssGetUPtrs |
                               SS_OPTION_EXCEPTION_FREE_CODE));
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTimeEvent(S, 0, 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    const int_T dmax  = mxGetPr(DMAX_ARG)[0];
    const int_T hlen  = mxGetM(FILT_ARG)/2;

#ifdef MATLAB_MEX_FILE
    if (ssGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
        ssSetErrorStatus(S,"Inputs to block must have discrete sample times");
        return;
    }
#endif

    ssSetIWorkValue(S, BUF_LEN,    dmax+1 + hlen);
    ssSetIWorkValue(S, BUF_OFFSET, 0);
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    const int_T nchans = ssGetNumInputs(S) - 2;  /* # input signals */
    const int_T buflen = ssGetIWorkValue(S, BUF_LEN);
    int_T      *bufoff = ssGetIWork(S) + BUF_OFFSET;
    int_T       i;
    real_T      t;

    /*
     * Service inputs:
     */
    {
        /* Store new input samples: */
        UPtrsType uptr    = ssGetUPtrs(S);
        real_T   *in_curr = ssGetRWork(S);

        if (++(*bufoff) == buflen) {  /* Rotate circular buffer */
            *bufoff = 0;
        }
        in_curr += *bufoff;
        for(i=nchans; i-- != 0; ) {   /* Record input samples */
            *in_curr  = **(uptr++);
             in_curr += buflen;
        }
        t = **(uptr++);  /* Get delay time from 2nd input port */

        /* Check width of delay time input */
        if (**uptr != 1.0) {
            ssSetErrorStatus(S,"Delay input must be scalar.");
            return;
        }
    }

    /*
     * Output new interpolation point:
     */
    {
        const int_T   dmax    = buflen - 1;
        const int_T   hlen    = mxGetM(FILT_ARG)/2;
        const real_T *in_curr = (const real_T *)ssGetRWork(S);  /* need to add *bufoff */

        /* Get relative sample time from second port
         * u[0] through u[N-2] = signal inputs
         * u[N-1] = sample delay input
         */
        int_T  ti;

        /* Clip delay time to legal range: [0,dmax] */
        if (t < 0) {
            t = 0;
        } else if (t > dmax) {
            t = dmax;
        }
        ti = t;         /* Integer part of delay time */
                     
        /*
         * Check if we need to fallback to linear interp:
         */
         if (mxIsEmpty(FILT_ARG) || (ti < (hlen-1))) {
            /* Linear interpolation: */

            const real_T frac  = t - ti;    /* Fractional part of delay time */
            const real_T frac1 = 1 - frac;

            /* Add offset to current input pos'n in buffer, backing up by
             * the integer number of delay samples.  If we've backed up too
             * far past the start of the buffer, wrap to the end:
             */
            ti = *bufoff - ti;
            if (ti < 0) {
                ti += buflen;
            }
            in_curr += ti; /* Get pointer into buffer */

            if (ti > 0) {
                /* The two points are adjacent in memory: */
                for (i=nchans; i-- != 0; ) {
                    *y++     = in_curr[0]*frac1 + in_curr[-1]*frac;
                    in_curr += buflen;
                }
            } else {
                /* The two points are at the end and start of buffer: */
                for (i=nchans; i-- != 0; ) {
                    *y++     = in_curr[0]*frac1 + in_curr[buflen-1]*frac;
                    in_curr += buflen;
                }
            }

        } else {
            /* FIR Interpolation: */

            const real_T frac  = t - ti;    /* Fractional part of delay time */
            const int_T  filtlen = mxGetM(FILT_ARG);
            const int_T  nphases = mxGetN(FILT_ARG);
            int_T        phase   = nphases * frac + 0.5; /* [0,nphases] */

            ti    += phase/nphases;
            phase %= nphases;

            /* Add offset to current input pos'n in buffer, backing up by the
             * integer number of delay samples then incrementing by half the
             * filter length.  If we've backed up too far past the start of the
             * buffer, wrap:
             */
            ti = *bufoff - ti + (hlen-1);

            if (ti < 0) {
                ti += buflen;
            }
            in_curr += ti; /* Get pointer into buffer */

            if (ti+1 >= filtlen) {
                /* Contiguous input samples in buffer: */
                for (i=0; i<nchans; i++) {
                    real_T *filt = mxGetPr(FILT_ARG) + phase*filtlen; /* Point to correct polyphase filter */
                    int_T   kn;

                    *y = 0.0;
                    for(kn=0; kn<filtlen; kn++) {
	              *y += in_curr[-kn] * *filt++;
                    }
                    in_curr += buflen;
                    y++;
                }

            } else {
                /* Discontiguous input samples in buffer: */
                for (i=0; i<nchans; i++) {
                    real_T       *filt    = mxGetPr(FILT_ARG) + phase*filtlen;
                    const int_T   k1      = ti+1;
                    const int_T   k2      = filtlen-k1;
                    const real_T *in_last = in_curr-ti+buflen-1;
                    int_T kn;

                    *y = 0.0;
                    for(kn=0; kn<k1; kn++) {
	              *y += in_curr[-kn] * *filt++;
                    }
                    for(kn=0; kn<k2; kn++) {
	              *y += in_last[-kn] * *filt++;
                    }
                    in_curr += buflen;
                    y++;
                }
            }
        }
    }
}


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
#define MDL_GET_INPUT_PORT_WIDTH
/* Function: mdlGetInputPortWidth =============================================
 *
 */
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
{
    return(outputPortWidth+2);
}
#endif


#ifdef MATLAB_MEX_FILE
#define MDL_GET_OUTPUT_PORT_WIDTH
/* Function: mdlGetOutputPortWidth ============================================
 *
 */
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    return(inputPortWidth-2);
}
#endif


/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *
 */
#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    /*
     * Extend buffer to allow delays up to DMAX without
     * having to fall back to linear interpolation.
     */
    const int_T hlen   = mxGetM(FILT_ARG)/2;
    const int_T dmax   = mxGetPr(DMAX_ARG)[0];
    const int_T nchans = ssGetNumInputs(S) - 2;
    ssSetNumRWork(S, (dmax+1 + hlen) * nchans );
}
#endif

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