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