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