/*
 *  SDF2TFRM: SIMULINK multichannel Frame-based IIR (Direct Form II Transposed) filter block.
 *  DSP Blockset S-Function for multichannel IIR filtering.
 *
 *    Syntax:  [sys, x0] = sdf2tfrm(t,x,u,flag,NUM,DEN,IC,numCHANS)
 *
 *  The code is borrowed from sdf2t.c to take in a frame of data at each step.
 *  Thus, the input data is arranged as a matrix: 
 *
 *   Time Step |  Channel 0     |  Channel 1     |  ... | Channel n-1    |
 *       0     | 0 1 2 ... m-1  | 0 1 2 ... m-1  |  ... | 0 1 2 ... m-1  |
 *       1     | 0 1 2 ... m-1  | 0 1 2 ... m-1  |  ... | 0 1 2 ... m-1  |
 *       2     | 0 1 2 ... m-1  | 0 1 2 ... m-1  |  ... | 0 1 2 ... m-1  |
 *       ...
 *
 *   where n = number of channels, and m = length of time frame.
 *
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.5 $  $Date: 2000/06/14 14:27:57 $
 */

#define S_FUNCTION_NAME sdf2tfrm

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

#include "simstruc.h"

#define NUM_ARGS     4
#define NUM          ssGetArg(S,0)
#define DEN          ssGetArg(S,1)
#define IC           ssGetArg(S,2)
#define NUMCHANS_ARG ssGetArg(S,3)

#ifndef MAX
#define MAX(a,b) ((a)>(b)?(a):(b))
#endif

#ifndef MIN
#define MIN(a,b) ((a)<(b)?(a):(b))
#endif


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

    /* Check size of NUM and DEN: */
    M = mxGetM(NUM);
    N = mxGetN(NUM);
    if ( ((N != 1) && (M != 1)) || (M == 0) || (N == 0) ) {
        msg = "Numerator must be a vector";
        goto FCN_EXIT;
    }

    M = mxGetM(DEN);
    N = mxGetN(DEN);
    if ( ((M != 1) && (N != 1)) || (M == 0) || (N == 0) ) {
        msg = "Denominator must be a vector";
        goto FCN_EXIT;
    }

    if (*mxGetPr(DEN) == 0.0) {
        msg = "First denominator coefficient must be nonzero";
        goto FCN_EXIT;
    }
    
    M = mxGetM(NUMCHANS_ARG);
    N = mxGetN(NUMCHANS_ARG);
    if (M*N != 1) {
       msg = "Number of channels argument must be a scalar";
       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)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Simulink will report a parameter mismatch error */
    }
#endif
    
    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetOptions(          S, SS_OPTION_EXCEPTION_FREE_CODE |
                              SS_OPTION_USING_ssGetUPtrs);
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTimeEvent(S, 0, 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    real_T *pIC       = mxGetPr(IC);   /* Initial Conditions vector */
    real_T *DlyBuf    = ssGetRWork(S); /* Delay buffer storage      */
    const int_T   lenNUM    = mxGetNumberOfElements(NUM);
    const int_T   lenDEN    = mxGetNumberOfElements(DEN);
    const int_T   numIC     = mxGetNumberOfElements(IC);
    const int_T   numCHANS  = (int_T)(mxGetPr(NUMCHANS_ARG)[0]);
    const int_T   lenFRAME  = ssGetNumInputs(S)/numCHANS;
    const int_T   numDELAYS = MAX(lenNUM, lenDEN) - 1;
    const int_T   numELE    = numCHANS * numDELAYS;
    int_T  i;

#ifdef MATLAB_MEX_FILE
    if ((numIC != 0) && (numIC != 1)
                     && (numIC != numDELAYS)
                     && (numIC != numELE)) {
        ssSetErrorStatus(S, "Initial condition vector has incorrect dimensions");
        return;
    }

    if (lenFRAME*numCHANS != ssGetNumInputs(S)) {
        ssSetErrorStatus(S, "Number of channels not commensurate with input width");
        return;
    }

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

#endif

    /*
     * Initialize the delay buffers with initial conditions.
     */
    if (numIC <= 1) {
        /* Scalar expansion, or no IC's given: */
        real_T ic = (numIC == 0) ? (real_T)0.0 : *pIC;
        for(i=numELE; i-- > 0; ) {
            *DlyBuf++ = ic;
        }

    } else if (numIC == numDELAYS) {
        /* Same IC's for all channels: */
        for (i=numCHANS; i-- > 0; ) {
            memcpy(DlyBuf, pIC, numDELAYS*sizeof(real_T));
            DlyBuf += numDELAYS;
        }

    } else {
        /*
         * Matrix of IC's:
         * Assume numDELAYS rows and numCHANS columns
         */
        memcpy(DlyBuf, pIC, numELE*sizeof(real_T));
    }
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
  /*
   * Algorithm: Direct Form-II Transposed
   *
   *   y  = x*b0        + D0   (output)       b# = numerator polynomial,
   *   D0 = x*b1 - y*a1 + D1   (delay         a# = denominator polynomial,
   *   D1 = x*b2 - y*a2 + D2    updates)      D# = delay buffers
   *   ...                                    y  = output value
   *   DN = x*bN - y*aN                       x  = input value
   *
   * Implementation:
   *   *y++  = x * *b++             + *DR++;
   *   *DL++ = x * *b++ - y * *a+++ + *DR++;
   *   *DL++ = x * *b++ - y * *a+++ + *DR++;
   *   ...
   *   *DL++ = x * *b   - y * *a;
   *
   * Buffer storage:
   *   ----------------------------------------------
   *   |    Channel 0     |     Channel 1      |  ...
   *   |D0 | D1 | D2 |... | D0 | D1 | D2 | ... |  ...
   *   ----------------------------------------------
   */
    const int_T    lenNUM    = mxGetNumberOfElements(NUM);
    const int_T    lenDEN    = mxGetNumberOfElements(DEN);
    const int_T    lenMIN    = MIN(lenNUM, lenDEN);  /* "Common" number of coeff's */
    const int_T    numDELAYS = MAX(lenNUM, lenDEN) - 1;
    int_T    numCHANS  = (int_T)(mxGetPr(NUMCHANS_ARG)[0]);
    const int_T    lenFRAME  = ssGetNumInputs(S)/numCHANS;

    real_T    *tmpDL_top    = ssGetRWork(S); /* Start of buffer */
    real_T    *tmpDL;
    real_T    *tmpDR;
    real_T    *numPoly  = mxGetPr(NUM);
    real_T    *denPoly  = mxGetPr(DEN);
    UPtrsType  uptr     = ssGetUPtrs(S);

    /* Loop over each input channel: */
    while(numCHANS-- > 0) {
      int_T frameIndex=lenFRAME;
      /* Loop over a frame of time inputs */
      while(frameIndex-- > 0) {
      real_T *tmpNUM = numPoly;
      real_T *tmpDEN = denPoly;
      real_T  in     = **(uptr++);  /* Get next channel input sample */
      real_T  out;
      int_T   i;

      tmpDL = tmpDL_top;  /* beginning of state vector for this channel */
      tmpDR = tmpDL;

      /* Compute filter output value: */
      out = in * *tmpNUM++;
      if (numDELAYS > 0) {
	    out += *tmpDR++;  /* No delays present if lenMIN=1 */
      }
      out /= *tmpDEN++;  /* Scale filter output accordingly     */
      *y++ = out;        /* Record filter output                */
      out = -out;        /* We can use addition uniformly below */


      if (lenNUM != lenDEN) {
	/*
	 * Unequal length coefficient vectors:
	 */
	i = lenMIN;
	while(--i != 0) {
	  *tmpDL++ = *tmpDR++ + in * *tmpNUM++ + out * *tmpDEN++;
	}
	if (lenNUM > lenDEN) {
	  i = lenNUM - lenMIN;   /* More numerator coeffs */
	  while(--i != 0) {
	    *tmpDL++ = *tmpDR++ + in * *tmpNUM++;
	  }
	  *tmpDL++ = in * *tmpNUM++;

	} else if (lenDEN > lenNUM) {
	  i = lenDEN - lenMIN;   /* More denominator coeffs */
	  while(--i != 0) {
	    *tmpDL++ = *tmpDR++ + out * *tmpDEN++;
	  }
	  *tmpDL++ = out * *tmpDEN++;
	}

      } else {
	/*
	 * Equal length coefficient vectors:
	 */
	if (lenNUM > 1) {   /* or lenDEN   */
	  i = lenNUM-1;     /* or lenDEN-1 */
	  while(--i != 0) {
	    *tmpDL++ = *tmpDR++ + in * *tmpNUM++ + out * *tmpDEN++;
	  }
	  *tmpDL++ = in * *tmpNUM++ + out * *tmpDEN++;
	}
      }
      /* At this point, tmpDL and tmpDR correctly point */
      /* to the start of the next delay buffer segment  */
      
    } /* time loop */
      /* Top of state vector for this channel */
      tmpDL_top += numDELAYS;  

    } /* channel loop */
}


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


/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *
 */
#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    /* Allocate delay buffer.
     *
     * The delay buffer has numCHANS "segments", each with numDELAYS
     * indices for each filter channel, for storing the delay values
     * for each IIR filter.
     */
    const int_T lenNUM    = mxGetNumberOfElements(NUM);
    const int_T lenDEN    = mxGetNumberOfElements(DEN);
    const int_T numCHANS  = (int_T)(mxGetPr(NUMCHANS_ARG)[0]);
    const int_T numDELAYS = MAX(lenNUM, lenDEN) - 1;
    const int_T numELE    = numCHANS * numDELAYS;

    ssSetNumRWork(S, numELE);
}
#endif


#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

/* [EOF] sdf2t.c */