/*
 * SBUFFER   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] = sbuffer(t,x,u,flag,bufSize,overlap,sampleTime)
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.9 $  $Date: 2000/05/05 20:15:57 $
 */

#define S_FUNCTION_NAME sbuffer

#include <math.h>

#include "simstruc.h"  /* SimStruct, etc */

/*
 * Defines for easy access of the input parameters
 */

#define NUM_ARGS	3
#define BUFFERSIZE	ssGetArg(S,0)
#define BUFFER_OVERLAP	ssGetArg(S,1)
#define SAMPLE_TIME	ssGetArg(S,2)
#define sampleTol       1E-8            /* sqrt(eps), more or less */

/*
 * IWork indices
 */
#define BUFFER_INDEX 0
#define OUTPUT_INDEX 1
#define FILL_COUNT   2
#define UL_COUNT     3


/*
 *  Order of initialization calls:
 *    InitializeSizes
 *    InitializeSampleTimes
 *    InitializeConditions
 */


/*
 * mdlInitializeSizes - called to initialize the sizes array stored in
 *                      the SimStruct.  The sizes array defines the
 *                      characteristics (number of inputs, outputs,
 *                      states, etc.) of the S-Function.
 */

static void mdlInitializeSizes(SimStruct *S)
{
#ifdef MATLAB_MEX_FILE
    mexWarnMsgTxt("Obsolete blocks calling the CMEX S-Function 'sbuffer' have\n"
                  "been detected.  If you wish to upgrade this model, replace\n"
                  "all previous Buffer blocks with the new version found in\n"
                  "the block library:\n"
                  "        'dsplib/General DSP/Buffers'\n."
                  "Note that if any Buffer blocks are upgraded, all Unbuffer\n"
                  "blocks must also be upgraded at the same time for compat-\n"
                  "ibility.  Also, note that an additional pipeline delay\n"
                  "equal to one buffer length will occur in the upgraded\n"
                  "model for each Buffer and Unbuffer block.\n"
                  "If you wish to disable this warning message, type\n"
                  "'warning off' at the MATLAB command line.\n\n");
#endif
  /*
   * Set-up size information:
   */ 
  if (ssGetNumArgs(S) == NUM_ARGS) {
    int_T bufSize, bufOverlap, num_sample_times;

    if ((mxGetM(SAMPLE_TIME) != 2) || (mxGetN(SAMPLE_TIME) != 2)) {
      num_sample_times = 0;
    } else {
      real_T *pr = mxGetPr(SAMPLE_TIME);
      if ((fabs(pr[0]-pr[1]) < sampleTol) && (fabs(pr[2]-pr[3]) < sampleTol)) {
	num_sample_times = 1;
      } else {
	num_sample_times = 2;
      }
    }

#ifdef MATLAB_MEX_FILE
    if (num_sample_times < 1) {
      mexErrMsgTxt("The sample time must be a 2 x 2 matrix");
    }
    if ((mxGetM(BUFFERSIZE) != 1) || (mxGetN(BUFFERSIZE) != 1)) {
      mexErrMsgTxt("The buffer size must be a scalar");
    }    
    if ((mxGetM(BUFFER_OVERLAP) != 1) || (mxGetN(BUFFER_OVERLAP) != 1)) {
      mexErrMsgTxt("The buffer overlap must be a scalar");
    }
#endif

    bufSize    = (int_T) mxGetPr(BUFFERSIZE)[0];
    bufOverlap = (int_T) mxGetPr(BUFFER_OVERLAP)[0];

#ifdef MATLAB_MEX_FILE
    if (bufOverlap >= bufSize) {
      mexErrMsgTxt("The buffer overlap cannot be larger than the buffer size");
    } 
#endif

    /*
     * NOTE:  Number of sample times is usually 2, but could
     * be 1 if the times are identical.
     */

    ssSetNumContStates(    S, 0);
    ssSetNumDiscStates(    S, 0);
    ssSetNumInputs(        S, 1);
    ssSetNumOutputs(       S, bufSize);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumInputArgs(     S, NUM_ARGS);
    ssSetNumSampleTimes(   S, num_sample_times);
    ssSetNumRWork(         S, bufSize * 2);
    ssSetNumIWork(         S, 4);
    ssSetNumPWork(         S, 0);
    ssSetNumModes(         S, 0);
    ssSetNumNonsampledZCs( S, 0);
  }
#ifdef MATLAB_MEX_FILE
  else {
    mexErrMsgTxt("Wrong number of input arguments");
  }
#endif
}

/*
 * mdlInitializeSampleTimes - initializes the array of sample times stored in
 *                            the SimStruct associated with this S-Function.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
  real_T *pr = mxGetPr(SAMPLE_TIME);
  int_T   n  = ssGetNumSampleTimes(S);

  ssSetSampleTimeEvent(S, 0, pr[0]);
  ssSetOffsetTimeEvent(S, 0, pr[2]);

  if (n > 1) {
    ssSetSampleTimeEvent(S, 1, pr[1]);
    ssSetOffsetTimeEvent(S, 1, pr[3]);
  }
}

/*
 * mdlInitializeConditions - initializes the states for the S-Function
 */
static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
  real_T *buffer         = ssGetRWork(S);
  int_T  *bufIndex       = ssGetIWork(S) + BUFFER_INDEX;
  int_T  *bufOutputIndex = ssGetIWork(S) + OUTPUT_INDEX;
  int_T  *bufFillCount   = ssGetIWork(S) + FILL_COUNT;
  int_T  *ul_count       = ssGetIWork(S) + UL_COUNT;  /* underlap count for
						         neg. overlap case */
  real_T overlap         = mxGetPr(BUFFER_OVERLAP)[0];
  int_T  bufSize         = ssGetNumOutputs(S);
  int_T  n;
  
  /*
   * Initialize the buffer to all-zero:
   */
  for (n=2*bufSize; n-- != 0; ) {
      *buffer++ = (real_T)0.0;
  }
  
  /*
   * Initialize the current buffer position and buffer start
   */
  *bufIndex = 0;
  if (overlap < 0) {
    *bufFillCount = bufSize;
  } else {
    *bufFillCount = bufSize - overlap;
  }
  n = (bufSize - 1)/(*bufFillCount);
  *bufOutputIndex = 2 * bufSize - n * (*bufFillCount);
  *ul_count = 0;
}

/*
 * mdlOutputs - computes the outputs of the S-Function
 */

static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
  int_T   num_out = ssGetNumOutputs(S);
  int_T   bufSize = 2 * num_out;
  real_T *buffer;

  /*
   * Acquire the buffer data
   */
  if (ssIsSampleHitEvent(S, 0, tid)) {
    int_T *ul_count = ssGetIWork(S) + UL_COUNT;
    
    if (*ul_count>0) {
      (*ul_count)--;      /* skip this sample because of negative overlap */

    } else {
      /* collect data */
      int_T *bufIndex = ssGetIWork(S) + BUFFER_INDEX;
      buffer = ssGetRWork(S);
      buffer[(*bufIndex)++] = *u;

      /* if we have collected num_out samples, reset underlap count */
      if ((*bufIndex == num_out) || (*bufIndex == bufSize)) {
	real_T overlap = mxGetPr(BUFFER_OVERLAP)[0];
	if (overlap < 0) {
	  *ul_count = -overlap;
	}
      }

      /* if we have reached the end of the real_T buffer,
	 reset buffer index */
      if (*bufIndex == bufSize) {
	*bufIndex = 0;
      }
    }
  }

  /*
   * Only update the outputs when the next wave of inputs have been acquired
   */
  if ((ssGetNumSampleTimes(S) == 1) || ssIsSampleHitEvent(S, 1, tid)) {
    int_T *bufOutputIndex = ssGetIWork(S) + OUTPUT_INDEX;
    int_T bufFillCount    = *(ssGetIWork(S) + FILL_COUNT);

    buffer = ssGetRWork(S) + *bufOutputIndex;

    if (*bufOutputIndex + num_out < bufSize) {
      real_T *y_end = y + num_out;
      while(y < y_end) *y++ = *buffer++;

    } else {
      real_T *y_end = y + bufSize - *bufOutputIndex;
      while(y < y_end) *y++ = *buffer++;

      buffer = ssGetRWork(S);
      y_end = y + num_out - (bufSize - *bufOutputIndex);
      while(y < y_end) *y++ = *buffer++;
    }

    *bufOutputIndex += bufFillCount;
    if (*bufOutputIndex >= bufSize) {
      *bufOutputIndex -= bufSize;
    }
  }
}

/*
 * mdlUpdate - computes the discrete states of the S-Function
 */
static void mdlUpdate(real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
}

/*
 * mdlDerivatives - computes the derivatives of the S-Function
 */
static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
                           SimStruct *S, int_T tid)
{
}

/*
 * mdlTerminate - called at termination of model execution.
 */
static void mdlTerminate(SimStruct *S)
{
}

#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