/* $Revision: 1.10 $ */
/*
 *  SDSPIIRV: SIMULINK multichannel IIR (Direct Form II Transposed) filter block
 *  with variable numerator and denominator coefficients.  The number of coefficients
 *  must be fixed, but their values may vary.
 *  DSP Blockset S-Function for multichannel IIR filtering.
 *
 *    Syntax:  [sys, x0] = sdspiirv(t,x,u,flag,NNUM,NDEN,IC,sampleTime)
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.10 $  $Date: 2000/06/14 14:27:58 $
 *
 */

#define S_FUNCTION_NAME sdspiirv

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

#include "dsp_sim.h"

#ifdef MATLAB_MEX_FILE
#include "mex.h"
#endif

#define NUM_ARGS    3
#define LEN_NUM_ARG    ssGetArg(S,0)
#define LEN_DEN_ARG    ssGetArg(S,1)
#define IC_ARG         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;
    int_T temp;
    const char *msg = NULL;
    real_T *pi;

    /* Check size of LEN_NUM_ARG and LEN_DEN_ARG: */
    M = mxGetM(LEN_NUM_ARG);
    N = mxGetN(LEN_NUM_ARG);
    if ( (N != 1) || (M != 1) ) {
        msg = "Numerator length must be a scalar";
        goto FCN_EXIT;
    }

    M = mxGetM(LEN_DEN_ARG);
    N = mxGetN(LEN_DEN_ARG);
    if ( (N != 1) || (M != 1) ) {
        msg = "Denominator length must be a scalar";
        goto FCN_EXIT;
    }

    /* Check that inputs are real: */
    if (mxIsComplex(LEN_NUM_ARG) || 
        mxIsComplex(LEN_DEN_ARG) ||
        mxIsComplex(IC_ARG)) {
        msg = "Parameters must be real valued";
        goto FCN_EXIT;
    }
    
    /* Check numerator and denominator lengths are valid values */
    temp = (int_T)(mxGetPr(LEN_NUM_ARG)[0]);
    if (temp <= 0) {
        msg = "Numerator length must be positive";
        goto FCN_EXIT;
    }
    
    temp = (int_T)(mxGetPr(LEN_DEN_ARG)[0]);
    if (temp <= 0) {
        msg = "Denominator length must be positive";
        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);
}


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_ARG);   /* Initial Conditions vector */
    real_T       *DlyBuf    = ssGetRWork(S);     /* Delay buffer storage      */
    const int_T   lenNUM    = (int_T)(*mxGetPr(LEN_NUM_ARG));
    const int_T   lenDEN    = (int_T)(*mxGetPr(LEN_DEN_ARG));
    const int_T   numIC     = mxGetNumberOfElements(IC_ARG);
    const int_T   numCHANS  = ssGetNumInputs(S) - lenNUM - lenDEN;
    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)) {
        THROW_ERROR(S, "Initial condition vector has incorrect dimensions");
    }

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

#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    = (int_T)(*mxGetPr(LEN_NUM_ARG));
    const int_T    lenDEN    = (int_T)(*mxGetPr(LEN_DEN_ARG));
    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) - lenNUM - lenDEN;

    real_T          *tmpDL    = ssGetRWork(S); /* Start of buffer */
    real_T          *tmpDR    = tmpDL;
    const real_T    *numPoly  = u;
    const real_T    *denPoly  = numPoly + lenNUM;
    const real_T    *inPtr    = denPoly + lenDEN;

    /* Loop over each input channel: */
    while(numCHANS-- > 0) {
      const real_T *tmpNUM = numPoly;
      const real_T *tmpDEN = denPoly;
      real_T  in     = *inPtr++;  /* 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 */
      }
      if ( *tmpDEN == 0.0 ) {
 #ifdef MATLAB_MEX_FILE
        mexWarnMsgTxt("Leading denominator coefficient is 0.  Setting output to 0.");
 #endif
        out = 0.0;
        tmpDEN++;
      } else {
        out /= *tmpDEN++;  /* Scale filter output by leading denominator coefficient */
      }
      *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)
{
}

#ifdef MATLAB_MEX_FILE
#define MDL_GET_INPUT_PORT_WIDTH
/* Function: mdlGetInputPortWidth =============================================
 *
 */
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
{
    const int_T    lenNUM    = (int_T)(*mxGetPr(LEN_NUM_ARG));
    const int_T    lenDEN    = (int_T)(*mxGetPr(LEN_DEN_ARG));
    return(outputPortWidth + lenNUM + lenDEN);
}
#endif


#ifdef MATLAB_MEX_FILE
#define MDL_GET_OUTPUT_PORT_WIDTH
/* Function: mdlGetOutputPortWidth ============================================
 *
 */
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    const int_T    lenNUM    = (int_T)(*mxGetPr(LEN_NUM_ARG));
    const int_T    lenDEN    = (int_T)(*mxGetPr(LEN_DEN_ARG));
    return(inputPortWidth - lenNUM - lenDEN);
}
#endif


/* 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    = (int_T)(*mxGetPr(LEN_NUM_ARG));
    const int_T lenDEN    = (int_T)(*mxGetPr(LEN_DEN_ARG));
    const int_T numCHANS  = ssGetNumInputs(S) - lenNUM - lenDEN;
    const int_T numDELAYS = MAX(lenNUM, lenDEN) - 1;
    const int_T numELE    = numCHANS * numDELAYS;

    ssSetNumRWork(S, numELE);
}
#endif


#include "dsp_trailer.c"
 
/* [EOF] sdspiirv.c */