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

#define M            16             /* Order of LP filter                  */
#define MP1          (M + 1)
#define M16k         20
#define NC16k        (M16k / 2)
#define MU           10923          /* Prediction factor (1.0/3.0) in Q15  */
#define L_MEANBUF    3
#define ALPHA        29491          /* 0. 9 in Q15                         */
#define ONE_ALPHA    (32768-ALPHA)  /* (1.0 - ALPHA) in Q15                */
#define ORDER        16             /* order of linear prediction filter   */
#define ISF_GAP      128            /* 50 Hz */
#define INV_LENGTH   2731           /* 1/12                                */

extern const Word16 D_ROM_dico1_isf[];
extern const Word16 D_ROM_dico2_isf[];
extern const Word16 D_ROM_dico21_isf_36b[];
extern const Word16 D_ROM_dico22_isf_36b[];
extern const Word16 D_ROM_dico23_isf_36b[];
extern const Word16 D_ROM_dico21_isf[];
extern const Word16 D_ROM_dico22_isf[];
extern const Word16 D_ROM_dico23_isf[];
extern const Word16 D_ROM_dico24_isf[];
extern const Word16 D_ROM_dico25_isf[];
extern const Word16 D_ROM_dico1_isf_noise[];
extern const Word16 D_ROM_dico2_isf_noise[];
extern const Word16 D_ROM_dico3_isf_noise[];
extern const Word16 D_ROM_dico4_isf_noise[];
extern const Word16 D_ROM_dico5_isf_noise[];
extern const Word16 D_ROM_mean_isf[];
extern const Word16 D_ROM_mean_isf_noise[];
extern const Word16 D_ROM_cos[];


/*
 * D_LPC_isf_reorder
 *
 * Parameters:
 *    isf          I/O: vector of isfs
 *    min_dist       I: quantized ISFs (in frequency domain)
 *    n              I: LPC order
 *
 * Function:
 *    To make sure that the  isfs are properly order and to keep a certain
 *    minimum distance between consecutive isfs.
 *
 * Returns:
 *    void
 */
static void D_LPC_isf_reorder(Word16 *isf, Word16 min_dist, Word16 n)
{
   Word32 i, isf_min;

   isf_min = min_dist;

   for(i = 0; i < n - 1; i++)
   {
      if(isf[i] < isf_min)
      {
         isf[i] = (Word16)isf_min;
      }
      isf_min = isf[i] + min_dist;
   }

   return;
}


/*
 * D_LPC_isf_noise_d
 *
 * Parameters:
 *    indice         I: indices of the selected codebook entries
 *    isf_q          O: quantized ISFs (in frequency domain)
 *
 * Function:
 *    Decoding of ISF parameters
 *
 * Returns:
 *    void
 */
void D_LPC_isf_noise_d(Word16 *indice, Word16 *isf_q)
{
   Word32 i;

   for(i = 0; i < 2; i++)
   {
      isf_q[i] = D_ROM_dico1_isf_noise[indice[0] * 2 + i];
   }

   for(i = 0; i < 3; i++)
   {
      isf_q[i + 2] = D_ROM_dico2_isf_noise[indice[1] * 3 + i];
   }

   for(i = 0; i < 3; i++)
   {
      isf_q[i + 5] = D_ROM_dico3_isf_noise[indice[2] * 3 + i];
   }

   for(i = 0; i < 4; i++)
   {
      isf_q[i + 8] = D_ROM_dico4_isf_noise[indice[3] * 4 + i];
   }

   for(i = 0; i < 4; i++)
   {
      isf_q[i + 12] = D_ROM_dico5_isf_noise[indice[4] * 4 + i];
   }

   for(i = 0; i < ORDER; i++)
   {
      isf_q[i] = (Word16)(isf_q[i]+ D_ROM_mean_isf_noise[i]);
   }

   D_LPC_isf_reorder(isf_q, ISF_GAP, ORDER);

   return;
}


/*
 * D_LPC_isf_isp_conversion
 *
 * Parameters:
 *    isp            O: (Q15) isp[m] (range: -1<=val<1)
 *    isf            I: (Q15) isf[m] normalized (range: 0.0 <= val <= 0.5)
 *    m              I: LPC order
 *
 * Function:
 *    Transformation isf to isp
 *
 *    ISP are immitance spectral pair in cosine domain (-1 to 1).
 *    ISF are immitance spectral pair in frequency domain (0 to 6400).
 * Returns:
 *    void
 */
void D_LPC_isf_isp_conversion(Word16 isf[], Word16 isp[], Word16 m)
{
   Word32 i, ind, offset, tmp;

   for(i = 0; i < m - 1; i++)
   {
      isp[i] = isf[i];
   }
   isp[m - 1] = (Word16)(isf[m - 1] << 1);

   for(i = 0; i < m; i++)
   {
      ind = isp[i] >> 7;         /* ind = b7-b15 of isf[i]     */
      offset = isp[i] & 0x007f;  /* offset = b0-b6 of isf[i]   */

      /* isp[i] = table[ind]+ ((table[ind+1]-table[ind])*offset) / 128 */
      tmp = (D_ROM_cos[ind + 1] - D_ROM_cos[ind]) * offset;
      isp[i] = (Word16)(D_ROM_cos[ind] + (tmp >> 7));
   }
   return;
}


/*
 * D_LPC_isp_pol_get
 *
 * Parameters:
 *    isp            I: Immitance spectral pairs (cosine domaine)
 *    f              O: the coefficients of F1 or F2
 *    n              I: no of coefficients (m/2)
 *    k16            I: 16k flag
 *
 * Function:
 *    Find the polynomial F1(z) or F2(z) from the ISPs.
 *    This is performed by expanding the product polynomials:
 *
 *    F1(z) =   product   ( 1 - 2 isp_i z^-1 + z^-2 )
 *            i=0,2,4,6,8
 *    F2(z) =   product   ( 1 - 2 isp_i z^-1 + z^-2 )
 *             i=1,3,5,7
 *
 *    where isp_i are the ISPs in the cosine domain.
 *
 * Returns:
 *    void
 */
static void D_LPC_isp_pol_get(Word16 *isp, Word32 *f, Word32 n, Word16 k16)
{
   Word32 i, j, t0, s1, s2;
   Word16 hi, lo;

   s1 = 8388608;
   s2 = 512;

   if(k16)
   {
      s1 >>= 2;
      s2 >>= 2;
   }

   /* All computation in Q23 */
   f[0] = s1;              /* f[0] = 1.0; in Q23         */
   f[1] = isp[0] * (-s2);  /* f[1] = -2.0*isp[0] in Q23  */
   f += 2;                 /* Advance f pointer          */
   isp += 2;               /* Advance isp pointer        */

   for(i = 2; i <= n; i++)
   {
      *f = f[ - 2];

      for(j = 1; j < i; j++, f--)
      {
         D_UTIL_l_extract(f[- 1], &hi, &lo);
         t0 = D_UTIL_mpy_32_16(hi, lo, *isp);   /* t0 = f[-1] * isp */
         t0 = (t0 << 1);
         *f = (*f - t0);         /* *f -= t0    */
         *f = (*f + f[ - 2]);    /* *f += f[-2] */
      }

      *f = *f - (*isp * s2);     /* *f -= isp << 8 */
      f += i;     /* Advance f pointer   */
      isp += 2;   /* Advance isp pointer */
   }

   return;
}


/*
 * D_LPC_isp_a_conversion
 *
 * Parameters:
 *    isp            I: (Q15) Immittance spectral pairs
 *    a              O: (Q12) Predictor coefficients (order = M)
 *    m              I: order of LP filter
 *
 * Function:
 *    Convert ISPs to predictor coefficients a[]
 *
 * Returns:
 *    void
 */
void D_LPC_isp_a_conversion(Word16 isp[], Word16 a[], Word16 m)
{
   Word32 j, i, nc;
   Word32 f1[NC16k + 1], f2[NC16k];
   Word32 t0;
   Word16 hi, lo;

   nc = m >> 1;

   if(nc > 8)
   {
      D_LPC_isp_pol_get(&isp[0], f1, nc, 1);

      for(i = 0; i <= nc; i++)
      {
         f1[i] = (f1[i] << 2);
      }
   }
   else
   {
      D_LPC_isp_pol_get(&isp[0], f1, nc, 0);
   }

   if(nc > 8)
   {
      D_LPC_isp_pol_get(&isp[1], f2, nc - 1, 1);

      for(i = 0; i <= nc - 1; i++)
      {
         f2[i] = (f2[i] << 2);
      }
   }
   else
   {
      D_LPC_isp_pol_get(&isp[1], f2, nc - 1, 0);
   }

   /*
    *  Multiply F2(z) by (1 - z^-2)
    */
   for(i = nc - 1; i > 1; i--)
   {
      f2[i] = f2[i] - f2[i - 2];   /* f2[i] -= f2[i-2]; */
   }

   /*
    *  Scale F1(z) by (1+isp[m-1]) and F2(z) by (1-isp[m-1])
    */
   for(i = 0; i < nc; i++)
   {
      /* f1[i] *= (1.0 + isp[M-1]); */
      D_UTIL_l_extract(f1[i], &hi, &lo);
      t0 = D_UTIL_mpy_32_16(hi, lo, isp[m - 1]);
      f1[i] = f1[i] + t0;

      /* f2[i] *= (1.0 - isp[M-1]); */
      D_UTIL_l_extract(f2[i], &hi, &lo);
      t0 = D_UTIL_mpy_32_16(hi, lo, isp[m - 1]);
      f2[i] = f2[i] - t0;
   }

   /*
    *  A(z) = (F1(z)+F2(z))/2
    *  F1(z) is symmetric and F2(z) is antisymmetric
    */

   /* a[0] = 1.0; */
   a[0] = 4096;

   for(i = 1, j = m - 1; i < nc; i++, j--)
   {
      /* a[i] = 0.5*(f1[i] + f2[i]); */
      t0 = f1[i] + f2[i];   /* f1[i] + f2[i] */
      a[i] = (Word16)((t0 + 0x800) >> 12);   /* from Q23 to Q12 and * 0.5 */

      /* a[j] = 0.5*(f1[i] - f2[i]); */
      t0 = (f1[i] - f2[i]);   /* f1[i] - f2[i] */
      a[j] = (Word16)((t0 + 0x800) >> 12);   /* from Q23 to Q12 and * 0.5 */
   }

   /* a[NC] = 0.5*f1[NC]*(1.0 + isp[M-1]); */
   D_UTIL_l_extract(f1[nc], &hi, &lo);
   t0 = D_UTIL_mpy_32_16(hi, lo, isp[m - 1]);
   t0 = (f1[nc] + t0);
   a[nc] = (Word16)((t0 + 0x800) >> 12);  /* from Q23 to Q12 and * 0.5 */

   /* a[m] = isp[m-1]; */
   a[m] = (Word16)((isp[m - 1] + 0x4) >> 3); /* from Q15 to Q12 */

   return;
}


/*
 * D_LPC_a_weight
 *
 * Parameters:
 *    a              I: LP filter coefficients
 *    ap             O: weighted LP filter coefficients
 *    gamma          I: weighting factor
 *    m              I: order of LP filter
 *
 * Function:
 *    Weighting of LP filter coefficients, ap[i] = a[i] * (gamma^i).
 *
 * Returns:
 *    void
 */
void D_LPC_a_weight(Word16 a[], Word16 ap[], Word16 gamma, Word16 m)
{
   Word32 i, fac;

   ap[0] = a[0];
   fac = gamma;

   for(i = 1; i < m; i++)
   {
      ap[i] = (Word16)(((a[i] * fac) + 0x4000) >> 15);
      fac = ((fac * gamma)  + 0x4000) >> 15;
   }

   ap[m] = (Word16)(((a[m] * fac) + 0x4000) >> 15);

   return;
}


/*
 * D_LPC_isf_2s3s_decode
 *
 * Parameters:
 *    indice            I: quantisation indices
 *    isf_q             O: quantised ISFs in the cosine domain
 *    past_isfq       I/O: past ISF quantizer
 *    isfold            I: past quantised ISF
 *    isf_buf           O: isf buffer
 *    bfi               I: Bad frame indicator
 *
 * Function:
 *    Decoding of ISF parameters.
 *
 * Returns:
 *    void
 */
void D_LPC_isf_2s3s_decode(Word16 *indice, Word16 *isf_q, Word16 *past_isfq,
                           Word16 *isfold, Word16 *isf_buf, Word16 bfi)
{

   Word32 ref_isf[M];
   Word32 L_tmp, i, j;
   Word16 tmp;

   if(bfi == 0) /* Good frame */
   {
      for(i = 0; i < 9; i++)
      {
         isf_q[i] = D_ROM_dico1_isf[indice[0] * 9 + i];
      }

      for(i = 0; i < 7; i++)
      {
         isf_q[i + 9] = D_ROM_dico2_isf[indice[1] * 7 + i];
      }

      for(i = 0; i < 5; i++)
      {
         isf_q[i] =
            (Word16)(isf_q[i] + D_ROM_dico21_isf_36b[indice[2] * 5 + i]);
      }

      for(i = 0; i < 4; i++)
      {