/*
 *  SDF2T: SIMULINK multichannel IIR (Direct Form II Transposed) filter block.
 *  DSP Blockset S-Function for multichannel IIR filtering.
 *
 *    Syntax:  [sys, x0] = sdf2(t,x,u,flag,NUM,DEN,IC,sampleTime)
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.19 $  $Date: 2000/06/14 14:27:57 $
 */

#define S_FUNCTION_NAME sdf2t

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

#include "simstruc.h"

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

#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 ERROR_EXIT;
    }

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

    if (*mxGetPr(DEN) == 0.0) {
        msg = "First denominator coefficient must be nonzero";
        goto ERROR_EXIT;
    }

ERROR_EXIT:
    if (msg != NULL) {
        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
    
    ssSetNumContStates(    S, 0);
    ssSetNumDiscStates(    S, 0);
    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumIWork(         S, 0);
    ssSetNumPWork(         S, 0);
    ssSetNumModes(         S, 0);
    ssSetNumNonsampledZCs( S, 0);
    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  = ssGetNumInputs(S);
    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 (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((void *)DlyBuf, (void *)pIC, numDELAYS*sizeof(real_T));
            DlyBuf += numDELAYS;
        }

    } else {
        /*
         * Matrix of IC's:
         * Assume numDELAYS rows and numCHANS columns
         */
        memcpy((void *)DlyBuf, (void *)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  = ssGetNumInputs(S);

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

    /* Loop over each input channel: */
    while(numCHANS-- != 0) {
      real_T *tmpNUM = numPoly;
      real_T *tmpDEN = denPoly;
      real_T  in     = **(uptr++);  /* Get next channel input sample */
      real_T  out;
      int_T   i;

      /* 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  */

    } /* 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  = ssGetNumInputs(S);
    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 */