//#include "stdafx.h"
#include <math.h>
#include <memory.h>
#include <float.h>

#include "LanAudio.h"
#include "Global.h"

/*
**
** 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()
*/

/*
**
** 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);
  /*  E = DotProd10(&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);
        C = DotProd10(&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;
}*/
int Estim_Pitch(float *Dpnt, int Start)
{
    int     i;

    int     Pr,Indx = PitchMin,Indx2;
    float   MaxE = (float)1.0;
    float   MaxC = (float)0.0;
    float   E,C,C2,Diff;
	float	EARR[144];
	;//Pr = Start - PitchMin + 1;
    Pr = Start - PitchMin + 1;

    /* Init the energy estimate */

    E = DotProd10(&Dpnt[Pr],&Dpnt[Pr],2*SubFrLen);
	for(i=PitchMin;i<=PitchMax-3;i++)
	{
		E=E - Dpnt[Start-i+2*SubFrLen]*Dpnt[Start-i+2*SubFrLen] + Dpnt[Start-i]*Dpnt[Start-i];
		EARR[i]=E;
	}
    /* Main Open loop pitch search loop */
    for (i=PitchMin+1; i <= PitchMax-3; i+=2)
    {
        Pr-=2;

        /* Update energy, compute cross */

		E=EARR[i];
        C = DotProd10(&Dpnt[Start],&Dpnt[Start-i],2*SubFrLen);     ;//not allign
        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;
            }
        }
    }
    Indx2=Indx;
	C = DotProd10(&Dpnt[Start],&Dpnt[Start-(Indx-1)],2*SubFrLen);     
	E=EARR[Indx-1];
	C2=C*C;
	Diff = C2*MaxE - E*MaxC;
    if (E > (float)0.0 && C > (float)0.0)
    {
        if (Diff > (float)0.0)
               
        {
            Indx2 = Indx-1;
            MaxE = E;
            MaxC = C2;
        }
     }
	C = DotProd10(&Dpnt[Start],&Dpnt[Start-(Indx+1)],2*SubFrLen);     
	E=EARR[Indx+1];
	C2=C*C;
	Diff = C2*MaxE - E*MaxC;
    if (E > (float)0.0 && C > (float)0.0)
    {
        if (Diff > (float)0.0)
            
        {
            Indx2 = Indx+1;
            MaxE = E;
            MaxC = C2;
        }
     }
	return Indx2;
}

/*
**
** 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   short  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);
    Energy = DotProd10(&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);
        C = DotProd10(&Dpnt[Start],&Dpnt[k],SubFrLen);
        E = DotProd10(&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:        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 */