/*
   ITU-T G.729A Speech Coder    ANSI-C Source Code
   Version 1.1    Last modified: September 1996

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

/*---------------------------------------------------------------------------*
 * Pitch related functions                                                   *
 * ~~~~~~~~~~~~~~~~~~~~~~~                                                   *
 *---------------------------------------------------------------------------*/

#include "typedef.h"
#include "intrindefs.h"
#include "oper_32b.h"
#include "ld8a.h"
#include "tab_ld8a.h"

/*---------------------------------------------------------------------------*
 * Function  Pitch_ol_fast                                                   *
 * ~~~~~~~~~~~~~~~~~~~~~~~                                                   *
 * Compute the open loop pitch lag. (fast version)                           *
 *                                                                           *
 *---------------------------------------------------------------------------*/


Word16 Pitch_ol_fast(  /* 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_max,   /* input : maximum pitch lag                          */
   Word16   L_frame    /* input : length of frame to compute pitch           */
)
{
  Word16  i, j;
  Word16  max1, max2, max3;
  Word16  max_h, max_l, ener_h, ener_l;
  Word16  T1, T2, T3;
  Word16  *p, *p1;
  Word32  max, sum, 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;
   sum = 0;

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

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

   
   
       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 maxima of each section by favoring small lag.      *
    *                                                                    *
    *  First section:  lag delay = 20 to 39                              *
    *  Second section: lag delay = 40 to 79                              *
    *  Third section:  lag delay = 80 to 143                             *
    *--------------------------------------------------------------------*/

    /* First section */

    max = MIN_32;
    T1  = 20;    /* Only to remove warning from some compilers */
    for (i = 20; i < 40; i++) {
        p  = scal_sig;
        p1 = &scal_sig[-i];
        sum = 0;
        for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
            sum = L_mac(sum, *p, *p1);
        L_temp = L_sub(sum, max);
        if (L_temp > 0) { max = sum; T1 = i;   }
    }

    /* compute energy of maximum */

    sum = 1;                   /* to avoid division by zero */
    p = &scal_sig[-T1];
    for(i=0; i<L_frame; i+=2, p+=2)
        sum = L_mac(sum, *p, *p);

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

    sum = Inv_sqrt(sum);            /* 1/sqrt(energy),    result in Q30 */
    L_Extract(max, &max_h, &max_l);
    L_Extract(sum, &ener_h, &ener_l);
    sum  = Mpy_32(max_h, max_l, ener_h, ener_l);
    max1 = extract_l(sum);

    /* Second section */

    max = MIN_32;
    T2  = 40;    /* Only to remove warning from some compilers */
    for (i = 40; i < 80; i++) {
        p  = scal_sig;
        p1 = &scal_sig[-i];
        sum = 0;
        for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
            sum = L_mac(sum, *p, *p1);
        L_temp = L_sub(sum, max);
        if (L_temp > 0) { max = sum; T2 = i;   }
    }

    /* compute energy of maximum */

    sum = 1;                   /* to avoid division by zero */
    p = &scal_sig[-T2];
    for(i=0; i<L_frame; i+=2, p+=2)
        sum = L_mac(sum, *p, *p);

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

    sum = Inv_sqrt(sum);            /* 1/sqrt(energy),    result in Q30 */
    L_Extract(max, &max_h, &max_l);
    L_Extract(sum, &ener_h, &ener_l);
    sum  = Mpy_32(max_h, max_l, ener_h, ener_l);
    max2 = extract_l(sum);

    /* Third section */

    max = MIN_32;
    T3  = 80;    /* Only to remove warning from some compilers */
    for (i = 80; i < 143; i+=2) {
        p  = scal_sig;
        p1 = &scal_sig[-i];
        sum = 0;
        for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
            sum = L_mac(sum, *p, *p1);
        L_temp = L_sub(sum, max);
        if (L_temp > 0) { max = sum; T3 = i;   }
    }

     /* Test around max3 */

     i = T3;
     p  = scal_sig;
     p1 = &scal_sig[-(i+1)];
     sum = 0;
     for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
         sum = L_mac(sum, *p, *p1);
     L_temp = L_sub(sum, max);
     if (L_temp > 0) { max = sum; T3 = i+(Word16)1;   }

     p  = scal_sig;
     p1 = &scal_sig[-(i-1)];
     sum = 0;
     for (j=0; j<L_frame; j+=2, p+=2, p1+=2)
         sum = L_mac(sum, *p, *p1);
     L_temp = L_sub(sum, max);
     if (L_temp > 0) { max = sum; T3 = i-(Word16)1;   }

    /* compute energy of maximum */

    sum = 1;                   /* to avoid division by zero */
    p = &scal_sig[-T3];
    for(i=0; i<L_frame; i+=2, p+=2)
        sum = L_mac(sum, *p, *p);

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

    sum = Inv_sqrt(sum);            /* 1/sqrt(energy),    result in Q30 */
    L_Extract(max, &max_h, &max_l);
    L_Extract(sum, &ener_h, &ener_l);
    sum  = Mpy_32(max_h, max_l, ener_h, ener_l);
    max3 = extract_l(sum);

   /*-----------------------*
    * Test for multiple.    *
    *-----------------------*/

    /* if( abs(T2*2 - T3) < 5)  */
    /*    max2 += max3 * 0.25;  */

    i = sub(shl(T2,1), T3);
    j = sub(abs_s(i), 5);
    if(j < 0)
      max2 = add(max2, shr(max3, 2));

    /* if( abs(T2*3 - T3) < 7)  */
    /*    max2 += max3 * 0.25;  */

    i = add(i, T2);
    j = sub(abs_s(i), 7);
    if(j < 0)
      max2 = add(max2, shr(max3, 2));

    /* if( abs(T1*2 - T2) < 5)  */
    /*    max1 += max2 * 0.20;  */

    i = sub(shl(T1,1), T2);
    j = sub(abs_s(i), 5);
    if(j < 0)
      max1 = add(max1, mult(max2, 6554));

    /* if( abs(T1*3 - T2) < 7)  */
    /*    max1 += max2 * 0.20;  */

    i = add(i, T1);
    j = sub(abs_s(i), 7);
    if(j < 0)
      max1 = add(max1, mult(max2, 6554));

   /*--------------------------------------------------------------------*
    * Compare the 3 sections maxima.                                     *
    *--------------------------------------------------------------------*/

    if( sub(max1, max2) < 0 ) {max1 = max2; T1 = T2;  }
    if( sub(max1, max3) <0 )  {T1 = T3; }

    return T1;
}




/*--------------------------------------------------------------------------*
 *  Function  Dot_Product()                                                 *
 *  ~~~~~~~~~~~~~~~~~~~~~~                                                  *
 *--------------------------------------------------------------------------*/

Word32 Dot_Product(      /* (o)   :Result of scalar product. */
       Word16   x[],     /* (i)   :First vector.             */
       Word16   y[],     /* (i)   :Second vector.            */
       Word16   lg       /* (i)   :Number of point.          */
)
{
  Word16 i;
  Word32 sum;

  sum = 0;
  for(i=0; i<lg; i++)
    sum = L_mac(sum, x[i], y[i]);