/*
* SBUFFERC  A SIMULINK overlapping buffer block.
* DSP Blockset S-Function for buffering signals into vectors.
* This SIMULINK S-function buffers a scalar length one input, 
* converting it to a vector output; it is called by the Buffer 
* blocks in the DSP Blockset.
*
*    Syntax:  [sys, x0] = sbufferc(t,x,u,flag,bufSize,overlap,sampleTime)
*
*  Copyright (c) 1995-98 by The MathWorks, Inc.
*  $Revision: 1.22 $  $Date: 2000/06/14 14:27:57 $
*/

#define S_FUNCTION_NAME sbufferc
#define S_FUNCTION_LEVEL 2

#include <string.h>  /* memcpy */
#include "dsp_sim.h"

/*
* Defines for easy access of the input parameters
*/
#define BUFFER_SIZE     ssGetSFcnParam(S,0)
#define BUFFER_OVERLAP  ssGetSFcnParam(S,1)
#define SAMPLE_TIME     ssGetSFcnParam(S,2)
#define INIT_COND       ssGetSFcnParam(S,3)
#define NUM_ARGS        (4)

/*
* PWork indices
*/
#define OUTBUF_PTR 0
#define INBUF_PTR  1
#define NUM_PWORKS 2

/*
* IWork indices
*/
#define UL_COUNT     0
#define CIRC_BUF_LEN 1
#define BUF_N        2
#define BUF_V        3
#define NUM_IWORKS   4

#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    
    if ((mxGetM(BUFFER_SIZE) != 1) || (mxGetN(BUFFER_SIZE) != 1)) {
        THROW_ERROR(S,"The buffer size must be a scalar");
    }
    if ((mxGetM(BUFFER_OVERLAP) != 1) || (mxGetN(BUFFER_OVERLAP) != 1)) {
        THROW_ERROR(S,"The buffer overlap must be a scalar");
    }
    if ((mxGetM(SAMPLE_TIME) != 1) ||
        (mxGetN(SAMPLE_TIME) == 0) ||
        (mxGetN(SAMPLE_TIME) > 2)   ) {
        THROW_ERROR(S,"The sample time must be either a scalar or 2-element row vector");
    }
    
    {
        int_T N = (int_T) mxGetPr(BUFFER_SIZE)[0];
        int_T V = (int_T) mxGetPr(BUFFER_OVERLAP)[0];
        if (V >= N) {
            THROW_ERROR(S,"The buffer overlap must be less than the buffer size");
        }
    }
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    int_T numUserArgs = ssGetSFcnParamsCount(S);
    int_T N, V, num_Ts;
    
    /*
     * Compatibility with previous buffers that did not
     * pass IC's in the S-function argument list
     */
    if (numUserArgs == NUM_ARGS - 1) {
        ssSetNumSFcnParams(S, NUM_ARGS-1);
    } else {
        ssSetNumSFcnParams(S, NUM_ARGS);
    }
    
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) != numUserArgs) return;
    mdlCheckParameters(S);
    if (ssGetErrorStatus(S) != NULL) return;
#endif
    
    /*
     * NOTE:  Number of sample times is usually 2,
     * but could be 1 if the times are identical.
     */
    N      = (int_T) (mxGetPr(BUFFER_SIZE)[0]);
    V      = (int_T) (mxGetPr(BUFFER_OVERLAP)[0]);
    num_Ts = (N-V == 1) ? (int_T)1 : (int_T)2;

    
    /* Inputs: */
    if (!ssSetNumInputPorts(S,1)) return;
    ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);
    ssSetInputPortDirectFeedThrough(S, 0, 0);

    /* Outputs: */
    if (!ssSetNumOutputPorts(S,1)) return;
    ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);

    ssSetNumSampleTimes(   S, num_Ts);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumIWork(         S, NUM_IWORKS);
    ssSetNumPWork(         S, NUM_PWORKS);
    ssSetOptions(          S, (SS_OPTION_EXCEPTION_FREE_CODE |
                               SS_OPTION_NONSTANDARD_PORT_WIDTHS));
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    real_T *pr     = mxGetPr(SAMPLE_TIME);
    int_T   N      = (int_T) (mxGetPr(BUFFER_SIZE)[0]);
    int_T   V      = (int_T) (mxGetPr(BUFFER_OVERLAP)[0]);
    int_T   num_Ts = (N-V == 1) ? (int_T)1 : (int_T)2;
    real_T  Tsi    = pr[0];  /* Input sample time */
    real_T  offset = (mxGetN(SAMPLE_TIME) > 1) ? pr[1] : 0.0;

    /* Consider disallowing non-zero sample time offsets */

    ssSetSampleTime(S, 0, Tsi);
    ssSetOffsetTime(S, 0, offset);
    
    if (num_Ts == 2) {
        real_T Tso = Tsi*(N-V);  /* Correct even for V<0 */
        ssSetSampleTime(S, 1, Tso);
        ssSetOffsetTime(S, 1, offset);
    }
}


#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
    /*
     * Initialize input and output buffer pointers.
     * Preset any output overlap samples to zero.
     *
     * For convenience in presetting initial "overlap" samples,
     * the buffer starts in the following configuration.
     * NOTE: (N-V) is the number of "new" samples required per buffer.
     *
     *    |-----| <- inBuf
     *    |     |
     *    | N-V |  (or just N if V<0)
     *    |     |
     *    |-----| <- outBuf
     *    |     | (preset this region to init conditions)
     *    |     |
     *    |  N  |
     *    |     |
     *    |     |
     *    |-----|
     */
    const int_T nchans = ssGetInputPortWidth(S,0);
    const int_T N      = (int_T)(mxGetPr(BUFFER_SIZE)[0]);
    const int_T V      = (int_T)(mxGetPr(BUFFER_OVERLAP)[0]);
    
    /*
    * newSPB is the # of new samples recorded for each new buffer
    * produced.  Note that for underlap, "-V" new samples
    * will need to be skipped in addition to newSPB.
    */
    const int_T circbuf_len = (V<=0) ? 2*N : 2*N-V;
    const int_T newSPB      = (V<=0) ? N   : N-V;
    
    real_T *circbuf = ssGetRWork(S);
    real_T *outBuf  = circbuf + newSPB;
    
    const int_T   numELE  = nchans * N;	
    real_T       *pIC;
    int_T         numIC;
    
    int_T numUserArgs = ssGetSFcnParamsCount(S);
    if (numUserArgs == NUM_ARGS - 1) {
    /* If no initial conditions were passed through the S-function
    * set them to be 0 (backwards compatible)
        */
        numIC = 0;
        pIC = NULL;
    } else {
        numIC   = mxGetNumberOfElements(INIT_COND);
        pIC     = mxGetPr(INIT_COND);
    }
    
    ssSetPWorkValue(S, INBUF_PTR,  circbuf);
    ssSetPWorkValue(S, OUTBUF_PTR, outBuf);
    
#ifdef MATLAB_MEX_FILE
    if ((numIC != 0) && (numIC != 1)			/* scalar */
        && (numIC != N)            /* vector */
        && (numIC != numELE)) {    /* matrix */
        ssSetErrorStatus(S, "Initial condition vector has incorrect dimensions");
        return;
    }
#endif
    
    /* Preset output buffers to initial conditions: */
    
    {
        int_T   i, j;
        real_T *p;
        
        if (numIC <= 1) {
            /* Scalar expansion, or no IC's given: */
            real_T ic = (numIC == 0) ? (real_T)0.0 : *pIC;
            
            for(i = 0; i < nchans; i++) {
                p = outBuf;
                for (j = 0; j < N; j++) {
                    *p++ = ic;
                }
                outBuf += circbuf_len;
            }
            
        } else if (numIC == N) {
            /* Same IC's for all channels: */
            for(i = 0; i < nchans; i++) {
                memcpy((void *)outBuf, (void *)pIC, N*sizeof(real_T));
                outBuf += circbuf_len;
            }
            
        } else {
            /* Matrix of IC's: */
            for(i = 0; i < nchans; i++) {
                memcpy((void *)outBuf, (void *)pIC, N*sizeof(real_T));
                pIC += N;
                outBuf += circbuf_len;
            }
        }
    }
    
    /*
    * Underlap is used to skip samples for negative overlap case.
    * Counts TOTAL # of samples to acquire, whether we store them
    * or not.  Underlap requires skipping samples AFTER the buffer
    * fills, so no unnecessary initial sample-delay occurs.
    */
    ssSetIWorkValue(S, UL_COUNT,     0);
    ssSetIWorkValue(S, CIRC_BUF_LEN, circbuf_len);
    ssSetIWorkValue(S, BUF_N,        N);
    ssSetIWorkValue(S, BUF_V,        V);
}


static void mdlOutputs(SimStruct *S, int_T tid)
{
    const int_T numTs = ssGetNumSampleTimes(S);
    
    if ((numTs == 1) || ssIsSampleHit(S, 1, tid)) {
        /* Output next buffer: */
        real_T      *y           = ssGetOutputPortSignal(S, 0);
        const int_T  nchans      = ssGetInputPortWidth(S, 0);
        const int_T  N           = ssGetIWorkValue(S, BUF_N);
        const int_T  circbuf_len = ssGetIWorkValue(S, CIRC_BUF_LEN);
        real_T      *circbuf     = ssGetRWork(S);
        real_T      *outBuf      = ssGetPWorkValue(S, OUTBUF_PTR);
        const int_T  cnt         = (circbuf + circbuf_len) - outBuf;
        /* # samples from outBuf to end of buffer */
        
        if (cnt >= N) {
            /* Output buffer is one contiguous chunk: */
            real_T *p = outBuf;
            int_T   i;
            
            for(i=0; i<nchans; i++) {
                memcpy((void *)y, (void *)p, N*sizeof(real_T));
                y += N; p += circbuf_len;
            }
            
            outBuf = (cnt == N) ? circbuf : outBuf + N;
            
        } else {
            /* Output buffer is split into two chunks: */
            real_T *pout  = outBuf;
            real_T *pcirc = circbuf;
            int_T   i;
            
            for(i=0; i<nchans; i++) {
                memcpy((void *)y,       (void *)pout,      cnt*sizeof(real_T));
                memcpy((void *)(y+cnt), (void *)pcirc, (N-cnt)*sizeof(real_T));
                y += N; pout += circbuf_len; pcirc += circbuf_len;
            }
            
            outBuf = circbuf + (N-cnt);
        }
        
        /* Bump output buffer pointer if overlap present: */
        {
            const int_T V = ssGetIWorkValue(S, BUF_V);
            
            if (V > 0) {
                outBuf -= V;  /* Backup for overlap */
                if (outBuf < circbuf) {
                    outBuf += circbuf_len;
                }
            }
        }
        
        /* Store next output buffer pointer position: */
        ssSetPWorkValue(S, OUTBUF_PTR, (void *)outBuf);
    }
}


#define MDL_UPDATE
static void mdlUpdate(SimStruct *S, int_T tid)
{
    const int_T numTs = ssGetNumSampleTimes(S);
    
    if ((numTs == 1) || ssIsSampleHit(S, 0, tid)) {
        /* Acquire input sample: */
        
        const int_T V = ssGetIWorkValue(S, BUF_V);
        
        if (V < 0)  {
            /* Underlap case: */
            const int_T  N        = ssGetIWorkValue(S, BUF_N);
            int_T       *ul_count = ssGetIWork(S) + UL_COUNT;
            ++(*ul_count);
            
            /* skip this sample because of negative overlap */
            if (*ul_count > N) {
                if (*ul_count == N-V) {
                    *ul_count = 0;
                }
                return; /* Skip acquisition */
            }
        }
        
        /* Store the latest sample: */
        {
            const int_T  nchans      = ssGetInputPortWidth(S, 0);
            const int_T  circbuf_len = ssGetIWorkValue(S, CIRC_BUF_LEN);
            real_T      *circbuf     = ssGetRWork(S);
            real_T      *inBuf       = ssGetPWorkValue(S, INBUF_PTR);
            InputRealPtrsType uptr   = ssGetInputPortRealSignalPtrs(S,0);
            
            {
                real_T *p = inBuf++;
                int_T   i;
                
                for (i=0; i<nchans; i++) {
                    *p = **(uptr++);
                    p += circbuf_len;
                }
            }
            
            /*
            * If we have reached the end of the circular buffer,
            * reset the input buffer index:
            */
            if (inBuf == circbuf + circbuf_len) {
                inBuf = circbuf;
            }
            
            /* Store next input buffer pointer position: */
            ssSetPWorkValue(S, INBUF_PTR, (void *)inBuf);
        }
    }
}


static void mdlTerminate(SimStruct *S)
{
}


#if defined(MATLAB_MEX_FILE)

#define MDL_SET_INPUT_PORT_WIDTH
static void mdlSetInputPortWidth(SimStruct *S, int_T port,
                                  int_T inputPortWidth)
{
    const int_T N = (int_T)(mxGetPr(BUFFER_SIZE)[0]);

    ssSetInputPortWidth(S,  port, inputPortWidth);
    ssSetOutputPortWidth(S, 0,    inputPortWidth * N);
}

#define MDL_SET_OUTPUT_PORT_WIDTH
static void mdlSetOutputPortWidth(SimStruct *S, int_T port,
                                   int_T outputPortWidth)
{
    const int_T N = (int_T)(mxGetPr(BUFFER_SIZE)[0]);
    int_T inputPortWidth = outputPortWidth / N;

    ssSetOutputPortWidth(S, port, outputPortWidth);

    if (outputPortWidth != inputPortWidth*N) {
        THROW_ERROR(S,"Output vector width inconsistent with buffer size and number of channels.");
    }
    ssSetInputPortWidth(S, 0, inputPortWidth);
}

#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    const int_T N = (int_T)(mxGetPr(BUFFER_SIZE)[0]);
    const int_T V = (int_T)(mxGetPr(BUFFER_OVERLAP)[0]);
    const int_T circbuf_len = (V<=0) ? 2*N : 2*N-V;
    
    ssSetNumRWork(S, circbuf_len * ssGetInputPortWidth(S, 0));
}

#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