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

    iLBC Speech Coder ANSI-C Source Code

    filter.c 

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

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



#include "iLBC_define.h"
#include "filter.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;
    }
}