/*
 * 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