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