/*/////////////////////////////////////////////////////////////////////////////
//
//                  INTEL CORPORATION PROPRIETARY INFORMATION
//     This software is supplied under the terms of a license agreement or
//     nondisclosure agreement with Intel Corporation and may not be copied
//     or disclosed except in accordance with the terms of that agreement.
//          Copyright(c) 2005 Intel Corporation. All Rights Reserved.
//
//     Intel(R) Integrated Performance Primitives
//     USC - Unified Speech Codec interface library
//
// By downloading and installing USC codec, you hereby agree that the
// accompanying Materials are being provided to you under the terms and
// conditions of the End User License Agreement for the Intel(R) Integrated
// Performance Primitives product previously accepted by you. Please refer
// to the file ipplic.htm located in the root directory of your Intel(R) IPP
// product installation for more information.
//
// A speech coding standards promoted by ITU, ETSI, 3GPP and other
// organizations. Implementations of these standards, or the standard enabled
// platforms may require licenses from various entities, including
// Intel Corporation.
//
//
// Purpose: AMRWB speech codec encoder API functions.
//
*/

#include "ownamrwb.h"

static short ownSynthesis(short *pLPCQuantvec, short *pExc, short valQNew,
                          const unsigned short *pSynthSignal, AMRWBEncoder_Obj *st);
static void ownPrms2Bits(const short* pPrms, unsigned char *pBitstream, AMRWB_Rate_t rate);



AMRWB_CODECFUN( APIAMRWB_Status, apiAMRWBEncoder_GetSize,
         (AMRWBEncoder_Obj* encoderObj, unsigned int *pCodecSize))
{
   if(NULL == encoderObj)
      return APIAMRWB_StsBadArgErr;
   if(NULL == pCodecSize)
      return APIAMRWB_StsBadArgErr;
   if(encoderObj->objPrm.iKey != DEC_KEY)
      return APIAMRWB_StsNotInitialized;

   *pCodecSize = encoderObj->objPrm.iObjSize;
   return APIAMRWB_StsNoErr;
}

AMRWB_CODECFUN( APIAMRWB_Status, apiAMRWBEncoder_Alloc,
         (const AMRWBEnc_Params *amrwb_Params, unsigned int *pCodecSize))
{
   int hpfltsize;
   HighPassFIRGetSize_AMRWB_16s_ISfs(&hpfltsize);
   *pCodecSize=sizeof(AMRWBEncoder_Obj);
   *pCodecSize += (2*hpfltsize);
   ippsHighPassFilterGetSize_AMRWB_16s(2, &hpfltsize);
   *pCodecSize += (3*hpfltsize);
   ippsHighPassFilterGetSize_AMRWB_16s(3, &hpfltsize);
   *pCodecSize += hpfltsize;
   ippsVADGetSize_AMRWB_16s(&hpfltsize);
   *pCodecSize += hpfltsize;

   return APIAMRWB_StsNoErr;
}

static void InitEncoder(AMRWBEncoder_Obj* encoderObj)
{
   ippsZero_16s(encoderObj->asiExcOld, PITCH_LAG_MAX + INTERPOL_LEN);
   ippsZero_16s(encoderObj->asiSynthesis, LP_ORDER);
   ippsZero_16s(encoderObj->asiIsfQuantPast, LP_ORDER);

   encoderObj->siSpeechWgt = 0;
   encoderObj->siTiltCode = 0;
   encoderObj->siFrameFirst = 1;

   encoderObj->asiGainPitchClip[0] = MAX_DISTANCE_ISF;
   encoderObj->asiGainPitchClip[1] = MIN_GAIN_PITCH;

   encoderObj->iNoiseEnhancerThres = 0;

   return;
}

AMRWB_CODECFUN( APIAMRWB_Status, apiAMRWBEncoder_Init,
         (AMRWBEncoder_Obj* encoderObj, int mode))
{
   int hpfltsize;

   InitEncoder(encoderObj);

   encoderObj->pSHPFIRState = (HighPassFIRState_AMRWB_16s_ISfs *)((char*)encoderObj + sizeof(AMRWBEncoder_Obj));
   HighPassFIRGetSize_AMRWB_16s_ISfs(&hpfltsize);
   encoderObj->pSHPFIRState2 = (HighPassFIRState_AMRWB_16s_ISfs *)((char*)encoderObj->pSHPFIRState + hpfltsize);
   encoderObj->pSHPFiltStateSgnlIn = (IppsHighPassFilterState_AMRWB_16s *)((char*)encoderObj->pSHPFIRState2 + hpfltsize);
   ippsHighPassFilterGetSize_AMRWB_16s(2, &hpfltsize);
   encoderObj->pSHPFiltStateSgnlOut = (IppsHighPassFilterState_AMRWB_16s *)((char*)encoderObj->pSHPFiltStateSgnlIn + hpfltsize);
   encoderObj->pSHPFiltStateSgnl400 = (IppsHighPassFilterState_AMRWB_16s *)((char*)encoderObj->pSHPFiltStateSgnlOut + hpfltsize);
   encoderObj->pSHPFiltStateWsp = (IppsHighPassFilterState_AMRWB_16s *)((char*)encoderObj->pSHPFiltStateSgnl400 + hpfltsize);
   ippsHighPassFilterGetSize_AMRWB_16s(3, &hpfltsize);
   encoderObj->pSVadState = (IppsVADState_AMRWB_16s *)((char*)encoderObj->pSHPFiltStateWsp + hpfltsize);

   /* Static vectors to zero */

   ippsZero_16s(encoderObj->asiSpeechOld, SPEECH_SIZE - FRAME_SIZE);
   ippsZero_16s(encoderObj->asiWspOld, (PITCH_LAG_MAX / OPL_DECIM));
   ippsZero_16s(encoderObj->asiWspDecimate, 3);

   /* routines initialization */

   ippsZero_16s(encoderObj->asiSpeechDecimate, 2 * NB_COEF_DOWN);
   ippsHighPassFilterInit_AMRWB_16s((short*)ACoeffHP50Tbl, (short*)BCoeffHP50Tbl, 2, encoderObj->pSHPFiltStateSgnlIn);
   ippsZero_16s(encoderObj->asiLevinsonMem, 32);
   ippsSet_16s(-14336, encoderObj->asiGainQuant, PRED_ORDER);
   ippsHighPassFilterInit_AMRWB_16s((short*)ACoeffTbl, (short*)BCoeffTbl, 3, encoderObj->pSHPFiltStateWsp);

   /* isp initialization */

   ippsCopy_16s(IspInitTbl, encoderObj->asiIspOld, LP_ORDER);
   ippsCopy_16s(IspInitTbl, encoderObj->asiIspQuantOld, LP_ORDER);

   /* variable initialization */

   encoderObj->siPreemph = 0;
   encoderObj->siWsp = 0;
   encoderObj->siScaleFactorOld = 15;
   encoderObj->asiScaleFactorMax[0] = 15;
   encoderObj->asiScaleFactorMax[1] = 15;
   encoderObj->siWspOldMax = 0;
   encoderObj->siWspOldShift = 0;

   /* pitch ol initialization */

   encoderObj->siMedianOld = 40;
   encoderObj->siOpenLoopGain = 0;
   encoderObj->siAdaptiveParam = 0;
   encoderObj->siWeightFlag = 0;
   ippsSet_16s(40, encoderObj->asiPitchLagOld, 5);
   ippsZero_16s(encoderObj->asiHypassFiltWspOld, (FRAME_SIZE / 2) / OPL_DECIM + (PITCH_LAG_MAX / OPL_DECIM));

   ippsZero_16s(encoderObj->asiSynthHighFilt, LP_ORDER);
   ippsZero_16s((short*)encoderObj->aiSynthMem, LP_ORDER * 2);

   ippsHighPassFilterInit_AMRWB_16s((short*)ACoeffHP50Tbl, (short*)BCoeffHP50Tbl, 2, encoderObj->pSHPFiltStateSgnlOut);
   HighPassFIRInit_AMRWB_16s_ISfs((short*)Fir6k_7kTbl, 2, encoderObj->pSHPFIRState);
   ippsHighPassFilterInit_AMRWB_16s((short*)ACoeffHP400Tbl, (short*)BCoeffHP400Tbl, 2, encoderObj->pSHPFiltStateSgnl400);

   ippsCopy_16s(IsfInitTbl, encoderObj->asiIsfOld, LP_ORDER);

   encoderObj->siDeemph = 0;

   encoderObj->siHFSeed = RND_INIT;

   HighPassFIRInit_AMRWB_16s_ISfs((short*)Fir6k_7kTbl, 2, encoderObj->pSHPFIRState2);
   encoderObj->siAlphaGain = IPP_MAX_16S;

   encoderObj->siVadHist = 0;
   encoderObj->iDtx = mode;

   ippsVADInit_AMRWB_16s(encoderObj->pSVadState);
   encoderObj->siToneFlag = 0;
   ownDTXEncReset(&encoderObj->dtxEncState, (short*)IsfInitTbl);
   encoderObj->siScaleExp = 0;

   return APIAMRWB_StsNoErr;
}

AMRWB_CODECFUN( APIAMRWB_Status, apiAMRWBEncoder_Mode,
               (AMRWBEncoder_Obj* encoderObj, int mode)) {
    if(encoderObj->iDtx != mode) encoderObj->iDtx = mode;
    return APIAMRWB_StsNoErr;
}

AMRWB_CODECFUN( APIAMRWB_Status, apiAMRWBEncode,
         (AMRWBEncoder_Obj* encoderObj, const unsigned short* src,
          unsigned char* dst, AMRWB_Rate_t *rate, int Vad ))
{
    /* Coder states */
    AMRWBEncoder_Obj *st;

    /* Speech vector */
    IPP_ALIGNED_ARRAY(16, short, pSpeechOldvec, SPEECH_SIZE);
    short *pSpeechNew, *pSpeech, *pWindow;

    /* Weighted speech vector */
    IPP_ALIGNED_ARRAY(16, short, pWgtSpchOldvec, FRAME_SIZE + (PITCH_LAG_MAX / OPL_DECIM));
    short *pWgtSpch;

    /* Excitation vector */
    IPP_ALIGNED_ARRAY(16, short, pExcOldvec, (FRAME_SIZE + 1) + PITCH_LAG_MAX + INTERPOL_LEN);
    short *pExc;

    /* LPC coefficients */

    IPP_ALIGNED_ARRAY(16, int, pAutoCorrvec, LP_ORDER + 1);
    IPP_ALIGNED_ARRAY(16, short, pRCvec, LP_ORDER);
    IPP_ALIGNED_ARRAY(16, short, pLPCvec, LP_ORDER + 1);
    IPP_ALIGNED_ARRAY(16, short, pIspvec, LP_ORDER);
    IPP_ALIGNED_ARRAY(16, short, pIspQuantvec, LP_ORDER);
    IPP_ALIGNED_ARRAY(16, short, pIsfvec, LP_ORDER);
    short *pLPCUnQuant, *pLPCQuant;
    IPP_ALIGNED_ARRAY(16, short, pLPCUnQuantvec, NUMBER_SUBFRAME*(LP_ORDER+1));
    IPP_ALIGNED_ARRAY(16, short, pLPCQuantvec, NUMBER_SUBFRAME*(LP_ORDER+1));

    /* Other vectors */

    IPP_ALIGNED_ARRAY(16, short, pAdptTgtPitch, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pAdptTgtCdbk, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pCorrvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pResidvec, SUBFRAME_SIZE);

    IPP_ALIGNED_ARRAY(16, short, pImpRespPitchvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pImpRespCdbkvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pFixCdbkvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pFixCdbkExcvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pFltAdptvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pFltAdpt2vec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pErrQuant, LP_ORDER + SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pSynthvec, SUBFRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pExc2vec, FRAME_SIZE);
    IPP_ALIGNED_ARRAY(16, short, pVadvec, FRAME_SIZE);

    /* Scalars */

    short valSubFrame, valSelect, valClipFlag, valVadFlag;
    short valPitchLag, valFracPitchLag;
    short pOpenLoopLag[2],pPitchLagBounds[2];
    short valPitchGain, valScaleCodeGain, pGainCoeff[4], pGainCoeff2[4];
    short tmp, valGainCoeff, valAdptGain, valExp, valQNew, valShift, max;
    short valVoiceFac;

    int i, z, valCodeGain, valMax;

    short valStabFac, valFac, valCodeGainLow;
    short pSrcCoeff[2];

    short valCorrGain;
    int valSize, subFrame;
    IppSpchBitRate irate = Mode2RateTbl[*rate];
    IppSpchBitRate valCodecMode = irate;
    short *pHPWgtSpch;
    IPP_ALIGNED_ARRAY(16, short, pPrmvec, MAX_PARAM_SIZE);
    short *pPrm = pPrmvec;

    st = encoderObj;

    pSpeechNew = pSpeechOldvec + SPEECH_SIZE - FRAME_SIZE - UP_SAMPL_FILT_DELAY;
    pSpeech = pSpeechOldvec + SPEECH_SIZE - FRAME_SIZE - SUBFRAME_SIZE;
    pWindow = pSpeechOldvec + SPEECH_SIZE - WINDOW_SIZE;

    pExc = pExcOldvec + PITCH_LAG_MAX + INTERPOL_LEN;
    pWgtSpch = pWgtSpchOldvec + (PITCH_LAG_MAX / OPL_DECIM);

    /* copy coder memory state into working space (internal memory for DSP) */

    ippsCopy_16s(st->asiSpeechOld, pSpeechOldvec, SPEECH_SIZE - FRAME_SIZE);
    ippsCopy_16s(st->asiWspOld, pWgtSpchOldvec, PITCH_LAG_MAX / OPL_DECIM);
    ippsCopy_16s(st->asiExcOld, pExcOldvec, PITCH_LAG_MAX + INTERPOL_LEN);

    /* Down sampling signal from 16kHz to 12.8kHz */

    ownDecimation_AMRWB_16s((short*)src, L_FRAME16k, pSpeechNew, st->asiSpeechDecimate);

    /* last UP_SAMPL_FILT_DELAY samples for autocorrelation window */
    ippsCopy_16s(st->asiSpeechDecimate, pFixCdbkvec, 2*DOWN_SAMPL_FILT_DELAY);
    ippsZero_16s(pErrQuant, DOWN_SAMPL_FILT_DELAY);            /* set next sample to zero */
    ownDecimation_AMRWB_16s(pErrQuant, DOWN_SAMPL_FILT_DELAY, pSpeechNew + FRAME_SIZE, pFixCdbkvec);

    /* Perform 50Hz HP filtering of input signal */

    ippsHighPassFilter_AMRWB_16s_ISfs(pSpeechNew, FRAME_SIZE, st->pSHPFiltStateSgnlIn, 14);

    /* last UP_SAMPL_FILT_DELAY samples for autocorrelation window */
    ippsHighPassFilterGetSize_AMRWB_16s(2, &valSize);
    ippsCopy_8u((const Ipp8u*)st->pSHPFiltStateSgnlIn, (Ipp8u*)pFixCdbkvec, valSize);
    ippsHighPassFilter_AMRWB_16s_ISfs(pSpeechNew + FRAME_SIZE, UP_SAMPL_FILT_DELAY, (IppsHighPassFilterState_AMRWB_16s *)pFixCdbkvec, 14);

    /* get max of new preemphased samples (FRAME_SIZE+UP_SAMPL_FILT_DELAY) */

    z = pSpeechNew[0] << 15;
    z -= st->siPreemph * PREEMPH_FACTOR;
    valMax = Abs_32s(z);

    for (i = 1; i < FRAME_SIZE + UP_SAMPL_FILT_DELAY; i++)
    {
        z  = pSpeechNew[i] << 15;
        z -= pSpeechNew[i - 1] * PREEMPH_FACTOR;
        z  = Abs_32s(z);
        if (z > valMax) valMax = z;
    }

    /* get scaling factor for new and previous samples */
    tmp = (short)(valMax >> 16);

    if (tmp == 0)
    {
        valShift = MAX_SCALE_FACTOR;
    } else
    {
        valShift = Exp_16s(tmp) - 1;
        if (valShift < 0) valShift = 0;
        if (valShift > MAX_SCALE_FACTOR) valShift = MAX_SCALE_FACTOR;
    }
    valQNew = valShift;

    if (valQNew > st->asiScaleFactorMax[0]) valQNew = st->asiScaleFactorMax[0];
    if (valQNew > st->asiScaleFactorMax[1]) valQNew = st->asiScaleFactorMax[1];
    valExp = valQNew - st->siScaleFactorOld;
    st->siScaleFactorOld = valQNew;
    st->asiScaleFactorMax[1] = st->asiScaleFactorMax[0];
    st->asiScaleFactorMax[0] = valShift;

    /* preemphasis with scaling (FRAME_SIZE+UP_SAMPL_FILT_DELAY) */

    tmp = pSpeechNew[FRAME_SIZE - 1];

    ippsPreemphasize_AMRWB_16s_ISfs(PREEMPH_FACTOR, pSpeechNew, FRAME_SIZE + UP_SAMPL_FILT_DELAY, 15-valQNew, &st->siPreemph);

    st->siPreemph = tmp;

    /* scale previous samples and memory */

    ownScaleSignal_AMRWB_16s_ISfs(pSpeechOldvec, SPEECH_SIZE - FRAME_SIZE - UP_SAMPL_FILT_DELAY, valExp);
    ownScaleSignal_AMRWB_16s_ISfs(pExcOldvec, PITCH_LAG_MAX + INTERPOL_LEN, valExp);

    ownScaleSignal_AMRWB_16s_ISfs(st->asiSynthesis, LP_ORDER, valExp);
    ownScaleSignal_AMRWB_16s_ISfs(st->asiWspDecimate, 3, valExp);
    ownScaleSignal_AMRWB_16s_ISfs(&(st->siWsp), 1, valExp);
    ownScaleSignal_AMRWB_16s_ISfs(&(st->siSpeechWgt), 1, valExp);

     /*  Call VAD */

    ippsCopy_16s(pSpeechNew, pVadvec, FRAME_SIZE);
    ownScaleSignal_AMRWB_16s_ISfs(pVadvec, FRAME_SIZE, (short)(1 - valQNew));
    ippsVAD_AMRWB_16s(pVadvec, st->pSVadState, &encoderObj->siToneFlag, &valVadFlag);
    if (valVadFlag == 0)
        st->siVadHist += 1;
    else
        st->siVadHist = 0;

    /* DTX processing */

    if (Vad != 0)
    {
       short foo;
       short dtxMode = ~IPP_SPCHBR_DTX;