/*====================================================================================
    EVS Codec 3GPP TS26.443 Jun 30, 2015. Version CR 26.443-0006
  ====================================================================================*/

#include <assert.h>
#include "typedef.h"
#include "options.h"
#include "prot.h"




/*
 * hp20
 *
 * Function:
 *    2nd order high pass filter with nominal cut off frequency at 20 Hz.
 *
 * Returns:
 *    void
 */

void hp20_input(Float32 signal[], Word32 lg, Float32 mem[], Word32 fs)
{
    Word16 i;
    Float32 x0, x1, x2, y0, y1, y2;
    Float32 a1, a2, b1, b2;



    y1 = mem[0];
    y2 = mem[1];
    x0 = mem[2];
    x1 = mem[3];

    if (fs == 8000)
    {
        /* hp filter 20Hz at 3dB for 8000KHz input sampling rate
           [b,a] = butter(2, 20.0/4000.0, 'high');
           b = [0.988954248067140  -1.977908496134280   0.988954248067140]
           a =[1.000000000000000  -1.977786483776764   0.978030508491796]*/
        a1 = 1.977786483776764f;
        a2 = -0.978030508491796f;
        b1 = -1.977908496134280f;
        b2 = 0.988954248067140f;

    }
    else if (fs==16000)
    {
        /* hp filter 20Hz at 3dB for 16000KHz sampling rate
           [b,a] = butter(2, 20.0/8000.0, 'high');
           b =[ 0.994461788958195  -1.988923577916390   0.994461788958195]
           a =[1.000000000000000  -1.988892905899653   0.988954249933127] */
        a1 = 1.988892905899653f;
        a2 = -0.988954249933127f;
        b1 = -1.988923577916390f;
        b2 = 0.994461788958195f;

    }
    else if (fs==24000)
    {
        /* hp filter 20Hz at 3dB for 24000KHz sampling rate
           [b,a] = butter(2, 20.0/12000.0, 'high');
           b= [0.996304442969349  -1.992608885938698   0.996304442969349]
           a=[1.000000000000000  -1.992595228750302   0.992622543127095]*/
        a1= 1.992595228750302f;
        a2= -0.992622543127095f;
        b1= -1.992608885938698f;
        b2= 0.996304442969349f;

    }
    else if (fs==32000)
    {
        /* hp filter 20Hz at 3dB for 32000KHz sampling rate
           [b,a] = butter(2, 20.0/16000.0, 'high');
           b =[0.997227049904470  -1.994454099808940   0.997227049904470]
           a =[1.000000000000000  -1.994446410541927   0.994461789075954]*/
        a1 = 1.994446410541927f;
        a2 = -0.994461789075954f;
        b1 = -1.994454099808940f;
        b2 = 0.997227049904470f;


    }
    else
    {
        /* hp filter 20Hz at 3dB for 48000KHz sampling rate
           [b,a] = butter(2, 20.0/24000.0, 'high');
           b =[ 0.998150511190452  -1.996301022380904   0.998150511190452]
           a =[1.000000000000000  -1.996297601769122   0.996304442992686]*/
        a1 = 1.996297601769122f;
        a2 = -0.996304442992686f;
        b1 = -1.996301022380904f;
        b2 = 0.998150511190452f;

    }

    for(i = 0; i < lg; i++)
    {
        x2 = x1;
        x1 = x0;
        x0 = signal[i];
        y0 = (y1*a1) + (y2*a2) + (x0*b2) + (x1*b1) + (x2*b2);
        signal[i] = y0;
        y2 = y1;
        y1 = y0;
    }

    mem[0] = ((y1 > 1e-10) | (y1 < -1e-10)) ? y1 : 0;
    mem[1] = ((y2 > 1e-10) | (y2 < -1e-10)) ? y2 : 0;
    mem[2] = ((x0 > 1e-10) | (x0 < -1e-10)) ? x0 : 0;
    mem[3] = ((x1 > 1e-10) | (x1 < -1e-10)) ? x1 : 0;


    return;
}