Added a comfort noise mode...
[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=PUSH(stack, nsf);
93   h=PUSH(stack, nsf);
94   impulse=PUSH(stack, nsf);
95
96   for(i=0; i<nsf; i++)
97     impulse[i] = 0.0;
98    
99   *gain = 0.0;
100   *index = 0;
101   bscore = 0.0;
102   impulse[0] = 1.0;
103
104   /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
105   residue_zero(impulse, awk1, h, nsf, p);
106   syn_filt_zero(h, ak, h, nsf, p);
107   syn_filt_zero(h, awk2, h, nsf,p);
108   
109   /* Calculate codebook zero-response */
110   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
111   syn_filt_zero(resp,ak,resp,nsf,p);
112   syn_filt_zero(resp,awk2,resp,nsf,p);
113     
114   /* Search codebook backwards using end correction for synthesis */
115   
116   for(k=entries-1; k>=0; k--) {
117
118     d = 0.0; e = 0.0;
119     for(i=0; i<nsf; i++) {
120       d += target[i]*resp[i];
121       e += resp[i]*resp[i];
122     }
123     g = d/(e+.0001);
124     score = g*d;
125     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
126     if (score >= bscore) {
127       bscore = score;
128       *gain = g;
129       *index = k;
130     }
131     
132     /* Synthesise next entry */
133     
134     if (k) {
135       for(i=nsf-1; i>=1; i--)
136         resp[i] = resp[i-1] + codebook[k-1]*h[i];
137       resp[0] = codebook[k-1]*h[0];
138     }
139   }
140
141   POP(stack);
142   POP(stack);
143   POP(stack);
144   return bscore;
145 }
146
147
148
149 void split_cb_search(
150 float target[],                 /* target vector */
151 float ak[],                     /* LPCs for this subframe */
152 float awk1[],                   /* Weighted LPCs for this subframe */
153 float awk2[],                   /* Weighted LPCs for this subframe */
154 void *par,                      /* Codebook/search parameters*/
155 int   p,                        /* number of LPC coeffs */
156 int   nsf,                      /* number of samples in subframe */
157 float *exc,
158 SpeexBits *bits,
159 float *stack
160 )
161 {
162    int i,j, id;
163    float *resp, *E, q;
164    float *t, *r, *e;
165    float *gains;
166    int *ind;
167    float *shape_cb;
168    int shape_cb_size, subvect_size, nb_subvect;
169    float exc_energy=0;
170    split_cb_params *params;
171
172    params = (split_cb_params *) par;
173    subvect_size = params->subvect_size;
174    nb_subvect = params->nb_subvect;
175    shape_cb_size = 1<<params->shape_bits;
176    shape_cb = params->shape_cb;
177    resp = PUSH(stack, shape_cb_size*subvect_size);
178    E = PUSH(stack, shape_cb_size);
179    t = PUSH(stack, nsf);
180    r = PUSH(stack, nsf);
181    e = PUSH(stack, nsf);
182    gains = PUSH(stack, nb_subvect);
183    ind = (int*)PUSH(stack, nb_subvect);
184
185    /* Compute energy of the "real excitation" */
186    syn_filt_zero(target, awk1, e, nsf, p);
187    residue_zero(e, ak, e, nsf, p);
188    residue_zero(e, awk2, e, nsf, p);
189    for (i=0;i<nsf;i++)
190       exc_energy += e[i]*e[i];
191    exc_energy=sqrt(exc_energy/nb_subvect);
192
193    /* Quantize global ("average") gain */
194    q=log(exc_energy+.1);
195    q=floor(.5+2*(q-2));
196    if (q<0)
197       q=0;
198    if (q>15)
199       q=15;
200    id = (int)q;
201    speex_bits_pack(bits, id, 4);
202    exc_energy=exp(.5*q+2);
203
204
205    for (i=0;i<nsf;i++)
206       t[i]=target[i];
207
208    e[0]=1;
209    for (i=1;i<nsf;i++)
210       e[i]=0;
211    residue_zero(e, awk1, r, nsf, p);
212    syn_filt_zero(r, ak, r, nsf, p);
213    syn_filt_zero(r, awk2, r, nsf,p);
214    
215    /* Pre-compute codewords response and energy */
216    for (i=0;i<shape_cb_size;i++)
217    {
218       float *res = resp+i*subvect_size;
219
220       /* Compute codeword response */
221       int k;
222       for(j=0;j<subvect_size;j++)
223          res[j]=0;
224       for(j=0;j<subvect_size;j++)
225       {
226          for (k=j;k<subvect_size;k++)
227             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
228       }
229       /* Compute energy of codeword response */
230       E[i]=0;
231       for(j=0;j<subvect_size;j++)
232          E[i]+=res[j]*res[j];
233       E[i]=1/(.001+E[i]);
234    }
235
236    for (i=0;i<nb_subvect;i++)
237    {
238       int best_index=0, k, m;
239       float g, corr, best_gain=0, score, best_score=-1;
240       /* Find best codeword for current sub-vector */
241       for (j=0;j<shape_cb_size;j++)
242       {
243          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
244          score=corr*corr*E[j];
245          g = corr*E[j];
246          if (score>best_score)
247          {
248             best_index=j;
249             best_score=score;
250             best_gain=g;
251          }
252       }
253       speex_bits_pack(bits,best_index,params->shape_bits);
254       
255       /* Quantize gain */
256       {
257          int s=0, best_id;
258          best_gain /= .01+exc_energy;
259          if (best_gain<0)
260          {
261             best_gain=-best_gain;
262             s=1;
263          }
264
265          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
266          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
267
268          best_gain=scal_gains4[best_id];
269          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
270          if (s)
271             best_gain=-best_gain;
272          best_gain *= exc_energy;
273          speex_bits_pack(bits,s,1);
274          speex_bits_pack(bits,best_id,3);
275       }
276       ind[i]=best_index;
277       gains[i]=best_gain;
278       /* Update target for next subvector */
279       for (j=0;j<subvect_size;j++)
280       {
281          g=best_gain*shape_cb[best_index*subvect_size+j];
282          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
283             t[k] -= g*r[m];
284       }
285    }
286    
287    /* Put everything back together */
288    for (i=0;i<nb_subvect;i++)
289       for (j=0;j<subvect_size;j++)
290          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
291
292    /* Update excitation */
293    for (j=0;j<nsf;j++)
294       exc[j]+=e[j];
295    
296    /* Update target */
297    residue_zero(e, awk1, r, nsf, p);
298    syn_filt_zero(r, ak, r, nsf, p);
299    syn_filt_zero(r, awk2, r, nsf,p);
300    for (j=0;j<nsf;j++)
301       target[j]-=r[j];
302
303    
304
305
306    POP(stack);
307    POP(stack);
308    POP(stack);
309    POP(stack);
310    POP(stack);
311    POP(stack);
312    POP(stack);
313 }
314
315 void split_cb_search_nogain(
316 float target[],                 /* target vector */
317 float ak[],                     /* LPCs for this subframe */
318 float awk1[],                   /* Weighted LPCs for this subframe */
319 float awk2[],                   /* Weighted LPCs for this subframe */
320 void *par,                      /* Codebook/search parameters*/
321 int   p,                        /* number of LPC coeffs */
322 int   nsf,                      /* number of samples in subframe */
323 float *exc,
324 SpeexBits *bits,
325 float *stack
326 )
327 {
328    int i,j;
329    float *resp;
330    float *t, *r, *e;
331    int *ind;
332    float *shape_cb;
333    int shape_cb_size, subvect_size, nb_subvect;
334    split_cb_params *params;
335
336    params = (split_cb_params *) par;
337    subvect_size = params->subvect_size;
338    nb_subvect = params->nb_subvect;
339    shape_cb_size = 1<<params->shape_bits;
340    shape_cb = params->shape_cb;
341    resp = PUSH(stack, shape_cb_size*subvect_size);
342    t = PUSH(stack, nsf);
343    r = PUSH(stack, nsf);
344    e = PUSH(stack, nsf);
345    ind = (int*)PUSH(stack, nb_subvect);
346
347    for (i=0;i<nsf;i++)
348       t[i]=target[i];
349
350    e[0]=1;
351    for (i=1;i<nsf;i++)
352       e[i]=0;
353    residue_zero(e, awk1, r, nsf, p);
354    syn_filt_zero(r, ak, r, nsf, p);
355    syn_filt_zero(r, awk2, r, nsf,p);
356    
357    /* Pre-compute codewords response and energy */
358    for (i=0;i<shape_cb_size;i++)
359    {
360       float *res = resp+i*subvect_size;
361
362       /* Compute codeword response */
363       int k;
364       for(j=0;j<subvect_size;j++)
365          res[j]=0;
366       for(j=0;j<subvect_size;j++)
367       {
368          for (k=j;k<subvect_size;k++)
369             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
370       }
371    }
372
373    for (i=0;i<nb_subvect;i++)
374    {
375       int best_index=0, k, m;
376       float g, dist, best_dist=-1;
377       float *a, *b;
378
379       /* Find best codeword for current sub-vector */
380       for (j=0;j<shape_cb_size;j++)
381       {
382          dist=0;
383          a=resp+j*subvect_size;
384          b=t+subvect_size*i;
385          for (k=0;k<subvect_size;k++)
386             dist += (a[k]-b[k])*(a[k]-b[k]);
387          if (dist<best_dist || j==0)
388          {
389             best_dist=dist;
390             best_index=j;
391          }
392       }
393       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
394       speex_bits_pack(bits,best_index,params->shape_bits);
395
396       ind[i]=best_index;
397       /* Update target for next subvector */
398       for (j=0;j<subvect_size;j++)
399       {
400          g=shape_cb[best_index*subvect_size+j];
401          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
402             t[k] -= g*r[m];
403       }
404
405    }
406    
407    /* Put everything back together */
408    for (i=0;i<nb_subvect;i++)
409       for (j=0;j<subvect_size;j++)
410          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
411
412    /* Update excitation */
413    for (j=0;j<nsf;j++)
414       exc[j]+=e[j];
415    
416    /* Update target */
417    residue_zero(e, awk1, r, nsf, p);
418    syn_filt_zero(r, ak, r, nsf, p);
419    syn_filt_zero(r, awk2, r, nsf,p);
420    for (j=0;j<nsf;j++)
421       target[j]-=r[j];
422
423    
424
425    POP(stack);
426    POP(stack);
427    POP(stack);
428    POP(stack);
429    POP(stack);
430 }
431
432
433 void split_cb_search_nogain2(
434 float target[],                 /* target vector */
435 float ak[],                     /* LPCs for this subframe */
436 float awk1[],                   /* Weighted LPCs for this subframe */
437 float awk2[],                   /* Weighted LPCs for this subframe */
438 void *par,                      /* Codebook/search parameters*/
439 int   p,                        /* number of LPC coeffs */
440 int   nsf,                      /* number of samples in subframe */
441 float *exc,
442 SpeexBits *bits,
443 float *stack
444 )
445 {
446    int i,j;
447    float *resp;
448    float *t, *r, *e, *E;
449    int *ind;
450    float *shape_cb;
451    int shape_cb_size, subvect_size, nb_subvect;
452    split_cb_params *params;
453
454    params = (split_cb_params *) par;
455    subvect_size = params->subvect_size;
456    nb_subvect = params->nb_subvect;
457    shape_cb_size = 1<<params->shape_bits;
458    shape_cb = params->shape_cb;
459    resp = PUSH(stack, shape_cb_size*subvect_size);
460    t = PUSH(stack, nsf);
461    r = PUSH(stack, nsf);
462    e = PUSH(stack, nsf);
463    E = PUSH(stack, shape_cb_size);
464    ind = (int*)PUSH(stack, nb_subvect);
465
466    for (i=0;i<nsf;i++)
467       t[i]=target[i];
468
469    e[0]=1;
470    for (i=1;i<nsf;i++)
471       e[i]=0;
472    residue_zero(e, awk1, r, nsf, p);
473    syn_filt_zero(r, ak, r, nsf, p);
474    syn_filt_zero(r, awk2, r, nsf,p);
475    
476    /* Pre-compute codewords response and energy */
477    for (i=0;i<shape_cb_size;i++)
478    {
479       float *res = resp+i*subvect_size;
480
481       /* Compute codeword response */
482       int k;
483       for(j=0;j<subvect_size;j++)
484          res[j]=0;
485       for(j=0;j<subvect_size;j++)
486       {
487          for (k=j;k<subvect_size;k++)
488             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
489       }
490       E[i]=0;
491       for(j=0;j<subvect_size;j++)
492          E[i]+=res[j]*res[j];
493    }
494
495    for (i=0;i<nb_subvect;i++)
496    {
497       int best_index[2]={0,0}, k, m;
498       float g, dist, best_dist[2]={-1,-1};
499       float *a, *x;
500       float energy=0;
501       x=t+subvect_size*i;
502
503 #ifdef TRAINING
504       syn_filt_zero(x, awk1, e, nsf, p);
505       residue_zero(e, ak, e, nsf, p);
506       residue_zero(e, awk2, e, nsf,p);
507 #endif
508
509       for (k=0;k<subvect_size;k++)
510          energy+=x[k]*x[k];
511       /* Find best codeword for current sub-vector */
512       for (j=0;j<shape_cb_size;j++)
513       {
514          dist=0;
515          a=resp+j*subvect_size;
516          dist=energy+E[j];
517          for (k=0;k<subvect_size;k++)
518             dist -= 2*a[k]*x[k];
519          if (dist<best_dist[0] || best_dist[0]<0)
520          {
521             best_dist[1]=best_dist[0];
522             best_index[1]=best_index[0];
523             best_dist[0]=dist;
524             best_index[0]=j;
525          } else if (dist<best_dist[1] || best_dist[1]<0)
526          {
527             best_dist[1]=dist;
528             best_index[1]=j;
529          }
530       }
531       if (i<nb_subvect-1)
532       {
533          int nbest;
534          float *tt, err[2];
535          float best_score[2];
536          tt=PUSH(stack,nsf);
537          for (nbest=0;nbest<2;nbest++)
538          {
539             for (j=0;j<nsf;j++)
540                tt[j]=t[j];
541             for (j=0;j<subvect_size;j++)
542             {
543                g=shape_cb[best_index[nbest]*subvect_size+j];
544                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
545                   tt[k] -= g*r[m];
546             }
547             
548             {
549                int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);
550                for (j=0;j<subvect_size;j++)
551                {
552                   g=shape_cb[i2*subvect_size+j];
553                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
554                      tt[k] -= g*r[m];
555                }
556             }
557
558             err[nbest]=0;
559             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
560                err[nbest]-=tt[j]*tt[j];
561             
562             best_score[nbest]=err[nbest];
563          }
564
565          if (best_score[1]>best_score[0])
566          {
567             best_index[0]=best_index[1];
568             best_score[0]=best_score[1];
569          }
570          POP(stack);
571
572       }
573
574 #ifdef TRAINING
575       accum_cb(e, best_index[0], r, subvect_size);
576 #endif
577       ind[i]=best_index[0];
578
579       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
580       speex_bits_pack(bits,ind[i],params->shape_bits);
581
582       /* Update target for next subvector */
583       for (j=0;j<subvect_size;j++)
584       {
585          g=shape_cb[ind[i]*subvect_size+j];
586          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
587             t[k] -= g*r[m];
588       }
589    }
590    
591    /* Put everything back together */
592    for (i=0;i<nb_subvect;i++)
593       for (j=0;j<subvect_size;j++)
594          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
595
596    /* Update excitation */
597    for (j=0;j<nsf;j++)
598       exc[j]+=e[j];
599    
600    /* Update target */
601    residue_zero(e, awk1, r, nsf, p);
602    syn_filt_zero(r, ak, r, nsf, p);
603    syn_filt_zero(r, awk2, r, nsf,p);
604    for (j=0;j<nsf;j++)
605       target[j]-=r[j];
606
607    
608    POP(stack);
609    POP(stack);
610    POP(stack);
611    POP(stack);
612    POP(stack);
613    POP(stack);
614 }
615
616 void split_cb_search_shape_sign(
617 float target[],                 /* target vector */
618 float ak[],                     /* LPCs for this subframe */
619 float awk1[],                   /* Weighted LPCs for this subframe */
620 float awk2[],                   /* Weighted LPCs for this subframe */
621 void *par,                      /* Codebook/search parameters*/
622 int   p,                        /* number of LPC coeffs */
623 int   nsf,                      /* number of samples in subframe */
624 float *exc,
625 SpeexBits *bits,
626 float *stack
627 )
628 {
629    int i,j;
630    float *resp;
631    float *t, *r, *e, *E;
632    int *ind, *signs;
633    float *shape_cb;
634    int shape_cb_size, subvect_size, nb_subvect;
635    split_cb_params *params;
636
637    params = (split_cb_params *) par;
638    subvect_size = params->subvect_size;
639    nb_subvect = params->nb_subvect;
640    shape_cb_size = 1<<params->shape_bits;
641    shape_cb = params->shape_cb;
642    resp = PUSH(stack, shape_cb_size*subvect_size);
643    t = PUSH(stack, nsf);
644    r = PUSH(stack, nsf);
645    e = PUSH(stack, nsf);
646    E = PUSH(stack, shape_cb_size);
647    ind = (int*)PUSH(stack, nb_subvect);
648    signs = (int*)PUSH(stack, nb_subvect);
649
650    for (i=0;i<nsf;i++)
651       t[i]=target[i];
652
653    e[0]=1;
654    for (i=1;i<nsf;i++)
655       e[i]=0;
656    residue_zero(e, awk1, r, nsf, p);
657    syn_filt_zero(r, ak, r, nsf, p);
658    syn_filt_zero(r, awk2, r, nsf,p);
659    
660    /* Pre-compute codewords response and energy */
661    for (i=0;i<shape_cb_size;i++)
662    {
663       float *res = resp+i*subvect_size;
664
665       /* Compute codeword response */
666       int k;
667       for(j=0;j<subvect_size;j++)
668          res[j]=0;
669       for(j=0;j<subvect_size;j++)
670       {
671          for (k=j;k<subvect_size;k++)
672             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
673       }
674       E[i]=0;
675       for(j=0;j<subvect_size;j++)
676          E[i]+=res[j]*res[j];
677    }
678
679    for (i=0;i<nb_subvect;i++)
680    {
681       int best_index[2]={0,0}, k, m;
682       float g, dist, best_dist[2]={-1,-1}, best_sign[2]={0,0};
683       float *a, *x;
684       float energy=0;
685       x=t+subvect_size*i;
686
687       for (k=0;k<subvect_size;k++)
688          energy+=x[k]*x[k];
689       /* Find best codeword for current sub-vector */
690       for (j=0;j<shape_cb_size;j++)
691       {
692          int sign;
693          dist=0;
694          a=resp+j*subvect_size;
695          dist=0;
696          for (k=0;k<subvect_size;k++)
697             dist -= 2*a[k]*x[k];
698          if (dist > 0)
699          {
700             sign=1;
701             dist =- dist;
702          } else
703             sign=0;
704          dist += energy+E[j];
705          if (dist<best_dist[0] || best_dist[0]<0)
706          {
707             best_dist[1]=best_dist[0];
708             best_index[1]=best_index[0];
709             best_sign[1]=best_sign[0];
710             best_dist[0]=dist;
711             best_index[0]=j;
712             best_sign[0]=sign;
713          } else if (dist<best_dist[1] || best_dist[1]<0)
714          {
715             best_dist[1]=dist;
716             best_index[1]=j;
717             best_sign[1]=sign;
718          }
719       }
720       if (i<nb_subvect-1)
721       {
722          int nbest;
723          float *tt, err[2];
724          float best_score[2];
725          tt=PUSH(stack,nsf);
726          for (nbest=0;nbest<2;nbest++)
727          {
728             float s=1;
729             if (best_sign[nbest])
730                s=-1;
731             for (j=0;j<nsf;j++)
732                tt[j]=t[j];
733             for (j=0;j<subvect_size;j++)
734             {
735                g=s*shape_cb[best_index[nbest]*subvect_size+j];
736                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
737                   tt[k] -= g*r[m];
738             }
739             
740             {
741                int best_index2=0, best_sign2=0, sign2;
742                float  best_dist2=0;
743                x=t+subvect_size*(i+1);
744                for (j=0;j<shape_cb_size;j++)
745                {
746                   a=resp+j*subvect_size;
747                   dist = 0;
748                   for (k=0;k<subvect_size;k++)
749                      dist -= 2*a[k]*x[k];
750                   if (dist > 0)
751                   {
752                      sign2=1;
753                      dist =- dist;
754                   } else
755                      sign2=0;
756                   dist += energy+E[j];
757                   if (dist<best_dist2 || j==0)
758                   {
759                      best_dist2=dist;
760                      best_index2=j;
761                      best_sign2=sign2;
762                   }
763                }
764                s=1;
765                if (best_sign2)
766                   s=-1;
767                /*int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);*/
768                
769                for (j=0;j<subvect_size;j++)
770                {
771                   g=s*shape_cb[best_index2*subvect_size+j];
772                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
773                      tt[k] -= g*r[m];
774                }
775             }
776
777             err[nbest]=0;
778             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
779                err[nbest]-=tt[j]*tt[j];
780             
781             best_score[nbest]=err[nbest];
782          }
783
784          if (best_score[1]>best_score[0])
785          {
786             best_sign[0]=best_sign[1];
787             best_index[0]=best_index[1];
788             best_score[0]=best_score[1];
789          }
790          POP(stack);
791
792       }
793
794       ind[i]=best_index[0];
795       signs[i] = best_sign[0];
796
797       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
798       speex_bits_pack(bits,signs[i],1);
799       speex_bits_pack(bits,ind[i],params->shape_bits);
800
801       /* Update target for next subvector */
802       for (j=0;j<subvect_size;j++)
803       {
804          g=shape_cb[ind[i]*subvect_size+j];
805          if (signs[i])
806             g=-g;
807          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
808             t[k] -= g*r[m];
809       }
810    }
811    
812    /* Put everything back together */
813    for (i=0;i<nb_subvect;i++)
814    {
815       float s=1;
816       if (signs[i])
817          s=-1;
818       for (j=0;j<subvect_size;j++)
819          e[subvect_size*i+j]=s*shape_cb[ind[i]*subvect_size+j];
820    }
821    /* Update excitation */
822    for (j=0;j<nsf;j++)
823       exc[j]+=e[j];
824    
825    /* Update target */
826    residue_zero(e, awk1, r, nsf, p);
827    syn_filt_zero(r, ak, r, nsf, p);
828    syn_filt_zero(r, awk2, r, nsf,p);
829    for (j=0;j<nsf;j++)
830       target[j]-=r[j];
831
832    
833    POP(stack);
834    POP(stack);
835    POP(stack);
836    POP(stack);
837    POP(stack);
838    POP(stack);
839    POP(stack);
840 }
841
842
843 void split_cb_search2(
844 float target[],                 /* target vector */
845 float ak[],                     /* LPCs for this subframe */
846 float awk1[],                   /* Weighted LPCs for this subframe */
847 float awk2[],                   /* Weighted LPCs for this subframe */
848 void *par,                      /* Codebook/search parameters*/
849 int   p,                        /* number of LPC coeffs */
850 int   nsf,                      /* number of samples in subframe */
851 float *exc,
852 SpeexBits *bits,
853 float *stack
854 )
855 {
856    int i,j, id;
857    float *resp, *E, q;
858    float *t, *r, *e;
859    float *gains;
860    int *ind, *gain_ind;
861    float *shape_cb;
862    int shape_cb_size, subvect_size, nb_subvect;
863    float exc_energy=0;
864    split_cb_params *params;
865
866    params = (split_cb_params *) par;
867    subvect_size = params->subvect_size;
868    nb_subvect = params->nb_subvect;
869    shape_cb_size = 1<<params->shape_bits;
870    shape_cb = params->shape_cb;
871    resp = PUSH(stack, shape_cb_size*subvect_size);
872    E = PUSH(stack, shape_cb_size);
873    t = PUSH(stack, nsf);
874    r = PUSH(stack, nsf);
875    e = PUSH(stack, nsf);
876    gains = PUSH(stack, nb_subvect);
877    ind = (int*)PUSH(stack, nb_subvect);
878    gain_ind = (int*)PUSH(stack, nb_subvect);
879
880    /* Compute energy of the "real excitation" */
881    syn_filt_zero(target, awk1, e, nsf, p);
882    residue_zero(e, ak, e, nsf, p);
883    residue_zero(e, awk2, e, nsf, p);
884    for (i=0;i<nsf;i++)
885       exc_energy += e[i]*e[i];
886    exc_energy=sqrt(exc_energy/nb_subvect);
887
888    /* Quantize global ("average") gain */
889    q=log(exc_energy+.1);
890    q=floor(.5+2*(q-2));
891    if (q<0)
892       q=0;
893    if (q>15)
894       q=15;
895    id = (int)q;
896    speex_bits_pack(bits, id, 4);
897    exc_energy=exp(.5*q+2);
898
899
900    for (i=0;i<nsf;i++)
901       t[i]=target[i];
902
903    e[0]=1;
904    for (i=1;i<nsf;i++)
905       e[i]=0;
906    residue_zero(e, awk1, r, nsf, p);
907    syn_filt_zero(r, ak, r, nsf, p);
908    syn_filt_zero(r, awk2, r, nsf,p);
909    
910    /* Pre-compute codewords response and energy */
911    for (i=0;i<shape_cb_size;i++)
912    {
913       float *res = resp+i*subvect_size;
914
915       /* Compute codeword response */
916       int k;
917       for(j=0;j<subvect_size;j++)
918          res[j]=0;
919       for(j=0;j<subvect_size;j++)
920       {
921          for (k=j;k<subvect_size;k++)
922             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
923       }
924       /* Compute energy of codeword response */
925       E[i]=0;
926       for(j=0;j<subvect_size;j++)
927          E[i]+=res[j]*res[j];
928       E[i]=1/(.001+E[i]);
929    }
930
931    for (i=0;i<nb_subvect;i++)
932    {
933       int best_index[2]={0,0}, k, m, best_gain_ind[2]={0,0};
934       float g, corr, best_gain[2]={0,0}, score, best_score[2]={-1,-1};
935       /* Find best codeword for current sub-vector */
936       for (j=0;j<shape_cb_size;j++)
937       {
938          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
939          score=corr*corr*E[j];
940          g = corr*E[j];
941          if (score>best_score[0])
942          {
943             best_index[1]=best_index[0];
944             best_score[1]=best_score[0];
945             best_gain[1]=best_gain[0];
946
947             best_index[0]=j;
948             best_score[0]=score;
949             best_gain[0]=g;
950          } else if (score>best_score[1]) {
951             best_index[1]=j;
952             best_score[1]=score;
953             best_gain[1]=g;            
954          }
955       }
956       
957       /* Quantize gain */
958       for (k=0;k<2;k++) {
959          int s=0, best_id;
960          best_gain[k] /= .01+exc_energy;
961          if (best_gain[k]<0)
962          {
963             best_gain[k]=-best_gain[k];
964             s=1;
965          }
966
967          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
968          best_id = vq_index(&best_gain[k], scal_gains4, 1, 8);
969
970          best_gain_ind[k]=best_id;
971          best_gain[k]=scal_gains4[best_id];
972          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
973          if (s)
974             best_gain[k]=-best_gain[k];
975          best_gain[k] *= exc_energy;
976       }
977
978
979
980       if (i<nb_subvect-1) {
981          int best_index2=0;
982          float best_score2=-1, best_gain2=0;
983          int nbest;
984          float err[2]={0,0};
985          float *tt=PUSH(stack,nsf);
986          for (nbest=0;nbest<2;nbest++)
987          {
988             for (j=0;j<nsf;j++)
989                tt[j]=t[j];
990             for (j=0;j<subvect_size;j++)
991             {
992                g=best_gain[nbest]*shape_cb[best_index[nbest]*subvect_size+j];
993                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
994                   tt[k] -= g*r[m];
995             }
996             
997
998             for (j=0;j<shape_cb_size;j++)
999             {
1000                corr=xcorr(resp+j*subvect_size,tt+subvect_size*(i+1),subvect_size);
1001                score=corr*corr*E[j];
1002                g = corr*E[j];
1003                if (score>best_score2)
1004                {
1005                   best_index2=j;
1006                   best_score2=score;
1007                   best_gain2=g;
1008                }
1009             }
1010
1011             {
1012                int s=0, best_id;
1013                best_gain2 /= .01+exc_energy;
1014                if (best_gain2<0)
1015                {
1016                   best_gain2=-best_gain2;
1017                   s=1;
1018                }
1019                best_id = vq_index(&best_gain2, scal_gains4, 1, 8);
1020                best_gain2=scal_gains4[best_id];
1021                if (s)
1022                   best_gain2=-best_gain2;
1023                best_gain2 *= exc_energy;
1024             }
1025
1026             for (j=0;j<subvect_size;j++)
1027             {
1028                g=best_gain2*shape_cb[best_index2*subvect_size+j];
1029                for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
1030                   tt[k] -= g*r[m];
1031             }
1032             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
1033                err[nbest]-=tt[j]*tt[j];
1034             
1035             best_score[nbest]=err[nbest];
1036          }
1037
1038          if (best_score[1]>best_score[0])
1039          {
1040             best_index[0]=best_index[1];
1041             best_score[0]=best_score[1];
1042             best_gain[0]=best_gain[1];
1043             best_gain_ind[0]=best_gain_ind[1];
1044          }
1045          POP(stack);
1046       }
1047
1048
1049       
1050
1051       ind[i]=best_index[0];
1052       gain_ind[i]=best_gain_ind[0];
1053       gains[i]=best_gain[0];
1054       /* Update target for next subvector */
1055       for (j=0;j<subvect_size;j++)
1056       {
1057          g=best_gain[0]*shape_cb[best_index[0]*subvect_size+j];
1058          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
1059             t[k] -= g*r[m];
1060       }
1061    }
1062    for (i=0;i<nb_subvect;i++)
1063    {
1064       speex_bits_pack(bits, ind[i], params->shape_bits);
1065       if (gains[i]<0)
1066          speex_bits_pack(bits, 1, 1);
1067       else
1068          speex_bits_pack(bits, 0, 1);
1069       speex_bits_pack(bits, gain_ind[i], 3);
1070       /*printf ("encode split: %d %d %f\n", i, ind[i], gains[i]);*/
1071
1072    }
1073    /* Put everything back together */
1074    for (i=0;i<nb_subvect;i++)
1075       for (j=0;j<subvect_size;j++)
1076          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
1077
1078    /* Update excitation */
1079    for (j=0;j<nsf;j++)
1080       exc[j]+=e[j];
1081    
1082    /* Update target */
1083    residue_zero(e, awk1, r, nsf, p);
1084    syn_filt_zero(r, ak, r, nsf, p);
1085    syn_filt_zero(r, awk2, r, nsf,p);
1086    for (j=0;j<nsf;j++)
1087       target[j]-=r[j];
1088
1089    
1090
1091    POP(stack);
1092    POP(stack);
1093    POP(stack);
1094    POP(stack);
1095    POP(stack);
1096    POP(stack);
1097    POP(stack);
1098    POP(stack);
1099 }
1100
1101
1102
1103
1104 void split_cb_unquant(
1105 float *exc,
1106 void *par,                      /* non-overlapping codebook */
1107 int   nsf,                      /* number of samples in subframe */
1108 SpeexBits *bits,
1109 float *stack
1110 )
1111 {
1112    int i,j;
1113    int *ind;
1114    float *gains;
1115    float *sign;
1116    float *shape_cb, exc_energy;
1117    int shape_cb_size, subvect_size, nb_subvect;
1118    split_cb_params *params;
1119
1120    params = (split_cb_params *) par;
1121    subvect_size = params->subvect_size;
1122    nb_subvect = params->nb_subvect;
1123    shape_cb_size = 1<<params->shape_bits;
1124    shape_cb = params->shape_cb;
1125    
1126    ind = (int*)PUSH(stack, nb_subvect);
1127    gains = PUSH(stack, nb_subvect);
1128    sign = PUSH(stack, nb_subvect);
1129
1130    /* Decode global (average) gain */
1131    {
1132       int id;
1133       id = speex_bits_unpack_unsigned(bits, 4);
1134       exc_energy=exp(.5*id+2);
1135    }
1136
1137    /* Decode codewords and gains */
1138    for (i=0;i<nb_subvect;i++)
1139    {
1140       int gain_id;
1141       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1142       if (speex_bits_unpack_unsigned(bits, 1))
1143          sign[i]=-1;
1144       else
1145          sign[i]=1;
1146       
1147       gain_id = speex_bits_unpack_unsigned(bits, 3);
1148       gains[i]=scal_gains4[gain_id];
1149       gains[i] *= sign[i];
1150       gains[i] *= exc_energy;
1151
1152       /*printf ("decode split: %d %d %f\n", i, ind[i], gains[i]);*/
1153    }
1154
1155    /* Compute decoded excitation */
1156    for (i=0;i<nb_subvect;i++)
1157       for (j=0;j<subvect_size;j++)
1158          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
1159
1160    POP(stack);
1161    POP(stack);
1162    POP(stack);
1163 }
1164
1165
1166
1167 void split_cb_nogain_unquant(
1168 float *exc,
1169 void *par,                      /* non-overlapping codebook */
1170 int   nsf,                      /* number of samples in subframe */
1171 SpeexBits *bits,
1172 float *stack
1173 )
1174 {
1175    int i,j;
1176    int *ind;
1177    float *shape_cb;
1178    int shape_cb_size, subvect_size, nb_subvect;
1179    split_cb_params *params;
1180
1181    params = (split_cb_params *) par;
1182    subvect_size = params->subvect_size;
1183    nb_subvect = params->nb_subvect;
1184    shape_cb_size = 1<<params->shape_bits;
1185    shape_cb = params->shape_cb;
1186    
1187    ind = (int*)PUSH(stack, nb_subvect);
1188
1189    /* Decode codewords and gains */
1190    for (i=0;i<nb_subvect;i++)
1191       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1192
1193    /* Compute decoded excitation */
1194    for (i=0;i<nb_subvect;i++)
1195       for (j=0;j<subvect_size;j++)
1196          exc[subvect_size*i+j]+=shape_cb[ind[i]*subvect_size+j];
1197
1198    POP(stack);
1199 }
1200
1201 void split_cb_shape_sign_unquant(
1202 float *exc,
1203 void *par,                      /* non-overlapping codebook */
1204 int   nsf,                      /* number of samples in subframe */
1205 SpeexBits *bits,
1206 float *stack
1207 )
1208 {
1209    int i,j;
1210    int *ind, *signs;
1211    float *shape_cb;
1212    int shape_cb_size, subvect_size, nb_subvect;
1213    split_cb_params *params;
1214
1215    params = (split_cb_params *) par;
1216    subvect_size = params->subvect_size;
1217    nb_subvect = params->nb_subvect;
1218    shape_cb_size = 1<<params->shape_bits;
1219    shape_cb = params->shape_cb;
1220    
1221    ind = (int*)PUSH(stack, nb_subvect);
1222    signs = (int*)PUSH(stack, nb_subvect);
1223
1224    /* Decode codewords and gains */
1225    for (i=0;i<nb_subvect;i++)
1226    {
1227       signs[i] = speex_bits_unpack_unsigned(bits, 1);
1228       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1229    }
1230    /* Compute decoded excitation */
1231    for (i=0;i<nb_subvect;i++)
1232    {
1233       float s=1;
1234       if (signs[i])
1235          s=-1;
1236       for (j=0;j<subvect_size;j++)
1237          exc[subvect_size*i+j]+=s*shape_cb[ind[i]*subvect_size+j];
1238    }
1239    POP(stack);
1240    POP(stack);
1241 }