Code cleanups (bug #66)
[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                 // XXXXXX is this the coder's intent?
121                 //*ppe+=(*pp)*(*pp++); 
122                 *ppe+=(pp[0])*(pp[1]); 
123             } 
124              
125             if(*ppe>0.0) { 
126                 invenergy[0] = (float) 1.0 / (*ppe + EPS); 
127             } else { 
128                 invenergy[0] = (float) 0.0;  
129             } 
130             ppe++; 
131  
132             measure=(float)-10000000.0;  
133              
134             if (crossDot > 0.0) { 
135                    measure = crossDot*crossDot*invenergy[0]; 
136             } 
137         } 
138         else { 
139             measure = crossDot*crossDot*invenergy[0]; 
140         } 
141  
142         /* check if measure is better */ 
143         ftmp = crossDot*invenergy[0]; 
144          
145         if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) { 
146             best_index = 0; 
147             max_measure = measure; 
148             gain = ftmp; 
149         } 
150  
151         /* loop over lags 40+ in the first codebook section,  
152            full search */ 
153  
154         for (icount=1; icount<range; icount++) { 
155  
156             /* calculate measure */ 
157  
158             crossDot=0.0; 
159             pp = buf+LPC_FILTERORDER+lMem-lTarget-icount; 
160  
161             for (j=0;j<lTarget;j++) { 
162                 crossDot += target[j]*(*pp++); 
163             } 
164              
165             if (stage==0) { 
166                 *ppe++ = energy[icount-1] + (*ppi)*(*ppi) -  
167                     (*ppo)*(*ppo); 
168                 ppo--; 
169                 ppi--; 
170                  
171                 if(energy[icount]>0.0) { 
172                     invenergy[icount] =  
173                         (float)1.0/(energy[icount]+EPS); 
174                 } else { 
175                     invenergy[icount] = (float) 0.0; 
176                 } 
177  
178                 measure=(float)-10000000.0; 
179                  
180                 if (crossDot > 0.0) { 
181                     measure = crossDot*crossDot*invenergy[icount]; 
182                 } 
183             } 
184             else { 
185                 measure = crossDot*crossDot*invenergy[icount]; 
186             } 
187  
188             /* check if measure is better */ 
189             ftmp = crossDot*invenergy[icount]; 
190  
191              
192             if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)){ 
193                 best_index = icount; 
194                 max_measure = measure; 
195                 gain = ftmp; 
196             } 
197         } 
198  
199         /* Loop over lags 20-39 in the first codebook section,  
200          * full search. 
201          * The vectors are interpolated. 
202          */ 
203          
204         if(lTarget==SUBL) {          
205              
206             /* Search for best possible lag and compute  
207                the CB-vectors' energy. */ 
208  
209             searchAugmentedCB(20, 39, stage, base_size-lTarget/2,  
210                 target, buf+LPC_FILTERORDER+lMem, 
211                 &max_measure, &best_index, &gain, energy, invenergy); 
212  
213         } 
214  
215         /* set search range for following codebook sections */ 
216  
217         base_index=best_index; 
218  
219         /* unrestricted search */ 
220  
221         if (CB_RESRANGE == -1) { 
222             sInd=0; 
223             eInd=range-1; 
224             sIndAug=20; 
225             eIndAug=39; 
226         } 
227  
228         /* restriced search around best index from first  
229         codebook section */ 
230  
231         else { 
232             /* Initialize search indices */ 
233             sIndAug=0; 
234             eIndAug=0; 
235             sInd=base_index-CB_RESRANGE/2; 
236             eInd=sInd+CB_RESRANGE; 
237              
238             if(lTarget==SUBL) { 
239  
240                 if (sInd<0) { 
241                      
242                     sIndAug = 40 + sInd; 
243                     eIndAug = 39; 
244                     sInd=0; 
245  
246                 } else if( base_index < (base_size-20) ) { 
247                      
248                     if(eInd > range) { 
249                         sInd -= (eInd-range); 
250                         eInd = range; 
251                     } 
252                 } else { /* base_index >= (base_size-20) */ 
253                      
254                     if(sInd < (base_size-20)) { 
255                         sIndAug = 20; 
256                         sInd = 0; 
257                         eInd = 0; 
258                         eIndAug = 19 + CB_RESRANGE; 
259                          
260                         if(eIndAug > 39) { 
261                             eInd = eIndAug-39; 
262                             eIndAug = 39; 
263                         } 
264                     } else { 
265                         sIndAug = 20 + sInd - (base_size-20); 
266                         eIndAug = 39; 
267                         sInd = 0; 
268                         eInd = CB_RESRANGE - (eIndAug-sIndAug+1); 
269                     } 
270                 } 
271  
272             } else { /* lTarget = 22 */ 
273                  
274                 if (sInd < 0) { 
275                     eInd -= sInd; 
276                     sInd = 0; 
277                 } 
278                  
279                 if(eInd > range) { 
280                     sInd -= (eInd - range); 
281                     eInd = range; 
282                 } 
283             } 
284         } 
285  
286         /* search of higher codebook section */ 
287  
288         /* index search range */ 
289         counter = sInd; 
290         sInd += base_size; 
291         eInd += base_size; 
292          
293          
294         if(stage==0) { 
295             ppe = energy+base_size; 
296             *ppe=0.0; 
297  
298             pp=cbvectors+lMem-lTarget; 
299             for (j=0; j<lTarget; j++) { 
300                 // XXXXXX is this the coder's intent?
301                 //*ppe+=(*pp)*(*pp++); 
302                 *ppe+=(pp[0])*(pp[1]); 
303             } 
304  
305             ppi = cbvectors + lMem - 1 - lTarget; 
306             ppo = cbvectors + lMem - 1; 
307              
308             for(j=0;j<(range-1);j++) { 
309                 *(ppe+1) = *ppe + (*ppi)*(*ppi) - (*ppo)*(*ppo); 
310                 ppo--; 
311                 ppi--; 
312                 ppe++; 
313             } 
314         } 
315  
316         /* loop over search range */ 
317  
318         for (icount=sInd; icount<eInd; icount++) { 
319  
320             /* calculate measure */ 
321  
322             crossDot=0.0; 
323             pp=cbvectors + lMem - (counter++) - lTarget; 
324  
325             for (j=0;j<lTarget;j++) { 
326                 crossDot += target[j]*(*pp++); 
327             } 
328              
329             if(energy[icount]>0.0) { 
330                 invenergy[icount] = (float) 1.0/(energy[icount]+EPS); 
331             } else { 
332                 invenergy[icount] = (float) 0.0; 
333             } 
334              
335             if (stage==0) { 
336  
337                 measure=(float)-10000000.0; 
338                  
339                 if (crossDot > 0.0) { 
340                     measure = crossDot*crossDot* 
341                         invenergy[icount]; 
342                 } 
343             } 
344             else { 
345                 measure = crossDot*crossDot*invenergy[icount]; 
346             } 
347  
348             /* check if measure is better */ 
349             ftmp = crossDot*invenergy[icount]; 
350  
351             if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)){ 
352                 best_index = icount; 
353                 max_measure = measure; 
354                 gain = ftmp; 
355             } 
356         } 
357  
358         /* Search the augmented CB inside the limited range. */ 
359          
360         if ((lTarget==SUBL)&&(sIndAug!=0)) { 
361  
362             searchAugmentedCB(sIndAug, eIndAug, stage,  
363                 2*base_size-20, target, cbvectors+lMem, 
364                 &max_measure, &best_index, &gain, energy, invenergy); 
365  
366         } 
367  
368         /* record best index */ 
369  
370         index[stage] = best_index; 
371  
372         /* gain quantization */ 
373  
374         if (stage==0){ 
375              
376             if (gain<0.0){ 
377                 gain = 0.0; 
378             } 
379              
380             if (gain>CB_MAXGAIN) { 
381                 gain = (float)CB_MAXGAIN; 
382             } 
383             gain = gainquant(gain, 1.0, 32, &gain_index[stage]); 
384         } 
385         else { 
386             if (stage==1) { 
387                 gain = gainquant(gain, (float)fabs(gains[stage-1]),  
388                     16, &gain_index[stage]); 
389             } else { 
390                 gain = gainquant(gain, (float)fabs(gains[stage-1]),  
391                     8, &gain_index[stage]); 
392             } 
393         } 
394  
395         /* Extract the best (according to measure) codebook vector */ 
396          
397         if(lTarget==(STATE_LEN-STATE_SHORT_LEN)) { 
398              
399             if(index[stage]<base_size) { 
400                 pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage]; 
401             } else { 
402                 pp=cbvectors+lMem-lTarget- 
403                     index[stage]+base_size; 
404             } 
405         } else { 
406              
407             if (index[stage]<base_size) { 
408                 if (index[stage]<(base_size-20)) { 
409                     pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage]; 
410                 } else { 
411                     createAugmentedVec(index[stage]-base_size+40, 
412                             buf+LPC_FILTERORDER+lMem,aug_vec); 
413                     pp=aug_vec; 
414                 } 
415             } else { 
416                 int filterno, lag_val; 
417  
418                 filterno=index[stage]/base_size; 
419                 lag_val=index[stage]-filterno*base_size; 
420  
421                  
422                 if (lag_val<(base_size-20)) { 
423                     pp=cbvectors+filterno*lMem-lTarget- 
424                         index[stage]+filterno*base_size; 
425                 } else { 
426                     createAugmentedVec( 
427                         index[stage]-(filterno+1)*base_size+40, 
428                         cbvectors+filterno*lMem,aug_vec); 
429                     pp=aug_vec; 
430                 } 
431             } 
432         } 
433  
434         /* Subtract the best codebook vector, according  
435            to measure, from the target vector */ 
436  
437         for(j=0;j<lTarget;j++){ 
438             cvec[j] += gain*(*pp); 
439             target[j] -= gain*(*pp++); 
440         } 
441  
442         /* record quantized gain */ 
443  
444         gains[stage]=gain; 
445  
446     }/* end of Main Loop. for (stage=0;... */ 
447  
448     /* Gain adjustment for energy matching */ 
449     cene=0.0; 
450     for (i=0;i<lTarget;i++) { 
451         cene+=cvec[i]*cvec[i]; 
452     } 
453     j=gain_index[0]; 
454  
455     for (i=gain_index[0];i<32;i++) { 
456         ftmp=cene*gain_sq5Tbl[i]*gain_sq5Tbl[i]; 
457          
458         if ((ftmp<(tene*gains[0]*gains[0])) &&  
459             (gain_sq5Tbl[j]<(2.0*gains[0]))) { 
460             j=i; 
461         } 
462     } 
463     gain_index[0]=j; 
464
465  
466