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

#include "typedef.h"
#include "enc_util.h"

#define ORDER        16          /* order of linear prediction filter   */
#define ISF_GAP      128         /* 50                                  */
#define M            16
#define M16k         20          /* Order of LP filter                  */
#define MP1          (M+1)
#define NC16k        (M16k / 2)
#define MU           10923       /* Prediction factor (1.0/3.0) in Q15  */
#define F_MU         (1.0 / 3.0) /* prediction factor                   */
#define N_SURV_MAX   4           /* 4 survivors max                     */

#ifndef PI
#define PI           3.141592654
#endif

/* isp_isf_conversion */
#define SCALE1  (6400.0/PI)

/* chebyshev */
#define  NO_ITER   4    /* no of iterations for tracking the root */
#define  NO_POINTS 100

#define SIZE_BK1        256
#define SIZE_BK2        256
#define SIZE_BK21       64
#define SIZE_BK22       128
#define SIZE_BK23       128
#define SIZE_BK24       32
#define SIZE_BK25       32
#define SIZE_BK21_36b   128
#define SIZE_BK22_36b   128
#define SIZE_BK23_36b   64
#define SIZE_BK_NOISE1  64
#define SIZE_BK_NOISE2  64
#define SIZE_BK_NOISE3  64
#define SIZE_BK_NOISE4  32
#define SIZE_BK_NOISE5  32

extern const Word16 E_ROM_mean_isf[];
extern const Word16 E_ROM_cos[];

extern const Float32 E_ROM_dico1_isf[];
extern const Float32 E_ROM_dico2_isf[];
extern const Float32 E_ROM_dico21_isf[];
extern const Float32 E_ROM_dico22_isf[];
extern const Float32 E_ROM_dico23_isf[];
extern const Float32 E_ROM_dico24_isf[];
extern const Float32 E_ROM_dico25_isf[];
extern const Float32 E_ROM_dico21_isf_36b[];
extern const Float32 E_ROM_dico22_isf_36b[];
extern const Float32 E_ROM_dico23_isf_36b[];
extern const Float32 E_ROM_f_mean_isf[];
extern const Float32 E_ROM_lag_window[];
extern const Float32 E_ROM_grid[];
extern const Float32 E_ROM_f_interpol_frac[];

/*
 * E_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 E_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;
}

/*
 * E_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 E_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--)
      {
         E_UTIL_l_extract(f[- 1], &hi, &lo);
         t0 = E_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;
}

static void E_LPC_f_isp_pol_get(Float32 isp[], Float32 f[], Word32 n)
{
   Float32 b;
   Word32 i, j;

   f[0] = 1;
   b = (Float32)(-2.0 * *isp);
   f[1] = b;

   for (i = 2; i <= n; i++)
   {
      isp += 2;
      b = (Float32)(-2.0 * *isp);
      f[i] = (Float32)(b * f[i - 1] + 2.0 * f[i - 2]);

      for (j = i - 1; j > 1; j--)
      {
         f[j] += b * f[j - 1] + f[j - 2];
      }

      f[1] += b;
   }

   return;
}

/*
 * E_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 E_LPC_isp_a_conversion(Word16 isp[], Word16 a[], Word16 m)
{
   Word32 f1[NC16k + 1], f2[NC16k];
   Word32 i, j, nc, t0;
   Word16 hi, lo;

   nc = m >> 1;

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

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

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

      for (i = 0; i <= nc - 1; i++)
      {
         f2[i] = (f2[i] << 2);
      }
   }
   else
   {
      E_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]); */

      E_UTIL_l_extract(f1[i], &hi, &lo);
      t0 = E_UTIL_mpy_32_16(hi, lo, isp[m - 1]);
      f1[i] = f1[i] + t0;

      /* f2[i] *= (1.0 - isp[M-1]); */

      E_UTIL_l_extract(f2[i], &hi, &lo);
      t0 = E_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]); */
   E_UTIL_l_extract(f1[nc], &hi, &lo);
   t0 = E_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;
}

void E_LPC_f_isp_a_conversion(Float32 *isp, Float32 *a, Word32 m)
{
   Float32 f1[(M16k / 2) + 1], f2[M16k / 2];
   Word32 i, j, nc;

   nc = m / 2;

   /*
    *  Find the polynomials F1(z) and F2(z)
    */

   E_LPC_f_isp_pol_get(&isp[0], f1, nc);
   E_LPC_f_isp_pol_get(&isp[1], f2, nc-1);

   /*
    *  Multiply F2(z) by (1 - z^-2)
    */
   for (i = (nc - 1); i > 1; i--)
   {
      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] *= (Float32)(1.0 + isp[m - 1]);
      f2[i] *= (Float32)(1.0 - isp[m - 1]);
   }

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

   a[0] = 1.0;

   for (i = 1, j = m - 1; i < nc; i++, j--)
   {
      a[i] = (Float32)(0.5 * (f1[i] + f2[i]));
      a[j] = (Float32)(0.5 * (f1[i] - f2[i]));
   }

   a[nc] = (Float32)(0.5 * f1[nc] * (1.0 + isp[m - 1]));
   a[m] = isp[m - 1];

   return;
}

/*
 * E_LPC_int_isp_find
 *
 * Parameters:
 *    isp_old           I: isps from past frame
 *    isp_new           I: isps from present frame
 *    frac              I: (Q15) fraction for 3 first subfr
 *    Az                O: LP coefficients in 4 subframes
 *
 * Function:
 *    Find the Word32erpolated ISP parameters for all subframes.
 *
 * Returns:
 *    void
 */
void E_LPC_int_isp_find(Word16 isp_old[], Word16 isp_new[],
                        const Word16 frac[], Word16 Az[])
{
   Word32 i, k, fac_old, fac_new, tmp;
   Word16 isp[M];

   for (k = 0; k < 3; k++)
   {
      fac_new = frac[k];
      fac_old = ((32767 - fac_new) + 1);  /* 1.0 - fac_new */

      for (i = 0; i < M; i++)
      {
         tmp = isp_old[i] * fac_old;
         tmp += (isp_new[i] * fac_new);
         isp[i] = (Word16)((tmp + 0x4000) >> 15);
      }

      E_LPC_isp_a_conversion(isp, Az, M);
      Az += MP1;
   }

   /* 4th subframe: isp_new (frac=1.0) */
   E_LPC_isp_a_conversion(isp_new, Az, M);

   return;
}

void E_LPC_f_int_isp_find(Float32 isp_old[], Float32 isp_new[], Float32 a[],
                          Word32 nb_subfr, Word32 m)
{
  Float32 isp[M], fnew, fold;
  Float32 *p_a;
  Word32 i, k;

  p_a = a;

  for (k = 0; k < nb_subfr; k++)
  {
    fnew = E_ROM_f_interpol_frac[k];
    fold = (Float32)(1.0 - fnew);

    for (i = 0; i < m; i++)
    {
      isp[i] = isp_old[i] * fold + isp_new[i] * fnew;
    }

    E_LPC_f_isp_a_conversion(isp, p_a, m);
    p_a += (m + 1);
  }

  return;
}

/*
 * E_LPC_a_weight
 *
 * Parameters: