/*/////////////////////////////////////////////////////////////////////////////
//
//                  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) 2004-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: G.729 floating-point speech codec: internal functions.
//
*/

#include <math.h>
#include <ippsr.h>
#include "owng729fp.h"

#define         sqr(a)          ((a)*(a))

/* Hamming_cos window for LPC analysis.           */

static __ALIGN32 CONST float HammingWindow[WINDOW_LEN] = {        /* hamming-cosine window */
0.08000000f, 0.08005703f, 0.08022812f, 0.08051321f,
0.08091225f, 0.08142514f, 0.08205172f, 0.08279188f,
0.08364540f, 0.08461212f, 0.08569173f, 0.08688401f,
0.08818865f, 0.08960532f, 0.09113365f, 0.09277334f,
0.09452391f, 0.09638494f, 0.09835598f, 0.10043652f,
0.10262608f, 0.10492408f, 0.10732999f, 0.10984316f,
0.11246302f, 0.11518890f, 0.11802010f, 0.12095598f,
0.12399574f, 0.12713866f, 0.13038395f, 0.13373083f,
0.13717847f, 0.14072597f, 0.14437246f, 0.14811710f,
0.15195890f, 0.15589692f, 0.15993017f, 0.16405767f,
0.16827843f, 0.17259133f, 0.17699537f, 0.18148938f,
0.18607232f, 0.19074300f, 0.19550033f, 0.20034306f,
0.20527001f, 0.21027996f, 0.21537170f, 0.22054392f,
0.22579536f, 0.23112471f, 0.23653066f, 0.24201185f,
0.24756692f, 0.25319457f, 0.25889328f, 0.26466170f,
0.27049842f, 0.27640197f, 0.28237087f, 0.28840363f,
0.29449883f, 0.30065489f, 0.30687031f, 0.31314352f,
0.31947297f, 0.32585713f, 0.33229437f, 0.33878314f,
0.34532180f, 0.35190874f, 0.35854232f, 0.36522087f,
0.37194279f, 0.37870640f, 0.38550997f, 0.39235184f,
0.39923036f, 0.40614375f, 0.41309035f, 0.42006844f,
0.42707625f, 0.43411207f, 0.44117412f, 0.44826069f,
0.45537004f, 0.46250033f, 0.46964988f, 0.47681686f,
0.48399949f, 0.49119604f, 0.49840465f, 0.50562358f,
0.51285106f, 0.52008528f, 0.52732444f, 0.53456670f,
0.54181033f, 0.54905349f, 0.55629444f, 0.56353134f,
0.57076240f, 0.57798582f, 0.58519983f, 0.59240264f,
0.59959245f, 0.60676748f, 0.61392599f, 0.62106609f,
0.62818617f, 0.63528436f, 0.64235890f, 0.64940804f,
0.65643007f, 0.66342324f, 0.67038584f, 0.67731601f,
0.68421221f, 0.69107264f, 0.69789559f, 0.70467937f,
0.71142232f, 0.71812278f, 0.72477907f, 0.73138952f,
0.73795253f, 0.74446648f, 0.75092971f, 0.75734061f,
0.76369762f, 0.76999915f, 0.77624369f, 0.78242958f,
0.78855544f, 0.79461962f, 0.80062068f, 0.80655706f,
0.81242740f, 0.81823015f, 0.82396388f, 0.82962728f,
0.83521879f, 0.84073710f, 0.84618086f, 0.85154873f,
0.85683930f, 0.86205131f, 0.86718345f, 0.87223446f,
0.87720311f, 0.88208807f, 0.88688827f, 0.89160240f,
0.89622939f, 0.90076804f, 0.90521723f, 0.90957582f,
0.91384280f, 0.91801709f, 0.92209762f, 0.92608339f,
0.92997342f, 0.93376678f, 0.93746245f, 0.94105959f,
0.94455731f, 0.94795465f, 0.95125085f, 0.95444512f,
0.95753652f, 0.96052444f, 0.96340811f, 0.96618676f,
0.96885973f, 0.97142631f, 0.97388595f, 0.97623801f,
0.97848189f, 0.98061699f, 0.98264289f, 0.98455900f,
0.98636484f, 0.98806006f, 0.98964417f, 0.99111670f,
0.99247742f, 0.99372596f, 0.99486196f, 0.99588519f,
0.99679530f, 0.99759221f, 0.99827564f, 0.99884540f,
0.99930143f, 0.99964350f, 0.99987161f, 0.99998569f,
1.00000000f, 0.99921930f, 0.99687845f, 0.99298108f,
0.98753333f, 0.98054361f, 0.97202289f, 0.96198452f,
0.95044410f, 0.93741965f, 0.92293155f, 0.90700239f,
0.88965708f, 0.87092263f, 0.85082841f, 0.82940567f,
0.80668795f, 0.78271067f, 0.75751126f, 0.73112911f,
0.70360541f, 0.67498308f, 0.64530689f, 0.61462307f,
0.58297962f, 0.55042595f, 0.51701277f, 0.48279238f,
0.44781810f, 0.41214463f, 0.37582767f, 0.33892387f,
0.30149087f, 0.26358715f, 0.22527184f, 0.18660481f,
0.14764643f, 0.10845750f, 0.06909923f, 0.02963307f};

void ownAutoCorr_G729_32f(Ipp32f *pSrc, int len, Ipp32f *pDst, float *pExtBuff)
{
   ippsMul_32f(pSrc, HammingWindow, pExtBuff, WINDOW_LEN);

   ippsAutoCorr_32f(pExtBuff, WINDOW_LEN, pDst, len+1);

   if (pDst[0]<(Ipp32f)1.0) pDst[0]=(Ipp32f)1.0;

   return;
}

void ownACOS_G729_32f(Ipp32f *pSrc, Ipp32f *pDst, Ipp32s len)
{

   int i;

   for (i=0; i<len; i++ )
      pDst[i] = (Ipp32f)acos(pSrc[i]);
   return;
}

void ownCOS_G729_32f(Ipp32f *pSrc, Ipp32f *pDst, Ipp32s len)
{

   int i;

   for (i=0; i<len; i++ )
      pDst[i] = (Ipp32f)cos(pSrc[i]);
   return;
}


Ipp32f ownAdaptiveCodebookGainCoeff_G729_32f(Ipp32f *pSrcTargetVector, Ipp32f *pSrcFltAdaptivCdbkVec,
               Ipp32f *pDstCorrCoeff, int len)
{
    Ipp32f fCorr, fEnergy, fGain;
    Ipp64f sum;

    /* energy of filtered excitation */
    ippsDotProd_32f64f(pSrcFltAdaptivCdbkVec, pSrcFltAdaptivCdbkVec, len, &sum);
    fEnergy = (Ipp32f)(sum + 0.01);

    ippsDotProd_32f64f(pSrcTargetVector, pSrcFltAdaptivCdbkVec, len, &sum);
    fCorr = (Ipp32f)sum;

    pDstCorrCoeff[0] = fEnergy;
    pDstCorrCoeff[1] = -2.0f*fCorr +0.01f;

    /* find pitch fGain and bound it by [0,1.2] */

    fGain = fCorr/fEnergy;

    CLIP_TO_LOWLEVEL(fGain,0.f);
    CLIP_TO_UPLEVEL(fGain,GAIN_PIT_MAX);

    return fGain;
}


void WeightLPCCoeff_G729(float *pSrcLPC, float valWeightingFactor, int len, float *pDstWeightedLPC)
{
  float fFactor;
  int i;

  pDstWeightedLPC[0]=pSrcLPC[0];
  fFactor=valWeightingFactor;
  for (i = 1; i < len; i++)
  {
    pDstWeightedLPC[i] = fFactor*pSrcLPC[i];
    fFactor *= valWeightingFactor;
  }
  pDstWeightedLPC[len] = fFactor*pSrcLPC[len];
  return;
}

void AdaptiveCodebookGainCoeff_G729_32f( float *pSrcTargetVector, float *pSrcFltAdaptiveCodebookVector,
                                         float *pSrcFltInnovation, float *pDstCoeff)
{
   Ipp64f sum;

   ippsDotProd_32f64f(pSrcFltInnovation, pSrcFltInnovation, SUBFR_LEN, &sum);
   pDstCoeff[2] = (Ipp32f)(sum + 0.01);

   ippsDotProd_32f64f(pSrcTargetVector, pSrcFltInnovation, SUBFR_LEN, &sum);
   pDstCoeff[3] = (Ipp32f)(-2.0*(sum + 0.01));

   ippsDotProd_32f64f(pSrcFltAdaptiveCodebookVector, pSrcFltInnovation, SUBFR_LEN, &sum);
   pDstCoeff[4] = (Ipp32f)(2.0*(sum + 0.01));

   return;

}

int ownAdaptiveCodebookSearch_G729A_32f(Ipp32f *pSrcExc, Ipp32f *pSrcTargetVector, Ipp32f *pSrcImpulseResponse,
  int minPitchDelay, int maxPitchDelay, int nSbfr, int *fracPartPitchDelay, float *pExtBuff)
{
  int  pitchPeriod;
  float  *pCorr;
  float  *pTmpExcitation;
  Ipp64f  corr, max;
  int delayLine[2];

  pCorr = &pExtBuff[0];
  pTmpExcitation = &pExtBuff[SUBFR_LEN];

  /* Compute  correlations of input response with the target vector.*/

  ippsCrossCorr_32f(pSrcImpulseResponse, SUBFR_LEN, pSrcTargetVector, SUBFR_LEN, pCorr,  SUBFR_LEN, 0);

  /* Find maximum integer delay */

  ippsCrossCorrLagMax_32f64f(pCorr, &pSrcExc[-maxPitchDelay], SUBFR_LEN, maxPitchDelay-minPitchDelay, &max, &pitchPeriod);
  pitchPeriod = (maxPitchDelay-minPitchDelay-pitchPeriod) + minPitchDelay;

  /* Test fractions */

  /* Fraction 0 */
  delayLine[0] = pitchPeriod;
  delayLine[1] = 0;
  ippsDecodeAdaptiveVector_G729_32f_I(delayLine, pSrcExc);
  ippsDotProd_32f64f( pCorr, pSrcExc, SUBFR_LEN, &max);
  *fracPartPitchDelay = 0;

  /* If first subframe and lag > 84 do not search fractional pitch */

  if( (nSbfr == 0) && (pitchPeriod > 84) )
    return pitchPeriod;

  ippsCopy_32f(pSrcExc, pTmpExcitation, SUBFR_LEN);

  /* Fraction -1/3 */

  delayLine[1] = -1;

  ippsDecodeAdaptiveVector_G729_32f_I(delayLine, pSrcExc);
  ippsDotProd_32f64f( pCorr, pSrcExc, SUBFR_LEN, &corr);
  if(corr > max){
    max = corr;
    *fracPartPitchDelay = -1;
    ippsCopy_32f(pSrcExc, pTmpExcitation, SUBFR_LEN);
  }

  /* Fraction +1/3 */

  delayLine[1] = 1;
  ippsDecodeAdaptiveVector_G729_32f_I(delayLine, pSrcExc);
  ippsDotProd_32f64f( pCorr, pSrcExc, SUBFR_LEN, &corr);
  if(corr > max){
    max = corr;
    *fracPartPitchDelay = 1;
  }
  else
    ippsCopy_32f(pTmpExcitation, pSrcExc, SUBFR_LEN);

  return pitchPeriod;
}

int  ExtractBitsG729FP( const unsigned char **pBits, int *nBit, int Count )
{
    int     i ;
    int  Rez = 0L ;

    for ( i = 0 ; i < Count ; i ++ ){
        int  fTmp ;
        fTmp = ((*pBits)[(i + *nBit)>>3] >> (7 - ((i + *nBit) & 0x0007)) ) & 1;
        Rez <<= 1 ;
        Rez += fTmp ;
    }

    *pBits += (Count + *nBit)>>3;
    *nBit = (Count + *nBit) & 0x0007;

    return Rez ;
}

void PWGammaFactor_G729(float *pGamma1, float *pGamma2, float *pIntLSF, float *CurrLSF,
                   float *ReflectCoeff, int   *isFlat, float *PrevLogAreaRatioCoeff)
{
   float    logAreaRatioCoeff[4];
   float   *logAreaRatioCoeffNew;
   float   *lsf;
   float    minDist, fTmp;
   int      i, k;

   logAreaRatioCoeffNew = &logAreaRatioCoeff[2];

   /* Convert reflection coefficients to the Log Area Ratio coefficient*/
   for (i=0; i<2; i++)
      logAreaRatioCoeffNew[i] = (float)log10( (double)( ( 1.0f + ReflectCoeff[i]) / (1.0f - ReflectCoeff[i])));

   /* Interpolation of lar for the 1st subframe */
   for (i=0; i<2; i++) {
      logAreaRatioCoeff[i] = 0.5f * (logAreaRatioCoeffNew[i] + PrevLogAreaRatioCoeff[i]);
      PrevLogAreaRatioCoeff[i] = logAreaRatioCoeffNew[i];
   }

   for (k=0; k<2; k++) {    /* LOOP : gamma2 for 1st to 2nd subframes */
      if (*isFlat != 0) {
         if ((logAreaRatioCoeff[2*k] <LAR_THRESH1 )&&(logAreaRatioCoeff[2*k+1] > LAR_THRESH3)) *isFlat = 0;
      } else {
         if ((logAreaRatioCoeff[2*k] > LAR_THRESH2)||(logAreaRatioCoeff[2*k+1] < LAR_THRESH4)) *isFlat = 1;
      }

      if (*isFlat == 0) {
         /* Second criterion based on the minimum distance between two successives lsfs. */
         pGamma1[k] = GAMMA1_TILTED;
         if (k == 0) lsf = pIntLSF;
         else lsf = CurrLSF;

         minDist = lsf[1] - lsf[0];
         for (i=1; i<LPC_ORDER-1; i++) {
            fTmp = lsf[i+1] - lsf[i];
            if (fTmp < minDist) minDist = fTmp;
         }

         pGamma2[k] =  GAMMA2_TILTED_SCALE * minDist + GAMMA2_TILTED_SHIFT;

         if (pGamma2[k] > GAMMA2_TILTED_MAX) pGamma2[k] = GAMMA2_TILTED_MAX;
         if (pGamma2[k] < GAMMA2_TILTED_MIN) pGamma2[k] = GAMMA2_TILTED_MIN;
      } else {
         pGamma1[k] = GAMMA1_FLAT;
         pGamma2[k] = GAMMA2_FLAT;
      }
   }
   return;
}


void CodewordImpConv_G729_32f(int index, const float *pSrc1,const float *pSrc2,float *pDst)
{
   int i;
   int lPos0, lPos1, lPos2, lPos3; /* position*/
   int lSign0, lSign1, lSign2, lSign3;     /*signs: 1,-1*/

   lPos0 = index & 0x7;
   lPos1 = (index>>3) & 0x7;
   lPos2 = (index>>6) & 0x7;
   lPos3 = index>>9;

   lPos0 = (lPos0<<2)+lPos0;                      /* lPos0*5;*/
   lPos1 = (lPos1<<2)+lPos1+1;                    /* 1+lPos1*5;*/
   lPos2 = (lPos2<<2)+lPos2+2;                    /* 2+lPos2*5;*/
   lPos3 = ((lPos3>>1)<<2)+(lPos3>>1)+(lPos3&1)+3;  /* 3+(lPos3>>1)*5+(lPos3&1);*/

   if (lPos0>lPos1) {i=lPos0; lPos0=lPos1; lPos1=i; }
   if (lPos2>lPos3) {i=lPos2; lPos2=lPos3; lPos3=i; }
   if (lPos0>lPos2) {i=lPos0; lPos0=lPos2; lPos2=i; }
   if (lPos1>lPos3) {i=lPos1; lPos1=lPos3; lPos3=i; }
   if (lPos1>lPos2) {i=lPos1; lPos1=lPos2; lPos2=i; }

   lSign0 = (pSrc1[lPos0] > 0)? 1:-1;
   lSign1 = (pSrc1[lPos1] > 0)? 1:-1;
   lSign2 = (pSrc1[lPos2] > 0)? 1:-1;
   lSign3 = (pSrc1[lPos3] > 0)? 1:-1;

   for (i=0; i<lPos0; i++)
      pDst[i]=0;
   for (; i<lPos1; i++)
      pDst[i]=lSign0*pSrc2[i-lPos0];
   for (; i<lPos2; i++)
      pDst[i]=lSign0*pSrc2[i-lPos0]+lSign1*pSrc2[i-lPos1];
   for (; i<lPos3; i++)
      pDst[i]=lSign0*pSrc2[i-lPos0]+lSign1*pSrc2[i-lPos1]+lSign2*pSrc2[i-lPos2];
   for (; i<SUBFR_LEN; i++)
      pDst[i]=lSign0*pSrc2[i-lPos0]+lSign1*pSrc2[i-lPos1]+lSign2*pSrc2[i-lPos2]+lSign3*pSrc2[i-lPos3];
}

void MSDGetSize(Ipp32s *pDstSize)
{
   *pDstSize = sizeof(CNGmemory);
   return;
}

void MSDInit(char *msdMem)
{
   MusDetectMemory *msdState = (MusDetectMemory *)msdMem;

   ippsZero_16s((short*)msdState,sizeof(MusDetectMemory)>>1) ;
   ippsZero_32f(msdState->MeanRC,10);
   msdState->lMusicCounter=0;
   msdState->fMusicCounter=0.0f;
   msdState->lZeroMusicCounter=0;