/* $Revision: 1.13 $ */
/* $Date: 2000/06/14 14:28:01 $ */

/* Copyright 1995-2000 The MathWorks, Inc.
 * All Rights Reserved.
 *
 * File: sndelay.c
 *
 * Abstract:
 *      S-function for the discrete integer delay block: y(t) = u(t-n*Ts)
 *
 */

#define S_FUNCTION_NAME  sndelay
#define S_FUNCTION_LEVEL 2
#include "simstruc.h"

#if !defined(MATLAB_MEX_FILE)
/*
 * Since we have a target file for this S-function, declare an error here so
 * that, if for some reason this file is being used (instead of the target
 * file) for code generation, we can trap this problem during compilation.
 */
# error This_file_can_be_used_only_during_simulation_inside_Simulink
#endif

/*===========================================================================*
 *  Number of S-function Parameters and macros to access from the SimStruct  *
 *===========================================================================*/
#define NUM_PARAMS         (2)
#define DELAY_PARAM        (ssGetSFcnParam(S,0))
#define IC_PARAM           (ssGetSFcnParam(S,1))


/*====================*
 * S-function methods *
 *====================*/


/* Function: mdlCheckParameters ===============================================
 * Abstract:
 *      Check if the specified parameters are ok. If they are, then check if
 *      this block's sub-class record is set, if it is not, then cache away
 *      the parameter values in this record.
 */
static void mdlCheckParameters(SimStruct *S)
{
    int_T       i;
    real_T      *delay     = mxGetPr(DELAY_PARAM);
    int_T       delayLen   = mxGetNumberOfElements(DELAY_PARAM);
    int_T       icLen      = mxGetNumberOfElements(IC_PARAM);
    int_T       paramWidth = (delayLen > icLen) ? delayLen : icLen;
    const char  *msg       = NULL;

    /* Check parameters for validity */

    /* ic */
    if ( mxIsEmpty(IC_PARAM) || mxIsSparse(IC_PARAM) ||
         mxIsComplex(IC_PARAM) || !mxIsNumeric(IC_PARAM) ) {
        msg = "Initial condition must be a real vector";
        goto ERROR_EXIT;
    }

    /* delay */
    if ( mxIsEmpty(DELAY_PARAM) || mxIsSparse(DELAY_PARAM) ||
         mxIsComplex(DELAY_PARAM) || !mxIsNumeric(DELAY_PARAM) ) {
        msg = "Delay must be a positive integer vector";
        goto ERROR_EXIT;
    }
    for (i=0; i<delayLen; i++) {
        int_T j = (int_T)delay[i];
        if ( (delay[i] < 0.0) || ((real_T)j != delay[i]) ) {
            msg = "Delay must be a nonnegative integer vector";
            goto ERROR_EXIT;
        }
    }

    /* Check that the parameter widths are compatible  */
    if ( ((delayLen != 1) && (delayLen != paramWidth)) ||
         ((icLen    != 1) && (icLen    != paramWidth))  ) {
        msg = "The widths of the Delay and Initial "
              "condition vectors are not compatible";
        goto ERROR_EXIT;
    }

    return;

ERROR_EXIT:
    if (msg != NULL) {
        ssSetErrorStatus(S, msg);
    }
} /* end mdlCheckParameters */



/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *      Allocate memory for the sub-class record, and save the pointer in this
 *      s-function's user data. Then call mdlCheckParams() to check and cache
 *      the block parameters. Set all the sizes to be  dynamically sized, this
 *      block can scalar expand.
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_PARAMS);  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink */
    }
#endif

    ssSetSFcnParamNotTunable(S,0); /* Delay and IC parameter can't be */
    ssSetSFcnParamNotTunable(S,1); /* changed during simulation.      */

    ssSetNumContStates(    S, 0);
    ssSetNumDiscStates(    S, 0);


    /* Setup input/output ports */
    {
        int_T  icLen      = mxGetNumberOfElements(IC_PARAM);
        int_T  delayLen   = mxGetNumberOfElements(DELAY_PARAM);
        int_T  paramWidth = (delayLen > icLen) ? delayLen : icLen;
        int_T  portWidth  = (paramWidth==1)? DYNAMICALLY_SIZED: paramWidth;
        real_T *delay     = mxGetPr(DELAY_PARAM);
        int_T  df         = 0;   /* assume no direct feedthrough */
        int_T  i;

        if (!ssSetNumInputPorts(S, 1)) return;
        ssSetInputPortWidth(S, 0, portWidth);

        /*
         * Compute direct feedthrough based upon the delay parameter
         */
        for (i=0; i<delayLen; i++) {
            if (delay[i] == 0.0) {
                df = 1;
                break;
            }
        }
        ssSetInputPortDirectFeedThrough(S, 0, df);

        if (!ssSetNumOutputPorts(S,1)) return;
        ssSetOutputPortWidth(S, 0, portWidth);
    }

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, DYNAMICALLY_SIZED);
    ssSetNumIWork(S, DYNAMICALLY_SIZED);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs( S, 0);
    ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE |
                     SS_OPTION_ALLOW_INPUT_SCALAR_EXPANSION));
}


/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *      Set the sample time to be inherited, however if this inherited
 *      sample time is continuous, we detect this and set error status
 *      in mdlSetWorkWidths() which is called after the sample times are
 *      propagated.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}



/* Function: mdlInitializeConditions ==========================================
 * Abstract:
 *      Initialize the buffers to the specified initial values and reset the
 *      head pointers to zero (This reset is required if this block is used
 *      in an enabled sub-system which resets on disable).
 */
#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
    real_T      *delay      = mxGetPr(DELAY_PARAM);
    int_T       delayLen    = mxGetNumberOfElements(DELAY_PARAM);
    int_T       delayInc    = (delayLen > 1);
    real_T      *ic         = mxGetPr(IC_PARAM);
    int_T       icLen       = mxGetNumberOfElements(IC_PARAM);
    int_T       icInc       = (icLen > 1);
    real_T      *buffer     = ssGetRWork(S);
    int_T       *head       = ssGetIWork(S);
    int_T       headInc     = (ssGetNumIWork(S) > 1);
    int_T       i           = ssGetOutputPortWidth(S,0);

    while (i-- != 0) {
        int_T j = (int_T)*delay;
        while (j-- != 0) {
            *buffer++ = *ic;
        }
        ic    += icInc;
        delay += delayInc;

        *head = 0;
        head += headInc;
    }
}



/* Function: mdlOutputs =======================================================
 * Abstract:
 *      Read the buffer at the head.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T *buffer  = ssGetRWork(S);
    real_T *delay   = mxGetPr(DELAY_PARAM);
    int_T  delayLen = mxGetNumberOfElements(DELAY_PARAM);
    int_T  delayInc = (delayLen > 1);
    int_T  *head    = ssGetIWork(S);
    int_T  headInc  = (ssGetNumIWork(S) > 1);
    int_T  i        = ssGetOutputPortWidth(S,0);
    real_T *y       = ssGetOutputPortRealSignal(S,0);

    if (!ssGetInputPortDirectFeedThrough(S,0)) {
        while(i-- != 0) {
            *y++ = buffer[*head];
            buffer += (int_T)*delay;  /* start of next buffer channel */
            head   += headInc;
            delay  += delayInc;
        }

    } else {
        /* At least one delay is zero: */
        InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
        int_T             uIdx  = 0;
        int_T             uInc  = (ssGetInputPortWidth(S,0) > 1);
        while(i-- != 0) {
            if (*delay > 0) {
                *y++ = buffer[*head];
                buffer += (int_T)*delay;  /* start of next buffer channel */
            } else {
                *y++ = *uPtrs[uIdx];
            }
            head  += headInc;
            delay += delayInc;
            uIdx  += uInc;
        }
    }
}



/* Function: mdlUpdate ========================================================
 * Abstract:
 *      Write into the buffer at the head, and bump the head to the next
 *      location in a circular fashion.
 */
#define MDL_UPDATE
static void mdlUpdate(SimStruct *S, int_T tid)
{
    real_T            *delay   = mxGetPr(DELAY_PARAM);
    int_T             delayLen = mxGetNumberOfElements(DELAY_PARAM);
    int_T             delayInc = (delayLen > 1);
    real_T            *buffer  = ssGetRWork(S);
    int_T             *head    = ssGetIWork(S);
    InputRealPtrsType uPtrs    = ssGetInputPortRealSignalPtrs(S,0);
    int_T             uIdx     = 0;
    int_T             uInc     = (ssGetInputPortWidth(S,0) > 1);
    int_T             i        = ssGetOutputPortWidth(S,0);

    if (ssGetNumIWork(S) > 1) {
        /* More than one delay time specified */
        while(i-- != 0) {
            if (*delay > 0) {
                buffer[*head] = *uPtrs[uIdx];
                if ( (++(*head)) == *delay ) {
                    *head = 0;
                }
                buffer += (int_T)*delay;
            }
            ++head;
            delay += delayInc;
            uIdx  += uInc;
        }

    } else {
        /* One delay time (but possibly multiple channels) */
        if (*delay > 0) {
            while(i-- != 0) {
                buffer[*head] = *uPtrs[uIdx];
                buffer += (int_T)*delay;
                delay  += delayInc;
                uIdx   += uInc;
            }
            if ( (++(*head)) == *delay ) {
                *head = 0;
            }
        }
    }
}



/* Function: mdlTerminate =====================================================
 *
 */
static void mdlTerminate(SimStruct *S)
{
}


#if defined(MATLAB_MEX_FILE)
/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *      Called after all the width and sample times have been propagated.
 *      Determine the size of the buffers (RWork) needed and the number
 *      of buffer indices.
 */
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    int_T   i, blockWidth;
    int_T   bufferSize = 0;
    real_T *delay      = mxGetPr(DELAY_PARAM);
    int_T   delayLen   = mxGetNumberOfElements(DELAY_PARAM);
    int_T   delayInc   = (delayLen > 1);
    int_T   icLen      = mxGetNumberOfElements(IC_PARAM);
    int_T   paramWidth = (delayLen > icLen) ? delayLen : icLen;

    /*
     * The sample time of this block is now set, error out if it is continuous.
     */
    if (ssGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
        ssSetErrorStatus(S, "Input to block must have a discrete sample time");
        return;
    }

    blockWidth = (paramWidth == 1) ? ssGetOutputPortWidth(S,0) : paramWidth;
    for (i=0; i<blockWidth; i++) {
        bufferSize += (int_T) *delay;
        delay += delayInc;
    }

    ssSetNumRWork(S, bufferSize);
    ssSetNumIWork(S, delayLen);
}
#endif


/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE
#include "simulink.c"   /* MEX Interface Mechanism   */
#else
#include "cg_sfun.h"    /* RTW Registration Function */
#endif


/* EOF: sndelay.c */