2279deb733a162034f97f0288b1e2eee75cdb531
[asterisk/asterisk.git] / codecs / ilbc / iCBSearch.c
1  
2 /****************************************************************** 
3  
4     iLBC Speech Coder ANSI-C Source Code 
5  
6     iCBSearch.c  
7  
8     Copyright (c) 2001, 
9     Global IP Sound AB. 
10     All rights reserved. 
11  
12 ******************************************************************/ 
13  
14 #include <math.h> 
15 #include <string.h> 
16  
17 #include "iLBC_define.h" 
18 #include "iCBSearch.h"
19 #include "gainquant.h" 
20 #include "createCB.h" 
21 #include "filter.h" 
22 #include "constants.h" 
23  
24 /*----------------------------------------------------------------* 
25  *  Search routine for codebook encoding and gain quantization. 
26  *---------------------------------------------------------------*/ 
27  
28 void iCBSearch( 
29     int *index,         /* (o) Codebook indices */ 
30     int *gain_index,/* (o) Gain quantization indices */ 
31     float *intarget,/* (i) Target vector for encoding */     
32     float *mem,         /* (i) Buffer for codebook construction */ 
33     int lMem,           /* (i) Length of buffer */ 
34     int lTarget,    /* (i) Length of vector */ 
35     int nStages,    /* (i) Number of codebook stages */ 
36     float *weightDenum, /* (i) weighting filter coefficients */ 
37     float *weightState, /* (i) weighting filter state */ 
38     int block           /* (i) the subblock number */ 
39 ){ 
40     int i, j, icount, stage, best_index, range, counter; 
41     float max_measure, gain, measure, crossDot, ftmp; 
42     float gains[CB_NSTAGES]; 
43     float target[SUBL]; 
44     int base_index, sInd, eInd, base_size; 
45     int sIndAug=0, eIndAug=0; 
46     float buf[CB_MEML+SUBL+2*LPC_FILTERORDER]; 
47     float invenergy[CB_EXPAND*128], energy[CB_EXPAND*128]; 
48     float *pp, *ppi=0, *ppo=0, *ppe=0; 
49     float cbvectors[CB_MEML]; 
50     float tene, cene, cvec[SUBL]; 
51     float aug_vec[SUBL]; 
52  
53     memset(cvec,0,SUBL*sizeof(float));   
54  
55     /* Determine size of codebook sections */ 
56  
57     base_size=lMem-lTarget+1; 
58      
59     if (lTarget==SUBL) { 
60         base_size=lMem-lTarget+1+lTarget/2; 
61     } 
62  
63     /* setup buffer for weighting */ 
64  
65     memcpy(buf,weightState,sizeof(float)*LPC_FILTERORDER); 
66     memcpy(buf+LPC_FILTERORDER,mem,lMem*sizeof(float)); 
67     memcpy(buf+LPC_FILTERORDER+lMem,intarget,lTarget*sizeof(float)); 
68  
69     /* weighting */ 
70  
71     AllPoleFilter(buf+LPC_FILTERORDER, weightDenum,  
72         lMem+lTarget, LPC_FILTERORDER); 
73      
74     /* Construct the codebook and target needed */ 
75  
76     memcpy(target, buf+LPC_FILTERORDER+lMem, lTarget*sizeof(float)); 
77  
78     tene=0.0; 
79     for (i=0;i<lTarget;i++) { 
80         tene+=target[i]*target[i]; 
81     } 
82  
83     /* Prepare search over one more codebook section. This section  
84        is created by filtering the original buffer with a filter. */ 
85          
86     filteredCBvecs(cbvectors, buf+LPC_FILTERORDER, lMem); 
87  
88     /* The Main Loop over stages */ 
89  
90     for (stage=0;stage<nStages; stage++) { 
91  
92         range = search_rangeTbl[block][stage]; 
93  
94         /* initialize search measure */ 
95  
96         max_measure = (float)-10000000.0; 
97         gain = (float)0.0; 
98         best_index = 0; 
99  
100         /* Compute cross dot product between the target  
101            and the CB memory */ 
102  
103         crossDot=0.0; 
104         pp=buf+LPC_FILTERORDER+lMem-lTarget; 
105         for (j=0; j<lTarget; j++) { 
106             crossDot += target[j]*(*pp++); 
107         }        
108          
109         if (stage==0) { 
110  
111             /* Calculate energy in the first block of  
112               'lTarget' sampels. */ 
113             ppe = energy; 
114             ppi = buf+LPC_FILTERORDER+lMem-lTarget-1; 
115             ppo = buf+LPC_FILTERORDER+lMem-1; 
116  
117             *ppe=0.0; 
118             pp=buf+LPC_FILTERORDER+lMem-lTarget; 
119             for (j=0; j<lTarget; j++) { 
120                 *ppe+=(*pp)*(*pp++); 
121             } 
122              
123             if(*ppe>0.0) { 
124                 invenergy[0] = (float) 1.0 / (*ppe + EPS); 
125             } else { 
126                 invenergy[0] = (float) 0.0;  
127             } 
128             ppe++; 
129  
130             measure=(float)-10000000.0;  
131              
132             if (crossDot > 0.0) { 
133                    measure = crossDot*crossDot*invenergy[0]; 
134             } 
135         } 
136         else { 
137             measure = crossDot*crossDot*invenergy[0]; 
138         } 
139  
140         /* check if measure is better */ 
141         ftmp = crossDot*invenergy[0]; 
142          
143         if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) { 
144             best_index = 0; 
145             max_measure = measure; 
146             gain = ftmp; 
147         } 
148  
149         /* loop over lags 40+ in the first codebook section,  
150            full search */ 
151  
152         for (icount=1; icount<range; icount++) { 
153  
154             /* calculate measure */ 
155  
156             crossDot=0.0; 
157             pp = buf+LPC_FILTERORDER+lMem-lTarget-icount; 
158  
159             for (j=0;j<lTarget;j++) { 
160                 crossDot += target[j]*(*pp++); 
161             } 
162              
163             if (stage==0) { 
164                 *ppe++ = energy[icount-1] + (*ppi)*(*ppi) -  
165                     (*ppo)*(*ppo); 
166                 ppo--; 
167                 ppi--; 
168                  
169                 if(energy[icount]>0.0) { 
170                     invenergy[icount] =  
171                         (float)1.0/(energy[icount]+EPS); 
172                 } else { 
173                     invenergy[icount] = (float) 0.0; 
174                 } 
175  
176                 measure=(float)-10000000.0; 
177                  
178                 if (crossDot > 0.0) { 
179                     measure = crossDot*crossDot*invenergy[icount]; 
180                 } 
181             } 
182             else { 
183                 measure = crossDot*crossDot*invenergy[icount]; 
184             } 
185  
186             /* check if measure is better */ 
187             ftmp = crossDot*invenergy[icount]; 
188  
189              
190             if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)){ 
191                 best_index = icount; 
192                 max_measure = measure; 
193                 gain = ftmp; 
194             } 
195         } 
196  
197         /* Loop over lags 20-39 in the first codebook section,  
198          * full search. 
199          * The vectors are interpolated. 
200          */ 
201          
202         if(lTarget==SUBL) {          
203              
204             /* Search for best possible lag and compute  
205                the CB-vectors' energy. */ 
206  
207             searchAugmentedCB(20, 39, stage, base_size-lTarget/2,  
208                 target, buf+LPC_FILTERORDER+lMem, 
209                 &max_measure, &best_index, &gain, energy, invenergy); 
210  
211         } 
212  
213         /* set search range for following codebook sections */ 
214  
215         base_index=best_index; 
216  
217         /* unrestricted search */ 
218  
219         if (CB_RESRANGE == -1) { 
220             sInd=0; 
221             eInd=range-1; 
222             sIndAug=20; 
223             eIndAug=39; 
224         } 
225  
226         /* restriced search around best index from first  
227         codebook section */ 
228  
229         else { 
230             /* Initialize search indices */ 
231             sIndAug=0; 
232             eIndAug=0; 
233             sInd=base_index-CB_RESRANGE/2; 
234             eInd=sInd+CB_RESRANGE; 
235              
236             if(lTarget==SUBL) { 
237  
238                 if (sInd<0) { 
239                      
240                     sIndAug = 40 + sInd; 
241                     eIndAug = 39; 
242                     sInd=0; 
243  
244                 } else if( base_index < (base_size-20) ) { 
245                      
246                     if(eInd > range) { 
247                         sInd -= (eInd-range); 
248                         eInd = range; 
249                     } 
250                 } else { /* base_index >= (base_size-20) */ 
251                      
252                     if(sInd < (base_size-20)) { 
253                         sIndAug = 20; 
254                         sInd = 0; 
255                         eInd = 0; 
256                         eIndAug = 19 + CB_RESRANGE; 
257                          
258                         if(eIndAug > 39) { 
259                             eInd = eIndAug-39; 
260                             eIndAug = 39; 
261                         } 
262                     } else { 
263                         sIndAug = 20 + sInd - (base_size-20); 
264                         eIndAug = 39; 
265                         sInd = 0; 
266                         eInd = CB_RESRANGE - (eIndAug-sIndAug+1); 
267                     } 
268                 } 
269  
270             } else { /* lTarget = 22 */ 
271                  
272                 if (sInd < 0) { 
273                     eInd -= sInd; 
274                     sInd = 0; 
275                 } 
276                  
277                 if(eInd > range) { 
278                     sInd -= (eInd - range); 
279                     eInd = range; 
280                 } 
281             } 
282         } 
283  
284         /* search of higher codebook section */ 
285  
286         /* index search range */ 
287         counter = sInd; 
288         sInd += base_size; 
289         eInd += base_size; 
290          
291          
292         if(stage==0) { 
293             ppe = energy+base_size; 
294             *ppe=0.0; 
295  
296             pp=cbvectors+lMem-lTarget; 
297             for (j=0; j<lTarget; j++) { 
298                 *ppe+=(*pp)*(*pp++); 
299             } 
300  
301             ppi = cbvectors + lMem - 1 - lTarget; 
302             ppo = cbvectors + lMem - 1; 
303              
304             for(j=0;j<(range-1);j++) { 
305                 *(ppe+1) = *ppe + (*ppi)*(*ppi) - (*ppo)*(*ppo); 
306                 ppo--; 
307                 ppi--; 
308                 ppe++; 
309             } 
310         } 
311  
312         /* loop over search range */ 
313  
314         for (icount=sInd; icount<eInd; icount++) { 
315  
316             /* calculate measure */ 
317  
318             crossDot=0.0; 
319             pp=cbvectors + lMem - (counter++) - lTarget; 
320  
321             for (j=0;j<lTarget;j++) { 
322                 crossDot += target[j]*(*pp++); 
323             } 
324              
325             if(energy[icount]>0.0) { 
326                 invenergy[icount] = (float) 1.0/(energy[icount]+EPS); 
327             } else { 
328                 invenergy[icount] = (float) 0.0; 
329             } 
330              
331             if (stage==0) { 
332  
333                 measure=(float)-10000000.0; 
334                  
335                 if (crossDot > 0.0) { 
336                     measure = crossDot*crossDot* 
337                         invenergy[icount]; 
338                 } 
339             } 
340             else { 
341                 measure = crossDot*crossDot*invenergy[icount]; 
342             } 
343  
344             /* check if measure is better */ 
345             ftmp = crossDot*invenergy[icount]; 
346  
347             if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)){ 
348                 best_index = icount; 
349                 max_measure = measure; 
350                 gain = ftmp; 
351             } 
352         } 
353  
354         /* Search the augmented CB inside the limited range. */ 
355          
356         if ((lTarget==SUBL)&&(sIndAug!=0)) { 
357  
358             searchAugmentedCB(sIndAug, eIndAug, stage,  
359                 2*base_size-20, target, cbvectors+lMem, 
360                 &max_measure, &best_index, &gain, energy, invenergy); 
361  
362         } 
363  
364         /* record best index */ 
365  
366         index[stage] = best_index; 
367  
368         /* gain quantization */ 
369  
370         if (stage==0){ 
371              
372             if (gain<0.0){ 
373                 gain = 0.0; 
374             } 
375              
376             if (gain>CB_MAXGAIN) { 
377                 gain = (float)CB_MAXGAIN; 
378             } 
379             gain = gainquant(gain, 1.0, 32, &gain_index[stage]); 
380         } 
381         else { 
382             if (stage==1) { 
383                 gain = gainquant(gain, (float)fabs(gains[stage-1]),  
384                     16, &gain_index[stage]); 
385             } else { 
386                 gain = gainquant(gain, (float)fabs(gains[stage-1]),  
387                     8, &gain_index[stage]); 
388             } 
389         } 
390  
391         /* Extract the best (according to measure) codebook vector */ 
392          
393         if(lTarget==(STATE_LEN-STATE_SHORT_LEN)) { 
394              
395             if(index[stage]<base_size) { 
396                 pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage]; 
397             } else { 
398                 pp=cbvectors+lMem-lTarget- 
399                     index[stage]+base_size; 
400             } 
401         } else { 
402              
403             if (index[stage]<base_size) { 
404                 if (index[stage]<(base_size-20)) { 
405                     pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage]; 
406                 } else { 
407                     createAugmentedVec(index[stage]-base_size+40, 
408                             buf+LPC_FILTERORDER+lMem,aug_vec); 
409                     pp=aug_vec; 
410                 } 
411             } else { 
412                 int filterno, lag_val; 
413  
414                 filterno=index[stage]/base_size; 
415                 lag_val=index[stage]-filterno*base_size; 
416  
417                  
418                 if (lag_val<(base_size-20)) { 
419                     pp=cbvectors+filterno*lMem-lTarget- 
420                         index[stage]+filterno*base_size; 
421                 } else { 
422                     createAugmentedVec( 
423                         index[stage]-(filterno+1)*base_size+40, 
424                         cbvectors+filterno*lMem,aug_vec); 
425                     pp=aug_vec; 
426                 } 
427             } 
428         } 
429  
430         /* Subtract the best codebook vector, according  
431            to measure, from the target vector */ 
432  
433         for(j=0;j<lTarget;j++){ 
434             cvec[j] += gain*(*pp); 
435             target[j] -= gain*(*pp++); 
436         } 
437  
438         /* record quantized gain */ 
439  
440         gains[stage]=gain; 
441  
442     }/* end of Main Loop. for (stage=0;... */ 
443  
444     /* Gain adjustment for energy matching */ 
445     cene=0.0; 
446     for (i=0;i<lTarget;i++) { 
447         cene+=cvec[i]*cvec[i]; 
448     } 
449     j=gain_index[0]; 
450  
451     for (i=gain_index[0];i<32;i++) { 
452         ftmp=cene*gain_sq5Tbl[i]*gain_sq5Tbl[i]; 
453          
454         if ((ftmp<(tene*gains[0]*gains[0])) &&  
455             (gain_sq5Tbl[j]<(2.0*gains[0]))) { 
456             j=i; 
457         } 
458     } 
459     gain_index[0]=j; 
460
461  
462