/******************************************************************

       iLBC Speech Coder ANSI-C Source Code

       createCB.c

       Copyright (C) The Internet Society (2004).

       All Rights Reserved.

   ******************************************************************/

   #include "iLBC_define.h"

   #include "constants.h"

   #include <string.h>

   #include <math.h>

   /*----------------------------------------------------------------*

    *  Construct an additional codebook vector by filtering the

    *  initial codebook buffer. This vector is then used to expand

    *  the codebook with an additional section.

    *---------------------------------------------------------------*/

   void filteredCBvecs(

       float *cbvectors,   /* (o) Codebook vectors for the

                                  higher section */

       float *mem,         /* (i) Buffer to create codebook

                                  vector from */

       int lMem        /* (i) Length of buffer */

   ){

       int j, k;

       float *pp, *pp1;

       float tempbuff2[CB_MEML+CB_FILTERLEN];

       float *pos;

       memset(tempbuff2, 0, (CB_HALFFILTERLEN-1)*sizeof(float));

       memcpy(&tempbuff2[CB_HALFFILTERLEN-1], mem, lMem*sizeof(float));

       memset(&tempbuff2[lMem+CB_HALFFILTERLEN-1], 0,

           (CB_HALFFILTERLEN+1)*sizeof(float));

       /* Create codebook vector for higher section by filtering */

       /* do filtering */

       pos=cbvectors;

       memset(pos, 0, lMem*sizeof(float));

       for (k=0; k<lMem; k++) {

           pp=&tempbuff2[k];

           pp1=&cbfiltersTbl[CB_FILTERLEN-1];

           for (j=0;j<CB_FILTERLEN;j++) {

               (*pos)+=(*pp++)*(*pp1--);

           }

           pos++;

       }

   }

   /*----------------------------------------------------------------*

    *  Search the augmented part of the codebook to find the best

    *  measure.

    *----------------------------------------------------------------*/

   void searchAugmentedCB(

       int low,        /* (i) Start index for the search */

       int high,           /* (i) End index for the search */

       int stage,          /* (i) Current stage */

       int startIndex,     /* (i) Codebook index for the first

                                  aug vector */

       float *target,      /* (i) Target vector for encoding */

       float *buffer,      /* (i) Pointer to the end of the buffer for

                                  augmented codebook construction */

       float *max_measure, /* (i/o) Currently maximum measure */

       int *best_index,/* (o) Currently the best index */

       float *gain,    /* (o) Currently the best gain */

       float *energy,      /* (o) Energy of augmented codebook

                                  vectors */

       float *invenergy/* (o) Inv energy of augmented codebook

                                  vectors */

   ) {

       int icount, ilow, j, tmpIndex;

       float *pp, *ppo, *ppi, *ppe, crossDot, alfa;

       float weighted, measure, nrjRecursive;

       float ftmp;

       /* Compute the energy for the first (low-5)

          noninterpolated samples */

       nrjRecursive = (float) 0.0;

       pp = buffer - low + 1;

       for (j=0; j<(low-5); j++) {

           nrjRecursive += ( (*pp)*(*pp) );

           pp++;

       }

       ppe = buffer - low;

       for (icount=low; icount<=high; icount++) {

           /* Index of the codebook vector used for retrieving

              energy values */

           tmpIndex = startIndex+icount-20;

           ilow = icount-4;

           /* Update the energy recursively to save complexity */

           nrjRecursive = nrjRecursive + (*ppe)*(*ppe);

           ppe--;

           energy[tmpIndex] = nrjRecursive;

           /* Compute cross dot product for the first (low-5)

              samples */

           crossDot = (float) 0.0;

           pp = buffer-icount;

           for (j=0; j<ilow; j++) {

               crossDot += target[j]*(*pp++);

           }

           /* interpolation */

           alfa = (float) 0.2;

           ppo = buffer-4;

           ppi = buffer-icount-4;

           for (j=ilow; j<icount; j++) {

               weighted = ((float)1.0-alfa)*(*ppo)+alfa*(*ppi);

               ppo++;

               ppi++;

               energy[tmpIndex] += weighted*weighted;

               crossDot += target[j]*weighted;

               alfa += (float)0.2;

           }

           /* Compute energy and cross dot product for the

              remaining samples */

           pp = buffer - icount;

           for (j=icount; j<SUBL; j++) {

               energy[tmpIndex] += (*pp)*(*pp);

               crossDot += target[j]*(*pp++);

           }

           if (energy[tmpIndex]>0.0) {

               invenergy[tmpIndex]=(float)1.0/(energy[tmpIndex]+EPS);

           } else {

               invenergy[tmpIndex] = (float) 0.0;

           }

           if (stage==0) {

               measure = (float)-10000000.0;

               if (crossDot > 0.0) {

                   measure = crossDot*crossDot*invenergy[tmpIndex];

               }

           }

           else {

               measure = crossDot*crossDot*invenergy[tmpIndex];

           }

           /* check if measure is better */

           ftmp = crossDot*invenergy[tmpIndex];

           if ((measure>*max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {

               *best_index = tmpIndex;

               *max_measure = measure;

               *gain = ftmp;

           }

       }

   }

   /*----------------------------------------------------------------*

    *  Recreate a specific codebook vector from the augmented part.

    *

    *----------------------------------------------------------------*/

   void createAugmentedVec(

       int index,      /* (i) Index for the augmented vector

                              to be created */

       float *buffer,  /* (i) Pointer to the end of the buffer for

                              augmented codebook construction */

       float *cbVec/* (o) The construced codebook vector */

   ) {

       int ilow, j;

       float *pp, *ppo, *ppi, alfa, alfa1, weighted;

       ilow = index-5;

       /* copy the first noninterpolated part */

       pp = buffer-index;

       memcpy(cbVec,pp,sizeof(float)*index);

       /* interpolation */

       alfa1 = (float)0.2;

       alfa = 0.0;

       ppo = buffer-5;

       ppi = buffer-index-5;

       for (j=ilow; j<index; j++) {

           weighted = ((float)1.0-alfa)*(*ppo)+alfa*(*ppi);

           ppo++;

           ppi++;

           cbVec[j] = weighted;

           alfa += alfa1;

       }

       /* copy the second noninterpolated part */

       pp = buffer - index;

       memcpy(cbVec+index,pp,sizeof(float)*(SUBL-index));

   }