/***** 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**  Word16 *Src     Fixed codebook excitation vector without train of Dirac**  Word16 Olp      Closed-loop pitch lag of subframe 0 (for subframes 0 & 1)**                  Closed-loop pitch lag of subframe 2 (for subframes 2 & 3)**** Outputs:****  Word16 *Dst     excitation vector**** Return value:    None***/void  Gen_Trn( Word16 *Dst, Word16 *Src, Word16 Olp ){    int   i  ;    Word16   Tmp0,Tmp1   ;    Word16   Tmp[SubFrLen] ;