/*
 * SUNBUFFC  A SIMULINK unbuffer block
 * DSP Blockset S-Function for unbuffering vectors.
 *	This SIMULINK S-function unbuffers a vector input, converting it
 *	to a scalar output; it is called by the Unbuffer blocks in the
 *      DSP Blockset.
 *
 *   Syntax:  [sys, x0] = sunbuff(t,x,u,flag,...
 *               inputWidth, sampleTime)
 *
 *  Copyright (c) 1995-98 by The MathWorks, Inc.
 *  $Revision: 1.15 $  $Date: 1997/11/26 20:24:45 $
 */
#define S_FUNCTION_NAME sunbuffc

#include "simstruc.h"

#define NUM_CHANS ssGetArg(S,0)
#define SAMPLE_TIME ssGetArg(S,1)
#define NUM_ARGS    2

#define OUTBUF_PTR 0
#define INBUF_PTR  1
#define NUM_PWORK  2

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

    if ((mxGetM(NUM_CHANS) != 1) || (mxGetN(NUM_CHANS) != 1)) {
       msg = "Number_of_channels must be a scalar";
       goto ERROR_EXIT;
    }
    
    if (mxGetPr(NUM_CHANS)[0] != (int_T)mxGetPr(NUM_CHANS)[0]) {
       msg = "Number_of_channels must be an integer";
       goto ERROR_EXIT;
    }

    if ((mxGetM(SAMPLE_TIME) != 1) ||
        (mxGetN(SAMPLE_TIME) == 0) || (mxGetN(SAMPLE_TIME) > 2)) {
        msg = "The sample time must be a scalar or 2-element row vector";
        goto ERROR_EXIT;
    }
    if (mxGetPr(SAMPLE_TIME)[0] <= (real_T)0.0) {
        msg = "Block must have a discrete sample time";
        goto ERROR_EXIT;
    }

    {
        real_T offset = (mxGetN(SAMPLE_TIME) > 1) ? mxGetPr(SAMPLE_TIME)[1] : 0.0;
        if (offset != (real_T)0.0) {
            mexWarnMsgTxt("Ignoring non-zero sample time offset in Unbuffer block");
        }
    }

ERROR_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, 2);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumPWork(         S, NUM_PWORK);
    ssSetOptions(          S, SS_OPTION_EXCEPTION_FREE_CODE |
                              SS_OPTION_USING_ssGetUPtrs    );
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    /*
     * The CMEX S-Fcn API does not allow registration of 2 identical sample
     * times.  Since we do not have access to # inputs during mdlInitSizes
     * call, we cannot determine whether 1 or 2 distinct sample times will
     * be present (it is based on whether there are 1 or >1 inputs).
     *
     * Therefore, we always register 2 sample times, and must either forego
     * the case of an input of width 1, or register a "dummy" second
     * sample time.  We choose the latter.
     *
     * The dummy sample time is chosen to be a SLOWER multiple of the input
     * sample time for efficiency.
     *
     * Finally, note that a requirement for CMEX S-Fcns is that we register
     * sample times in decreasing-rate order, that is, the fast rate
     * (shortest sample time) first, then the slow rate.
     */
    real_T ts_in = (real_T)(mxGetPr(SAMPLE_TIME)[0]);

    int_T nchans      = (int_T) (mxGetPr(NUM_CHANS)[0]);
    int_T buffer_size = ssGetNumInputs(S) / nchans;

    if (buffer_size == 1) {
        /* One sample per column:
         * TID 0: Input/Output rate (faster)
         * TID 1: Dummy rate        (slower)
         */
        ssSetSampleTimeEvent(S, 0, ts_in);
        ssSetSampleTimeEvent(S, 1, 2*ts_in);

    } else {
        /* TID 0: Output rate (faster)
         * TID 1: Input rate  (slower)
         */
        ssSetSampleTimeEvent(S, 0, ts_in / (real_T)buffer_size);
        ssSetSampleTimeEvent(S, 1, ts_in);
    }

    /* Ignore sample time offsets: */
    ssSetOffsetTimeEvent(S, 0, 0.0);
    ssSetOffsetTimeEvent(S, 1, 0.0);
}


#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    ssSetNumRWork(S, 2 * ssGetNumInputs(S));
}
#endif


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    int_T   N       = ssGetNumInputs(S);
    real_T *circbuf = ssGetRWork(S);
    real_T *outBuf  = circbuf + N;

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

    ssSetPWorkValue(S, INBUF_PTR,  circbuf);
    ssSetPWorkValue(S, OUTBUF_PTR, outBuf);

    /* Preset initial buffer condition to all-zeros: */
    while(N-- != 0) {
        *outBuf++ = 0.0;
    }
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
/* A buffer of length twice the input length is used because, in a real-time 
 * system, the input may not be finished reading before some of the fast output
 * times occur.  This introduces a delay of one input length. 
 */
    const int_T N            = ssGetNumInputs(S);
    const int_T nchans       = (int_T)(mxGetPr(NUM_CHANS)[0]);
    const int_T buffer_width = N/nchans;

	/* Scalar input.  Just pass through */
    if (N == 1) {
        if (ssIsSampleHit(S, 0, tid)) {
            UPtrsType uptr = ssGetUPtrs(S);
            *y = **uptr;
        }
        return;
    }

	/* Output at fast rate */
    if (ssIsSampleHit(S, 0, tid)) {
        /* Output the current sample: */
        real_T *buf     = ssGetRWork(S);
        real_T *outBuf  = ssGetPWorkValue(S, OUTBUF_PTR);

        if (nchans==1) {
            /* Single channel (a vector) of data: */
            *y++ = *outBuf++;

            if (outBuf == buf + 2*N) {
                outBuf = buf;
            }

        } else {
            /* Multiple channels (a matrix) of data: */
            real_T *p  = outBuf++;
            int_T kcnt = nchans;

            while (kcnt-- > 0) {
            	*y++ = *p;
        	p += buffer_width;
            }

            if (outBuf == buf + buffer_width) {
	   	     outBuf = buf + N;  /* wrap circular buffer */
            } else if (outBuf == buf + N + buffer_width) {
        	    outBuf = buf;
            }
        }

        ssSetPWorkValue(S, OUTBUF_PTR, outBuf);
    }

	/* Input at slow rate */
    if (ssIsSampleHit(S, 1, tid)) {
        /* Store the next input vector: */
        real_T   *buf   = ssGetRWork(S);
        real_T   *inBuf = ssGetPWorkValue(S, INBUF_PTR);
        UPtrsType uptr  = ssGetUPtrs(S);
        int_T     i     = N;

        while(i-- != 0) {
            *inBuf++ = **(uptr++);
        }

        if (inBuf == buf + 2*N) {
            inBuf = buf;  /* wrap circular buffer */
        }
        ssSetPWorkValue(S, INBUF_PTR, inBuf);
    }
}


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

#if defined(MATLAB_MEX_FILE)

#define MDL_GET_INPUT_PORT_WIDTH
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
{
    /* Input width is not a function of output width */
    return(DYNAMICALLY_SIZED);
}

#define MDL_GET_OUTPUT_PORT_WIDTH
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    /* 
     * Output width is always equal to the number of channels.  
     * First verify that the input width is evenly divisible by the number of
     * channels.
     */
    const int_T nchans = (int_T)(mxGetPr(NUM_CHANS)[0]);
    real_T bufflen_real = inputPortWidth / nchans;
    int_T  bufflen_int = bufflen_real;
    
    if (bufflen_real != bufflen_int) {
        ssSetErrorStatus(S, "Input vector width inconsistent with buffer size and number of channels.");
        return(1);
    }
         
    return(nchans);
}


#endif /* MATLAB_MEX_FILE */


#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