/*
********************************************************************************
*
*      GSM AMR-NB speech codec   R98   Version 7.6.0   December 12, 2001
*                                R99   Version 3.3.0                
*                                REL-4 Version 4.1.0                
*
********************************************************************************
*
*      File             : pitch_fr.c
*      Purpose          : Find the pitch period with 1/3 or 1/6 subsample
*                       : resolution (closed loop).
*
********************************************************************************
*/
/*
********************************************************************************
*                         MODULE INCLUDE FILE AND VERSION ID
********************************************************************************
*/
#include "namespace.h"
#include "pitch_fr.h"
/*
********************************************************************************
*                         INCLUDE FILES
********************************************************************************
*/
#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "no_count.h"
#include "cnst.h"
#include "enc_lag3.h"
#include "enc_lag6.h"
#include "inter_36.h"
#include "inv_sqrt.h"
#include "convolve.h"

/*
********************************************************************************
*                         LOCAL VARIABLES AND TABLES
********************************************************************************
*/

/*
 * mode dependent parameters used in Pitch_fr()
 * Note: order of MRxx in 'enum Mode' is important!
 */
static const struct {
    Word16 max_frac_lag;     /* lag up to which fractional lags are used    */
    Word16 flag3;            /* enable 1/3 instead of 1/6 fract. resolution */
    Word16 first_frac;       /* first fractional to check                   */
    Word16 last_frac;        /* last fractional to check                    */
    Word16 delta_int_low;    /* integer lag below TO to start search from   */
    Word16 delta_int_range;  /* integer range around T0                     */
    Word16 delta_frc_low;    /* fractional below T0                         */
    Word16 delta_frc_range;  /* fractional range around T0                  */
    Word16 pit_min;          /* minimum pitch                               */
} mode_dep_parm[8] = {
    /* MR475 */  { 84,  1, -2,  2,  5, 10,  5,  9, PIT_MIN },
    /* MR515 */  { 84,  1, -2,  2,  5, 10,  5,  9, PIT_MIN },                 
    /* MR59  */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },
    /* MR67  */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },
    /* MR74  */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },
    /* MR795 */  { 84,  1, -2,  2,  3,  6, 10, 19, PIT_MIN },
    /* MR102 */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },                 
    /* MR122 */  { 94,  0, -3,  3,  3,  6,  5,  9, PIT_MIN_MR122 }
};

/*
********************************************************************************
*                         LOCAL PROGRAM CODE
********************************************************************************
*/
/*************************************************************************
 *
 *  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.
 *
 *************************************************************************/
static 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;

    /* Usally dynamic allocation of (L_subfr) */
    Word16 excf[L_SUBFR];
    Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];

    k = -t_min;                                move16 ();

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

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

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

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

    /* Compute 1/sqrt(energy of excf[]) */

    s = 0;                                     move32 ();
    for (j = 0; j < L_subfr; j++) {
        s = L_mac (s, excf[j], excf[j]);
    }
    test ();
    if (L_sub (s, 67108864L) <= 0) {            /* if (s <= 2^26) */
        s_excf = excf;                         move16 ();
        h_fac = 15 - 12;                       move16 ();
        scaling = 0;                           move16 ();
    }
    else {
        /* "excf[]" is divided by 2 */
        s_excf = scaled_excf;                  move16 ();
        h_fac = 15 - 12 - 2;                   move16 ();
        scaling = 2;                           move16 ();
    }

    /* loop for every possible period */

    for (i = t_min; i <= t_max; i++) {
        /* Compute 1/sqrt(energy of excf[]) */
        
        s = 0;                                 move32 ();
        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;                                  move32 ();
        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));
        move16 ();

            /* modify the filtered excitation excf[] for the next iteration */
        
        test ();
        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]); move16 ();
            }
            s_excf[0] = shr (exc[k], scaling);  move16 ();
        }
    }
    return;
}

/*************************************************************************
 *
 *  FUNCTION:   searchFrac()
 *
 *  PURPOSE: Find fractional pitch
 *
 *  DESCRIPTION:
 *     The function interpolates the normalized correlation at the
 *     fractional positions around lag T0. The position at which the
 *     interpolation function reaches its maximum is the fractional pitch.
 *     Starting point of the search is frac, end point is last_frac.
 *     frac is overwritten with the fractional pitch.
 *
 *************************************************************************/
static void searchFrac (
    Word16 *lag,       /* i/o : integer pitch           */
    Word16 *frac,      /* i/o : start point of search -
                                fractional pitch        */
    Word16 last_frac,  /* i   : endpoint of search      */
    Word16 corr[],     /* i   : normalized correlation  */
    Word16 flag3       /* i   : subsample resolution
                                (3: =1 / 6: =0)         */
)
{
    Word16 i;
    Word16 max;
    Word16 corr_int;

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

    max = Interpol_3or6 (&corr[*lag], *frac, flag3); move16 (); /* function result */

    for (i = add (*frac, 1); i <= last_frac; i++) {
        corr_int = Interpol_3or6 (&corr[*lag], i, flag3);
        move16 ();
        test ();
        if (sub (corr_int, max) > 0) {
            max = corr_int;                       move16 ();
            *frac = i;                            move16 ();
        }
    }

    test();
    if (flag3 == 0) {
        /* Limit the fraction value in the interval [-2,-1,0,1,2,3] */

        test ();
        if (sub (*frac, -3) == 0) {
            *frac = 3;                            move16 ();
            *lag = sub (*lag, 1);
        }
    }
    else {
        /* limit the fraction value between -1 and 1 */

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

/*************************************************************************
 *
 *  FUNCTION:   getRange()
 *
 *  PURPOSE: Sets range around open-loop pitch or integer pitch of last subframe
 *
 *  DESCRIPTION:
 *     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
 *
 *************************************************************************/
static void getRange (
    Word16 T0,           /* i : integer pitch          */
    Word16 delta_low,    /* i : search start offset    */
    Word16 delta_range,  /* i : search range           */
    Word16 pitmin,       /* i : minimum pitch          */
    Word16 pitmax,       /* i : maximum pitch          */
    Word16 *t0_min,      /* o : search range minimum   */
    Word16 *t0_max)      /* o : search range maximum   */
{
    *t0_min = sub(T0, delta_low);
    test ();
    if (sub(*t0_min, pitmin) < 0) {
        *t0_min = pitmin;                                  move16();
    }
    *t0_max = add(*t0_min, delta_range);
    test ();
    if (sub(*t0_max, pitmax) > 0) {
        *t0_max = pitmax;                                  move16();
        *t0_min = sub(*t0_max, delta_range);
    }
}

/*
********************************************************************************
*                         PUBLIC PROGRAM CODE
********************************************************************************
*/

/*************************************************************************
*
*  Function:   Pitch_fr_reset
*  Purpose:    Initializes state memory to zero
*
**************************************************************************
*/
void Pitch_fr_reset (Pitch_frState *state)
{
    state->T0_prev_subframe = 0;
}

/*************************************************************************
 *
 *  FUNCTION:   Pitch_fr()
 *
 *  PURPOSE: Find the pitch period with 1/3 or 1/6 subsample resolution
 *           (closed loop).
 *
 *  DESCRIPTION:
 *        - find the normalized correlation between the target and filtered
 *          past excitation in the search range.
 *        - select the delay with maximum normalized correlation.
 *        - interpolate the normalized correlation at fractions -3/6 to 3/6
 *          with step 1/6 around the chosen delay.
 *        - The fraction which gives the maximum interpolated value is chosen.
 *
 *************************************************************************/
Word16 Pitch_fr (        /* o   : pitch period (integer)                    */
    Pitch_frState *st,   /* i/o : State struct                              */
    enum Mode mode,      /* i   : codec mode                                */
    Word16 T_op[],       /* i   : open loop pitch lags                      */
    Word16 exc[],        /* i   : excitation buffer                      Q0 */
    Word16 xn[],         /* i   : target vector                          Q0 */
    Word16 h[],          /* i   : impulse response of synthesis and
                                  weighting filters                     Q12 */
    Word16 L_subfr,      /* i   : Length of subframe                        */
    Word16 i_subfr,      /* i   : subframe offset                           */
    Word16 *pit_frac,    /* o   : pitch period (fractional)                 */
    Word16 *resu3,       /* o   : subsample resolution 1/3 (=1) or 1/6 (=0) */
    Word16 *ana_index    /* o   : index of encoding                         */
)
{
    Word16 i;
    Word16 t_min, t_max;
    Word16 t0_min, t0_max;
    Word16 max, lag, frac;
    Word16 tmp_lag;
    Word16 *corr;
    Word16 corr_v[40];    /* Total length = t0_max-t0_min+1+2*L_INTER_SRCH */

    Word16 max_frac_lag;
    Word16 flag3, flag4;
    Word16 last_frac;
    Word16 delta_int_low, delta_int_range;
    Word16 delta_frc_low, delta_frc_range;
    Word16 pit_min;
    Word16 frame_offset;
    Word16 delta_search;

    /*-----------------------------------------------------------------------*
     *                      set mode specific variables                      *
     *-----------------------------------------------------------------------*/

    max_frac_lag    = mode_dep_parm[mode].max_frac_lag;           move16 ();
    flag3           = mode_dep_parm[mode].flag3;                  move16 ();
    frac            = mode_dep_parm[mode].first_frac;             move16 ();
    last_frac       = mode_dep_parm[mode].last_frac;              move16 ();
    delta_int_low   = mode_dep_parm[mode].delta_int_low;          move16 ();
    delta_int_range = mode_dep_parm[mode].delta_int_range;        move16 ();
    
    delta_frc_low   = mode_dep_parm[mode].delta_frc_low;          move16 ();
    delta_frc_range = mode_dep_parm[mode].delta_frc_range;        move16 ();
    pit_min         = mode_dep_parm[mode].pit_min;                move16 ();
    
    /*-----------------------------------------------------------------------*
     *                 decide upon full or differential search               *
     *-----------------------------------------------------------------------*/
    
    delta_search = 1;                                             move16 ();
    
    test (); test ();
    if ((i_subfr == 0) || (sub(i_subfr,L_FRAME_BY2) == 0)) {
      
        /* Subframe 1 and 3 */
      
        test (); test (); test ();
        if (((sub(mode, MR475) != 0) && (sub(mode, MR515) != 0)) ||
            (sub(i_subfr,L_FRAME_BY2) != 0)) {
        
            /* set t0_min, t0_max for full search */
            /* this is *not* done for mode MR475, MR515 in subframe 3 */
        
            delta_search = 0; /* no differential search */         move16 ();
            
            /* calculate index into T_op which contains the open-loop */
            /* pitch estimations for the 2 big subframes */
            
            frame_offset = 1;                                      move16 ();
            test ();
            if (i_subfr == 0)
                frame_offset = 0;                                  move16 ();
            
            /* get T_op from the corresponding half frame and */
            /* set t0_min, t0_max */
            
            getRange (T_op[frame_offset], delta_int_low, delta_int_range,
                      pit_min, PIT_MAX, &t0_min, &t0_max);
        }
        else {
            
            /* mode MR475, MR515 and 3. Subframe: delta search as well */
            getRange (st->T0_prev_subframe, delta_frc_low, delta_frc_range,
                      pit_min, PIT_MAX, &t0_min, &t0_max);
        }
    }
    else {
        
        /* for Subframe 2 and 4 */
        /* get range around T0 of previous subframe for delta search */
        
        getRange (st->T0_prev_subframe, delta_frc_low, delta_frc_range,
                  pit_min, PIT_MAX, &t0_min, &t0_max);
    }

    /*-----------------------------------------------------------------------*
     *           Find interval to compute normalized correlation             *
     *-----------------------------------------------------------------------*/

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

    corr = &corr_v[-t_min];                                        move16 ();

    /*-----------------------------------------------------------------------*
     * 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];                                            move16 ();
    lag = t0_min;                                                  move16 ();

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

    /*-----------------------------------------------------------------------*
     *                        Find fractional pitch                          *
     *-----------------------------------------------------------------------*/
    test (); test ();
    if ((delta_search == 0) && (sub (lag, max_frac_lag) > 0)) {

        /* full search and integer pitch greater than max_frac_lag */
        /* fractional search is not needed, set fractional to zero */

        frac = 0;                                                  move16 ();
    }
    else {

        /* if differential search AND mode MR475 OR MR515 OR MR59 OR MR67   */
        /* then search fractional with 4 bits resolution           */
       
       test (); test (); test (); test (); test (); 
       if ((delta_search != 0) &&
           ((sub (mode, MR475) == 0) ||
            (sub (mode, MR515) == 0) ||
            (sub (mode, MR59) == 0) ||
            (sub (mode, MR67) == 0))) {

          /* modify frac or last_frac according to position of last */
          /* integer pitch: either search around integer pitch, */
          /* or only on left or right side */
          
          tmp_lag = st->T0_prev_subframe;                          move16 ();
          test ();
          if ( sub( sub(tmp_lag, t0_min), 5) > 0)
             tmp_lag = add (t0_min, 5);
          test ();
          if ( sub( sub(t0_max, tmp_lag), 4) > 0)
               tmp_lag = sub (t0_max, 4);
          
          test (); test ();
          if ((sub (lag, tmp_lag) == 0) ||
              (sub (lag, sub(tmp_lag, 1)) == 0)) {
             
             /* normal search in fractions around T0 */
             
             searchFrac (&lag, &frac, last_frac, corr, flag3);
             
          }
          else if (sub (lag, sub (tmp_lag, 2)) == 0) {
             test ();
             /* limit search around T0 to the right side */
             frac = 0;                                            move16 ();
             searchFrac (&lag, &frac, last_frac, corr, flag3);
          }
          else if (sub (lag, add(tmp_lag, 1)) == 0) {
             test (); test ();
             /* limit search around T0 to the left side */
             last_frac = 0;                                       move16 ();
             searchFrac (&lag, &frac, last_frac, corr, flag3);
          }
          else {
             test (); test ();
             /* no fractional search */
             frac = 0;                                            move16 ();
            }
       }
       else
          /* test the fractions around T0 */
          searchFrac (&lag, &frac, last_frac, corr, flag3);
    }
    
    /*-----------------------------------------------------------------------*
     *                           encode pitch                                *
     *-----------------------------------------------------------------------*/
    
    test ();
    if (flag3 != 0) {       
       /* flag4 indicates encoding with 4 bit resolution;         */
       /* this is needed for mode MR475, MR515 and MR59           */
       
       flag4 = 0;                                                 move16 ();
       test (); test (); test (); test (); 
       if ( (sub (mode, MR475) == 0) ||
            (sub (mode, MR515) == 0) ||
            (sub (mode, MR59) == 0) ||
            (sub (mode, MR67) == 0) ) {
          flag4 = 1;                                              move16 ();
       }
       
       /* encode with 1/3 subsample resolution */
       
       *ana_index = Enc_lag3(lag, frac, st->T0_prev_subframe,
                             t0_min, t0_max, delta_search, flag4); move16 (); /* function result */

    }  
    else
    {
       /* encode with 1/6 subsample resolution */
       
       *ana_index = Enc_lag6(lag, frac, t0_min, delta_search); move16 (); /* function result */
    }
    
    /*-----------------------------------------------------------------------*
     *                          update state variables                       *
     *-----------------------------------------------------------------------*/
    
    st->T0_prev_subframe = lag;                                    move16 ();
    
    /*-----------------------------------------------------------------------*
     *                      update output variables                          *
     *-----------------------------------------------------------------------*/
    
    *resu3    = flag3;                                             move16 ();

    *pit_frac = frac;                                              move16 ();

    return (lag);
}