/*
   ITU-T G.729 Speech Coder ANSI-C Source Code
   Version 3.3    Last modified: December 26, 1995

   Copyright (c) 1996,
   AT&T, France Telecom, NTT, Universite de Sherbrooke, Lucent Technologies
   All rights reserved.
*/

/*---------------------------------------------------------------------------*
 * procedure Pitch_ol                                                        *
 * ~~~~~~~~~~~~~~~~~~                                                        *
 * Compute the open loop pitch lag.                                          *
 *                                                                           *
 *---------------------------------------------------------------------------*/

#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "ld8k.h"
#include "tab_ld8k.h"

/* local function */

static Word16 Lag_max(        /* output: lag found                           */
  Word16 signal[],     /* input : signal used to compute the open loop pitch */
  Word16 L_frame,      /* input : length of frame to compute pitch           */
  Word16 lag_max,      /* input : maximum lag                                */
  Word16 lag_min,      /* input : minimum lag                                */
  Word16 *cor_max);    /* output: normalized correlation of selected lag     */


Word16 Pitch_ol(       /* output: open loop pitch lag                        */
   Word16 signal[],    /* input : signal used to compute the open loop pitch */
                       /*     signal[-pit_max] to signal[-1] should be known */
   Word16   pit_min,   /* input : minimum pitch lag                          */
   Word16   pit_max,   /* input : maximum pitch lag                          */
   Word16   L_frame    /* input : length of frame to compute pitch           */
)
{
  Word16  i, j;
  Word16  max1, max2, max3;
  Word16  p_max1, p_max2, p_max3;
  Word32  t0, L_temp;

  /* Scaled signal */

  Word16 scaled_signal[L_FRAME+PIT_MAX];
  Word16 *scal_sig;

  scal_sig = &scaled_signal[pit_max];

  /*--------------------------------------------------------*
   *  Verification for risk of overflow.                    *
   *--------------------------------------------------------*/

   Overflow = 0;
   t0 = 0;

   for(i= -pit_max; i< L_frame; i++)
     t0 = L_mac(t0, signal[i], signal[i]);

  /*--------------------------------------------------------*
   * Scaling of input signal.                               *
   *                                                        *
   *   if Overflow        -> scal_sig[i] = signal[i]>>3     *
   *   else if t0 < 1^20  -> scal_sig[i] = signal[i]<<3     *
   *   else               -> scal_sig[i] = signal[i]        *
   *--------------------------------------------------------*/

   if(Overflow == 1)
   {
     for(i=-pit_max; i<L_frame; i++)
       scal_sig[i] = shr(signal[i], 3);
   }
   else {
     L_temp = L_sub(t0, (Word32)1048576L);
     if ( L_temp < (Word32)0 )  /* if (t0 < 2^20) */
     {
        for(i=-pit_max; i<L_frame; i++)
          scal_sig[i] = shl(signal[i], 3);
     }
     else
     {
       for(i=-pit_max; i<L_frame; i++)
         scal_sig[i] = signal[i];
     }
   }
  /*--------------------------------------------------------------------*
   *  The pitch lag search is divided in three sections.                *
   *  Each section cannot have a pitch multiple.                        *
   *  We find a maximum for each section.                               *
   *  We compare the maximum of each section by favoring small lag.     *
   *                                                                    *
   *  First section:  lag delay = pit_max     downto 4*pit_min          *
   *  Second section: lag delay = 4*pit_min-1 downto 2*pit_min          *
   *  Third section:  lag delay = 2*pit_min-1 downto pit_min            *
   *--------------------------------------------------------------------*/


   j = shl(pit_min, 2);
   p_max1 = Lag_max(scal_sig, L_frame, pit_max, j, &max1);

   i = sub(j, 1); j = shl(pit_min, 1);
   p_max2 = Lag_max(scal_sig, L_frame, i, j, &max2);

   i = sub(j, 1);
   p_max3 = Lag_max(scal_sig, L_frame, i, pit_min , &max3);

  /*--------------------------------------------------------------------*
   * Compare the 3 sections maximum, and favor small lag.               *
   *--------------------------------------------------------------------*/

  if( sub(mult(max1, THRESHPIT), max2)  < 0)
  {
    max1 = max2;
    p_max1 = p_max2;
  }

  if( sub(mult(max1, THRESHPIT), max3)  < 0)
  {
    p_max1 = p_max3;
  }


  return (p_max1);
}

/*---------------------------------------------------------------------------*
 * procedure Lag_max                                                         *
 * ~~~~~~~~~~~~~~~~~                                                         *
 * Find the lag that has maximum correlation with scal_sig[]                 *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * arguments:                                                                *
 *                                                                           *
 *   signal[]   :Signal used to compute the open loop pitch.                 *
 *   L_frame    :Length of frame to compute pitch.                           *
 *   lag_max    :Maximum lag                                                 *
 *   lag_min    :Minimum lag                                                 *
 *   *cor_max   ;Maximum of normalized correlation of lag found.             *
 *                                                                           *
 *   Return lag found.                                                       *
 *--------------------------------------------------------------------------*/

static Word16 Lag_max( /* output: lag found                                  */
  Word16 signal[],     /* input : signal used to compute the open loop pitch */
  Word16 L_frame,      /* input : length of frame to compute pitch           */
  Word16 lag_max,      /* input : maximum lag                                */
  Word16 lag_min,      /* input : minimum lag                                */
  Word16 *cor_max)     /* output: normalized correlation of selected lag     */
{
  Word16  i, j;
  Word16  *p, *p1;
  Word32  max, t0, L_temp;
  Word16  max_h, max_l, ener_h, ener_l;
  Word16  p_max;

  max = MIN_32;

   /* initialization used only to suppress Microsoft Visual C++  warnings */

  p_max = lag_max;

  for (i = lag_max; i >= lag_min; i--)
  {
    p  = signal;
    p1 = &signal[-i];
    t0 = 0;

    for (j=0; j<L_frame; j++, p++, p1++)
      t0 = L_mac(t0, *p, *p1);

    L_temp = L_sub(t0,max);
    if (L_temp >= 0L)
    {
      max    = t0;
      p_max = i;
    }
  }

  /* compute energy */

  t0 = 0;
  p = &signal[-p_max];
  for(i=0; i<L_frame; i++, p++)
    t0 = L_mac(t0, *p, *p);

  /* 1/sqrt(energy),    result in Q30 */

  t0 = Inv_sqrt(t0);

  /* max = max/sqrt(energy)                   */
  /* This result will always be on 16 bits !! */

  L_Extract(max, &max_h, &max_l);
  L_Extract(t0, &ener_h, &ener_l);

  t0 = Mpy_32(max_h, max_l, ener_h, ener_l);
  *cor_max = extract_l(t0);

  return(p_max);
}

/*--------------------------------------------------------------------------*
 * Function  Pitch_fr3()                                                    *
 * ~~~~~~~~~~~~~~~~~~~~~                                                    *
 * Find the pitch period with 1/3 subsample resolution.                     *
 *--------------------------------------------------------------------------*/

        /* Local functions */

static void Norm_Corr(Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
                      Word16 t_min, Word16 t_max, Word16 corr_norm[]);


Word16 Pitch_fr3(    /* (o)     : pitch period.                          */
  Word16 exc[],      /* (i)     : excitation buffer                      */
  Word16 xn[],       /* (i)     : target vector                          */
  Word16 h[],        /* (i) Q12 : impulse response of filters.           */
  Word16 L_subfr,    /* (i)     : Length of subframe                     */
  Word16 t0_min,     /* (i)     : minimum value in the searched range.   */
  Word16 t0_max,     /* (i)     : maximum value in the searched range.   */
  Word16 i_subfr,    /* (i)     : indicator for first subframe.          */
  Word16 *pit_frac   /* (o)     : chosen fraction.                       */
)
{
  Word16 i;
  Word16 t_min, t_max;
  Word16 max, lag, frac;
  Word16 *corr;
  Word16 corr_int;
  Word16 corr_v[40];           /* Total length = t0_max-t0_min+1+2*L_INTER */


  /* Find interval to compute normalized correlation */

  t_min = sub(t0_min, L_INTER4);
  t_max = add(t0_max, L_INTER4);

  corr = &corr_v[-t_min];

  /* Compute normalized correlation between target and filtered excitation */

  Norm_Corr(exc, xn, h, L_subfr, t_min, t_max, corr);

  /* Find integer pitch */

  max = corr[t0_min];
  lag = t0_min;

  for(i= t0_min+(Word16)1; i<=t0_max; i++)
  {
    if( sub(corr[i], max) >= 0)
    {
      max = corr[i];
      lag = i;
    }
  }

  /* If first subframe and lag > 84 do not search fractional pitch */

  if( (i_subfr == 0) && (sub(lag, 84) > 0) )
  {
    *pit_frac = 0;
    return(lag);
  }

  /* Test the fractions around T0 and choose the one which maximizes   */
  /* the interpolated normalized correlation.                          */

  max  = Interpol_3(&corr[lag], -2);
  frac = -2;

  for (i = -1; i <= 2; i++)
  {
    corr_int = Interpol_3(&corr[lag], i);
    if (sub(corr_int, max) > 0)
    {
      max = corr_int;
      frac = i;
    }
  }

  /* limit the fraction value between -1 and 1 */

  if (sub(frac, -2) == 0)
  {
    frac = 1;
    lag = sub(lag, 1);
  }
  if (sub(frac, 2) == 0)
  {
    frac = -1;
    lag = add(lag, 1);
  }

  *pit_frac = frac;


  return(lag);
}

/*---------------------------------------------------------------------------*
 * Function Norm_Corr()                                                      *
 * ~~~~~~~~~~~~~~~~~~~~                                                      *
 * Find the normalized correlation between the target vector and the         *
 * filtered past excitation.                                                 *
 *---------------------------------------------------------------------------*
 * Input arguments:                                                          *
 *     exc[]    : excitation buffer                                          *
 *     xn[]     : target vector                                              *
 *     h[]      : impulse response of synthesis and weighting filters (Q12)  *
 *     L_subfr  : Length of subframe                                         *