/*
 *===================================================================
 *  3GPP AMR Wideband Floating-point Speech Codec
 *===================================================================
 */
#include <memory.h>
#include "typedef.h"
#include "dec_util.h"

#define L_SUBFR         64       /* Subframe size */
#define L_LTPHIST       5
#define ONE_PER_3       10923
#define ONE_PER_LTPHIST 6554
#define UP_SAMP         4
#define L_INTERPOL2     16

extern const Word16 D_ROM_inter4_2[];
extern const Word16 D_ROM_pdown_unusable[];
extern const Word16 D_ROM_pdown_usable[];
extern const Word16 D_ROM_cdown_unusable[];
extern const Word16 D_ROM_cdown_usable[];
extern const Word16 D_ROM_qua_gain6b[];
extern const Word16 D_ROM_qua_gain7b[];

/*
 * D_GAIN_init
 *
 * Parameters:
 *    mem         O: static memory
 *
 * Function:
 *    Initialisation of 2nd order quantiser energy predictor.
 *
 * Returns:
 *    void
 */
void D_GAIN_init(Word16 *mem)
{

   /* 4nd order quantizer energy predictor (init to -14.0 in Q10) */
   mem[0] = -14336;   /* past_qua_en[0] */
   mem[1] = -14336;   /* past_qua_en[1] */
   mem[2] = -14336;   /* past_qua_en[2] */
   mem[3] = -14336;   /* past_qua_en[3] */
   /*
    * mem[4] = 0;       past_gain_pit
    * mem[5] = 0;       past_gain_code
    * mem[6] = 0;       prev_gc
    * mem[7 - 11] = 0;  pbuf[i]
    * mem[12 - 16] = 0; gbuf[i]
    * mem[17 - 21] = 0; pbuf2[i]
    */
   memset(&mem[4], 0, 18 * sizeof(Word16));

   mem[22] = 21845;   /* seed */
   return;
}


/*
 * D_GAIN_median
 *
 * Parameters:
 *    buf            I: previous gains
 *
 * Function:
 *    Median of gains
 *
 * Returns:
 *    median of 5 previous gains
 */
static Word16 D_GAIN_median(Word16 x[])
{
   Word16 x1, x2, x3, x4, x5;
   Word16 tmp;
   x1 = x[ - 2];
   x2 = x[ - 1];
   x3 = x[0];
   x4 = x[1];
   x5 = x[2];

   if(x2 < x1)
   {
      tmp = x1;
      x1 = x2;
      x2 = tmp;
   }

   if(x3 < x1)
   {
      tmp = x1;
      x1 = x3;
      x3 = tmp;
   }

   if(x4 < x1)
   {
      tmp = x1;
      x1 = x4;
      x4 = tmp;
   }

   if(x5 < x1)
   {
      x5 = x1;
   }

   if(x3 < x2)
   {
      tmp = x2;
      x2 = x3;
      x3 = tmp;
   }

   if(x4 < x2)
   {
      tmp = x2;
      x2 = x4;
      x4 = tmp;
   }

   if(x5 < x2)
   {
      x5 = x2;
   }

   if(x4 < x3)
   {
      x3 = x4;
   }

   if(x5 < x3)
   {
      x3 = x5;
   }

   return(x3);
}


/*
 * D_GAIN_decode
 *
 * Parameters:
 *    index             I: Quantization index
 *    nbits             I: number of bits (6 or 7)
 *    code              I: Innovative code vector
 *    L_subfr           I: Subframe size
 *    gain_pit          O: (Q14) Quantized pitch gain
 *    gain_code         O: (Q16) Quantized codebook gain
 *    bfi               I: Bad frame indicator
 *    prev_bfi          I: Previous BF indicator
 *    state             I: State of BFH
 *    unusable_frame    I: UF indicator
 *    vad_hist          I: number of non-speech frames
 *    mem             I/O: static memory (4 words)
 *
 *
 * Function:
 *    Decoding of pitch and codebook gains
 *
 * Returns:
 *    void
 */
void D_GAIN_decode(Word16 index, Word16 nbits, Word16 code[], Word16 *gain_pit,
                   Word32 *gain_cod, Word16 bfi, Word16 prev_bfi,
                   Word16 state, Word16 unusable_frame, Word16 vad_hist,
                   Word16 *mem)
{

   Word32 gcode0, qua_ener, L_tmp;
   const Word16 * p;
   Word16 *past_gain_pit, *past_gain_code, *past_qua_en, *prev_gc;
   Word16 *gbuf, *pbuf, *pbuf2;
   Word16 i, tmp, exp, frac, exp_gcode0, gcode_inov;
   Word16 g_code;

   past_qua_en = mem;
   past_gain_pit = mem + 4;
   past_gain_code = mem + 5;
   prev_gc = mem + 6;
   pbuf = mem + 7;
   gbuf = mem + 12;
   pbuf2 = mem + 17;

   /*
    * Find energy of code and compute:
    *
    *    L_tmp = 1.0 / sqrt(energy of code/ L_subfr)
    */
   L_tmp = D_UTIL_dot_product12(code, code, L_SUBFR, &exp);
   exp = (Word16)(exp - (18 + 6));   /* exp: -18 (code in Q9), -6 (/L_subfr) */
   D_UTIL_normalised_inverse_sqrt(&L_tmp, &exp);

   if(exp > 3)
   {
      L_tmp <<= (exp - 3);
   }
   else
   {
      L_tmp >>= (3 - exp);
   }

   gcode_inov = (Word16)(L_tmp >>16);   /* g_code_inov in Q12 */

   /*
    * Case of erasure.
    */
   if(bfi != 0)
   {
      tmp = D_GAIN_median(&pbuf[2]);
      *past_gain_pit = tmp;

      if(*past_gain_pit > 15565)
      {
         *past_gain_pit = 15565;   /* 0.95 in Q14 */
      }

      if(unusable_frame != 0)
      {
         *gain_pit =
            (Word16)((D_ROM_pdown_unusable[state] * *past_gain_pit) >> 15);
      }
      else
      {
         *gain_pit =
            (Word16)((D_ROM_pdown_usable[state] * *past_gain_pit) >> 15);
      }

      tmp = D_GAIN_median(&gbuf[2]);

      if(vad_hist > 2)
      {
         *past_gain_code = tmp;
      }
      else
      {
         if(unusable_frame != 0)
         {
            *past_gain_code =
               (Word16)((D_ROM_cdown_unusable[state] * tmp) >> 15);
         }
         else
         {
            *past_gain_code =
               (Word16)((D_ROM_cdown_usable[state] * tmp) >> 15);
         }
      }

      /* update table of past quantized energies */
      L_tmp = past_qua_en[0] + past_qua_en[1]+ past_qua_en[2] + past_qua_en[3];
      qua_ener = L_tmp >> 2;
      qua_ener = qua_ener - 3072;   /* -3 in Q10 */

      if(qua_ener < - 14336)
      {
         qua_ener = -14336;   /* -14 in Q10 */
      }

      past_qua_en[3] = past_qua_en[2];
      past_qua_en[2] = past_qua_en[1];
      past_qua_en[1] = past_qua_en[0];
      past_qua_en[0] = (Word16)qua_ener;

      for(i = 1; i < 5; i++)
      {
         gbuf[i - 1] = gbuf[i];
      }
      gbuf[4] = *past_gain_code;

      for(i = 1; i < 5; i++)
      {
         pbuf[i - 1] = pbuf[i];
      }
      pbuf[4] = *past_gain_pit;

      /* adjust gain according to energy of code */
      /* past_gain_code(Q3) * gcode_inov(Q12) => Q16 */
      *gain_cod = (*past_gain_code * gcode_inov) << 1;

      return;
   }

   /*
    * Compute gcode0.
    *  = Sum(i=0,1) pred[i]*past_qua_en[i] + mean_ener - ener_code
    */

   /* MEAN_ENER in Q24 = 0x1e000000 */
   /* MA prediction coeff = {0.5, 0.4, 0.3, 0.2} in Q13 */
   L_tmp = 0xF000000 + (4096 * past_qua_en[0]); /* Q13*Q10 -> Q24 */
   L_tmp = L_tmp + (3277 * past_qua_en[1]);     /* Q13*Q10 -> Q24 */
   L_tmp = L_tmp + (2458 * past_qua_en[2]);     /* Q13*Q10 -> Q24 */
   L_tmp = L_tmp + (1638 * past_qua_en[3]);     /* Q13*Q10 -> Q24 */
   gcode0 = L_tmp >> 15;               /* From Q24 to Q8 */

   /*
    * gcode0 = pow(10.0, gcode0/20)
    *        = pow(2, 3.321928*gcode0/20)
    *        = pow(2, 0.166096*gcode0)
    */
   L_tmp = (gcode0 * 5443) >> 7;
   /* *0.166096 in Q15 -> Q24, From Q24 to Q16 */
   D_UTIL_l_extract(L_tmp, &exp_gcode0, &frac);
   /* Extract exponant of gcode0  */
   gcode0 = D_UTIL_pow2(14, frac); /* Put 14 as exponant so that */

   /*
    * output of Pow2() will be:
    * 16384 < Pow2() <= 32767
    */
   exp_gcode0 = (Word16)(exp_gcode0 - 14);

   /* Read the quantized gains */
   if(nbits == 6)
   {
      p = &D_ROM_qua_gain6b[(index << 1)];
   }
   else
   {
      p = &D_ROM_qua_gain7b[(index << 1)];
   }

   *gain_pit = *p++; /* selected pitch gain in Q14 */
   g_code = *p++;    /* selected code gain in Q11  */
   L_tmp = g_code * gcode0;
   exp_gcode0 += 5;

   if(exp_gcode0 >= 0)
   {
      *gain_cod = L_tmp << exp_gcode0;    /* gain of code in Q16 */
   }
   else
   {
      *gain_cod = L_tmp >> -exp_gcode0;   /* gain of code in Q16 */
   }

   if(prev_bfi == 1)
   {
      L_tmp = (*prev_gc * 5120) << 1;  /* prev_gc(Q3) * 1.25(Q12) = Q16 */

      /* if((*gain_cod > ((*prev_gc) * 1.25)) && (*gain_cod > 100.0)) */
      if((*gain_cod > L_tmp) & (*gain_cod > 6553600))
      {
         *gain_cod = L_tmp;
      }
   }

   /* keep past gain code in Q3 for frame erasure (can saturate) */
   L_tmp = (*gain_cod + 0x1000) >> 13;

   if(L_tmp < 32768)
   {
      *past_gain_code = (Word16)L_tmp;
   }
   else
   {
      *past_gain_code = 32767;
   }

   *past_gain_pit = *gain_pit;
   *prev_gc = *past_gain_code;

   for(i = 1; i < 5; i++)
   {
      gbuf[i - 1] = gbuf[i];
   }
   gbuf[4] = *past_gain_code;

   for(i = 1; i < 5; i++)
   {
      pbuf[i - 1] = pbuf[i];
   }
   pbuf[4] = *past_gain_pit;

   for(i = 1; i < 5; i++)
   {
      pbuf2[i - 1] = pbuf2[i];
   }
   pbuf2[4] = *past_gain_pit;

   /* adjust gain according to energy of code */
   D_UTIL_l_extract(*gain_cod, &exp, &frac);
   L_tmp = D_UTIL_mpy_32_16(exp, frac, gcode_inov);

   if(L_tmp < 0xFFFFFFF)
   {
      *gain_cod = (L_tmp << 3);   /* gcode_inov in Q12 */
   }
   else
   {
      *gain_cod = 0x7FFFFFFF;
   }

   /*
    * qua_ener = 20*log10(g_code)
    *          = 6.0206*log2(g_code)
    *          = 6.0206*(log2(g_codeQ11) - 11)
    */
   L_tmp = (Word32)(g_code);
   D_UTIL_log2(L_tmp, &exp, &frac);
   exp = (Word16)(exp - 11);
   L_tmp = D_UTIL_mpy_32_16(exp, frac, 24660);   /* x 6.0206 in Q12 */
   qua_ener = L_tmp >>3;   /* result in Q10 */

   /* update table of past quantized energies */
   past_qua_en[3] = past_qua_en[2];
   past_qua_en[2] = past_qua_en[1];
   past_qua_en[1] = past_qua_en[0];
   past_qua_en[0] = (Word16)qua_ener;

   return;
}


/*
 * D_GAIN_adaptive_control
 *
 * Parameters:
 *    sig_in            I: postfilter input signal
 *    sig_out         I/O: postfilter output signal
 *    l_trm             I: subframe size
 *
 * Function:
 *    Adaptive gain control is used to compensate for
 *    the gain difference between the non-emphasized excitation and
 *    emphasized excitation.
 *
 * Returns:
 *    void
 */
void D_GAIN_adaptive_control(Word16 *sig_in, Word16 *sig_out, Word16 l_trm)
{
   Word32 s, temp, i, exp;
   Word32 gain_in, gain_out, g0;

   /* calculate gain_out with exponent */
   temp = sig_out[0] >> 2;
   s = temp * temp;

   for(i = 1; i < l_trm; i++)
   {
      temp = sig_out[i] >> 2;
      s += temp * temp;
   }

   s <<= 1;

   if(s == 0)
   {
      return;
   }
   exp = (D_UTIL_norm_l(s) - 1);

   if(exp >= 0)
   {
      gain_out = ((s << exp) + 0x8000) >> 16;
   }
   else
   {
      gain_out = ((s >> -exp) + 0x8000) >> 16;
   }

   /* calculate gain_in with exponent */
   temp = sig_in[0] >> 2;
   s = temp * temp;

   for(i = 1; i < l_trm; i++)
   {
      temp = sig_in[i] >> 2;
      s += temp * temp;
   }

   s <<= 1;

   if(s == 0)
   {
      g0 = 0;
   }
   else
   {
      i = D_UTIL_norm_l(s);