/******************************************************************

       iLBC Speech Coder ANSI-C Source Code

       filter.c

       Copyright (C) The Internet Society (2004).
       All Rights Reserved.

   ******************************************************************/

#include "iLBC_define.h"

   /*----------------------------------------------------------------*
    *  all-pole filter
    *---------------------------------------------------------------*/

void AllPoleFilter(float *InOut,        /* (i/o) on entrance InOut[-orderCoef] to
                                           InOut[-1] contain the state of the
                                           filter (delayed samples). InOut[0] to
                                           InOut[lengthInOut-1] contain the filter
                                           input, on en exit InOut[-orderCoef] to
                                           InOut[-1] is unchanged and InOut[0] to
                                           InOut[lengthInOut-1] contain filtered
                                           samples */
  float *Coef,                  /* (i) filter coefficients, Coef[0] is assumed
                                   to be 1.0 */
  int lengthInOut,              /* (i) number of input/output samples */
  int orderCoef                 /* (i) number of filter coefficients */
  )
{
  int n, k;

  for (n = 0; n < lengthInOut; n++) {
    for (k = 1; k <= orderCoef; k++) {
      *InOut -= Coef[k] * InOut[-k];





    }
    InOut++;
  }
}

   /*----------------------------------------------------------------*
    *  all-zero filter
    *---------------------------------------------------------------*/

void AllZeroFilter(float *In,   /* (i) In[0] to In[lengthInOut-1] contain
                                   filter input samples */
  float *Coef,                  /* (i) filter coefficients (Coef[0] is assumed
                                   to be 1.0) */
  int lengthInOut,              /* (i) number of input/output samples */
  int orderCoef,                /* (i) number of filter coefficients */
  float *Out                    /* (i/o) on entrance Out[-orderCoef] to Out[-1]
                                   contain the filter state, on exit Out[0]
                                   to Out[lengthInOut-1] contain filtered
                                   samples */
  )
{
  int n, k;

  for (n = 0; n < lengthInOut; n++) {
    *Out = Coef[0] * In[0];
    for (k = 1; k <= orderCoef; k++) {
      *Out += Coef[k] * In[-k];
    }
    Out++;
    In++;
  }
}

   /*----------------------------------------------------------------*
    *  pole-zero filter
    *---------------------------------------------------------------*/

void ZeroPoleFilter(float *In,  /* (i) In[0] to In[lengthInOut-1] contain
                                   filter input samples In[-orderCoef] to
                                   In[-1] contain state of all-zero
                                   section */
  float *ZeroCoef,              /* (i) filter coefficients for all-zero
                                   section (ZeroCoef[0] is assumed to
                                   be 1.0) */
  float *PoleCoef,              /* (i) filter coefficients for all-pole section
                                   (ZeroCoef[0] is assumed to be 1.0) */
  int lengthInOut,              /* (i) number of input/output samples */
  int orderCoef,                /* (i) number of filter coefficients */
  float *Out                    /* (i/o) on entrance Out[-orderCoef] to Out[-1]
                                   contain state of all-pole section. On
                                   exit Out[0] to Out[lengthInOut-1]
                                   contain filtered samples */
  )
{
  AllZeroFilter(In, ZeroCoef, lengthInOut, orderCoef, Out);
  AllPoleFilter(Out, PoleCoef, lengthInOut, orderCoef);
}

   /*----------------------------------------------------------------*
    * downsample (LP filter and decimation)
    *---------------------------------------------------------------*/

void DownSample(float *In,      /* (i) input samples */
  float *Coef,                  /* (i) filter coefficients */
  int lengthIn,                 /* (i) number of input samples */
  float *state,                 /* (i) filter state */
  float *Out                    /* (o) downsampled output */
  )
{
  float o;
  float *Out_ptr = Out;
  float *Coef_ptr, *In_ptr;
  float *state_ptr;
  int i, j, stop;

  /* LP filter and decimate at the same time */

  for (i = DELAY_DS; i < lengthIn; i += FACTOR_DS) {
    Coef_ptr = &Coef[0];
    In_ptr = &In[i];
    state_ptr = &state[FILTERORDER_DS - 2];

    o = (float) 0.0;

    stop = (i < FILTERORDER_DS) ? i + 1 : FILTERORDER_DS;

    for (j = 0; j < stop; j++) {
      o += *Coef_ptr++ * (*In_ptr--);
    }
    for (j = i + 1; j < FILTERORDER_DS; j++) {
      o += *Coef_ptr++ * (*state_ptr--);
    }






    *Out_ptr++ = o;
  }

  /* Get the last part (use zeros as input for the future) */

  for (i = (lengthIn + FACTOR_DS); i < (lengthIn + DELAY_DS);
    i += FACTOR_DS) {

    o = (float) 0.0;

    if (i < lengthIn) {
      Coef_ptr = &Coef[0];
      In_ptr = &In[i];
      for (j = 0; j < FILTERORDER_DS; j++) {
        o += *Coef_ptr++ * (*Out_ptr--);
      }
    } else {
      Coef_ptr = &Coef[i - lengthIn];
      In_ptr = &In[lengthIn - 1];
      for (j = 0; j < FILTERORDER_DS - (i - lengthIn); j++) {
        o += *Coef_ptr++ * (*In_ptr--);
      }
    }
    *Out_ptr++ = o;
  }
}