/*/////////////////////////////////////////////////////////////////////////////
//
//                  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: GSMAMR speech codec: common utilities.
//
*/

#include "owngsmamr.h"

/****************************************************************************
 *  Function: ownVADPitchDetection_GSMAMR()
 ***************************************************************************/
void ownVADPitchDetection_GSMAMR(IppGSMAMRVad1State *st, short *pTimeVec,
                                 short *vLagCountOld, short *vLagOld)
{
   short lagcount, i;
   lagcount = 0;

   for (i = 0; i < 2; i++) {
      if(abs(*vLagOld - pTimeVec[i]) < LOW_THRESHOLD) lagcount++;
      *vLagOld = pTimeVec[i];
   }

   st->pitchFlag >>= 1;
   if (*vLagCountOld + lagcount >= NUM_THRESHOLD)
      st->pitchFlag = st->pitchFlag | 0x4000;

   *vLagCountOld = lagcount;
}

/***************************************************************************
 *   Function: ownUpdateLTPFlag_GSMAMR()
 *************************************************************************/
void ownUpdateLTPFlag_GSMAMR(IppSpchBitRate rate, int L_Rmax, int L_R0, Ipp16s *vFlagLTP)
{
    short thresh;
    short hi1;
    short lo1;
    int Ltmp;

    if ((rate == IPP_SPCHBR_4750) || (rate == IPP_SPCHBR_5150)) thresh = 18022;
    else if (rate == IPP_SPCHBR_10200) thresh = 19660;
    else thresh = 21299;

    hi1 = L_R0 >> 16;
    lo1 = (L_R0 >> 1) & 0x7fff;
    Ltmp = (hi1 * thresh +  ((lo1 * thresh) >> 15));
    if (L_Rmax > 2*Ltmp) *vFlagLTP = 1;
    else                 *vFlagLTP = 0;

    return;
    /* End of ownUpdateLTPFlag_GSMAMR() */
}

static __ALIGN32 CONST short TableLog2[33] = {
     0,  1455,  2866,  4236,  5568,  6863,  8124,  9352,
 10549, 11716, 12855, 13967, 15054, 16117, 17156, 18172,
 19167, 20142, 21097, 22033, 22951, 23852, 24735, 25603,
 26455, 27291, 28113, 28922, 29716, 30497, 31266, 32023,
 32767 };

 /*************************************************************************
 * Function:   ownLog2_GSMAMR_norm
 *************************************************************************/
void ownLog2_GSMAMR_norm(int inVal, short exp, short *expPart, short *fracPart)
{
  short idx, a, tmp;
  int outVal;

  if( inVal <= 0 ) {
    *expPart = 0;
    *fracPart = 0;
    return;
  }

  *expPart = 30 - exp;
  idx = inVal >> 25;
  a = (inVal >> 10) & 0x7fff;
  idx   -=  32;

  outVal = TableLog2[idx] << 16;
  tmp = TableLog2[idx] - TableLog2[idx+1];
  outVal -= 2 * tmp * a;

  *fracPart =  outVal >> 16;

  return;
  /* End of ownLog2_GSMAMR_norm() */
}
/*************************************************************************
 *  Function:  ownLog2_GSMAMR()
 *************************************************************************/
void ownLog2_GSMAMR(int inVal, short *expPart, short *fracPart)
{
  short exp;

  exp = Norm_32s_I(&inVal);
  ownLog2_GSMAMR_norm (inVal, exp, expPart, fracPart);

  return;
  /* End of ownLog2_GSMAMR() */
}

/*************************************************************************
 *  Function:  ownPow2_GSMAMR
 *************************************************************************/
static __ALIGN32 CONST short TablePow2[33] = {
 16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066,
 19484, 19911, 20347, 20792, 21247, 21713, 22188, 22674,
 23170, 23678, 24196, 24726, 25268, 25821, 26386, 26964,
 27554, 28158, 28774, 29405, 30048, 30706, 31379, 32066,
 32767 };

int ownPow2_GSMAMR(short expPart, short fracPart)
{
  short exp, idx, a, tmp;
  int inVal, outVal, temp;

  exp = 30 - expPart;
  if (exp > 31) return 0;
  idx = fracPart >> 10;
  a   = (short)((fracPart << 5) & 0x7fff);

  inVal = TablePow2[idx] << 16;
  tmp = TablePow2[idx] - TablePow2[idx+1];
  inVal = inVal - 2*tmp*a;

  if (exp >= 31) outVal = (inVal < 0) ? -1 : 0;
  else           outVal = inVal >> exp;

  temp = 1 << (exp-1);
  if(inVal & temp) outVal++;

  return(outVal);
  /* End of ownPow2_GSMAMR() */
}

/* table[i] = sqrt((i+16)*2^-6) * 2^15, i.e. sqrt(x) scaled Q15 */
static __ALIGN32 CONST short TableSqrt[49] =
{
 16384, 16888, 17378, 17854, 18318, 18770, 19212, 19644,
 20066, 20480, 20886, 21283, 21674, 22058, 22435, 22806,
 23170, 23530, 23884, 24232, 24576, 24915, 25249, 25580,
 25905, 26227, 26545, 26859, 27170, 27477, 27780, 28081,
 28378, 28672, 28963, 29251, 29537, 29819, 30099, 30377,
 30652, 30924, 31194, 31462, 31727, 31991, 32252, 32511,
 32767};

/********************************************************************************
 *  Function: ownSqrt_Exp_GSMAMR()
 ********************************************************************************/

int ownSqrt_Exp_GSMAMR (int inVal, short *exp)
{
    short idx, a, tmp;
    int outVal;

    if (inVal <= 0) { *exp = 0; return 0; }

    outVal = inVal;
    *exp = Norm_32s_I(&outVal) & 0xFFFE;
    inVal <<= *exp;

    idx = inVal >> 25;
    a = (inVal >> 10) & 0x7fff;

    idx -= 16;
    outVal = TableSqrt[idx] << 16;
    tmp = TableSqrt[idx] - TableSqrt[idx+1];
    outVal = outVal - 2*tmp*a;

    return (outVal);
    /* End of ownSqrt_Exp_GSMAMR() */
}

/*************************************************************************
 *  Function: Reorder_lsf()
 *************************************************************************/
void ownReorderLSFVec_GSMAMR(short *lsf, short minDistance, short len)
{
    int i;
    short lsf_min;

    lsf_min = minDistance;

    for (i = 0; i < len; i++)  {
        if(lsf[i] < lsf_min) lsf[i] = lsf_min;
        lsf_min = lsf[i] + minDistance;
    }
}
/*************************************************************************
 *  Function: ownGetMedianElements_GSMAMR()
 *************************************************************************/
short ownGetMedianElements_GSMAMR (short *pPastGainVal, short num)
{
    short i, j, idx = 0;
    short max;
    short medianIndex;
    short tmp[MAX_MED_SIZE];
    short tmp2[MAX_MED_SIZE];

    ippsCopy_16s(pPastGainVal, tmp2, num);

    for (i = 0; i < num; i++) {
        max = -32767;
        for (j = 0; j < num; j++)  {
            if (tmp2[j] >= max) {
                max = tmp2[j];
                idx = j;
            }
        }
        tmp2[idx] = -32768;
        tmp[i] = idx;
    }
    medianIndex = tmp[num >> 1];

    return (pPastGainVal[medianIndex]);
}
/**************************************************************************
*  Proc: ownCtrlDetectBackgroundNoise_GSMAMR()  -
***************************************************************************/
short ownCtrlDetectBackgroundNoise_GSMAMR (short excitation[], short excEnergy, short exEnergyHist[],
                                           short vVoiceHangover, short prevBFI, short carefulFlag)
{
   short exp;
   short testEnergy, scaleFactor, avgEnergy, prevEnergy;
   int t0;

   avgEnergy = ownGetMedianElements_GSMAMR(exEnergyHist, 9);
   prevEnergy = (exEnergyHist[7] + exEnergyHist[8]) >> 1;

   if( exEnergyHist[8] < prevEnergy) prevEnergy = exEnergyHist[8];

   if( excEnergy < avgEnergy && excEnergy > 5) {
      testEnergy = prevEnergy << 2;

      if( vVoiceHangover < 7 || prevBFI != 0 ) testEnergy -= prevEnergy;
      if( avgEnergy > testEnergy) avgEnergy = testEnergy;

      exp = 0;
      for(; excEnergy < 0x4000; exp++) excEnergy <<= 1;
      excEnergy = (16383<<15)/excEnergy;
      t0 = avgEnergy * excEnergy;
      t0 >>= 19 - exp;
      if(t0 > 32767)  t0 = 32767;
      scaleFactor = (short)t0;

      if((carefulFlag != 0) && (scaleFactor > 3072))  scaleFactor = 3072;

      ippsMulC_16s_ISfs(scaleFactor, excitation, SUBFR_SIZE_GSMAMR, 10);
   }

   return 0;
}
/*************************************************************************
 *  Function: ownComputeCodebookGain_GSMAMR
 *************************************************************************/
short ownComputeCodebookGain_GSMAMR (short *pTargetVec, short *pFltVec)
{
    short i;
    short xy, yy, exp_xy, exp_yy, gain;
    IPP_ALIGNED_ARRAY(16, short, pFltVecScale, SUBFR_SIZE_GSMAMR);
    int s;

    ippsRShiftC_16s(pFltVec, 1, pFltVecScale, SUBFR_SIZE_GSMAMR);
    ippsDotProd_16s32s_Sfs(pTargetVec, pFltVecScale, SUBFR_SIZE_GSMAMR, &s, 0);

    if(s == 0) s = 1;
    s <<= 1;
    exp_xy = Norm_32s_I(&s);
    xy = (short)(s >> 16);
    if(xy <= 0) return ((short) 0);

    ippsDotProd_16s32s_Sfs(pFltVecScale, pFltVecScale, SUBFR_SIZE_GSMAMR, &s, -1);

    exp_yy = Norm_32s_I(&s);
    yy = (short)(s >> 16);
    xy >>= 1;
    gain = (yy > 0)? (xy<<15)/yy : IPP_MAX_16S;

    i = exp_xy + 5;
    i -= exp_yy;
    gain = (gain >> i) << 1;

    return (gain);
}
/*************************************************************************
 *  Function: ownGainAdaptAlpha_GSMAMR
 *************************************************************************/
void ownGainAdaptAlpha_GSMAMR(short *vOnSetQntGain, short *vPrevAdaptOut, short *vPrevGainZero,
                              short *a_LTPHistoryGain, short ltpg, short gainCode, short *alpha)
{
    short adapt;
    short result;
    short filt;
    short tmp, i;
    int temp;

    if (ltpg <= LTP_GAIN_LOG10_1) adapt = 0;
    else if (ltpg <= LTP_GAIN_LOG10_2) adapt = 1;
    else adapt = 2;

    tmp = gainCode >> 1;
    if (gainCode & 1) tmp++;

    if (tmp > *vPrevGainZero && gainCode > 200) *vOnSetQntGain = 8;
    else if (*vOnSetQntGain != 0) *vOnSetQntGain -= 1;

    if ((*vOnSetQntGain != 0) && (adapt < 2)) adapt += 1;

    a_LTPHistoryGain[0] = ltpg;
    filt = ownGetMedianElements_GSMAMR(a_LTPHistoryGain, 5);

    if(adapt == 0) {
       if (filt > 5443)   result = 0;
       else if (filt < 0)  result = 16384;
       else {
           temp = (24660 * filt)>>13;
           result = 16384 - temp;
       }
    } else result = 0;

    if (*vPrevAdaptOut == 0) result >>= 1;
    *alpha = result;

    *vPrevAdaptOut = result;
    *vPrevGainZero = gainCode;

    for (i = NUM_MEM_LTPG-1; i > 0; i--)
        a_LTPHistoryGain[i] = a_LTPHistoryGain[i-1];

}
/**************************************************************************
*  Function: ownCBGainAverage_GSMAMR()
**************************************************************************/
short ownCBGainAverage_GSMAMR(short *a_GainHistory, short *vHgAverageVar, short *vHgAverageCount,
                              IppSpchBitRate rate, short gainCode, short *lsp, short *lspAver,
                              short badFrame, short vPrevBadFr, short potDegBadFrame, short vPrevDegBadFr,
                              short vBackgroundNoise, short vVoiceHangover)
{
   short i;
   short cbGainMix, diff, bgMix, cbGainMean;
   int L_sum;
   short tmp[LP_ORDER_SIZE], tmp1, tmp2, shift1, shift2, shift;

   cbGainMix = gainCode;
   for (i = 0; i < (CBGAIN_HIST_SIZE-1); i++)
      a_GainHistory[i] = a_GainHistory[i+1];

   a_GainHistory[CBGAIN_HIST_SIZE-1] = gainCode;

   for (i = 0; i < LP_ORDER_SIZE; i++) {
      tmp1 = abs(lspAver[i] - lsp[i]);
      shift1 = Exp_16s(tmp1) - 1;
      tmp1 <<= shift1;
      shift2 = Exp_16s(lspAver[i]);
      tmp2 = lspAver[i] << shift2;
      tmp[i] = (tmp2>0)? (tmp1<<15)/tmp2 : IPP_MAX_16S;