Stack allocation cleanup...
[speexdsp.git] / libspeex / sb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: sb_celp.c
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32
33 #include <stdlib.h>
34 #include <math.h>
35 #include <stdio.h>
36 #include "nb_celp.h"
37 #include "sb_celp.h"
38 #include "stdlib.h"
39 #include "filters.h"
40 #include "lpc.h"
41 #include "lsp.h"
42 #include "stack_alloc.h"
43 #include "cb_search.h"
44 #include "quant_lsp.h"
45 #include "vq.h"
46 #include "ltp.h"
47 #include "misc.h"
48
49 #ifndef M_PI
50 #define M_PI           3.14159265358979323846  /* pi */
51 #endif
52
53 #define sqr(x) ((x)*(x))
54
55 #define SUBMODE(x) st->submodes[st->submodeID]->x
56
57 #define QMF_ORDER 64
58 static float h0[64] = {
59    3.596189e-05, -0.0001123515,
60    -0.0001104587, 0.0002790277,
61    0.0002298438, -0.0005953563,
62    -0.0003823631, 0.00113826,
63    0.0005308539, -0.001986177,
64    -0.0006243724, 0.003235877,
65    0.0005743159, -0.004989147,
66    -0.0002584767, 0.007367171,
67    -0.0004857935, -0.01050689,
68    0.001894714, 0.01459396,
69    -0.004313674, -0.01994365,
70    0.00828756, 0.02716055,
71    -0.01485397, -0.03764973,
72    0.026447, 0.05543245,
73    -0.05095487, -0.09779096,
74    0.1382363, 0.4600981,
75    0.4600981, 0.1382363,
76    -0.09779096, -0.05095487,
77    0.05543245, 0.026447,
78    -0.03764973, -0.01485397,
79    0.02716055, 0.00828756,
80    -0.01994365, -0.004313674,
81    0.01459396, 0.001894714,
82    -0.01050689, -0.0004857935,
83    0.007367171, -0.0002584767,
84    -0.004989147, 0.0005743159,
85    0.003235877, -0.0006243724,
86    -0.001986177, 0.0005308539,
87    0.00113826, -0.0003823631,
88    -0.0005953563, 0.0002298438,
89    0.0002790277, -0.0001104587,
90    -0.0001123515, 3.596189e-05
91 };
92
93 static float h1[64] = {
94    3.596189e-05, 0.0001123515,
95    -0.0001104587, -0.0002790277,
96    0.0002298438, 0.0005953563,
97    -0.0003823631, -0.00113826,
98    0.0005308539, 0.001986177,
99    -0.0006243724, -0.003235877,
100    0.0005743159, 0.004989147,
101    -0.0002584767, -0.007367171,
102    -0.0004857935, 0.01050689,
103    0.001894714, -0.01459396,
104    -0.004313674, 0.01994365,
105    0.00828756, -0.02716055,
106    -0.01485397, 0.03764973,
107    0.026447, -0.05543245,
108    -0.05095487, 0.09779096,
109    0.1382363, -0.4600981,
110    0.4600981, -0.1382363,
111    -0.09779096, 0.05095487,
112    0.05543245, -0.026447,
113    -0.03764973, 0.01485397,
114    0.02716055, -0.00828756,
115    -0.01994365, 0.004313674,
116    0.01459396, -0.001894714,
117    -0.01050689, 0.0004857935,
118    0.007367171, 0.0002584767,
119    -0.004989147, -0.0005743159,
120    0.003235877, 0.0006243724,
121    -0.001986177, -0.0005308539,
122    0.00113826, 0.0003823631,
123    -0.0005953563, -0.0002298438,
124    0.0002790277, 0.0001104587,
125    -0.0001123515, -3.596189e-05
126 };
127
128 void *sb_encoder_init(SpeexMode *m)
129 {
130    int i;
131    SBEncState *st;
132    SpeexSBMode *mode;
133
134    st = speex_alloc(sizeof(SBEncState));
135    st->mode = m;
136    mode = m->mode;
137
138    st->st_low = nb_encoder_init(mode->nb_mode);
139    st->full_frame_size = 2*mode->frameSize;
140    st->frame_size = mode->frameSize;
141    st->subframeSize = mode->subframeSize;
142    st->nbSubframes = mode->frameSize/mode->subframeSize;
143    st->windowSize = st->frame_size*3/2;
144    st->lpcSize=mode->lpcSize;
145    st->bufSize=mode->bufSize;
146
147    st->submodes=mode->submodes;
148    st->submodeID=mode->defaultSubmode;
149    {
150       int mod=6;
151       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &mod);
152    }
153
154    st->lag_factor = mode->lag_factor;
155    st->lpc_floor = mode->lpc_floor;
156    st->gamma1=mode->gamma1;
157    st->gamma2=mode->gamma2;
158    st->first=1;
159    st->stack = speex_alloc(10000*sizeof(float));
160
161    st->x0d=speex_alloc(st->frame_size*sizeof(float));
162    st->x1d=speex_alloc(st->frame_size*sizeof(float));
163    st->high=speex_alloc(st->full_frame_size*sizeof(float));
164    st->y0=speex_alloc(st->full_frame_size*sizeof(float));
165    st->y1=speex_alloc(st->full_frame_size*sizeof(float));
166
167    st->h0_mem=speex_alloc(QMF_ORDER*sizeof(float));
168    st->h1_mem=speex_alloc(QMF_ORDER*sizeof(float));
169    st->g0_mem=speex_alloc(QMF_ORDER*sizeof(float));
170    st->g1_mem=speex_alloc(QMF_ORDER*sizeof(float));
171
172    st->buf=speex_alloc(st->windowSize*sizeof(float));
173    st->excBuf=speex_alloc(st->bufSize*sizeof(float));
174    st->exc = st->excBuf + st->bufSize - st->windowSize;
175    /*st->exc=st->excBuf+st->frame_size;*/
176
177    st->res=speex_alloc(st->frame_size*sizeof(float));
178    st->sw=speex_alloc(st->frame_size*sizeof(float));
179    st->target=speex_alloc(st->frame_size*sizeof(float));
180    /*Asymetric "pseudo-Hamming" window*/
181    {
182       int part1, part2;
183       part1 = st->subframeSize*7/2;
184       part2 = st->subframeSize*5/2;
185       st->window = speex_alloc(st->windowSize*sizeof(float));
186       for (i=0;i<part1;i++)
187          st->window[i]=.54-.46*cos(M_PI*i/part1);
188       for (i=0;i<part2;i++)
189          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
190    }
191
192    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(float));
193    for (i=0;i<st->lpcSize+1;i++)
194       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
195
196    st->rc = speex_alloc(st->lpcSize*sizeof(float));
197    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(float));
198    st->lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
199    st->bw_lpc1 = speex_alloc((st->lpcSize+1)*sizeof(float));
200    st->bw_lpc2 = speex_alloc((st->lpcSize+1)*sizeof(float));
201    st->lsp = speex_alloc(st->lpcSize*sizeof(float));
202    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
203    st->old_lsp = speex_alloc(st->lpcSize*sizeof(float));
204    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
205    st->interp_lsp = speex_alloc(st->lpcSize*sizeof(float));
206    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
207    st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
208    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
209
210    st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
211    st->mem_sp2 = speex_alloc(st->lpcSize*sizeof(float));
212    st->mem_sw = speex_alloc(st->lpcSize*sizeof(float));
213    st->complexity=2;
214
215    return st;
216 }
217
218 void sb_encoder_destroy(void *state)
219 {
220    SBEncState *st=state;
221
222    nb_encoder_destroy(st->st_low);
223    speex_free(st->x0d);
224    speex_free(st->x1d);
225    speex_free(st->high);
226    speex_free(st->y0);
227    speex_free(st->y1);
228    speex_free(st->h0_mem);
229    speex_free(st->h1_mem);
230    speex_free(st->g0_mem);
231    speex_free(st->g1_mem);
232    
233    speex_free(st->buf);
234    speex_free(st->window);
235    speex_free(st->excBuf);
236    speex_free(st->sw);
237    speex_free(st->res);
238    speex_free(st->target);
239    speex_free(st->lagWindow);
240    speex_free(st->rc);
241    speex_free(st->autocorr);
242    speex_free(st->lpc);
243    speex_free(st->bw_lpc1);
244    speex_free(st->bw_lpc2);
245    speex_free(st->lsp);
246    speex_free(st->qlsp);
247    speex_free(st->old_lsp);
248    speex_free(st->old_qlsp);
249    speex_free(st->interp_lsp);
250    speex_free(st->interp_qlsp);
251    speex_free(st->interp_lpc);
252    speex_free(st->interp_qlpc);
253
254    speex_free(st->mem_sp);
255    speex_free(st->mem_sp2);
256    speex_free(st->mem_sw);
257
258    speex_free(st->stack);
259
260    speex_free(st);
261 }
262
263
264 void sb_encode(void *state, float *in, SpeexBits *bits)
265 {
266    SBEncState *st;
267    int i, roots, sub;
268    float *stack;
269    float *mem, *innov, *syn_resp;
270
271    st = state;
272    stack=st->stack;
273
274    /* Compute the two sub-bands by filtering with h0 and h1*/
275    qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem);
276
277    /* Encode the narrowband part*/
278    nb_encode(st->st_low, st->x0d, bits);
279
280    speex_bits_pack(bits, 1, 1);
281    speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
282
283    /* High-band buffering / sync with low band */
284    for (i=0;i<st->windowSize-st->frame_size;i++)
285       st->high[i] = st->high[st->frame_size+i];
286    for (i=0;i<st->frame_size;i++)
287       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
288
289    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
290
291    /* Start encoding the high-band */
292
293    for (i=0;i<st->windowSize;i++)
294       st->buf[i] = st->high[i] * st->window[i];
295
296    /* Compute auto-correlation */
297    autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
298
299    st->autocorr[0] += 1;        /* prevents NANs */
300    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
301    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
302    for (i=0;i<st->lpcSize+1;i++)
303       st->autocorr[i] *= st->lagWindow[i];
304
305    /* Levinson-Durbin */
306    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
307    st->lpc[0]=1;
308
309    /* LPC to LSPs (x-domain) transform */
310    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
311    if (roots!=st->lpcSize)
312    {
313       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, stack);
314       if (roots!=st->lpcSize) {
315          /*fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);*/
316
317          /*If we can't find all LSP's, do some damage control and use a flat filter*/
318          for (i=0;i<st->lpcSize;i++)
319          {
320             st->lsp[i]=cos(M_PI*((float)(i+1))/(st->lpcSize+1));
321          }
322       }
323    }
324
325    /* x-domain to angle domain*/
326    for (i=0;i<st->lpcSize;i++)
327       st->lsp[i] = acos(st->lsp[i]);
328
329    /* If null mode (no transmission), just set a couple things to zero*/
330    if (st->submodes[st->submodeID] == NULL)
331    {
332       for (i=0;i<st->frame_size;i++)
333          st->exc[i]=st->sw[i]=0;
334
335       for (i=0;i<st->lpcSize;i++)
336          st->mem_sw[i]=0;
337       st->first=1;
338
339       /* Final signal synthesis from excitation */
340       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
341
342 #ifndef RELEASE
343
344       /* Reconstruct the original */
345       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
346       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
347
348       for (i=0;i<st->full_frame_size;i++)
349          in[i]=2*(st->y0[i]-st->y1[i]);
350 #endif
351
352       return;
353
354    }
355
356
357    /* LSP quantization */
358    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
359    
360    /*printf ("high_lsp:");
361    for (i=0;i<st->lpcSize;i++)
362       printf (" %f", st->lsp[i]);
363       printf ("\n");*/
364    /*for (i=0;i<st->lpcSize;i++)
365      st->qlsp[i]=st->lsp[i];*/
366    
367
368    if (st->first)
369    {
370       for (i=0;i<st->lpcSize;i++)
371          st->old_lsp[i] = st->lsp[i];
372       for (i=0;i<st->lpcSize;i++)
373          st->old_qlsp[i] = st->qlsp[i];
374    }
375    
376    mem=PUSH(stack, st->lpcSize, float);
377    syn_resp=PUSH(stack, st->subframeSize, float);
378    innov = PUSH(stack, st->subframeSize, float);
379
380    for (sub=0;sub<st->nbSubframes;sub++)
381    {
382       float *exc, *sp, *res, *target, *sw, tmp, filter_ratio;
383       int offset;
384       float rl, rh, eh=0, el=0;
385       int fold;
386
387       offset = st->subframeSize*sub;
388       sp=st->high+offset;
389       exc=st->excBuf+offset;
390       res=st->res+offset;
391       target=st->target+offset;
392       sw=st->sw+offset;
393       
394       /* LSP interpolation (quantized and unquantized) */
395       tmp = (1.0 + 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       for (i=0;i<st->lpcSize;i++)
405          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
406
407       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
408       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
409
410       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
411       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
412
413       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
414       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
415
416       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
417          filters */
418       rl=rh=0;
419       tmp=1;
420       for (i=0;i<=st->lpcSize;i++)
421       {
422          rh += tmp*st->interp_qlpc[i];
423          tmp = -tmp;
424       }
425       rl = ((EncState*)st->st_low)->pi_gain[sub];
426       rl=1/(fabs(rl)+.01);
427       rh=1/(fabs(rh)+.01);
428       /* Compute ratio, will help predict the gain */
429       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
430
431       fold = filter_ratio<5;
432       /*printf ("filter_ratio %f\n", filter_ratio);*/
433       fold=0;
434
435       /* Compute "real excitation" */
436       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
437       /* Compute energy of low-band and high-band excitation */
438       for (i=0;i<st->subframeSize;i++)
439          eh+=sqr(exc[i]);
440
441       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
442          float g;
443          /*speex_bits_pack(bits, 1, 1);*/
444          for (i=0;i<st->subframeSize;i++)
445             el+=sqr(((EncState*)st->st_low)->innov[offset+i]);
446
447          /* Gain to use if we want to use the low-band excitation for high-band */
448          g=eh/(.01+el);
449          g=sqrt(g);
450
451          g *= filter_ratio;
452          /* Gain quantization */
453          {
454             int quant = (int) floor(.5 + 27 + 8.0 * log((g+.0001)));
455             if (quant<0)
456                quant=0;
457             if (quant>31)
458                quant=31;
459             speex_bits_pack(bits, quant, 5);
460             g= .1*exp(quant/9.4);
461          }
462          /*printf ("folding gain: %f\n", g);*/
463          g /= filter_ratio;
464
465       } else {
466          float gc, scale, scale_1;
467
468          for (i=0;i<st->subframeSize;i++)
469             el+=sqr(((EncState*)st->st_low)->exc[offset+i]);
470          /*speex_bits_pack(bits, 0, 1);*/
471
472          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
473          {
474             int qgc = (int)floor(.5+3.7*(log(gc)+2));
475             if (qgc<0)
476                qgc=0;
477             if (qgc>15)
478                qgc=15;
479             speex_bits_pack(bits, qgc, 4);
480             gc = exp((1/3.7)*qgc-2);
481          }
482
483          scale = gc*sqrt(1+el)/filter_ratio;
484          scale_1 = 1/scale;
485          if (0 && rand()%5==0)
486          {
487             float sc = 1/sqrt(.1+eh/st->subframeSize);
488             if (rand()&1)
489                sc=-sc;
490             for (i=0;i<st->subframeSize;i++)
491             {
492                float tmp=exc[i]*sc;
493                if (i%8==0)
494                   printf ("\nhexc");
495                printf (" %f", tmp);
496             }
497          }
498
499          for (i=0;i<st->subframeSize;i++)
500             exc[i]=0;
501          exc[0]=1;
502          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
503          
504          /* Reset excitation */
505          for (i=0;i<st->subframeSize;i++)
506             exc[i]=0;
507          
508          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
509          for (i=0;i<st->lpcSize;i++)
510             mem[i]=st->mem_sp[i];
511          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
512
513          for (i=0;i<st->lpcSize;i++)
514             mem[i]=st->mem_sw[i];
515          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
516
517          /* Compute weighted signal */
518          for (i=0;i<st->lpcSize;i++)
519             mem[i]=st->mem_sw[i];
520          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
521
522          /* Compute target signal */
523          for (i=0;i<st->subframeSize;i++)
524             target[i]=sw[i]-res[i];
525
526          for (i=0;i<st->subframeSize;i++)
527            exc[i]=0;
528
529
530          for (i=0;i<st->subframeSize;i++)
531             target[i]*=scale_1;
532          
533          /* Reset excitation */
534          for (i=0;i<st->subframeSize;i++)
535             innov[i]=0;
536
537          /*print_vec(target, st->subframeSize, "\ntarget");*/
538          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
539                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
540                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
541          /*print_vec(target, st->subframeSize, "after");*/
542
543          for (i=0;i<st->subframeSize;i++)
544             exc[i] += innov[i]*scale;
545
546          if (SUBMODE(double_codebook)) {
547             float *tmp_stack=stack;
548             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
549             for (i=0;i<st->subframeSize;i++)
550                innov2[i]=0;
551             for (i=0;i<st->subframeSize;i++)
552                target[i]*=2.5;
553             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
554                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
555                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
556             for (i=0;i<st->subframeSize;i++)
557                innov2[i]*=scale*(1/2.5);
558             for (i=0;i<st->subframeSize;i++)
559                exc[i] += innov2[i];
560          }
561
562
563          if (0) {
564             float en=0;
565             for (i=0;i<st->subframeSize;i++)
566                en+=exc[i]*exc[i];
567             en=sqrt(eh/(1+en));
568             for (i=0;i<st->subframeSize;i++)
569                exc[i]*=en;
570             printf ("correction high: %f\n", en);
571          }
572
573       }
574
575          /*Keep the previous memory*/
576          for (i=0;i<st->lpcSize;i++)
577             mem[i]=st->mem_sp[i];
578          /* Final signal synthesis from excitation */
579          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
580          
581          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
582          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
583    }
584
585
586 #ifndef RELEASE
587
588    /* Reconstruct the original */
589    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
590    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
591
592    for (i=0;i<st->full_frame_size;i++)
593       in[i]=2*(st->y0[i]-st->y1[i]);
594 #endif
595    for (i=0;i<st->lpcSize;i++)
596       st->old_lsp[i] = st->lsp[i];
597    for (i=0;i<st->lpcSize;i++)
598       st->old_qlsp[i] = st->qlsp[i];
599
600    st->first=0;
601 }
602
603
604
605
606
607 void *sb_decoder_init(SpeexMode *m)
608 {
609    SBDecState *st;
610    SpeexSBMode *mode;
611    st = speex_alloc(sizeof(SBDecState));
612    st->mode = m;
613    mode=m->mode;
614
615    st->st_low = nb_decoder_init(mode->nb_mode);
616    st->full_frame_size = 2*mode->frameSize;
617    st->frame_size = mode->frameSize;
618    st->subframeSize = 40;
619    st->nbSubframes = 4;
620    st->lpcSize=8;
621
622    st->submodes=mode->submodes;
623    st->submodeID=mode->defaultSubmode;
624
625    st->first=1;
626    st->stack = speex_alloc(10000*sizeof(float));
627
628    st->x0d=speex_alloc(st->frame_size*sizeof(float));
629    st->x1d=speex_alloc(st->frame_size*sizeof(float));
630    st->high=speex_alloc(st->full_frame_size*sizeof(float));
631    st->y0=speex_alloc(st->full_frame_size*sizeof(float));
632    st->y1=speex_alloc(st->full_frame_size*sizeof(float));
633
634    st->h0_mem=speex_alloc(QMF_ORDER*sizeof(float));
635    st->h1_mem=speex_alloc(QMF_ORDER*sizeof(float));
636    st->g0_mem=speex_alloc(QMF_ORDER*sizeof(float));
637    st->g1_mem=speex_alloc(QMF_ORDER*sizeof(float));
638
639    st->exc=speex_alloc(st->frame_size*sizeof(float));
640
641    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
642    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
643    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
644    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
645
646    st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
647    return st;
648 }
649
650 void sb_decoder_destroy(void *state)
651 {
652    SBDecState *st;
653    st = state;
654    nb_decoder_destroy(st->st_low);
655    speex_free(st->x0d);
656    speex_free(st->x1d);
657    speex_free(st->high);
658    speex_free(st->y0);
659    speex_free(st->y1);
660    speex_free(st->h0_mem);
661    speex_free(st->h1_mem);
662    speex_free(st->g0_mem);
663    speex_free(st->g1_mem);
664    
665    speex_free(st->exc);
666    speex_free(st->qlsp);
667    speex_free(st->old_qlsp);
668    speex_free(st->interp_qlsp);
669    speex_free(st->interp_qlpc);
670
671    speex_free(st->mem_sp);
672
673    speex_free(st->stack);
674
675    speex_free(state);
676 }
677
678 static void sb_decode_lost(SBDecState *st, float *out, float *stack)
679 {
680    int i;
681    for (i=0;i<st->frame_size;i++)
682       st->exc[i]*=0.8;
683    
684    st->first=1;
685    
686    /* Final signal synthesis from excitation */
687    iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
688    
689    /* Reconstruct the original */
690    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
691    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
692
693    for (i=0;i<st->full_frame_size;i++)
694       out[i]=2*(st->y0[i]-st->y1[i]);
695    
696    return;
697 }
698
699 int sb_decode(void *state, SpeexBits *bits, float *out)
700 {
701    int i, sub;
702    SBDecState *st;
703    int wideband;
704    int ret;
705    float *stack;
706
707    st = state;
708    stack=st->stack;
709
710    /* Decode the low-band */
711    ret = nb_decode(st->st_low, bits, st->x0d);
712
713    /* If error decoding the narrowband part, propagate error */
714    if (ret!=0)
715    {
716       return ret;
717    }
718
719    if (!bits)
720    {
721       sb_decode_lost(st, out, stack);
722       return 0;
723    }
724
725    /*Check "wideband bit"*/
726    wideband = speex_bits_peek(bits);
727    if (wideband)
728    {
729       /*Regular wideband frame, read the submode*/
730       wideband = speex_bits_unpack_unsigned(bits, 1);
731       st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
732    } else
733    {
734       /*Was a narrowband frame, set "null submode"*/
735       st->submodeID = 0;
736    }
737
738    for (i=0;i<st->frame_size;i++)
739       st->exc[i]=0;
740
741    /* If null mode (no transmission), just set a couple things to zero*/
742    if (st->submodes[st->submodeID] == NULL)
743    {
744       for (i=0;i<st->frame_size;i++)
745          st->exc[i]=0;
746
747       st->first=1;
748
749       /* Final signal synthesis from excitation */
750       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
751
752       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
753       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
754
755       for (i=0;i<st->full_frame_size;i++)
756          out[i]=2*(st->y0[i]-st->y1[i]);
757
758       return 0;
759
760    }
761
762
763    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
764    
765    if (st->first)
766    {
767       for (i=0;i<st->lpcSize;i++)
768          st->old_qlsp[i] = st->qlsp[i];
769    }
770    
771    for (sub=0;sub<st->nbSubframes;sub++)
772    {
773       float *exc, *sp, tmp, filter_ratio, el=0;
774       int offset;
775       
776       offset = st->subframeSize*sub;
777       sp=st->high+offset;
778       exc=st->exc+offset;
779       
780       /* LSP interpolation */
781       tmp = (1.0 + sub)/st->nbSubframes;
782       for (i=0;i<st->lpcSize;i++)
783          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
784
785       /* LSPs to x-domain */
786       for (i=0;i<st->lpcSize;i++)
787          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
788
789       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
790
791       /* LSP to LPC */
792       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
793
794       /* Calculate reponse ratio between the low and high filter in the middle
795          of the band (4000 Hz) */
796       {
797          float rl=0, rh=0;
798          tmp=1;
799          for (i=0;i<=st->lpcSize;i++)
800          {
801             rh += tmp*st->interp_qlpc[i];
802             tmp = -tmp;
803          }
804          rl = ((DecState*)st->st_low)->pi_gain[sub];
805          rl=1/(fabs(rl)+.01);
806          rh=1/(fabs(rh)+.01);
807          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
808       }
809       
810       for (i=0;i<st->subframeSize;i++)
811          exc[i]=0;
812       if (!SUBMODE(innovation_unquant))
813       {
814          float g;
815          int quant;
816
817          for (i=0;i<st->subframeSize;i++)
818             el+=sqr(((DecState*)st->st_low)->innov[offset+i]);
819          quant = speex_bits_unpack_unsigned(bits, 5);
820          g= exp(((float)quant-27)/8.0);
821          
822          /*printf ("unquant folding gain: %f\n", g);*/
823          g /= filter_ratio;
824          
825          /* High-band excitation using the low-band excitation and a gain */
826          for (i=0;i<st->subframeSize;i++)
827             exc[i]=.8*g*((DecState*)st->st_low)->innov[offset+i];
828       } else {
829          float gc, scale;
830          int qgc = speex_bits_unpack_unsigned(bits, 4);
831          for (i=0;i<st->subframeSize;i++)
832             el+=sqr(((DecState*)st->st_low)->exc[offset+i]);
833
834
835          gc = exp((1/3.7)*qgc-2);
836
837          scale = gc*sqrt(1+el)/filter_ratio;
838
839
840          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
841                                 bits, stack);
842          for (i=0;i<st->subframeSize;i++)
843             exc[i]*=scale;
844
845          if (SUBMODE(double_codebook)) {
846             float *tmp_stack=stack;
847             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
848             for (i=0;i<st->subframeSize;i++)
849                innov2[i]=0;
850             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
851                                 bits, tmp_stack);
852             for (i=0;i<st->subframeSize;i++)
853                innov2[i]*=scale*(1/2.5);
854             for (i=0;i<st->subframeSize;i++)
855                exc[i] += innov2[i];
856          }
857
858       }
859       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
860
861    }
862
863    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
864    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
865
866    for (i=0;i<st->full_frame_size;i++)
867       out[i]=2*(st->y0[i]-st->y1[i]);
868
869    for (i=0;i<st->lpcSize;i++)
870       st->old_qlsp[i] = st->qlsp[i];
871
872    st->first=0;
873
874    return 0;
875 }
876
877
878 void sb_encoder_ctl(void *state, int request, void *ptr)
879 {
880    SBEncState *st;
881    st=state;
882    switch(request)
883    {
884    case SPEEX_GET_FRAME_SIZE:
885       (*(int*)ptr) = st->full_frame_size;
886       break;
887    case SPEEX_SET_HIGH_MODE:
888       st->submodeID = (*(int*)ptr);
889       break;
890    case SPEEX_SET_LOW_MODE:
891       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, ptr);
892       break;
893    case SPEEX_SET_VBR:
894       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
895       break;
896    case SPEEX_SET_VBR_QUALITY:
897       {
898          int qual = (*(int*)ptr)+1;
899          if (qual>10)
900             qual=10;
901          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
902          speex_encoder_ctl(state, SPEEX_SET_QUALITY, ptr);
903          break;
904       }
905    case SPEEX_SET_QUALITY:
906       {
907          int nb_mode;
908          int quality = (*(int*)ptr);
909          switch (quality)
910          {
911          case 0:
912             nb_mode=0;
913             st->submodeID = 0;
914             break;
915          case 1:
916             nb_mode=1;
917             st->submodeID = 1;
918             break;
919          case 2:
920             nb_mode=2;
921             st->submodeID = 1;
922             break;
923          case 3:
924             nb_mode=3;
925             st->submodeID = 1;
926             break;
927          case 4:
928             nb_mode=4;
929             st->submodeID = 1;
930             break;
931          case 5:
932             nb_mode=5;
933             st->submodeID = 1;
934             break;
935          case 6:
936             nb_mode=5;
937             st->submodeID = 2;
938             break;
939          case 7:
940             nb_mode=6;
941             st->submodeID = 2;
942             break;
943          case 8:
944             nb_mode=6;
945             st->submodeID = 3;
946             break;
947          case 9:
948             nb_mode=7;
949             st->submodeID = 3;
950             break;
951          case 10:
952             nb_mode=7;
953             st->submodeID = 4;
954             break;
955          default:
956             fprintf(stderr, "Unknown sb_ctl quality: %d\n", quality);
957          }
958          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_mode);
959       }
960       break;
961    case SPEEX_SET_COMPLEXITY:
962       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
963       st->complexity = (*(int*)ptr);
964       if (st->complexity<1)
965          st->complexity=1;
966       break;
967    case SPEEX_GET_COMPLEXITY:
968       (*(int*)ptr) = st->complexity;
969       break;
970    case SPEEX_GET_BITRATE:
971       speex_encoder_ctl(st->st_low, request, ptr);
972       if (st->submodes[st->submodeID])
973          (*(int*)ptr) += 50*SUBMODE(bits_per_frame);
974       else
975          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
976       break;
977    default:
978       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
979    }
980
981 }
982
983 void sb_decoder_ctl(void *state, int request, void *ptr)
984 {
985    SBDecState *st;
986    st=state;
987    switch(request)
988    {
989    case SPEEX_GET_FRAME_SIZE:
990       (*(int*)ptr) = st->full_frame_size;
991       break;
992    case SPEEX_SET_ENH:
993       speex_decoder_ctl(st->st_low, request, ptr);
994       break;
995    case SPEEX_GET_BITRATE:
996       speex_decoder_ctl(st->st_low, request, ptr);
997       if (st->submodes[st->submodeID])
998          (*(int*)ptr) += 50*SUBMODE(bits_per_frame);
999       else
1000          (*(int*)ptr) += 50*(SB_SUBMODE_BITS+1);
1001       break;
1002    default:
1003       fprintf(stderr, "Unknown sb_ctl request: %d\n", request);
1004    }
1005
1006 }