/*
 * Copyright (C) 2004-2010 NXP Software
 * Copyright (C) 2010 The Android Open Source Project
 *
 * 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.
 */

/****************************************************************************************/
/*                                                                                      */
/*    Includes                                                                          */
/*                                                                                      */
/****************************************************************************************/

#include "LVEQNB_Private.h"


/****************************************************************************************/
/*                                                                                      */
/*    Defines                                                                           */
/*                                                                                      */
/****************************************************************************************/

#define PI 3.14159265358979

/****************************************************************************************/
/*                                                                                      */
/* FUNCTION:                  LVEQNB_DoublePrecCoefs                                    */
/*                                                                                      */
/* DESCRIPTION:                                                                         */
/*    Calculate double precision coefficients    for a peaking filter                   */
/*                                                                                      */
/* PARAMETERS:                                                                          */
/*  Fs                           Sampling frequency index                               */
/*  pFilterDefinition          Pointer to the filter definition                         */
/*  pCoefficients            Pointer to the coefficients                                */
/*                                                                                      */
/* RETURNS:                                                                             */
/*  LVEQNB_SUCCESS            Always succeeds                                           */
/*                                                                                      */
/* NOTES:                                                                               */
/*  1. The equations used are as follows:                                               */
/*                                                                                      */
/*      G  = 10^(GaindB/20) - 1                                                         */
/*      t0 = 2 * Pi * Fc / Fs                                                           */
/*      D  = 1                  if GaindB >= 0                                          */
/*      D  = 1 / (1 + G)        if GaindB <  0                                          */
/*                                                                                      */
/*      b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0)                                       */
/*      b1 = (0.5 - b2) * (1 - coserr(t0))                                              */
/*      a0 = (0.5 + b2) / 2                                                             */
/*                                                                                      */
/*  Where:                                                                              */
/*      GaindB      is the gain in dBs, range -15dB to +15dB                            */
/*      Fc          is the centre frequency, DC to Fs/50                                */
/*      Fs          is the sample frequency, 8000 to 48000 in descrete steps            */
/*      Q           is the Q factor, 0.25 to 12 (represented by 25 to 1200)             */
/*                                                                                      */
/*  2. The double precision coefficients are only used when fc is less than fs/85, so   */
/*     the cosine of t0 is always close to 1.0. Instead of calculating the cosine       */
/*     itself the difference from the value 1.0 is calculated, this can be done with    */
/*     lower precision maths.                                                           */
/*                                                                                      */
/*  3. The value of the B2 coefficient is only calculated as a single precision value,  */
/*     small errors in this value have a combined effect on the Q and Gain but not the  */
/*     the frequency of the filter.                                                     */
/*                                                                                      */
/****************************************************************************************/


LVEQNB_ReturnStatus_en LVEQNB_DoublePrecCoefs(LVM_UINT16        Fs,
                                              LVEQNB_BandDef_t  *pFilterDefinition,
                                              PK_C32_Coefs_t    *pCoefficients)
{

    extern LVM_INT16    LVEQNB_GainTable[];
    extern LVM_INT16    LVEQNB_TwoPiOnFsTable[];
    extern LVM_INT16    LVEQNB_DTable[];
    extern LVM_INT16    LVEQNB_DPCosCoef[];

    /*
     * Get the filter definition
     */
    LVM_INT16           Gain        = pFilterDefinition->Gain;
    LVM_UINT16          Frequency   = pFilterDefinition->Frequency;
    LVM_UINT16          QFactor     = pFilterDefinition->QFactor;

    /*
     * Intermediate variables and temporary values
     */
    LVM_INT32           T0;
    LVM_INT16           D;
    LVM_INT32           A0;
    LVM_INT32           B1;
    LVM_INT32           B2;
    LVM_INT32           Dt0;
    LVM_INT32           B2_Den;
    LVM_INT32           B2_Num;
    LVM_INT32           CosErr;
    LVM_INT16           coef;
    LVM_INT32           factor;
    LVM_INT16           t0;
    LVM_INT16           i;

    /*
     * Calculating the intermediate values
     */
    T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs];        /* T0 = 2 * Pi * Fc / Fs */
    if (Gain >= 0)
    {
        D = LVEQNB_DTable[15];                         /* D = 1            if GaindB >= 0 */
    }
    else
    {
        D = LVEQNB_DTable[Gain+15];                    /* D = 1 / (1 + G)  if GaindB <  0 */
    }

    /*
     * Calculate the B2 coefficient
     */
    Dt0 = D * (T0 >> 10);
    B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2);
    B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18);
    B2 = (B2_Num / (B2_Den >> 16)) << 15;

    /*
     * Calculate the cosine error by a polynomial expansion using the equation:
     *
     *  CosErr += coef(n) * t0^n                For n = 0 to 4
     */
    T0 = (T0 >> 6) * 0x7f53;                    /* Scale to 1.0 in 16-bit for range 0 to fs/50 */
    t0 = (LVM_INT16)(T0 >> 16);
    factor = 0x7fff;                            /* Initialise to 1.0 for the a0 coefficient */
    CosErr = 0;                                 /* Initialise the error to zero */
    for (i=1; i<5; i++)
    {
        coef = LVEQNB_DPCosCoef[i];             /* Get the nth coefficient */
        CosErr += (factor * coef) >> 5;         /* The nth partial sum */
        factor = (factor * t0) >> 15;           /* Calculate t0^n */
    }
    CosErr = CosErr << (LVEQNB_DPCosCoef[0]);   /* Correct the scaling */

    /*
     * Calculate the B1 and A0 coefficients
     */
    B1 = (0x40000000 - B2);                     /* B1 = (0.5 - b2/2) */
    A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6;    /* Temporary storage for (0.5 - b2/2) * coserr(t0) */
    B1 -= A0;                                   /* B1 = (0.5 - b2/2) * (1 - coserr(t0))  */
    A0 = (0x40000000 + B2) >> 1;                /* A0 = (0.5 + b2) */

    /*
     * Write coeff into the data structure
     */
    pCoefficients->A0 = A0;
    pCoefficients->B1 = B1;
    pCoefficients->B2 = B2;
    pCoefficients->G  = LVEQNB_GainTable[Gain+15];

    return(LVEQNB_SUCCESS);

}


/****************************************************************************************/
/*                                                                                      */
/* FUNCTION:                  LVEQNB_SinglePrecCoefs                                    */
/*                                                                                      */
/* DESCRIPTION:                                                                         */
/*    Calculate single precision coefficients    for a peaking filter                   */
/*                                                                                      */
/* PARAMETERS:                                                                          */
/*  Fs                           Sampling frequency index                               */
/*  pFilterDefinition          Pointer to the filter definition                         */
/*  pCoefficients            Pointer to the coefficients                                */
/*                                                                                      */
/* RETURNS:                                                                             */
/*  LVEQNB_SUCCESS            Always succeeds                                           */
/*                                                                                      */
/* NOTES:                                                                               */
/*  1. The equations used are as follows:                                               */
/*                                                                                      */
/*      G  = 10^(GaindB/20) - 1                                                         */
/*      t0 = 2 * Pi * Fc / Fs                                                           */
/*      D  = 1                  if GaindB >= 0                                          */
/*      D  = 1 / (1 + G)        if GaindB <  0                                          */
/*                                                                                      */
/*      b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0)                                       */
/*      b1 = (0.5 - b2) * cos(t0)                                                       */
/*      a0 = (0.5 + b2) / 2                                                             */
/*                                                                                      */
/*  Where:                                                                              */
/*      GaindB      is the gain in dBs, range -15dB to +15dB                            */
/*      Fc          is the centre frequency, DC to Nyquist                              */
/*      Fs          is the sample frequency, 8000 to 48000 in descrete steps            */
/*      Q           is the Q factor, 0.25 to 12                                         */
/*                                                                                      */
/****************************************************************************************/


LVEQNB_ReturnStatus_en LVEQNB_SinglePrecCoefs(LVM_UINT16        Fs,
                                              LVEQNB_BandDef_t  *pFilterDefinition,
                                              PK_C16_Coefs_t    *pCoefficients)
{

    extern LVM_INT16    LVEQNB_GainTable[];
    extern LVM_INT16    LVEQNB_TwoPiOnFsTable[];
    extern LVM_INT16    LVEQNB_DTable[];
    extern LVM_INT16    LVEQNB_CosCoef[];


    /*
     * Get the filter definition
     */
    LVM_INT16           Gain        = pFilterDefinition->Gain;
    LVM_UINT16          Frequency   = pFilterDefinition->Frequency;
    LVM_UINT16          QFactor     = pFilterDefinition->QFactor;


    /*
     * Intermediate variables and temporary values
     */
    LVM_INT32           T0;
    LVM_INT16           D;
    LVM_INT32           A0;
    LVM_INT32           B1;
    LVM_INT32           B2;
    LVM_INT32           Dt0;
    LVM_INT32           B2_Den;
    LVM_INT32           B2_Num;
    LVM_INT32           COS_T0;
    LVM_INT16           coef;
    LVM_INT32           factor;
    LVM_INT16           t0;
    LVM_INT16           i;

    /*
     * Calculating the intermediate values
     */
    T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs];        /* T0 = 2 * Pi * Fc / Fs */
    if (Gain >= 0)
    {
        D = LVEQNB_DTable[15];                         /* D = 1            if GaindB >= 0 */
    }
    else
    {
        D = LVEQNB_DTable[Gain+15];                    /* D = 1 / (1 + G)  if GaindB <  0 */
    }

    /*
     * Calculate the B2 coefficient
     */
    Dt0 = D * (T0 >> 10);
    B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2);
    B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18);
    B2 = (B2_Num / (B2_Den >> 16)) << 15;

    /*
     * Calculate the cosine by a polynomial expansion using the equation:
     *
     *  Cos += coef(n) * t0^n                   For n = 0 to 6
     */
    T0 = (T0 >> 10) * 20859;                    /* Scale to 1.0 in 16-bit for range 0 to fs/2 */
    t0 = (LVM_INT16)(T0 >> 16);
    factor = 0x7fff;                            /* Initialise to 1.0 for the a0 coefficient */
    COS_T0 = 0;                                 /* Initialise the error to zero */
    for (i=1; i<7; i++)
    {
        coef = LVEQNB_CosCoef[i];               /* Get the nth coefficient */
        COS_T0 += (factor * coef) >> 5;         /* The nth partial sum */
        factor = (factor * t0) >> 15;           /* Calculate t0^n */
    }
    COS_T0 = COS_T0 << (LVEQNB_CosCoef[0]+6);          /* Correct the scaling */


    B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16);    /* B1 = (0.5 - b2/2) * cos(t0) */
    A0 = (0x40000000 + B2) >> 1;                        /* A0 = (0.5 + b2/2) */

    /*
     * Write coeff into the data structure
     */
    pCoefficients->A0 = (LVM_INT16)(A0>>16);
    pCoefficients->B1 = (LVM_INT16)(B1>>15);
    pCoefficients->B2 = (LVM_INT16)(B2>>16);
    pCoefficients->G  = LVEQNB_GainTable[Gain+15];


    return(LVEQNB_SUCCESS);

}