/*************************************************************************
 *
 *  FUNCTION:  Pre_Process()
 *
 *  PURPOSE: Preprocessing of input speech.
 *
 *  DESCRIPTION:
 *     - 2nd order high pass filtering with cut off frequency at 80 Hz.
 *     - Divide input by two.
 *
 *************************************************************************/

#include "gsm_efr.h"
#include "typedef.h"
#include "namespace.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "no_count.h"
#include "sig_proc.h"
#include "cnst.h"
#include "enc_state.h"

/*------------------------------------------------------------------------*
 *                                                                        *
 * Algorithm:                                                             *
 *                                                                        *
 *  y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b[2]*x[i-2]/2                    *
 *                     + a[1]*y[i-1]   + a[2]*y[i-2];                     *
 *                                                                        *
 *                                                                        *
 *  Input is divided by two in the filtering process.                     *
 *------------------------------------------------------------------------*/

/* filter coefficients (fc = 80 Hz, coeff. b[] is divided by 2) */

static const Word16 b[3] = {1899, -3798, 1899};
static const Word16 a[3] = {4096, 7807, -3733};

/* Initialization of static values */

void Init_Pre_Process (struct EFR_encoder_state *st)
{
    struct preproc_state *pps = &st->pre_proc;

    pps->y2_hi = 0;
    pps->y2_lo = 0;
    pps->y1_hi = 0;
    pps->y1_lo = 0;
    pps->x0 = 0;
    pps->x1 = 0;
}

void Pre_Process (
    struct EFR_encoder_state *st,
    Word16 signal[], /* input/output signal */
    Word16 lg)       /* lenght of signal    */
{
    struct preproc_state *pps = &st->pre_proc;
    Word16 i, x2;
    Word32 L_tmp;

    for (i = 0; i < lg; i++)
    {
        x2 = pps->x1;
        pps->x1 = pps->x0;
        pps->x0 = signal[i];

        /*  y[i] = b[0]*x[i]/2 + b[1]*x[i-1]/2 + b140[2]*x[i-2]/2  */
        /*                     + a[1]*y[i-1] + a[2] * y[i-2];      */

        L_tmp = Mpy_32_16 (pps->y1_hi, pps->y1_lo, a[1]);
        L_tmp = L_add (L_tmp, Mpy_32_16 (pps->y2_hi, pps->y2_lo, a[2]));
        L_tmp = L_mac (L_tmp, pps->x0, b[0]);
        L_tmp = L_mac (L_tmp, pps->x1, b[1]);
        L_tmp = L_mac (L_tmp, x2, b[2]);
        L_tmp = L_shl (L_tmp, 3);
        signal[i] = round (L_tmp);

        pps->y2_hi = pps->y1_hi;
        pps->y2_lo = pps->y1_lo;
        L_Extract (L_tmp, &pps->y1_hi, &pps->y1_lo);
    }
    return;
}