/*
**
** File:    exc_lbc.c
**
** Description: Functions that implement adaptive and fixed codebook
**       operations.
**
** Functions:
**
**  Computing Open loop Pitch lag:
**
**      Estim_Pitch()
**
**  Harmonic noise weighting:
**
**      Comp_Pw()
**      Filt_Pw()
**
**  Fixed Cobebook computation:
**
**      Find_Fcbk()
**      Gen_Trn()
**      Find_Best()
**      Find_Pack()
**      Find_Unpk()
**      ACELP_LBC_code()
**      Cor_h()
**      Cor_h_X()
**      reset_max_time()
**      D4i64_LBC()
**      G_code()
**      search_T0()
**
**  Adaptive Cobebook computation:
**
**      Find_Acbk()
**      Get_Rez()
**      Decod_Acbk()
**
**  Pitch postfilter:
**      Comp_Lpf()
**      Find_B()
**      Find_F()
**      Filt_Lpf()
**
**  Residual interpolation:
**
**      Comp_Info()
**      Regen()
**
*/

/*
    ITU-T G.723 Speech Coder   ANSI-C Source Code     Version 5.00
    copyright (c) 1995, AudioCodes, DSP Group, France Telecom,
    Universite de Sherbrooke.  All rights reserved.
*/


#include <stdlib.h>
#include <stdio.h>

#include "typedef.h"
#include "basop.h"
#include "cst_lbc.h"
#include "tab_lbc.h"
#include "lbccodec.h"
#include "util_lbc.h"
#include "exc_lbc.h"
#include "tame.h"
#include "util_cng.h"

/*
**
** Function:        Estim_Pitch()
**
** Description: Open loop pitch estimation made twice per frame (one for
**              the first two subframes and one for the last two).
**              The method is based on the maximization of the
**              crosscorrelation of the speech.
**
** Links to text:   Section 2.9
**
** Arguments:
**
**  Word16 *Dpnt    Perceptually weighted speech
**  Word16 Start    Starting index defining the subframes under study
**
** Outputs:
**
** Return value:
**
**  Word16      Open loop pitch period
**
*/
Word16   Estim_Pitch( Word16 *Dpnt, Word16 Start )
{
    int   i,j   ;

    Word32   Acc0,Acc1   ;

    Word16   Exp,Tmp  ;
    Word16   Ccr,Enr  ;

    Word16   Indx = (Word16) PitchMin ;

    Word16   Mxp = (Word16) 30 ;
    Word16   Mcr = (Word16) 0x4000 ;
    Word16   Mnr = (Word16) 0x7fff ;

    Word16   Pr ;

    /* Init the energy estimate */
    Pr = Start - (Word16)PitchMin + (Word16)1 ;
    Acc1 = (Word32) 0 ;
    for ( j = 0 ; j < 2*SubFrLen ; j ++ )
        Acc1 = L_mac( Acc1, Dpnt[Pr+j], Dpnt[Pr+j] ) ;

    /* Main Olp search loop */
    for ( i = PitchMin ; i <= PitchMax-3 ; i ++ ) {

        Pr = sub( Pr, (Word16) 1 ) ;

        /* Energy update */
        Acc1 = L_msu( Acc1, Dpnt[Pr+2*SubFrLen], Dpnt[Pr+2*SubFrLen] ) ;
        Acc1 = L_mac( Acc1, Dpnt[Pr], Dpnt[Pr] ) ;

        /*  Compute the cross */
        Acc0 = (Word32) 0 ;
        for ( j = 0 ; j < 2*SubFrLen ; j ++ )
            Acc0 = L_mac( Acc0, Dpnt[Start+j], Dpnt[Pr+j] ) ;

        if ( Acc0 > (Word32) 0 ) {

            /* Compute Exp and mant of the cross */
            Exp = norm_l( Acc0 ) ;
            Acc0 = L_shl( Acc0, Exp ) ;
            Exp = shl( Exp, (Word16) 1 ) ;
            Ccr = round( Acc0 ) ;
            Acc0 = L_mult( Ccr, Ccr ) ;
            Ccr = norm_l( Acc0 ) ;
            Acc0 = L_shl( Acc0, Ccr ) ;
            Exp = add( Exp, Ccr ) ;
            Ccr = extract_h( Acc0 ) ;

            /* Do the same with energy */
            Acc0 = Acc1 ;
            Enr = norm_l( Acc0 ) ;
            Acc0 = L_shl( Acc0, Enr ) ;
            Exp = sub( Exp, Enr ) ;
            Enr = round( Acc0 ) ;

            if ( Ccr >= Enr ) {
                Exp = sub( Exp, (Word16) 1 ) ;
                Ccr = shr( Ccr, (Word16) 1 ) ;
            }

            if ( Exp <= Mxp ) {

                if ( (Exp+1) < Mxp ) {
                    Indx = (Word16) i ;
                    Mxp = Exp ;
                    Mcr = Ccr ;
                    Mnr = Enr ;
                    continue ;
                }

                if ( (Exp+1) == Mxp )
                    Tmp = shr( Mcr, (Word16) 1 ) ;
                else
                    Tmp = Mcr ;

                /* Compare with equal exponents */
                Acc0 = L_mult( Ccr, Mnr ) ;
                Acc0 = L_msu( Acc0, Enr, Tmp ) ;
                if ( Acc0 > (Word32) 0 ) {

                    if ( ((Word16)i - Indx) < (Word16) PitchMin ) {
                        Indx = (Word16) i ;
                        Mxp = Exp ;
                        Mcr = Ccr ;
                        Mnr = Enr ;
                    }

                    else {
                        Acc0 = L_mult( Ccr, Mnr ) ;
                        Acc0 = L_negate(L_shr( Acc0, (Word16) 2 ) ) ;
                        Acc0 = L_mac( Acc0, Ccr, Mnr ) ;
                        Acc0 = L_msu( Acc0, Enr, Tmp ) ;
                        if ( Acc0 > (Word32) 0 ) {
                            Indx = (Word16) i ;
                            Mxp = Exp ;
                            Mcr = Ccr ;
                            Mnr = Enr ;
                        }
                    }
                }
            }
        }
    }

    return Indx ;
}

/*
**
** Function:        Comp_Pw()
**
** Description:     Computes harmonic noise filter coefficients.
**                  For each subframe, the optimal lag is searched around the
**                  open loop pitch lag based on only positive correlation
**                  maximization.
**
** Links to text:   Section 2.11
**
** Arguments:
**
**  Word16 *Dpnt    Formant perceptually weighted speech
**  Word16 Start
**  Word16 Olp      Open loop pitch lag
**
** Outputs:         None
**
** Return value:
**
**  PWDEF   Word16  Indx  lag of the harmonic noise shaping filter
**          Word16  Gain  gain of the harmonic noise shaping filter
**
*/
PWDEF Comp_Pw( Word16 *Dpnt, Word16 Start, Word16 Olp )
{

    int   i,j   ;

    Word32   Lcr[15] ;
    Word16   Scr[15] ;
    PWDEF    Pw ;

    Word32   Acc0,Acc1   ;
    Word16   Exp   ;

    Word16   Ccr,Enr  ;
    Word16   Mcr,Mnr  ;

    /* Compute and save target energy */
    Lcr[0] = (Word32) 0 ;
    for ( i = 0 ; i < SubFrLen ; i ++ )
        Lcr[0] = L_mac( Lcr[0], Dpnt[Start+i], Dpnt[Start+i] ) ;

    /* Compute all Crosses and energys */
    for ( i = 0 ; i <= 2*PwRange ; i ++ ) {

        Acc1 = Acc0 = (Word32) 0 ;
        for ( j = 0 ; j < SubFrLen ; j ++ ) {
            Acc0 = L_mac( Acc0, Dpnt[Start+j],
                                            Dpnt[Start-(Olp-PwRange+i)+j]) ;
            Acc1 = L_mac( Acc1, Dpnt[Start-(Olp-PwRange+i)+j],
                                            Dpnt[Start-(Olp-PwRange+i)+j] ) ;
        }

        /* Save both */
        Lcr[2*i+1] = Acc1 ;
        Lcr[2*i+2] = Acc0 ;
    }

    /* Normalize to maximum */
    Acc1 = (Word32) 0 ;
    for ( i = 0 ; i < 15 ; i ++ ) {
        Acc0 = Lcr[i] ;
        Acc0 = L_abs( Acc0 ) ;
        if ( Acc0 > Acc1 )
            Acc1 = Acc0 ;
    }

    Exp = norm_l( Acc1 ) ;
    for ( i = 0 ; i < 15 ; i ++ ) {
        Acc0 = L_shl( Lcr[i], Exp ) ;
        Scr[i] = round( Acc0 ) ;

    }

    /* Find the best pair */
    Pw.Indx = (Word16) -1 ;
    Pw.Gain = (Word16) 0  ;

    Mcr = (Word16) 1 ;
    Mnr = (Word16) 0x7fff ;

    for ( i = 0 ; i <= 2*PwRange ; i ++ ) {

        Enr = Scr[2*i+1] ;
        Ccr = Scr[2*i+2] ;

        if ( Ccr <= (Word16) 0 )
            continue ;

        Exp = mult_r( Ccr, Ccr ) ;

        /* Compute the cross */
        Acc0 = L_mult( Exp, Mnr ) ;
        Acc0 = L_msu ( Acc0, Enr, Mcr ) ;

        if ( Acc0 > (Word32) 0 ) {
            Mcr = Exp ;
            Mnr = Enr ;
            Pw.Indx = (Word16)i ;
        }
    }

    if ( Pw.Indx == -1 ) {
        Pw.Indx = Olp ;
        return Pw ;
    }

    /* Check the db limit */
    Acc0 = L_mult( Scr[0], Mnr ) ;
    Acc1 = Acc0 ;
    Acc0 = L_shr( Acc0, (Word16) 2 ) ;
    Acc1 = L_shr( Acc1, (Word16) 3 ) ;
    Acc0 = L_add( Acc0, Acc1 ) ;
    Acc1 = L_mult( Scr[2*Pw.Indx+2], Scr[2*Pw.Indx+2] ) ;
    Acc0 = L_sub( Acc0, Acc1 ) ;

    if ( Acc0 < (Word32) 0 ) {

        Exp = Scr[2*Pw.Indx + 2] ;

        if ( Exp >= Mnr )
            Pw.Gain = PwConst ;
        else {
            Pw.Gain = div_s( Exp, Mnr ) ;
            Pw.Gain = mult_r( Pw.Gain, PwConst ) ;
        }
    }

    Pw.Indx = Olp - PwRange + Pw.Indx ;

    return Pw ;

}

/*
**
** Function:        Filt_Pw()
**
** Description:     Applies harmonic noise shaping filter.
**                  Lth order FIR filter on each subframe (L: lag of the filter).
**
** Links to text:   Section 2.11
**
** Arguments:
**
**  Word16 *DataBuff    Target vector
**  Word16 *Dpnt        Formant perceptually weighted speech
**  Word16 Start
**  PWDEF   Pw          Parameters of the harmonic noise shaping filter
**
** Outputs:
**
**  Word16 *DataBuff    Target vector
**
** Return value:        None
**
*/
void  Filt_Pw( Word16 *DataBuff, Word16 *Dpnt, Word16 Start, PWDEF Pw )
{
    int   i  ;

    Word32   Acc0 ;

    /* Perform the harmonic weighting */
    for ( i = 0 ; i < SubFrLen ; i ++ ) {
        Acc0 = L_deposit_h( Dpnt[PitchMax+Start+i] ) ;
        Acc0 = L_msu( Acc0, Pw.Gain, Dpnt[PitchMax+Start-Pw.Indx+i] ) ;
        DataBuff[Start+(Word16)i] = round( Acc0 ) ;
    }

    return;
}

/*
**
** Function:        Find_Fcbk()
**
** Description:     Fixed codebook excitation computation.
**
**
** Links to text:   Sections 2.15 & 2.16
**
** Arguments:
**
**  Word16 *Dpnt    Target vector
**  Word16 *ImpResp Impulse response of the synthesis filter
**  LineDef *Line   Excitation parameters for one subframe
**  Word16 Sfc      Subframe index
**
** Outputs:
**
**  Word16 *Dpnt    Excitation vector
**  LINEDEF *Line   Fixed codebook parameters for one subframe
**
** Return value:        None
**
*/
void  Find_Fcbk( Word16 *Dpnt, Word16 *ImpResp, LINEDEF *Line, Word16 Sfc )
{
    int   i  ;
    Word16 T0_acelp, gain_T0;
    Word16   Srate ;


    BESTDEF  Best ;

    switch(WrkRate)  {

        case Rate63: {

            Srate = Nb_puls[(int)Sfc] ;
            Best.MaxErr = (Word32) 0xc0000000L ;
            Find_Best( &Best, Dpnt, ImpResp, Srate, (Word16) SubFrLen ) ;
            if ( (*Line).Olp[Sfc>>1] < (Word16) (SubFrLen-2) ) {
                Find_Best( &Best, Dpnt, ImpResp, Srate, (*Line).Olp[Sfc>>1]);
            }

            /* Reconstruct the excitation */
            for ( i = 0 ; i <  SubFrLen ; i ++ )
                Dpnt[i] = (Word16) 0 ;
            for ( i = 0 ; i < Srate ; i ++ )
                Dpnt[Best.Ploc[i]] = Best.Pamp[i] ;

            /* Code the excitation */
            Fcbk_Pack( Dpnt, &((*Line).Sfs[Sfc]), &Best, Srate ) ;

            if ( Best.UseTrn == (Word16) 1 )
                Gen_Trn( Dpnt, Dpnt, (*Line).Olp[Sfc>>1] ) ;

            break;
        }

        case Rate53: {

            T0_acelp = search_T0(
                    (Word16) ((*Line).Olp[Sfc>>1]-1+(*Line).Sfs[Sfc].AcLg),
                    (*Line).Sfs[Sfc].AcGn, &gain_T0 );

            (*Line).Sfs[Sfc].Ppos = ACELP_LBC_code(
                    Dpnt, ImpResp, T0_acelp, Dpnt, &(*Line).Sfs[Sfc].Mamp,
                    &(*Line).Sfs[Sfc].Grid, &(*Line).Sfs[Sfc].Pamp, gain_T0 );

            (*Line).Sfs[Sfc].Tran = 0;

            break;
        }
    }

    return;
}

/*
**
** Function:        Gen_Trn()
**
** Description:     Generation of a train of Dirac functions with the period
**                  Olp.
**
** Links to text:   Section 2.15
**
** Arguments:
**
**  Word16 *Dst     Fixed codebook excitation vector with  train of Dirac