Big changes in both narrowband and wideband. Retrained LSP codebook,
[speexdsp.git] / libspeex / cb_search.c
1 /* Original copyright */
2 /*-----------------------------------------------------------------------*\
3
4     FILE........: GAINSHAPE.C
5     TYPE........: C Module
6     AUTHOR......: David Rowe
7     COMPANY.....: Voicetronix
8     DATE CREATED: 19/2/02
9
10     General gain-shape codebook search.
11
12 \*-----------------------------------------------------------------------*/
13
14
15 /* Modified by Jean-Marc Valin 2002
16
17    This library is free software; you can redistribute it and/or
18    modify it under the terms of the GNU Lesser General Public
19    License as published by the Free Software Foundation; either
20    version 2.1 of the License, or (at your option) any later version.
21    
22    This library is distributed in the hope that it will be useful,
23    but WITHOUT ANY WARRANTY; without even the implied warranty of
24    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
25    Lesser General Public License for more details.
26    
27    You should have received a copy of the GNU Lesser General Public
28    License along with this library; if not, write to the Free Software
29    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
30 */
31
32
33
34 #include <stdlib.h>
35 #include <cb_search.h>
36 #include "filters.h"
37 #include <math.h>
38 #include <stdio.h>
39 #include "stack_alloc.h"
40 #include "vq.h"
41
42 #define min(a,b) ((a) < (b) ? (a) : (b))
43 #define max(a,b) ((a) > (b) ? (a) : (b))
44
45
46 static float scal_gains4[16] = {
47    0.27713,
48    0.49282,
49    0.69570,
50    0.90786,
51    1.14235,
52    1.42798,
53    1.80756,
54    2.42801
55 };
56
57
58 /*---------------------------------------------------------------------------*\
59                                                                              
60  void overlap_cb_search()                                                             
61                                                                               
62  Searches a gain/shape codebook consisting of overlapping entries for the    
63  closest vector to the target.  Gives identical results to search() above   
64  buts uses fast end correction algorithm for the synthesis of response       
65  vectors.                                                                     
66                                                                              
67 \*---------------------------------------------------------------------------*/
68
69 float overlap_cb_search(
70 float target[],                 /* target vector */
71 float ak[],                     /* LPCs for this subframe */
72 float awk1[],                   /* Weighted LPCs for this subframe */
73 float awk2[],                   /* Weighted LPCs for this subframe */
74 float codebook[],               /* overlapping codebook */
75 int   entries,                  /* number of overlapping entries to search */
76 float *gain,                    /* gain of optimum entry */
77 int   *index,                   /* index of optimum entry */
78 int   p,                        /* number of LPC coeffs */
79 int   nsf,                      /* number of samples in subframe */
80 float *stack
81 )
82 {
83   float *resp;                  /* zero state response to current entry */
84   float *h;                     /* impulse response of synthesis filter */
85   float *impulse;               /* excitation vector containing one impulse */
86   float d,e,g,score;            /* codebook searching variables */
87   float bscore;                 /* score of "best" vector so far */
88   int i,k;                      /* loop variables */
89
90   /* Initialise */
91   
92   /*resp = (float*)malloc(sizeof(float)*nsf);
93   h = (float*)malloc(sizeof(float)*nsf);
94   impulse = (float*)malloc(sizeof(float)*nsf);
95   */
96   resp=PUSH(stack, nsf);
97   h=PUSH(stack, nsf);
98   impulse=PUSH(stack, nsf);
99
100   for(i=0; i<nsf; i++)
101     impulse[i] = 0.0;
102    
103   *gain = 0.0;
104   *index = 0;
105   bscore = 0.0;
106   impulse[0] = 1.0;
107
108   /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
109   residue_zero(impulse, awk1, h, nsf, p);
110   syn_filt_zero(h, ak, h, nsf, p);
111   syn_filt_zero(h, awk2, h, nsf,p);
112   
113   /* Calculate codebook zero-response */
114   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
115   syn_filt_zero(resp,ak,resp,nsf,p);
116   syn_filt_zero(resp,awk2,resp,nsf,p);
117     
118   /* Search codebook backwards using end correction for synthesis */
119   
120   for(k=entries-1; k>=0; k--) {
121
122     d = 0.0; e = 0.0;
123     for(i=0; i<nsf; i++) {
124       d += target[i]*resp[i];
125       e += resp[i]*resp[i];
126     }
127     g = d/(e+.0001);
128     score = g*d;
129     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
130     if (score >= bscore) {
131       bscore = score;
132       *gain = g;
133       *index = k;
134     }
135     
136     /* Synthesise next entry */
137     
138     if (k) {
139       for(i=nsf-1; i>=1; i--)
140         resp[i] = resp[i-1] + codebook[k-1]*h[i];
141       resp[0] = codebook[k-1]*h[0];
142     }
143   }
144
145   /*free(resp);
146   free(h);
147   free(impulse);*/
148   POP(stack);
149   POP(stack);
150   POP(stack);
151   return bscore;
152 }
153
154
155
156 void split_cb_search(
157 float target[],                 /* target vector */
158 float ak[],                     /* LPCs for this subframe */
159 float awk1[],                   /* Weighted LPCs for this subframe */
160 float awk2[],                   /* Weighted LPCs for this subframe */
161 void *par,                      /* Codebook/search parameters*/
162 int   p,                        /* number of LPC coeffs */
163 int   nsf,                      /* number of samples in subframe */
164 float *exc,
165 FrameBits *bits,
166 float *stack
167 )
168 {
169    int i,j, id;
170    float *resp, *E, q;
171    float *t, *r, *e;
172    float *gains;
173    int *ind;
174    float *shape_cb;
175    int shape_cb_size, subvect_size, nb_subvect;
176    float exc_energy=0;
177    split_cb_params *params;
178
179    params = (split_cb_params *) par;
180    subvect_size = params->subvect_size;
181    nb_subvect = params->nb_subvect;
182    shape_cb_size = 1<<params->shape_bits;
183    shape_cb = params->shape_cb;
184    resp = PUSH(stack, shape_cb_size*subvect_size);
185    E = PUSH(stack, shape_cb_size);
186    t = PUSH(stack, nsf);
187    r = PUSH(stack, nsf);
188    e = PUSH(stack, nsf);
189    gains = PUSH(stack, nb_subvect);
190    ind = (int*)PUSH(stack, nb_subvect);
191
192    /* Compute energy of the "real excitation" */
193    syn_filt_zero(target, awk1, e, nsf, p);
194    residue_zero(e, ak, e, nsf, p);
195    residue_zero(e, awk2, e, nsf, p);
196    for (i=0;i<nsf;i++)
197       exc_energy += e[i]*e[i];
198    exc_energy=sqrt(exc_energy/nb_subvect);
199
200    /* Quantize global ("average") gain */
201    q=log(exc_energy+.1);
202    q=floor(.5+2*(q-2));
203    if (q<0)
204       q=0;
205    if (q>15)
206       q=15;
207    id = (int)q;
208    speex_bits_pack(bits, id, 4);
209    exc_energy=exp(.5*q+2);
210
211
212    for (i=0;i<nsf;i++)
213       t[i]=target[i];
214
215    e[0]=1;
216    for (i=1;i<nsf;i++)
217       e[i]=0;
218    residue_zero(e, awk1, r, nsf, p);
219    syn_filt_zero(r, ak, r, nsf, p);
220    syn_filt_zero(r, awk2, r, nsf,p);
221    
222    /* Pre-compute codewords response and energy */
223    for (i=0;i<shape_cb_size;i++)
224    {
225       float *res = resp+i*subvect_size;
226
227       /* Compute codeword response */
228       int k;
229       for(j=0;j<subvect_size;j++)
230          res[j]=0;
231       for(j=0;j<subvect_size;j++)
232       {
233          for (k=j;k<subvect_size;k++)
234             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
235       }
236       /* Compute energy of codeword response */
237       E[i]=0;
238       for(j=0;j<subvect_size;j++)
239          E[i]+=res[j]*res[j];
240       E[i]=1/(.001+E[i]);
241    }
242
243    for (i=0;i<nb_subvect;i++)
244    {
245       int best_index=0, k, m;
246       float g, corr, best_gain=0, score, best_score=-1;
247       /* Find best codeword for current sub-vector */
248       for (j=0;j<shape_cb_size;j++)
249       {
250          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
251          score=corr*corr*E[j];
252          g = corr*E[j];
253          if (score>best_score)
254          {
255             best_index=j;
256             best_score=score;
257             best_gain=g;
258          }
259       }
260       speex_bits_pack(bits,best_index,params->shape_bits);
261       
262       /* Quantize gain */
263       {
264          int s=0, best_id;
265          best_gain /= .01+exc_energy;
266          if (best_gain<0)
267          {
268             best_gain=-best_gain;
269             s=1;
270          }
271
272          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
273          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
274
275          best_gain=scal_gains4[best_id];
276          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
277          if (s)
278             best_gain=-best_gain;
279          best_gain *= exc_energy;
280          speex_bits_pack(bits,s,1);
281          speex_bits_pack(bits,best_id,3);
282       }
283       ind[i]=best_index;
284       gains[i]=best_gain;
285       /* Update target for next subvector */
286       for (j=0;j<subvect_size;j++)
287       {
288          g=best_gain*shape_cb[best_index*subvect_size+j];
289          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
290             t[k] -= g*r[m];
291       }
292    }
293    
294    /* Put everything back together */
295    for (i=0;i<nb_subvect;i++)
296       for (j=0;j<subvect_size;j++)
297          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
298
299    /* Update excitation */
300    for (j=0;j<nsf;j++)
301       exc[j]+=e[j];
302    
303    /* Update target */
304    residue_zero(e, awk1, r, nsf, p);
305    syn_filt_zero(r, ak, r, nsf, p);
306    syn_filt_zero(r, awk2, r, nsf,p);
307    for (j=0;j<nsf;j++)
308       target[j]-=r[j];
309
310    
311
312
313    POP(stack);
314    POP(stack);
315    POP(stack);
316    POP(stack);
317    POP(stack);
318    POP(stack);
319 }
320
321 void split_cb_search_nogain(
322 float target[],                 /* target vector */
323 float ak[],                     /* LPCs for this subframe */
324 float awk1[],                   /* Weighted LPCs for this subframe */
325 float awk2[],                   /* Weighted LPCs for this subframe */
326 void *par,                      /* Codebook/search parameters*/
327 int   p,                        /* number of LPC coeffs */
328 int   nsf,                      /* number of samples in subframe */
329 float *exc,
330 FrameBits *bits,
331 float *stack
332 )
333 {
334    int i,j;
335    float *resp;
336    float *t, *r, *e;
337    int *ind;
338    float *shape_cb;
339    int shape_cb_size, subvect_size, nb_subvect;
340    split_cb_params *params;
341
342    params = (split_cb_params *) par;
343    subvect_size = params->subvect_size;
344    nb_subvect = params->nb_subvect;
345    shape_cb_size = 1<<params->shape_bits;
346    shape_cb = params->shape_cb;
347    resp = PUSH(stack, shape_cb_size*subvect_size);
348    t = PUSH(stack, nsf);
349    r = PUSH(stack, nsf);
350    e = PUSH(stack, nsf);
351    ind = (int*)PUSH(stack, nb_subvect);
352
353    for (i=0;i<nsf;i++)
354       t[i]=target[i];
355
356    e[0]=1;
357    for (i=1;i<nsf;i++)
358       e[i]=0;
359    residue_zero(e, awk1, r, nsf, p);
360    syn_filt_zero(r, ak, r, nsf, p);
361    syn_filt_zero(r, awk2, r, nsf,p);
362    
363    /* Pre-compute codewords response and energy */
364    for (i=0;i<shape_cb_size;i++)
365    {
366       float *res = resp+i*subvect_size;
367
368       /* Compute codeword response */
369       int k;
370       for(j=0;j<subvect_size;j++)
371          res[j]=0;
372       for(j=0;j<subvect_size;j++)
373       {
374          for (k=j;k<subvect_size;k++)
375             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
376       }
377    }
378
379    for (i=0;i<nb_subvect;i++)
380    {
381       int best_index=0, k, m;
382       float g, dist, best_dist=-1;
383       float *a, *b;
384       /* Find best codeword for current sub-vector */
385       for (j=0;j<shape_cb_size;j++)
386       {
387          dist=0;
388          a=resp+j*subvect_size;
389          b=t+subvect_size*i;
390          for (k=0;k<subvect_size;k++)
391             dist += (a[k]-b[k])*(a[k]-b[k]);
392          if (dist<best_dist || j==0)
393          {
394             best_dist=dist;
395             best_index=j;
396          }
397       }
398       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
399       speex_bits_pack(bits,best_index,params->shape_bits);
400
401       ind[i]=best_index;
402       /* Update target for next subvector */
403       for (j=0;j<subvect_size;j++)
404       {
405          g=shape_cb[best_index*subvect_size+j];
406          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
407             t[k] -= g*r[m];
408       }
409    }
410    
411    /* Put everything back together */
412    for (i=0;i<nb_subvect;i++)
413       for (j=0;j<subvect_size;j++)
414          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
415
416    /* Update excitation */
417    for (j=0;j<nsf;j++)
418       exc[j]+=e[j];
419    
420    /* Update target */
421    residue_zero(e, awk1, r, nsf, p);
422    syn_filt_zero(r, ak, r, nsf, p);
423    syn_filt_zero(r, awk2, r, nsf,p);
424    for (j=0;j<nsf;j++)
425       target[j]-=r[j];
426
427    
428
429
430    POP(stack);
431    POP(stack);
432    POP(stack);
433    POP(stack);
434 }
435
436
437 void split_cb_search_nogain2(
438 float target[],                 /* target vector */
439 float ak[],                     /* LPCs for this subframe */
440 float awk1[],                   /* Weighted LPCs for this subframe */
441 float awk2[],                   /* Weighted LPCs for this subframe */
442 void *par,                      /* Codebook/search parameters*/
443 int   p,                        /* number of LPC coeffs */
444 int   nsf,                      /* number of samples in subframe */
445 float *exc,
446 FrameBits *bits,
447 float *stack
448 )
449 {
450    int i,j;
451    float *resp;
452    float *t, *r, *e, *E;
453    int *ind;
454    float *shape_cb;
455    int shape_cb_size, subvect_size, nb_subvect;
456    split_cb_params *params;
457
458    params = (split_cb_params *) par;
459    subvect_size = params->subvect_size;
460    nb_subvect = params->nb_subvect;
461    shape_cb_size = 1<<params->shape_bits;
462    shape_cb = params->shape_cb;
463    resp = PUSH(stack, shape_cb_size*subvect_size);
464    t = PUSH(stack, nsf);
465    r = PUSH(stack, nsf);
466    e = PUSH(stack, nsf);
467    E = PUSH(stack, shape_cb_size);
468    ind = (int*)PUSH(stack, nb_subvect);
469
470    for (i=0;i<nsf;i++)
471       t[i]=target[i];
472
473    e[0]=1;
474    for (i=1;i<nsf;i++)
475       e[i]=0;
476    residue_zero(e, awk1, r, nsf, p);
477    syn_filt_zero(r, ak, r, nsf, p);
478    syn_filt_zero(r, awk2, r, nsf,p);
479    
480    /* Pre-compute codewords response and energy */
481    for (i=0;i<shape_cb_size;i++)
482    {
483       float *res = resp+i*subvect_size;
484
485       /* Compute codeword response */
486       int k;
487       for(j=0;j<subvect_size;j++)
488          res[j]=0;
489       for(j=0;j<subvect_size;j++)
490       {
491          for (k=j;k<subvect_size;k++)
492             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
493       }
494       E[i]=0;
495       for(j=0;j<subvect_size;j++)
496          E[i]+=res[j]*res[j];
497    }
498
499    for (i=0;i<nb_subvect;i++)
500    {
501       int best_index[2]={0,0}, k, m;
502       float g, dist, best_dist[2]={-1,-1};
503       float *a, *x;
504       float energy=0;
505       x=t+subvect_size*i;
506       for (k=0;k<subvect_size;k++)
507          energy+=x[k]*x[k];
508       /* Find best codeword for current sub-vector */
509       for (j=0;j<shape_cb_size;j++)
510       {
511          dist=0;
512          a=resp+j*subvect_size;
513          dist=energy+E[j];
514          for (k=0;k<subvect_size;k++)
515             dist -= 2*a[k]*x[k];
516          if (dist<best_dist[0] || best_dist[0]<0)
517          {
518             best_dist[1]=best_dist[0];
519             best_index[1]=best_index[0];
520             best_dist[0]=dist;
521             best_index[0]=j;
522          } else if (dist<best_dist[1] || best_dist[1]<0)
523          {
524             best_dist[1]=dist;
525             best_index[1]=j;
526          }
527       }
528       if (i<nb_subvect-1)
529       {
530          int nbest;
531          float *tt, err[2];
532          float best_score[2];
533          tt=PUSH(stack,nsf);
534          for (nbest=0;nbest<2;nbest++)
535          {
536             for (j=0;j<nsf;j++)
537                tt[j]=t[j];
538             for (j=0;j<subvect_size;j++)
539             {
540                g=shape_cb[best_index[nbest]*subvect_size+j];
541                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
542                   tt[k] -= g*r[m];
543             }
544             
545             {
546                int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);
547                for (j=0;j<subvect_size;j++)
548                {
549                   g=shape_cb[i2*subvect_size+j];
550                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
551                      tt[k] -= g*r[m];
552                }
553             }
554
555             err[nbest]=0;
556             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
557                err[nbest]-=tt[j]*tt[j];
558             
559             best_score[nbest]=err[nbest];
560          }
561
562          if (best_score[1]>best_score[0])
563          {
564             best_index[0]=best_index[1];
565             best_score[0]=best_score[1];
566          }
567          POP(stack);
568
569       }
570
571       ind[i]=best_index[0];
572
573       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
574       speex_bits_pack(bits,ind[i],params->shape_bits);
575
576       /* Update target for next subvector */
577       for (j=0;j<subvect_size;j++)
578       {
579          g=shape_cb[ind[i]*subvect_size+j];
580          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
581             t[k] -= g*r[m];
582       }
583    }
584    
585    /* Put everything back together */
586    for (i=0;i<nb_subvect;i++)
587       for (j=0;j<subvect_size;j++)
588          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
589
590    /* Update excitation */
591    for (j=0;j<nsf;j++)
592       exc[j]+=e[j];
593    
594    /* Update target */
595    residue_zero(e, awk1, r, nsf, p);
596    syn_filt_zero(r, ak, r, nsf, p);
597    syn_filt_zero(r, awk2, r, nsf,p);
598    for (j=0;j<nsf;j++)
599       target[j]-=r[j];
600
601    
602
603    POP(stack);
604    POP(stack);
605    POP(stack);
606    POP(stack);
607    POP(stack);
608 }
609
610
611 void split_cb_search2(
612 float target[],                 /* target vector */
613 float ak[],                     /* LPCs for this subframe */
614 float awk1[],                   /* Weighted LPCs for this subframe */
615 float awk2[],                   /* Weighted LPCs for this subframe */
616 void *par,                      /* Codebook/search parameters*/
617 int   p,                        /* number of LPC coeffs */
618 int   nsf,                      /* number of samples in subframe */
619 float *exc,
620 FrameBits *bits,
621 float *stack
622 )
623 {
624    int i,j, id;
625    float *resp, *E, q;
626    float *t, *r, *e;
627    float *gains;
628    int *ind, *gain_ind;
629    float *shape_cb;
630    int shape_cb_size, subvect_size, nb_subvect;
631    float exc_energy=0;
632    split_cb_params *params;
633
634    params = (split_cb_params *) par;
635    subvect_size = params->subvect_size;
636    nb_subvect = params->nb_subvect;
637    shape_cb_size = 1<<params->shape_bits;
638    shape_cb = params->shape_cb;
639    resp = PUSH(stack, shape_cb_size*subvect_size);
640    E = PUSH(stack, shape_cb_size);
641    t = PUSH(stack, nsf);
642    r = PUSH(stack, nsf);
643    e = PUSH(stack, nsf);
644    gains = PUSH(stack, nb_subvect);
645    ind = (int*)PUSH(stack, nb_subvect);
646    gain_ind = (int*)PUSH(stack, nb_subvect);
647
648    /* Compute energy of the "real excitation" */
649    syn_filt_zero(target, awk1, e, nsf, p);
650    residue_zero(e, ak, e, nsf, p);
651    residue_zero(e, awk2, e, nsf, p);
652    for (i=0;i<nsf;i++)
653       exc_energy += e[i]*e[i];
654    exc_energy=sqrt(exc_energy/nb_subvect);
655
656    /* Quantize global ("average") gain */
657    q=log(exc_energy+.1);
658    q=floor(.5+2*(q-2));
659    if (q<0)
660       q=0;
661    if (q>15)
662       q=15;
663    id = (int)q;
664    speex_bits_pack(bits, id, 4);
665    exc_energy=exp(.5*q+2);
666
667
668    for (i=0;i<nsf;i++)
669       t[i]=target[i];
670
671    e[0]=1;
672    for (i=1;i<nsf;i++)
673       e[i]=0;
674    residue_zero(e, awk1, r, nsf, p);
675    syn_filt_zero(r, ak, r, nsf, p);
676    syn_filt_zero(r, awk2, r, nsf,p);
677    
678    /* Pre-compute codewords response and energy */
679    for (i=0;i<shape_cb_size;i++)
680    {
681       float *res = resp+i*subvect_size;
682
683       /* Compute codeword response */
684       int k;
685       for(j=0;j<subvect_size;j++)
686          res[j]=0;
687       for(j=0;j<subvect_size;j++)
688       {
689          for (k=j;k<subvect_size;k++)
690             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
691       }
692       /* Compute energy of codeword response */
693       E[i]=0;
694       for(j=0;j<subvect_size;j++)
695          E[i]+=res[j]*res[j];
696       E[i]=1/(.001+E[i]);
697    }
698
699    for (i=0;i<nb_subvect;i++)
700    {
701       int best_index[2]={0,0}, k, m, best_gain_ind[2]={0,0};
702       float g, corr, best_gain[2]={0,0}, score, best_score[2]={-1,-1};
703       /* Find best codeword for current sub-vector */
704       for (j=0;j<shape_cb_size;j++)
705       {
706          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
707          score=corr*corr*E[j];
708          g = corr*E[j];
709          if (score>best_score[0])
710          {
711             best_index[1]=best_index[0];
712             best_score[1]=best_score[0];
713             best_gain[1]=best_gain[0];
714
715             best_index[0]=j;
716             best_score[0]=score;
717             best_gain[0]=g;
718          } else if (score>best_score[1]) {
719             best_index[1]=j;
720             best_score[1]=score;
721             best_gain[1]=g;            
722          }
723       }
724       
725       /* Quantize gain */
726       for (k=0;k<2;k++) {
727          int s=0, best_id;
728          best_gain[k] /= .01+exc_energy;
729          if (best_gain[k]<0)
730          {
731             best_gain[k]=-best_gain[k];
732             s=1;
733          }
734
735          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
736          best_id = vq_index(&best_gain[k], scal_gains4, 1, 8);
737
738          best_gain_ind[k]=best_id;
739          best_gain[k]=scal_gains4[best_id];
740          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
741          if (s)
742             best_gain[k]=-best_gain[k];
743          best_gain[k] *= exc_energy;
744       }
745
746
747
748       if (i<nb_subvect-1) {
749          int best_index2=0;
750          float best_score2=-1, best_gain2=0;
751          int nbest;
752          float err[2]={0,0};
753          float *tt=PUSH(stack,nsf);
754          for (nbest=0;nbest<2;nbest++)
755          {
756             for (j=0;j<nsf;j++)
757                tt[j]=t[j];
758             for (j=0;j<subvect_size;j++)
759             {
760                g=best_gain[nbest]*shape_cb[best_index[nbest]*subvect_size+j];
761                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
762                   tt[k] -= g*r[m];
763             }
764             
765
766             for (j=0;j<shape_cb_size;j++)
767             {
768                corr=xcorr(resp+j*subvect_size,tt+subvect_size*(i+1),subvect_size);
769                score=corr*corr*E[j];
770                g = corr*E[j];
771                if (score>best_score2)
772                {
773                   best_index2=j;
774                   best_score2=score;
775                   best_gain2=g;
776                }
777             }
778
779             {
780                int s=0, best_id;
781                best_gain2 /= .01+exc_energy;
782                if (best_gain2<0)
783                {
784                   best_gain2=-best_gain2;
785                   s=1;
786                }
787                best_id = vq_index(&best_gain2, scal_gains4, 1, 8);
788                best_gain2=scal_gains4[best_id];
789                if (s)
790                   best_gain2=-best_gain2;
791                best_gain2 *= exc_energy;
792             }
793
794             for (j=0;j<subvect_size;j++)
795             {
796                g=best_gain2*shape_cb[best_index2*subvect_size+j];
797                for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
798                   tt[k] -= g*r[m];
799             }
800             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
801                err[nbest]-=tt[j]*tt[j];
802             
803             best_score[nbest]=err[nbest];
804          }
805
806          if (best_score[1]>best_score[0])
807          {
808             best_index[0]=best_index[1];
809             best_score[0]=best_score[1];
810             best_gain[0]=best_gain[1];
811             best_gain_ind[0]=best_gain_ind[1];
812          }
813          POP(stack);
814       }
815
816
817       
818
819       ind[i]=best_index[0];
820       gain_ind[i]=best_gain_ind[0];
821       gains[i]=best_gain[0];
822       /* Update target for next subvector */
823       for (j=0;j<subvect_size;j++)
824       {
825          g=best_gain[0]*shape_cb[best_index[0]*subvect_size+j];
826          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
827             t[k] -= g*r[m];
828       }
829    }
830    for (i=0;i<nb_subvect;i++)
831    {
832       speex_bits_pack(bits, ind[i], params->shape_bits);
833       if (gains[i]<0)
834          speex_bits_pack(bits, 1, 1);
835       else
836          speex_bits_pack(bits, 0, 1);
837       speex_bits_pack(bits, gain_ind[i], 3);
838       /*printf ("encode split: %d %d %f\n", i, ind[i], gains[i]);*/
839
840    }
841    /* Put everything back together */
842    for (i=0;i<nb_subvect;i++)
843       for (j=0;j<subvect_size;j++)
844          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
845
846    /* Update excitation */
847    for (j=0;j<nsf;j++)
848       exc[j]+=e[j];
849    
850    /* Update target */
851    residue_zero(e, awk1, r, nsf, p);
852    syn_filt_zero(r, ak, r, nsf, p);
853    syn_filt_zero(r, awk2, r, nsf,p);
854    for (j=0;j<nsf;j++)
855       target[j]-=r[j];
856
857    
858
859
860    POP(stack);
861    POP(stack);
862    POP(stack);
863    POP(stack);
864    POP(stack);
865    POP(stack);
866    POP(stack);
867 }
868
869
870
871
872 void split_cb_unquant(
873 float *exc,
874 void *par,                      /* non-overlapping codebook */
875 int   nsf,                      /* number of samples in subframe */
876 FrameBits *bits,
877 float *stack
878 )
879 {
880    int i,j;
881    int *ind;
882    float *gains;
883    float *sign;
884    float *shape_cb, exc_energy;
885    int shape_cb_size, subvect_size, nb_subvect;
886    split_cb_params *params;
887
888    params = (split_cb_params *) par;
889    subvect_size = params->subvect_size;
890    nb_subvect = params->nb_subvect;
891    shape_cb_size = 1<<params->shape_bits;
892    shape_cb = params->shape_cb;
893    
894    ind = (int*)PUSH(stack, nb_subvect);
895    gains = PUSH(stack, nb_subvect);
896    sign = PUSH(stack, nb_subvect);
897
898    /* Decode global (average) gain */
899    {
900       int id;
901       id = speex_bits_unpack_unsigned(bits, 4);
902       exc_energy=exp(.5*id+2);
903    }
904
905    /* Decode codewords and gains */
906    for (i=0;i<nb_subvect;i++)
907    {
908       int gain_id;
909       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
910       if (speex_bits_unpack_unsigned(bits, 1))
911          sign[i]=-1;
912       else
913          sign[i]=1;
914       
915       gain_id = speex_bits_unpack_unsigned(bits, 3);
916       gains[i]=scal_gains4[gain_id];
917       gains[i] *= sign[i];
918       gains[i] *= exc_energy;
919
920       /*printf ("decode split: %d %d %f\n", i, ind[i], gains[i]);*/
921    }
922
923    /* Compute decoded excitation */
924    for (i=0;i<nb_subvect;i++)
925       for (j=0;j<subvect_size;j++)
926          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
927
928    POP(stack);
929    POP(stack);
930    POP(stack);
931 }
932
933
934
935 void split_cb_nogain_unquant(
936 float *exc,
937 void *par,                      /* non-overlapping codebook */
938 int   nsf,                      /* number of samples in subframe */
939 FrameBits *bits,
940 float *stack
941 )
942 {
943    int i,j;
944    int *ind;
945    float *shape_cb;
946    int shape_cb_size, subvect_size, nb_subvect;
947    split_cb_params *params;
948
949    params = (split_cb_params *) par;
950    subvect_size = params->subvect_size;
951    nb_subvect = params->nb_subvect;
952    shape_cb_size = 1<<params->shape_bits;
953    shape_cb = params->shape_cb;
954    
955    ind = (int*)PUSH(stack, nb_subvect);
956
957    /* Decode codewords and gains */
958    for (i=0;i<nb_subvect;i++)
959       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
960
961    /* Compute decoded excitation */
962    for (i=0;i<nb_subvect;i++)
963       for (j=0;j<subvect_size;j++)
964          exc[subvect_size*i+j]+=shape_cb[ind[i]*subvect_size+j];
965
966    POP(stack);
967 }