more replacements of int with spx_int32_t
[speexdsp.git] / libspeex / sb_celp.c
1 /* Copyright (C) 2002-2006 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 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.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 /* Default size for the encoder and decoder stack (can be changed at compile time).
50    This does not apply when using variable-size arrays or alloca. */
51 #ifndef SB_ENC_STACK
52 #define SB_ENC_STACK (10000*sizeof(spx_sig_t))
53 #endif
54
55 #ifndef SB_DEC_STACK
56 #define SB_DEC_STACK (6000*sizeof(spx_sig_t))
57 #endif
58
59
60 #ifdef DISABLE_WIDEBAND
61 void *sb_encoder_init(const SpeexMode *m)
62 {
63    speex_error("Wideband and Ultra-wideband are disabled");
64    return NULL;
65 }
66 void sb_encoder_destroy(void *state)
67 {
68    speex_error("Wideband and Ultra-wideband are disabled");
69 }
70 int sb_encode(void *state, void *vin, SpeexBits *bits)
71 {
72    speex_error("Wideband and Ultra-wideband are disabled");
73    return -2;
74 }
75 void *sb_decoder_init(const SpeexMode *m)
76 {
77    speex_error("Wideband and Ultra-wideband are disabled");
78    return NULL;
79 }
80 void sb_decoder_destroy(void *state)
81 {
82    speex_error("Wideband and Ultra-wideband are disabled");
83 }
84 int sb_decode(void *state, SpeexBits *bits, void *vout)
85 {
86    speex_error("Wideband and Ultra-wideband are disabled");
87    return -2;
88 }
89 int sb_encoder_ctl(void *state, int request, void *ptr)
90 {
91    speex_error("Wideband and Ultra-wideband are disabled");
92    return -2;
93 }
94 int sb_decoder_ctl(void *state, int request, void *ptr)
95 {
96    speex_error("Wideband and Ultra-wideband are disabled");
97    return -2;
98 }
99 #else
100
101
102 #ifndef M_PI
103 #define M_PI           3.14159265358979323846  /* pi */
104 #endif
105
106 #define sqr(x) ((x)*(x))
107
108 #define SUBMODE(x) st->submodes[st->submodeID]->x
109
110 #ifdef FIXED_POINT
111 static const spx_word16_t gc_quant_bound[16] = {125, 164, 215, 282, 370, 484, 635, 832, 1090, 1428, 1871, 2452, 3213, 4210, 5516, 7228};
112 #define LSP_MARGIN 410
113 #define LSP_DELTA1 6553
114 #define LSP_DELTA2 1638
115
116 #else
117
118 #define LSP_MARGIN .05
119 #define LSP_DELTA1 .2
120 #define LSP_DELTA2 .05
121
122 #endif
123
124 #define QMF_ORDER 64
125
126 #ifdef FIXED_POINT
127 static const spx_word16_t h0[64] = {2, -7, -7, 18, 15, -39, -25, 75, 35, -130, -41, 212, 38, -327, -17, 483, -32, -689, 124, 956, -283, -1307, 543, 1780, -973, -2467, 1733, 3633, -3339, -6409, 9059, 30153, 30153, 9059, -6409, -3339, 3633, 1733, -2467, -973, 1780, 543, -1307, -283, 956, 124, -689, -32, 483, -17, -327, 38, 212, -41, -130, 35, 75, -25, -39, 15, 18, -7, -7, 2};
128
129 static const spx_word16_t h1[64] = {2, 7, -7, -18, 15, 39, -25, -75, 35, 130, -41, -212, 38, 327, -17, -483, -32, 689, 124, -956, -283, 1307, 543, -1780, -973, 2467, 1733, -3633, -3339, 6409, 9059, -30153, 30153, -9059, -6409, 3339, 3633, -1733, -2467, 973, 1780, -543, -1307, 283, 956, -124, -689, 32, 483, 17, -327, -38, 212, 41, -130, -35, 75, 25, -39, -15, 18, 7, -7, -2};
130
131
132 #else
133 static const float h0[64] = {
134    3.596189e-05, -0.0001123515,
135    -0.0001104587, 0.0002790277,
136    0.0002298438, -0.0005953563,
137    -0.0003823631, 0.00113826,
138    0.0005308539, -0.001986177,
139    -0.0006243724, 0.003235877,
140    0.0005743159, -0.004989147,
141    -0.0002584767, 0.007367171,
142    -0.0004857935, -0.01050689,
143    0.001894714, 0.01459396,
144    -0.004313674, -0.01994365,
145    0.00828756, 0.02716055,
146    -0.01485397, -0.03764973,
147    0.026447, 0.05543245,
148    -0.05095487, -0.09779096,
149    0.1382363, 0.4600981,
150    0.4600981, 0.1382363,
151    -0.09779096, -0.05095487,
152    0.05543245, 0.026447,
153    -0.03764973, -0.01485397,
154    0.02716055, 0.00828756,
155    -0.01994365, -0.004313674,
156    0.01459396, 0.001894714,
157    -0.01050689, -0.0004857935,
158    0.007367171, -0.0002584767,
159    -0.004989147, 0.0005743159,
160    0.003235877, -0.0006243724,
161    -0.001986177, 0.0005308539,
162    0.00113826, -0.0003823631,
163    -0.0005953563, 0.0002298438,
164    0.0002790277, -0.0001104587,
165    -0.0001123515, 3.596189e-05
166 };
167
168 static const float h1[64] = {
169    3.596189e-05, 0.0001123515,
170    -0.0001104587, -0.0002790277,
171    0.0002298438, 0.0005953563,
172    -0.0003823631, -0.00113826,
173    0.0005308539, 0.001986177,
174    -0.0006243724, -0.003235877,
175    0.0005743159, 0.004989147,
176    -0.0002584767, -0.007367171,
177    -0.0004857935, 0.01050689,
178    0.001894714, -0.01459396,
179    -0.004313674, 0.01994365,
180    0.00828756, -0.02716055,
181    -0.01485397, 0.03764973,
182    0.026447, -0.05543245,
183    -0.05095487, 0.09779096,
184    0.1382363, -0.4600981,
185    0.4600981, -0.1382363,
186    -0.09779096, 0.05095487,
187    0.05543245, -0.026447,
188    -0.03764973, 0.01485397,
189    0.02716055, -0.00828756,
190    -0.01994365, 0.004313674,
191    0.01459396, -0.001894714,
192    -0.01050689, 0.0004857935,
193    0.007367171, 0.0002584767,
194    -0.004989147, -0.0005743159,
195    0.003235877, 0.0006243724,
196    -0.001986177, -0.0005308539,
197    0.00113826, 0.0003823631,
198    -0.0005953563, -0.0002298438,
199    0.0002790277, 0.0001104587,
200    -0.0001123515, -3.596189e-05
201 };
202 #endif
203
204 extern const spx_word16_t lpc_window[];
205
206 static void mix_and_saturate(spx_word32_t *x0, spx_word32_t *x1, spx_word16_t *out, int len)
207 {
208    int i;
209    for (i=0;i<len;i++)
210    {
211       spx_word32_t tmp;
212 #ifdef FIXED_POINT
213       tmp=PSHR(x0[i]-x1[i],SIG_SHIFT-1);
214 #else
215       tmp=2*(x0[i]-x1[i]);
216 #endif
217       if (tmp>32767)
218          out[i] = 32767;
219       else if (tmp<-32767)
220          out[i] = -32767;
221       else
222          out[i] = tmp;
223    }
224 }
225
226 void *sb_encoder_init(const SpeexMode *m)
227 {
228    int i;
229    spx_int32_t tmp;
230    SBEncState *st;
231    const SpeexSBMode *mode;
232
233    st = (SBEncState*)speex_alloc(sizeof(SBEncState));
234    if (!st)
235       return NULL;
236 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
237    st->stack = NULL;
238 #else
239    st->stack = (char*)speex_alloc_scratch(SB_ENC_STACK);
240 #endif
241    st->mode = m;
242    mode = (const SpeexSBMode*)m->mode;
243
244
245    st->st_low = speex_encoder_init(mode->nb_mode);
246    st->full_frame_size = 2*mode->frameSize;
247    st->frame_size = mode->frameSize;
248    st->subframeSize = mode->subframeSize;
249    st->nbSubframes = mode->frameSize/mode->subframeSize;
250    st->windowSize = st->frame_size+st->subframeSize;
251    st->lpcSize=mode->lpcSize;
252    st->bufSize=mode->bufSize;
253
254    st->encode_submode = 1;
255    st->submodes=mode->submodes;
256    st->submodeSelect = st->submodeID=mode->defaultSubmode;
257    
258    tmp=9;
259    speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &tmp);
260    tmp=1;
261    speex_encoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);
262
263    st->lag_factor = mode->lag_factor;
264    st->lpc_floor = mode->lpc_floor;
265    st->gamma1=mode->gamma1;
266    st->gamma2=mode->gamma2;
267    st->first=1;
268
269    st->x0d=(spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
270    st->x1d=(spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
271    st->high=(spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
272    st->y0=(spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
273    st->y1=(spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
274
275    st->h0_mem=(spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
276    st->h1_mem=(spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
277    st->g0_mem=(spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
278    st->g1_mem=(spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
279
280    st->excBuf=(spx_sig_t*)speex_alloc((st->bufSize)*sizeof(spx_sig_t));
281    st->exc = st->excBuf + st->bufSize - st->windowSize;
282
283    st->res=(spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
284    st->sw=(spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
285    st->window= lpc_window;
286
287    st->lagWindow = (spx_word16_t*)speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
288    for (i=0;i<st->lpcSize+1;i++)
289       st->lagWindow[i]=16384*exp(-.5*sqr(2*M_PI*st->lag_factor*i));
290
291    st->autocorr = (spx_word16_t*)speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
292    st->lpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
293    st->bw_lpc1 = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
294    st->bw_lpc2 = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
295    st->lsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
296    st->qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
297    st->old_lsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
298    st->old_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
299    st->interp_lsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
300    st->interp_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
301    st->interp_lpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
302    st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
303    st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
304    st->low_innov = (spx_word32_t*)speex_alloc((st->frame_size)*sizeof(spx_word32_t));
305    speex_encoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, st->low_innov);
306    st->innov_save = NULL;
307    
308    st->mem_sp = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
309    st->mem_sp2 = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
310    st->mem_sw = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
311
312    st->vbr_quality = 8;
313    st->vbr_enabled = 0;
314    st->vbr_max = 0;
315    st->vbr_max_high = 20000;  /* We just need a big value here */
316    st->vad_enabled = 0;
317    st->abr_enabled = 0;
318    st->relative_quality=0;
319
320    st->complexity=2;
321    speex_encoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
322    st->sampling_rate*=2;
323 #ifdef ENABLE_VALGRIND
324    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
325 #endif
326    return st;
327 }
328
329 void sb_encoder_destroy(void *state)
330 {
331    SBEncState *st=(SBEncState*)state;
332
333    speex_encoder_destroy(st->st_low);
334 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
335    speex_free_scratch(st->stack);
336 #endif
337
338    speex_free(st->x0d);
339    speex_free(st->x1d);
340    speex_free(st->high);
341    speex_free(st->y0);
342    speex_free(st->y1);
343
344    speex_free(st->h0_mem);
345    speex_free(st->h1_mem);
346    speex_free(st->g0_mem);
347    speex_free(st->g1_mem);
348
349    speex_free(st->excBuf);
350    speex_free(st->res);
351    speex_free(st->sw);
352    speex_free(st->lagWindow);
353
354    speex_free(st->autocorr);
355    speex_free(st->lpc);
356    speex_free(st->bw_lpc1);
357    speex_free(st->bw_lpc2);
358    speex_free(st->lsp);
359    speex_free(st->qlsp);
360    speex_free(st->old_lsp);
361    speex_free(st->old_qlsp);
362    speex_free(st->interp_lsp);
363    speex_free(st->interp_qlsp);
364    speex_free(st->interp_lpc);
365    speex_free(st->interp_qlpc);
366    speex_free(st->pi_gain);
367    speex_free(st->low_innov);
368
369    speex_free(st->mem_sp);
370    speex_free(st->mem_sp2);
371    speex_free(st->mem_sw);
372
373    
374    speex_free(st);
375 }
376
377
378 int sb_encode(void *state, void *vin, SpeexBits *bits)
379 {
380    SBEncState *st;
381    int i, roots, sub;
382    char *stack;
383    VARDECL(spx_mem_t *mem);
384    VARDECL(spx_sig_t *innov);
385    VARDECL(spx_word16_t *target);
386    VARDECL(spx_word16_t *syn_resp);
387    VARDECL(spx_word32_t *low_pi_gain);
388    VARDECL(spx_word16_t *low_exc);
389    const SpeexSBMode *mode;
390    spx_int32_t dtx;
391    spx_word16_t *in = (spx_word16_t*)vin;
392
393    st = (SBEncState*)state;
394    stack=st->stack;
395    mode = (const SpeexSBMode*)(st->mode->mode);
396
397    {
398       VARDECL(spx_word16_t *low);
399       ALLOC(low, st->frame_size, spx_word16_t);
400
401       /* Compute the two sub-bands by filtering with h0 and h1*/
402       qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
403       
404       for (i=0;i<st->frame_size;i++)
405          low[i] = SATURATE(PSHR(st->x0d[i],SIG_SHIFT),32767);
406       
407       /* Encode the narrowband part*/
408       speex_encode_native(st->st_low, low, bits);
409
410       for (i=0;i<st->frame_size;i++)
411          st->x0d[i] = SHL(low[i],SIG_SHIFT);
412    }
413    /* High-band buffering / sync with low band */
414    for (i=0;i<st->windowSize-st->frame_size;i++)
415       st->high[i] = st->high[st->frame_size+i];
416    for (i=0;i<st->frame_size;i++)
417       st->high[st->windowSize-st->frame_size+i]=SATURATE(st->x1d[i],536854528);
418
419    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(spx_sig_t));
420
421
422    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
423    ALLOC(low_exc, st->frame_size, spx_word16_t);
424    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
425    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
426    
427    speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
428
429    if (dtx==0)
430       dtx=1;
431    else
432       dtx=0;
433
434    {
435       VARDECL(spx_word16_t *w_sig);
436       ALLOC(w_sig, st->windowSize, spx_word16_t);
437       /* Window for analysis */
438       for (i=0;i<st->windowSize;i++)
439          w_sig[i] = SHR(MULT16_16(SHR((spx_word32_t)(st->high[i]),SIG_SHIFT),st->window[i]),SIG_SHIFT);
440
441       /* Compute auto-correlation */
442       _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize);
443    }
444    st->autocorr[0] = ADD16(st->autocorr[0],MULT16_16_Q15(st->autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
445
446    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
447    for (i=0;i<st->lpcSize+1;i++)
448       st->autocorr[i] = MULT16_16_Q14(st->autocorr[i],st->lagWindow[i]);
449
450    /* Levinson-Durbin */
451    _spx_lpc(st->lpc, st->autocorr, st->lpcSize);
452
453    /* LPC to LSPs (x-domain) transform */
454    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 10, LSP_DELTA1, stack);
455    if (roots!=st->lpcSize)
456    {
457       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 10, LSP_DELTA2, stack);
458       if (roots!=st->lpcSize) {
459          /*If we can't find all LSP's, do some damage control and use a flat filter*/
460          for (i=0;i<st->lpcSize;i++)
461          {
462             st->lsp[i]=LSP_SCALING*M_PI*((float)(i+1))/(st->lpcSize+1);
463          }
464       }
465    }
466
467    /* VBR code */
468    if ((st->vbr_enabled || st->vad_enabled) && !dtx)
469    {
470       float e_low=0, e_high=0;
471       float ratio;
472       if (st->abr_enabled)
473       {
474          float qual_change=0;
475          if (st->abr_drift2 * st->abr_drift > 0)
476          {
477             /* Only adapt if long-term and short-term drift are the same sign */
478             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
479             if (qual_change>.1)
480                qual_change=.1;
481             if (qual_change<-.1)
482                qual_change=-.1;
483          }
484          st->vbr_quality += qual_change;
485          if (st->vbr_quality>10)
486             st->vbr_quality=10;
487          if (st->vbr_quality<0)
488             st->vbr_quality=0;
489       }
490
491
492       /*FIXME: Are the two signals (low, high) in sync? */
493       e_low = compute_rms(st->x0d, st->frame_size);
494       e_high = compute_rms(st->high, st->frame_size);
495       ratio = 2*log((1+e_high)/(1+e_low));
496       
497       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &st->relative_quality);
498       if (ratio<-4)
499          ratio=-4;
500       if (ratio>2)
501          ratio=2;
502       /*if (ratio>-2)*/
503       if (st->vbr_enabled) 
504       {
505          spx_int32_t modeid;
506          modeid = mode->nb_modes-1;
507          st->relative_quality+=1.0*(ratio+2);
508          if (st->relative_quality<-1)
509             st->relative_quality=-1;
510          while (modeid)
511          {
512             int v1;
513             float thresh;
514             v1=(int)floor(st->vbr_quality);
515             if (v1==10)
516                thresh = mode->vbr_thresh[modeid][v1];
517             else
518                thresh = (st->vbr_quality-v1)   * mode->vbr_thresh[modeid][v1+1] + 
519                         (1+v1-st->vbr_quality) * mode->vbr_thresh[modeid][v1];
520             if (st->relative_quality >= thresh && st->sampling_rate*st->submodes[modeid]->bits_per_frame/st->full_frame_size <= st->vbr_max_high)
521                break;
522             modeid--;
523          }
524          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &modeid);
525          if (st->abr_enabled)
526          {
527             spx_int32_t bitrate;
528             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
529             st->abr_drift+=(bitrate-st->abr_enabled);
530             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
531             st->abr_count += 1.0;
532          }
533
534       } else {
535          /* VAD only */
536          int modeid;
537          if (st->relative_quality<2.0)
538             modeid=1;
539          else
540             modeid=st->submodeSelect;
541          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
542          st->submodeID=modeid;
543
544       }
545       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
546    }
547
548    if (st->encode_submode)
549    {
550       speex_bits_pack(bits, 1, 1);
551       if (dtx)
552          speex_bits_pack(bits, 0, SB_SUBMODE_BITS);
553       else
554          speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
555    }
556
557    /* If null mode (no transmission), just set a couple things to zero*/
558    if (dtx || st->submodes[st->submodeID] == NULL)
559    {
560       for (i=0;i<st->frame_size;i++)
561          st->exc[i]=st->sw[i]=VERY_SMALL;
562
563       for (i=0;i<st->lpcSize;i++)
564          st->mem_sw[i]=0;
565       st->first=1;
566
567       /* Final signal synthesis from excitation */
568       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
569
570 #ifdef RESYNTH
571       /* Reconstruct the original */
572       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
573       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
574
575       for (i=0;i<st->full_frame_size;i++)
576          in[i]=SHR(st->y0[i]-st->y1[i], SIG_SHIFT-1);
577 #endif
578
579       if (dtx)
580          return 0;
581       else
582          return 1;
583    }
584
585
586    /* LSP quantization */
587    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);   
588
589    if (st->first)
590    {
591       for (i=0;i<st->lpcSize;i++)
592          st->old_lsp[i] = st->lsp[i];
593       for (i=0;i<st->lpcSize;i++)
594          st->old_qlsp[i] = st->qlsp[i];
595    }
596    
597    ALLOC(mem, st->lpcSize, spx_mem_t);
598    ALLOC(syn_resp, st->subframeSize, spx_word16_t);
599    ALLOC(innov, st->subframeSize, spx_sig_t);
600    ALLOC(target, st->subframeSize, spx_word16_t);
601
602    for (sub=0;sub<st->nbSubframes;sub++)
603    {
604       spx_sig_t *exc, *sp, *res, *sw, *innov_save=NULL;
605       spx_word16_t filter_ratio;
606       int offset;
607       spx_word32_t rl, rh;
608       spx_word16_t eh=0;
609
610       offset = st->subframeSize*sub;
611       sp=st->high+offset;
612       exc=st->exc+offset;
613       res=st->res+offset;
614       sw=st->sw+offset;
615       /* Pointer for saving innovation */
616       if (st->innov_save)
617       {
618          innov_save = st->innov_save+2*offset;
619          for (i=0;i<2*st->subframeSize;i++)
620             innov_save[i]=0;
621       }
622       
623       /* LSP interpolation (quantized and unquantized) */
624       lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, sub, st->nbSubframes);
625       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
626
627       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);
628       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
629
630       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
631       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
632
633       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
634       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
635
636       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
637          filters */
638       st->pi_gain[sub]=LPC_SCALING;
639       rh = LPC_SCALING;
640       for (i=0;i<st->lpcSize;i+=2)
641       {
642          rh += st->interp_qlpc[i+1] - st->interp_qlpc[i];
643          st->pi_gain[sub] += st->interp_qlpc[i] + st->interp_qlpc[i+1];
644       }
645       
646       rl = low_pi_gain[sub];
647 #ifdef FIXED_POINT
648       filter_ratio=PDIV32_16(SHL(rl+82,2),SHR(82+rh,5));
649 #else
650       filter_ratio=(rl+.01)/(rh+.01);
651 #endif
652       
653       /* Compute "real excitation" */
654       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
655       /* Compute energy of low-band and high-band excitation */
656
657       eh = compute_rms(exc, st->subframeSize);
658
659       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
660          float g;
661          spx_word16_t el;
662          el = compute_rms(st->low_innov+offset, st->subframeSize);
663
664          /* Gain to use if we want to use the low-band excitation for high-band */
665          g=eh/(1.+el);
666          
667 #if 0
668          {
669             char *tmp_stack=stack;
670             float *tmp_sig;
671             float g2;
672             ALLOC(tmp_sig, st->subframeSize, spx_sig_t);
673             for (i=0;i<st->lpcSize;i++)
674                mem[i]=st->mem_sp[i];
675             iir_mem2(st->low_innov+offset, st->interp_qlpc, tmp_sig, st->subframeSize, st->lpcSize, mem);
676             g2 = compute_rms(sp, st->subframeSize)/(.01+compute_rms(tmp_sig, st->subframeSize));
677             /*fprintf (stderr, "gains: %f %f\n", g, g2);*/
678             g = g2;
679             stack = tmp_stack;
680          }
681 #endif
682
683 #ifdef FIXED_POINT
684          g *= filter_ratio/128.;
685 #else
686          g *= filter_ratio;
687 #endif
688          /*print_vec(&g, 1, "gain factor");*/
689          /* Gain quantization */
690          {
691             int quant = (int) floor(.5 + 10 + 8.0 * log((g+.0001)));
692             /*speex_warning_int("tata", quant);*/
693             if (quant<0)
694                quant=0;
695             if (quant>31)
696                quant=31;
697             speex_bits_pack(bits, quant, 5);
698          }
699
700       } else {
701          spx_word16_t gc;
702          spx_word32_t scale;
703          spx_word16_t el;
704          el = compute_rms16(low_exc+offset, st->subframeSize);
705
706          gc = PDIV32_16(MULT16_16(filter_ratio,1+eh),1+el);
707
708          /* This is a kludge that cleans up a historical bug */
709          if (st->subframeSize==80)
710             gc *= 0.70711;
711          /*printf ("%f %f %f %f\n", el, eh, filter_ratio, gc);*/
712 #ifdef FIXED_POINT
713          {
714             int qgc = scal_quant(gc, gc_quant_bound, 16);
715             speex_bits_pack(bits, qgc, 4);
716             gc = MULT16_32_Q15(28626,gc_quant_bound[qgc]);
717          }
718 #else
719          {
720             int qgc = (int)floor(.5+3.7*(log(gc)+0.15556));
721             if (qgc<0)
722                qgc=0;
723             if (qgc>15)
724                qgc=15;
725             speex_bits_pack(bits, qgc, 4);
726             gc = exp((1/3.7)*qgc-0.15556);
727          }         
728 #endif
729          if (st->subframeSize==80)
730             gc *= 1.4142;
731
732          scale = SHL32(MULT16_16(PDIV32_16(SHL32(EXTEND32(gc),SIG_SHIFT-6),filter_ratio),(1+el)),6);
733
734          compute_impulse_response(st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
735
736          
737          /* Reset excitation */
738          for (i=0;i<st->subframeSize;i++)
739             exc[i]=VERY_SMALL;
740          
741          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
742          for (i=0;i<st->lpcSize;i++)
743             mem[i]=st->mem_sp[i];
744          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
745
746          for (i=0;i<st->lpcSize;i++)
747             mem[i]=st->mem_sw[i];
748          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
749
750          /* Compute weighted signal */
751          for (i=0;i<st->lpcSize;i++)
752             mem[i]=st->mem_sw[i];
753          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
754
755          /* Compute target signal */
756          for (i=0;i<st->subframeSize;i++)
757             target[i]=PSHR32(sw[i]-res[i],SIG_SHIFT);
758
759          for (i=0;i<st->subframeSize;i++)
760            exc[i]=0;
761
762          signal_div(target, target, scale, st->subframeSize);
763
764          /* Reset excitation */
765          for (i=0;i<st->subframeSize;i++)
766             innov[i]=0;
767
768          /*print_vec(target, st->subframeSize, "\ntarget");*/
769          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
770                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
771                                    innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
772          /*print_vec(target, st->subframeSize, "after");*/
773
774          signal_mul(innov, innov, scale, st->subframeSize);
775
776          for (i=0;i<st->subframeSize;i++)
777             exc[i] = ADD32(exc[i], innov[i]);
778
779          if (st->innov_save)
780          {
781             for (i=0;i<st->subframeSize;i++)
782                innov_save[2*i]=innov[i];
783          }
784          
785          if (SUBMODE(double_codebook)) {
786             char *tmp_stack=stack;
787             VARDECL(spx_sig_t *innov2);
788             ALLOC(innov2, st->subframeSize, spx_sig_t);
789             for (i=0;i<st->subframeSize;i++)
790                innov2[i]=0;
791             for (i=0;i<st->subframeSize;i++)
792                target[i]*=2.5;
793             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
794                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
795                                       innov2, syn_resp, bits, stack, st->complexity, 0);
796             for (i=0;i<st->subframeSize;i++)
797                innov2[i]*=scale*(1/2.5)/SIG_SCALING;
798             for (i=0;i<st->subframeSize;i++)
799                exc[i] = ADD32(exc[i],innov2[i]);
800             stack = tmp_stack;
801          }
802
803       }
804
805       /*Keep the previous memory*/
806       for (i=0;i<st->lpcSize;i++)
807          mem[i]=st->mem_sp[i];
808       /* Final signal synthesis from excitation */
809       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
810       
811       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
812       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
813    }
814
815
816 #ifdef RESYNTH
817    /* Reconstruct the original */
818    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
819    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
820
821    for (i=0;i<st->full_frame_size;i++)
822       in[i]=SHR(st->y0[i]-st->y1[i], SIG_SHIFT-1);
823 #endif
824    for (i=0;i<st->lpcSize;i++)
825       st->old_lsp[i] = st->lsp[i];
826    for (i=0;i<st->lpcSize;i++)
827       st->old_qlsp[i] = st->qlsp[i];
828
829    st->first=0;
830
831    return 1;
832 }
833
834
835
836
837
838 void *sb_decoder_init(const SpeexMode *m)
839 {
840    spx_int32_t tmp;
841    SBDecState *st;
842    const SpeexSBMode *mode;
843    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
844    if (!st)
845       return NULL;
846 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
847    st->stack = NULL;
848 #else
849    st->stack = (char*)speex_alloc_scratch(SB_DEC_STACK);
850 #endif
851    st->mode = m;
852    mode=(const SpeexSBMode*)m->mode;
853
854    st->encode_submode = 1;
855
856
857
858
859    st->st_low = speex_decoder_init(mode->nb_mode);
860    st->full_frame_size = 2*mode->frameSize;
861    st->frame_size = mode->frameSize;
862    st->subframeSize = mode->subframeSize;
863    st->nbSubframes = mode->frameSize/mode->subframeSize;
864    st->lpcSize=mode->lpcSize;
865    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
866    st->sampling_rate*=2;
867    tmp=1;
868    speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);
869
870    st->submodes=mode->submodes;
871    st->submodeID=mode->defaultSubmode;
872
873    st->first=1;
874
875
876    st->x0d = (spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
877    st->x1d = (spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
878    st->high = (spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
879    st->y0 = (spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
880    st->y1 = (spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
881
882    st->g0_mem = (spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
883    st->g1_mem = (spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
884
885    st->exc = (spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
886    st->excBuf = (spx_sig_t*)speex_alloc((st->subframeSize)*sizeof(spx_sig_t));
887
888    st->qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
889    st->old_qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
890    st->interp_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
891    st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
892
893    st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
894    st->mem_sp = (spx_mem_t*)speex_alloc((2*st->lpcSize)*sizeof(spx_mem_t));
895    
896    st->low_innov = (spx_word32_t*)speex_alloc((st->frame_size)*sizeof(spx_word32_t));
897    speex_decoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, st->low_innov);
898    st->innov_save = NULL;
899
900
901    st->lpc_enh_enabled=0;
902    st->seed = 1000;
903
904 #ifdef ENABLE_VALGRIND
905    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
906 #endif
907    return st;
908 }
909
910 void sb_decoder_destroy(void *state)
911 {
912    SBDecState *st;
913    st = (SBDecState*)state;
914    speex_decoder_destroy(st->st_low);
915 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
916    speex_free_scratch(st->stack);
917 #endif
918
919    speex_free(st->x0d);
920    speex_free(st->x1d);
921    speex_free(st->high);
922    speex_free(st->y0);
923    speex_free(st->y1);
924    speex_free(st->g0_mem);
925    speex_free(st->g1_mem);
926    speex_free(st->exc);
927    speex_free(st->excBuf);
928    speex_free(st->qlsp);
929    speex_free(st->old_qlsp);
930    speex_free(st->interp_qlsp);
931    speex_free(st->interp_qlpc);
932    speex_free(st->pi_gain);
933    speex_free(st->low_innov);
934    speex_free(st->mem_sp);
935
936    speex_free(state);
937 }
938
939 static void sb_decode_lost(SBDecState *st, spx_word16_t *out, int dtx, char *stack)
940 {
941    int i;
942    int saved_modeid=0;
943
944    if (dtx)
945    {
946       saved_modeid=st->submodeID;
947       st->submodeID=1;
948    } else {
949       bw_lpc(GAMMA_SCALING*0.99, st->interp_qlpc, st->interp_qlpc, st->lpcSize);
950    }
951
952    st->first=1;
953    
954    
955    /* Final signal synthesis from excitation */
956    if (!dtx)
957    {
958       spx_word16_t low_ener;
959       low_ener = .9*compute_rms(st->exc, st->frame_size);
960       for (i=0;i<st->frame_size;i++)
961          st->exc[i] = speex_rand(low_ener, &st->seed);
962    }
963
964    for (i=0;i<st->frame_size;i++)
965       st->high[i]=st->exc[i];
966
967    iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
968             st->mem_sp);
969    
970    
971    /* Reconstruct the original */
972    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
973    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
974
975    mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);
976
977    if (dtx)
978    {
979       st->submodeID=saved_modeid;
980    }
981
982    return;
983 }
984
985 int sb_decode(void *state, SpeexBits *bits, void *vout)
986 {
987    int i, sub;
988    SBDecState *st;
989    int wideband;
990    int ret;
991    char *stack;
992    VARDECL(spx_word32_t *low_pi_gain);
993    VARDECL(spx_word16_t *low_exc);
994    VARDECL(spx_coef_t *ak);
995    spx_int32_t dtx;
996    const SpeexSBMode *mode;
997    spx_word16_t *out = (spx_word16_t*)vout;
998    
999    st = (SBDecState*)state;
1000    stack=st->stack;
1001    mode = (const SpeexSBMode*)(st->mode->mode);
1002
1003    {
1004       VARDECL(spx_word16_t *low);
1005       ALLOC(low, st->frame_size, spx_word16_t);
1006       
1007       /* Decode the low-band */
1008       ret = speex_decode_native(st->st_low, bits, low);
1009       
1010       for (i=0;i<st->frame_size;i++)
1011          st->x0d[i] = SHL((spx_sig_t)low[i], SIG_SHIFT);
1012    }
1013
1014    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
1015
1016    /* If error decoding the narrowband part, propagate error */
1017    if (ret!=0)
1018    {
1019       return ret;
1020    }
1021
1022    if (!bits)
1023    {
1024       sb_decode_lost(st, out, dtx, stack);
1025       return 0;
1026    }
1027
1028    if (st->encode_submode)
1029    {
1030
1031       /*Check "wideband bit"*/
1032       if (speex_bits_remaining(bits)>0)
1033          wideband = speex_bits_peek(bits);
1034       else
1035          wideband = 0;
1036       if (wideband)
1037       {
1038          /*Regular wideband frame, read the submode*/
1039          wideband = speex_bits_unpack_unsigned(bits, 1);
1040          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1041       } else
1042       {
1043          /*Was a narrowband frame, set "null submode"*/
1044          st->submodeID = 0;
1045       }
1046       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
1047       {
1048          speex_warning("Invalid mode encountered: corrupted stream?");
1049          return -2;
1050       }
1051    }
1052
1053    /* If null mode (no transmission), just set a couple things to zero*/
1054    if (st->submodes[st->submodeID] == NULL)
1055    {
1056       if (dtx)
1057       {
1058          sb_decode_lost(st, out, 1, stack);
1059          return 0;
1060       }
1061
1062       for (i=0;i<st->frame_size;i++)
1063          st->exc[i]=VERY_SMALL;
1064
1065       st->first=1;
1066
1067       /* Final signal synthesis from excitation */
1068       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
1069
1070       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1071       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1072
1073       mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);
1074
1075       return 0;
1076
1077    }
1078
1079    for (i=0;i<st->frame_size;i++)
1080       st->exc[i]=0;
1081
1082    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
1083    ALLOC(low_exc, st->frame_size, spx_word16_t);
1084    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
1085    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
1086
1087    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1088    
1089    if (st->first)
1090    {
1091       for (i=0;i<st->lpcSize;i++)
1092          st->old_qlsp[i] = st->qlsp[i];
1093    }
1094    
1095    ALLOC(ak, st->lpcSize, spx_coef_t);
1096
1097    for (sub=0;sub<st->nbSubframes;sub++)
1098    {
1099       spx_sig_t *exc, *sp, *innov_save=NULL;
1100       spx_word16_t filter_ratio;
1101       spx_word16_t el=0;
1102       int offset;
1103       spx_word32_t rl=0,rh=0;
1104       
1105       offset = st->subframeSize*sub;
1106       sp=st->high+offset;
1107       exc=st->exc+offset;
1108       /* Pointer for saving innovation */
1109       if (st->innov_save)
1110       {
1111          innov_save = st->innov_save+2*offset;
1112          for (i=0;i<2*st->subframeSize;i++)
1113             innov_save[i]=0;
1114       }
1115       
1116       /* LSP interpolation */
1117       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1118
1119       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
1120
1121       /* LSP to LPC */
1122       lsp_to_lpc(st->interp_qlsp, ak, st->lpcSize, stack);
1123
1124       /* Calculate reponse ratio between the low and high filter in the middle
1125          of the band (4000 Hz) */
1126       
1127          st->pi_gain[sub]=LPC_SCALING;
1128          rh = LPC_SCALING;
1129          for (i=0;i<st->lpcSize;i+=2)
1130          {
1131             rh += st->interp_qlpc[i+1] - st->interp_qlpc[i];
1132             st->pi_gain[sub] += st->interp_qlpc[i] + st->interp_qlpc[i+1];
1133          }
1134
1135          rl = low_pi_gain[sub];
1136 #ifdef FIXED_POINT
1137          filter_ratio=PDIV32_16(SHL(rl+82,2),SHR(82+rh,5));
1138 #else
1139          filter_ratio=(rl+.01)/(rh+.01);
1140 #endif
1141       
1142       for (i=0;i<st->subframeSize;i++)
1143          exc[i]=0;
1144       if (!SUBMODE(innovation_unquant))
1145       {
1146          float g;
1147          int quant;
1148
1149          quant = speex_bits_unpack_unsigned(bits, 5);
1150          g= exp(((float)quant-10)/8.0);
1151          
1152 #ifdef FIXED_POINT
1153          g /= filter_ratio/128.;
1154 #else
1155          g /= filter_ratio;
1156 #endif
1157          /* High-band excitation using the low-band excitation and a gain */
1158          
1159 #if 0
1160          for (i=0;i<st->subframeSize;i++)
1161             exc[i]=mode->folding_gain*g*st->low_innov[offset+i];
1162 #else
1163          {
1164             float tmp=1;
1165             /*static tmp1=0,tmp2=0;
1166             static int seed=1;
1167             el = compute_rms(low_innov+offset, st->subframeSize);*/
1168             for (i=0;i<st->subframeSize;i++)
1169             {
1170                float e=tmp*g*mode->folding_gain*st->low_innov[offset+i];
1171                tmp *= -1;
1172                exc[i] = e;
1173                /*float r = speex_rand(g*el,&seed);
1174                exc[i] = .5*(r+tmp2 + e-tmp1);
1175                tmp1 = e;
1176                tmp2 = r;*/               
1177             }
1178             
1179          }
1180          
1181 #endif    
1182       } else {
1183          spx_word16_t gc;
1184          spx_word32_t scale;
1185          int qgc = speex_bits_unpack_unsigned(bits, 4);
1186
1187          el = compute_rms16(low_exc+offset, st->subframeSize);
1188
1189 #ifdef FIXED_POINT
1190          gc = MULT16_32_Q15(28626,gc_quant_bound[qgc]);
1191 #else
1192          gc = exp((1/3.7)*qgc-0.15556);
1193 #endif
1194
1195          if (st->subframeSize==80)
1196             gc *= 1.4142;
1197
1198          scale = SHL(MULT16_16(PDIV32_16(SHL(gc,SIG_SHIFT-6),filter_ratio),(1+el)),6);
1199
1200          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1201                                      bits, stack, &st->seed);
1202
1203          signal_mul(exc,exc,scale,st->subframeSize);
1204
1205          if (SUBMODE(double_codebook)) {
1206             char *tmp_stack=stack;
1207             VARDECL(spx_sig_t *innov2);
1208             ALLOC(innov2, st->subframeSize, spx_sig_t);
1209             for (i=0;i<st->subframeSize;i++)
1210                innov2[i]=0;
1211             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1212                                         bits, stack, &st->seed);
1213             for (i=0;i<st->subframeSize;i++)
1214                innov2[i]*=scale/(float)SIG_SCALING*(1/2.5);
1215             for (i=0;i<st->subframeSize;i++)
1216                exc[i] = ADD32(exc[i],innov2[i]);
1217             stack = tmp_stack;
1218          }
1219
1220       }
1221       
1222       if (st->innov_save)
1223       {
1224          for (i=0;i<st->subframeSize;i++)
1225             innov_save[2*i]=exc[i];
1226       }
1227       
1228       for (i=0;i<st->subframeSize;i++)
1229          sp[i]=st->excBuf[i];
1230       iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1231                st->mem_sp);
1232       for (i=0;i<st->subframeSize;i++)
1233          st->excBuf[i]=exc[i];
1234       for (i=0;i<st->lpcSize;i++)
1235          st->interp_qlpc[i] = ak[i];
1236
1237    }
1238
1239    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1240    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1241
1242    mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);
1243
1244    for (i=0;i<st->lpcSize;i++)
1245       st->old_qlsp[i] = st->qlsp[i];
1246
1247    st->first=0;
1248
1249    return 0;
1250 }
1251
1252
1253 int sb_encoder_ctl(void *state, int request, void *ptr)
1254 {
1255    SBEncState *st;
1256    st=(SBEncState*)state;
1257    switch(request)
1258    {
1259    case SPEEX_GET_FRAME_SIZE:
1260       (*(spx_int32_t*)ptr) = st->full_frame_size;
1261       break;
1262    case SPEEX_SET_HIGH_MODE:
1263       st->submodeSelect = st->submodeID = (*(spx_int32_t*)ptr);
1264       break;
1265    case SPEEX_SET_LOW_MODE:
1266       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1267       break;
1268    case SPEEX_SET_DTX:
1269       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
1270       break;
1271    case SPEEX_GET_DTX:
1272       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
1273       break;
1274    case SPEEX_GET_LOW_MODE:
1275       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1276       break;
1277    case SPEEX_SET_MODE:
1278       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1279       break;
1280    case SPEEX_SET_VBR:
1281       st->vbr_enabled = (*(spx_int32_t*)ptr);
1282       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1283       break;
1284    case SPEEX_GET_VBR:
1285       (*(spx_int32_t*)ptr) = st->vbr_enabled;
1286       break;
1287    case SPEEX_SET_VAD:
1288       st->vad_enabled = (*(spx_int32_t*)ptr);
1289       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1290       break;
1291    case SPEEX_GET_VAD:
1292       (*(spx_int32_t*)ptr) = st->vad_enabled;
1293       break;
1294    case SPEEX_SET_VBR_QUALITY:
1295       {
1296          spx_int32_t q;
1297          float qual = (*(float*)ptr)+.6;
1298          st->vbr_quality = (*(float*)ptr);
1299          if (qual>10)
1300             qual=10;
1301          q=(int)floor(.5+*(float*)ptr);
1302          if (q>10)
1303             q=10;
1304          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1305          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1306          break;
1307       }
1308    case SPEEX_GET_VBR_QUALITY:
1309       (*(float*)ptr) = st->vbr_quality;
1310       break;
1311    case SPEEX_SET_ABR:
1312       st->abr_enabled = (*(spx_int32_t*)ptr);
1313       st->vbr_enabled = st->abr_enabled!=0;
1314       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
1315       if (st->vbr_enabled) 
1316       {
1317          spx_int32_t i=10, rate, target;
1318          float vbr_qual;
1319          target = (*(spx_int32_t*)ptr);
1320          while (i>=0)
1321          {
1322             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1323             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1324             if (rate <= target)
1325                break;
1326             i--;
1327          }
1328          vbr_qual=i;
1329          if (vbr_qual<0)
1330             vbr_qual=0;
1331          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1332          st->abr_count=0;
1333          st->abr_drift=0;
1334          st->abr_drift2=0;
1335       }
1336       
1337       break;
1338    case SPEEX_GET_ABR:
1339       (*(spx_int32_t*)ptr) = st->abr_enabled;
1340       break;
1341    case SPEEX_SET_QUALITY:
1342       {
1343          spx_int32_t nb_qual;
1344          int quality = (*(spx_int32_t*)ptr);
1345          if (quality < 0)
1346             quality = 0;
1347          if (quality > 10)
1348             quality = 10;
1349          st->submodeSelect = st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1350          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1351          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1352       }
1353       break;
1354    case SPEEX_SET_COMPLEXITY:
1355       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1356       st->complexity = (*(spx_int32_t*)ptr);
1357       if (st->complexity<1)
1358          st->complexity=1;
1359       break;
1360    case SPEEX_GET_COMPLEXITY:
1361       (*(spx_int32_t*)ptr) = st->complexity;
1362       break;
1363    case SPEEX_SET_BITRATE:
1364       {
1365          spx_int32_t i=10;
1366          spx_int32_t rate, target;
1367          target = (*(spx_int32_t*)ptr);
1368          while (i>=0)
1369          {
1370             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1371             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1372             if (rate <= target)
1373                break;
1374             i--;
1375          }
1376       }
1377       break;
1378    case SPEEX_GET_BITRATE:
1379       speex_encoder_ctl(st->st_low, request, ptr);
1380       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1381       if (st->submodes[st->submodeID])
1382          (*(spx_int32_t*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1383       else
1384          (*(spx_int32_t*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1385       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1386       break;
1387    case SPEEX_SET_SAMPLING_RATE:
1388       {
1389          spx_int32_t tmp=(*(spx_int32_t*)ptr);
1390          st->sampling_rate = tmp;
1391          tmp>>=1;
1392          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1393       }
1394       break;
1395    case SPEEX_GET_SAMPLING_RATE:
1396       (*(spx_int32_t*)ptr)=st->sampling_rate;
1397       break;
1398    case SPEEX_RESET_STATE:
1399       {
1400          int i;
1401          st->first = 1;
1402          for (i=0;i<st->lpcSize;i++)
1403             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1404          for (i=0;i<st->lpcSize;i++)
1405             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1406          for (i=0;i<st->bufSize;i++)
1407             st->excBuf[i]=0;
1408          for (i=0;i<QMF_ORDER;i++)
1409             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1410       }
1411       break;
1412    case SPEEX_SET_SUBMODE_ENCODING:
1413       st->encode_submode = (*(spx_int32_t*)ptr);
1414       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1415       break;
1416    case SPEEX_GET_SUBMODE_ENCODING:
1417       (*(spx_int32_t*)ptr) = st->encode_submode;
1418       break;
1419    case SPEEX_GET_LOOKAHEAD:
1420       speex_encoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
1421       (*(spx_int32_t*)ptr) = 2*(*(spx_int32_t*)ptr) + QMF_ORDER - 1;
1422       break;
1423    case SPEEX_SET_PLC_TUNING:
1424       speex_encoder_ctl(st->st_low, SPEEX_SET_PLC_TUNING, ptr);
1425       break;
1426    case SPEEX_GET_PLC_TUNING:
1427       speex_encoder_ctl(st->st_low, SPEEX_GET_PLC_TUNING, ptr);
1428       break;
1429    case SPEEX_SET_VBR_MAX_BITRATE:
1430       {
1431          st->vbr_max = (*(spx_int32_t*)ptr);
1432          if (SPEEX_SET_VBR_MAX_BITRATE<1)
1433          {
1434             speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_MAX_BITRATE, &st->vbr_max);
1435             st->vbr_max_high = 17600;
1436          } else {
1437             spx_int32_t low_rate;
1438             if (st->vbr_max >= 42200)
1439             {
1440                st->vbr_max_high = 17600;
1441             } else if (st->vbr_max >= 27800)
1442             {
1443                st->vbr_max_high = 9600;
1444             } else if (st->vbr_max > 20600)
1445             {
1446                st->vbr_max_high = 5600;
1447             } else {
1448                st->vbr_max_high = 1800;
1449             }
1450             if (st->subframeSize==80)
1451                st->vbr_max_high = 1800;
1452             low_rate = st->vbr_max - st->vbr_max_high;
1453             speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_MAX_BITRATE, &low_rate);
1454          }
1455       }
1456       break;
1457    case SPEEX_GET_VBR_MAX_BITRATE:
1458       (*(spx_int32_t*)ptr) = st->vbr_max;
1459       break;
1460    case SPEEX_SET_HIGHPASS:
1461       speex_encoder_ctl(st->st_low, SPEEX_SET_HIGHPASS, ptr);
1462       break;
1463    case SPEEX_GET_HIGHPASS:
1464       speex_encoder_ctl(st->st_low, SPEEX_GET_HIGHPASS, ptr);
1465       break;
1466
1467
1468    /* This is all internal stuff past this point */
1469    case SPEEX_GET_PI_GAIN:
1470       {
1471          int i;
1472          spx_word32_t *g = (spx_word32_t*)ptr;
1473          for (i=0;i<st->nbSubframes;i++)
1474             g[i]=st->pi_gain[i];
1475       }
1476       break;
1477    case SPEEX_GET_EXC:
1478       {
1479          int i;
1480          spx_sig_t *e = (spx_sig_t*)ptr;
1481          for (i=0;i<st->full_frame_size;i++)
1482             e[i]=0;
1483          for (i=0;i<st->frame_size;i++)
1484             e[2*i]=2*st->exc[i];
1485       }
1486       break;
1487    case SPEEX_GET_INNOV:
1488       {
1489          int i;
1490          spx_sig_t *e = (spx_sig_t*)ptr;
1491          for (i=0;i<st->full_frame_size;i++)
1492             e[i]=0;
1493          for (i=0;i<st->frame_size;i++)
1494             e[2*i]=2*st->exc[i];
1495       }
1496       break;
1497    case SPEEX_GET_RELATIVE_QUALITY:
1498       (*(float*)ptr)=st->relative_quality;
1499       break;
1500    case SPEEX_SET_INNOVATION_SAVE:
1501       st->innov_save = (spx_sig_t*)ptr;
1502       break;
1503    case SPEEX_SET_WIDEBAND:
1504       speex_encoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, ptr);
1505       break;
1506
1507    default:
1508       speex_warning_int("Unknown nb_ctl request: ", request);
1509       return -1;
1510    }
1511    return 0;
1512 }
1513
1514 int sb_decoder_ctl(void *state, int request, void *ptr)
1515 {
1516    SBDecState *st;
1517    st=(SBDecState*)state;
1518    switch(request)
1519    {
1520    case SPEEX_SET_HIGH_MODE:
1521       st->submodeID = (*(spx_int32_t*)ptr);
1522       break;
1523    case SPEEX_SET_LOW_MODE:
1524       speex_decoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1525       break;
1526    case SPEEX_GET_LOW_MODE:
1527       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1528       break;
1529    case SPEEX_GET_FRAME_SIZE:
1530       (*(spx_int32_t*)ptr) = st->full_frame_size;
1531       break;
1532    case SPEEX_SET_ENH:
1533       speex_decoder_ctl(st->st_low, request, ptr);
1534       st->lpc_enh_enabled = *((spx_int32_t*)ptr);
1535       break;
1536    case SPEEX_GET_ENH:
1537       *((spx_int32_t*)ptr) = st->lpc_enh_enabled;
1538       break;
1539    case SPEEX_SET_MODE:
1540    case SPEEX_SET_QUALITY:
1541       {
1542          spx_int32_t nb_qual;
1543          int quality = (*(spx_int32_t*)ptr);
1544          if (quality < 0)
1545             quality = 0;
1546          if (quality > 10)
1547             quality = 10;
1548          st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1549          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1550          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1551       }
1552       break;
1553    case SPEEX_GET_BITRATE:
1554       speex_decoder_ctl(st->st_low, request, ptr);
1555       if (st->submodes[st->submodeID])
1556          (*(spx_int32_t*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1557       else
1558          (*(spx_int32_t*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1559       break;
1560    case SPEEX_SET_SAMPLING_RATE:
1561       {
1562          spx_int32_t tmp=(*(spx_int32_t*)ptr);
1563          st->sampling_rate = tmp;
1564          tmp>>=1;
1565          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1566       }
1567       break;
1568    case SPEEX_GET_SAMPLING_RATE:
1569       (*(spx_int32_t*)ptr)=st->sampling_rate;
1570       break;
1571    case SPEEX_SET_HANDLER:
1572       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1573       break;
1574    case SPEEX_SET_USER_HANDLER:
1575       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1576       break;
1577    case SPEEX_RESET_STATE:
1578       {
1579          int i;
1580          for (i=0;i<2*st->lpcSize;i++)
1581             st->mem_sp[i]=0;
1582          for (i=0;i<QMF_ORDER;i++)
1583             st->g0_mem[i]=st->g1_mem[i]=0;
1584       }
1585       break;
1586    case SPEEX_SET_SUBMODE_ENCODING:
1587       st->encode_submode = (*(spx_int32_t*)ptr);
1588       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1589       break;
1590    case SPEEX_GET_SUBMODE_ENCODING:
1591       (*(spx_int32_t*)ptr) = st->encode_submode;
1592       break;
1593    case SPEEX_GET_LOOKAHEAD:
1594       speex_decoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
1595       (*(spx_int32_t*)ptr) = 2*(*(spx_int32_t*)ptr);
1596       break;
1597    case SPEEX_SET_HIGHPASS:
1598       speex_decoder_ctl(st->st_low, SPEEX_SET_HIGHPASS, ptr);
1599       break;
1600    case SPEEX_GET_HIGHPASS:
1601       speex_decoder_ctl(st->st_low, SPEEX_GET_HIGHPASS, ptr);
1602       break;
1603
1604    case SPEEX_GET_PI_GAIN:
1605       {
1606          int i;
1607          spx_word32_t *g = (spx_word32_t*)ptr;
1608          for (i=0;i<st->nbSubframes;i++)
1609             g[i]=st->pi_gain[i];
1610       }
1611       break;
1612    case SPEEX_GET_EXC:
1613       {
1614          int i;
1615          spx_sig_t *e = (spx_sig_t*)ptr;
1616          for (i=0;i<st->full_frame_size;i++)
1617             e[i]=0;
1618          for (i=0;i<st->frame_size;i++)
1619             e[2*i]=2*st->exc[i];
1620       }
1621       break;
1622    case SPEEX_GET_INNOV:
1623       {
1624          int i;
1625          spx_sig_t *e = (spx_sig_t*)ptr;
1626          for (i=0;i<st->full_frame_size;i++)
1627             e[i]=0;
1628          for (i=0;i<st->frame_size;i++)
1629             e[2*i]=2*st->exc[i];
1630       }
1631       break;
1632    case SPEEX_GET_DTX_STATUS:
1633       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
1634       break;
1635    case SPEEX_SET_INNOVATION_SAVE:
1636       st->innov_save = (spx_sig_t*)ptr;
1637       break;
1638    case SPEEX_SET_WIDEBAND:
1639       speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, ptr);
1640       break;
1641
1642    default:
1643       speex_warning_int("Unknown nb_ctl request: ", request);
1644       return -1;
1645    }
1646    return 0;
1647 }
1648
1649 #endif
1650