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

#define L_FRAME         256   /* Frame size                                */
#define L_SUBFR         64    /* Subframe size                             */
#define HP_ORDER        3
#define L_INTERPOL1     4
#define L_INTERPOL2     16
#define PIT_SHARP       27853 /* pitch sharpening factor = 0.85 Q15        */
#define F_PIT_SHARP     0.85F /* pitch sharpening factor                   */
#define PIT_MIN         34    /* Minimum pitch lag with resolution 1/4     */
#define UP_SAMP         4
#define DIST_ISF_MAX    120
#define DIST_ISF_THRES  60
#define GAIN_PIT_THRES  0.9F
#define GAIN_PIT_MIN    0.6F

extern const Float32 E_ROM_corrweight[];
extern const Float32 E_ROM_inter4_1[];
extern const Word16 E_ROM_inter4_2[];

/*
 * E_GAIN_clip_init
 *
 * Parameters:
 *    mem        O: memory of gain of pitch clipping algorithm
 *
 * Function:
 *    Initialises state memory
 *
 * Returns:
 *    void
 */
void E_GAIN_clip_init(Float32 mem[])
{
   mem[0] = DIST_ISF_MAX;
   mem[1] = GAIN_PIT_MIN;
}

/*
 * E_GAIN_clip_test
 *
 * Parameters:
 *    mem         I: memory of gain of pitch clipping algorithm
 *
 * Function:
 *    Gain clipping test to avoid unstable synthesis on frame erasure
 *
 * Returns:
 *    Test result
 */
Word32 E_GAIN_clip_test(Float32 mem[])
{
   Word32 clip;

   clip = 0;
   if ((mem[0] < DIST_ISF_THRES) && (mem[1] > GAIN_PIT_THRES))
   {
      clip = 1;
   }

   return (clip);
}

/*
 * E_GAIN_clip_isf_test
 *
 * Parameters:
 *    isf         I: isf values (in frequency domain)
 *    mem       I/O: memory of gain of pitch clipping algorithm
 *
 * Function:
 *    Check resonance for pitch clipping algorithm
 *
 * Returns:
 *    void
 */
void E_GAIN_clip_isf_test(Float32 isf[], Float32 mem[])
{
   Word32 i;
   Float32 dist, dist_min;

   dist_min = isf[1] - isf[0];

   for (i = 2; i < M - 1; i++)
   {
      dist = isf[i] - isf[i-1];

      if (dist < dist_min)
      {
         dist_min = dist;
      }
   }

   dist = 0.8F * mem[0] + 0.2F * dist_min;

   if (dist > DIST_ISF_MAX)
   {
      dist = DIST_ISF_MAX;
   }

   mem[0] = dist;

   return;
}

/*
 * E_GAIN_clip_pit_test
 *
 * Parameters:
 *    gain_pit       I: gain of quantized pitch
 *    mem          I/O: memory of gain of pitch clipping algorithm
 *
 * Function:
 *    Test quantised gain of pitch for pitch clipping algorithm
 *
 * Returns:
 *    void
 */
void E_GAIN_clip_pit_test(Float32 gain_pit, Float32 mem[])
{
   Float32 gain;

   gain = 0.9F * mem[1] + 0.1F * gain_pit;

   if (gain < GAIN_PIT_MIN)
   {
      gain = GAIN_PIT_MIN;
   }

   mem[1] = gain;

   return;
}

/*
 * E_GAIN_lp_decim2
 *
 * Parameters:
 *    x            I/O: signal to process
 *    l              I: size of filtering
 *    mem          I/O: memory (size = 3)
 *
 * Function:
 *    Decimate a vector by 2 with 2nd order fir filter.
 *
 * Returns:
 *    void
 */
void E_GAIN_lp_decim2(Float32 x[], Word32 l, Float32 *mem)
{
   Float32 x_buf[L_FRAME + 3];
   Float32 temp;
   Word32 i, j;

   /* copy initial filter states into buffer */

   memcpy(x_buf, mem, 3 * sizeof(Float32));
   memcpy(&x_buf[3], x, l * sizeof(Float32));

   for (i = 0; i < 3; i++)
   {
      mem[i] =
         ((x[l - 3 + i] > 1e-10) | (x[l - 3 + i] < -1e-10)) ? x[l - 3 + i] : 0;
   }

   for (i = 0, j = 0; i < l; i += 2, j++)
   {
      temp = x_buf[i] * 0.13F;
      temp += x_buf[i + 1] * 0.23F;
      temp += x_buf[i + 2] * 0.28F;
      temp += x_buf[i + 3] * 0.23F;
      temp += x_buf[i + 4] * 0.13F;
      x[j] = temp;
   }

   return;
}

/*
 * E_GAIN_open_loop_search
 *
 * Parameters:
 *    wsp               I: signal (end pntr) used to compute the open loop pitch
 *    L_min             I: minimum pitch lag
 *    L_max             I: maximum pitch lag
 *    nFrame            I: length of frame to compute pitch
 *    L_0               I: old open-loop lag
 *    gain              O: open-loop pitch-gain
 *    hp_wsp_mem      I/O: memory of the highpass filter for hp_wsp[] (lg = 9)
 *    hp_old_wsp        O: highpass wsp[]
 *    weight_flg        I: is weighting function used
 *
 * Function:
 *    Find open loop pitch lag
 *
 * Returns:
 *    open loop pitch lag
 */
Word32 E_GAIN_open_loop_search(Float32 *wsp, Word32 L_min, Word32 L_max,
                           Word32 nFrame, Word32 L_0, Float32 *gain,
                           Float32 *hp_wsp_mem, Float32 hp_old_wsp[],
                           UWord8 weight_flg)
{
   Word32  i, j, k, L = 0;
   Float32  o, R0, R1, R2, R0_max = -1.0e23f;
   const Float32 *ww, *we;
   Float32 *data_a, *data_b, *hp_wsp, *p, *p1;

   ww = &E_ROM_corrweight[198];
   we = &E_ROM_corrweight[98 + L_max - L_0];

   for (i = L_max; i > L_min; i--)
   {
      p  = &wsp[0];
      p1 = &wsp[-i];

      /* Compute the correlation R0 and the energy R1. */
      R0 = 0.0;

      for (j = 0; j < nFrame; j += 2)
      {
         R0 += p[j] * p1[j];
         R0 += p[j + 1] * p1[j + 1];
      }

      /* Weighting of the correlation function. */

      R0 *= *ww--;

      /* Weight the neighborhood of the old lag. */
      if ((L_0 > 0) & (weight_flg == 1))
      {
         R0 *= *we--;
      }

      /* Store the values if a currest maximum has been found. */

      if (R0 >= R0_max)
      {
         R0_max = R0;
         L = i;
      }
   }

   data_a = hp_wsp_mem;
   data_b = hp_wsp_mem + HP_ORDER;
   hp_wsp = hp_old_wsp + L_max;

   for (k = 0; k < nFrame; k++)
   {

      data_b[0] = data_b[1];
      data_b[1] = data_b[2];
      data_b[2] = data_b[3];

      data_b[HP_ORDER] = wsp[k];

      o = data_b[0] * 0.83787057505665F;
      o += data_b[1] * -2.50975570071058F;
      o += data_b[2] * 2.50975570071058F;
      o += data_b[3] * -0.83787057505665F;

      o -= data_a[0] * -2.64436711600664F;
      o -= data_a[1] * 2.35087386625360F;
      o -= data_a[2] * -0.70001156927424F;

      data_a[2] = data_a[1];
      data_a[1] = data_a[0];

      data_a[0] = o;
      hp_wsp[k] = o;
   }

   p  = &hp_wsp[0];
   p1 = &hp_wsp[-L];

   R0 = 0.0F;
   R1 = 0.0F;
   R2 = 0.0F;

   for (j = 0; j < nFrame; j++)
   {
      R1 += p1[j] * p1[j];
      R2 += p[j] * p[j];
      R0 += p[j] * p1[j];
   }

   *gain = (Float32)(R0 / (sqrt(R1 * R2) + 1e-5));

   memcpy(hp_old_wsp, &hp_old_wsp[nFrame], L_max * sizeof(Float32));

   return(L);
}

/*
 * E_GAIN_sort
 *
 * Parameters:
 *    n              I: number of lags
 *    ra           I/O: lags / sorted lags
 *
 * Function:
 *    Sort open-loop lags
 *
 * Returns:
 *    void
 */
static void E_GAIN_sort(Word32 n, Word32 *ra)
{
   Word32 l, j, ir, i, rra;

   l = (n >> 1) + 1;
   ir = n;
   for (;;)
   {
      if (l > 1)
      {
         rra = ra[--l];
      }
      else
      {
         rra = ra[ir];
         ra[ir] = ra[1];

         if (--ir == 1)
         {
            ra[1] = rra;
            return;
         }
      }

      i = l;
      j = l << 1;
      while (j <= ir)
      {
         if (j < ir && ra[j] < ra[j + 1])
         {
            ++j;
         }

         if (rra < ra[j])
         {
            ra[i] = ra[j];
            j += (i = j);
         }
         else
         {
            j = ir + 1;
         }
      }

      ra[i] = rra;
   }
}

/*
 * E_GAIN_olag_median
 *
 * Parameters:
 *    prev_ol_lag            I: previous open-loop lag
 *    old_ol_lag             I: old open-loop lags
 *
 * Function:
 *    Median of 5 previous open-loop lags
 *
 * Returns:
 *    median of 5 previous open-loop lags
 */
Word32 E_GAIN_olag_median(Word32 prev_ol_lag, Word32 old_ol_lag[5])
{
   Word32 tmp[6] = {0};
   Word32 i;

   /* Use median of 5 previous open-loop lags as old lag */

   for (i = 4; i > 0; i--)
   {
      old_ol_lag[i] = old_ol_lag[i-1];
   }

   old_ol_lag[0] = prev_ol_lag;

   for (i = 0; i < 5; i++)