/* $Revision: 1.15 $ */
/* SUPFIRDN
 *           DSP Blockset S-Function for the efficient polyphase
 *           implementation of upsampling (interpolating) by a factor of L,
 *           followed by FIR filtering, followed by downsamplinig (decimating)
 *           by a factor of M.
 *
 *           This is the buffered version.  A buffer is input for each channel,
 *           it is operated on, and an output buffer is computed.  All 
 *           calculations occur within one time step. 
 *
 *    Syntax: [sys, xo] = supfirdn(t,x,u,flag, H,L,M,N,n0,n1,sampleTime)
 *
 *         Vector inputs are allowed.
 *
 *         H is a vector of filter coefficients, distributed among the polyphase
 *         filter bank by M-file dsprprim.m.
 *
 *         L is the integer upsampling (interpolating) factor
 *
 *         M is the downsampling (decimating) factor
 *
 *         L and M must be relatively prime.  That is, their greatest common
 *         divisor must be 1.
 *
 *         N is the number of coefficients in each filter in the filter banks.
 *         Thus, length(H) = L*M*N.
 *
 *         n0 and n1 are positive integers such that -n0*L + n1*M = 1.  The 
 *         existance of n0 and n1 is guaranteed because L and M are relatively
 *         prime.
 *
 *         sampleTime is the input sample period.
 *
 *    References:
 *         N. J. Fliege, Multirate Digital Signal Processing, Wiley,
 *           Chichester, England, 1994, ISBN 0 471 93976 5, pp. 109-114
 *
 *         P. P. Vaidyanathan, Multirate Systems and Filter Banks,
 *           Prentice Hall, Englewood Cliffs, New Jersey, 1993, pp. 127-130
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 */
#define S_FUNCTION_NAME sdspupdn

#include "dsp_sim.h"
#include <stdio.h>  /* sprintf */

/*
 * Defines for easy access to the input parameters
 */
#define NUM_ARGS        7
#define FILTCOEFF_ARG   ssGetArg(S,0)
#define L_ARG           ssGetArg(S,1)
#define M_ARG           ssGetArg(S,2)
#define N_ARG           ssGetArg(S,3)
#define N0_ARG          ssGetArg(S,4)
#define N1_ARG          ssGetArg(S,5)
#define SAMPLETIME_ARG  ssGetArg(S,6)

/* 
 * IWork indices
 */
#define INBUFTOP_IDX     0
#define OUTBUFTOP_IDX    1
#define NUM_IWORK        2


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

  if ((mxGetM(SAMPLETIME_ARG) != 1) ||
      (mxGetN(SAMPLETIME_ARG) == 0) ||
      (mxGetN(SAMPLETIME_ARG) > 2)   ) {
    msg = "The sample time must be either a scalar or 2-element row vector";
    goto FCN_EXIT;
  }

  if ((mxGetM(L_ARG) != 1) || (mxGetN(L_ARG) != 1)) {
    msg = "The upsample factor must be a scalar value";
    goto FCN_EXIT;
  }
  L = (int_T)(mxGetPr(L_ARG)[0]);
  if ((L < 1)) {
    msg = "The upsample factor must be >= 1";
    goto FCN_EXIT;
  }

  if ((mxGetM(M_ARG) != 1) || (mxGetN(M_ARG) != 1)) {
    msg = "The downsample factor must be a scalar value";
    goto FCN_EXIT;
  }
  M = (int_T)(mxGetPr(M_ARG)[0]);
  if ((M < 1)) {
    msg = "The downsample factor must be >= 1";
    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)) return;
  mdlCheckParameters(S);
  if (ssGetErrorStatus(S) != NULL) return;
#endif

    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);  /* 1 == inputs read in mdlOutputs */
    ssSetNumSampleTimes(   S, 1);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumIWork(         S, NUM_IWORK);
    ssSetOptions(          S, SS_OPTION_USING_ssGetUPtrs |
                              SS_OPTION_EXCEPTION_FREE_CODE);
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
  int_T  L      = (int_T)(mxGetPr(L_ARG)[0]);
  real_T offset = (mxGetN(SAMPLETIME_ARG) > 1 ) ? mxGetPr(SAMPLETIME_ARG)[1] : 0.0;

  if (offset != 0.0) {
    ssSetErrorStatus(S, "Non-zero offset times not supported");
    return;
  }

  /* 
   * S-Function runs at single slow rate: 
   * M-buffer in, L-buffer out
   */
  ssSetSampleTimeEvent(S, 0, mxGetPr(SAMPLETIME_ARG)[0]);
  ssSetOffsetTimeEvent(S, 0, 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{

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

  ssSetIWorkValue(S, INBUFTOP_IDX,  0);
  ssSetIWorkValue(S, OUTBUFTOP_IDX, 0);
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
  UPtrsType uptr = ssGetUPtrs(S);
  
  const int_T L         = (int_T)(mxGetPr(L_ARG)[0]);
  const int_T M         = (int_T)(mxGetPr(M_ARG)[0]);
  const int_T N         = (int_T)(mxGetPr(N_ARG)[0]);
  const int_T n0        = (int_T)(mxGetPr(N0_ARG)[0]);
  const int_T n1        = (int_T)(mxGetPr(N1_ARG)[0]);
  const int_T num_chans = (int_T)(ssGetNumInputs(S)/M);

  const int_T ldinbuf   = n0*(L-1)+M;
  const int_T ldfiltbuf = (N-1)*M*L;
  const int_T ldoutbuf  = n1*(L-1);

  real_T *inbuf   = ssGetRWork(S);
  real_T *filtbuf = inbuf + ldinbuf*num_chans;
  real_T *bankbuf = filtbuf + ldfiltbuf*num_chans;
  real_T *outbuf  = bankbuf + L;
  int_T   inbuftop;
  int_T   outbuftop;
  int_T   chan;  /* channel counter */

/* In the following: 
 * i is the i'th coefficient in a filter (there are N coefficients per filter) 
 * j is the j'th filter in a filter bank (there are L filter banks) 
 * k is the k'th filter bank (there are M filters in each bank)
 */
  for (chan=0; chan < num_chans; chan++) { /* Loop over all channels */
    real_T *chanfiltbuf = filtbuf + chan*ldfiltbuf; /* set to top of channel column */
    real_T *chaninbuf = inbuf + chan*ldinbuf; /* set to top of channel column */
    real_T *H = mxGetPr(FILTCOEFF_ARG); /* reset to start of filter */
    int_T chanldinbuf = chan*ldinbuf;  /* Index to top of inbuf column for current channel */
    int_T incnt;

    inbuftop = ssGetIWorkValue(S, INBUFTOP_IDX);  /* reset for each channel */
    outbuftop = ssGetIWorkValue(S, OUTBUFTOP_IDX); /* reset for each channel */

    /* 
     * Read M values into the input buffer 
     */
    for (incnt=M; incnt-- != 0;) {
      inbuf[inbuftop + chanldinbuf] = **(uptr++);
      inbuftop++;
      inbuftop %= ldinbuf;
    }
         
    /*
     * Execute the filter banks
     */
    if (N > 1) { /* There is at least one delay element in the filters */
      int_T k; /* k is the k'th filter bank (there are M filters in each bank) */
      for (k=0; k<L; k++) { /* the kth filter bank */
        int_T j; /* j is the j'th filter in a filter bank (there are L filter banks) */
        bankbuf[k] = 0.0;  /* Clear the filter sum */
        for (j=0; j<M; j++) { /* the jth filter within a bank */
          int_T i; /* i is the i'th coefficient in a filter (there are N coefficients per filter) */
          real_T *chan1filtbuf = chanfiltbuf;
          real_T xn;
          xn = chaninbuf[(k*n0 - j + M + inbuftop - 1) % ldinbuf];
          bankbuf[k] += *chanfiltbuf + xn * (*H++);

          /* Update delay line */
          for (i=0; i<N-2; i++) {
            *chanfiltbuf++ = *(++chan1filtbuf) + xn * (*H++);
          }
          *chanfiltbuf++ = xn * (*H++);
        } 
      } 
     } else { /* The degenerate case: no delay elements in the filters */
       int_T k; /* k is the k'th filter bank (there are M filters in each bank) */
       for (k=0; k<L; k++) { /* the kth filter bank */
         int_T j; /* j is the j'th filter in a filter bank (there are L filter banks) */
         bankbuf[k] = 0.0;  /* Clear the filter sum */
         for (j=0; j<M; j++) {
           real_T xn;
           xn = chaninbuf[(k*n0 - j + M + inbuftop - 1) % ldinbuf];
           bankbuf[k] += xn * (*H++);
         }
       }
     } 
     
    /*
     * The first filter bank feeds the output y directly
     */
    *y++ = bankbuf[0];
    
    /*
     * The remaining filter banks feed into the output buffer
     * (if there is an output buffer)
     */
    if (ldoutbuf > 0) {
      int_T k; /* k is the k'th filter bank (there are M filters in each bank) */
      int_T xo = chan*ldoutbuf;
      int_T ki = n1 + outbuftop - 1;
      for (k=1; k<L; k++) { /* for each of the remaining filter banks after the top one */
        /* Insert filter bank output down the output buffer */
        outbuf[(ki % ldoutbuf) + xo] = bankbuf[k];
        ki += n1;
      }
      /*
       * The last L-1 outputs come from the output buffer
       */
      for (k=0; k<L-1; k++) {
        *y++ = outbuf[outbuftop + chan*ldoutbuf];
        ++outbuftop;
        outbuftop = outbuftop % ldoutbuf;
      }
      /* The statement 
       *   outbuftop = ++outbuftop % ldoutbuf;
       * is not deterministic in ANSI C and the VMS compiler catches it.
       * The ++ and % need to be in two lines.
       */ 
      ++outbuftop;
      outbuftop = outbuftop % ldoutbuf;
    }
            
  } /* Loop over all channels */
  
  /*
   * Set Circular Buffer indexes 
   */
  ssSetIWorkValue(S, INBUFTOP_IDX, inbuftop);
  ssSetIWorkValue(S, OUTBUFTOP_IDX, outbuftop);
}


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)
{
    int_T L = (int_T)mxGetPr(L_ARG)[0];
    int_T M = (int_T)mxGetPr(M_ARG)[0];
    return(outputPortWidth*M/L);
}
#endif


#ifdef MATLAB_MEX_FILE
#define MDL_GET_OUTPUT_PORT_WIDTH
/* Function: mdlGetOutputPortWidth ============================================
 *
 */
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    int_T L = (int_T)mxGetPr(L_ARG)[0];
    int_T M = (int_T)mxGetPr(M_ARG)[0];
    return(inputPortWidth*L/M);
}
#endif



/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *
 */
#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    int_T L         = (int_T)(mxGetPr(L_ARG)[0]);
    int_T M         = (int_T)(mxGetPr(M_ARG)[0]);
    int_T n0        = (int_T)(mxGetPr(N0_ARG)[0]);
    int_T n1        = (int_T)(mxGetPr(N1_ARG)[0]);
    int_T N         = (int_T)(mxGetPr(N_ARG)[0]);
    real_T numinputs = (real_T)(ssGetNumInputs(S));
    int_T num_chans = (int_T)(numinputs/M);

    /*
     * Leading dimension (ld) notation is from LINPACK.
     * It specifies the number of rows in a matrix.  In memory,
     * an m-by-n matrix X is stored as a vector X(:).  Then ldX = m.
     */
    int_T ldinbuf   = n0*(L-1)+M;
    int_T ldfiltbuf = (N-1)*M*L;
    int_T ldoutbuf  = n1*(L-1);
    
    /* Verify that the number of inputs is divisible by M */
    if ( numinputs/M  != num_chans ) { 
          char msg[256];
          sprintf(msg,"The input buffer size (%d) must be divisible by \nthe decimation factor (%d).",
                       (int_T)numinputs,M);
          ssSetErrorStatus(S,msg);
          return;
      }

    /* Bankbuf is a vector of length L that gets overwritten by each channel */
    
    ssSetNumRWork(S, L + num_chans * (ldinbuf+ldfiltbuf+ldoutbuf));
}
#endif


#include "dsp_trailer.c"
 
/* [EOF]: sdspupdn.c */