/*
 * 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.h"
#include "LVEQNB_Private.h"
#include "VectorArithmetic.h"
#include "BIQUAD.h"


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

#define     LOW_FREQ            298             /* 32768/110 for low test frequency */
#define     HIGH_FREQ           386             /* 32768/85 for high test frequency */

/****************************************************************************************/
/*                                                                                      */
/* FUNCTION:                 LVEQNB_GetParameters                                       */
/*                                                                                      */
/* DESCRIPTION:                                                                         */
/*  Request the N-Band equaliser parameters. The current parameter set is returned via  */
/*  the parameter pointer.                                                              */
/*                                                                                      */
/* PARAMETERS:                                                                          */
/*  hInstance                Instance handle                                            */
/*  pParams                  Pointer to an empty parameter structure                    */
/*                                                                                      */
/* RETURNS:                                                                             */
/*  LVEQNB_SUCCESS          Succeeds                                                    */
/*  LVEQNB_NULLADDRESS      Instance or pParams  is NULL pointer                        */
/*                                                                                      */
/* NOTES:                                                                               */
/*  1.  This function may be interrupted by the LVEQNB_Process function                 */
/*                                                                                      */
/****************************************************************************************/

LVEQNB_ReturnStatus_en LVEQNB_GetParameters(LVEQNB_Handle_t     hInstance,
                                            LVEQNB_Params_t     *pParams)
{

    LVEQNB_Instance_t    *pInstance =(LVEQNB_Instance_t  *)hInstance;

   /*
     * Check for error conditions
     */
    if((hInstance == LVM_NULL) || (pParams == LVM_NULL))
    {
        return LVEQNB_NULLADDRESS;
    }

    *pParams = pInstance->Params;

    return(LVEQNB_SUCCESS);
}


/************************************************************************************/
/*                                                                                  */
/* FUNCTION:                 LVEQNB_GetCapabilities                                 */
/*                                                                                  */
/* DESCRIPTION:                                                                     */
/*  Get the N-Band equaliser capabilities. The current capabilities are returned    */
/*  via the pointer.                                                                */
/*                                                                                  */
/* PARAMETERS:                                                                      */
/*  hInstance                Instance handle                                        */
/*  pCapabilities            Pointer to an empty capability structure               */
/*                                                                                  */
/* RETURNS:                                                                         */
/*  LVEQNB_Success           Succeeds                                               */
/*  LVEQNB_NULLADDRESS       hInstance or pCapabilities is NULL                     */
/*                                                                                  */
/* NOTES:                                                                           */
/*  1.  This function may be interrupted by the LVEQNB_Process function             */
/*                                                                                  */
/************************************************************************************/

LVEQNB_ReturnStatus_en LVEQNB_GetCapabilities(LVEQNB_Handle_t           hInstance,
                                              LVEQNB_Capabilities_t     *pCapabilities)
{

    LVEQNB_Instance_t    *pInstance =(LVEQNB_Instance_t  *)hInstance;

    if((hInstance == LVM_NULL) || (pCapabilities == LVM_NULL))
    {
        return LVEQNB_NULLADDRESS;
    }

    *pCapabilities = pInstance->Capabilities;

    return(LVEQNB_SUCCESS);
}


/************************************************************************************/
/*                                                                                  */
/* FUNCTION:            LVEQNB_SetFilters                                           */
/*                                                                                  */
/* DESCRIPTION:                                                                     */
/*  Sets the filter type based on the definition.                                   */
/*                                                                                  */
/* PARAMETERS:                                                                      */
/*  pInstance           Pointer to the instance                                     */
/*  pParams             Initialisation parameters                                   */
/*                                                                                  */
/* RETURNS:                                                                         */
/*  void                Nothing                                                     */
/*                                                                                  */
/* NOTES:                                                                           */
/*  1. To select the biquad type the follow rules are applied:                      */
/*          Double precision    if (fc <= fs/110)                                   */
/*          Double precision    if (fs/110 < fc < fs/85) & (Q>3)                    */
/*          Single precision    otherwise                                           */
/*                                                                                  */
/************************************************************************************/

void    LVEQNB_SetFilters(LVEQNB_Instance_t     *pInstance,
                          LVEQNB_Params_t       *pParams)
{

    extern const LVM_UINT16   LVEQNB_SampleRateTab[];           /* Sample rate table */
    LVM_UINT16          i;                                      /* Filter band index */
    LVM_UINT32          fs = (LVM_UINT32)LVEQNB_SampleRateTab[(LVM_UINT16)pParams->SampleRate];  /* Sample rate */
    LVM_UINT32          fc;                                     /* Filter centre frequency */
    LVM_INT16           QFactor;                                /* Filter Q factor */


    pInstance->NBands = pParams->NBands;

    for (i=0; i<pParams->NBands; i++)
    {
        /*
         * Get the filter settings
         */
        fc = (LVM_UINT32)pParams->pBandDefinition[i].Frequency;     /* Get the band centre frequency */
        QFactor = (LVM_INT16)pParams->pBandDefinition[i].QFactor;   /* Get the band Q factor */


        /*
         * For each filter set the type of biquad required
         */
        pInstance->pBiquadType[i] = LVEQNB_SinglePrecision;         /* Default to single precision */
        if ((fc << 15) <= (LOW_FREQ * fs))
        {
            /*
             * fc <= fs/110
             */
            pInstance->pBiquadType[i] = LVEQNB_DoublePrecision;
        }
        else if (((fc << 15) <= (HIGH_FREQ * fs)) && (QFactor > 300))
        {
            /*
             * (fs/110 < fc < fs/85) & (Q>3)
             */
            pInstance->pBiquadType[i] = LVEQNB_DoublePrecision;
        }


        /*
         * Check for out of range frequencies
         */
        if (fc > (fs >> 1))
        {
            pInstance->pBiquadType[i] = LVEQNB_OutOfRange;
        }


        /*
         * Copy the filter definition to persistant memory
         */
        pInstance->pBandDefinitions[i] = pParams->pBandDefinition[i];

    }
}


/************************************************************************************/
/*                                                                                  */
/* FUNCTION:            LVEQNB_SetCoefficients                                      */
/*                                                                                  */
/* DESCRIPTION:                                                                     */
/*  Sets the filter coefficients. This uses the type to select single or double     */
/*  precision coefficients.                                                         */
/*                                                                                  */
/* PARAMETERS:                                                                      */
/*  pInstance           Pointer to the instance                                     */
/*  pParams             Initialisation parameters                                   */
/*                                                                                  */
/************************************************************************************/

void    LVEQNB_SetCoefficients(LVEQNB_Instance_t     *pInstance)
{

    LVM_UINT16              i;                          /* Filter band index */
    LVEQNB_BiquadType_en    BiquadType;                 /* Filter biquad type */


    /*
     * Set the coefficients for each band by the init function
     */
    for (i=0; i<pInstance->Params.NBands; i++)
    {

        /*
         * Check band type for correct initialisation method and recalculate the coefficients
         */
        BiquadType = pInstance->pBiquadType[i];
        switch  (BiquadType)
        {
            case    LVEQNB_DoublePrecision:
            {
                PK_C32_Coefs_t      Coefficients;

                /*
                 * Calculate the double precision coefficients
                 */
                LVEQNB_DoublePrecCoefs((LVM_UINT16)pInstance->Params.SampleRate,
                                       &pInstance->pBandDefinitions[i],
                                       &Coefficients);

                /*
                 * Set the coefficients
                 */
                PK_2I_D32F32CllGss_TRC_WRA_01_Init(&pInstance->pEQNB_FilterState[i],
                                                   &pInstance->pEQNB_Taps[i],
                                                   &Coefficients);
                break;
            }

            case    LVEQNB_SinglePrecision:
            {
                PK_C16_Coefs_t      Coefficients;

                /*
                 * Calculate the single precision coefficients
                 */
                LVEQNB_SinglePrecCoefs((LVM_UINT16)pInstance->Params.SampleRate,
                                       &pInstance->pBandDefinitions[i],
                                       &Coefficients);

                /*
                 * Set the coefficients
                 */
                PK_2I_D32F32CssGss_TRC_WRA_01_Init(&pInstance->pEQNB_FilterState[i],
                                                   &pInstance->pEQNB_Taps[i],
                                                   &Coefficients);
                break;
            }
            default:
                break;
        }
    }

}


/************************************************************************************/
/*                                                                                  */
/* FUNCTION:            LVEQNB_ClearFilterHistory                                   */
/*                                                                                  */
/* DESCRIPTION:                                                                     */
/*  Clears the filter data history                                                  */
/*                                                                                  */
/* PARAMETERS:                                                                      */
/*  pInstance           Pointer to the instance                                     */
/*                                                                                  */
/************************************************************************************/

void    LVEQNB_ClearFilterHistory(LVEQNB_Instance_t     *pInstance)
{
    LVM_INT16       *pTapAddress;
    LVM_INT16       NumTaps;


    pTapAddress = (LVM_INT16 *)pInstance->pEQNB_Taps;
    NumTaps     = (LVM_INT16)((pInstance->Capabilities.MaxBands * sizeof(Biquad_2I_Order2_Taps_t))/sizeof(LVM_INT16));

    if (NumTaps != 0)
    {
        LoadConst_16(0,                                 /* Clear the history, value 0 */
                     pTapAddress,                       /* Destination */
                     NumTaps);                          /* Number of words */
    }
}


/****************************************************************************************/
/*                                                                                      */
/* FUNCTION:                LVEQNB_Control                                              */
/*                                                                                      */
/* DESCRIPTION:                                                                         */
/*  Sets or changes the LifeVibes module parameters.                                    */
/*                                                                                      */
/* PARAMETERS:                                                                          */
/*  hInstance               Instance handle                                             */
/*  pParams                 Pointer to a parameter structure                            */
/*                                                                                      */
/* RETURNS:                                                                             */
/*  LVEQNB_Success          Always succeeds                                             */
/*  LVEQNB_NULLADDRESS      Instance or pParams  is NULL pointer                        */
/*  LVEQNB_NULLADDRESS      NULL address for the equaliser filter definitions and the   */
/*                          number of bands is non-zero                                 */
/*                                                                                      */
/* NOTES:                                                                               */
/*  1.  This function may be interrupted by the LVEQNB_Process function                 */
/*                                                                                      */
/****************************************************************************************/

LVEQNB_ReturnStatus_en LVEQNB_Control(LVEQNB_Handle_t        hInstance,
                                      LVEQNB_Params_t        *pParams)
{

    LVEQNB_Instance_t    *pInstance = (LVEQNB_Instance_t  *)hInstance;
    LVM_INT16            bChange    = LVM_FALSE;
    LVM_INT16            i = 0;
    LVEQNB_Mode_en       OperatingModeSave ;

    /*
     * Check for error conditions
     */
    if((hInstance == LVM_NULL) || (pParams == LVM_NULL))
    {
        return LVEQNB_NULLADDRESS;
    }

    if((pParams->NBands !=0) && (pParams->pBandDefinition==LVM_NULL))
    {
        return LVEQNB_NULLADDRESS;
    }

    OperatingModeSave = pInstance->Params.OperatingMode;

    /* Set the alpha factor of the mixer */
    if (pParams->SampleRate != pInstance->Params.SampleRate)
    {
        LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[0],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2);
        LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[1],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2);
    }


    if( (pInstance->Params.NBands            !=  pParams->NBands          ) ||
        (pInstance->Params.OperatingMode     !=  pParams->OperatingMode   ) ||
        (pInstance->Params.pBandDefinition   !=  pParams->pBandDefinition ) ||
        (pInstance->Params.SampleRate        !=  pParams->SampleRate      ) ||
        (pInstance->Params.SourceFormat      !=  pParams->SourceFormat    ))
    {

        bChange = LVM_TRUE;
    }
    else
    {
        for(i = 0; i < pParams->NBands; i++)
        {

            if((pInstance->pBandDefinitions[i].Frequency  != pParams->pBandDefinition[i].Frequency )||
                (pInstance->pBandDefinitions[i].Gain       != pParams->pBandDefinition[i].Gain      )||
                (pInstance->pBandDefinitions[i].QFactor    != pParams->pBandDefinition[i].QFactor   ))
            {

                bChange = LVM_TRUE;
            }
        }
    }


    if(bChange){

        /*
         * If the sample rate has changed clear the history
         */
        if (pInstance->Params.SampleRate != pParams->SampleRate)
        {
            LVEQNB_ClearFilterHistory(pInstance);           /* Clear the history */
        }

        /*
         * Update the instance parameters
         */
        pInstance->Params = *pParams;


        /*
         * Reset the filters except if the algo is switched off
         */
        if(pParams->OperatingMode != LVEQNB_BYPASS){
            /*
             * Reset the filters as all parameters could have changed
             */
            LVEQNB_SetFilters(pInstance,                        /* Instance pointer */
                              pParams);                         /* New parameters */

            /*
             * Update the filters
             */
            LVEQNB_SetCoefficients(pInstance);                  /* Instance pointer */
        }

        if(pParams->OperatingMode != OperatingModeSave)
        {
            if(pParams->OperatingMode == LVEQNB_ON)
            {
                LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[0],LVM_MAXINT_16);
                LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[1],0);

                pInstance->BypassMixer.MixerStream[0].CallbackSet        = 1;
                pInstance->BypassMixer.MixerStream[1].CallbackSet        = 1;
            }
            else
            {
                /* Stay on the ON operating mode until the transition is done */
                pInstance->Params.OperatingMode = LVEQNB_ON;

                LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[0],0);
                LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[1],LVM_MAXINT_16);
                pInstance->BypassMixer.MixerStream[0].CallbackSet        = 1;
                pInstance->BypassMixer.MixerStream[1].CallbackSet        = 1;
            }
            LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[0],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2);
            LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[1],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2);

            pInstance->bInOperatingModeTransition = LVM_TRUE;
        }

    }
    return(LVEQNB_SUCCESS);
}


/****************************************************************************************/
/*                                                                                      */
/* FUNCTION:                LVEQNB_BypassMixerCallBack                                  */
/*                                                                                      */
/* DESCRIPTION:                                                                         */
/*  CallBack function of the mixer                                                      */
/*  transition                                                                          */
/*                                                                                      */
/****************************************************************************************/
LVM_INT32 LVEQNB_BypassMixerCallBack (void* hInstance,
                                      void *pGeneralPurpose,
                                      LVM_INT16 CallbackParam)
{
    LVEQNB_Instance_t      *pInstance =(LVEQNB_Instance_t  *)hInstance;
    LVM_Callback            CallBack  = pInstance->Capabilities.CallBack;

    (void) pGeneralPurpose;

     /*
      * Send an ALGOFF event if the ON->OFF switch transition is finished
      */
    if((LVC_Mixer_GetTarget(&pInstance->BypassMixer.MixerStream[0]) == 0x00000000) &&
       (CallbackParam == 0)){
        pInstance->Params.OperatingMode = LVEQNB_BYPASS;
        if (CallBack != LVM_NULL){
            CallBack(pInstance->Capabilities.pBundleInstance, LVM_NULL, ALGORITHM_EQNB_ID|LVEQNB_EVENT_ALGOFF);
        }
    }

    /*
     *  Exit transition state
     */
    pInstance->bInOperatingModeTransition = LVM_FALSE;

    return 1;
}