Starting (still incomplete) average bit-rate (ABR) implementation
[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 = (SBEncState*)speex_alloc(sizeof(SBEncState));
135    st->mode = m;
136    mode = (SpeexSBMode*)m->mode;
137
138    st->st_low = speex_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->submodeSelect = st->submodeID=mode->defaultSubmode;
149    
150    {
151       /* FIXME: Should do this without explicit reference to a mode */
152 #if 0
153       int mod=6;
154       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &mod);
155 #else
156       int mod=9;
157       speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &mod);
158 #endif
159    }
160
161    st->lag_factor = mode->lag_factor;
162    st->lpc_floor = mode->lpc_floor;
163    st->gamma1=mode->gamma1;
164    st->gamma2=mode->gamma2;
165    st->first=1;
166    st->stack = speex_alloc(4000*sizeof(float));
167
168    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
169    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
170    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
171    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
172    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
173
174    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
175    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
176    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
177    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
178
179    st->buf=(float*)speex_alloc(st->windowSize*sizeof(float));
180    st->excBuf=(float*)speex_alloc(st->bufSize*sizeof(float));
181    st->exc = st->excBuf + st->bufSize - st->windowSize;
182
183    st->res=(float*)speex_alloc(st->frame_size*sizeof(float));
184    st->sw=(float*)speex_alloc(st->frame_size*sizeof(float));
185    st->target=(float*)speex_alloc(st->frame_size*sizeof(float));
186    /*Asymetric "pseudo-Hamming" window*/
187    {
188       int part1, part2;
189       part1 = st->subframeSize*7/2;
190       part2 = st->subframeSize*5/2;
191       st->window = (float*)speex_alloc(st->windowSize*sizeof(float));
192       for (i=0;i<part1;i++)
193          st->window[i]=.54-.46*cos(M_PI*i/part1);
194       for (i=0;i<part2;i++)
195          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
196    }
197
198    st->lagWindow = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
199    for (i=0;i<st->lpcSize+1;i++)
200       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
201
202    st->rc = (float*)speex_alloc(st->lpcSize*sizeof(float));
203    st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
204    st->lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
205    st->bw_lpc1 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
206    st->bw_lpc2 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
207    st->lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
208    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
209    st->old_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
210    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
211    st->interp_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
212    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
213    st->interp_lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
214    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
215    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
216
217    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
218    st->mem_sp2 = (float*)speex_alloc(st->lpcSize*sizeof(float));
219    st->mem_sw = (float*)speex_alloc(st->lpcSize*sizeof(float));
220
221    st->vbr_quality = 8;
222    st->vbr_enabled = 0;
223    st->vad_enabled = 0;
224    st->relative_quality=0;
225
226    st->complexity=2;
227    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
228    st->sampling_rate*=2;
229
230    return st;
231 }
232
233 void sb_encoder_destroy(void *state)
234 {
235    SBEncState *st=(SBEncState*)state;
236
237    speex_encoder_destroy(st->st_low);
238    speex_free(st->x0d);
239    speex_free(st->x1d);
240    speex_free(st->high);
241    speex_free(st->y0);
242    speex_free(st->y1);
243    speex_free(st->h0_mem);
244    speex_free(st->h1_mem);
245    speex_free(st->g0_mem);
246    speex_free(st->g1_mem);
247    
248    speex_free(st->buf);
249    speex_free(st->window);
250    speex_free(st->excBuf);
251    speex_free(st->sw);
252    speex_free(st->res);
253    speex_free(st->target);
254    speex_free(st->lagWindow);
255    speex_free(st->rc);
256    speex_free(st->autocorr);
257    speex_free(st->lpc);
258    speex_free(st->bw_lpc1);
259    speex_free(st->bw_lpc2);
260    speex_free(st->lsp);
261    speex_free(st->qlsp);
262    speex_free(st->old_lsp);
263    speex_free(st->old_qlsp);
264    speex_free(st->interp_lsp);
265    speex_free(st->interp_qlsp);
266    speex_free(st->interp_lpc);
267    speex_free(st->interp_qlpc);
268
269    speex_free(st->mem_sp);
270    speex_free(st->mem_sp2);
271    speex_free(st->mem_sw);
272    speex_free(st->pi_gain);
273
274    speex_free(st->stack);
275
276    speex_free(st);
277 }
278
279
280 void sb_encode(void *state, float *in, SpeexBits *bits)
281 {
282    SBEncState *st;
283    int i, roots, sub;
284    void *stack;
285    float *mem, *innov, *syn_resp;
286    float *low_pi_gain, *low_exc, *low_innov;
287    SpeexSBMode *mode;
288
289    st = (SBEncState*)state;
290    stack=st->stack;
291    mode = (SpeexSBMode*)(st->mode->mode);
292
293    /* Compute the two sub-bands by filtering with h0 and h1*/
294    qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
295     
296    /* Encode the narrowband part*/
297    speex_encode(st->st_low, st->x0d, bits);
298
299    /* High-band buffering / sync with low band */
300    for (i=0;i<st->windowSize-st->frame_size;i++)
301       st->high[i] = st->high[st->frame_size+i];
302    for (i=0;i<st->frame_size;i++)
303       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
304
305    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
306
307
308    low_pi_gain = PUSH(stack, st->nbSubframes, float);
309    low_exc = PUSH(stack, st->frame_size, float);
310    low_innov = PUSH(stack, st->frame_size, float);
311    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
312    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
313    speex_encoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
314    
315    /* Start encoding the high-band */
316    for (i=0;i<st->windowSize;i++)
317       st->buf[i] = st->high[i] * st->window[i];
318
319    /* Compute auto-correlation */
320    _spx_autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
321
322    st->autocorr[0] += 1;        /* prevents NANs */
323    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
324    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
325    for (i=0;i<st->lpcSize+1;i++)
326       st->autocorr[i] *= st->lagWindow[i];
327
328    /* Levinson-Durbin */
329    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
330    st->lpc[0]=1;
331
332    /* LPC to LSPs (x-domain) transform */
333    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
334    if (roots!=st->lpcSize)
335    {
336       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, stack);
337       if (roots!=st->lpcSize) {
338          /*fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);*/
339
340          /*If we can't find all LSP's, do some damage control and use a flat filter*/
341          for (i=0;i<st->lpcSize;i++)
342          {
343             st->lsp[i]=cos(M_PI*((float)(i+1))/(st->lpcSize+1));
344          }
345       }
346    }
347
348    /* x-domain to angle domain*/
349    for (i=0;i<st->lpcSize;i++)
350       st->lsp[i] = acos(st->lsp[i]);
351
352    /* VBR code */
353    if (st->vbr_enabled || st->vad_enabled){
354       float e_low=0, e_high=0;
355       float ratio;
356       for (i=0;i<st->frame_size;i++)
357       {
358          e_low  += st->x0d[i]* st->x0d[i];
359          e_high += st->high[i]* st->high[i];
360       }
361       ratio = log((1+e_high)/(1+e_low));
362       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &st->relative_quality);
363       if (ratio<-4)
364          ratio=-4;
365       if (ratio>2)
366          ratio=2;
367       /*if (ratio>-2)*/
368       if (st->vbr_enabled) 
369       {
370          int modeid;
371          modeid = mode->nb_modes-1;
372          st->relative_quality+=1.0*(ratio+2);
373          while (modeid)
374          {
375             int v1;
376             float thresh;
377             v1=(int)floor(st->vbr_quality);
378             if (v1==10)
379                thresh = mode->vbr_thresh[modeid][v1];
380             else
381                thresh = (st->vbr_quality-v1)   * mode->vbr_thresh[modeid][v1+1] + 
382                         (1+v1-st->vbr_quality) * mode->vbr_thresh[modeid][v1];
383             if (st->relative_quality > thresh)
384                break;
385             modeid--;
386          }
387          /*fprintf (stderr, "%f %d\n", low_qual, modeid);*/
388          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &modeid);
389          /*fprintf (stderr, "%d %d\n", st->submodeID, modeid);*/
390       } else {
391          /* VAD only */
392          int modeid;
393          if (st->relative_quality<2.0)
394             modeid=1;
395          else
396             modeid=st->submodeSelect;
397          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
398          st->submodeID=modeid;
399
400       }
401       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
402    }
403
404    speex_bits_pack(bits, 1, 1);
405    speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
406
407    /* If null mode (no transmission), just set a couple things to zero*/
408    if (st->submodes[st->submodeID] == NULL)
409    {
410       for (i=0;i<st->frame_size;i++)
411          st->exc[i]=st->sw[i]=0;
412
413       for (i=0;i<st->lpcSize;i++)
414          st->mem_sw[i]=0;
415       st->first=1;
416
417       /* Final signal synthesis from excitation */
418       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
419
420 #ifndef RELEASE
421
422       /* Reconstruct the original */
423       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
424       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
425
426       for (i=0;i<st->full_frame_size;i++)
427          in[i]=2*(st->y0[i]-st->y1[i]);
428 #endif
429
430       return;
431
432    }
433
434
435    /* LSP quantization */
436    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
437    
438    /*printf ("high_lsp:");
439    for (i=0;i<st->lpcSize;i++)
440       printf (" %f", st->lsp[i]);
441       printf ("\n");*/
442    /*for (i=0;i<st->lpcSize;i++)
443      st->qlsp[i]=st->lsp[i];*/
444    
445
446    if (st->first)
447    {
448       for (i=0;i<st->lpcSize;i++)
449          st->old_lsp[i] = st->lsp[i];
450       for (i=0;i<st->lpcSize;i++)
451          st->old_qlsp[i] = st->qlsp[i];
452    }
453    
454    mem=PUSH(stack, st->lpcSize, float);
455    syn_resp=PUSH(stack, st->subframeSize, float);
456    innov = PUSH(stack, st->subframeSize, float);
457
458    for (sub=0;sub<st->nbSubframes;sub++)
459    {
460       float *exc, *sp, *res, *target, *sw, tmp, filter_ratio;
461       int offset;
462       float rl, rh, eh=0, el=0;
463       int fold;
464
465       offset = st->subframeSize*sub;
466       sp=st->high+offset;
467       exc=st->exc+offset;
468       res=st->res+offset;
469       target=st->target+offset;
470       sw=st->sw+offset;
471       
472       /* LSP interpolation (quantized and unquantized) */
473       tmp = (1.0 + sub)/st->nbSubframes;
474       for (i=0;i<st->lpcSize;i++)
475          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
476       for (i=0;i<st->lpcSize;i++)
477          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
478       
479       /* Compute interpolated LPCs (quantized and unquantized) */
480       for (i=0;i<st->lpcSize;i++)
481          st->interp_lsp[i] = cos(st->interp_lsp[i]);
482       for (i=0;i<st->lpcSize;i++)
483          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
484
485       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
486       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
487
488       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
489       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
490
491       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
492       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
493
494       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
495          filters */
496       rl=rh=0;
497       tmp=1;
498       st->pi_gain[sub]=0;
499       for (i=0;i<=st->lpcSize;i++)
500       {
501          rh += tmp*st->interp_qlpc[i];
502          tmp = -tmp;
503          st->pi_gain[sub]+=st->interp_qlpc[i];
504       }
505       rl = low_pi_gain[sub];
506       rl=1/(fabs(rl)+.01);
507       rh=1/(fabs(rh)+.01);
508       /* Compute ratio, will help predict the gain */
509       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
510
511       fold = filter_ratio<5;
512       /*printf ("filter_ratio %f\n", filter_ratio);*/
513       fold=0;
514
515       /* Compute "real excitation" */
516       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
517       /* Compute energy of low-band and high-band excitation */
518       for (i=0;i<st->subframeSize;i++)
519          eh+=sqr(exc[i]);
520
521       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
522          float g;
523          /*speex_bits_pack(bits, 1, 1);*/
524          for (i=0;i<st->subframeSize;i++)
525             el+=sqr(low_innov[offset+i]);
526
527          /* Gain to use if we want to use the low-band excitation for high-band */
528          g=eh/(.01+el);
529          g=sqrt(g);
530
531          g *= filter_ratio;
532
533          /* Gain quantization */
534          {
535             int quant = (int) floor(.5 + 27 + 8.0 * log((g+.0001)));
536             if (quant<0)
537                quant=0;
538             if (quant>31)
539                quant=31;
540             speex_bits_pack(bits, quant, 5);
541             g= .1*exp(quant/9.4);
542          }
543          /*printf ("folding gain: %f\n", g);*/
544          g /= filter_ratio;
545
546       } else {
547          float gc, scale, scale_1;
548
549          for (i=0;i<st->subframeSize;i++)
550             el+=sqr(low_exc[offset+i]);
551          /*speex_bits_pack(bits, 0, 1);*/
552
553          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
554          {
555             int qgc = (int)floor(.5+3.7*(log(gc)+2));
556             if (qgc<0)
557                qgc=0;
558             if (qgc>15)
559                qgc=15;
560             speex_bits_pack(bits, qgc, 4);
561             gc = exp((1/3.7)*qgc-2);
562          }
563
564          scale = gc*sqrt(1+el)/filter_ratio;
565          scale_1 = 1/scale;
566          if (0 && rand()%5==0)
567          {
568             float sc = 1/sqrt(.1+eh/st->subframeSize);
569             if (rand()&1)
570                sc=-sc;
571             for (i=0;i<st->subframeSize;i++)
572             {
573                float tmp=exc[i]*sc;
574                if (i%8==0)
575                   printf ("\nhexc");
576                printf (" %f", tmp);
577             }
578          }
579
580          for (i=0;i<st->subframeSize;i++)
581             exc[i]=0;
582          exc[0]=1;
583          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
584          
585          /* Reset excitation */
586          for (i=0;i<st->subframeSize;i++)
587             exc[i]=0;
588          
589          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
590          for (i=0;i<st->lpcSize;i++)
591             mem[i]=st->mem_sp[i];
592          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
593
594          for (i=0;i<st->lpcSize;i++)
595             mem[i]=st->mem_sw[i];
596          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
597
598          /* Compute weighted signal */
599          for (i=0;i<st->lpcSize;i++)
600             mem[i]=st->mem_sw[i];
601          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
602
603          /* Compute target signal */
604          for (i=0;i<st->subframeSize;i++)
605             target[i]=sw[i]-res[i];
606
607          for (i=0;i<st->subframeSize;i++)
608            exc[i]=0;
609
610
611          for (i=0;i<st->subframeSize;i++)
612             target[i]*=scale_1;
613          
614          /* Reset excitation */
615          for (i=0;i<st->subframeSize;i++)
616             innov[i]=0;
617
618          /*print_vec(target, st->subframeSize, "\ntarget");*/
619          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
620                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
621                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
622          /*print_vec(target, st->subframeSize, "after");*/
623
624          for (i=0;i<st->subframeSize;i++)
625             exc[i] += innov[i]*scale;
626
627          if (SUBMODE(double_codebook)) {
628             void *tmp_stack=stack;
629             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
630             for (i=0;i<st->subframeSize;i++)
631                innov2[i]=0;
632             for (i=0;i<st->subframeSize;i++)
633                target[i]*=2.5;
634             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
635                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
636                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
637             for (i=0;i<st->subframeSize;i++)
638                innov2[i]*=scale*(1/2.5);
639             for (i=0;i<st->subframeSize;i++)
640                exc[i] += innov2[i];
641          }
642
643
644          if (0) {
645             float en=0;
646             for (i=0;i<st->subframeSize;i++)
647                en+=exc[i]*exc[i];
648             en=sqrt(eh/(1+en));
649             for (i=0;i<st->subframeSize;i++)
650                exc[i]*=en;
651             printf ("correction high: %f\n", en);
652          }
653
654       }
655
656          /*Keep the previous memory*/
657          for (i=0;i<st->lpcSize;i++)
658             mem[i]=st->mem_sp[i];
659          /* Final signal synthesis from excitation */
660          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
661          
662          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
663          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
664    }
665
666
667 #ifndef RELEASE
668
669    /* Reconstruct the original */
670    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
671    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
672
673    for (i=0;i<st->full_frame_size;i++)
674       in[i]=2*(st->y0[i]-st->y1[i]);
675 #endif
676    for (i=0;i<st->lpcSize;i++)
677       st->old_lsp[i] = st->lsp[i];
678    for (i=0;i<st->lpcSize;i++)
679       st->old_qlsp[i] = st->qlsp[i];
680
681    st->first=0;
682 }
683
684
685
686
687
688 void *sb_decoder_init(SpeexMode *m)
689 {
690    SBDecState *st;
691    SpeexSBMode *mode;
692    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
693    st->mode = m;
694    mode=(SpeexSBMode*)m->mode;
695
696    st->st_low = speex_decoder_init(mode->nb_mode);
697    st->full_frame_size = 2*mode->frameSize;
698    st->frame_size = mode->frameSize;
699    st->subframeSize = mode->subframeSize;
700    st->nbSubframes = mode->frameSize/mode->subframeSize;
701    st->lpcSize=8;
702    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
703    st->sampling_rate*=2;
704
705    st->submodes=mode->submodes;
706    st->submodeID=mode->defaultSubmode;
707
708    st->first=1;
709    st->stack = speex_alloc(2000*sizeof(float));
710
711    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
712    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
713    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
714    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
715    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
716
717    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
718    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
719    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
720    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
721
722    st->exc=(float*)speex_alloc(st->frame_size*sizeof(float));
723
724    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
725    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
726    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
727    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
728
729    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
730    st->mem_sp = (float*)speex_alloc(2*st->lpcSize*sizeof(float));
731    
732    st->lpc_enh_enabled=0;
733
734    return st;
735 }
736
737 void sb_decoder_destroy(void *state)
738 {
739    SBDecState *st;
740    st = (SBDecState*)state;
741    speex_decoder_destroy(st->st_low);
742    speex_free(st->x0d);
743    speex_free(st->x1d);
744    speex_free(st->high);
745    speex_free(st->y0);
746    speex_free(st->y1);
747    speex_free(st->h0_mem);
748    speex_free(st->h1_mem);
749    speex_free(st->g0_mem);
750    speex_free(st->g1_mem);
751    
752    speex_free(st->exc);
753    speex_free(st->qlsp);
754    speex_free(st->old_qlsp);
755    speex_free(st->interp_qlsp);
756    speex_free(st->interp_qlpc);
757    speex_free(st->pi_gain);
758
759    speex_free(st->mem_sp);
760
761    speex_free(st->stack);
762
763    speex_free(state);
764 }
765
766 static void sb_decode_lost(SBDecState *st, float *out, void *stack)
767 {
768    int i;
769    float *awk1, *awk2, *awk3;
770    for (i=0;i<st->frame_size;i++)
771       st->exc[i]*=0.8;
772    
773    st->first=1;
774    
775    awk1=PUSH(stack, st->lpcSize+1, float);
776    awk2=PUSH(stack, st->lpcSize+1, float);
777    awk3=PUSH(stack, st->lpcSize+1, float);
778    
779    if (st->lpc_enh_enabled)
780    {
781       float r=.9;
782       
783       float k1,k2,k3;
784       k1=SUBMODE(lpc_enh_k1);
785       k2=SUBMODE(lpc_enh_k2);
786       k3=(1-(1-r*k1)/(1-r*k2))/r;
787       k3=k1-k2;
788       if (!st->lpc_enh_enabled)
789       {
790          k1=k2;
791          k3=0;
792       }
793       bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
794       bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
795       bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
796       /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
797    }
798    
799    
800    /* Final signal synthesis from excitation */
801    for (i=0;i<st->frame_size;i++)
802       st->exc[i] *= .9;
803    for (i=0;i<st->frame_size;i++)
804       st->high[i]=st->exc[i];
805    if (st->lpc_enh_enabled)
806    {
807       /* Use enhanced LPC filter */
808       filter_mem2(st->high, awk2, awk1, st->high, st->frame_size, st->lpcSize, 
809                   st->mem_sp+st->lpcSize);
810       filter_mem2(st->high, awk3, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
811                   st->mem_sp);
812    } else {
813       /* Use regular filter */
814       for (i=0;i<st->lpcSize;i++)
815          st->mem_sp[st->lpcSize+i] = 0;
816       iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
817                st->mem_sp);
818    }
819    
820    /*iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);*/
821    
822    /* Reconstruct the original */
823    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
824    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
825
826    for (i=0;i<st->full_frame_size;i++)
827       out[i]=2*(st->y0[i]-st->y1[i]);
828    
829    return;
830 }
831
832 int sb_decode(void *state, SpeexBits *bits, float *out)
833 {
834    int i, sub;
835    SBDecState *st;
836    int wideband;
837    int ret;
838    void *stack;
839    float *low_pi_gain, *low_exc, *low_innov;
840    float *awk1, *awk2, *awk3;
841
842    st = (SBDecState*)state;
843    stack=st->stack;
844
845    /* Decode the low-band */
846    ret = speex_decode(st->st_low, bits, st->x0d);
847
848    /* If error decoding the narrowband part, propagate error */
849    if (ret!=0)
850    {
851       return ret;
852    }
853
854    if (!bits)
855    {
856       sb_decode_lost(st, out, stack);
857       return 0;
858    }
859
860    /*Check "wideband bit"*/
861    wideband = speex_bits_peek(bits);
862    if (wideband)
863    {
864       /*Regular wideband frame, read the submode*/
865       wideband = speex_bits_unpack_unsigned(bits, 1);
866       st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
867    } else
868    {
869       /*Was a narrowband frame, set "null submode"*/
870       st->submodeID = 0;
871    }
872
873    for (i=0;i<st->frame_size;i++)
874       st->exc[i]=0;
875
876    /* If null mode (no transmission), just set a couple things to zero*/
877    if (st->submodes[st->submodeID] == NULL)
878    {
879       for (i=0;i<st->frame_size;i++)
880          st->exc[i]=0;
881
882       st->first=1;
883
884       /* Final signal synthesis from excitation */
885       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
886
887       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
888       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
889
890       for (i=0;i<st->full_frame_size;i++)
891          out[i]=2*(st->y0[i]-st->y1[i]);
892
893       return 0;
894
895    }
896
897    low_pi_gain = PUSH(stack, st->nbSubframes, float);
898    low_exc = PUSH(stack, st->frame_size, float);
899    low_innov = PUSH(stack, st->frame_size, float);
900    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
901    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
902    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
903
904    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
905    
906    if (st->first)
907    {
908       for (i=0;i<st->lpcSize;i++)
909          st->old_qlsp[i] = st->qlsp[i];
910    }
911    
912    awk1=PUSH(stack, st->lpcSize+1, float);
913    awk2=PUSH(stack, st->lpcSize+1, float);
914    awk3=PUSH(stack, st->lpcSize+1, float);
915
916    for (sub=0;sub<st->nbSubframes;sub++)
917    {
918       float *exc, *sp, tmp, filter_ratio, el=0;
919       int offset;
920       float rl=0,rh=0;
921       
922       offset = st->subframeSize*sub;
923       sp=st->high+offset;
924       exc=st->exc+offset;
925       
926       /* LSP interpolation */
927       tmp = (1.0 + sub)/st->nbSubframes;
928       for (i=0;i<st->lpcSize;i++)
929          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
930
931       /* LSPs to x-domain */
932       for (i=0;i<st->lpcSize;i++)
933          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
934
935       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
936
937       /* LSP to LPC */
938       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
939
940
941       if (st->lpc_enh_enabled)
942       {
943          float r=.9;
944          
945          float k1,k2,k3;
946          k1=SUBMODE(lpc_enh_k1);
947          k2=SUBMODE(lpc_enh_k2);
948          k3=(1-(1-r*k1)/(1-r*k2))/r;
949          k3=k1-k2;
950          if (!st->lpc_enh_enabled)
951          {
952             k1=k2;
953             k3=0;
954          }
955          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
956          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
957          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
958          /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
959       }
960
961
962       /* Calculate reponse ratio between the low and high filter in the middle
963          of the band (4000 Hz) */
964       
965          tmp=1;
966          st->pi_gain[sub]=0;
967          for (i=0;i<=st->lpcSize;i++)
968          {
969             rh += tmp*st->interp_qlpc[i];
970             tmp = -tmp;
971             st->pi_gain[sub]+=st->interp_qlpc[i];
972          }
973          rl = low_pi_gain[sub];
974          rl=1/(fabs(rl)+.01);
975          rh=1/(fabs(rh)+.01);
976          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
977       
978       
979       for (i=0;i<st->subframeSize;i++)
980          exc[i]=0;
981       if (!SUBMODE(innovation_unquant))
982       {
983          float g;
984          int quant;
985
986          for (i=0;i<st->subframeSize;i++)
987             el+=sqr(low_innov[offset+i]);
988          quant = speex_bits_unpack_unsigned(bits, 5);
989          g= exp(((float)quant-27)/8.0);
990          
991          /*printf ("unquant folding gain: %f\n", g);*/
992          g /= filter_ratio;
993          
994          /* High-band excitation using the low-band excitation and a gain */
995          for (i=0;i<st->subframeSize;i++)
996             exc[i]=.8*g*low_innov[offset+i];
997       } else {
998          float gc, scale;
999          int qgc = speex_bits_unpack_unsigned(bits, 4);
1000          for (i=0;i<st->subframeSize;i++)
1001             el+=sqr(low_exc[offset+i]);
1002
1003
1004          gc = exp((1/3.7)*qgc-2);
1005
1006          scale = gc*sqrt(1+el)/filter_ratio;
1007
1008
1009          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1010                                 bits, stack);
1011          for (i=0;i<st->subframeSize;i++)
1012             exc[i]*=scale;
1013
1014          if (SUBMODE(double_codebook)) {
1015             void *tmp_stack=stack;
1016             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1017             for (i=0;i<st->subframeSize;i++)
1018                innov2[i]=0;
1019             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1020                                 bits, tmp_stack);
1021             for (i=0;i<st->subframeSize;i++)
1022                innov2[i]*=scale*(1/2.5);
1023             for (i=0;i<st->subframeSize;i++)
1024                exc[i] += innov2[i];
1025          }
1026
1027       }
1028
1029       for (i=0;i<st->subframeSize;i++)
1030          sp[i]=exc[i];
1031       if (st->lpc_enh_enabled)
1032       {
1033          /* Use enhanced LPC filter */
1034          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1035                      st->mem_sp+st->lpcSize);
1036          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1037                      st->mem_sp);
1038       } else {
1039          /* Use regular filter */
1040          for (i=0;i<st->lpcSize;i++)
1041             st->mem_sp[st->lpcSize+i] = 0;
1042          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1043                      st->mem_sp);
1044       }
1045       /*iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);*/
1046
1047    }
1048
1049    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1050    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1051
1052    for (i=0;i<st->full_frame_size;i++)
1053       out[i]=2*(st->y0[i]-st->y1[i]);
1054
1055    for (i=0;i<st->lpcSize;i++)
1056       st->old_qlsp[i] = st->qlsp[i];
1057
1058    st->first=0;
1059
1060    return 0;
1061 }
1062
1063
1064 void sb_encoder_ctl(void *state, int request, void *ptr)
1065 {
1066    SBEncState *st;
1067    st=(SBEncState*)state;
1068    switch(request)
1069    {
1070    case SPEEX_GET_FRAME_SIZE:
1071       (*(int*)ptr) = st->full_frame_size;
1072       break;
1073    case SPEEX_SET_HIGH_MODE:
1074       st->submodeSelect = st->submodeID = (*(int*)ptr);
1075       break;
1076    case SPEEX_SET_LOW_MODE:
1077       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, ptr);
1078       break;
1079    case SPEEX_SET_MODE:
1080       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1081       break;
1082    case SPEEX_SET_VBR:
1083       st->vbr_enabled = (*(int*)ptr);
1084       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1085       break;
1086    case SPEEX_GET_VBR:
1087       (*(int*)ptr) = st->vbr_enabled;
1088       break;
1089    case SPEEX_SET_VAD:
1090       st->vad_enabled = (*(int*)ptr);
1091       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1092       break;
1093    case SPEEX_GET_VAD:
1094       (*(int*)ptr) = st->vad_enabled;
1095       break;
1096    case SPEEX_SET_VBR_QUALITY:
1097       {
1098          int q;
1099          float qual = (*(float*)ptr)+.5;
1100          st->vbr_quality = (*(float*)ptr);
1101          if (qual>10)
1102             qual=10;
1103          q=(int)floor(.5+*(float*)ptr);
1104          if (q>10)
1105             q=10;
1106          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1107          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1108          break;
1109       }
1110    case SPEEX_SET_QUALITY:
1111       {
1112          int nb_qual;
1113          int quality = (*(int*)ptr);
1114          if (quality < 0)
1115             quality = 0;
1116          if (quality > 10)
1117             quality = 10;
1118          st->submodeSelect = st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1119          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1120          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1121       }
1122       break;
1123    case SPEEX_SET_COMPLEXITY:
1124       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1125       st->complexity = (*(int*)ptr);
1126       if (st->complexity<1)
1127          st->complexity=1;
1128       break;
1129    case SPEEX_GET_COMPLEXITY:
1130       (*(int*)ptr) = st->complexity;
1131       break;
1132    case SPEEX_SET_BITRATE:
1133       {
1134          int i=10, rate, target;
1135          target = (*(int*)ptr);
1136          while (i>=0)
1137          {
1138             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1139             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1140             if (rate <= target)
1141                break;
1142             i--;
1143          }
1144       }
1145       break;
1146    case SPEEX_GET_BITRATE:
1147       speex_encoder_ctl(st->st_low, request, ptr);
1148       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1149       if (st->submodes[st->submodeID])
1150          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1151       else
1152          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1153       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1154       break;
1155    case SPEEX_SET_SAMPLING_RATE:
1156       {
1157          int tmp=(*(int*)ptr);
1158          st->sampling_rate = tmp;
1159          tmp>>=1;
1160          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1161       }
1162       break;
1163    case SPEEX_GET_SAMPLING_RATE:
1164       (*(int*)ptr)=st->sampling_rate;
1165       break;
1166    case SPEEX_RESET_STATE:
1167       {
1168          int i;
1169          st->first = 1;
1170          for (i=0;i<st->lpcSize;i++)
1171             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1172          for (i=0;i<st->lpcSize;i++)
1173             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1174          for (i=0;i<st->bufSize;i++)
1175             st->excBuf[i]=0;
1176          for (i=0;i<QMF_ORDER;i++)
1177             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1178       }
1179       break;
1180    case SPEEX_GET_PI_GAIN:
1181       {
1182          int i;
1183          float *g = (float*)ptr;
1184          for (i=0;i<st->nbSubframes;i++)
1185             g[i]=st->pi_gain[i];
1186       }
1187       break;
1188    case SPEEX_GET_EXC:
1189       {
1190          int i;
1191          float *e = (float*)ptr;
1192          for (i=0;i<st->full_frame_size;i++)
1193             e[i]=0;
1194          for (i=0;i<st->frame_size;i++)
1195             e[2*i]=2*st->exc[i];
1196       }
1197       break;
1198    case SPEEX_GET_INNOV:
1199       {
1200          int i;
1201          float *e = (float*)ptr;
1202          for (i=0;i<st->full_frame_size;i++)
1203             e[i]=0;
1204          for (i=0;i<st->frame_size;i++)
1205             e[2*i]=2*st->exc[i];
1206       }
1207       break;
1208    case SPEEX_GET_RELATIVE_QUALITY:
1209       (*(float*)ptr)=st->relative_quality;
1210       break;
1211    default:
1212       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1213    }
1214
1215 }
1216
1217 void sb_decoder_ctl(void *state, int request, void *ptr)
1218 {
1219    SBDecState *st;
1220    st=(SBDecState*)state;
1221    switch(request)
1222    {
1223    case SPEEX_GET_FRAME_SIZE:
1224       (*(int*)ptr) = st->full_frame_size;
1225       break;
1226    case SPEEX_SET_ENH:
1227       speex_decoder_ctl(st->st_low, request, ptr);
1228       st->lpc_enh_enabled = *((int*)ptr);
1229       break;
1230    case SPEEX_GET_BITRATE:
1231       speex_decoder_ctl(st->st_low, request, ptr);
1232       if (st->submodes[st->submodeID])
1233          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1234       else
1235          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1236       break;
1237    case SPEEX_SET_SAMPLING_RATE:
1238       {
1239          int tmp=(*(int*)ptr);
1240          st->sampling_rate = tmp;
1241          tmp>>=1;
1242          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1243       }
1244       break;
1245    case SPEEX_GET_SAMPLING_RATE:
1246       (*(int*)ptr)=st->sampling_rate;
1247       break;
1248    case SPEEX_SET_HANDLER:
1249       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1250       break;
1251    case SPEEX_SET_USER_HANDLER:
1252       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1253       break;
1254    case SPEEX_RESET_STATE:
1255       {
1256          int i;
1257          for (i=0;i<2*st->lpcSize;i++)
1258             st->mem_sp[i]=0;
1259          for (i=0;i<QMF_ORDER;i++)
1260             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1261       }
1262       break;
1263    case SPEEX_GET_PI_GAIN:
1264       {
1265          int i;
1266          float *g = (float*)ptr;
1267          for (i=0;i<st->nbSubframes;i++)
1268             g[i]=st->pi_gain[i];
1269       }
1270       break;
1271    case SPEEX_GET_EXC:
1272       {
1273          int i;
1274          float *e = (float*)ptr;
1275          for (i=0;i<st->full_frame_size;i++)
1276             e[i]=0;
1277          for (i=0;i<st->frame_size;i++)
1278             e[2*i]=2*st->exc[i];
1279       }
1280       break;
1281    case SPEEX_GET_INNOV:
1282       {
1283          int i;
1284          float *e = (float*)ptr;
1285          for (i=0;i<st->full_frame_size;i++)
1286             e[i]=0;
1287          for (i=0;i<st->frame_size;i++)
1288             e[2*i]=2*st->exc[i];
1289       }
1290       break;
1291    default:
1292       fprintf(stderr, "Unknown sb_ctl request: %d\n", request);
1293    }
1294
1295 }