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