Many, many updates. Better split-VQ search, better handling of pitch for
[speexdsp.git] / libspeex / sb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: sb_celp.c
3
4    This library is free software; you can redistribute it and/or
5    modify it under the terms of the GNU Lesser General Public
6    License as published by the Free Software Foundation; either
7    version 2.1 of the License, or (at your option) any later version.
8    
9    This library is distributed in the hope that it will be useful,
10    but WITHOUT ANY WARRANTY; without even the implied warranty of
11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12    Lesser General Public License for more details.
13    
14    You should have received a copy of the GNU Lesser General Public
15    License along with this library; if not, write to the Free Software
16    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17 */
18
19
20 #include "speex.h"
21 #include "sb_celp.h"
22 #include "stdlib.h"
23 #include "filters.h"
24 #include <math.h>
25 #include "lpc.h"
26 #include "lsp.h"
27 #include <stdio.h>
28 #include "stack_alloc.h"
29 #include "cb_search.h"
30 #include "quant_lsp.h"
31 #include "vq.h"
32
33 #ifndef M_PI
34 #define M_PI           3.14159265358979323846  /* pi */
35 #endif
36
37 extern float stoc[];
38
39 #define sqr(x) ((x)*(x))
40
41 float quant_high_gain[16]={
42    -2.387860,
43    -1.504710,
44    -0.988013,
45    -0.610249,
46    -0.310298,
47    -0.050495,
48    0.188963,
49    0.413744,
50    0.628971,
51    0.840555,
52    1.055630,
53    1.283410,
54    1.544990,
55    1.855790,
56    2.281910,
57    3.002660
58 };
59
60
61 float quant_high_gain2[8] = {
62    -1.51541,
63    -0.70324,
64    -0.17024,
65    0.26748,
66    0.67232,
67    1.08402,
68    1.56110,
69    2.25160,
70 };
71
72
73 #if 0
74 #define QMF_ORDER 32
75 static float h0[32] = {
76    0.0006910579, -0.001403793,
77    -0.001268303, 0.004234195,
78    0.001414246, -0.009458318,
79    -0.0001303859, 0.01798145,
80    -0.004187483, -0.03123862,
81    0.01456844, 0.05294745,
82    -0.03934878, -0.09980243,
83    0.1285579, 0.4664053,
84    0.4664053, 0.1285579,
85    -0.09980243, -0.03934878,
86    0.05294745, 0.01456844,
87    -0.03123862, -0.004187483,
88    0.01798145, -0.0001303859,
89    -0.009458318, 0.001414246,
90    0.004234195, -0.001268303,
91    -0.001403793, 0.0006910579
92 };
93
94 static float h1[32] = {
95    0.0006910579, 0.001403793,
96    -0.001268303, -0.004234195,
97    0.001414246, 0.009458318,
98    -0.0001303859, -0.01798145,
99    -0.004187483, 0.03123862,
100    0.01456844, -0.05294745,
101    -0.03934878, 0.09980243,
102    0.1285579, -0.4664053,
103    0.4664053, -0.1285579,
104    -0.09980243, 0.03934878,
105    0.05294745, -0.01456844,
106    -0.03123862, 0.004187483,
107    0.01798145, 0.0001303859,
108    -0.009458318, -0.001414246,
109    0.004234195, 0.001268303,
110    -0.001403793, -0.0006910579
111 };
112 #else 
113 #define QMF_ORDER 64
114 static float h0[64] = {
115    3.596189e-05, -0.0001123515,
116    -0.0001104587, 0.0002790277,
117    0.0002298438, -0.0005953563,
118    -0.0003823631, 0.00113826,
119    0.0005308539, -0.001986177,
120    -0.0006243724, 0.003235877,
121    0.0005743159, -0.004989147,
122    -0.0002584767, 0.007367171,
123    -0.0004857935, -0.01050689,
124    0.001894714, 0.01459396,
125    -0.004313674, -0.01994365,
126    0.00828756, 0.02716055,
127    -0.01485397, -0.03764973,
128    0.026447, 0.05543245,
129    -0.05095487, -0.09779096,
130    0.1382363, 0.4600981,
131    0.4600981, 0.1382363,
132    -0.09779096, -0.05095487,
133    0.05543245, 0.026447,
134    -0.03764973, -0.01485397,
135    0.02716055, 0.00828756,
136    -0.01994365, -0.004313674,
137    0.01459396, 0.001894714,
138    -0.01050689, -0.0004857935,
139    0.007367171, -0.0002584767,
140    -0.004989147, 0.0005743159,
141    0.003235877, -0.0006243724,
142    -0.001986177, 0.0005308539,
143    0.00113826, -0.0003823631,
144    -0.0005953563, 0.0002298438,
145    0.0002790277, -0.0001104587,
146    -0.0001123515, 3.596189e-05
147 };
148
149 static float h1[64] = {
150    3.596189e-05, 0.0001123515,
151    -0.0001104587, -0.0002790277,
152    0.0002298438, 0.0005953563,
153    -0.0003823631, -0.00113826,
154    0.0005308539, 0.001986177,
155    -0.0006243724, -0.003235877,
156    0.0005743159, 0.004989147,
157    -0.0002584767, -0.007367171,
158    -0.0004857935, 0.01050689,
159    0.001894714, -0.01459396,
160    -0.004313674, 0.01994365,
161    0.00828756, -0.02716055,
162    -0.01485397, 0.03764973,
163    0.026447, -0.05543245,
164    -0.05095487, 0.09779096,
165    0.1382363, -0.4600981,
166    0.4600981, -0.1382363,
167    -0.09779096, 0.05095487,
168    0.05543245, -0.026447,
169    -0.03764973, 0.01485397,
170    0.02716055, -0.00828756,
171    -0.01994365, 0.004313674,
172    0.01459396, -0.001894714,
173    -0.01050689, 0.0004857935,
174    0.007367171, 0.0002584767,
175    -0.004989147, -0.0005743159,
176    0.003235877, 0.0006243724,
177    -0.001986177, -0.0005308539,
178    0.00113826, 0.0003823631,
179    -0.0005953563, -0.0002298438,
180    0.0002790277, 0.0001104587,
181    -0.0001123515, -3.596189e-05
182 };
183 #endif
184
185 void sb_encoder_init(SBEncState *st, SpeexMode *mode)
186 {
187    int i;
188    encoder_init(&st->st_low, mode);
189    st->full_frame_size = 2*st->st_low.frameSize;
190    st->frame_size = st->st_low.frameSize;
191    st->subframeSize = st->st_low.subframeSize;
192    st->nbSubframes = st->st_low.nbSubframes;
193    st->windowSize = mode->windowSize;
194    st->lpcSize=8;
195    st->bufSize=st->st_low.bufSize;
196
197    st->lag_factor = .002;
198    st->lpc_floor = 1.0001;
199    st->gamma1=.9;
200    st->gamma2=.6;
201    st->first=1;
202    st->stack = calloc(10000, sizeof(float));
203
204    st->x0=calloc(st->full_frame_size, sizeof(float));
205    st->x1=calloc(st->full_frame_size, sizeof(float));
206    st->x0d=calloc(st->frame_size, sizeof(float));
207    st->x1d=calloc(st->frame_size, sizeof(float));
208    st->high=calloc(st->full_frame_size, sizeof(float));
209    st->y0=calloc(st->full_frame_size, sizeof(float));
210    st->y1=calloc(st->full_frame_size, sizeof(float));
211
212    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
213    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
214    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
215    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
216
217    st->buf=calloc(st->windowSize, sizeof(float));
218    st->excBuf=calloc(st->bufSize, sizeof(float));
219    st->exc = st->excBuf + st->bufSize - st->windowSize;
220    /*st->exc=st->excBuf+st->frame_size;*/
221
222    st->res=calloc(st->frame_size, sizeof(float));
223    st->sw=calloc(st->frame_size, sizeof(float));
224    st->target=calloc(st->frame_size, sizeof(float));
225    st->window=calloc(st->windowSize, sizeof(float));
226    for (i=0;i<st->windowSize;i++)
227       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
228
229    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
230    for (i=0;i<st->lpcSize+1;i++)
231       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
232
233    st->rc = malloc(st->lpcSize*sizeof(float));
234    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
235    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
236    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
237    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
238    st->lsp = malloc(st->lpcSize*sizeof(float));
239    st->qlsp = malloc(st->lpcSize*sizeof(float));
240    st->old_lsp = malloc(st->lpcSize*sizeof(float));
241    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
242    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
243    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
244    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
245    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
246
247    st->mem_sp = calloc(st->lpcSize, sizeof(float));
248    st->mem_sp2 = calloc(st->lpcSize, sizeof(float));
249    st->mem_sw = calloc(st->lpcSize, sizeof(float));
250
251 }
252
253 void sb_encoder_destroy(SBEncState *st)
254 {
255    encoder_destroy(&st->st_low);
256    free(st->x0);
257    free(st->x0d);
258    free(st->x1);
259    free(st->x1d);
260    free(st->high);
261    free(st->y0);
262    free(st->y1);
263    free(st->h0_mem);
264    free(st->h1_mem);
265    free(st->g0_mem);
266    free(st->g1_mem);
267    
268    free(st->buf);
269    free(st->window);
270    free(st->excBuf);
271    free(st->sw);
272    free(st->res);
273    free(st->target);
274    free(st->lagWindow);
275    free(st->rc);
276    free(st->autocorr);
277    free(st->lpc);
278    free(st->bw_lpc1);
279    free(st->bw_lpc2);
280    free(st->lsp);
281    free(st->qlsp);
282    free(st->old_lsp);
283    free(st->old_qlsp);
284    free(st->interp_lsp);
285    free(st->interp_qlsp);
286    free(st->interp_lpc);
287    free(st->interp_qlpc);
288
289    free(st->mem_sp);
290    free(st->mem_sp2);
291    free(st->mem_sw);
292
293    free(st->stack);
294    
295 }
296
297 extern float hexc_table[];
298 split_cb_params split_cb_high = {
299    8,               /*subvect_size*/
300    5,               /*nb_subvect*/
301    hexc_table,       /*shape_cb*/
302    8,               /*shape_bits*/
303 };
304
305 void sb_encode(SBEncState *st, float *in, FrameBits *bits)
306 {
307    int i, roots, sub;
308    /* Compute the two sub-bands by filtering with h0 and h1*/
309    fir_mem(in, h0, st->x0, st->full_frame_size, QMF_ORDER, st->h0_mem);
310    fir_mem(in, h1, st->x1, st->full_frame_size, QMF_ORDER, st->h1_mem);
311    /* Down-sample x0 and x1 */
312    for (i=0;i<st->frame_size;i++)
313    {
314       st->x0d[i]=st->x0[i<<1];
315       st->x1d[i]=st->x1[i<<1];
316    }
317    /* Encode the narrowband part*/
318    encode(&st->st_low, st->x0d, bits);
319
320    /* High-band buffering / sync with low band */
321    for (i=0;i<st->frame_size;i++)
322    {
323       /*st->excBuf[i]=st->exc[i];*/
324       st->high[i]=st->high[st->frame_size+i];
325       st->high[st->frame_size+i]=st->x1d[i];
326    }
327
328    memmove(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
329
330    /* Start encoding the high-band */
331
332    for (i=0;i<st->windowSize;i++)
333       st->buf[i] = st->high[i] * st->window[i];
334
335    /* Compute auto-correlation */
336    autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
337
338    st->autocorr[0] += 1;        /* prevents NANs */
339    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
340    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
341    for (i=0;i<st->lpcSize+1;i++)
342       st->autocorr[i] *= st->lagWindow[i];
343
344    /* Levinson-Durbin */
345    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
346    st->lpc[0]=1;
347
348    /* LPC to LSPs (x-domain) transform */
349    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
350    if (roots!=st->lpcSize)
351    {
352       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
353       exit(1);
354    }
355
356    /* x-domain to angle domain*/
357    for (i=0;i<st->lpcSize;i++)
358       st->lsp[i] = acos(st->lsp[i]);
359
360    /* LSP quantization */
361    lsp_quant_high(st->lsp, st->qlsp, st->lpcSize, bits);
362    
363    /*printf ("high_lsp:");
364    for (i=0;i<st->lpcSize;i++)
365       printf (" %f", st->lsp[i]);
366       printf ("\n");
367    for (i=0;i<st->lpcSize;i++)
368    st->qlsp[i]=st->lsp[i];*/
369    
370
371    if (st->first)
372    {
373       for (i=0;i<st->lpcSize;i++)
374          st->old_lsp[i] = st->lsp[i];
375       for (i=0;i<st->lpcSize;i++)
376          st->old_qlsp[i] = st->qlsp[i];
377    }
378    
379    for (sub=0;sub<st->nbSubframes;sub++)
380    {
381       float *exc, *sp, *mem, *res, *target, *sw, tmp, filter_ratio;
382       int offset;
383       float rl, rh;
384
385       offset = st->subframeSize*sub;
386       sp=st->high+offset;
387       exc=st->excBuf+offset;
388       res=st->res+offset;
389       target=st->target+offset;
390       sw=st->sw+offset;
391
392       mem=PUSH(st->stack, st->lpcSize);
393       
394       /* LSP interpolation (quantized and unquantized) */
395       tmp = (.5 + sub)/st->nbSubframes;
396       for (i=0;i<st->lpcSize;i++)
397          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
398       for (i=0;i<st->lpcSize;i++)
399          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
400
401       /* Compute interpolated LPCs (quantized and unquantized) */
402       for (i=0;i<st->lpcSize;i++)
403          st->interp_lsp[i] = cos(st->interp_lsp[i]);
404       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
405
406       for (i=0;i<st->lpcSize;i++)
407          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
408       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
409
410       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
411       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
412
413       /* Compute mid-band (4000 for wideband) response of low-band and high-band
414          filters */
415       rl=rh=0;
416       tmp=1;
417       for (i=0;i<=st->lpcSize;i++)
418       {
419          rh += tmp*st->interp_qlpc[i];
420          tmp = -tmp;
421       }
422       rl = st->st_low.pi_gain[sub];
423       rl=1/(fabs(rl)+.001);
424       rh=1/(fabs(rh)+.001);
425       /* Compute ratio, will help predict the gain */
426       filter_ratio=fabs(.001+rh)/(.001+fabs(rl));
427
428       
429       if (0) {/* 1 for spectral folding excitation, 0 for stochastic */
430          float el=0,eh=0,g;
431          /* Backup memory */
432          for (i=0;i<st->lpcSize;i++)
433             mem[i]=st->mem_sp[i];
434          /* Compute "real excitation" */
435          residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp);
436          
437 #if 0
438          /* Compute energy of low-band and high-band excitation */
439          for (i=0;i<st->subframeSize;i++)
440             eh+=sqr(exc[i]);
441          for (i=0;i<st->subframeSize;i++)
442             el+=sqr(st->st_low.exc[offset+i]);
443
444          for (i=0;i<st->subframeSize;i++)
445          {
446             float p=(.1+exc[i])*filter_ratio/(1+sqrt(el/st->subframeSize));
447             if (i%8==0)
448                printf ("\nhexc: ");
449             printf ("%f ", p);
450          }
451          printf ("\n");
452
453          /* Gain to use if we want to use the low-band excitation for high-band */
454          g=eh/(.01+el);
455          g=sqrt(g);
456
457          /* High-band excitation using the low-band excitation and a gain */
458          for (i=0;i<st->subframeSize;i++)
459             exc[i]=g*st->st_low.exc[offset+i];
460          
461          /* FIXME: Should encode the gain here */
462 #endif
463          /* Update the input signal using the non-coded memory */
464          syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, mem);
465       } else if (0) {/* Stochastic split-VQ excitation */
466          int k,N=4;
467          float el=0,eh=0,eb=0,g;
468          int *index;
469          float *gains;
470          gains = PUSH(st->stack, N);
471          index = (int*) PUSH(st->stack, N);
472          
473          /* Reset excitation */
474          for (i=0;i<st->subframeSize;i++)
475             exc[i]=0;
476          
477          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
478          for (i=0;i<st->lpcSize;i++)
479             mem[i]=st->mem_sp[i];
480          syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
481          for (i=0;i<st->lpcSize;i++)
482             mem[i]=st->mem_sp[i];
483          residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
484          for (i=0;i<st->lpcSize;i++)
485             mem[i]=st->mem_sw[i];
486          syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
487          
488          /* Compute weighted signal */
489          for (i=0;i<st->lpcSize;i++)
490             mem[i]=st->mem_sp[i];
491          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
492          for (i=0;i<st->lpcSize;i++)
493             mem[i]=st->mem_sw[i];
494          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
495          
496          /* Compute target signal */
497          for (i=0;i<st->subframeSize;i++)
498             target[i]=sw[i]-res[i];
499          
500          
501          /* Compute "real excitation" */
502          residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
503          /* Energy of "real excitation" */
504          for (i=0;i<st->subframeSize;i++)
505             eh+=sqr(exc[i]);
506
507          for (i=0;i<st->subframeSize;i++)
508             exc[i]=0;
509          /*
510 st->st_low.ltp_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
511                     exc, st->st_low.ltp_params, st->st_low.min_pitch, st->st_low.max_pitch, 
512                     st->lpcSize, st->subframeSize, bits, st->stack, exc);
513          */
514          /* For all sub-vectors, find best gain and codeword/shape */
515          for (k=0;k<N;k++)
516          {
517             int of=k*st->subframeSize/N;
518             overlap_cb_search(target+of, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
519                               &stoc[0], 64, &gains[k], &index[k], st->lpcSize,
520                               st->subframeSize/N, st->stack);
521             
522             frame_bits_pack(bits,index[k],6);
523             
524             /* Compute response */
525             for (i=0;i<st->subframeSize;i++)
526                res[i]=0;
527             for (i=0;i<st->subframeSize/N;i++)
528                res[of+i]=gains[k]*stoc[index[k]+i];
529             residue_zero(res, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
530             syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
531             syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
532             /* Update target */
533             for (i=0;i<st->subframeSize;i++)
534                target[i]-=res[i];
535
536             /* Update excitation */
537             for (i=0;i<st->subframeSize/N;i++)
538                exc[of+i]+=gains[k]*stoc[index[k]+i];
539          }
540          /* Compute energy of best excitation found */
541          for (i=0;i<st->subframeSize;i++)
542             eb+=sqr(exc[i]);
543
544          /* Compute adjustment gain for the new excitation to have the same energy 
545             as the "real" one */
546          g=sqrt(eh/(eb+.001));
547
548          /* Compute low-band excitation energy*/
549          for (i=0;i<st->subframeSize;i++)
550             el+=sqr(st->st_low.exc[offset+i]);
551          
552          /* Quantize all gains */
553          for (k=0;k<N;k++)
554          {
555             int sign=0;
556             float quant;
557             int best_ind;
558             int of=k*st->subframeSize/N;
559             gains[k]*=g;
560
561             /* Get sign separately */
562             if (gains[k]<0)
563             {
564                sign=1;
565                gains[k] = -gains[k];
566             }
567             /* Use prediction with low-band energy and filter pi-response ratio and 
568                then convert to the log domain */
569             quant = log((1+gains[k])*filter_ratio/(1+sqrt(el/st->subframeSize)));
570             
571             /* Quantize the gain */
572             best_ind = vq_index(&quant, quant_high_gain2, 1, 8);
573             quant=quant_high_gain2[best_ind];
574
575             frame_bits_pack(bits,sign,1);
576             frame_bits_pack(bits,best_ind,3);
577
578             gains[k]=exp(quant)*(1+sqrt(el/st->subframeSize))/filter_ratio;
579             if (sign)
580                gains[k] = -gains[k];
581
582             /* FIXME: Should we use the "adjusted excitation" in the encoder? */
583             for (i=0;i<st->subframeSize/N;i++)
584               exc[of+i]=gains[k]*stoc[index[k]+i];
585
586          }
587          POP(st->stack);
588          POP(st->stack);
589
590
591          /*Keep the previous memory*/
592          for (i=0;i<st->lpcSize;i++)
593             mem[i]=st->mem_sp[i];
594          /* Final signal synthesis from excitation */
595          syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
596          
597          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
598          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
599          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
600       } else {
601          float el=0;
602          float gc;
603
604          for (i=0;i<st->subframeSize;i++)
605             el+=sqr(st->st_low.exc[offset+i]);
606
607          gc = (.01+filter_ratio)/(1+sqrt(el/st->subframeSize));
608
609          /* Reset excitation */
610          for (i=0;i<st->subframeSize;i++)
611             exc[i]=0;
612          
613          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
614          for (i=0;i<st->lpcSize;i++)
615             mem[i]=st->mem_sp[i];
616          syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
617          for (i=0;i<st->lpcSize;i++)
618             mem[i]=st->mem_sp[i];
619          residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
620          for (i=0;i<st->lpcSize;i++)
621             mem[i]=st->mem_sw[i];
622          syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
623          
624          /* Compute weighted signal */
625          for (i=0;i<st->lpcSize;i++)
626             mem[i]=st->mem_sp[i];
627          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
628          for (i=0;i<st->lpcSize;i++)
629             mem[i]=st->mem_sw[i];
630          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
631          
632          /* Compute target signal */
633          for (i=0;i<st->subframeSize;i++)
634             target[i]=gc*(sw[i]-res[i]);
635          
636          /* Reset excitation */
637          for (i=0;i<st->subframeSize;i++)
638             exc[i]=0;
639
640          print_vec(target, st->subframeSize, "\ntarget");
641          split_cb_search_nogain(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
642                                 &split_cb_high, st->lpcSize, st->subframeSize, 
643                                 exc, bits, st->stack);
644          print_vec(target, st->subframeSize, "after");
645
646          for (i=0;i<st->subframeSize;i++)
647             exc[i] *= 1/gc;
648 #if 1
649          /*Keep the previous memory*/
650          for (i=0;i<st->lpcSize;i++)
651             mem[i]=st->mem_sp[i];
652          /* Final signal synthesis from excitation */
653          syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
654          
655          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
656          residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
657          syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
658 #endif
659       }
660       
661       POP(st->stack);
662    }
663
664
665 #ifndef RELEASE
666    /* Up-sample coded low-band and high-band*/
667    for (i=0;i<st->frame_size;i++)
668    {
669       st->x0[(i<<1)]=st->x0d[i];
670       st->x1[(i<<1)]=st->high[i];
671       st->x0[(i<<1)+1]=0;
672       st->x1[(i<<1)+1]=0;
673    }
674    /* Reconstruct the original */
675    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
676    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
677    for (i=0;i<st->full_frame_size;i++)
678       in[i]=2*(st->y0[i]-st->y1[i]);
679 #endif
680    for (i=0;i<st->lpcSize;i++)
681       st->old_lsp[i] = st->lsp[i];
682    for (i=0;i<st->lpcSize;i++)
683       st->old_qlsp[i] = st->qlsp[i];
684
685    st->first=0;
686 }
687
688
689
690
691
692 void sb_decoder_init(SBDecState *st, SpeexMode *mode)
693 {
694    decoder_init(&st->st_low, mode);
695    st->full_frame_size = 2*st->st_low.frameSize;
696    st->frame_size = st->st_low.frameSize;
697    st->subframeSize = 40;
698    st->nbSubframes = 4;
699    st->lpcSize=8;
700
701    st->first=1;
702    st->stack = calloc(10000, sizeof(float));
703
704    st->x0=calloc(st->full_frame_size, sizeof(float));
705    st->x1=calloc(st->full_frame_size, sizeof(float));
706    st->x0d=calloc(st->frame_size, sizeof(float));
707    st->x1d=calloc(st->frame_size, sizeof(float));
708    st->high=calloc(st->full_frame_size, sizeof(float));
709    st->y0=calloc(st->full_frame_size, sizeof(float));
710    st->y1=calloc(st->full_frame_size, sizeof(float));
711
712    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
713    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
714    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
715    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
716
717    st->exc=calloc(st->frame_size, sizeof(float));
718
719    st->qlsp = malloc(st->lpcSize*sizeof(float));
720    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
721    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
722    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
723
724    st->mem_sp = calloc(st->lpcSize, sizeof(float));
725    st->mem_sw = calloc(st->lpcSize, sizeof(float));
726
727 }
728
729 void sb_decoder_destroy(SBDecState *st)
730 {
731    decoder_destroy(&st->st_low);
732    free(st->x0);
733    free(st->x0d);
734    free(st->x1);
735    free(st->x1d);
736    free(st->high);
737    free(st->y0);
738    free(st->y1);
739    free(st->h0_mem);
740    free(st->h1_mem);
741    free(st->g0_mem);
742    free(st->g1_mem);
743    
744    free(st->exc);
745    free(st->qlsp);
746    free(st->old_qlsp);
747    free(st->interp_qlsp);
748    free(st->interp_qlpc);
749
750    free(st->mem_sp);
751    free(st->mem_sw);
752
753    free(st->stack);
754    
755 }
756
757
758 void sb_decode(SBDecState *st, FrameBits *bits, float *out)
759 {
760    int i, sub;
761
762    /* Decode the low-band */
763    decode(&st->st_low, bits, st->x0d);
764
765    for (i=0;i<st->frame_size;i++)
766       st->exc[i]=0;
767
768    lsp_unquant_high(st->qlsp, st->lpcSize, bits);
769    
770    if (st->first)
771    {
772       for (i=0;i<st->lpcSize;i++)
773          st->old_qlsp[i] = st->qlsp[i];
774    }
775    
776    for (sub=0;sub<st->nbSubframes;sub++)
777    {
778       float *exc, *sp, tmp, filter_ratio;
779       int offset;
780       
781       offset = st->subframeSize*sub;
782       sp=st->high+offset;
783       exc=st->exc+offset;
784       
785       /* LSP interpolation */
786       tmp = (.5 + sub)/st->nbSubframes;
787       for (i=0;i<st->lpcSize;i++)
788          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
789
790       /* LSPs to x-domain */
791       for (i=0;i<st->lpcSize;i++)
792          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
793
794       /* LSP to LPC */
795       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
796
797       {
798          float rl=0, rh=0;
799          tmp=1;
800          for (i=0;i<=st->lpcSize;i++)
801          {
802             rh += tmp*st->interp_qlpc[i];
803             tmp = -tmp;
804          }
805          rl = st->st_low.pi_gain[sub];
806          rl=1/(fabs(rl)+.001);
807          rh=1/(fabs(rh)+.001);
808          filter_ratio=fabs(.001+rh)/(.001+fabs(rl));
809       }
810
811       {
812          int k,N=4,el=0;
813          int *index;
814          float *gains;
815          int of;
816          gains = PUSH(st->stack, N);
817          index = (int*) PUSH(st->stack, N);
818
819          for (i=0;i<st->subframeSize;i++)
820            el+=sqr(st->st_low.exc[offset+i]);
821          
822          for (k=0;k<N;k++)
823             index[k] = frame_bits_unpack_unsigned(bits,6);
824
825          for (k=0;k<N;k++)
826          {
827             int sign, gain_ind;
828             sign = frame_bits_unpack_unsigned(bits,1);
829             gain_ind = frame_bits_unpack_unsigned(bits,3);
830             gains[k]=exp(quant_high_gain2[gain_ind])*(1+sqrt(el/st->subframeSize))/filter_ratio;
831             if (sign)
832                gains[k] =- gains[k];
833          }
834
835          for (k=0;k<N;k++)
836          {
837             of=k*st->subframeSize/N;
838             for (i=0;i<st->subframeSize/N;i++)
839                exc[of+i]=gains[k]*stoc[index[k]+i];
840          }
841
842          POP(st->stack);
843          POP(st->stack);
844       }
845
846       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
847
848    }
849
850    /* Up-sample coded low-band and high-band*/
851    for (i=0;i<st->frame_size;i++)
852    {
853       st->x0[(i<<1)]=st->x0d[i];
854       st->x1[(i<<1)]=st->high[i];
855       st->x0[(i<<1)+1]=0;
856       st->x1[(i<<1)+1]=0;
857    }
858    /* Reconstruct the original */
859    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
860    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
861    for (i=0;i<st->full_frame_size;i++)
862       out[i]=2*(st->y0[i]-st->y1[i]);
863
864    for (i=0;i<st->lpcSize;i++)
865       st->old_qlsp[i] = st->qlsp[i];
866
867    st->first=0;
868
869 }