    
   /****************************************************************** 
    
       iLBC Speech Coder ANSI-C Source Code 
    
       helpfun.c  
    
       Copyright (c) 2001, 
       Global IP Sound AB. 
       All rights reserved. 
    
   ******************************************************************/ 
    
   #include <math.h> 
    
   #include "iLBC_define.h" 
   #include "constants.h" 
    
   /*----------------------------------------------------------------* 
    *  calculation of auto correlation  
    *---------------------------------------------------------------*/ 
    
   void iLBC_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;   
   } 
    
    
     
    
    
