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

    iLBC Speech Coder ANSI-C Source Code

    createCB.c

    Copyright (C) The Internet Society (2004). 
    All Rights Reserved.

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

#include "iLBC_define.h"
#include "createCB.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));
}