/*
**
** File:    exc2.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()
**      Fcbk_Pack()
**      Fcbk_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()
**
** Functions used to avoid possible explosion of the decoder
** excitation in case of series of long term unstable filters
** and when the encoder and the decoder are de-synchronized
**
**      Update_Err()
**      Test_Err()
*/

/*
    ITU-T G.723.1 Floating Point Speech Coder ANSI C Source Code.  Version 5.1F

    Original fixed-point code copyright (c) 1995,
    AudioCodes, DSP Group, France Telecom, Universite de Sherbrooke.
    All rights reserved.

    Floating-point code copyright (c) 1995,
    Intel Corporation and France Telecom (CNET).
    All rights reserved.
*/

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

#include "typedef2.h"
#include "cst2.h"
#include "tab2.h"
#include "lbccode2.h"
#include "coder2.h"
#include "util2.h"
#include "exc2.h"
#include "utilcng2.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:
**
**  FLOAT *Dpnt    Perceptually weighted speech
**  int   Start    Starting index defining the subframes under study
**
** Outputs:
**
** Return value:
**
**  int      Open loop pitch period
**
*/
int Estim_Pitch(FLOAT *Dpnt, int Start)
{
    int     i;

    int     Pr,Indx = PitchMin;
    FLOAT   MaxE = (FLOAT)1.0;
    FLOAT   MaxC = (FLOAT)0.0;
    FLOAT   E,C,C2,Diff;

    Pr = Start - PitchMin + 1;

    /* Init the energy estimate */

    E = DotProd(&Dpnt[Pr],&Dpnt[Pr],2*SubFrLen);

    /* Main Open loop pitch search loop */

    for (i=PitchMin; i <= PitchMax-3; i++)
    {
        Pr--;

        /* Update energy, compute cross */

        E = E - Dpnt[Pr+2*SubFrLen]*Dpnt[Pr+2*SubFrLen] + Dpnt[Pr]*Dpnt[Pr];
        C = DotProd(&Dpnt[Start],&Dpnt[Pr],2*SubFrLen);
        C2 = C*C;

        /* Check for new maximum */

        Diff = C2*MaxE - E*MaxC;
        if (E > (FLOAT)0.0 && C > (FLOAT)0.0)
        {
            if ((Diff > (FLOAT)0.0 && ((i - Indx) < PitchMin))
                 || (Diff > (FLOAT)0.25*C2*MaxE))
            {
                Indx = i;
                MaxE = E;
                MaxC = C2;
            }
        }
    }
    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:
**
**  FLOAT *Dpnt    Formant perceptually weighted speech
**  int   Start
**  int   Olp      Open loop pitch lag
**
** Outputs:         None
**
** Return value:
**
**  PWDEF   Word16  Indx  lag of the harmonic noise shaping filter
**          FLOAT   Gain  gain of the harmonic noise shaping filter
**
*/
PWDEF Comp_Pw(FLOAT *Dpnt, int Start, int Olp)
{

    int     i, k;
    FLOAT   Energy,C,E,C2,MaxE,MaxC2,MaxC,Gopt;
    PWDEF   Pw;

    /* Compute target energy */

    Energy = DotProd(&Dpnt[Start],&Dpnt[Start],SubFrLen);

    /* Find position with maximum C2/E value */

    MaxE = (FLOAT)1.0;
    MaxC = (FLOAT)0.0;
    MaxC2 = (FLOAT)0.0;
    Pw.Indx = -1;
    Pw.Gain = (FLOAT)0.0;
    k = Start - (Olp-PwRange);

    for (i=0; i <= 2*PwRange; i++)
    {
        C = DotProd(&Dpnt[Start],&Dpnt[k],SubFrLen);
        E = DotProd(&Dpnt[k],&Dpnt[k],SubFrLen);
        k--;

        if (E > (FLOAT)0.0 && C > (FLOAT)0.0)
        {
            C2 = C*C;
            if (C2*MaxE > E*MaxC2)
            {
                Pw.Indx = i;
                MaxE = E;
                MaxC = C;
                MaxC2 = C2;
            }
        }
    }

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

    Pw.Gain = (FLOAT)0.0;
    if (MaxC2 > MaxE*Energy*(FLOAT)0.375)
    {
        if (MaxC > MaxE || MaxE == (FLOAT)0.0)
            Gopt = (FLOAT)1.0;
        else
            Gopt = MaxC/MaxE;

        Pw.Gain = (FLOAT)0.3125*Gopt;
    }
    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: filter lag).
**
** Links to text:   Section 2.11
**
** Arguments:
**
**  FLOAT *DataBuff     Target vector
**  FLOAT *Dpnt         Formant perceptually weighted speech
**  int   Start
**  PWDEF Pw            Parameters of the harmonic noise shaping filter
**
** Outputs:
**
**  FLOAT *DataBuff     Target vector
**
** Return value:        None
**
*/
void  Filt_Pw(FLOAT *DataBuff, FLOAT *Dpnt, int Start, PWDEF Pw)
{
    int i;

    /* Perform the harmonic weighting */

    for (i=0; i < SubFrLen; i++)
        DataBuff[Start+i] = Dpnt[PitchMax+Start+i] -
                            Pw.Gain*Dpnt[PitchMax+Start-Pw.Indx+i];
    return;
}

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

    BESTDEF  Best;

    switch(WrkRate)
    {
    case Rate63:
        Srate = Nb_puls[Sfc];
        Best.MaxErr = (FLOAT)(-99999999.9);

        Find_Best(&Best, Dpnt, ImpResp, Srate, SubFrLen);

        if ((*Line).Olp[Sfc>>1] < SubFrLen-2)
            Find_Best(&Best, Dpnt, ImpResp, Srate, (*Line).Olp[Sfc>>1]);

        /* Reconstruct the excitation */

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

    Tmp0 = Olp;

    for (i=0; i < SubFrLen; i++) {
        Tmp[i] = Src[i];
        Dst[i] = Src[i];
    }

    while (Tmp0 < SubFrLen) {
        for (i=Tmp0; i < SubFrLen; i++)
            Dst[i] += Tmp[i-Tmp0];

        Tmp0 += Olp;
    }
    return;
}


/*
**
** Function:        Find_Best()
**
** Description:     Fixed codebook search for the high rate encoder.
**                  It performs the quantization of the residual signal.
**                  The excitation made of Np positive or negative pulses
**                  multiplied by a gain and whose positions on the grid are
**                  either all odd or all even, should approximate as best as
**                  possible the residual signal (perceptual criterion).
**
** Links to text:   Section 2.15
**
** Arguments:
**
**  BESTDEF *Best   Parameters of the best excitation model
**  FLOAT  *Tv      Target vector
**  FLOAT  *ImpResp Impulse response of the combined filter
**  int    Np       Number of pulses (6 for even subframes, 5 for odd)
**  int    Olp      Closed-loop pitch lag of subframe 0 (for subframes 0 & 1)
**                  Closed-loop pitch lag of subframe 2 (for subframes 2 & 3)
**
** Outputs:
**
**  BESTDEF *Best
**
** Return value:    None
**
*/
void  Find_Best(BESTDEF *Best, FLOAT *Tv, FLOAT *ImpResp,int Np,int Olp)
{

    int     i,j,k,l;
    BESTDEF  Temp;

    int     MaxAmpId;
    FLOAT   MaxAmp;
    FLOAT   Acc0,Acc1,Acc2;

    FLOAT   Imr[SubFrLen];
    FLOAT   OccPos[SubFrLen];
    FLOAT   ImrCorr[SubFrLen];
    FLOAT   ErrBlk[SubFrLen];
    FLOAT   WrkBlk[SubFrLen];


    /* Update Impulse responce */

    if (Olp < (SubFrLen-2)) {
        Temp.UseTrn = 1;
        Gen_Trn(Imr, ImpResp, Olp);
    }
    else {
        Temp.UseTrn = 0;
        for (i = 0; i < SubFrLen; i++)
            Imr[i] = ImpResp[i];
    }

    /* Copy Imr */

    for (i=0; i < SubFrLen; i++)
        OccPos[i] = Imr[i];

    /* Compute Imr AutoCorr function */

    for (i=0;i<SubFrLen;i++)
        ImrCorr[i] = DotProd(&Imr[i],Imr,SubFrLen-i);

    /* Cross correlation with the signal */

    for (i=0;i<SubFrLen;i++)
        ErrBlk[i] = DotProd(&Tv[i],Imr,SubFrLen-i);

    /* Search for the best sequence */

    for (k=0; k < Sgrid; k++)
    {
        Temp.GridId = k;

        /*Find maximum amplitude */

        Acc1 = (FLOAT)0.0;
        for (i=k; i < SubFrLen; i +=Sgrid)
        {
            Acc0 = (FLOAT) fabs(ErrBlk[i]);
            if (Acc0 >= Acc1)
            {
                Acc1 = Acc0;
                Temp.Ploc[0] = i;
            }
        }

        /* Quantize the maximum amplitude */
        Acc2 = Acc1;
        Acc1 = (FLOAT)32767.0;
        MaxAmpId = (NumOfGainLev - MlqSteps);

        for (i=MaxAmpId; i >= MlqSteps; i--)
        {
            Acc0 = (FLOAT) fabs(FcbkGainTable[i]*ImrCorr[0] - Acc2);
            if (Acc0 < Acc1)
            {
                Acc1 = Acc0;
                MaxAmpId = i;
            }