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