/*
 * SDSPVDLY A SIMULINK variable integer delay block.
 *
 *    Syntax:  [sys, x0] = sdspvdly(t,x,u,flag, maxDelay)
 *
 * maxDelay is the maximum sample delay to support.  If the requested delay
 * exceeds maxDelay, the maximum delay is substituted.
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.9 $  $Date: 2000/06/14 14:28:00 $
 */

#define S_FUNCTION_NAME sdspvdly

#include "dsp_sim.h"

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

 /* 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 max delay: */
    if (mxGetNumberOfElements(DMAX_ARG) != 1) {
      msg = "Maximum delay must be a scalar integer";
      goto FCN_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 FCN_EXIT;
    }

FCN_EXIT:
    ssSetErrorStatus(S, msg);
}
#endif


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

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
    mdlCheckParameters(S);
    if (ssGetErrorStatus(S) != NULL) return;
#endif

    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumIWork(         S, NUM_IWORKS);
    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];

#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);
    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);
    const int_T dmax   = buflen - 1;
    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 sample point:
     */
    {
        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) {
            ti = 0;
        } else if (t > dmax) {
            ti = dmax;
        } else {
            ti = t + 0.5; /* Round delay time to nearest integer */
        }

        /* 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 */
        for (i=nchans; i-- != 0; ) {
            *y++     = *in_curr;
            in_curr += buflen;
        }
    }
}


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
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
{
    return(outputPortWidth+2);
}

#define MDL_GET_OUTPUT_PORT_WIDTH
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    return(inputPortWidth-2);
}

#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 dmax   = mxGetPr(DMAX_ARG)[0];
    const int_T nchans = ssGetNumInputs(S) - 2;
    ssSetNumRWork(S, (dmax+1) * nchans );
}
#endif


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