Removed some warnings, fixed posfilter mode for wideband
[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 SpeexBits *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 SpeexBits *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
385       /* Find best codeword for current sub-vector */
386       for (j=0;j<shape_cb_size;j++)
387       {
388          dist=0;
389          a=resp+j*subvect_size;
390          b=t+subvect_size*i;
391          for (k=0;k<subvect_size;k++)
392             dist += (a[k]-b[k])*(a[k]-b[k]);
393          if (dist<best_dist || j==0)
394          {
395             best_dist=dist;
396             best_index=j;
397          }
398       }
399       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
400       speex_bits_pack(bits,best_index,params->shape_bits);
401
402       ind[i]=best_index;
403       /* Update target for next subvector */
404       for (j=0;j<subvect_size;j++)
405       {
406          g=shape_cb[best_index*subvect_size+j];
407          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
408             t[k] -= g*r[m];
409       }
410
411    }
412    
413    /* Put everything back together */
414    for (i=0;i<nb_subvect;i++)
415       for (j=0;j<subvect_size;j++)
416          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
417
418    /* Update excitation */
419    for (j=0;j<nsf;j++)
420       exc[j]+=e[j];
421    
422    /* Update target */
423    residue_zero(e, awk1, r, nsf, p);
424    syn_filt_zero(r, ak, r, nsf, p);
425    syn_filt_zero(r, awk2, r, nsf,p);
426    for (j=0;j<nsf;j++)
427       target[j]-=r[j];
428
429    
430
431
432    POP(stack);
433    POP(stack);
434    POP(stack);
435    POP(stack);
436 }
437
438
439 void split_cb_search_nogain2(
440 float target[],                 /* target vector */
441 float ak[],                     /* LPCs for this subframe */
442 float awk1[],                   /* Weighted LPCs for this subframe */
443 float awk2[],                   /* Weighted LPCs for this subframe */
444 void *par,                      /* Codebook/search parameters*/
445 int   p,                        /* number of LPC coeffs */
446 int   nsf,                      /* number of samples in subframe */
447 float *exc,
448 SpeexBits *bits,
449 float *stack
450 )
451 {
452    int i,j;
453    float *resp;
454    float *t, *r, *e, *E;
455    int *ind;
456    float *shape_cb;
457    int shape_cb_size, subvect_size, nb_subvect;
458    split_cb_params *params;
459
460    params = (split_cb_params *) par;
461    subvect_size = params->subvect_size;
462    nb_subvect = params->nb_subvect;
463    shape_cb_size = 1<<params->shape_bits;
464    shape_cb = params->shape_cb;
465    resp = PUSH(stack, shape_cb_size*subvect_size);
466    t = PUSH(stack, nsf);
467    r = PUSH(stack, nsf);
468    e = PUSH(stack, nsf);
469    E = PUSH(stack, shape_cb_size);
470    ind = (int*)PUSH(stack, nb_subvect);
471
472    for (i=0;i<nsf;i++)
473       t[i]=target[i];
474
475    e[0]=1;
476    for (i=1;i<nsf;i++)
477       e[i]=0;
478    residue_zero(e, awk1, r, nsf, p);
479    syn_filt_zero(r, ak, r, nsf, p);
480    syn_filt_zero(r, awk2, r, nsf,p);
481    
482    /* Pre-compute codewords response and energy */
483    for (i=0;i<shape_cb_size;i++)
484    {
485       float *res = resp+i*subvect_size;
486
487       /* Compute codeword response */
488       int k;
489       for(j=0;j<subvect_size;j++)
490          res[j]=0;
491       for(j=0;j<subvect_size;j++)
492       {
493          for (k=j;k<subvect_size;k++)
494             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
495       }
496       E[i]=0;
497       for(j=0;j<subvect_size;j++)
498          E[i]+=res[j]*res[j];
499    }
500
501    for (i=0;i<nb_subvect;i++)
502    {
503       int best_index[2]={0,0}, k, m;
504       float g, dist, best_dist[2]={-1,-1};
505       float *a, *x;
506       float energy=0;
507       x=t+subvect_size*i;
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       ind[i]=best_index[0];
575
576       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
577       speex_bits_pack(bits,ind[i],params->shape_bits);
578
579       /* Update target for next subvector */
580       for (j=0;j<subvect_size;j++)
581       {
582          g=shape_cb[ind[i]*subvect_size+j];
583          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
584             t[k] -= g*r[m];
585       }
586    }
587    
588    /* Put everything back together */
589    for (i=0;i<nb_subvect;i++)
590       for (j=0;j<subvect_size;j++)
591          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
592
593    /* Update excitation */
594    for (j=0;j<nsf;j++)
595       exc[j]+=e[j];
596    
597    /* Update target */
598    residue_zero(e, awk1, r, nsf, p);
599    syn_filt_zero(r, ak, r, nsf, p);
600    syn_filt_zero(r, awk2, r, nsf,p);
601    for (j=0;j<nsf;j++)
602       target[j]-=r[j];
603
604    
605
606    POP(stack);
607    POP(stack);
608    POP(stack);
609    POP(stack);
610    POP(stack);
611 }
612
613 void split_cb_search_shape_sign(
614 float target[],                 /* target vector */
615 float ak[],                     /* LPCs for this subframe */
616 float awk1[],                   /* Weighted LPCs for this subframe */
617 float awk2[],                   /* Weighted LPCs for this subframe */
618 void *par,                      /* Codebook/search parameters*/
619 int   p,                        /* number of LPC coeffs */
620 int   nsf,                      /* number of samples in subframe */
621 float *exc,
622 SpeexBits *bits,
623 float *stack
624 )
625 {
626    int i,j;
627    float *resp;
628    float *t, *r, *e, *E;
629    int *ind, *signs;
630    float *shape_cb;
631    int shape_cb_size, subvect_size, nb_subvect;
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    t = PUSH(stack, nsf);
641    r = PUSH(stack, nsf);
642    e = PUSH(stack, nsf);
643    E = PUSH(stack, shape_cb_size);
644    ind = (int*)PUSH(stack, nb_subvect);
645    signs = (int*)PUSH(stack, nb_subvect);
646
647    for (i=0;i<nsf;i++)
648       t[i]=target[i];
649
650    e[0]=1;
651    for (i=1;i<nsf;i++)
652       e[i]=0;
653    residue_zero(e, awk1, r, nsf, p);
654    syn_filt_zero(r, ak, r, nsf, p);
655    syn_filt_zero(r, awk2, r, nsf,p);
656    
657    /* Pre-compute codewords response and energy */
658    for (i=0;i<shape_cb_size;i++)
659    {
660       float *res = resp+i*subvect_size;
661
662       /* Compute codeword response */
663       int k;
664       for(j=0;j<subvect_size;j++)
665          res[j]=0;
666       for(j=0;j<subvect_size;j++)
667       {
668          for (k=j;k<subvect_size;k++)
669             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
670       }
671       E[i]=0;
672       for(j=0;j<subvect_size;j++)
673          E[i]+=res[j]*res[j];
674    }
675
676    for (i=0;i<nb_subvect;i++)
677    {
678       int best_index[2]={0,0}, k, m;
679       float g, dist, best_dist[2]={-1,-1}, best_sign[2]={0,0};
680       float *a, *x;
681       float energy=0;
682       x=t+subvect_size*i;
683
684       for (k=0;k<subvect_size;k++)
685          energy+=x[k]*x[k];
686       /* Find best codeword for current sub-vector */
687       for (j=0;j<shape_cb_size;j++)
688       {
689          int sign;
690          dist=0;
691          a=resp+j*subvect_size;
692          dist=0;
693          for (k=0;k<subvect_size;k++)
694             dist -= 2*a[k]*x[k];
695          if (dist > 0)
696          {
697             sign=1;
698             dist =- dist;
699          } else
700             sign=0;
701          dist += energy+E[j];
702          if (dist<best_dist[0] || best_dist[0]<0)
703          {
704             best_dist[1]=best_dist[0];
705             best_index[1]=best_index[0];
706             best_sign[1]=best_sign[0];
707             best_dist[0]=dist;
708             best_index[0]=j;
709             best_sign[0]=sign;
710          } else if (dist<best_dist[1] || best_dist[1]<0)
711          {
712             best_dist[1]=dist;
713             best_index[1]=j;
714             best_sign[1]=sign;
715          }
716       }
717       if (i<nb_subvect-1)
718       {
719          int nbest;
720          float *tt, err[2];
721          float best_score[2];
722          tt=PUSH(stack,nsf);
723          for (nbest=0;nbest<2;nbest++)
724          {
725             float s=1;
726             if (best_sign[nbest])
727                s=-1;
728             for (j=0;j<nsf;j++)
729                tt[j]=t[j];
730             for (j=0;j<subvect_size;j++)
731             {
732                g=s*shape_cb[best_index[nbest]*subvect_size+j];
733                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
734                   tt[k] -= g*r[m];
735             }
736             
737             {
738                int best_index2=0, best_sign2=0, sign2;
739                float  best_dist2=0;
740                x=t+subvect_size*(i+1);
741                for (j=0;j<shape_cb_size;j++)
742                {
743                   a=resp+j*subvect_size;
744                   dist = 0;
745                   for (k=0;k<subvect_size;k++)
746                      dist -= 2*a[k]*x[k];
747                   if (dist > 0)
748                   {
749                      sign2=1;
750                      dist =- dist;
751                   } else
752                      sign2=0;
753                   dist += energy+E[j];
754                   if (dist<best_dist2 || j==0)
755                   {
756                      best_dist2=dist;
757                      best_index2=j;
758                      best_sign2=sign2;
759                   }
760                }
761                s=1;
762                if (best_sign2)
763                   s=-1;
764                /*int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);*/
765                
766                for (j=0;j<subvect_size;j++)
767                {
768                   g=s*shape_cb[best_index2*subvect_size+j];
769                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
770                      tt[k] -= g*r[m];
771                }
772             }
773
774             err[nbest]=0;
775             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
776                err[nbest]-=tt[j]*tt[j];
777             
778             best_score[nbest]=err[nbest];
779          }
780
781          if (best_score[1]>best_score[0])
782          {
783             best_sign[0]=best_sign[1];
784             best_index[0]=best_index[1];
785             best_score[0]=best_score[1];
786          }
787          POP(stack);
788
789       }
790
791       ind[i]=best_index[0];
792       signs[i] = best_sign[0];
793
794       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
795       speex_bits_pack(bits,signs[i],1);
796       speex_bits_pack(bits,ind[i],params->shape_bits);
797
798       /* Update target for next subvector */
799       for (j=0;j<subvect_size;j++)
800       {
801          g=shape_cb[ind[i]*subvect_size+j];
802          if (signs[i])
803             g=-g;
804          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
805             t[k] -= g*r[m];
806       }
807    }
808    
809    /* Put everything back together */
810    for (i=0;i<nb_subvect;i++)
811    {
812       float s=1;
813       if (signs[i])
814          s=-1;
815       for (j=0;j<subvect_size;j++)
816          e[subvect_size*i+j]=s*shape_cb[ind[i]*subvect_size+j];
817    }
818    /* Update excitation */
819    for (j=0;j<nsf;j++)
820       exc[j]+=e[j];
821    
822    /* Update target */
823    residue_zero(e, awk1, r, nsf, p);
824    syn_filt_zero(r, ak, r, nsf, p);
825    syn_filt_zero(r, awk2, r, nsf,p);
826    for (j=0;j<nsf;j++)
827       target[j]-=r[j];
828
829    
830
831    POP(stack);
832    POP(stack);
833    POP(stack);
834    POP(stack);
835    POP(stack);
836    POP(stack);
837 }
838
839
840 void split_cb_search2(
841 float target[],                 /* target vector */
842 float ak[],                     /* LPCs for this subframe */
843 float awk1[],                   /* Weighted LPCs for this subframe */
844 float awk2[],                   /* Weighted LPCs for this subframe */
845 void *par,                      /* Codebook/search parameters*/
846 int   p,                        /* number of LPC coeffs */
847 int   nsf,                      /* number of samples in subframe */
848 float *exc,
849 SpeexBits *bits,
850 float *stack
851 )
852 {
853    int i,j, id;
854    float *resp, *E, q;
855    float *t, *r, *e;
856    float *gains;
857    int *ind, *gain_ind;
858    float *shape_cb;
859    int shape_cb_size, subvect_size, nb_subvect;
860    float exc_energy=0;
861    split_cb_params *params;
862
863    params = (split_cb_params *) par;
864    subvect_size = params->subvect_size;
865    nb_subvect = params->nb_subvect;
866    shape_cb_size = 1<<params->shape_bits;
867    shape_cb = params->shape_cb;
868    resp = PUSH(stack, shape_cb_size*subvect_size);
869    E = PUSH(stack, shape_cb_size);
870    t = PUSH(stack, nsf);
871    r = PUSH(stack, nsf);
872    e = PUSH(stack, nsf);
873    gains = PUSH(stack, nb_subvect);
874    ind = (int*)PUSH(stack, nb_subvect);
875    gain_ind = (int*)PUSH(stack, nb_subvect);
876
877    /* Compute energy of the "real excitation" */
878    syn_filt_zero(target, awk1, e, nsf, p);
879    residue_zero(e, ak, e, nsf, p);
880    residue_zero(e, awk2, e, nsf, p);
881    for (i=0;i<nsf;i++)
882       exc_energy += e[i]*e[i];
883    exc_energy=sqrt(exc_energy/nb_subvect);
884
885    /* Quantize global ("average") gain */
886    q=log(exc_energy+.1);
887    q=floor(.5+2*(q-2));
888    if (q<0)
889       q=0;
890    if (q>15)
891       q=15;
892    id = (int)q;
893    speex_bits_pack(bits, id, 4);
894    exc_energy=exp(.5*q+2);
895
896
897    for (i=0;i<nsf;i++)
898       t[i]=target[i];
899
900    e[0]=1;
901    for (i=1;i<nsf;i++)
902       e[i]=0;
903    residue_zero(e, awk1, r, nsf, p);
904    syn_filt_zero(r, ak, r, nsf, p);
905    syn_filt_zero(r, awk2, r, nsf,p);
906    
907    /* Pre-compute codewords response and energy */
908    for (i=0;i<shape_cb_size;i++)
909    {
910       float *res = resp+i*subvect_size;
911
912       /* Compute codeword response */
913       int k;
914       for(j=0;j<subvect_size;j++)
915          res[j]=0;
916       for(j=0;j<subvect_size;j++)
917       {
918          for (k=j;k<subvect_size;k++)
919             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
920       }
921       /* Compute energy of codeword response */
922       E[i]=0;
923       for(j=0;j<subvect_size;j++)
924          E[i]+=res[j]*res[j];
925       E[i]=1/(.001+E[i]);
926    }
927
928    for (i=0;i<nb_subvect;i++)
929    {
930       int best_index[2]={0,0}, k, m, best_gain_ind[2]={0,0};
931       float g, corr, best_gain[2]={0,0}, score, best_score[2]={-1,-1};
932       /* Find best codeword for current sub-vector */
933       for (j=0;j<shape_cb_size;j++)
934       {
935          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
936          score=corr*corr*E[j];
937          g = corr*E[j];
938          if (score>best_score[0])
939          {
940             best_index[1]=best_index[0];
941             best_score[1]=best_score[0];
942             best_gain[1]=best_gain[0];
943
944             best_index[0]=j;
945             best_score[0]=score;
946             best_gain[0]=g;
947          } else if (score>best_score[1]) {
948             best_index[1]=j;
949             best_score[1]=score;
950             best_gain[1]=g;            
951          }
952       }
953       
954       /* Quantize gain */
955       for (k=0;k<2;k++) {
956          int s=0, best_id;
957          best_gain[k] /= .01+exc_energy;
958          if (best_gain[k]<0)
959          {
960             best_gain[k]=-best_gain[k];
961             s=1;
962          }
963
964          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
965          best_id = vq_index(&best_gain[k], scal_gains4, 1, 8);
966
967          best_gain_ind[k]=best_id;
968          best_gain[k]=scal_gains4[best_id];
969          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
970          if (s)
971             best_gain[k]=-best_gain[k];
972          best_gain[k] *= exc_energy;
973       }
974
975
976
977       if (i<nb_subvect-1) {
978          int best_index2=0;
979          float best_score2=-1, best_gain2=0;
980          int nbest;
981          float err[2]={0,0};
982          float *tt=PUSH(stack,nsf);
983          for (nbest=0;nbest<2;nbest++)
984          {
985             for (j=0;j<nsf;j++)
986                tt[j]=t[j];
987             for (j=0;j<subvect_size;j++)
988             {
989                g=best_gain[nbest]*shape_cb[best_index[nbest]*subvect_size+j];
990                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
991                   tt[k] -= g*r[m];
992             }
993             
994
995             for (j=0;j<shape_cb_size;j++)
996             {
997                corr=xcorr(resp+j*subvect_size,tt+subvect_size*(i+1),subvect_size);
998                score=corr*corr*E[j];
999                g = corr*E[j];
1000                if (score>best_score2)
1001                {
1002                   best_index2=j;
1003                   best_score2=score;
1004                   best_gain2=g;
1005                }
1006             }
1007
1008             {
1009                int s=0, best_id;
1010                best_gain2 /= .01+exc_energy;
1011                if (best_gain2<0)
1012                {
1013                   best_gain2=-best_gain2;
1014                   s=1;
1015                }
1016                best_id = vq_index(&best_gain2, scal_gains4, 1, 8);
1017                best_gain2=scal_gains4[best_id];
1018                if (s)
1019                   best_gain2=-best_gain2;
1020                best_gain2 *= exc_energy;
1021             }
1022
1023             for (j=0;j<subvect_size;j++)
1024             {
1025                g=best_gain2*shape_cb[best_index2*subvect_size+j];
1026                for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
1027                   tt[k] -= g*r[m];
1028             }
1029             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
1030                err[nbest]-=tt[j]*tt[j];
1031             
1032             best_score[nbest]=err[nbest];
1033          }
1034
1035          if (best_score[1]>best_score[0])
1036          {
1037             best_index[0]=best_index[1];
1038             best_score[0]=best_score[1];
1039             best_gain[0]=best_gain[1];
1040             best_gain_ind[0]=best_gain_ind[1];
1041          }
1042          POP(stack);
1043       }
1044
1045
1046       
1047
1048       ind[i]=best_index[0];
1049       gain_ind[i]=best_gain_ind[0];
1050       gains[i]=best_gain[0];
1051       /* Update target for next subvector */
1052       for (j=0;j<subvect_size;j++)
1053       {
1054          g=best_gain[0]*shape_cb[best_index[0]*subvect_size+j];
1055          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
1056             t[k] -= g*r[m];
1057       }
1058    }
1059    for (i=0;i<nb_subvect;i++)
1060    {
1061       speex_bits_pack(bits, ind[i], params->shape_bits);
1062       if (gains[i]<0)
1063          speex_bits_pack(bits, 1, 1);
1064       else
1065          speex_bits_pack(bits, 0, 1);
1066       speex_bits_pack(bits, gain_ind[i], 3);
1067       /*printf ("encode split: %d %d %f\n", i, ind[i], gains[i]);*/
1068
1069    }
1070    /* Put everything back together */
1071    for (i=0;i<nb_subvect;i++)
1072       for (j=0;j<subvect_size;j++)
1073          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
1074
1075    /* Update excitation */
1076    for (j=0;j<nsf;j++)
1077       exc[j]+=e[j];
1078    
1079    /* Update target */
1080    residue_zero(e, awk1, r, nsf, p);
1081    syn_filt_zero(r, ak, r, nsf, p);
1082    syn_filt_zero(r, awk2, r, nsf,p);
1083    for (j=0;j<nsf;j++)
1084       target[j]-=r[j];
1085
1086    
1087
1088
1089    POP(stack);
1090    POP(stack);
1091    POP(stack);
1092    POP(stack);
1093    POP(stack);
1094    POP(stack);
1095    POP(stack);
1096 }
1097
1098
1099
1100
1101 void split_cb_unquant(
1102 float *exc,
1103 void *par,                      /* non-overlapping codebook */
1104 int   nsf,                      /* number of samples in subframe */
1105 SpeexBits *bits,
1106 float *stack
1107 )
1108 {
1109    int i,j;
1110    int *ind;
1111    float *gains;
1112    float *sign;
1113    float *shape_cb, exc_energy;
1114    int shape_cb_size, subvect_size, nb_subvect;
1115    split_cb_params *params;
1116
1117    params = (split_cb_params *) par;
1118    subvect_size = params->subvect_size;
1119    nb_subvect = params->nb_subvect;
1120    shape_cb_size = 1<<params->shape_bits;
1121    shape_cb = params->shape_cb;
1122    
1123    ind = (int*)PUSH(stack, nb_subvect);
1124    gains = PUSH(stack, nb_subvect);
1125    sign = PUSH(stack, nb_subvect);
1126
1127    /* Decode global (average) gain */
1128    {
1129       int id;
1130       id = speex_bits_unpack_unsigned(bits, 4);
1131       exc_energy=exp(.5*id+2);
1132    }
1133
1134    /* Decode codewords and gains */
1135    for (i=0;i<nb_subvect;i++)
1136    {
1137       int gain_id;
1138       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1139       if (speex_bits_unpack_unsigned(bits, 1))
1140          sign[i]=-1;
1141       else
1142          sign[i]=1;
1143       
1144       gain_id = speex_bits_unpack_unsigned(bits, 3);
1145       gains[i]=scal_gains4[gain_id];
1146       gains[i] *= sign[i];
1147       gains[i] *= exc_energy;
1148
1149       /*printf ("decode split: %d %d %f\n", i, ind[i], gains[i]);*/
1150    }
1151
1152    /* Compute decoded excitation */
1153    for (i=0;i<nb_subvect;i++)
1154       for (j=0;j<subvect_size;j++)
1155          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
1156
1157    POP(stack);
1158    POP(stack);
1159    POP(stack);
1160 }
1161
1162
1163
1164 void split_cb_nogain_unquant(
1165 float *exc,
1166 void *par,                      /* non-overlapping codebook */
1167 int   nsf,                      /* number of samples in subframe */
1168 SpeexBits *bits,
1169 float *stack
1170 )
1171 {
1172    int i,j;
1173    int *ind;
1174    float *shape_cb;
1175    int shape_cb_size, subvect_size, nb_subvect;
1176    split_cb_params *params;
1177
1178    params = (split_cb_params *) par;
1179    subvect_size = params->subvect_size;
1180    nb_subvect = params->nb_subvect;
1181    shape_cb_size = 1<<params->shape_bits;
1182    shape_cb = params->shape_cb;
1183    
1184    ind = (int*)PUSH(stack, nb_subvect);
1185
1186    /* Decode codewords and gains */
1187    for (i=0;i<nb_subvect;i++)
1188       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1189
1190    /* Compute decoded excitation */
1191    for (i=0;i<nb_subvect;i++)
1192       for (j=0;j<subvect_size;j++)
1193          exc[subvect_size*i+j]+=shape_cb[ind[i]*subvect_size+j];
1194
1195    POP(stack);
1196 }
1197
1198 void split_cb_shape_sign_unquant(
1199 float *exc,
1200 void *par,                      /* non-overlapping codebook */
1201 int   nsf,                      /* number of samples in subframe */
1202 SpeexBits *bits,
1203 float *stack
1204 )
1205 {
1206    int i,j;
1207    int *ind, *signs;
1208    float *shape_cb;
1209    int shape_cb_size, subvect_size, nb_subvect;
1210    split_cb_params *params;
1211
1212    params = (split_cb_params *) par;
1213    subvect_size = params->subvect_size;
1214    nb_subvect = params->nb_subvect;
1215    shape_cb_size = 1<<params->shape_bits;
1216    shape_cb = params->shape_cb;
1217    
1218    ind = (int*)PUSH(stack, nb_subvect);
1219    signs = (int*)PUSH(stack, nb_subvect);
1220
1221    /* Decode codewords and gains */
1222    for (i=0;i<nb_subvect;i++)
1223    {
1224       signs[i] = speex_bits_unpack_unsigned(bits, 1);
1225       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
1226    }
1227    /* Compute decoded excitation */
1228    for (i=0;i<nb_subvect;i++)
1229    {
1230       float s=1;
1231       if (signs[i])
1232          s=-1;
1233       for (j=0;j<subvect_size;j++)
1234          exc[subvect_size*i+j]+=s*shape_cb[ind[i]*subvect_size+j];
1235    }
1236    POP(stack);
1237    POP(stack);
1238 }