/*
***********************************************************************
*
* Copyright (C) 1999 Ericsson Radio Systems AB
* 
* The Association of Radio Industries and Businesses (ARIB) has the
* licence to reproduce, distribute and publish the Bit Exact C Code
* programs in this CD-ROM together with the other parts of the RCR
* STD-27 (Personal Digital Cellular Telecommunication System)
* specification.
* 
* Ericsson has declared Option 1 (royalty-free licence) to ARIB for
* its Essential Industrial Property Rights related to the ACELP
* specification and the Bit Exact C Code programs. The royalty-free
* licence is limited to the development and manufacturing of
* products complying with the RCR STD-27H specification (or
* subsequent versions) for sale and use in Japan. Copying of the
* program(s) for any other purpose requires the prior written
* permission from Ericsson.
* 
* The user of the Bit Exact C Code hereby acknowledges the
* following:
* 
* 1.  Ericsson does not provide the user with any warranties,
*     whether express or implied, including but not limited to
*     warranties of merchantability and fitness for a particular
*     purpose relating to the Bit Exact C Code.
* 
* 2.  Ericsson shall have no responsibility whatsoever to indemnify
*     the user against actions or claims for infringement of any
*     intellectual property rights, e.g. patents, copyrights,
*     registered designs or any other rights, by reason of the
*     user's use of the Bit Exact C Code.
* 
***********************************************************************
*/

/*************************************************************************
 *
 *  FUNCTION:  Pitch_ol
 *
 *  PURPOSE: Compute the open loop pitch lag.
 *
 *  DESCRIPTION:
 *      The open-loop pitch lag is determined based on the perceptually
 *      weighted speech signal. This is done in the following steps:
 *        - find three maxima of the correlation <sw[n],sw[n-T]> in the
 *          follwing three ranges of T : [20,39], [40,79], and [80, 143]
 *        - divide each maximum by <sw[n-t], sw[n-t]> where t is the delay at
 *          that maximum correlation.
 *        - select the delay of maximum normalized correlation (among the
 *          three candidates) while favoring the lower delay ranges.
 *
 *************************************************************************/


#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "cnst.h"
#include "sig_proc.h"

#define THRESHOLD 27853

/* local function */

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

Word16 Interpol_3(      /* (o)  : interpolated value  */
    Word16 * x,         /* (i)  : input vector        */
    Word16 frac         /* (i)  : fraction            */
);


Word16 Pitch_ol(      /* (o) : open loop pitch lag                        */
    Word16 signal[],  /* (i) : signal used to compute the open loop pitch */
                      /*   signal[-pit_max] to signal[-1] should be known */
    Word16 pit_min,   /* (i) : minimum pitch lag                          */
    Word16 pit_max,   /* (i) : maximum pitch lag                          */
    Word16 L_frame    /* (i) : 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 != 0) {
        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, THRESHOLD), max2) < 0) {
        max1 = max2;
        p_max1 = p_max2;
    }
    if (sub(mult(max1, THRESHOLD), max3) < 0) {
        p_max1 = p_max3;
    }
    return (p_max1);
}

/*************************************************************************
 *
 *  FUNCTION:  Lag_max
 *
 *  PURPOSE: Find the lag that has maximum correlation of scal_sig[] in a
 *           given delay range.
 *
 *  DESCRIPTION:
 *      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.
 *
 *************************************************************************/

static Word16
 Lag_max(             /* (o) : lag found                                  */
    Word16 signal[],  /* (i) : signal used to compute the open loop pitch */
    Word16 L_frame,   /* (i) : length of frame to compute pitch           */
    Word16 lag_max,   /* (i) : maximum lag                                */
    Word16 lag_min,   /* (i) : minimum lag                                */
    Word16 *cor_max   /* (o) : 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;

    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 >= 0) {
            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) */
    t0 = Inv_sqrt(t0);

    /* max = max/sqrt(energy) */
    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()
 *
 *  PURPOSE: Find the pitch period with 1/3 subsample resolution (closed loop).
 *
 *************************************************************************/

/* L_inter = Length for fractionnal interpolation = nb.coeff/2 */

#define L_INTER 4

 /* Local functions */

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) : 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 midLag, tmpLag;
    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_INTER);
    t_max = add(t0_max, L_INTER);

    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 + 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 fractionnal 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.                          */

    /* 4 bits delta lag */
    if (i_subfr == 0) {
       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;
          }
       }
    } else {
       midLag = sub(t0_max, 4);
       tmpLag = sub(lag, midLag);
       if ((add(tmpLag, 1) == 0) || sub(lag, midLag) == 0) {
          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;
             }
          }
       }
       else if (add(tmpLag, 2) == 0) {
          max  = Interpol_3(&corr[lag], 0);
          frac = 0;
              
          for (i = 1; i <= 2; i++) {
             corr_int = Interpol_3(&corr[lag], i);
             if(sub(corr_int, max) > 0) {
                max = corr_int;
                frac = i;
             }
          }
       }      
       else if (sub(tmpLag, 1) == 0) {
          max  = Interpol_3(&corr[lag], -2);
          frac = -2;
              
          for (i = -1; i <= 0; i++) {
             corr_int = Interpol_3(&corr[lag], i);
             if(sub(corr_int, max) > 0) {
                max = corr_int;
                frac = i;
             }
          }
       }
       else
          frac = 0;
    }

    /* 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()
 *
 *  PURPOSE: Find the normalized correlation between the target vector
 *           and the filtered past excitation.
 *
 *  DESCRIPTION:
 *     The normalized correlation is given by the correlation between the
 *     target and filtered past excitation divided by the square root of
 *     the energy of filtered excitation.
 *                   corr[k] = <x[], y_k[]>/sqrt(y_k[],y_k[])
 *     where x[] is the target vector and y_k[] is the filtered past
 *     excitation at delay k.
 *
 *************************************************************************/

void Norm_Corr(Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
               Word16 t_min, Word16 t_max, Word16 corr_norm[])
{
    Word16 i, j, k;
    Word16 corr_h, corr_l, norm_h, norm_l;
    Word32 s, L_temp;

    Word16 excf[L_SUBFR];
    Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];


    k = -t_min;

    /* compute the filtered excitation for the first delay t_min */

    Convolve(&exc[k], h, excf, L_subfr);

    /* scaled "excf[]" to avoid overflow */

    for (j = 0; j < L_subfr; j++) {
        scaled_excf[j] = shr(excf[j], 2);
    }

    /* Compute energy of excf[] for danger of overflow */

    s = 0;
    for (j = 0; j < L_subfr; j++)
        s = L_mac(s, excf[j], excf[j]);

    L_temp = L_sub(s, 67108864L);
    if (L_temp <= 0) {        /* if (s <= 2^26) */
        s_excf = excf;
        h_fac = 15 - 12;
        scaling = 0;
    } else {
        s_excf = scaled_excf;  /* "excf[]" is divide by 2 */
        h_fac = 15 - 12 - 2;
        scaling = 2;
    }

    /* loop for every possible period */

    for (i = t_min; i <= t_max; i++) {
        /* Compute 1/sqrt(energy of excf[]) */

        s = 0;
        for (j = 0; j < L_subfr; j++)
            s = L_mac(s, s_excf[j], s_excf[j]);

        s = Inv_sqrt(s);
        L_Extract(s, &norm_h, &norm_l);

        /* Compute correlation between xn[] and excf[] */

        s = 0;
        for (j = 0; j < L_subfr; j++)
            s = L_mac(s, xn[j], s_excf[j]);

        L_Extract(s, &corr_h, &corr_l);

        /* Normalize correlation = correlation * (1/sqrt(energy)) */

        s = Mpy_32(corr_h, corr_l, norm_h, norm_l);

        corr_norm[i] = extract_h(L_shl(s, 16));

        /* modify the filtered excitation excf[] for the next iteration */

        if (sub(i, t_max) != 0) {
            k--;
            for (j = L_subfr - 1; j > 0; j--) {
                s = L_mult(exc[k], h[j]);
                s = L_shl(s, h_fac);
                s_excf[j] = add(extract_h(s), s_excf[j - 1]);
            }
            s_excf[0] = shr(exc[k], scaling);
        }
    }
    return;
}

/*************************************************************************
 *
 *  FUNCTION:  G_pitch
 *
 *  PURPOSE:  Compute the pitch (adaptive codebook) gain.
 *
 *  DESCRIPTION:
 *      The adaptive codebook gain is given by
 *
 *              g = <x[], y[]> / <y[], y[]>
 *
 *      where x[] is the target vector, y[] is the filtered adaptive
 *      codevector, and <> denotes dot product.
 *      The gain is limited to the range [0,1.2]
 *
 *************************************************************************/

Word16
G_pitch(              /* (o) : Gain of pitch lag saturated to 1.2       */
    Word16 xn[],      /* (i) : Pitch target.                            */
    Word16 y1[],      /* (i) : Filtered adaptive codebook.              */
    Word16 g_coeff[], /* (i) : Correlations need for gain quantization. */
    Word16 L_subfr    /* (i) : Length of subframe.                      */
)
{
    Word16 i;
    Word16 xy, yy, exp_xy, exp_yy, gain;
    Word32 s;

    Word16 scaled_y1[L_SUBFR];

    /* divide "y1[]" by 4 to avoid overflow */

    for (i = 0; i < L_subfr; i++) {
        scaled_y1[i] = shr(y1[i], 2);
    }

    /* Compute scalar product <y1[],y1[]> */

    Overflow = 0;
    s = 1L;                     /* Avoid case of all zeros */
    for (i = 0; i < L_subfr; i++)
        s = L_mac(s, y1[i], y1[i]);

    if (Overflow == 0) {
        exp_yy = norm_l(s);
        yy = round(L_shl(s, exp_yy));
    } else {
        s = 1L;                 /* Avoid case of all zeros */
        for (i = 0; i < L_subfr; i++)
            s = L_mac(s, scaled_y1[i], scaled_y1[i]);
        exp_yy = norm_l(s);
        yy = round(L_shl(s, exp_yy));
        exp_yy = sub(exp_yy, 4);
    }

    /* Compute scalar product <xn[],y1[]> */

    Overflow = 0;
    s = 1L;         /* Avoid case of all zeros */
    for (i = 0; i < L_subfr; i++)
        s = L_mac(s, xn[i], y1[i]);

    if (Overflow == 0) {
        exp_xy = norm_l(s);
        xy = round(L_shl(s, exp_xy));
    } else {
        s = 1;       /* Avoid case of all zeros */
        for (i = 0; i < L_subfr; i++)
            s = L_mac(s, xn[i], scaled_y1[i]);
        exp_xy = norm_l(s);
        xy = round(L_shl(s, exp_xy));
        exp_xy = sub(exp_xy, 2);
    }

    g_coeff[0] = yy;
    g_coeff[1] = sub(15, exp_yy);
    g_coeff[2] = xy;
    g_coeff[3] = sub(15, exp_xy);

    /* If (xy < 4) gain = 0 */

    i = sub(xy, 4);

    if (i < 0)
        return ((Word16) 0);

    /* compute gain = xy/yy */

    xy = shr(xy, 1);           /* Be sure xy < yy */
    gain = div_s(xy, yy);

    i = sub(exp_xy, exp_yy);
    gain = shr(gain, i);

    if (sub(gain, 19661) > 0) {
        gain = 19661;
    }
    return (gain);
}


/*************************************************************************
 *
 *   FUNCTION:  Enc_lag3
 *
 *   PURPOSE:  Encoding of fractional pitch lag with 1/3 resolution.
 *
 *   DESCRIPTION:
 *                    First and third subframes:
 *                    --------------------------
 *   The pitch range is divided as follows:
 *           19 1/3  to   84 2/3   resolution 1/3
 *           85      to   143      resolution 1
 *
 *   The period is encoded with 8 bits.
 *   For the range with fractions:
 *     index = (T-19)*3 + frac - 1;
 *                         where T=[19..85] and frac=[-1,0,1]
 *   and for the integer only range
 *     index = (T - 85) + 197;        where T=[86..143]
 *
 *                    Second and fourth subframes:
 *                    ----------------------------
 *   For the 2nd and 4th subframes a resolution of 1/3 is always used,
 *   and the search range is relative to the lag in previous subframe.
 *   If t0 is the lag in the previous subframe then
 *   t_min=t0-5   and  t_max=t0+4   and  the range is given by
 *        t_min - 2/3   to  t_max + 2/3
 *
 *   The period in the 2nd (and 4th) subframe is encoded with 5 bits:
 *     index = (T-(t_min-1))*3 + frac - 1;
 *                 where T=[t_min-1..t_max+1]
 *
 *************************************************************************/


Word16
Enc_lag3(            /* (o)  : Return index of encoding */
    Word16 T0,       /* (i)  : Pitch delay              */
    Word16 T0_frac,  /* (i)  : Fractional pitch delay   */
    Word16 * T0_min, /* (i/o): Minimum search delay     */
    Word16 * T0_max, /* (i/o): Maximum search delay     */
    Word16 pit_min,  /* (i)  : Minimum pitch delay      */
    Word16 pit_max,  /* (i)  : Maximum pitch delay      */
    Word16 pit_flag  /* (i)  : Flag for 1st subframe    */
)
{
    Word16 index, i;

    if (pit_flag == 0) {        /* if 1st or 3rd subframe */
        /* encode pitch delay (with fraction) */

        if (sub(T0, 85) <= 0) {
            /* index = t0*3 - 58 + t0_frac   */
            i = add(add(T0, T0), T0);
            index = add(sub(i, 58), T0_frac);
        } else {
            index = add(T0, 112);
        }

        /* find T0_min and T0_max for second (or fourth) subframe */

        *T0_min = sub(T0, 5);
        if (sub(*T0_min, pit_min) < 0) {
            *T0_min = pit_min;
        }
        *T0_max = add(*T0_min, 9);
        if (sub(*T0_max, pit_max) > 0) {
            *T0_max = pit_max;
            *T0_min = sub(*T0_max, 9);
        }
    } else { /* if second or fourth subframe */

       /* 4 bits delta lag */
       i = sub(T0, *T0_min);
          
       if (sub(i, 3) < 0)
          index = i;
       else if (sub(i, 7) < 0) {
          i = sub(i, 3);
          i = add(i, add(i, i));
          index = add(i, add(T0_frac, 3));
       } 
       else {       
          index = add(i, 6);
       }
    }

    return index;
}

/*************************************************************************
 *
 *  FUNCTION:  Interpol_3()
 *
 *  PURPOSE:  Interpolating the normalized correlation with 1/3 resolution.
 *
 *************************************************************************/

#define UP_SAMP      3
#define L_INTER4     4
#define FIR_SIZE     (UP_SAMP*L_INTER4+1)

/* 1/3 resolution interpolation filter  (-3 dB at 3600 Hz) */

static Word16 inter_3[FIR_SIZE] = {
    29519,
    24906, 13896, 2755,
    -3459, -3969, -1561,
    534, 1023, 516,
    0, -194, 0};

Word16 Interpol_3( /* (o)  : interpolated value  */
    Word16 * x,    /* (i)  : input vector        */
    Word16 frac    /* (i)  : fraction            */
)
{
    Word16 i, k;
    Word16 *x1, *x2, *c1, *c2;
    Word32 s;

    if (frac < 0) {
        frac = add(frac, UP_SAMP);
        x--;
    }
    x1 = &x[0];
    x2 = &x[1];
    c1 = &inter_3[frac];
    c2 = &inter_3[sub(UP_SAMP, frac)];

    s = 0;
    for (i = 0, k = 0; i < L_INTER4; i++, k += UP_SAMP) {
        s = L_mac(s, x1[-i], c1[k]);
        s = L_mac(s, x2[i], c2[k]);
    }

    return round(s);
}
