   
   /******************************************************************
   
       iLBC Speech Coder ANSI-C Source Code
   
       helpfun.c 
   
       Copyright (C) The Internet Society (2004). 
       All Rights Reserved.
   
   ******************************************************************/
   
   #include <math.h>
   
   #include "iLBC_define.h"
   #include "constants.h"
   
   /*----------------------------------------------------------------*
    *  calculation of auto correlation 
    *---------------------------------------------------------------*/
   
   void autocorr( 
       float *r,       /* (o) autocorrelation vector */
       const float *x, /* (i) data vector */
       int N,          /* (i) length of data vector */
       int order       /* largest lag for calculated 
                          autocorrelations */
   ){
       int     lag, n;
       float   sum;
       
       for (lag = 0; lag <= order; lag++) {
           sum = 0;
           for (n = 0; n < N - lag; n++) {
               sum += x[n] * x[n+lag];
           }
           r[lag] = sum;
       }
   }
   
   
   
   /*----------------------------------------------------------------*
    *  window multiplication  
    *---------------------------------------------------------------*/
   
   void window( 
       float *z,       /* (o) the windowed data */
       const float *x, /* (i) the original data vector */
       const float *y, /* (i) the window */
       int N           /* (i) length of all vectors */
   ){
       int     i;
       
       for (i = 0; i < N; i++) {
           z[i] = x[i] * y[i];
       }
   }
   
   /*----------------------------------------------------------------*
    *  levinson-durbin solution for lpc coefficients
    *---------------------------------------------------------------*/
   
   void levdurb( 
       float *a,       /* (o) lpc coefficient vector starting 
                              with 1.0 */
       float *k,       /* (o) reflection coefficients */
       float *r,       /* (i) autocorrelation vector */
       int order       /* (i) order of lpc filter */
   ){
       float  sum, alpha;
       int     m, m_h, i;
   
       a[0] = 1.0;
       
       if (r[0] < EPS) { /* if r[0] <= 0, set LPC coeff. to zero */
           for (i = 0; i < order; i++) {
               k[i] = 0;
               a[i+1] = 0;
           }   
       } else {
           a[1] = k[0] = -r[1]/r[0];
           alpha = r[0] + r[1] * k[0];
           for (m = 1; m < order; m++){
               sum = r[m + 1];
               for (i = 0; i < m; i++){
                   sum += a[i+1] * r[m - i];
               }
               k[m] = -sum / alpha;
               alpha += k[m] * sum;
               m_h = (m + 1) >> 1;
               for (i = 0; i < m_h; i++){
                   sum = a[i+1] + k[m] * a[m - i];
                   a[m - i] += k[m] * a[i+1];
                   a[i+1] = sum;
   
   
               }
               a[m+1] = k[m];
           }
       }
   }
   
   /*----------------------------------------------------------------*
    *  interpolation between vectors 
    *---------------------------------------------------------------*/
   
   void interpolate( 
       float *out,      /* (o) the interpolated vector */
       float *in1,     /* (i) the first vector for the 
                              interpolation */
       float *in2,     /* (i) the second vector for the 
                              interpolation */
       float coef,      /* (i) interpolation weights */
       int length      /* (i) length of all vectors */
   ){
       int i;
       float invcoef;
   
       invcoef = (float)1.0 - coef;
       for (i = 0; i < length; i++) {
           out[i] = coef * in1[i] + invcoef * in2[i];
       }
   }
   
   /*----------------------------------------------------------------*
    *  lpc bandwidth expansion                   
    *---------------------------------------------------------------*/
   
   void bwexpand( 
       float *out,      /* (o) the bandwidth expanded lpc 
                              coefficients */
       float *in,      /* (i) the lpc coefficients before bandwidth 
                              expansion */
       float coef,     /* (i) the bandwidth expansion factor */
       int length      /* (i) the length of lpc coefficient vectors */
   ){
       int i;
       float  chirp;
       
       chirp = coef;
       
       out[0] = in[0];
       for (i = 1; i < length; i++) {
           out[i] = chirp * in[i];
           chirp *= coef;
       }
   }
   
   /*----------------------------------------------------------------*
    *  vector quantization 
   
   
    *---------------------------------------------------------------*/
   
   void vq( 
       float *Xq,      /* (o) the quantized vector */
       int *index,     /* (o) the quantization index */
       const float *CB,/* (i) the vector quantization codebook */
       float *X,       /* (i) the vector to quantize */
       int n_cb,       /* (i) the number of vectors in the codebook */
       int dim         /* (i) the dimension of all vectors */
   ){
       int     i, j;
       int     pos, minindex;
       float   dist, tmp, mindist;
   
       pos = 0;
       mindist = FLOAT_MAX;
       minindex = 0;
       for (j = 0; j < n_cb; j++) {
           dist = X[0] - CB[pos];
           dist *= dist;
           for (i = 1; i < dim; i++) {
               tmp = X[i] - CB[pos + i];
               dist += tmp*tmp;
           }
           
           if (dist < mindist) {
               mindist = dist;
               minindex = j;
           }
           pos += dim;
       }
       for (i = 0; i < dim; i++) {
           Xq[i] = CB[minindex*dim + i];
       }
       *index = minindex;
   }
   
   /*----------------------------------------------------------------*
    *  split vector quantization 
    *---------------------------------------------------------------*/
   
   void SplitVQ( 
       float *qX,      /* (o) the quantized vector */
       int *index,     /* (o) a vector of indexes for all vector
                              codebooks in the split */
       float *X,       /* (i) the vector to quantize */
       const float *CB,/* (i) the quantizer codebook */
       int nsplit,     /* the number of vector splits */
       const int *dim, /* the dimension of X and qX */
       const int *cbsize /* the number of vectors in the codebook */
   ){
       int    cb_pos, X_pos, i;
       
       cb_pos = 0;
   
   
       X_pos= 0;
       for (i = 0; i < nsplit; i++) {
           vq(qX + X_pos, index + i, CB + cb_pos, X + X_pos, 
               cbsize[i], dim[i]);
           X_pos += dim[i];
           cb_pos += dim[i] * cbsize[i];
       }
   }
   
   /*----------------------------------------------------------------*
    *  scalar quantization 
    *---------------------------------------------------------------*/
   
   void sort_sq( 
       float *xq,      /* (o) the quantized value */
       int *index,     /* (o) the quantization index */
       float x,    /* (i) the value to quantize */
       const float *cb,/* (i) the quantization codebook */
       int cb_size      /* (i) the size of the quantization codebook */
   ){
       int i;
       
       if (x <= cb[0]) {
           *index = 0;
           *xq = cb[0];
       } else {
           i = 0;
           while ((x > cb[i]) && i < cb_size - 1) {
               i++;
           }
           
           if (x > ((cb[i] + cb[i - 1])/2)) {
               *index = i;
               *xq = cb[i];
           } else {
               *index = i - 1;
               *xq = cb[i - 1];
           }
       }
   }
   
   /*----------------------------------------------------------------*
    *  check for stability of lsf coefficients
    *---------------------------------------------------------------*/
   
   int LSF_check(    /* (o) 1 for stable lsf vectors and 0 for
                              nonstable ones */
       float *lsf,     /* (i) a table of lsf vectors */
       int dim,    /* (i) the dimension of each lsf vector */
       int NoAn    /* (i) the number of lsf vectors in the 
                              table */
   ){
       int k,n,m, Nit=2, change=0,pos;
       float tmp;
   
   
       static float eps=(float)0.039; /* 50 Hz */
       static float eps2=(float)0.0195;
       static float maxlsf=(float)3.14; /* 4000 Hz */
       static float minlsf=(float)0.01; /* 0 Hz */
       
       /* LSF separation check*/
   
       for (n=0; n<Nit; n++) { /* Run through a couple of times */
           for (m=0; m<NoAn; m++) { /* Number of analyses per frame */
               for (k=0; k<(dim-1); k++) {
                   pos=m*dim+k;
                   
                   if ((lsf[pos+1]-lsf[pos])<eps) {
                       
                       if (lsf[pos+1]<lsf[pos]) {
                           tmp=lsf[pos+1];
                           lsf[pos+1]= lsf[pos]+eps2;
                           lsf[pos]= lsf[pos+1]-eps2;
                       } else {
                           lsf[pos]-=eps2;
                           lsf[pos+1]+=eps2;
                       }
                       change=1;
                   }
                   
                   if (lsf[pos]<minlsf) { 
                       lsf[pos]=minlsf;
                       change=1;
                   }
                   
                   if (lsf[pos]>maxlsf) { 
                       lsf[pos]=maxlsf;
                       change=1;
                   }               
               }
           }
       }
       
       return change;  
   }
   
   
