fixed-point: LSP quantization work, also LSP's are now in the angle domain
[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 <math.h>
34 #include "sb_celp.h"
35 #include "stdlib.h"
36 #include "filters.h"
37 #include "lpc.h"
38 #include "lsp.h"
39 #include "stack_alloc.h"
40 #include "cb_search.h"
41 #include "quant_lsp.h"
42 #include "vq.h"
43 #include "ltp.h"
44 #include "misc.h"
45
46 #include <stdio.h>
47
48 #ifndef M_PI
49 #define M_PI           3.14159265358979323846  /* pi */
50 #endif
51
52 #define sqr(x) ((x)*(x))
53
54 #define SUBMODE(x) st->submodes[st->submodeID]->x
55
56 #define QMF_ORDER 64
57 static float h0[64] = {
58    3.596189e-05, -0.0001123515,
59    -0.0001104587, 0.0002790277,
60    0.0002298438, -0.0005953563,
61    -0.0003823631, 0.00113826,
62    0.0005308539, -0.001986177,
63    -0.0006243724, 0.003235877,
64    0.0005743159, -0.004989147,
65    -0.0002584767, 0.007367171,
66    -0.0004857935, -0.01050689,
67    0.001894714, 0.01459396,
68    -0.004313674, -0.01994365,
69    0.00828756, 0.02716055,
70    -0.01485397, -0.03764973,
71    0.026447, 0.05543245,
72    -0.05095487, -0.09779096,
73    0.1382363, 0.4600981,
74    0.4600981, 0.1382363,
75    -0.09779096, -0.05095487,
76    0.05543245, 0.026447,
77    -0.03764973, -0.01485397,
78    0.02716055, 0.00828756,
79    -0.01994365, -0.004313674,
80    0.01459396, 0.001894714,
81    -0.01050689, -0.0004857935,
82    0.007367171, -0.0002584767,
83    -0.004989147, 0.0005743159,
84    0.003235877, -0.0006243724,
85    -0.001986177, 0.0005308539,
86    0.00113826, -0.0003823631,
87    -0.0005953563, 0.0002298438,
88    0.0002790277, -0.0001104587,
89    -0.0001123515, 3.596189e-05
90 };
91
92 static float h1[64] = {
93    3.596189e-05, 0.0001123515,
94    -0.0001104587, -0.0002790277,
95    0.0002298438, 0.0005953563,
96    -0.0003823631, -0.00113826,
97    0.0005308539, 0.001986177,
98    -0.0006243724, -0.003235877,
99    0.0005743159, 0.004989147,
100    -0.0002584767, -0.007367171,
101    -0.0004857935, 0.01050689,
102    0.001894714, -0.01459396,
103    -0.004313674, 0.01994365,
104    0.00828756, -0.02716055,
105    -0.01485397, 0.03764973,
106    0.026447, -0.05543245,
107    -0.05095487, 0.09779096,
108    0.1382363, -0.4600981,
109    0.4600981, -0.1382363,
110    -0.09779096, 0.05095487,
111    0.05543245, -0.026447,
112    -0.03764973, 0.01485397,
113    0.02716055, -0.00828756,
114    -0.01994365, 0.004313674,
115    0.01459396, -0.001894714,
116    -0.01050689, 0.0004857935,
117    0.007367171, 0.0002584767,
118    -0.004989147, -0.0005743159,
119    0.003235877, 0.0006243724,
120    -0.001986177, -0.0005308539,
121    0.00113826, 0.0003823631,
122    -0.0005953563, -0.0002298438,
123    0.0002790277, 0.0001104587,
124    -0.0001123515, -3.596189e-05
125 };
126
127 void *sb_encoder_init(SpeexMode *m)
128 {
129    int i;
130    SBEncState *st;
131    SpeexSBMode *mode;
132
133    st = (SBEncState*)speex_alloc(sizeof(SBEncState)+8000*sizeof(spx_sig_t));
134    st->mode = m;
135    mode = (SpeexSBMode*)m->mode;
136
137    st->stack = ((char*)st) + sizeof(SBEncState);
138
139    st->st_low = speex_encoder_init(mode->nb_mode);
140    st->full_frame_size = 2*mode->frameSize;
141    st->frame_size = mode->frameSize;
142    st->subframeSize = mode->subframeSize;
143    st->nbSubframes = mode->frameSize/mode->subframeSize;
144    st->windowSize = st->frame_size*3/2;
145    st->lpcSize=mode->lpcSize;
146    st->bufSize=mode->bufSize;
147
148    st->encode_submode = 1;
149    st->submodes=mode->submodes;
150    st->submodeSelect = st->submodeID=mode->defaultSubmode;
151    
152    i=9;
153    speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &i);
154
155    st->lag_factor = mode->lag_factor;
156    st->lpc_floor = mode->lpc_floor;
157    st->gamma1=mode->gamma1;
158    st->gamma2=mode->gamma2;
159    st->first=1;
160
161    st->x0d=PUSH(st->stack, st->frame_size, spx_sig_t);
162    st->x1d=PUSH(st->stack, st->frame_size, spx_sig_t);
163    st->high=PUSH(st->stack, st->full_frame_size, spx_sig_t);
164    st->y0=PUSH(st->stack, st->full_frame_size, spx_sig_t);
165    st->y1=PUSH(st->stack, st->full_frame_size, spx_sig_t);
166
167    st->h0_mem=PUSH(st->stack, QMF_ORDER, float);
168    st->h1_mem=PUSH(st->stack, QMF_ORDER, float);
169    st->g0_mem=PUSH(st->stack, QMF_ORDER, float);
170    st->g1_mem=PUSH(st->stack, QMF_ORDER, float);
171
172    st->buf=PUSH(st->stack, st->windowSize, spx_sig_t);
173    st->excBuf=PUSH(st->stack, st->bufSize, spx_sig_t);
174    st->exc = st->excBuf + st->bufSize - st->windowSize;
175
176    st->res=PUSH(st->stack, st->frame_size, spx_sig_t);
177    st->sw=PUSH(st->stack, st->frame_size, spx_sig_t);
178    st->target=PUSH(st->stack, st->frame_size, spx_sig_t);
179    /*Asymmetric "pseudo-Hamming" window*/
180    {
181       int part1, part2;
182       part1 = st->subframeSize*7/2;
183       part2 = st->subframeSize*5/2;
184       st->window = PUSH(st->stack, st->windowSize, spx_word16_t);
185       for (i=0;i<part1;i++)
186          st->window[i]=(spx_word16_t)(SIG_SCALING*(.54-.46*cos(M_PI*i/part1)));
187       for (i=0;i<part2;i++)
188          st->window[part1+i]=(spx_word16_t)(SIG_SCALING*(.54+.46*cos(M_PI*i/part2)));
189    }
190
191    st->lagWindow = PUSH(st->stack, st->lpcSize+1, float);
192    for (i=0;i<st->lpcSize+1;i++)
193       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
194
195    st->autocorr = PUSH(st->stack, st->lpcSize+1, spx_word16_t);
196    st->lpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
197    st->bw_lpc1 = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
198    st->bw_lpc2 = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
199    st->lsp = PUSH(st->stack, st->lpcSize, float);
200    st->qlsp = PUSH(st->stack, st->lpcSize, float);
201    st->old_lsp = PUSH(st->stack, st->lpcSize, float);
202    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
203    st->interp_lsp = PUSH(st->stack, st->lpcSize, float);
204    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
205    st->interp_lpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
206    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
207    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
208
209    st->mem_sp = PUSH(st->stack, st->lpcSize, spx_mem_t);
210    st->mem_sp2 = PUSH(st->stack, st->lpcSize, spx_mem_t);
211    st->mem_sw = PUSH(st->stack, st->lpcSize, spx_mem_t);
212
213    st->vbr_quality = 8;
214    st->vbr_enabled = 0;
215    st->vad_enabled = 0;
216    st->abr_enabled = 0;
217    st->relative_quality=0;
218
219    st->complexity=2;
220    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
221    st->sampling_rate*=2;
222
223    return st;
224 }
225
226 void sb_encoder_destroy(void *state)
227 {
228    SBEncState *st=(SBEncState*)state;
229
230    speex_encoder_destroy(st->st_low);
231
232    speex_free(st);
233 }
234
235
236 int sb_encode(void *state, short *in, SpeexBits *bits)
237 {
238    SBEncState *st;
239    int i, roots, sub;
240    char *stack;
241    spx_mem_t *mem;
242    spx_sig_t *innov, *syn_resp;
243    float *low_pi_gain;
244    spx_sig_t *low_exc, *low_innov;
245    SpeexSBMode *mode;
246    int dtx;
247
248    st = (SBEncState*)state;
249    stack=st->stack;
250    mode = (SpeexSBMode*)(st->mode->mode);
251
252    {
253       float *sig_in;
254       short *low = PUSH(stack, st->frame_size, short);
255
256       sig_in = PUSH(stack, st->full_frame_size, float);
257       for (i=0;i<st->full_frame_size;i++)
258          sig_in[i] = in[i];
259
260       /* Compute the two sub-bands by filtering with h0 and h1*/
261       qmf_decomp(sig_in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
262       
263       for (i=0;i<st->frame_size;i++)
264          low[i] = st->x0d[i];
265       
266       /* Encode the narrowband part*/
267       speex_encode(st->st_low, low, bits);
268
269       for (i=0;i<st->frame_size;i++)
270          st->x0d[i] = low[i];
271    }
272
273    for (i=0;i<st->frame_size;i++)
274       st->x0d[i] *= SIG_SCALING;
275    for (i=0;i<st->frame_size;i++)
276       st->x1d[i] *= SIG_SCALING;
277
278    /* High-band buffering / sync with low band */
279    for (i=0;i<st->windowSize-st->frame_size;i++)
280       st->high[i] = st->high[st->frame_size+i];
281    for (i=0;i<st->frame_size;i++)
282       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
283
284    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(spx_sig_t));
285
286
287    low_pi_gain = PUSH(stack, st->nbSubframes, float);
288    low_exc = PUSH(stack, st->frame_size, spx_sig_t);
289    low_innov = PUSH(stack, st->frame_size, spx_sig_t);
290    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
291    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
292    speex_encoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
293    
294    speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
295
296    if (dtx==0)
297       dtx=1;
298    else
299       dtx=0;
300
301    {
302       spx_word16_t *w_sig;
303       w_sig = PUSH(stack, st->windowSize, spx_word16_t);
304       /* Window for analysis */
305       for (i=0;i<st->windowSize;i++)
306          w_sig[i] = SHR(MULT16_16(SHR((spx_word32_t)(st->high[i]),SIG_SHIFT),st->window[i]),SIG_SHIFT);
307
308       /* Compute auto-correlation */
309       _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize);
310    }
311
312    st->autocorr[0] = (spx_word16_t)(st->autocorr[0]*st->lpc_floor); /* Noise floor in auto-correlation domain */
313    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
314    for (i=0;i<st->lpcSize+1;i++)
315       st->autocorr[i] = (spx_word16_t)(st->autocorr[i]*st->lagWindow[i]);
316
317    /* Levinson-Durbin */
318    _spx_lpc(st->lpc+1, st->autocorr, st->lpcSize);
319    st->lpc[0] = (spx_coef_t)LPC_SCALING;
320
321    /* LPC to LSPs (x-domain) transform */
322    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
323    if (roots!=st->lpcSize)
324    {
325       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, stack);
326       if (roots!=st->lpcSize) {
327          /*If we can't find all LSP's, do some damage control and use a flat filter*/
328          for (i=0;i<st->lpcSize;i++)
329          {
330             st->lsp[i]=M_PI*((float)(i+1))/(st->lpcSize+1);
331          }
332       }
333    }
334
335    /* VBR code */
336    if ((st->vbr_enabled || st->vad_enabled) && !dtx)
337    {
338       float e_low=0, e_high=0;
339       float ratio;
340       if (st->abr_enabled)
341       {
342          float qual_change=0;
343          if (st->abr_drift2 * st->abr_drift > 0)
344          {
345             /* Only adapt if long-term and short-term drift are the same sign */
346             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
347             if (qual_change>.1)
348                qual_change=.1;
349             if (qual_change<-.1)
350                qual_change=-.1;
351          }
352          st->vbr_quality += qual_change;
353          if (st->vbr_quality>10)
354             st->vbr_quality=10;
355          if (st->vbr_quality<0)
356             st->vbr_quality=0;
357       }
358
359
360       /*FIXME: Are the two signals (low, high) in sync? */
361       e_low = compute_rms(st->x0d, st->frame_size);
362       e_high = compute_rms(st->high, st->frame_size);
363       ratio = 2*log((1+e_high)/(1+e_low));
364       
365       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &st->relative_quality);
366       if (ratio<-4)
367          ratio=-4;
368       if (ratio>2)
369          ratio=2;
370       /*if (ratio>-2)*/
371       if (st->vbr_enabled) 
372       {
373          int modeid;
374          modeid = mode->nb_modes-1;
375          st->relative_quality+=1.0*(ratio+2);
376          if (st->relative_quality<-1)
377             st->relative_quality=-1;
378          while (modeid)
379          {
380             int v1;
381             float thresh;
382             v1=(int)floor(st->vbr_quality);
383             if (v1==10)
384                thresh = mode->vbr_thresh[modeid][v1];
385             else
386                thresh = (st->vbr_quality-v1)   * mode->vbr_thresh[modeid][v1+1] + 
387                         (1+v1-st->vbr_quality) * mode->vbr_thresh[modeid][v1];
388             if (st->relative_quality >= thresh)
389                break;
390             modeid--;
391          }
392          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &modeid);
393          if (st->abr_enabled)
394          {
395             int bitrate;
396             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
397             st->abr_drift+=(bitrate-st->abr_enabled);
398             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
399             st->abr_count += 1.0;
400          }
401
402       } else {
403          /* VAD only */
404          int modeid;
405          if (st->relative_quality<2.0)
406             modeid=1;
407          else
408             modeid=st->submodeSelect;
409          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
410          st->submodeID=modeid;
411
412       }
413       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
414    }
415
416    if (st->encode_submode)
417    {
418       speex_bits_pack(bits, 1, 1);
419       if (dtx)
420          speex_bits_pack(bits, 0, SB_SUBMODE_BITS);
421       else
422          speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
423    }
424
425    /* If null mode (no transmission), just set a couple things to zero*/
426    if (dtx || st->submodes[st->submodeID] == NULL)
427    {
428       for (i=0;i<st->frame_size;i++)
429          st->exc[i]=st->sw[i]=0;
430
431       for (i=0;i<st->lpcSize;i++)
432          st->mem_sw[i]=0;
433       st->first=1;
434
435       /* Final signal synthesis from excitation */
436       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
437
438 #ifndef RELEASE
439
440       /* Reconstruct the original */
441       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
442       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
443
444       for (i=0;i<st->full_frame_size;i++)
445          in[i]=2*(st->y0[i]-st->y1[i]) / SIG_SCALING;
446 #endif
447
448       if (dtx)
449          return 0;
450       else
451          return 1;
452    }
453
454
455    /* LSP quantization */
456    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);   
457
458    if (st->first)
459    {
460       for (i=0;i<st->lpcSize;i++)
461          st->old_lsp[i] = st->lsp[i];
462       for (i=0;i<st->lpcSize;i++)
463          st->old_qlsp[i] = st->qlsp[i];
464    }
465    
466    mem=PUSH(stack, st->lpcSize, spx_mem_t);
467    syn_resp=PUSH(stack, st->subframeSize, spx_sig_t);
468    innov = PUSH(stack, st->subframeSize, spx_sig_t);
469
470    for (sub=0;sub<st->nbSubframes;sub++)
471    {
472       spx_sig_t *exc, *sp, *res, *target, *sw;
473       float tmp, filter_ratio;
474       int offset;
475       float rl, rh, eh=0, el=0;
476       int fold;
477
478       offset = st->subframeSize*sub;
479       sp=st->high+offset;
480       exc=st->exc+offset;
481       res=st->res+offset;
482       target=st->target+offset;
483       sw=st->sw+offset;
484       
485       /* LSP interpolation (quantized and unquantized) */
486       tmp = (1.0 + sub)/st->nbSubframes;
487       for (i=0;i<st->lpcSize;i++)
488          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
489       for (i=0;i<st->lpcSize;i++)
490          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
491       
492       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .05);
493       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .05);
494
495       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
496       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
497
498       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
499       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
500
501       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
502          filters */
503       rl=rh=0;
504       tmp=1;
505       st->pi_gain[sub]=0;
506       for (i=0;i<=st->lpcSize;i++)
507       {
508          rh += tmp*st->interp_qlpc[i];
509          tmp = -tmp;
510          st->pi_gain[sub]+=st->interp_qlpc[i];
511       }
512       rh /= LPC_SCALING;
513       st->pi_gain[sub] /= LPC_SCALING;
514
515       rl = low_pi_gain[sub];
516       rl=1/(fabs(rl)+.01);
517       rh=1/(fabs(rh)+.01);
518       /* Compute ratio, will help predict the gain */
519       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
520
521       fold = filter_ratio<5;
522       /*printf ("filter_ratio %f\n", filter_ratio);*/
523       fold=0;
524
525       /* Compute "real excitation" */
526       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
527       /* Compute energy of low-band and high-band excitation */
528
529       eh = compute_rms(exc, st->subframeSize);
530
531       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
532          float g;
533
534          el = compute_rms(low_innov+offset, st->subframeSize);
535
536          /* Gain to use if we want to use the low-band excitation for high-band */
537          g=eh/(.01+el);
538
539          g *= filter_ratio;
540          /*print_vec(&g, 1, "gain factor");*/
541          /* Gain quantization */
542          {
543             int quant = (int) floor(.5 + 10 + 8.0 * log((g+.0001)));
544             /*speex_warning_int("tata", quant);*/
545             if (quant<0)
546                quant=0;
547             if (quant>31)
548                quant=31;
549             speex_bits_pack(bits, quant, 5);
550             g= .1*exp(quant/9.4);
551          }
552          /*printf ("folding gain: %f\n", g);*/
553          g /= filter_ratio;
554
555       } else {
556          float gc, scale, scale_1;
557
558          el = compute_rms(low_exc+offset, st->subframeSize);
559          /*FIXME: cleanup the "historical" mess with sqrt(st->subframeSize) */
560          gc = (1+eh)*filter_ratio/(1+el)/sqrt(st->subframeSize);
561
562          {
563             int qgc = (int)floor(.5+3.7*(log(gc)+2));
564             if (qgc<0)
565                qgc=0;
566             if (qgc>15)
567                qgc=15;
568             speex_bits_pack(bits, qgc, 4);
569             gc = exp((1/3.7)*qgc-2);
570          }
571
572          scale = gc*(1+el*sqrt(st->subframeSize))/filter_ratio;
573          scale_1 = 1/scale;
574
575          for (i=0;i<st->subframeSize;i++)
576             exc[i]=0;
577          exc[0]=SIG_SCALING;
578          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
579          
580          /* Reset excitation */
581          for (i=0;i<st->subframeSize;i++)
582             exc[i]=0;
583          
584          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
585          for (i=0;i<st->lpcSize;i++)
586             mem[i]=st->mem_sp[i];
587          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
588
589          for (i=0;i<st->lpcSize;i++)
590             mem[i]=st->mem_sw[i];
591          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
592
593          /* Compute weighted signal */
594          for (i=0;i<st->lpcSize;i++)
595             mem[i]=st->mem_sw[i];
596          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
597
598          /* Compute target signal */
599          for (i=0;i<st->subframeSize;i++)
600             target[i]=sw[i]-res[i];
601
602          for (i=0;i<st->subframeSize;i++)
603            exc[i]=0;
604
605
606          for (i=0;i<st->subframeSize;i++)
607             target[i]*=scale_1;
608
609          /* Reset excitation */
610          for (i=0;i<st->subframeSize;i++)
611             innov[i]=0;
612
613          /*print_vec(target, st->subframeSize, "\ntarget");*/
614          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
615                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
616                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
617          /*print_vec(target, st->subframeSize, "after");*/
618
619          for (i=0;i<st->subframeSize;i++)
620             exc[i] += innov[i]*scale;
621
622          if (SUBMODE(double_codebook)) {
623             char *tmp_stack=stack;
624             spx_sig_t *innov2 = PUSH(tmp_stack, st->subframeSize, spx_sig_t);
625             for (i=0;i<st->subframeSize;i++)
626                innov2[i]=0;
627             for (i=0;i<st->subframeSize;i++)
628                target[i]*=2.5;
629             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
630                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
631                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
632             for (i=0;i<st->subframeSize;i++)
633                innov2[i]*=scale*(1/2.5);
634             for (i=0;i<st->subframeSize;i++)
635                exc[i] += innov2[i];
636          }
637
638       }
639
640          /*Keep the previous memory*/
641          for (i=0;i<st->lpcSize;i++)
642             mem[i]=st->mem_sp[i];
643          /* Final signal synthesis from excitation */
644          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
645          
646          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
647          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
648    }
649
650
651 #ifndef RELEASE
652
653    /* Reconstruct the original */
654    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
655    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
656
657    for (i=0;i<st->full_frame_size;i++)
658       in[i]=2*(st->y0[i]-st->y1[i]) / SIG_SCALING;
659 #endif
660    for (i=0;i<st->lpcSize;i++)
661       st->old_lsp[i] = st->lsp[i];
662    for (i=0;i<st->lpcSize;i++)
663       st->old_qlsp[i] = st->qlsp[i];
664
665    st->first=0;
666
667    return 1;
668 }
669
670
671
672
673
674 void *sb_decoder_init(SpeexMode *m)
675 {
676    SBDecState *st;
677    SpeexSBMode *mode;
678    st = (SBDecState*)speex_alloc(sizeof(SBDecState)+6000*sizeof(spx_sig_t));
679    st->mode = m;
680    mode=(SpeexSBMode*)m->mode;
681
682    st->encode_submode = 1;
683
684    st->stack = ((char*)st) + sizeof(SBDecState);
685
686
687
688    st->st_low = speex_decoder_init(mode->nb_mode);
689    st->full_frame_size = 2*mode->frameSize;
690    st->frame_size = mode->frameSize;
691    st->subframeSize = mode->subframeSize;
692    st->nbSubframes = mode->frameSize/mode->subframeSize;
693    st->lpcSize=mode->lpcSize;
694    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
695    st->sampling_rate*=2;
696
697    st->submodes=mode->submodes;
698    st->submodeID=mode->defaultSubmode;
699
700    st->first=1;
701
702
703    st->x0d=PUSH(st->stack, st->frame_size, spx_sig_t);
704    st->x1d=PUSH(st->stack, st->frame_size, spx_sig_t);
705    st->high=PUSH(st->stack, st->full_frame_size, spx_sig_t);
706    st->y0=PUSH(st->stack, st->full_frame_size, spx_sig_t);
707    st->y1=PUSH(st->stack, st->full_frame_size, spx_sig_t);
708
709    st->h0_mem=PUSH(st->stack, QMF_ORDER, float);
710    st->h1_mem=PUSH(st->stack, QMF_ORDER, float);
711    st->g0_mem=PUSH(st->stack, QMF_ORDER, float);
712    st->g1_mem=PUSH(st->stack, QMF_ORDER, float);
713
714    st->exc=PUSH(st->stack, st->frame_size, spx_sig_t);
715
716    st->qlsp = PUSH(st->stack, st->lpcSize, float);
717    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
718    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
719    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
720
721    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
722    st->mem_sp = PUSH(st->stack, 2*st->lpcSize, spx_mem_t);
723    
724    st->lpc_enh_enabled=0;
725
726    return st;
727 }
728
729 void sb_decoder_destroy(void *state)
730 {
731    SBDecState *st;
732    st = (SBDecState*)state;
733    speex_decoder_destroy(st->st_low);
734
735    speex_free(state);
736 }
737
738 static void sb_decode_lost(SBDecState *st, short *out, int dtx, char *stack)
739 {
740    int i;
741    spx_coef_t *awk1, *awk2, *awk3;
742    int saved_modeid=0;
743
744    if (dtx)
745    {
746       saved_modeid=st->submodeID;
747       st->submodeID=1;
748    } else {
749       bw_lpc(0.99, st->interp_qlpc, st->interp_qlpc, st->lpcSize);
750    }
751
752    st->first=1;
753    
754    awk1=PUSH(stack, st->lpcSize+1, spx_coef_t);
755    awk2=PUSH(stack, st->lpcSize+1, spx_coef_t);
756    awk3=PUSH(stack, st->lpcSize+1, spx_coef_t);
757    
758    if (st->lpc_enh_enabled)
759    {
760       float r=.9;
761       
762       float k1,k2,k3;
763       if (st->submodes[st->submodeID] != NULL)
764       {
765          k1=SUBMODE(lpc_enh_k1);
766          k2=SUBMODE(lpc_enh_k2);
767       } else {
768          k1=k2=.7;
769       }
770       k3=(1-(1-r*k1)/(1-r*k2))/r;
771       k3=k1-k2;
772       if (!st->lpc_enh_enabled)
773       {
774          k1=k2;
775          k3=0;
776       }
777       bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
778       bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
779       bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
780       /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
781    }
782    
783    
784    /* Final signal synthesis from excitation */
785    if (!dtx)
786    {
787       for (i=0;i<st->frame_size;i++)
788          st->exc[i] *= .9;
789    }
790
791    for (i=0;i<st->frame_size;i++)
792       st->high[i]=st->exc[i];
793
794    if (st->lpc_enh_enabled)
795    {
796       /* Use enhanced LPC filter */
797       filter_mem2(st->high, awk2, awk1, st->high, st->frame_size, st->lpcSize, 
798                   st->mem_sp+st->lpcSize);
799       filter_mem2(st->high, awk3, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
800                   st->mem_sp);
801    } else {
802       /* Use regular filter */
803       for (i=0;i<st->lpcSize;i++)
804          st->mem_sp[st->lpcSize+i] = 0;
805       iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
806                st->mem_sp);
807    }
808    
809    /*iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);*/
810    
811    /* Reconstruct the original */
812    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
813    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
814
815 #ifdef FIXED_POINT
816    for (i=0;i<st->full_frame_size;i++)
817       out[i]=SHR(st->y0[i]-st->y1[i],SIG_SHIFT-1);
818 #else
819    for (i=0;i<st->full_frame_size;i++)
820       out[i]=2*(st->y0[i]-st->y1[i]);   
821 #endif
822
823    if (dtx)
824    {
825       st->submodeID=saved_modeid;
826    }
827
828    return;
829 }
830
831 int sb_decode(void *state, SpeexBits *bits, short *out)
832 {
833    int i, sub;
834    SBDecState *st;
835    int wideband;
836    int ret;
837    char *stack;
838    float *low_pi_gain;
839    spx_sig_t *low_exc, *low_innov;
840    spx_coef_t *awk1, *awk2, *awk3;
841    int dtx;
842    SpeexSBMode *mode;
843
844    st = (SBDecState*)state;
845    stack=st->stack;
846    mode = (SpeexSBMode*)(st->mode->mode);
847
848    {
849       short *low;
850       low = PUSH(stack, st->frame_size, short);
851       
852       /* Decode the low-band */
853       ret = speex_decode(st->st_low, bits, low);
854       
855       for (i=0;i<st->frame_size;i++)
856          st->x0d[i] = low[i]*SIG_SCALING;
857    }
858
859    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
860
861    /* If error decoding the narrowband part, propagate error */
862    if (ret!=0)
863    {
864       return ret;
865    }
866
867    if (!bits)
868    {
869       sb_decode_lost(st, out, dtx, stack);
870       return 0;
871    }
872
873    if (st->encode_submode)
874    {
875
876       /*Check "wideband bit"*/
877       if (speex_bits_remaining(bits)>0)
878          wideband = speex_bits_peek(bits);
879       else
880          wideband = 0;
881       if (wideband)
882       {
883          /*Regular wideband frame, read the submode*/
884          wideband = speex_bits_unpack_unsigned(bits, 1);
885          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
886       } else
887       {
888          /*Was a narrowband frame, set "null submode"*/
889          st->submodeID = 0;
890       }
891       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
892       {
893          speex_warning("Invalid mode encountered: corrupted stream?");
894          return -2;
895       }
896    }
897
898    /* If null mode (no transmission), just set a couple things to zero*/
899    if (st->submodes[st->submodeID] == NULL)
900    {
901       if (dtx)
902       {
903          sb_decode_lost(st, out, 1, stack);
904          return 0;
905       }
906
907       for (i=0;i<st->frame_size;i++)
908          st->exc[i]=0;
909
910       st->first=1;
911
912       /* Final signal synthesis from excitation */
913       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
914
915       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
916       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
917
918 #ifdef FIXED_POINT
919       for (i=0;i<st->full_frame_size;i++)
920          out[i]=SHR(st->y0[i]-st->y1[i],SIG_SHIFT-1);
921 #else
922       for (i=0;i<st->full_frame_size;i++)
923          out[i]=2*(st->y0[i]-st->y1[i]);   
924 #endif
925       
926       return 0;
927
928    }
929
930    for (i=0;i<st->frame_size;i++)
931       st->exc[i]=0;
932
933    low_pi_gain = PUSH(stack, st->nbSubframes, float);
934    low_exc = PUSH(stack, st->frame_size, spx_sig_t);
935    low_innov = PUSH(stack, st->frame_size, spx_sig_t);
936    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
937    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
938    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
939
940    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
941    
942    if (st->first)
943    {
944       for (i=0;i<st->lpcSize;i++)
945          st->old_qlsp[i] = st->qlsp[i];
946    }
947    
948    awk1=PUSH(stack, st->lpcSize+1, spx_coef_t);
949    awk2=PUSH(stack, st->lpcSize+1, spx_coef_t);
950    awk3=PUSH(stack, st->lpcSize+1, spx_coef_t);
951
952    for (sub=0;sub<st->nbSubframes;sub++)
953    {
954       spx_sig_t *exc, *sp;
955       float tmp, filter_ratio, el=0;
956       int offset;
957       float rl=0,rh=0;
958       
959       offset = st->subframeSize*sub;
960       sp=st->high+offset;
961       exc=st->exc+offset;
962       
963       /* LSP interpolation */
964       tmp = (1.0 + sub)/st->nbSubframes;
965       for (i=0;i<st->lpcSize;i++)
966          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
967
968       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .05);
969
970       /* LSP to LPC */
971       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
972
973
974       if (st->lpc_enh_enabled)
975       {
976          float r=.9;
977          
978          float k1,k2,k3;
979          k1=SUBMODE(lpc_enh_k1);
980          k2=SUBMODE(lpc_enh_k2);
981          k3=(1-(1-r*k1)/(1-r*k2))/r;
982          k3=k1-k2;
983          if (!st->lpc_enh_enabled)
984          {
985             k1=k2;
986             k3=0;
987          }
988          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
989          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
990          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
991          /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
992       }
993
994
995       /* Calculate reponse ratio between the low and high filter in the middle
996          of the band (4000 Hz) */
997       
998          tmp=1;
999          st->pi_gain[sub]=0;
1000          for (i=0;i<=st->lpcSize;i++)
1001          {
1002             rh += tmp*st->interp_qlpc[i];
1003             tmp = -tmp;
1004             st->pi_gain[sub]+=st->interp_qlpc[i];
1005          }
1006          rh /= LPC_SCALING;
1007          st->pi_gain[sub] /= LPC_SCALING;
1008
1009          rl = low_pi_gain[sub];
1010          rl=1/(fabs(rl)+.01);
1011          rh=1/(fabs(rh)+.01);
1012          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
1013       
1014       
1015       for (i=0;i<st->subframeSize;i++)
1016          exc[i]=0;
1017       if (!SUBMODE(innovation_unquant))
1018       {
1019          float g;
1020          int quant;
1021
1022          quant = speex_bits_unpack_unsigned(bits, 5);
1023          g= exp(((float)quant-10)/8.0);
1024          
1025          /*printf ("unquant folding gain: %f\n", g);*/
1026          g /= filter_ratio;
1027          
1028          /* High-band excitation using the low-band excitation and a gain */
1029          for (i=0;i<st->subframeSize;i++)
1030             exc[i]=mode->folding_gain*g*low_innov[offset+i];
1031          /*speex_rand_vec(mode->folding_gain*g*sqrt(el/st->subframeSize), exc, st->subframeSize);*/
1032       } else {
1033          float gc, scale;
1034          int qgc = speex_bits_unpack_unsigned(bits, 4);
1035
1036          el = compute_rms(low_exc+offset, st->subframeSize);
1037
1038          gc = exp((1/3.7)*qgc-2);
1039
1040          scale = gc*(1+el*sqrt(st->subframeSize))/filter_ratio;
1041
1042          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1043                                 bits, stack);
1044          for (i=0;i<st->subframeSize;i++)
1045             exc[i]*=scale;
1046
1047          if (SUBMODE(double_codebook)) {
1048             char *tmp_stack=stack;
1049             spx_sig_t *innov2 = PUSH(tmp_stack, st->subframeSize, spx_sig_t);
1050             for (i=0;i<st->subframeSize;i++)
1051                innov2[i]=0;
1052             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1053                                 bits, tmp_stack);
1054             for (i=0;i<st->subframeSize;i++)
1055                innov2[i]*=scale*(1/2.5);
1056             for (i=0;i<st->subframeSize;i++)
1057                exc[i] += innov2[i];
1058          }
1059
1060       }
1061
1062       for (i=0;i<st->subframeSize;i++)
1063          sp[i]=exc[i];
1064       if (st->lpc_enh_enabled)
1065       {
1066          /* Use enhanced LPC filter */
1067          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1068                      st->mem_sp+st->lpcSize);
1069          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1070                      st->mem_sp);
1071       } else {
1072          /* Use regular filter */
1073          for (i=0;i<st->lpcSize;i++)
1074             st->mem_sp[st->lpcSize+i] = 0;
1075          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1076                      st->mem_sp);
1077       }
1078       /*iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);*/
1079
1080    }
1081
1082    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1083    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1084
1085 #ifdef FIXED_POINT
1086    for (i=0;i<st->full_frame_size;i++)
1087       out[i]=SHR(st->y0[i]-st->y1[i],SIG_SHIFT-1);
1088 #else
1089    for (i=0;i<st->full_frame_size;i++)
1090       out[i]=2*(st->y0[i]-st->y1[i]);   
1091 #endif
1092
1093    for (i=0;i<st->lpcSize;i++)
1094       st->old_qlsp[i] = st->qlsp[i];
1095
1096    st->first=0;
1097
1098    return 0;
1099 }
1100
1101
1102 int sb_encoder_ctl(void *state, int request, void *ptr)
1103 {
1104    SBEncState *st;
1105    st=(SBEncState*)state;
1106    switch(request)
1107    {
1108    case SPEEX_GET_FRAME_SIZE:
1109       (*(int*)ptr) = st->full_frame_size;
1110       break;
1111    case SPEEX_SET_HIGH_MODE:
1112       st->submodeSelect = st->submodeID = (*(int*)ptr);
1113       break;
1114    case SPEEX_SET_LOW_MODE:
1115       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1116       break;
1117    case SPEEX_SET_DTX:
1118       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
1119       break;
1120    case SPEEX_GET_DTX:
1121       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
1122       break;
1123    case SPEEX_GET_LOW_MODE:
1124       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1125       break;
1126    case SPEEX_SET_MODE:
1127       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1128       break;
1129    case SPEEX_SET_VBR:
1130       st->vbr_enabled = (*(int*)ptr);
1131       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1132       break;
1133    case SPEEX_GET_VBR:
1134       (*(int*)ptr) = st->vbr_enabled;
1135       break;
1136    case SPEEX_SET_VAD:
1137       st->vad_enabled = (*(int*)ptr);
1138       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1139       break;
1140    case SPEEX_GET_VAD:
1141       (*(int*)ptr) = st->vad_enabled;
1142       break;
1143    case SPEEX_SET_VBR_QUALITY:
1144       {
1145          int q;
1146          float qual = (*(float*)ptr)+.6;
1147          st->vbr_quality = (*(float*)ptr);
1148          if (qual>10)
1149             qual=10;
1150          q=(int)floor(.5+*(float*)ptr);
1151          if (q>10)
1152             q=10;
1153          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1154          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1155          break;
1156       }
1157    case SPEEX_SET_ABR:
1158       st->abr_enabled = (*(int*)ptr);
1159       st->vbr_enabled = 1;
1160       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
1161       {
1162          int i=10, rate, target;
1163          float vbr_qual;
1164          target = (*(int*)ptr);
1165          while (i>=0)
1166          {
1167             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1168             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1169             if (rate <= target)
1170                break;
1171             i--;
1172          }
1173          vbr_qual=i;
1174          if (vbr_qual<0)
1175             vbr_qual=0;
1176          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1177          st->abr_count=0;
1178          st->abr_drift=0;
1179          st->abr_drift2=0;
1180       }
1181       
1182       break;
1183    case SPEEX_GET_ABR:
1184       (*(int*)ptr) = st->abr_enabled;
1185       break;
1186    case SPEEX_SET_QUALITY:
1187       {
1188          int nb_qual;
1189          int quality = (*(int*)ptr);
1190          if (quality < 0)
1191             quality = 0;
1192          if (quality > 10)
1193             quality = 10;
1194          st->submodeSelect = st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1195          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1196          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1197       }
1198       break;
1199    case SPEEX_SET_COMPLEXITY:
1200       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1201       st->complexity = (*(int*)ptr);
1202       if (st->complexity<1)
1203          st->complexity=1;
1204       break;
1205    case SPEEX_GET_COMPLEXITY:
1206       (*(int*)ptr) = st->complexity;
1207       break;
1208    case SPEEX_SET_BITRATE:
1209       {
1210          int i=10, rate, target;
1211          target = (*(int*)ptr);
1212          while (i>=0)
1213          {
1214             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1215             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1216             if (rate <= target)
1217                break;
1218             i--;
1219          }
1220       }
1221       break;
1222    case SPEEX_GET_BITRATE:
1223       speex_encoder_ctl(st->st_low, request, ptr);
1224       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1225       if (st->submodes[st->submodeID])
1226          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1227       else
1228          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1229       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1230       break;
1231    case SPEEX_SET_SAMPLING_RATE:
1232       {
1233          int tmp=(*(int*)ptr);
1234          st->sampling_rate = tmp;
1235          tmp>>=1;
1236          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1237       }
1238       break;
1239    case SPEEX_GET_SAMPLING_RATE:
1240       (*(int*)ptr)=st->sampling_rate;
1241       break;
1242    case SPEEX_RESET_STATE:
1243       {
1244          int i;
1245          st->first = 1;
1246          for (i=0;i<st->lpcSize;i++)
1247             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1248          for (i=0;i<st->lpcSize;i++)
1249             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1250          for (i=0;i<st->bufSize;i++)
1251             st->excBuf[i]=0;
1252          for (i=0;i<QMF_ORDER;i++)
1253             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1254       }
1255       break;
1256    case SPEEX_SET_SUBMODE_ENCODING:
1257       st->encode_submode = (*(int*)ptr);
1258       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1259       break;
1260    case SPEEX_GET_SUBMODE_ENCODING:
1261       (*(int*)ptr) = st->encode_submode;
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          spx_sig_t *e = (spx_sig_t*)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          spx_sig_t *e = (spx_sig_t*)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    case SPEEX_GET_RELATIVE_QUALITY:
1292       (*(float*)ptr)=st->relative_quality;
1293       break;
1294    default:
1295       speex_warning_int("Unknown nb_ctl request: ", request);
1296       return -1;
1297    }
1298    return 0;
1299 }
1300
1301 int sb_decoder_ctl(void *state, int request, void *ptr)
1302 {
1303    SBDecState *st;
1304    st=(SBDecState*)state;
1305    switch(request)
1306    {
1307    case SPEEX_SET_HIGH_MODE:
1308       st->submodeID = (*(int*)ptr);
1309       break;
1310    case SPEEX_SET_LOW_MODE:
1311       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1312       break;
1313    case SPEEX_GET_LOW_MODE:
1314       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1315       break;
1316    case SPEEX_GET_FRAME_SIZE:
1317       (*(int*)ptr) = st->full_frame_size;
1318       break;
1319    case SPEEX_SET_ENH:
1320       speex_decoder_ctl(st->st_low, request, ptr);
1321       st->lpc_enh_enabled = *((int*)ptr);
1322       break;
1323    case SPEEX_SET_MODE:
1324    case SPEEX_SET_QUALITY:
1325       {
1326          int nb_qual;
1327          int quality = (*(int*)ptr);
1328          if (quality < 0)
1329             quality = 0;
1330          if (quality > 10)
1331             quality = 10;
1332          st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1333          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1334          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1335       }
1336       break;
1337    case SPEEX_GET_BITRATE:
1338       speex_decoder_ctl(st->st_low, request, ptr);
1339       if (st->submodes[st->submodeID])
1340          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1341       else
1342          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1343       break;
1344    case SPEEX_SET_SAMPLING_RATE:
1345       {
1346          int tmp=(*(int*)ptr);
1347          st->sampling_rate = tmp;
1348          tmp>>=1;
1349          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1350       }
1351       break;
1352    case SPEEX_GET_SAMPLING_RATE:
1353       (*(int*)ptr)=st->sampling_rate;
1354       break;
1355    case SPEEX_SET_HANDLER:
1356       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1357       break;
1358    case SPEEX_SET_USER_HANDLER:
1359       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1360       break;
1361    case SPEEX_RESET_STATE:
1362       {
1363          int i;
1364          for (i=0;i<2*st->lpcSize;i++)
1365             st->mem_sp[i]=0;
1366          for (i=0;i<QMF_ORDER;i++)
1367             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1368       }
1369       break;
1370    case SPEEX_SET_SUBMODE_ENCODING:
1371       st->encode_submode = (*(int*)ptr);
1372       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1373       break;
1374    case SPEEX_GET_SUBMODE_ENCODING:
1375       (*(int*)ptr) = st->encode_submode;
1376       break;
1377    case SPEEX_GET_PI_GAIN:
1378       {
1379          int i;
1380          float *g = (float*)ptr;
1381          for (i=0;i<st->nbSubframes;i++)
1382             g[i]=st->pi_gain[i];
1383       }
1384       break;
1385    case SPEEX_GET_EXC:
1386       {
1387          int i;
1388          spx_sig_t *e = (spx_sig_t*)ptr;
1389          for (i=0;i<st->full_frame_size;i++)
1390             e[i]=0;
1391          for (i=0;i<st->frame_size;i++)
1392             e[2*i]=2*st->exc[i];
1393       }
1394       break;
1395    case SPEEX_GET_INNOV:
1396       {
1397          int i;
1398          spx_sig_t *e = (spx_sig_t*)ptr;
1399          for (i=0;i<st->full_frame_size;i++)
1400             e[i]=0;
1401          for (i=0;i<st->frame_size;i++)
1402             e[2*i]=2*st->exc[i];
1403       }
1404       break;
1405    case SPEEX_GET_DTX_STATUS:
1406       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
1407       break;
1408    default:
1409       speex_warning_int("Unknown nb_ctl request: ", request);
1410       return -1;
1411    }
1412    return 0;
1413 }