icbsearch.cxx

MiniSip Client with DomainKeys Authentication, Sip, Audio communications, Echo Cancel

//A.34 iCBSearch.c 
    
   /****************************************************************** 
    
       iLBC Speech Coder ANSI-C Source Code 
    
       iCBSearch.c  
    
       Copyright (c) 2001, 
       Global IP Sound AB. 
       All rights reserved. 
    
   ******************************************************************/ 
     
    
   #include <math.h> 
   #include <string.h> 
    
   #include"iLBC_define.h" 
   #include"gainquant.h" 
   #include"createCB.h" 
   #include"filter.h" 
   #include"constants.h" 
    
   /*----------------------------------------------------------------* 
    *  Search routine for codebook encoding and gain quantization. 
    *---------------------------------------------------------------*/ 
    
   void iCBSearch( 
       int *index,         /* (o) Codebook indices */ 
       int *gain_index,/* (o) Gain quantization indices */ 
       float *intarget,/* (i) Target vector for encoding */     
       float *mem,         /* (i) Buffer for codebook construction */ 
       int lMem,           /* (i) Length of buffer */ 
       int lTarget,    /* (i) Length of vector */ 
       int nStages,    /* (i) Number of codebook stages */ 
       float *weightDenum, /* (i) weighting filter coefficients */ 
       float *weightState, /* (i) weighting filter state */ 
       int block           /* (i) the subblock number */ 
   ){ 
       int i, j, icount, stage, best_index, range, counter; 
       float max_measure, gain, measure, crossDot, ftmp; 
       float gains[CB_NSTAGES]; 
       float target[SUBL]; 
       int base_index, sInd, eInd, base_size; 
       int sIndAug=0, eIndAug=0; 
       float buf[CB_MEML+SUBL+2*LPC_FILTERORDER]; 
       float invenergy[CB_EXPAND*128], energy[CB_EXPAND*128]; 
       float *pp, *ppi=0, *ppo=0, *ppe=0; 
       float cbvectors[CB_MEML]; 
       float tene, cene, cvec[SUBL]; 
       float aug_vec[SUBL]; 
    
       memset(cvec,0,SUBL*sizeof(float));   
    
       /* Determine size of codebook sections */ 
    
       base_size=lMem-lTarget+1; 
        
       if (lTarget==SUBL) { 
           base_size=lMem-lTarget+1+lTarget/2; 
       } 
    
       /* setup buffer for weighting */ 
    
       memcpy(buf,weightState,sizeof(float)*LPC_FILTERORDER); 
       memcpy(buf+LPC_FILTERORDER,mem,lMem*sizeof(float)); 
     
       memcpy(buf+LPC_FILTERORDER+lMem,intarget,lTarget*sizeof(float)); 
    
       /* weighting */ 
    
       AllPoleFilter(buf+LPC_FILTERORDER, weightDenum,  
           lMem+lTarget, LPC_FILTERORDER); 
        
       /* Construct the codebook and target needed */ 
    
       memcpy(target, buf+LPC_FILTERORDER+lMem, lTarget*sizeof(float)); 
    
       tene=0.0; 
       for (i=0;i<lTarget;i++) { 
           tene+=target[i]*target[i]; 
       } 
    
       /* Prepare search over one more codebook section. This section  
          is created by filtering the original buffer with a filter. */ 
            
       filteredCBvecs(cbvectors, buf+LPC_FILTERORDER, lMem); 
    
       /* The Main Loop over stages */ 
    
       for (stage=0;stage<nStages; stage++) { 
    
           range = search_rangeTbl[block][stage]; 
    
           /* initialize search measure */ 
    
           max_measure = (float)-10000000.0; 
           gain = (float)0.0; 
           best_index = 0; 
    
           /* Compute cross dot product between the target  
              and the CB memory */ 
    
           crossDot=0.0; 
           pp=buf+LPC_FILTERORDER+lMem-lTarget; 
           for (j=0; j<lTarget; j++) { 
               crossDot += target[j]*(*pp++); 
           }        
            
           if (stage==0) { 
    
               /* Calculate energy in the first block of  
                 'lTarget' sampels. */ 
               ppe = energy; 
               ppi = buf+LPC_FILTERORDER+lMem-lTarget-1; 
               ppo = buf+LPC_FILTERORDER+lMem-1; 
    
               *ppe=0.0; 
               pp=buf+LPC_FILTERORDER+lMem-lTarget; 
               for (j=0; j<lTarget; j++) { 
     
                   *ppe+=(float)(*pp) * ( (float)(*pp)++); 
               } 
                
               if(*ppe>0.0) { 
                   invenergy[0] = (float) 1.0 / (*ppe + EPS); 
               } else { 
                   invenergy[0] = (float) 0.0;  
               } 
               ppe++; 
    
               measure=(float)-10000000.0;  
                
               if (crossDot > 0.0) { 
                      measure = crossDot*crossDot*invenergy[0]; 
               } 
           } 
           else { 
               measure = crossDot*crossDot*invenergy[0]; 
           } 
    
           /* check if measure is better */ 
           ftmp = crossDot*invenergy[0]; 
            
           if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) { 
               best_index = 0; 
               max_measure = measure; 
               gain = ftmp; 
           } 
    
           /* loop over lags 40+ in the first codebook section,  
              full search */ 
    
           for (icount=1; icount<range; icount++) { 
    
               /* calculate measure */ 
    
               crossDot=0.0; 
               pp = buf+LPC_FILTERORDER+lMem-lTarget-icount; 
    
               for (j=0;j<lTarget;j++) { 
                   crossDot += target[j]*(*pp++); 
               } 
                
               if (stage==0) { 
                   *ppe++ = energy[icount-1] + (*ppi)*(*ppi) -  
                       (*ppo)*(*ppo); 
                   ppo--; 
                   ppi--; 
                    
                   if(energy[icount]>0.0) { 
                       invenergy[icount] =  
                           (float)1.0/(energy[icount]+EPS); 
                   } else { 
     
                       invenergy[icount] = (float) 0.0; 
                   } 
    
                   measure=(float)-10000000.0; 
                    
                   if (crossDot > 0.0) { 
                       measure = crossDot*crossDot*invenergy[icount]; 
                   } 
               } 
               else { 
                   measure = crossDot*crossDot*invenergy[icount]; 
               } 
    
               /* check if measure is better */ 
               ftmp = crossDot*invenergy[icount]; 
    
                
               if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)){ 
                   best_index = icount; 
                   max_measure = measure; 
                   gain = ftmp; 
               } 
           } 
    
           /* Loop over lags 20-39 in the first codebook section,  
            * full search. 
            * The vectors are interpolated. 
            */ 
            
           if(lTarget==SUBL) {          
                
               /* Search for best possible lag and compute  
                  the CB-vectors' energy. */ 
    
               searchAugmentedCB(20, 39, stage, base_size-lTarget/2,  
                   target, buf+LPC_FILTERORDER+lMem, 
                   &max_measure, &best_index, &gain, energy, invenergy); 
    
           } 
    
           /* set search range for following codebook sections */ 
    
           base_index=best_index; 
    
           /* unrestricted search */ 
    
           if (CB_RESRANGE == -1) { 
               sInd=0; 
               eInd=range-1; 
               sIndAug=20; 
               eIndAug=39; 
           } 
    
     
           /* restriced search around best index from first  
           codebook section */ 
    
           else { 
               /* Initialize search indices */ 
               sIndAug=0; 
               eIndAug=0; 
               sInd=base_index-CB_RESRANGE/2; 
               eInd=sInd+CB_RESRANGE; 
                
               if(lTarget==SUBL) { 
    
                   if (sInd<0) { 
                        
                       sIndAug = 40 + sInd; 
                       eIndAug = 39; 
                       sInd=0; 
    
                   } else if( base_index < (base_size-20) ) { 
                        
                       if(eInd > range) { 
                           sInd -= (eInd-range); 
                           eInd = range; 
                       } 
                   } else { /* base_index >= (base_size-20) */ 
                        
                       if(sInd < (base_size-20)) { 
                           sIndAug = 20; 
                           sInd = 0; 
                           eInd = 0; 
                           eIndAug = 19 + CB_RESRANGE; 
                            
                           if(eIndAug > 39) { 
                               eInd = eIndAug-39; 
                               eIndAug = 39; 
                           } 
                       } else { 
                           sIndAug = 20 + sInd - (base_size-20); 
                           eIndAug = 39; 
                           sInd = 0; 
                           eInd = CB_RESRANGE - (eIndAug-sIndAug+1); 
                       } 
                   } 
    
               } else { /* lTarget = 22 */ 
                    
                   if (sInd < 0) { 
                       eInd -= sInd; 
                       sInd = 0; 
                   } 
                    
                   if(eInd > range) { 
                       sInd -= (eInd - range); 
     
                       eInd = range; 
                   } 
               } 
           } 
    
           /* search of higher codebook section */ 
    
           /* index search range */ 
           counter = sInd; 
           sInd += base_size; 
           eInd += base_size; 
            
            
           if(stage==0) { 
               ppe = energy+base_size; 
               *ppe=0.0; 
    
               pp=cbvectors+lMem-lTarget; 
               for (j=0; j<lTarget; j++) { 
                   *ppe+=(*pp)* ( (*pp)++); 
               } 
    
               ppi = cbvectors + lMem - 1 - lTarget; 
               ppo = cbvectors + lMem - 1; 
                
               for(j=0;j<(range-1);j++) { 
                   *(ppe+1) = *ppe + (*ppi)*(*ppi) - (*ppo)*(*ppo); 
                   ppo--; 
                   ppi--; 
                   ppe++; 
               } 
           } 
    
           /* loop over search range */ 
    
           for (icount=sInd; icount<eInd; icount++) { 
    
               /* calculate measure */ 
    
               crossDot=0.0; 
               pp=cbvectors + lMem - (counter++) - lTarget; 
    
               for (j=0;j<lTarget;j++) { 
                   crossDot += target[j]*(*pp++); 
               } 
                
               if(energy[icount]>0.0) { 
                   invenergy[icount] = (float) 1.0/(energy[icount]+EPS); 
               } else { 
                   invenergy[icount] = (float) 0.0; 
               } 
                
               if (stage==0) { 
     
    
                   measure=(float)-10000000.0; 
                    
                   if (crossDot > 0.0) { 
                       measure = crossDot*crossDot* 
                           invenergy[icount]; 
                   } 
               } 
               else { 
                   measure = crossDot*crossDot*invenergy[icount]; 
               } 
    
               /* check if measure is better */ 
               ftmp = crossDot*invenergy[icount]; 
    
               if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)){ 
                   best_index = icount; 
                   max_measure = measure; 
                   gain = ftmp; 
               } 
           } 
    
           /* Search the augmented CB inside the limited range. */ 
            
           if ((lTarget==SUBL)&&(sIndAug!=0)) { 
    
               searchAugmentedCB(sIndAug, eIndAug, stage,  
                   2*base_size-20, target, cbvectors+lMem, 
                   &max_measure, &best_index, &gain, energy, invenergy); 
    
           } 
    
           /* record best index */ 
    
           index[stage] = best_index; 
    
           /* gain quantization */ 
    
           if (stage==0){ 
                
               if (gain<0.0){ 
                   gain = 0.0; 
               } 
                
               if (gain>CB_MAXGAIN) { 
                   gain = (float)CB_MAXGAIN; 
               } 
               gain = gainquant(gain, 1.0, 32, &gain_index[stage]); 
           } 
           else { 
               if (stage==1) { 
                   gain = gainquant(gain, (float)fabs(gains[stage-1]),  
                       16, &gain_index[stage]); 
     
               } else { 
                   gain = gainquant(gain, (float)fabs(gains[stage-1]),  
                       8, &gain_index[stage]); 
               } 
           } 
    
           /* Extract the best (according to measure) codebook vector */ 
            
           if(lTarget==(STATE_LEN-STATE_SHORT_LEN)) { 
                
               if(index[stage]<base_size) { 
                   pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage]; 
               } else { 
                   pp=cbvectors+lMem-lTarget- 
                       index[stage]+base_size; 
               } 
           } else { 
                
               if (index[stage]<base_size) { 
                   if (index[stage]<(base_size-20)) { 
                       pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage]; 
                   } else { 
                       createAugmentedVec(index[stage]-base_size+40, 
                               buf+LPC_FILTERORDER+lMem,aug_vec); 
                       pp=aug_vec; 
                   } 
               } else { 
                   int filterno, lag_val; 
    
                   filterno=index[stage]/base_size; 
                   lag_val=index[stage]-filterno*base_size; 
    
                    
                   if (lag_val<(base_size-20)) { 
                       pp=cbvectors+filterno*lMem-lTarget- 
                           index[stage]+filterno*base_size; 
                   } else { 
                       createAugmentedVec( 
                           index[stage]-(filterno+1)*base_size+40, 
                           cbvectors+filterno*lMem,aug_vec); 
                       pp=aug_vec; 
                   } 
               } 
           } 
    
           /* Subtract the best codebook vector, according  
              to measure, from the target vector */ 
    
           for(j=0;j<lTarget;j++){ 
               cvec[j] += gain*(*pp); 
               target[j] -= gain*(*pp++); 
           } 
    
     
           /* record quantized gain */ 
    
           gains[stage]=gain; 
    
       }/* end of Main Loop. for (stage=0;... */ 
    
       /* Gain adjustment for energy matching */ 
       cene=0.0; 
       for (i=0;i<lTarget;i++) { 
           cene+=cvec[i]*cvec[i]; 
       } 
       j=gain_index[0]; 
    
       for (i=gain_index[0];i<32;i++) { 
           ftmp=cene*gain_sq5Tbl[i]*gain_sq5Tbl[i]; 
            
           if ((ftmp<(tene*gains[0]*gains[0])) &&  
               (gain_sq5Tbl[j]<(2.0*gains[0]))) { 
               j=i; 
           } 
       } 
       gain_index[0]=j; 
   }