Symbian

开发平台：
C/C++

sp_enc.c：源码内容
							/*
 * ===================================================================
 *  TS 26.104
 *  R99   V3.4.0 2002-02
 *  REL-4 V4.3.0 2002-02
 *  3GPP AMR Floating-point Speech Codec
 * ===================================================================
 *
 */
/*
 * sp_enc.c
 *
 *
 * Project:
 *    AMR Floating-Point Codec
 *
 * Contains:
 *    This module contains all the functions needed encoding 160
 *    16-bit speech samples to AMR encoder parameters.
 *
 */
#include <stdlib.h>
#include <stdio.h>
#include "hlxclib/memory.h"
#include <math.h>
#include <float.h>
#include "sp_enc.h"
#include "rom_enc.h"
/*
 * Definition of structures used in encoding process
 */
typedef struct
{
   Float32 y2;
   Float32 y1;
   Float32 x0;
   Float32 x1;
}Pre_ProcessState;
#ifdef VAD2
/* Defines for VAD2 */
#define	FRM_LEN1		80
#define	DELAY0			24
#define	FFT_LEN1		128
#define	UPDATE_CNT_THLD1	50
#define	INIT_FRAMES		4
#define	CNE_SM_FAC1		0.1
#define	CEE_SM_FAC1		0.55
#define	HYSTER_CNT_THLD1	6	/* forced update constants... */
#define	HIGH_ALPHA1		0.9
#define	LOW_ALPHA1		0.7
#define	ALPHA_RANGE1		(HIGH_ALPHA1-LOW_ALPHA1)
#define NORM_ENRG		(4.0)	/* account for div by 2 by the HPF */
#define	MIN_CHAN_ENRG		(0.0625 / NORM_ENRG)
#define	INE			(16.0 / NORM_ENRG)
#define	NOISE_FLOOR		(1.0 / NORM_ENRG)
#define	PRE_EMP_FAC1		(-0.8)
#define	NUM_CHAN		16
#define	LO_CHAN			0
#define	HI_CHAN			15
#define	UPDATE_THLD		35
#define	SINE_START_CHAN		2
#define	P2A_THRESH		10.0
#define	DEV_THLD1		28.0
/* Defines for the FFT function */
#define	SIZE			128
#define	SIZE_BY_TWO		64
#define	NUM_STAGE		6
#define	PI			3.141592653589793
#define	TRUE			1
#define	FALSE			0
/* Macros */
#define	min(a,b)		((a)<(b)?(a):(b))
#define	max(a,b)		((a)>(b)?(a):(b))
#define	square(a)		((a)*(a))
/* structures */
typedef struct
{
  Float32 pre_emp_mem;
  Word16  update_cnt;
  Word16  hyster_cnt;
  Word16  last_update_cnt;
  Float32 ch_enrg_long_db[NUM_CHAN];
  Word32  Lframe_cnt;
  Float32 ch_enrg[NUM_CHAN];
  Float32 ch_noise[NUM_CHAN];
  Float32 tsnr;
  Word16  hangover;
  Word16  burstcount;
  Word16  fupdate_flag;
  Float32 negSNRvar;
  Float32 negSNRbias;
  Float32 R0;
  Float32 Rmax;
  Word16  LTP_flag;
}vadState;
#else
typedef struct
{
   Float32 bckr_est[COMPLEN];   /* background noise estimate */
   Float32 ave_level[COMPLEN];
   /* averaged input components for stationary estimation */
   Float32 old_level[COMPLEN];   /* input levels of the previous frame */
   Float32 sub_level[COMPLEN];
   /* input levels calculated at the end of a frame (lookahead) */
   Float32 a_data5[3][2];   /* memory for the filter bank */
   Float32 a_data3[5];   /* memory for the filter bank */
   Float32 best_corr_hp;   /* FIP filtered value */
   /* counts length of a speech burst incl HO addition */
   Float32 corr_hp_fast;   /* filtered value */
   Word32 vadreg;   /* flags for intermediate VAD decisions */
   Word32 pitch;   /* flags for pitch detection */
   Word32 oldlag_count, oldlag;   /* variables for pitch detection */
   Word32 complex_high;   /* flags for complex detection */
   Word32 complex_low;   /* flags for complex detection */
   Word32 complex_warning;   /* complex background warning */
   Word32 tone;   /* flags for tone detection */
   Word16 burst_count;   /* counts length of a speech burst */
   Word16 hang_count;   /* hangover counter */
   Word16 stat_count;   /* stationary counter */
   Word16 complex_hang_count;   /* complex hangover counter, used by VAD */
   Word16 complex_hang_timer;   /* hangover initiator, used by CAD */
   Word16 speech_vad_decision;   /* final decision */
   Word16 sp_burst_count;
}vadState;
#endif
#define DTX_HIST_SIZE 8
#define DTX_ELAPSED_FRAMES_THRESH (24 + 7 -1)
#define DTX_HANG_CONST 7   /* yields eight frames of SP HANGOVER */
typedef struct
{
   Float32 lsp_hist[M * DTX_HIST_SIZE];
   Float32 log_en_hist[DTX_HIST_SIZE];
   Word32 init_lsf_vq_index;
   Word16 hist_ptr;
   Word16 log_en_index;
   Word16 lsp_index[3];
   /* DTX handler stuff */
   Word16 dtxHangoverCount;
   Word16 decAnaElapsedCount;
}dtx_encState;
typedef struct
{
   /* gain history */
   Float32 gp[N_FRAME];
   /* counters */
   Word16 count;
}tonStabState;
typedef struct
{
   Word32 past_qua_en[4];
   /* normal MA predictor memory, (contains 20*log10(qua_err)) */
}gc_predState;
typedef struct
{
   Float32 prev_alpha;   /* previous adaptor output, */
   Float32 prev_gc;   /* previous code gain, */
   Float32 ltpg_mem[LTPG_MEM_SIZE];   /* LTP coding gain history, */
   Word16 onset;   /* onset state, */
   /* (ltpg_mem[0] not used for history) */
}gain_adaptState;
typedef struct
{
   Float32 sf0_target_en;
   Float32 sf0_coeff[5];
   Word32 sf0_gcode0_exp;
   Word32 sf0_gcode0_fra;
   Word16 *gain_idx_ptr;
   gc_predState * gc_predSt;
   gc_predState * gc_predUncSt;
   gain_adaptState * adaptSt;
}gainQuantState;
typedef struct
{
   Word32 T0_prev_subframe;   /* integer pitch lag of previous sub-frame */
}Pitch_frState;
typedef struct
{
   Pitch_frState * pitchSt;
}clLtpState;
typedef struct
{
   Float32 ada_w;
   Word32 old_T0_med;
   Word16 wght_flg;
}pitchOLWghtState;
typedef struct
{
   Float32 past_rq[M];   /* Past quantized prediction error */
}Q_plsfState;
typedef struct
{
   /* Past LSPs */
   Float32 lsp_old[M];
   Float32 lsp_old_q[M];
   /* Quantization state */
   Q_plsfState * qSt;
}lspState;
typedef struct
{
   Float32 old_A[M + 1];   /* Last A(z) for case of unstable filter */
}LevinsonState;
typedef struct
{
   LevinsonState * LevinsonSt;
}lpcState;
typedef struct
{
   /* Speech vector */
   Float32 old_speech[L_TOTAL];
   Float32 *speech, *p_window, *p_window_12k2;
   Float32 *new_speech;   /* Global variable */
   /* Weight speech vector */
   Float32 old_wsp[L_FRAME + PIT_MAX];
   Float32 *wsp;
   /* OL LTP states */
   Word32 old_lags[5];
   Float32 ol_gain_flg[2];
   /* Excitation vector */
   Float32 old_exc[L_FRAME + PIT_MAX + L_INTERPOL];
   Float32 *exc;
   /* Zero vector */
   Float32 ai_zero[L_SUBFR + MP1];
   Float32 *zero;
   /* Impulse response vector */
   Float32 *h1;
   Float32 hvec[L_SUBFR * 2];
   /* Substates */
   lpcState * lpcSt;
   lspState * lspSt;
   clLtpState * clLtpSt;
   gainQuantState * gainQuantSt;
   pitchOLWghtState * pitchOLWghtSt;
   tonStabState * tonStabSt;
   vadState * vadSt;
   Word32 dtx;
   dtx_encState * dtxEncSt;
   /* Filter's memory */
   Float32 mem_syn[M], mem_w0[M], mem_w[M];
   Float32 mem_err[M + L_SUBFR], *error;
   Float32 sharp;
}cod_amrState;
typedef struct
{
   cod_amrState * cod_amr_state;
   Pre_ProcessState * pre_state;
   Word32 dtx;
}Speech_Encode_FrameState;
/*
 * Dotproduct40
 *
 *
 * Parameters:
 *    x                 I: First input
 *    y                 I: Second input
 * Function:
 *    Computes dot product size 40
 *
 * Returns:
 *    acc                dot product
 */
static Float64 Dotproduct40( Float32 *x, Float32 *y )
{
   Float64 acc;
   acc = x[0] * y[0] + x[1] * y[1] + x[2] * y[2] + x[3] * y[3];
   acc += x[4] * y[4] + x[5] * y[5] + x[6] * y[6] + x[7] * y[7];
   acc += x[8] * y[8] + x[9] * y[9] + x[10] * y[10] + x[11] * y[11];
   acc += x[12] * y[12] + x[13] * y[13] + x[14] * y[14] + x[15] * y[15];
   acc += x[16] * y[16] + x[17] * y[17] + x[18] * y[18] + x[19] * y[19];
   acc += x[20] * y[20] + x[21] * y[21] + x[22] * y[22] + x[23] * y[23];
   acc += x[24] * y[24] + x[25] * y[25] + x[26] * y[26] + x[27] * y[27];
   acc += x[28] * y[28] + x[29] * y[29] + x[30] * y[30] + x[31] * y[31];
   acc += x[32] * y[32] + x[33] * y[33] + x[34] * y[34] + x[35] * y[35];
   acc += x[36] * y[36] + x[37] * y[37] + x[38] * y[38] + x[39] * y[39];
   return( acc );
}
/*
 * Autocorr
 *
 *
 * Parameters:
 *    x                 I: Input signal
 *    r                 O: Autocorrelations
 *    wind              I: Window for LPC analysis
 * Function:
 *    Calculate autocorrelation with window, LPC order = M
 *
 * Returns:
 *    void
 */
static void Autocorr( Float32 x[], Float32 r[], const Float32 wind[] )
{
   Word32 i, j;   /* Counters */
   Float32 y[L_WINDOW + M + 1];   /* Windowed signal */
   Float64 sum;   /* temp */
   /*
    * Windowing of signal
    */
   for ( i = 0; i < L_WINDOW; i++ ) {
      y[i] = x[i] * wind[i];
   }
   /*
    * Zero remaining memory
    */
   memset( &y[L_WINDOW], 0, 44 );
   /*
    * Autocorrelation
    */
   for ( i = 0; i <= M; i++ ) {
      sum = 0;
      for ( j = 0; j < L_WINDOW; j += 40 ) {
         sum += Dotproduct40( &y[j], &y[j + i] );
      }
      r[i] = (Float32)sum;
   }
}
/*
 * Levinson
 *
 *
 * Parameters:
 *    old_A             I: Vector of old LP coefficients [M+1]
 *    r                 I: Vector of autocorrelations    [M+1]
 *    a                 O: LP coefficients               [M+1]
 *    rc                O: Reflection coefficients       [4]
 * Function:
 *    Levinson-Durbin algorithm
 *
 * Returns:
 *    void
 *
 */
static void Levinson( Float32 *old_A, Float32 *r, Float32 *A, Float32 *rc )
{
   Float32 sum, at, err;
   Word32 l, j, i;
   Float32 rct[M];   /* temporary reflection coefficients  0,...,m-1 */
   rct[0] = ( -r[1] ) / r[0];
   A[0] = 1.0F;
   A[1] = rct[0];
   err = r[0] + r[1] * rct[0];
   if ( err <= 0.0 )
      err = 0.01F;
   for ( i = 2; i <= M; i++ ) {
      sum = 0.0F;
      for ( j = 0; j < i; j++ )
         sum += r[i - j] * A[j];
      rct[i - 1] = ( -sum ) / ( err );
      for ( j = 1; j <= ( i / 2 ); j++ ) {
         l = i - j;
         at = A[j] + rct[i - 1] *A[l];
         A[l] += rct[i - 1] *A[j];
         A[j] = at;
      }
      A[i] = rct[i - 1];
      err += rct[i - 1] *sum;
      if ( err <= 0.0 )
         err = 0.01F;
   }
   memcpy( rc, rct, 4 * sizeof( Float32 ) );
   memcpy( old_A, A, MP1 * sizeof( Float32 ) );
}
/*
 * lpc
 *
 *
 * Parameters:
 *    old_A             O: Vector of old LP coefficients [M+1]
 *    x                 I: Input signal
 *    x_12k2            I: Input signal 12.2k
 *    a                 O: predictor coefficients
 *    mode              I: AMR mode
 * Function:
 *    LP analysis
 *
 *    In 12.2 kbit/s mode linear prediction (LP) analysis is performed
 *    twice per speech frame using the auto-correlation approach with
 *    30 ms asymmetric windows. No lookahead is used in
 *    the auto-correlation computation.
 *
 *    In other modes analysis is performed once per speech frame
 *    using the auto-correlation approach with 30 ms asymmetric windows.
 *    A lookahead of 40 samples (5 ms) is used in the auto-correlation computation.
 *
 *    The auto-correlations of windowed speech are converted to the LP
 *    coefficients using the Levinson-Durbin algorithm.
 *    Then the LP coefficients are transformed to the Line Spectral Pair
 *    (LSP) domain  for quantization and interpolation purposes.
 *    The interpolated quantified and unquantized filter coefficients
 *    are converted back to the LP filter coefficients
 *    (to construct the synthesis and weighting filters at each subframe).
 *
 * Returns:
 *    void
 *
 */
static void lpc( Float32 *old_A, Float32 x[], Float32 x_12k2[], Float32 a[], enum Mode
      mode )
{
   Word32 i;
   Float32 r[MP1];
   Float32 rc[4];
   if ( mode == MR122 ) {
      Autocorr( x_12k2, r, window_160_80 );
      /*
       * Lag windowing
       */
      for ( i = 1; i <= M; i++ ) {
         r[i] = r[i] * lag_wind[i - 1];
      }
      r[0] *= 1.0001F;
      if ( r[0] < 1.0F )
         r[0] = 1.0F;
      /*
       * Levinson Durbin
       */
      Levinson( old_A, r, &a[MP1], rc );
      /*
       * Autocorrelations
       */
      Autocorr( x_12k2, r, window_232_8 );
      /*
       * Lag windowing
       */
      for ( i = 1; i <= M; i++ ) {
         r[i] = r[i] * lag_wind[i - 1];
      }
      r[0] *= 1.0001F;
      if ( r[0] < 1.0F )
         r[0] = 1.0F;
      /*
       * Levinson Durbin
       */
      Levinson( old_A, r, &a[MP1 * 3], rc );
   }
   else {
      /*
       * Autocorrelations
       */
      Autocorr( x, r, window_200_40 );
      /*
       * a 60 Hz bandwidth expansion is used by lag windowing
       * the auto-correlations. Further, auto-correlation[0] is
       * multiplied by the white noise correction factor 1.0001
       * which is equivalent to adding a noise floor at -40 dB.
       */
      for ( i = 1; i <= M; i++ ) {
         r[i] = r[i] * lag_wind[i - 1];
      }
      r[0] *= 1.0001F;
      if ( r[0] < 1.0F )
         r[0] = 1.0F;
      /*
       * Levinson Durbin
       */
      Levinson( old_A, r, &a[MP1 * 3], rc );
   }
}
/*
 * Chebps
 *
 *
 * Parameters:
 *    x                 I: Cosine value for the freqency given
 *    f                 I: angular freqency
 * Function:
 *    Evaluates the Chebyshev polynomial series
 *
 * Returns:
 *    result of polynomial evaluation
 */
static Float32 Chebps( Float32 x, Float32 f[] )
{
   Float32 b0, b1, b2, x2;
   Word32 i;
   x2 = 2.0F * x;
   b2 = 1.0F;
   b1 = x2 + f[1];
   for ( i = 2; i < 5; i++ ) {
      b0 = x2 * b1 - b2 + f[i];
      b2 = b1;
      b1 = b0;
   }
   return( x * b1 - b2 + f[i] );
}
/*
 * Az_lsp
 *
 *
 * Parameters:
 *    a                 I: Predictor coefficients              [MP1]
 *    lsp               O: Line spectral pairs                 [M]
 *    old_lsp           I: Old lsp, in case not found 10 roots [M]
 *
 * Function:
 *    LP to LSP conversion
 *
 *    The LP filter coefficients A[], are converted to the line spectral pair
 *    (LSP) representation for quantization and interpolation purposes.
 *
 * Returns:
 *    void
 */
static void Az_lsp( Float32 a[], Float32 lsp[], Float32 old_lsp[] )
{
   Word32 i, j, nf, ip;
   Float32 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
   Float32 y;
   Float32 *coef;
   Float32 f1[6], f2[6];
   /*
    *  find the sum and diff. pol. F1(z) and F2(z)
    */
   f1[0] = 1.0F;
   f2[0] = 1.0F;
   for ( i = 0; i < ( NC ); i++ ) {
      f1[i + 1] = a[i + 1] +a[M - i] - f1[i];
      f2[i + 1] = a[i + 1] -a[M - i] + f2[i];
   }
   f1[NC] *= 0.5F;
   f2[NC] *= 0.5F;
   /*
    * find the LSPs using the Chebychev pol. evaluation
    */
   nf = 0;   /* number of found frequencies */
   ip = 0;   /* indicator for f1 or f2 */
   coef = f1;
   xlow = grid[0];
   ylow = Chebps( xlow, coef );
   j = 0;
   while ( ( nf < M ) && ( j < 60 ) ) {
      j++;
      xhigh = xlow;
      yhigh = ylow;
      xlow = grid[j];
      ylow = Chebps( xlow, coef );
      if ( ylow * yhigh <= 0 ) {
         /* divide 4 times the interval */
         for ( i = 0; i < 4; i++ ) {
            xmid = ( xlow + xhigh ) * 0.5F;
            ymid = Chebps( xmid, coef );
            if ( ylow * ymid <= 0.0F ) {
               yhigh = ymid;
               xhigh = xmid;
            }
            else {
               ylow = ymid;
               xlow = xmid;
            }
         }
         /*
          * Linear interpolation
          * xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow)
          */
         y = yhigh - ylow;
         if ( y == 0 ) {
            xint = xlow;
         }
         else {
            y = ( xhigh - xlow ) / ( yhigh - ylow );
            xint = xlow - ylow * y;
         }
         lsp[nf] = xint;
         xlow = xint;
         nf++;
         if ( ip == 0 ) {
            ip = 1;
            coef = f2;
         }
         else {
            ip = 0;
            coef = f1;
         }
         ylow = Chebps( xlow, coef );
      }
   }
   /* Check if M roots found */
   if ( nf < M ) {
      memcpy( lsp, old_lsp, M <<2 );
   }
   return;
}
/*
 * Get_lsp_pol
 *
 *
 * Parameters:
 *    lsp                 I: line spectral frequencies
 *    f                   O: polynomial F1(z) or F2(z)
 *
 * Function:
 *    Find the polynomial F1(z) or F2(z) from the LSPs.
 *
 *    F1(z) = product ( 1 - 2 lsp[i] z^-1 + z^-2 )
 *             i=0,2,4,6,8
 *    F2(z) = product   ( 1 - 2 lsp[i] z^-1 + z^-2 )
 *             i=1,3,5,7,9
 *
 *    where lsp[] is the LSP vector in the cosine domain.
 *
 *    The expansion is performed using the following recursion:
 *
 *    f[0] = 1
 *    b = -2.0 * lsp[0]
 *    f[1] = b
 *    for i=2 to 5 do
 *       b = -2.0 * lsp[2*i-2];
 *       f[i] = b*f[i-1] + 2.0*f[i-2];
 *       for j=i-1 down to 2 do
 *          f[j] = f[j] + b*f[j-1] + f[j-2];
 *       f[1] = f[1] + b;
 *
 * Returns:
 *    void
 */
static void Get_lsp_pol( Float32 *lsp, Float32 *f )
{
   Word32 i, j;
   Float32 T0;
   f[0] = 1.0F;
   f[1] = -2.0F * lsp[0];
   for ( i = 2; i <= 5; i++ ) {
      T0 = -2.0F * lsp[2 * i - 2];
      f[i] = ( Float32 )( T0 * f[i - 1] +2.0F * f[i - 2] );
      for ( j = i - 1; j >= 2; j-- ) {
         f[j] = f[j] + T0 * f[j - 1] +f[j - 2];
      }
      f[1] = f[1] + T0;
   }
   return;
}
/*
 * Lsp_Az
 *
 *
 * Parameters:
 *    lsp                 I: Line spectral frequencies
 *    a                   O: Predictor coefficients
 *
 * Function:
 *    Converts from the line spectral pairs (LSP) to LP coefficients,
 *    for a 10th order filter.
 *
 * Returns:
 *    void
 */
static void Lsp_Az( Float32 lsp[], Float32 a[] )
{
   Float32 f1[6], f2[6];
   Word32 i, j;
   Get_lsp_pol( &lsp[0], f1 );
   Get_lsp_pol( &lsp[1], f2 );
   for ( i = 5; i > 0; i-- ) {
      f1[i] += f1[i - 1];
      f2[i] -= f2[i - 1];
   }
   a[0] = 1;
   for ( i = 1, j = 10; i <= 5; i++, j-- ) {
      a[i] = ( Float32 )( ( f1[i] + f2[i] ) * 0.5F );
      a[j] = ( Float32 )( ( f1[i] - f2[i] ) * 0.5F );
   }
   return;
}
/*
 * Int_lpc_1and3_2
 *
 *
 * Parameters:
 *    lsp_old        I: LSP vector at the 4th subfr. of past frame      [M]
 *    lsp_mid        I: LSP vector at the 2nd subframe of present frame [M]
 *    lsp_new        I: LSP vector at the 4th subframe of present frame [M]
 *    az             O: interpolated LP parameters in subframes 1 and 3
 *                                                                [AZ_SIZE]
 *
 * Function:
 *    Interpolation of the LPC parameters. Same as the Int_lpc
 *    function but we do not recompute Az() for subframe 2 and
 *	   4 because it is already available.
 *
 * Returns:
 *    void
 */
static void Int_lpc_1and3_2( Float32 lsp_old[], Float32 lsp_mid[], Float32
      lsp_new[], Float32 az[] )
{
   Float32 lsp[M];
   Word32 i;
   for ( i = 0; i < M; i += 2 ) {
      lsp[i] = ( lsp_mid[i] + lsp_old[i] ) * 0.5F;
      lsp[i + 1] = ( lsp_mid[i + 1] +lsp_old[i+1] )*0.5F;
   }
   /* Subframe 1 */
   Lsp_Az( lsp, az );
   az += MP1 * 2;
   for ( i = 0; i < M; i += 2 ) {
      lsp[i] = ( lsp_mid[i] + lsp_new[i] ) * 0.5F;
      lsp[i + 1] = ( lsp_mid[i + 1] +lsp_new[i+1] )*0.5F;
   }
   /* Subframe 3 */
   Lsp_Az( lsp, az );
   return;
}
/*
 * Lsp_lsf
 *
 *
 * Parameters:
 *    lsp               I: LSP vector
 *    lsf               O: LSF vector
 *
 * Function:
 *    Transformation lsp to lsf, LPC order M
 *
 * Returns:
 *    void
 */
static void Lsp_lsf( Float32 lsp[], Float32 lsf[] )
{
   Word32 i;
   for ( i = 0; i < M; i++ ) {
      lsf[i] = ( Float32 )( acos( lsp[i] )*SCALE_LSP_FREQ );
   }
   return;
}
/*
 * Lsf_wt
 *
 *
 * Parameters:
 *    lsf               I: LSF vector
 *    wf                O: square of weighting factors
 *
 * Function:
 *    Compute LSF weighting factors
 *
 * Returns:
 *    void
 */
static void Lsf_wt( Float32 *lsf, Float32 *wf )
{
   Float32 temp;
   Word32 i;
   wf[0] = lsf[1];
   for ( i = 1; i < 9; i++ ) {
      wf[i] = lsf[i + 1] -lsf[i - 1];
   }
   wf[9] = 4000.0F - lsf[8];
   for ( i = 0; i < 10; i++ ) {
      if ( wf[i] < 450.0F ) {
         temp = 3.347F - SLOPE1_WGHT_LSF * wf[i];
      }
      else {
         temp = 1.8F - SLOPE2_WGHT_LSF * ( wf[i] - 450.0F );
      }
      wf[i] = temp * temp;
   }
   return;
}
/*
 * Vq_subvec
 *
 *
 * Parameters:
 *    lsf_r1            I: 1st LSF residual vector
 *    lsf_r2            I: 2nd LSF residual vector
 *    dico              I: quantization codebook
 *    wf1               I: 1st LSF weighting factors
 *    wf2               I: 2nd LSF weighting factors
 *    dico_size         I: size of quantization codebook
 * Function:
 *    Quantization of a 4 dimensional subvector
 *
 * Returns:
 *    index             quantization index
 */
static Word16 Vq_subvec( Float32 *lsf_r1, Float32 *lsf_r2, const Float32 *dico,
      Float32 *wf1, Float32 *wf2, Word16 dico_size )
{
   Float64 temp, dist, dist_min;
   const Float32 *p_dico;
   Word32 i, index = 0;
   dist_min = DBL_MAX;
   p_dico = dico;
   for ( i = 0; i < dico_size; i++ ) {
      temp = lsf_r1[0] - *p_dico++;
      dist = temp * temp * wf1[0];
      temp = lsf_r1[1] - *p_dico++;
      dist += temp * temp * wf1[1];
      temp = lsf_r2[0] - *p_dico++;
      dist += temp * temp * wf2[0];
      temp = lsf_r2[1] - *p_dico++;
      dist += temp * temp * wf2[1];
      if ( dist < dist_min ) {
         dist_min = dist;
         index = i;
      }
   }
   /* Reading the selected vector */
   p_dico = &dico[index << 2];
   lsf_r1[0] = *p_dico++;
   lsf_r1[1] = *p_dico++;
   lsf_r2[0] = *p_dico++;
   lsf_r2[1] = *p_dico++;
   return( Word16 )index;
}
/*
 * Vq_subvec_s
 *
 *
 * Parameters:
 *    lsf_r1            I: 1st LSF residual vector
 *    lsf_r2            I: 2nd LSF residual vector
 *    dico              I: quantization codebook
 *    wf1               I: 1st LSF weighting factors
 *    wf2               I: 2nd LSF weighting factors
 *    dico_size         I: size of quantization codebook
 * Function:
 *    Quantization of a 4 dimensional subvector with a signed codebook
 *
 * Returns:
 *    index             quantization index
 */
static Word16 Vq_subvec_s( Float32 *lsf_r1, Float32 *lsf_r2, const Float32 *dico
      , Float32 *wf1, Float32 *wf2, Word16 dico_size )
{
   Float64 dist_min, dist1, dist2, temp1, temp2;
   const Float32 *p_dico;
   Word32 i, index = 0;
   Word16 sign = 0;
   dist_min = DBL_MAX;
   p_dico = dico;
   for ( i = 0; i < dico_size; i++ ) {
      temp1 = lsf_r1[0] - *p_dico;
      temp2 = lsf_r1[0] + *p_dico++;
      dist1 = temp1 * temp1 * wf1[0];
      dist2 = temp2 * temp2 * wf1[0];
      temp1 = lsf_r1[1] - *p_dico;
      temp2 = lsf_r1[1] + *p_dico++;
      dist1 += temp1 * temp1 * wf1[1];
      dist2 += temp2 * temp2 * wf1[1];
      temp1 = lsf_r2[0] - *p_dico;
      temp2 = lsf_r2[0] + *p_dico++;
      dist1 += temp1 * temp1 * wf2[0];
      dist2 += temp2 * temp2 * wf2[0];
      temp1 = lsf_r2[1] - *p_dico;
      temp2 = lsf_r2[1] + *p_dico++;
      dist1 += temp1 * temp1 * wf2[1];
      dist2 += temp2 * temp2 * wf2[1];
      if ( dist1 < dist_min ) {
         dist_min = dist1;
         index = i;
         sign = 0;
      }
      if ( dist2 < dist_min ) {
         dist_min = dist2;
         index = i;
         sign = 1;
      }
   }
   /* Reading the selected vector */
   p_dico = &dico[index << 2];
   if ( sign == 0 ) {
      lsf_r1[0] = *p_dico++;
      lsf_r1[1] = *p_dico++;
      lsf_r2[0] = *p_dico++;
      lsf_r2[1] = *p_dico++;
   }
   else {
      lsf_r1[0] = -( *p_dico++ );
      lsf_r1[1] = -( *p_dico++ );
      lsf_r2[0] = -( *p_dico++ );
      lsf_r2[1] = -( *p_dico++ );
   }
   index = index << 1;
   index = index + sign;
   return( Word16 )index;
}
/*
 * Reorder_lsf
 *
 *
 * Parameters:
 *    lsf               B: vector of LSFs
 *    min_dist          I: minimum required distance
 *
 * Function:
 *    Make sure that the LSFs are properly ordered and to keep a certain minimum
 *    distance between adjacent LSFs. LPC order = M.
 *
 * Returns:
 *    void
 */
static void Reorder_lsf( Float32 *lsf, Float32 min_dist )
{
   Float32 lsf_min;
   Word32 i;
   lsf_min = min_dist;
   for ( i = 0; i < M; i++ ) {
      if ( lsf[i] < lsf_min ) {
         lsf[i] = lsf_min;
      }
      lsf_min = lsf[i] + min_dist;
   }
}
/*
 * Lsf_lsp
 *
 *
 * Parameters:
 *    lsf               I: vector of LSFs
 *    lsp	            O: vector of LSPs
 *
 * Function:
 *    Transformation lsf to lsp, order M
 *
 * Returns:
 *    void
 */
static void Lsf_lsp( Float32 lsf[], Float32 lsp[] )
{
   Word32 i;
   for ( i = 0; i < M; i++ ) {
      lsp[i] = ( Float32 )cos( SCALE_FREQ_LSP * lsf[i] );
   }
   return;
}
/*
 * Vq_subvec3
 *
 *
 * Parameters:
 *    lsf_r1            I: 1st LSF residual vector
 *    dico              I: quantization codebook
 *    wf1               I: 1st LSF weighting factors
 *    dico_size         I: size of quantization codebook
 *    use_half          I: use every second entry in codebook
 *
 * Function:
 *    Quantization of a 3 dimensional subvector
 *
 * Returns:
 *    index             quantization index
 */
static Word16 Vq_subvec3( Float32 *lsf_r1, const Float32 *dico, Float32 *wf1,
      Word16 dico_size, Word32 use_half )
{
   Float64 dist, dist_min;
   Float32 temp;
   const Float32 *p_dico;
   Word32 i, index = 0;
   dist_min = FLT_MAX;
   p_dico = dico;
   if ( use_half == 0 ) {
      for ( i = 0; i < dico_size; i++ ) {
         temp = lsf_r1[0] - *p_dico++;
         temp *= wf1[0];
         dist = temp * temp;
         temp = lsf_r1[1] - *p_dico++;
         temp *= wf1[1];
         dist += temp * temp;
         temp = lsf_r1[2] - *p_dico++;
         temp *= wf1[2];
         dist += temp * temp;
         if ( dist < dist_min ) {
            dist_min = dist;
            index = i;
         }
      }
      p_dico = &dico[( 3 * index )];
   }
   else {
      for ( i = 0; i < dico_size; i++ ) {
         temp = lsf_r1[0] - *p_dico++;
         temp *= wf1[0];
         dist = temp * temp;
         temp = lsf_r1[1] - *p_dico++;
         temp *= wf1[1];
         dist += temp * temp;
         temp = lsf_r1[2] - *p_dico++;
         temp *= wf1[2];
         dist += temp * temp;
         if ( dist < dist_min ) {
            dist_min = dist;
            index = i;
         }
         p_dico = p_dico + 3;
      }
      p_dico = &dico[6 * index];
   }
   /* Reading the selected vector */
   lsf_r1[0] = *p_dico++;
   lsf_r1[1] = *p_dico++;
   lsf_r1[2] = *p_dico++;
   return( Word16 )index;
}
/*
 * Vq_subvec4
 *
 *
 * Parameters:
 *    lsf_r1            I: 1st LSF residual vector
 *    dico              I: quantization codebook
 *    wf1               I: 1st LSF weighting factors
 *    dico_size         I: size of quantization codebook
 *
 * Function:
 *    Quantization of a 4 dimensional subvector
 *
 * Returns:
 *    index             quantization index
 */
static Word16 Vq_subvec4( Float32 *lsf_r1, const Float32 *dico, Float32 *wf1,
      Word16 dico_size )
{
   Float64 dist, dist_min;
   Float32 temp;
   const Float32 *p_dico;
   Word32 i, index = 0;
   dist_min = FLT_MAX;
   p_dico = dico;
   for ( i = 0; i < dico_size; i++ ) {
      temp = lsf_r1[0] - *p_dico++;
      temp *= wf1[0];
      dist = temp * temp;
      temp = lsf_r1[1] - *p_dico++;
      temp *= wf1[1];
      dist += temp * temp;
      temp = lsf_r1[2] - *p_dico++;
      temp *= wf1[2];
      dist += temp * temp;
      temp = lsf_r1[3] - *p_dico++;
      temp *= wf1[3];
      dist += temp * temp;
      if ( dist < dist_min ) {
         dist_min = dist;
         index = i;
      }
   }
   /* Reading the selected vector */
   p_dico = &dico[index << 2];
   lsf_r1[0] = *p_dico++;
   lsf_r1[1] = *p_dico++;
   lsf_r1[2] = *p_dico++;
   lsf_r1[3] = *p_dico++;
   return( Word16 )index;
}
/*
 * Q_plsf_3
 *
 *
 * Parameters:
 *    mode              I: AMR mode
 *    past_rq           B: past quantized residual
 *    lsp1              I: 1st LSP vector
 *    lsp1_q            O: quantized 1st LSP vector
 *    indice            I: quantization indices of 5 matrices and
 *                         one sign for 3rd
 *    pred_init_i       O: init index for MA prediction in DTX mode
 *
 * Function:
 *    Quantization of LSF parameters with 1st order MA prediction and
 *    split by 3 vector quantization (split-VQ)
 *
 * Returns:
 *    void
 */
static void Q_plsf_3( enum Mode mode, Float32 *past_rq, Float32 *lsp1, Float32 *
      lsp1_q, Word16 *indice, Word32 *pred_init_i )
{
   Float32 lsf1[M], wf1[M], lsf_p[M], lsf_r1[M];
   Float32 lsf1_q[M];
   Float32 pred_init_err;
   Float32 min_pred_init_err;
   Float32 temp_r1[M];
   Float32 temp_p[M];
   Word32 j, i;
   /* convert LSFs to normalize frequency domain */
   Lsp_lsf( lsp1, lsf1 );
   /* compute LSF weighting factors */
   Lsf_wt( lsf1, wf1 );
   /* Compute predicted LSF and prediction error */
   if ( mode != MRDTX ) {
      for ( i = 0; i < M; i++ ) {
         lsf_p[i] = mean_lsf_3[i] + past_rq[i] * pred_fac[i];
         lsf_r1[i] = lsf1[i] - lsf_p[i];
      }
   }
   else {
      /*
       * DTX mode, search the init vector that yields
       * lowest prediction resuidual energy
       */
      *pred_init_i = 0;
      min_pred_init_err = FLT_MAX;
      for ( j = 0; j < PAST_RQ_INIT_SIZE; j++ ) {
         pred_init_err = 0;
         for ( i = 0; i < M; i++ ) {
            temp_p[i] = mean_lsf_3[i] + past_rq_init[j * M + i];
            temp_r1[i] = lsf1[i] - temp_p[i];
            pred_init_err += temp_r1[i] * temp_r1[i];
         }   /* next i */
         if ( pred_init_err < min_pred_init_err ) {
            min_pred_init_err = pred_init_err;
            memcpy( lsf_r1, temp_r1, M <<2 );
            memcpy( lsf_p, temp_p, M <<2 );
            memcpy( past_rq, &past_rq_init[j * M], M <<2 );
            *pred_init_i = j;
         }
      }
   }
   /* Split-VQ of prediction error */
   /* MR475, MR515 */
   if ( ( mode == MR475 ) || ( mode == MR515 ) ) {
      indice[0] = Vq_subvec3( &lsf_r1[0], dico1_lsf_3, &wf1[0], DICO1_SIZE_3, 0
            );
      indice[1] = Vq_subvec3( &lsf_r1[3], dico2_lsf_3, &wf1[3], DICO2_SIZE_3 /2,
            1 );
      indice[2] = Vq_subvec4( &lsf_r1[6], mr515_3_lsf, &wf1[6], MR515_3_SIZE );
   }
   /* MR795 */
   else if ( mode == MR795 ) {
      indice[0] = Vq_subvec3( &lsf_r1[0], mr795_1_lsf, &wf1[0], MR795_1_SIZE, 0
            );
      indice[1] = Vq_subvec3( &lsf_r1[3], dico2_lsf_3, &wf1[3], DICO2_SIZE_3, 0
            );
      indice[2] = Vq_subvec4( &lsf_r1[6], dico3_lsf_3, &wf1[6], DICO3_SIZE_3 );
   }
   /* MR59, MR67, MR74, MR102 , MRDTX */
   else {
      indice[0] = Vq_subvec3( &lsf_r1[0], dico1_lsf_3, &wf1[0], DICO1_SIZE_3, 0
            );
      indice[1] = Vq_subvec3( &lsf_r1[3], dico2_lsf_3, &wf1[3], DICO2_SIZE_3, 0
            );
      indice[2] = Vq_subvec4( &lsf_r1[6], dico3_lsf_3, &wf1[6], DICO3_SIZE_3 );
   }
   /* Compute quantized LSFs and update the past quantized residual */
   for ( i = 0; i < M; i++ ) {
      lsf1_q[i] = lsf_r1[i] + lsf_p[i];
      past_rq[i] = lsf_r1[i];
   }
   /* verification that LSFs has mimimum distance of LSF_GAP 50 Hz */
   Reorder_lsf( lsf1_q, 50.0F );
   /*  convert LSFs to the cosine domain */
   Lsf_lsp( lsf1_q, lsp1_q );
}
/*
 * Q_plsf_5
 *
 *
 * Parameters:
 *    past_rq           B: past quantized residual
 *    lsp1              I: 1st LSP vector
 *    lsp2              I: 2nd LSP vector
 *    lsp1_q            O: quantized 1st LSP vector
 *    lsp2_q            O: quantized 2nd LSP vector
 *    indice	         I: quantization indices of 5 matrices and
 *                         one sign for 3rd
 *
 * Function:
 *    Quantization of 2 sets of LSF parameters using 1st order MA
 *    prediction and split by 5 matrix quantization (split-MQ).
 *
 * Returns:
 *    void
 */
static void Q_plsf_5( Float32 *past_rq, Float32 *lsp1, Float32 *lsp2, Float32 *
      lsp1_q, Float32 *lsp2_q, Word16 *indice )
{
   Float32 lsf1[M], lsf2[M], wf1[M], wf2[M], lsf_p[M], lsf_r1[M], lsf_r2[M];
   Float32 lsf1_q[M], lsf2_q[M];
   Word32 i;
   /* convert LSFs to normalize frequency domain */
   Lsp_lsf( lsp1, lsf1 );
   Lsp_lsf( lsp2, lsf2 );
   /* Compute LSF weighting factors */
   Lsf_wt( lsf1, wf1 );
   Lsf_wt( lsf2, wf2 );
   /* Compute predicted LSF and prediction error */
   for ( i = 0; i < M; i++ ) {
      /* MR122 LSP prediction factor = 0.65 */
      lsf_p[i] = mean_lsf_5[i] + past_rq[i] * 0.65F;
      lsf_r1[i] = lsf1[i] - lsf_p[i];
      lsf_r2[i] = lsf2[i] - lsf_p[i];
   }
   /* Split-MQ of prediction error */
   indice[0] = Vq_subvec( &lsf_r1[0], &lsf_r2[0], dico1_lsf_5, &wf1[0], &wf2[0],
         DICO1_SIZE_5 );
   indice[1] = Vq_subvec( &lsf_r1[2], &lsf_r2[2], dico2_lsf_5, &wf1[2], &wf2[2],
         DICO2_SIZE_5 );
   indice[2] = Vq_subvec_s( &lsf_r1[4], &lsf_r2[4], dico3_lsf_5, &wf1[4], &wf2[4
         ], DICO3_SIZE_5 );
   indice[3] = Vq_subvec( &lsf_r1[6], &lsf_r2[6], dico4_lsf_5, &wf1[6], &wf2[6],
         DICO4_SIZE_5 );
   indice[4] = Vq_subvec( &lsf_r1[8], &lsf_r2[8], dico5_lsf_5, &wf1[8], &wf2[8],
         DICO5_SIZE_5 );
   /* Compute quantized LSFs and update the past quantized residual */
   for ( i = 0; i < M; i++ ) {
      lsf1_q[i] = lsf_r1[i] + lsf_p[i];
      lsf2_q[i] = lsf_r2[i] + lsf_p[i];
      past_rq[i] = lsf_r2[i];
   }
   /* verification that LSFs has minimum distance of LSF_GAP 50hz */
   Reorder_lsf( lsf1_q, 50.0F );
   Reorder_lsf( lsf2_q, 50.0F );
   /*  convert LSFs to the cosine domain */
   Lsf_lsp( lsf1_q, lsp1_q );
   Lsf_lsp( lsf2_q, lsp2_q );
}
/*
 * Int_lpc_1and3
 *
 *
 * Parameters:
 *    lsp_old        I: LSP vector at the 4th subfr. of past frame      [M]
 *    lsp_mid        I: LSP vector at the 2nd subframe of present frame [M]
 *    lsp_new        I: LSP vector at the 4th subframe of present frame [M]
 *    az             O: interpolated LP parameters in subframes 1 and 3
 *                                                                [AZ_SIZE]
 *
 * Function:
 *    Interpolates the LSPs and converts to LPC parameters
 *    to get a different LP filter in each subframe.
 *
 *    The 20 ms speech frame is divided into 4 subframes.
 *    The LSPs are quantized and transmitted at the 2nd and
 *    4th subframes (twice per frame) and interpolated at the
 *    1st and 3rd subframe.
 *
 * Returns:
 *    void
 */
static void Int_lpc_1and3( Float32 lsp_old[], Float32 lsp_mid[], Float32 lsp_new
      [], Float32 az[] )
{
   Word32 i;
   Float32 lsp[M];
   for ( i = 0; i < M; i++ ) {
      lsp[i] = ( lsp_mid[i] + lsp_old[i] ) * 0.5F;
   }
   /* Subframe 1 */
   Lsp_Az( lsp, az );
   az += MP1;
   /* Subframe 2 */
   Lsp_Az( lsp_mid, az );
   az += MP1;
   for ( i = 0; i < M; i++ ) {
      lsp[i] = ( lsp_mid[i] + lsp_new[i] ) * 0.5F;
   }
   /* Subframe 3 */
   Lsp_Az( lsp, az );
   az += MP1;
   /* Subframe 4 */
   Lsp_Az( lsp_new, az );
   return;
}
/*
 * Int_lpc_1to3_2
 *
 *
 * Parameters:
 *    lsp_old           I: LSP vector at the 4th subfr. of past frame      [M]
 *    lsp_new           I: LSP vector at the 4th subframe of present frame [M]
 *    az                O: interpolated LP parameters in subframes 1, 2 and 3
 *                                                                   [AZ_SIZE]
 *
 * Function:
 *    Interpolation of the LPC parameters.
 *
 * Returns:
 *    void
 */
static void Int_lpc_1to3_2( Float32 lsp_old[], Float32 lsp_new[], Float32 az[] )
{
   Float32 lsp[M];
   Word32 i;
   for ( i = 0; i < M; i += 2 ) {
      lsp[i] = lsp_new[i] * 0.25F + lsp_old[i] * 0.75F;
      lsp[i + 1] = lsp_new[i + 1] *0.25F + lsp_old[i + 1] *0.75F;
   }
   /* Subframe 1 */
   Lsp_Az( lsp, az );
   az += MP1;
   for ( i = 0; i < M; i += 2 ) {
      lsp[i] = ( lsp_old[i] + lsp_new[i] ) * 0.5F;
      lsp[i + 1] = ( lsp_old[i + 1] +lsp_new[i+1] )*0.5F;
   }
   /* Subframe 2 */
   Lsp_Az( lsp, az );
   az += MP1;
   for ( i = 0; i < M; i += 2 ) {
      lsp[i] = lsp_old[i] * 0.25F + lsp_new[i] * 0.75F;
      lsp[i + 1] = lsp_old[i + 1] *0.25F + lsp_new[i + 1] *0.75F;
   }
   /* Subframe 3 */
   Lsp_Az( lsp, az );
   return;
}
/*
 * Int_lpc_1to3
 *
 *
 * Parameters:
 *    lsp_old           I: LSP vector at the 4th subfr. of past frame      [M]
 *    lsp_new           I: LSP vector at the 4th subframe of present frame [M]
 *    az                O: interpolated LP parameters in all subframes
 *                                                                   [AZ_SIZE]
 *
 * Function:
 *    Interpolates the LSPs and converts to LPC parameters to get a different
 *    LP filter in each subframe.
 *
 *    The 20 ms speech frame is divided into 4 subframes.
 *    The LSPs are quantized and transmitted at the 4th
 *    subframes (once per frame) and interpolated at the
 *    1st, 2nd and 3rd subframe.
 *
 * Returns:
 *    void
 */
static void Int_lpc_1to3( Float32 lsp_old[], Float32 lsp_new[], Float32 az[] )
{
   Float32 lsp[M];
   Word32 i;
   for ( i = 0; i < M; i++ ) {
      lsp[i] = lsp_new[i] * 0.25F + lsp_old[i] * 0.75F;
   }
   /* Subframe 1 */
   Lsp_Az( lsp, az );
   az += MP1;
   for ( i = 0; i < M; i++ ) {
      lsp[i] = ( lsp_old[i] + lsp_new[i] ) * 0.5F;
   }
   /* Subframe 2 */
   Lsp_Az( lsp, az );
   az += MP1;
   for ( i = 0; i < M; i++ ) {
      lsp[i] = lsp_old[i] * 0.25F + lsp_new[i] * 0.75F;
   }
   /* Subframe 3 */
   Lsp_Az( lsp, az );
   az += MP1;
   /* Subframe 4 */
   Lsp_Az( lsp_new, az );
   return;
}
/*
 * lsp
 *
 *
 * Parameters:
 *    req_mode          I: requested mode
 *    used_mode         I: used mode
 *    lsp_old           B: old LSP vector
 *    lsp_old_q         B: old quantized LSP vector
 *    past_rq           B: past quantized residual
 *    az                B: interpolated LP parameters
 *    azQ               O: quantization interpol. LP parameters
 *    lsp_new           O: new lsp vector
 *    anap              O: analysis parameters
 *
 * Function:
 *    From A(z) to lsp. LSP quantization and interpolation
 *
 * Returns:
 *    void
 */
static void lsp( enum Mode req_mode, enum Mode used_mode, Float32 *lsp_old,
      Float32 *lsp_old_q, Float32 *past_rq, Float32 az[], Float32 azQ[], Float32
      lsp_new[], Word16 **anap )
{
   Float32 lsp_new_q[M];   /* LSPs at 4th subframe */
   Float32 lsp_mid[M], lsp_mid_q[M];   /* LSPs at 2nd subframe */
   Word32 pred_init_i;   /* init index for MA prediction in DTX mode */
   if ( req_mode == MR122 ) {
      Az_lsp( &az[MP1], lsp_mid, lsp_old );
      Az_lsp( &az[MP1 * 3], lsp_new, lsp_mid );
      /*
       * Find interpolated LPC parameters in all subframes
       * (both quantized and unquantized).
       * The interpolated parameters are in array A_t[] of size (M+1)*4
       * and the quantized interpolated parameters are in array Aq_t[]
       */
      Int_lpc_1and3_2( lsp_old, lsp_mid, lsp_new, az );
      if ( used_mode != MRDTX ) {
         /* LSP quantization (lsp_mid[] and lsp_new[] jointly quantized) */
         Q_plsf_5( past_rq, lsp_mid, lsp_new, lsp_mid_q, lsp_new_q, *anap );
         Int_lpc_1and3( lsp_old_q, lsp_mid_q, lsp_new_q, azQ );
         /* Advance analysis parameters pointer */
         ( *anap ) += 5;
      }
   }
   else {
      /* From A(z) to lsp */
      Az_lsp( &az[MP1 * 3], lsp_new, lsp_old );
      /*
       * Find interpolated LPC parameters in all subframes
       * (both quantized and unquantized).
       * The interpolated parameters are in array A_t[] of size (M+1)*4
       * and the quantized interpolated parameters are in array Aq_t[]
       */
      Int_lpc_1to3_2( lsp_old, lsp_new, az );
      /* LSP quantization */
      if ( used_mode != MRDTX ) {
         Q_plsf_3( req_mode, past_rq, lsp_new, lsp_new_q, *anap, &pred_init_i );
         Int_lpc_1to3( lsp_old_q, lsp_new_q, azQ );
         /* Advance analysis parameters pointer */
         ( *anap ) += 3;
      }
   }
   /* update the LSPs for the next frame */
   memcpy( lsp_old, lsp_new, M <<2 );
   memcpy( lsp_old_q, lsp_new_q, M <<2 );
}
/*
 * check_lsp
 *
 *
 * Parameters:
 *    count          B: counter for resonance
 *    lsp            B: LSP vector
 *
 * Function:
 *    Check the LSP's to detect resonances
 *
 *    Resonances in the LPC filter are monitored to detect possible problem
 *    areas where divergence between the adaptive codebook memories in
 *    the encoder and the decoder could cause unstable filters in areas
 *    with highly correlated continuos signals. Typically, this divergence
 *    is due to channel errors.
 *    The monitoring of resonance signals is performed using unquantized LSPs
 *    q(i), i = 1,...,10. The algorithm utilises the fact that LSPs are
 *    closely located at a peak in the spectrum. First, two distances,
 *    dist 1 and dist 2 ,are calculated in two different regions,
 *    defined as
 *
 *    dist1 = min[q(i) - q(i + 1)],  i = 4,...,8
 *    dist2 = min[q(i) - q(i + 1)],  i = 2,3
 *
 *    Either of these two minimum distance conditions must be fulfilled
 *    to classify the frame as a resonance frame and increase the resonance
 *    counter.
 *
 *    if(dist1 < TH1) || if (dist2 < TH2)
 *       counter++
 *    else
 *       counter = 0
 *
 *    TH1 = 0.046
 *    TH2 = 0.018, q(2) > 0.98
 *    TH2 = 0.024, 0.93 < q(2) <= 0.98
 *    TH2 = 0.018, otherwise
 *
 *    12 consecutive resonance frames are needed to indicate possible
 *    problem conditions, otherwise the LSP_flag is cleared.
 *
 * Returns:
 *    resonance flag
 */
static Word16 check_lsp( Word16 *count, Float32 *lsp )
{
   Float32 dist, dist_min1, dist_min2, dist_th;
   Word32 i;
   /*
    * Check for a resonance:
    * Find minimum distance between lsp[i] and lsp[i+1]
    */
   dist_min1 = FLT_MAX;
   for ( i = 3; i < 8; i++ ) {
      dist = lsp[i] - lsp[i + 1];
      if ( dist < dist_min1 ) {
         dist_min1 = dist;
      }
   }
   dist_min2 = FLT_MAX;
   for ( i = 1; i < 3; i++ ) {
      dist = lsp[i] - lsp[i + 1];
      if ( dist < dist_min2 ) {
         dist_min2 = dist;
      }
   }
   if ( lsp[1] > 0.98F ) {
      dist_th = 0.018F;
   }
   else if ( lsp[1] > 0.93F ) {
      dist_th = 0.024F;
   }
   else {
      dist_th = 0.034F;
   }
   if ( ( dist_min1 < 0.046F ) || ( dist_min2 < dist_th ) ) {
      *count += 1;
   }
   else {
      *count = 0;
   }
   /* Need 12 consecutive frames to set the flag */
   if ( *count >= 12 ) {
      *count = 12;
      return 1;
   }
   else {
      return 0;
   }
}
/*
 * Weight_Ai
 *
 *
 * Parameters:
 *    a                 I: LPC coefficients                    [M+1]
 *    fac               I: Spectral expansion factors.         [M+1]
 *    a_exp             O: Spectral expanded LPC coefficients  [M+1]
 *
 * Function:
 *    Spectral expansion of LP coefficients
 *
 * Returns:
 *    void
 */
static void Weight_Ai( Float32 a[], const Float32 fac[], Float32 a_exp[] )
{
   Word32 i;
   a_exp[0] = a[0];
   for ( i = 1; i <= M; i++ ) {
      a_exp[i] = a[i] * fac[i - 1];
   }
   return;
}
/*
 * Residu
 *
 *
 * Parameters:
 *    a                 I: prediction coefficients
 *    x                 I: speech signal
 *    y                 O: residual signal
 *
 * Function:
 *    Computes the LTP residual signal.
 *
 * Returns:
 *    void
 */
static void Residu( Float32 a[], Float32 x[], Float32 y[] )
{
   Float32 s;
   Word32 i;
   for ( i = 0; i < L_SUBFR; i += 4 ) {
      s = x[i] * a[0];
      s += x[i - 1] *a[1];
      s += x[i - 2] * a[2];
      s += x[i - 3] * a[3];
      s += x[i - 4] * a[4];
      s += x[i - 5] * a[5];
      s += x[i - 6] * a[6];
      s += x[i - 7] * a[7];
      s += x[i - 8] * a[8];
      s += x[i - 9] * a[9];
      s += x[i - 10] * a[10];
      y[i] = s;
      s = x[i + 1] *a[0];
      s += x[i] * a[1];
      s += x[i - 1] *a[2];
      s += x[i - 2] * a[3];
      s += x[i - 3] * a[4];
      s += x[i - 4] * a[5];
      s += x[i - 5] * a[6];
      s += x[i - 6] * a[7];
      s += x[i - 7] * a[8];
      s += x[i - 8] * a[9];
      s += x[i - 9] * a[10];
      y[i + 1] = s;
      s = x[i + 2] * a[0];
      s += x[i + 1] *a[1];
      s += x[i] * a[2];
      s += x[i - 1] *a[3];
      s += x[i - 2] * a[4];
      s += x[i - 3] * a[5];
      s += x[i - 4] * a[6];
      s += x[i - 5] * a[7];
      s += x[i - 6] * a[8];
      s += x[i - 7] * a[9];
      s += x[i - 8] * a[10];
      y[i + 2] = s;
      s = x[i + 3] * a[0];
      s += x[i + 2] * a[1];
      s += x[i + 1] *a[2];
      s += x[i] * a[3];
      s += x[i - 1] *a[4];
      s += x[i - 2] * a[5];
      s += x[i - 3] * a[6];
      s += x[i - 4] * a[7];
      s += x[i - 5] * a[8];
      s += x[i - 6] * a[9];
      s += x[i - 7] * a[10];
      y[i + 3] = s;
   }
   return;
}
/*
 * Syn_filt
 *
 *
 * Parameters:
 *    a                 I: prediction coefficients [M+1]
 *    x                 I: input signal
 *    y                 O: output signal
 *    mem               B: memory associated with this filtering
 *    update            I: 0=no update, 1=update of memory.
 *
 * Function:
 *    Perform synthesis filtering through 1/A(z).
 *
 * Returns:
 *    void
 */
static void Syn_filt( Float32 a[], Float32 x[], Float32 y[], Float32 mem[],
      Word16 update )
{
   Float64 tmp[50];
   Float64 sum;
   Float64 *yy;
   Word32 i;
   /* Copy mem[] to yy[] */
   yy = tmp;
   for ( i = 0; i < M; i++ ) {
      *yy++ = mem[i];
   }
   /* Do the filtering. */
   for ( i = 0; i < L_SUBFR; i = i + 4 ) {
      sum = x[i] * a[0];
      sum -= a[1] * yy[ - 1];
      sum -= a[2] * yy[ - 2];
      sum -= a[3] * yy[ - 3];
      sum -= a[4] * yy[ - 4];
      sum -= a[5] * yy[ - 5];
      sum -= a[6] * yy[ - 6];
      sum -= a[7] * yy[ - 7];
      sum -= a[8] * yy[ - 8];
      sum -= a[9] * yy[ - 9];
      sum -= a[10] * yy[ - 10];
      *yy++ = sum;
      y[i] = ( Float32 )yy[ - 1];
      sum = x[i + 1] *a[0];
      sum -= a[1] * yy[ - 1];
      sum -= a[2] * yy[ - 2];
      sum -= a[3] * yy[ - 3];
      sum -= a[4] * yy[ - 4];
      sum -= a[5] * yy[ - 5];
      sum -= a[6] * yy[ - 6];
      sum -= a[7] * yy[ - 7];
      sum -= a[8] * yy[ - 8];
      sum -= a[9] * yy[ - 9];
      sum -= a[10] * yy[ - 10];
      *yy++ = sum;
      y[i + 1] = ( Float32 )yy[ - 1];
      sum = x[i + 2] * a[0];
      sum -= a[1] * yy[ - 1];
      sum -= a[2] * yy[ - 2];
      sum -= a[3] * yy[ - 3];
      sum -= a[4] * yy[ - 4];
      sum -= a[5] * yy[ - 5];
      sum -= a[6] * yy[ - 6];
      sum -= a[7] * yy[ - 7];
      sum -= a[8] * yy[ - 8];
      sum -= a[9] * yy[ - 9];
      sum -= a[10] * yy[ - 10];
      *yy++ = sum;
      y[i + 2] = ( Float32 )yy[ - 1];
      sum = x[i + 3] * a[0];
      sum -= a[1] * yy[ - 1];
      sum -= a[2] * yy[ - 2];
      sum -= a[3] * yy[ - 3];
      sum -= a[4] * yy[ - 4];
      sum -= a[5] * yy[ - 5];
      sum -= a[6] * yy[ - 6];
      sum -= a[7] * yy[ - 7];
      sum -= a[8] * yy[ - 8];
      sum -= a[9] * yy[ - 9];
      sum -= a[10] * yy[ - 10];
      *yy++ = sum;
      y[i + 3] = ( Float32 )yy[ - 1];
   }
   /* Update of memory if update==1 */
   if ( update != 0 ) {
      for ( i = 0; i < M; i++ ) {
         mem[i] = y[30 + i];
      }
   }
   return;
}
/*
 * pre_big
 *
 *
 * Parameters:
 *    mode              I: AMR mode
 *    gamma1            I: spectral exp. factor 1
 *    gamma1_12k2       I: spectral exp. factor 1 for modes above MR795
 *    gamma2            I: spectral exp. factor 2
 *    A_t               I: A(z) unquantized, for 4 subframes
 *    frame_offset      I: frameoffset, 1st or second big_sbf
 *    speech            I: speech
 *    mem_w             B: synthesis filter memory state
 *    wsp               O: weighted speech
 *
 * Function:
 *    Big subframe (2 subframes) preprocessing
 *
 *    Open-loop pitch analysis is performed in order to simplify the pitch
 *    analysis and confine the closed-loop pitch search to a small number of
 *    lags around the open-loop estimated lags.
 *    Open-loop pitch estimation is based on the weighted speech signal Sw(n)
 *    which is obtained by filtering the input speech signal through
 *    the weighting filter
 *
 *    W(z) = A(z/g1) / A(z/g2)
 *
 *    That is, in a subframe of size L, the weighted speech is given by:
 *
 *                    10                           10
 *    Sw(n) = S(n) + SUM[a(i) * g1(i) * S(n-i)] - SUM[a(i) * g2(i) * Sw(n-i)],
 *                   i=1                          i=1
 *    n = 0, ..., L-1
 *
 * Returns:
 *    void
 */
static Word32 pre_big( enum Mode mode, const Float32 gamma1[], const Float32
      gamma1_12k2[], const Float32 gamma2[], Float32 A_t[], Word16 frame_offset,
      Float32 speech[], Float32 mem_w[], Float32 wsp[] )
{
   Float32 Ap1[MP1], Ap2[MP1];
   Word32 offset, i;
   /* A(z) with spectral expansion */
   const Float32 *g1;
   g1 = gamma1_12k2;
   if ( mode <= MR795 ) {
      g1 = gamma1;
   }
   offset = 0;
   if ( frame_offset > 0 ) {
      offset = MP1 << 1;
   }
   /* process two subframes (which form the "big" subframe) */
   for ( i = 0; i < 2; i++ ) {
      /* a(i) * g1(i) */
      Weight_Ai( &A_t[offset], g1, Ap1 );
      /* a(i) * g2(i) */
      Weight_Ai( &A_t[offset], gamma2, Ap2 );
      /*
       *       10
       *  S(n) + SUM[a(i) * g1(i) * S(n-i)]
       *       i=1
       */
      Residu( Ap1, &speech[frame_offset], &wsp[frame_offset] );
      /*
       *          10                            10
       *  S(n) + SUM[a(i) * g1(i) * S(n-i)]    SUM[a(i) * g2(i) * Sn(n-i)]
       *         i=1                           i=1
       */
      Syn_filt( Ap2, &wsp[frame_offset], &wsp[frame_offset], mem_w, 1 );
      offset += MP1;
      frame_offset += L_SUBFR;
   }
   return 0;
}
/*
 * comp_corr
 *
 *
 * Parameters:
 *    sig               I: signal
 *    L_frame           I: length of frame to compute pitch
 *    lag_max           I: maximum lag
 *    lag_min           I: minimum lag
 *    corr              O: correlation of selected lag
 *
 * Function:
 *    Calculate all correlations in a given delay range.
 *
 * Returns:
 *    void
 */
static void comp_corr( Float32 sig[], Word32 L_frame, Word32 lag_max, Word32
      lag_min, Float32 corr[] )
{
   Word32 i, j;
   Float32 *p, *p1;
   Float32 T0;
   for ( i = lag_max; i >= lag_min; i-- ) {
      p = sig;
      p1 = &sig[ - i];
      T0 = 0.0F;
      for ( j = 0; j < L_frame; j = j + 40, p += 40, p1 += 40 ) {
         T0 += p[0] * p1[0] + p[1] * p1[1] + p[2] * p1[2] + p[3] * p1[3];
         T0 += p[4] * p1[4] + p[5] * p1[5] + p[6] * p1[6] + p[7] * p1[7];
         T0 += p[8] * p1[8] + p[9] * p1[9] + p[10] * p1[10] + p[11] * p1[11];
         T0 += p[12] * p1[12] + p[13] * p1[13] + p[14] * p1[14] + p[15] * p1[15]
         ;
         T0 += p[16] * p1[16] + p[17] * p1[17] + p[18] * p1[18] + p[19] * p1[19]
         ;
         T0 += p[20] * p1[20] + p[21] * p1[21] + p[22] * p1[22] + p[23] * p1[23]
         ;
         T0 += p[24] * p1[24] + p[25] * p1[25] + p[26] * p1[26] + p[27] * p1[27]
         ;
         T0 += p[28] * p1[28] + p[29] * p1[29] + p[30] * p1[30] + p[31] * p1[31]
         ;
         T0 += p[32] * p1[32] + p[33] * p1[33] + p[34] * p1[34] + p[35] * p1[35]
         ;
         T0 += p[36] * p1[36] + p[37] * p1[37] + p[38] * p1[38] + p[39] * p1[39]
         ;
      }
      corr[ - i] = T0;
   }
   return;
}
/*
 * vad_tone_detection
 *
 *
 * Parameters:
 *    st->tone          B: flags indicating presence of a tone
 *    T0                I: autocorrelation maxima
 *    t1                I: energy
 *
 * Function:
 *    Set tone flag if pitch gain is high.
 *    This is used to detect signaling tones and other signals
 *    with high pitch gain.
 *
 * Returns:
 *    void
 */
#ifndef VAD2
static void vad_tone_detection( vadState *st, Float32 T0, Float32 t1 )
{
   if ( ( t1 > 0 ) && ( T0 > t1 * TONE_THR ) ) {
      st->tone = st->tone | 0x00004000;
   }
}
#endif
/*
 * Lag_max
 *
 *
 * Parameters:
 *    vadSt          B: vad structure
 *    corr           I: correlation vector
 *    sig            I: signal
 *    L_frame        I: length of frame to compute pitch
 *    lag_max        I: maximum lag
 *    lag_min        I: minimum lag
 *    cor_max        O: maximum correlation
 *    dtx            I: dtx on/off
 *
 * Function:
 *    Compute the open loop pitch lag.
 *
 * Returns:
 *    p_max             lag found
 */
#ifdef VAD2
static Word16 Lag_max( Float32 corr[], Float32 sig[], Word16 L_frame,
		       Word32 lag_max, Word32 lag_min, Float32 *cor_max,
		       Word32 dtx, Float32 *rmax, Float32 *r0 )
#else
static Word16 Lag_max( vadState *vadSt, Float32 corr[], Float32 sig[], Word16
      L_frame, Word32 lag_max, Word32 lag_min, Float32 *cor_max, Word32 dtx )
#endif
{
   Float32 max, T0;
   Float32 *p;
   Word32 i, j, p_max;
   max = -FLT_MAX;
   p_max = lag_max;
   for ( i = lag_max, j = ( PIT_MAX - lag_max - 1 ); i >= lag_min; i--, j-- ) {
      if ( corr[ - i] >= max ) {
         max = corr[ - i];
         p_max = i;
      }
   }
   /* compute energy for normalization */
   T0 = 0.0F;
   p = &sig[ - p_max];
   for ( i = 0; i < L_frame; i++, p++ ) {
      T0 += *p * *p;
   }
   if ( dtx ) {
#ifdef VAD2
     *rmax = max;
     *r0 = T0;
#else
     /* check tone */
     vad_tone_detection( vadSt, max, T0 );
#endif
   }
   if ( T0 > 0.0F )
      T0 = 1.0F / ( Float32 )sqrt( T0 );
   else
      T0 = 0.0F;
   /* max = max/sqrt(energy) */
   max *= T0;
   *cor_max = max;
   return( ( Word16 )p_max );
}
/*
 * hp_max
 *
 *
 * Parameters:
 *    corr           I: correlation vector
 *    sig            I: signal
 *    L_frame        I: length of frame to compute pitch
 *    lag_max        I: maximum lag
 *    lag_min        I: minimum lag
 *    cor_hp_max     O: max high-pass filtered correlation
 *
 * Function:
 *    Find the maximum correlation of scal_sig[] in a given delay range.
 *
 *    The correlation is given by
 *       cor[t] = <scal_sig[n],scal_sig[n-t]>,  t=lag_min,...,lag_max
 *    The functions outputs the maximum correlation after normalization
 *    and the corresponding lag.
 *
 * Returns:
 *    void
 */
#ifndef VAD2
static void hp_max( Float32 corr[], Float32 sig[], Word32 L_frame, Word32
      lag_max, Word32 lag_min, Float32 *cor_hp_max )
{
   Float32 T0, t1, max;
   Float32 *p, *p1;
   Word32 i;
   max = -FLT_MAX;
   T0 = 0;
   for ( i = lag_max - 1; i > lag_min; i-- ) {
      /* high-pass filtering */
      T0 = ( ( corr[ - i] * 2 ) - corr[ - i-1] )-corr[ - i + 1];
      T0 = ( Float32 )fabs( T0 );
      if ( T0 >= max ) {
         max = T0;
      }
   }
   /* compute energy */
   p = sig;
   p1 = &sig[0];
   T0 = 0;
   for ( i = 0; i < L_frame; i++, p++, p1++ ) {
      T0 += *p * *p1;
   }
   p = sig;
   p1 = &sig[ - 1];
   t1 = 0;
   for ( i = 0; i < L_frame; i++, p++, p1++ ) {
      t1 += *p * *p1;
   }
   /* high-pass filtering */
   T0 = T0 - t1;
   T0 = ( Float32 )fabs( T0 );
   /* max/T0 */
   if ( T0 != 0 ) {
      *cor_hp_max = max / T0;
   }
   else {
      *cor_hp_max = 0;
   }
}
#endif
/*
 * vad_tone_detection_update
 *
 *
 * Parameters:
 *    st->tone          B: flags indicating presence of a tone
 *    one_lag_per_frame I: 1 open-loop lag is calculated per each frame
 *
 * Function:
 *    Update the tone flag register.
 *
 * Returns:
 *    void
 */
#ifndef VAD2
static void vad_tone_detection_update( vadState *st, Word16 one_lag_per_frame )
{
   /* Shift tone flags right by one bit */
   st->tone = st->tone >> 1;
   /*
    * If open-loop lag is calculated only once in each frame,
    * do extra update and assume that the other tone flag
    * of the frame is one.
    */
   if ( one_lag_per_frame != 0 ) {
      st->tone = st->tone >> 1;
      st->tone = st->tone | 0x00002000;
   }
}
#endif
/*
 * Pitch_ol
 *
 *
 * Parameters:
 *    mode           I: AMR mode
 *    vadSt          B: VAD state struct
 *    signal         I: signal used to compute the open loop pitch
 *                                                 [[-pit_max]:[-1]]
 *    pit_min        I: minimum pitch lag
 *    pit_max        I: maximum pitch lag
 *    L_frame        I: length of frame to compute pitch
 *    dtx            I: DTX flag
 *    idx            I: frame index
 *
 * Function:
 *    Compute the open loop pitch lag.
 *
 *    Open-loop pitch analysis is performed twice per frame (each 10 ms)
 *    to find two estimates of the pitch lag in each frame.
 *    Open-loop pitch analysis is performed as follows.
 *    In the first step, 3 maxima of the correlation:
 *
 *          79
 *    O(k) = SUM Sw(n)*Sw(n-k)
 *          n=0
 *
 *    are found in the three ranges:
 *       pit_min     ...      2*pit_min-1
 *       2*pit_min   ...      4*pit_min-1
 *       4*pit_min   ...      pit_max
 *
 *    The retained maxima O(t(i)), i = 1, 2, 3, are normalized by dividing by
 *
 *    SQRT[SUM[POW(Sw(n-t(i)), 2]], i = 1, 2, 3,
 *         n
 *
 *    respectively.
 *    The normalized maxima and corresponding delays are denoted by
 *    (M(i), t(i)), i = 1, 2, 3. The winner, Top, among the three normalized
 *    correlations is selected by favouring the delays with the values
 *    in the lower range. This is performed by weighting the normalized
 *    correlations corresponding to the longer delays. The best
 *    open-loop delay Top is determined as follows:
 *
 *    Top = t(1)
 *    M(Top) = M(1)
 *    if M(2) > 0.85 * M(Top)
 *       M(Top) = M(2)
 *       Top = t(2)
 *    end
 *    if M(3) > 0.85 * M(Top)
 *       M(Top) = M(3)
 *       Top = t(3)
 *    end
 *
 * Returns:
 *    void
 */
static Word32 Pitch_ol( enum Mode mode, vadState *vadSt, Float32 signal[],
      Word32 pit_min, Word32 pit_max, Word16 L_frame, Word32 dtx, Word16 idx )
{
   Float32 corr[PIT_MAX + 1];
   Float32 max1, max2, max3, p_max1, p_max2, p_max3;
   Float32 *corr_ptr;
   Word32 i, j;
#ifdef VAD2
   Float32 r01, r02, r03;
   Float32 rmax1, rmax2, rmax3;
#else
   Float32 corr_hp_max;
#endif
#ifndef VAD2
   if ( dtx ) {
      /* update tone detection */
      if ( ( mode == MR475 ) || ( mode == MR515 ) ) {
         vad_tone_detection_update( vadSt, 1 );
      }
      else {
         vad_tone_detection_update( vadSt, 0 );
      }
   }
#endif
   corr_ptr = &corr[pit_max];
   /*        79             */
   /* O(k) = SUM Sw(n)*Sw(n-k)   */
   /*        n=0               */
   comp_corr( signal, L_frame, pit_max, pit_min, corr_ptr );
#ifdef VAD2
   /* Find a maximum for each section.	*/
   /* Maxima 1	*/
   j = pit_min << 2;
   p_max1 =
     Lag_max( corr_ptr, signal, L_frame, pit_max, j, &max1, dtx, &rmax1, &r01 );
   /* Maxima 2	*/
   i = j - 1;
   j = pit_min << 1;
   p_max2 = Lag_max( corr_ptr, signal, L_frame, i, j, &max2, dtx, &rmax2, &r02 );
   /* Maxima 3	*/
   i = j - 1;
   p_max3 =
     Lag_max( corr_ptr, signal, L_frame, i, pit_min, &max3, dtx, &rmax3, &r03 );
#else
   /* Find a maximum for each section.	*/
   /* Maxima 1	*/
   j = pit_min << 2;
   p_max1 = Lag_max( vadSt, corr_ptr, signal, L_frame, pit_max, j, &max1, dtx );
   /* Maxima 2 */
   i = j - 1;
   j = pit_min << 1;
   p_max2 = Lag_max( vadSt, corr_ptr, signal, L_frame, i, j, &max2, dtx );
   /* Maxima 3 */
   i = j - 1;
   p_max3 = Lag_max( vadSt, corr_ptr, signal, L_frame, i, pit_min, &max3, dtx );
   if ( dtx ) {
      if ( idx == 1 ) {
         /* calculate max high-passed filtered correlation of all lags */
         hp_max( corr_ptr, signal, L_frame, pit_max, pit_min, &corr_hp_max );
         /* update complex background detector */
         vadSt->best_corr_hp = corr_hp_max * 0.5F;
      }
   }
#endif
   /* The best open-loop delay */
   if ( ( max1 * 0.85F ) < max2 ) {
      max1 = max2;
      p_max1 = p_max2;
#ifdef VAD2
      if (dtx) {
	rmax1 = rmax2;
	r01 = r02;
      }
#endif
   }
   if ( ( max1 * 0.85F ) < max3 ) {
      p_max1 = p_max3;
#ifdef VAD2
      if (dtx) {
	rmax1 = rmax3;
	r01 = r03;
      }
#endif
   }
#ifdef VAD2
   if (dtx) {
     vadSt->Rmax += rmax1;   /* Save max correlation */
     vadSt->R0   += r01;     /* Save max energy */
   }
#endif
   return( Word32 )p_max1;
}
/*
 * Lag_max_wght
 *
 *
 * Parameters:
 *    vadSt          B: vad structure
 *    corr           I: correlation vector
 *    signal         I: signal
 *    L_frame        I: length of frame to compute pitch
 *    old_lag        I: old open-loop lag
 *    cor_max        O: maximum correlation
 *    wght_flg       I: weighting function flag
 *    gain_flg       O: open-loop flag
 *    dtx            I: dtx on/off
 *
 * Function:
 *    Find the lag that has maximum correlation of signal in a given delay range.
 *    maximum lag = 143
 *    minimum lag = 20
 *
 * Returns:
 *    p_max             lag found
 */
static Word32 Lag_max_wght( vadState *vadSt, Float32 corr[], Float32 signal[],
      Word32 old_lag, Word32 *cor_max, Word32 wght_flg, Float32 *gain_flg,
      Word32 dtx )
{
   Float32 t0, t1, max;
   Float32 *psignal, *p1signal;
   const Float32 *ww, *we;
   Word32 i, j, p_max;
   ww = &corrweight[250];
   we = &corrweight[266 - old_lag];
   max = -FLT_MAX;
   p_max = PIT_MAX;
   /* see if the neigbouring emphasis is used */
   if ( wght_flg > 0 ) {
      /* find maximum correlation with weighting */
      for ( i = PIT_MAX; i >= PIT_MIN; i-- ) {
         /* Weighting of the correlation function. */
         t0 = corr[ - i] * *ww--;
          /* Weight the neighbourhood of the old lag. */
         t0 *= *we--;
         if ( t0 >= max ) {
            max = t0;
            p_max = i;
         }
      }
   }
   else {
      /* find maximum correlation with weighting */
      for ( i = PIT_MAX; i >= PIT_MIN; i-- ) {
         /* Weighting of the correlation function. */
         t0 = corr[ - i] * *ww--;
         if ( t0 >= max ) {
            max = t0;
            p_max = i;
         }
      }
   }
   psignal = &signal[0];
   p1signal = &signal[ - p_max];
   t0 = 0;
   t1 = 0;
   /* Compute energy */
   for ( j = 0; j < L_FRAME_BY2; j++, psignal++, p1signal++ ) {
      t0 += *psignal * *p1signal;
      t1 += *p1signal * *p1signal;
   }
   if ( dtx ) {
#ifdef VAD2
       vadSt->Rmax += t0;   /* Save max correlation */
       vadSt->R0   += t1;   /* Save max energy */
#else
      /* update and detect tone */
      vad_tone_detection_update( vadSt, 0 );
      vad_tone_detection( vadSt, t0, t1 );
#endif
   }
   /*
    * gain flag is set according to the open_loop gain
    * is t2/t1 > 0.4 ?
    */
   *gain_flg = t0 - ( t1 * 0.4F );
   *cor_max = 0;
   return( p_max );
}
/*
 * gmed_n
 *
 *
 * Parameters:
 *    ind               I: values
 *    n                 I: The number of gains
 *
 * Function:
 *    Calculates N-point median.
 *
 * Returns:
 *    index of the median value
 */
static Word32 gmed_n( Word32 ind[], Word32 n )
{
   Word32 i, j, ix = 0;
   Word32 max;
   Word32 medianIndex;
   Word32 tmp[9];
   Word32 tmp2[9];
   for ( i = 0; i < n; i++ ) {
      tmp2[i] = ind[i];
   }
   for ( i = 0; i < n; i++ ) {
      max = -32767;
      for ( j = 0; j < n; j++ ) {
         if ( tmp2[j] >= max ) {
            max = tmp2[j];
            ix = j;
         }
      }
      tmp2[ix] = -32768;
      tmp[i] = ix;
   }
   medianIndex = tmp[( n >>1 )];
   return( ind[medianIndex] );
}
/*
 * Pitch_ol_wgh
 *
 *
 * Parameters:
 *    old_T0_med     O: old Cl lags median
 *    wght_flg       I: weighting function flag
 *    ada_w          B:
 *    vadSt          B: VAD state struct
 *    signal         I: signal used to compute the open loop pitch
 *                                                  [[-pit_max]:[-1]]
 *    old_lags       I: history with old stored Cl lags
 *    ol_gain_flg    I: OL gain flag
 *    idx            I: frame index
 *    dtx            I: DTX flag
 *
 * Function:
 *    Open-loop pitch search with weight
 *
 *    Open-loop pitch analysis is performed twice per frame (every 10 ms)
 *    for the 10.2 kbit/s mode to find two estimates of the pitch lag
 *    in each frame. The open-loop pitch analysis is done in order to simplify
 *    the pitch analysis and confine the closed loop pitch search to
 *    a small number of lags around the open-loop estimated lags.
 *    Open-loop pitch estimation is based on the weighted speech signal
 *    which is obtained by filtering the input speech signal through
 *    the weighting filter.
 *    The correlation of weighted speech is determined.
 *    The estimated pitch-lag is the delay that maximises
 *    the weighted autocorrelation function. To enhance  pitch-lag analysis
 *    the autocorrelation function estimate is modified by a weighting window.
 *    The weighting emphasises relevant pitch-lags, thus increasing
 *    the likelihood of selecting the correct delay.
 *    minimum pitch lag = 20
 *    maximum pitch lag = 143
 *
 * Returns:
 *    p_max1            open loop pitch lag
 */
static Word32 Pitch_ol_wgh( Word32 *old_T0_med, Word16 *wght_flg, Float32 *ada_w,
      vadState *vadSt, Float32 signal[], Word32 old_lags[], Float32 ol_gain_flg[],
      Word16 idx, Word32 dtx )
{
   Float32 corr[PIT_MAX + 1];
#ifndef VAD2
   Float32 corr_hp_max;
#endif
   Float32 *corrPtr;
   Word32 i, max1, p_max1;
   /* calculate all coreelations of signal, from pit_min to pit_max */
   corrPtr = &corr[PIT_MAX];
   comp_corr( signal, L_FRAME_BY2, PIT_MAX, PIT_MIN, corrPtr );
   p_max1 = Lag_max_wght( vadSt, corrPtr, signal, *old_T0_med,
         &max1, *wght_flg, &ol_gain_flg[idx], dtx );
   if ( ol_gain_flg[idx] > 0 ) {
      /* Calculate 5-point median of previous lags */
      /* Shift buffer */
      for ( i = 4; i > 0; i-- ) {
         old_lags[i] = old_lags[i - 1];
      }
      old_lags[0] = p_max1;
      *old_T0_med = gmed_n( old_lags, 5 );
      *ada_w = 1;
   }
   else {
      *old_T0_med = p_max1;
      *ada_w = *ada_w * 0.9F;
   }
   if ( *ada_w < 0.3 ) {
      *wght_flg = 0;
   }
   else {
      *wght_flg = 1;
   }
#ifndef VAD2
   if ( dtx ) {
      if ( idx == 1 ) {
         /* calculate max high-passed filtered correlation of all lags */
         hp_max( corrPtr, signal, L_FRAME_BY2, PIT_MAX, PIT_MIN, &corr_hp_max );
         /* update complex background detector */
         vadSt->best_corr_hp = corr_hp_max * 0.5F;
      }
   }
#endif
   return( p_max1 );
}
/*
 * ol_ltp
 *
 *
 * Parameters:
 *    mode              I: AMR mode
 *    vadSt             B: VAD state struct
 *    wsp               I: signal used to compute the OL pitch
 *    T_op              O: open loop pitch lag
 *    ol_gain_flg       I: OL gain flag
 *    old_T0_med        O: old Cl lags median
 *    wght_flg          I: weighting function flag
 *    ada_w             B:
 *    old_lags          I: history with old stored Cl lags
 *    ol_gain_flg       I: OL gain flag
 *    dtx               I: DTX flag
 *    idx               I: frame index
 *
 * Function:
 *    Compute the open loop pitch lag.
 *
 *    Open-loop pitch analysis is performed in order to simplify
 *    the pitch analysis and confine the closed-loop pitch search to
 *    a small number of lags around the open-loop estimated lags.
 *    Open-loop pitch estimation is based on the weighted speech signal Sw(n)
 *    which is obtained by filtering the input speech signal through
 *    the weighting filter W(z) = A(z/g1) / A(z/g2). That is,
 *    in a subframe of size L, the weighted speech is given by:
 *
 *                10
 *    Sw(n) = S(n) + SUM[ a(i) * g1(i) * S(n-i) ]
 *                i=1
 *                   10
 *                - SUM[ a(i) * g2(i) * Sw(n-i) ], n = 0, ..., L-1
 *                  i=1
 *
 * Returns:
 *    void
 */
static void ol_ltp( enum Mode mode, vadState *vadSt, Float32 wsp[], Word32 *T_op
      , Float32 ol_gain_flg[], Word32 *old_T0_med, Word16 *wght_flg, Float32 *ada_w
      , Word32 *old_lags, Word32 dtx, Word16 idx )
{
   if ( mode != MR102 ) {
      ol_gain_flg[0] = 0;
      ol_gain_flg[1] = 0;
   }
   if ( ( mode == MR475 ) || ( mode == MR515 ) ) {
      *T_op = Pitch_ol( mode, vadSt, wsp, PIT_MIN, PIT_MAX, L_FRAME, dtx, idx );
   }
   else {
      if ( mode <= MR795 ) {
         *T_op = Pitch_ol( mode, vadSt, wsp, PIT_MIN, PIT_MAX, L_FRAME_BY2, dtx,
               idx );
      }
      else if ( mode == MR102 ) {
         *T_op = Pitch_ol_wgh( old_T0_med, wght_flg, ada_w, vadSt, wsp, old_lags,
            ol_gain_flg, idx, dtx );
      }
      else {
         *T_op = Pitch_ol( mode, vadSt, wsp, PIT_MIN_MR122, PIT_MAX, L_FRAME_BY2
               , dtx, idx );
      }
   }
}
/*
 * subframePreProc
 *
 *
 * Parameters:
 *    mode           I: AMR mode
 *    gamma1         I: spectral exp. factor 1
 *    gamma1_12k2    I: spectral exp. factor 1 for EFR
 *    gamma2         I: spectral exp. factor 2
 *    A              I: A(z) unquantized for the 4 subframes
 *    Aq             I: A(z)   quantized for the 4 subframes
 *    speech         I: speech segment
 *    mem_err        I: pointer to error signal
 *    mem_w0         I: memory of weighting filter
 *    zero           I: pointer to zero vector
 *    ai_zero        O: history of weighted synth. filter
 *    exc            O: long term prediction residual
 *    h1             O: impulse response
 *    xn             O: target vector for pitch search
 *    res2           O: long term prediction residual
 *    error          O: error of LPC synthesis filter
 *
 * Function:
 *    Subframe preprocessing
 *
 *    Impulse response computation:
 *       The impulse response, h(n), of the weighted synthesis filter
 *
 *       H(z) * W(z) = A(z/g1) / ( A'(z) * A(z/g2) )
 *
 *       is computed each subframe. This impulse response is needed for
 *       the search of adaptive and fixed codebooks. The impulse response h(n)
 *       is computed by filtering the vector of coefficients of
 *       the filter A(z/g1) extended by zeros through the two filters
 *       1/A'(z) and 1/A(z/g2).
 *
 *    Target signal computation:
 *       The target signal for adaptive codebook search is usually computed
 *       by subtracting the zero input response of
 *       the weighted synthesis filter H(z) * W(z) from the weighted
 *       speech signal Sw(n). This is performed on a subframe basis.
 *       An equivalent procedure for computing the target signal is
 *       the filtering of the LP residual signal res(n) through
 *       the combination of synthesis filter 1/A'(z) and
 *       the weighting filter A(z/g1)/A(z/g2). After determining
 *       the excitation for the subframe, the initial states of
 *       these filters are updated by filtering the difference between
 *       the LP residual and excitation.
 *
 *       The residual signal res(n) which is needed for finding
 *       the target vector is also used in the adaptive codebook search
 *       to extend the past excitation buffer. This simplifies
 *       the adaptive codebook search procedure for delays less than
 *       the subframe size of 40. The LP residual is given by:
 *
 *                        10
 *       res(n) = S(n) + SUM[A'(i)* S(n-i)
 *                       i=1
 *
 * Returns:
 *    void
 */
static void subframePreProc( enum Mode mode, const Float32 gamma1[], const
      Float32 gamma1_12k2[], const Float32 gamma2[], Float32 *A, Float32 *Aq,
      Float32 *speech, Float32 *mem_err, Float32 *mem_w0, Float32 *zero, Float32
      ai_zero[], Float32 *exc, Float32 h1[], Float32 xn[], Float32 res2[],
      Float32 error[] )
{
   Float32 Ap1[MP1];   /* weighted LPC coefficients */
   Float32 Ap2[MP1];   /* weighted LPC coefficients */
   const Float32 *g1;
   /* mode specific pointer to gamma1 values */
   g1 = gamma1;
   if ( ( mode == MR122 ) || ( mode == MR102 ) ) {
      g1 = gamma1_12k2;
   }
   /* Find the weighted LPC coefficients for the weighting filter. */
   Weight_Ai( A, g1, Ap1 );
   Weight_Ai( A, gamma2, Ap2 );
   /*
    * Compute impulse response, h1[],
    * of weighted synthesis filter A(z/g1)/A(z/g2)
    */
   memcpy( ai_zero, Ap1, MP1 <<2 );
   Syn_filt( Aq, ai_zero, h1, zero, 0 );
   Syn_filt( Ap2, h1, h1, zero, 0 );
   /*
    * Find the target vector for pitch search:
    */
   /* LP residual */
   Residu( Aq, speech, res2 );
   memcpy( exc, res2, L_SUBFR <<2 );
   /* Synthesis filter */
   Syn_filt( Aq, exc, error, mem_err, 0 );
   Residu( Ap1, error, xn );
   /* target signal xn[] */
   Syn_filt( Ap2, xn, xn, mem_w0, 0 );
}
/*
 * getRange
 *
 *
 * Parameters:
 *    T0                I: integer pitch
 *    delta_low         I: search start offset
 *    delta_range       I: search range
 *    pitmin            I: minimum pitch
 *    pitmax            I: maximum pitch
 *    T0_min            I: search range minimum
 *    T0_max            I: search range maximum
 *
 * Function:
 *    Sets range around open-loop pitch or integer pitch of last subframe
 *
 *    Takes integer pitch T0 and calculates a range around it with
 *    T0_min = T0-delta_low and T0_max = (T0-delta_low) + delta_range
 *    T0_min and T0_max are bounded by pitmin and pitmax
 *
 * Returns:
 *    void
 */
static void getRange( Word32 T0, Word16 delta_low, Word16 delta_range,
      Word16 pitmin, Word16 pitmax, Word32 *T0_min, Word32 *T0_max )
{
   *T0_min = T0 - delta_low;
   if ( *T0_min < pitmin ) {
      *T0_min = pitmin;
   }
   *T0_max = *T0_min + delta_range;
   if ( *T0_max > pitmax ) {
      *T0_max = pitmax;
      *T0_min = *T0_max - delta_range;
   }
}
/*
 * Norm_Corr
 *
 *
 * Parameters:
 *    exc         I: excitation buffer                      [L_SUBFR]
 *    xn          I: target vector                          [L_SUBFR]
 *    h           I: impulse response of synthesis and weighting filters
 *                                                          [L_SUBFR]
 *    t_min       I: interval to compute normalized correlation
 *    t_max       I: interval to compute normalized correlation
 *    corr_norm   O: Normalized correlation                 [wT_min-wT_max]
 *
 * Function:
 *    Normalized correlation
 *
 *    The closed-loop pitch search is performed by minimizing
 *    the mean-square weighted error between the original and
 *    synthesized speech. This is achieved by maximizing the term:
 *
 *            39                           39
 *    R(k) = SUM[ X(n) * Yk(n)) ] / SQRT[ SUM[ Yk(n) * Yk(n)] ]
 *           n=0                          n=0
 *
 *    where X(n) is the target signal and Yk(n) is the past filtered
 *    excitation at delay k (past excitation convolved with h(n) ).
 *    The search range is limited around the open-loop pitch.
 *
 *    The convolution Yk(n) is computed for the first delay t_min in
 *    the searched range, and for the other delays in the search range
 *    k = t_min + 1, ..., t_max, it is updated using the recursive relation:
 *
 *    Yk(n) = Yk-1(n-1) + u(-k) * h(n),
 *
 *    where u(n), n = -( 143 + 11 ), ..., 39, is the excitation buffer.
 *    Note that in search stage, the samples u(n), n = 0, ..., 39,
 *    are not known, and they are needed for pitch delays less than 40.
 *    To simplify the search, the LP residual is copied to u(n) in order
 *    to make the relation in above equation valid for all delays.
 *
 * Returns:
 *    void
 */
static void Norm_Corr( Float32 exc[], Float32 xn[], Float32 h[], Word32 t_min,
      Word32 t_max, Float32 corr_norm[] )
{
   Float32 exc_temp[L_SUBFR];
   Float32 *p_exc;
   Float32 corr, norm;
   Float32 sum;
   Word32 i, j, k;
   k = -t_min;
   p_exc = &exc[ - t_min];
   /* compute the filtered excitation for the first delay t_min */
   /* convolution Yk(n) */
   for ( j = 0; j < L_SUBFR; j++ ) {
      sum = 0;
      for ( i = 0; i <= j; i++ ) {
         sum += p_exc[i] * h[j - i];
      }
      exc_temp[j] = sum;
   }
   /* loop for every possible period */
   for ( i = t_min; i <= t_max; i++ ) {
      /*        39                     */
      /* SQRT[ SUM[ Yk(n) * Yk(n)] ]   */
      /*       n=0                     */
      norm = (Float32)Dotproduct40( exc_temp, exc_temp );
      if ( norm == 0 )
         norm = 1.0;
      else
         norm = ( Float32 )( 1.0 / ( sqrt( norm ) ) );
      /*        39                  */
      /* SQRT[ SUM[ X(n) * Yk(n)] ] */
      /*       n=0                  */
      corr = (Float32)Dotproduct40( xn, exc_temp );
      /* R(k) */
      corr_norm[i] = corr * norm;
      /* modify the filtered excitation exc_tmp[] for the next iteration */
      if ( i != t_max ) {
         k--;
         for ( j = L_SUBFR - 1; j > 0; j-- ) {
            /* Yk(n) = Yk-1(n-1) + u(-k) * h(n) */
            exc_temp[j] = exc_temp[j - 1] + exc[k] * h[j];
         }
         exc_temp[0] = exc[k];
      }
   }
}
/*
 * Interpol_3or6
 *
 *
 * Parameters:
 *    x                 I: input vector
 *    frac              I: fraction  (-2..2 for 3*, -3..3 for 6*)
 *    flag3             I: if set, upsampling rate = 3 (6 otherwise)
 *
 * Function:
 *    Interpolating the normalized correlation with 1/3 or 1/6 resolution.
 *
 *    The interpolation is performed using an FIR filter b24
 *    based on a Hamming windowed sin(x)/x function truncated at 

						