/*
 ** Copyright 2003-2010, VisualOn, Inc.
 **
 ** Licensed under the Apache License, Version 2.0 (the "License");
 ** you may not use this file except in compliance with the License.
 ** You may obtain a copy of the License at
 **
 **     http://www.apache.org/licenses/LICENSE-2.0
 **
 ** Unless required by applicable law or agreed to in writing, software
 ** distributed under the License is distributed on an "AS IS" BASIS,
 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 ** See the License for the specific language governing permissions and
 ** limitations under the License.
 */

/***********************************************************************
*       File: hp_wsp.c                                                  *
*       Description:                                                    *
*       3nd order high pass filter with cut off frequency at 180Hz      *
* Algorithm:                                                            *
*                                                                       *
*  y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] + b[3]*x[i-3]           *
*                   + a[1]*y[i-1] + a[2]*y[i-2] + a[3]*y[i-3];          *
*                                                                       *
* float a_coef[HP_ORDER]= {                                             *
*    -2.64436711600664f,                                                *
*    2.35087386625360f,                                                 *
*   -0.70001156927424f};                                                *
*                                                                       *
* float b_coef[HP_ORDER+1]= {                                           *
*     -0.83787057505665f,                                               *
*    2.50975570071058f,                                                 *
*   -2.50975570071058f,                                                 *
*    0.83787057505665f};                                                *
*                                                                       *
*************************************************************************/

#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "acelp.h"

/* filter coefficients in Q12 */
static Word16 a[4] = {8192, 21663, -19258, 5734};
static Word16 b[4] = {-3432, +10280, -10280, +3432};

/* Initialization of static values */
void Init_Hp_wsp(Word16 mem[])
{
    Set_zero(mem, 9);

    return;
}

void scale_mem_Hp_wsp(Word16 mem[], Word16 exp)
{
    Word32 i;
    Word32 L_tmp;

    for (i = 0; i < 6; i += 2)
    {
        L_tmp = ((mem[i] << 16) + (mem[i + 1]<<1));
        L_tmp = L_shl(L_tmp, exp);
        mem[i] = L_tmp >> 16;
        mem[i + 1] = (L_tmp & 0xffff)>>1;
    }

    for (i = 6; i < 9; i++)
    {
        L_tmp = L_deposit_h(mem[i]);       /* x[i] */
        L_tmp = L_shl(L_tmp, exp);
        mem[i] = vo_round(L_tmp);
    }

    return;
}


void Hp_wsp(
        Word16 wsp[],                         /* i   : wsp[]  signal       */
        Word16 hp_wsp[],                      /* o   : hypass wsp[]        */
        Word16 lg,                            /* i   : lenght of signal    */
        Word16 mem[]                          /* i/o : filter memory [9]   */
       )
{
    Word16 x0, x1, x2, x3;
    Word16 y3_hi, y3_lo, y2_hi, y2_lo, y1_hi, y1_lo;
    Word32 i, L_tmp;

    y3_hi = mem[0];
    y3_lo = mem[1];
    y2_hi = mem[2];
    y2_lo = mem[3];
    y1_hi = mem[4];
    y1_lo = mem[5];
    x0 = mem[6];
    x1 = mem[7];
    x2 = mem[8];

    for (i = 0; i < lg; i++)
    {
        x3 = x2;
        x2 = x1;
        x1 = x0;
        x0 = wsp[i];
        /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b140[2]*x[i-2] + b[3]*x[i-3]  */
        /* + a[1]*y[i-1] + a[2] * y[i-2]  + a[3]*y[i-3]  */

        L_tmp = 16384L;                    /* rounding to maximise precision */
        L_tmp += (y1_lo * a[1])<<1;
        L_tmp += (y2_lo * a[2])<<1;
        L_tmp += (y3_lo * a[3])<<1;
        L_tmp = L_tmp >> 15;
        L_tmp += (y1_hi * a[1])<<1;
        L_tmp += (y2_hi * a[2])<<1;
        L_tmp += (y3_hi * a[3])<<1;
        L_tmp += (x0 * b[0])<<1;
        L_tmp += (x1 * b[1])<<1;
        L_tmp += (x2 * b[2])<<1;
        L_tmp += (x3 * b[3])<<1;

        L_tmp = L_tmp << 2;

        y3_hi = y2_hi;
        y3_lo = y2_lo;
        y2_hi = y1_hi;
        y2_lo = y1_lo;
        y1_hi = L_tmp >> 16;
        y1_lo = (L_tmp & 0xffff) >>1;

        hp_wsp[i] = (L_tmp + 0x4000)>>15;
    }

    mem[0] = y3_hi;
    mem[1] = y3_lo;
    mem[2] = y2_hi;
    mem[3] = y2_lo;
    mem[4] = y1_hi;
    mem[5] = y1_lo;
    mem[6] = x0;
    mem[7] = x1;
    mem[8] = x2;

    return;
}