b4662b605035e34451ba664f82c47b28e96ab2f8
[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->submodeID=mode->defaultSubmode;
149    
150    {
151       /* FIXME: Should do this without explicit reference to a mode */
152 #if 0
153       int mod=6;
154       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &mod);
155 #else
156       int mod=9;
157       speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &mod);
158 #endif
159    }
160
161    st->lag_factor = mode->lag_factor;
162    st->lpc_floor = mode->lpc_floor;
163    st->gamma1=mode->gamma1;
164    st->gamma2=mode->gamma2;
165    st->first=1;
166    st->stack = speex_alloc(20000*sizeof(float));
167
168    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
169    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
170    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
171    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
172    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
173
174    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
175    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
176    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
177    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
178
179    st->buf=(float*)speex_alloc(st->windowSize*sizeof(float));
180    st->excBuf=(float*)speex_alloc(st->bufSize*sizeof(float));
181    st->exc = st->excBuf + st->bufSize - st->windowSize;
182
183    st->res=(float*)speex_alloc(st->frame_size*sizeof(float));
184    st->sw=(float*)speex_alloc(st->frame_size*sizeof(float));
185    st->target=(float*)speex_alloc(st->frame_size*sizeof(float));
186    /*Asymetric "pseudo-Hamming" window*/
187    {
188       int part1, part2;
189       part1 = st->subframeSize*7/2;
190       part2 = st->subframeSize*5/2;
191       st->window = (float*)speex_alloc(st->windowSize*sizeof(float));
192       for (i=0;i<part1;i++)
193          st->window[i]=.54-.46*cos(M_PI*i/part1);
194       for (i=0;i<part2;i++)
195          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
196    }
197
198    st->lagWindow = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
199    for (i=0;i<st->lpcSize+1;i++)
200       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
201
202    st->rc = (float*)speex_alloc(st->lpcSize*sizeof(float));
203    st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
204    st->lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
205    st->bw_lpc1 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
206    st->bw_lpc2 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
207    st->lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
208    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
209    st->old_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
210    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
211    st->interp_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
212    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
213    st->interp_lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
214    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
215    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
216
217    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
218    st->mem_sp2 = (float*)speex_alloc(st->lpcSize*sizeof(float));
219    st->mem_sw = (float*)speex_alloc(st->lpcSize*sizeof(float));
220
221    st->vbr_quality = 8;
222    st->vbr_enabled = 0;
223
224    st->complexity=2;
225    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
226    st->sampling_rate*=2;
227
228    return st;
229 }
230
231 void sb_encoder_destroy(void *state)
232 {
233    SBEncState *st=(SBEncState*)state;
234
235    speex_encoder_destroy(st->st_low);
236    speex_free(st->x0d);
237    speex_free(st->x1d);
238    speex_free(st->high);
239    speex_free(st->y0);
240    speex_free(st->y1);
241    speex_free(st->h0_mem);
242    speex_free(st->h1_mem);
243    speex_free(st->g0_mem);
244    speex_free(st->g1_mem);
245    
246    speex_free(st->buf);
247    speex_free(st->window);
248    speex_free(st->excBuf);
249    speex_free(st->sw);
250    speex_free(st->res);
251    speex_free(st->target);
252    speex_free(st->lagWindow);
253    speex_free(st->rc);
254    speex_free(st->autocorr);
255    speex_free(st->lpc);
256    speex_free(st->bw_lpc1);
257    speex_free(st->bw_lpc2);
258    speex_free(st->lsp);
259    speex_free(st->qlsp);
260    speex_free(st->old_lsp);
261    speex_free(st->old_qlsp);
262    speex_free(st->interp_lsp);
263    speex_free(st->interp_qlsp);
264    speex_free(st->interp_lpc);
265    speex_free(st->interp_qlpc);
266
267    speex_free(st->mem_sp);
268    speex_free(st->mem_sp2);
269    speex_free(st->mem_sw);
270    speex_free(st->pi_gain);
271
272    speex_free(st->stack);
273
274    speex_free(st);
275 }
276
277
278 void sb_encode(void *state, float *in, SpeexBits *bits)
279 {
280    SBEncState *st;
281    int i, roots, sub;
282    void *stack;
283    float *mem, *innov, *syn_resp;
284    float *low_pi_gain, *low_exc, *low_innov;
285
286    st = (SBEncState*)state;
287    stack=st->stack;
288
289    /* Compute the two sub-bands by filtering with h0 and h1*/
290    qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
291     
292    /* Encode the narrowband part*/
293    speex_encode(st->st_low, st->x0d, bits);
294
295    /* High-band buffering / sync with low band */
296    for (i=0;i<st->windowSize-st->frame_size;i++)
297       st->high[i] = st->high[st->frame_size+i];
298    for (i=0;i<st->frame_size;i++)
299       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
300
301    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
302
303
304    low_pi_gain = PUSH(stack, st->nbSubframes, float);
305    low_exc = PUSH(stack, st->frame_size, float);
306    low_innov = PUSH(stack, st->frame_size, float);
307    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
308    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
309    speex_encoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
310    
311    /* Start encoding the high-band */
312    for (i=0;i<st->windowSize;i++)
313       st->buf[i] = st->high[i] * st->window[i];
314
315    /* Compute auto-correlation */
316    _spx_autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
317
318    st->autocorr[0] += 1;        /* prevents NANs */
319    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
320    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
321    for (i=0;i<st->lpcSize+1;i++)
322       st->autocorr[i] *= st->lagWindow[i];
323
324    /* Levinson-Durbin */
325    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
326    st->lpc[0]=1;
327
328    /* LPC to LSPs (x-domain) transform */
329    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
330    if (roots!=st->lpcSize)
331    {
332       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, stack);
333       if (roots!=st->lpcSize) {
334          /*fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);*/
335
336          /*If we can't find all LSP's, do some damage control and use a flat filter*/
337          for (i=0;i<st->lpcSize;i++)
338          {
339             st->lsp[i]=cos(M_PI*((float)(i+1))/(st->lpcSize+1));
340          }
341       }
342    }
343
344    /* x-domain to angle domain*/
345    for (i=0;i<st->lpcSize;i++)
346       st->lsp[i] = acos(st->lsp[i]);
347
348    /* VBR code */
349    if (0){
350       float e_low=0, e_high=0;
351       float ratio;
352       float low_qual;
353       for (i=0;i<st->frame_size;i++)
354       {
355          e_low  += st->x0d[i]* st->x0d[i];
356          e_high += st->high[i]* st->high[i];
357       }
358       ratio = log((1+e_high)/(1+e_low));
359       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &low_qual);
360       if (ratio<-4)
361          ratio=-4;
362       if (ratio>0)
363          ratio=0;
364       /*if (ratio>-2)*/
365       low_qual+=1.0*(ratio+2);
366       /*{
367          int high_mode=2;
368          if (low_qual>10)
369             high_mode=4;
370          else if (low_qual>7.5)
371             high_mode=3;
372          else if (low_qual>5)
373             high_mode=2;
374          speex_encoder_ctl(st, SPEEX_SET_HIGH_MODE, &high_mode);
375       }*/
376       {
377          int mode;
378          mode = 4;
379          while (mode)
380          {
381             int v1;
382             float thresh;
383             v1=(int)floor(st->vbr_quality);
384             if (v1==10)
385                thresh = vbr_nb_thresh[mode][v1];
386             else
387                thresh = (st->vbr_quality-v1)*vbr_hb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_hb_thresh[mode][v1];
388             if (low_qual > thresh)
389                break;
390             mode--;
391          }
392          fprintf (stderr, "%f %d\n", low_qual, mode);
393          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &mode);
394       }
395       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
396    }
397
398    speex_bits_pack(bits, 1, 1);
399    speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
400
401    /* If null mode (no transmission), just set a couple things to zero*/
402    if (st->submodes[st->submodeID] == NULL)
403    {
404       for (i=0;i<st->frame_size;i++)
405          st->exc[i]=st->sw[i]=0;
406
407       for (i=0;i<st->lpcSize;i++)
408          st->mem_sw[i]=0;
409       st->first=1;
410
411       /* Final signal synthesis from excitation */
412       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
413
414 #ifndef RELEASE
415
416       /* Reconstruct the original */
417       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
418       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
419
420       for (i=0;i<st->full_frame_size;i++)
421          in[i]=2*(st->y0[i]-st->y1[i]);
422 #endif
423
424       return;
425
426    }
427
428
429    /* LSP quantization */
430    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
431    
432    /*printf ("high_lsp:");
433    for (i=0;i<st->lpcSize;i++)
434       printf (" %f", st->lsp[i]);
435       printf ("\n");*/
436    /*for (i=0;i<st->lpcSize;i++)
437      st->qlsp[i]=st->lsp[i];*/
438    
439
440    if (st->first)
441    {
442       for (i=0;i<st->lpcSize;i++)
443          st->old_lsp[i] = st->lsp[i];
444       for (i=0;i<st->lpcSize;i++)
445          st->old_qlsp[i] = st->qlsp[i];
446    }
447    
448    mem=PUSH(stack, st->lpcSize, float);
449    syn_resp=PUSH(stack, st->subframeSize, float);
450    innov = PUSH(stack, st->subframeSize, float);
451
452    for (sub=0;sub<st->nbSubframes;sub++)
453    {
454       float *exc, *sp, *res, *target, *sw, tmp, filter_ratio;
455       int offset;
456       float rl, rh, eh=0, el=0;
457       int fold;
458
459       offset = st->subframeSize*sub;
460       sp=st->high+offset;
461       exc=st->exc+offset;
462       res=st->res+offset;
463       target=st->target+offset;
464       sw=st->sw+offset;
465       
466       /* LSP interpolation (quantized and unquantized) */
467       tmp = (1.0 + sub)/st->nbSubframes;
468       for (i=0;i<st->lpcSize;i++)
469          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
470       for (i=0;i<st->lpcSize;i++)
471          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
472       
473       /* Compute interpolated LPCs (quantized and unquantized) */
474       for (i=0;i<st->lpcSize;i++)
475          st->interp_lsp[i] = cos(st->interp_lsp[i]);
476       for (i=0;i<st->lpcSize;i++)
477          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
478
479       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
480       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
481
482       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
483       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
484
485       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
486       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
487
488       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
489          filters */
490       rl=rh=0;
491       tmp=1;
492       st->pi_gain[sub]=0;
493       for (i=0;i<=st->lpcSize;i++)
494       {
495          rh += tmp*st->interp_qlpc[i];
496          tmp = -tmp;
497          st->pi_gain[sub]+=st->interp_qlpc[i];
498       }
499       rl = low_pi_gain[sub];
500       rl=1/(fabs(rl)+.01);
501       rh=1/(fabs(rh)+.01);
502       /* Compute ratio, will help predict the gain */
503       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
504
505       fold = filter_ratio<5;
506       /*printf ("filter_ratio %f\n", filter_ratio);*/
507       fold=0;
508
509       /* Compute "real excitation" */
510       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
511       /* Compute energy of low-band and high-band excitation */
512       for (i=0;i<st->subframeSize;i++)
513          eh+=sqr(exc[i]);
514
515       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
516          float g;
517          /*speex_bits_pack(bits, 1, 1);*/
518          for (i=0;i<st->subframeSize;i++)
519             el+=sqr(low_innov[offset+i]);
520
521          /* Gain to use if we want to use the low-band excitation for high-band */
522          g=eh/(.01+el);
523          g=sqrt(g);
524
525          g *= filter_ratio;
526
527          /* Gain quantization */
528          {
529             int quant = (int) floor(.5 + 27 + 8.0 * log((g+.0001)));
530             if (quant<0)
531                quant=0;
532             if (quant>31)
533                quant=31;
534             speex_bits_pack(bits, quant, 5);
535             g= .1*exp(quant/9.4);
536          }
537          /*printf ("folding gain: %f\n", g);*/
538          g /= filter_ratio;
539
540       } else {
541          float gc, scale, scale_1;
542
543          for (i=0;i<st->subframeSize;i++)
544             el+=sqr(low_exc[offset+i]);
545          /*speex_bits_pack(bits, 0, 1);*/
546
547          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
548          {
549             int qgc = (int)floor(.5+3.7*(log(gc)+2));
550             if (qgc<0)
551                qgc=0;
552             if (qgc>15)
553                qgc=15;
554             speex_bits_pack(bits, qgc, 4);
555             gc = exp((1/3.7)*qgc-2);
556          }
557
558          scale = gc*sqrt(1+el)/filter_ratio;
559          scale_1 = 1/scale;
560          if (0 && rand()%5==0)
561          {
562             float sc = 1/sqrt(.1+eh/st->subframeSize);
563             if (rand()&1)
564                sc=-sc;
565             for (i=0;i<st->subframeSize;i++)
566             {
567                float tmp=exc[i]*sc;
568                if (i%8==0)
569                   printf ("\nhexc");
570                printf (" %f", tmp);
571             }
572          }
573
574          for (i=0;i<st->subframeSize;i++)
575             exc[i]=0;
576          exc[0]=1;
577          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
578          
579          /* Reset excitation */
580          for (i=0;i<st->subframeSize;i++)
581             exc[i]=0;
582          
583          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
584          for (i=0;i<st->lpcSize;i++)
585             mem[i]=st->mem_sp[i];
586          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
587
588          for (i=0;i<st->lpcSize;i++)
589             mem[i]=st->mem_sw[i];
590          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
591
592          /* Compute weighted signal */
593          for (i=0;i<st->lpcSize;i++)
594             mem[i]=st->mem_sw[i];
595          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
596
597          /* Compute target signal */
598          for (i=0;i<st->subframeSize;i++)
599             target[i]=sw[i]-res[i];
600
601          for (i=0;i<st->subframeSize;i++)
602            exc[i]=0;
603
604
605          for (i=0;i<st->subframeSize;i++)
606             target[i]*=scale_1;
607          
608          /* Reset excitation */
609          for (i=0;i<st->subframeSize;i++)
610             innov[i]=0;
611
612          /*print_vec(target, st->subframeSize, "\ntarget");*/
613          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
614                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
615                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
616          /*print_vec(target, st->subframeSize, "after");*/
617
618          for (i=0;i<st->subframeSize;i++)
619             exc[i] += innov[i]*scale;
620
621          if (SUBMODE(double_codebook)) {
622             void *tmp_stack=stack;
623             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
624             for (i=0;i<st->subframeSize;i++)
625                innov2[i]=0;
626             for (i=0;i<st->subframeSize;i++)
627                target[i]*=2.5;
628             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
629                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
630                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
631             for (i=0;i<st->subframeSize;i++)
632                innov2[i]*=scale*(1/2.5);
633             for (i=0;i<st->subframeSize;i++)
634                exc[i] += innov2[i];
635          }
636
637
638          if (0) {
639             float en=0;
640             for (i=0;i<st->subframeSize;i++)
641                en+=exc[i]*exc[i];
642             en=sqrt(eh/(1+en));
643             for (i=0;i<st->subframeSize;i++)
644                exc[i]*=en;
645             printf ("correction high: %f\n", en);
646          }
647
648       }
649
650          /*Keep the previous memory*/
651          for (i=0;i<st->lpcSize;i++)
652             mem[i]=st->mem_sp[i];
653          /* Final signal synthesis from excitation */
654          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
655          
656          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
657          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
658    }
659
660
661 #ifndef RELEASE
662
663    /* Reconstruct the original */
664    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
665    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
666
667    for (i=0;i<st->full_frame_size;i++)
668       in[i]=2*(st->y0[i]-st->y1[i]);
669 #endif
670    for (i=0;i<st->lpcSize;i++)
671       st->old_lsp[i] = st->lsp[i];
672    for (i=0;i<st->lpcSize;i++)
673       st->old_qlsp[i] = st->qlsp[i];
674
675    st->first=0;
676 }
677
678
679
680
681
682 void *sb_decoder_init(SpeexMode *m)
683 {
684    SBDecState *st;
685    SpeexSBMode *mode;
686    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
687    st->mode = m;
688    mode=(SpeexSBMode*)m->mode;
689
690    st->st_low = speex_decoder_init(mode->nb_mode);
691    st->full_frame_size = 2*mode->frameSize;
692    st->frame_size = mode->frameSize;
693    st->subframeSize = mode->subframeSize;
694    st->nbSubframes = mode->frameSize/mode->subframeSize;
695    st->lpcSize=8;
696    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
697    st->sampling_rate*=2;
698
699    st->submodes=mode->submodes;
700    st->submodeID=mode->defaultSubmode;
701
702    st->first=1;
703    st->stack = speex_alloc(20000*sizeof(float));
704
705    st->x0d=(float*)speex_alloc(st->frame_size*sizeof(float));
706    st->x1d=(float*)speex_alloc(st->frame_size*sizeof(float));
707    st->high=(float*)speex_alloc(st->full_frame_size*sizeof(float));
708    st->y0=(float*)speex_alloc(st->full_frame_size*sizeof(float));
709    st->y1=(float*)speex_alloc(st->full_frame_size*sizeof(float));
710
711    st->h0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
712    st->h1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
713    st->g0_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
714    st->g1_mem=(float*)speex_alloc(QMF_ORDER*sizeof(float));
715
716    st->exc=(float*)speex_alloc(st->frame_size*sizeof(float));
717
718    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
719    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
720    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
721    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
722
723    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
724    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
725    return st;
726 }
727
728 void sb_decoder_destroy(void *state)
729 {
730    SBDecState *st;
731    st = (SBDecState*)state;
732    speex_decoder_destroy(st->st_low);
733    speex_free(st->x0d);
734    speex_free(st->x1d);
735    speex_free(st->high);
736    speex_free(st->y0);
737    speex_free(st->y1);
738    speex_free(st->h0_mem);
739    speex_free(st->h1_mem);
740    speex_free(st->g0_mem);
741    speex_free(st->g1_mem);
742    
743    speex_free(st->exc);
744    speex_free(st->qlsp);
745    speex_free(st->old_qlsp);
746    speex_free(st->interp_qlsp);
747    speex_free(st->interp_qlpc);
748    speex_free(st->pi_gain);
749
750    speex_free(st->mem_sp);
751
752    speex_free(st->stack);
753
754    speex_free(state);
755 }
756
757 static void sb_decode_lost(SBDecState *st, float *out, void *stack)
758 {
759    int i;
760    for (i=0;i<st->frame_size;i++)
761       st->exc[i]*=0.8;
762    
763    st->first=1;
764    
765    /* Final signal synthesis from excitation */
766    iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
767    
768    /* Reconstruct the original */
769    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
770    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
771
772    for (i=0;i<st->full_frame_size;i++)
773       out[i]=2*(st->y0[i]-st->y1[i]);
774    
775    return;
776 }
777
778 int sb_decode(void *state, SpeexBits *bits, float *out)
779 {
780    int i, sub;
781    SBDecState *st;
782    int wideband;
783    int ret;
784    void *stack;
785    float *low_pi_gain, *low_exc, *low_innov;
786
787    st = (SBDecState*)state;
788    stack=st->stack;
789
790    /* Decode the low-band */
791    ret = speex_decode(st->st_low, bits, st->x0d);
792
793    /* If error decoding the narrowband part, propagate error */
794    if (ret!=0)
795    {
796       return ret;
797    }
798
799    if (!bits)
800    {
801       sb_decode_lost(st, out, stack);
802       return 0;
803    }
804
805    /*Check "wideband bit"*/
806    wideband = speex_bits_peek(bits);
807    if (wideband)
808    {
809       /*Regular wideband frame, read the submode*/
810       wideband = speex_bits_unpack_unsigned(bits, 1);
811       st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
812    } else
813    {
814       /*Was a narrowband frame, set "null submode"*/
815       st->submodeID = 0;
816    }
817
818    for (i=0;i<st->frame_size;i++)
819       st->exc[i]=0;
820
821    /* If null mode (no transmission), just set a couple things to zero*/
822    if (st->submodes[st->submodeID] == NULL)
823    {
824       for (i=0;i<st->frame_size;i++)
825          st->exc[i]=0;
826
827       st->first=1;
828
829       /* Final signal synthesis from excitation */
830       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
831
832       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
833       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
834
835       for (i=0;i<st->full_frame_size;i++)
836          out[i]=2*(st->y0[i]-st->y1[i]);
837
838       return 0;
839
840    }
841
842    low_pi_gain = PUSH(stack, st->nbSubframes, float);
843    low_exc = PUSH(stack, st->frame_size, float);
844    low_innov = PUSH(stack, st->frame_size, float);
845    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
846    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
847    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
848
849    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
850    
851    if (st->first)
852    {
853       for (i=0;i<st->lpcSize;i++)
854          st->old_qlsp[i] = st->qlsp[i];
855    }
856    
857    for (sub=0;sub<st->nbSubframes;sub++)
858    {
859       float *exc, *sp, tmp, filter_ratio, el=0;
860       int offset;
861       float rl=0,rh=0;
862       
863       offset = st->subframeSize*sub;
864       sp=st->high+offset;
865       exc=st->exc+offset;
866       
867       /* LSP interpolation */
868       tmp = (1.0 + sub)/st->nbSubframes;
869       for (i=0;i<st->lpcSize;i++)
870          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
871
872       /* LSPs to x-domain */
873       for (i=0;i<st->lpcSize;i++)
874          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
875
876       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
877
878       /* LSP to LPC */
879       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
880
881       /* Calculate reponse ratio between the low and high filter in the middle
882          of the band (4000 Hz) */
883       
884          tmp=1;
885          st->pi_gain[sub]=0;
886          for (i=0;i<=st->lpcSize;i++)
887          {
888             rh += tmp*st->interp_qlpc[i];
889             tmp = -tmp;
890             st->pi_gain[sub]+=st->interp_qlpc[i];
891          }
892          rl = low_pi_gain[sub];
893          rl=1/(fabs(rl)+.01);
894          rh=1/(fabs(rh)+.01);
895          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
896       
897       
898       for (i=0;i<st->subframeSize;i++)
899          exc[i]=0;
900       if (!SUBMODE(innovation_unquant))
901       {
902          float g;
903          int quant;
904
905          for (i=0;i<st->subframeSize;i++)
906             el+=sqr(low_innov[offset+i]);
907          quant = speex_bits_unpack_unsigned(bits, 5);
908          g= exp(((float)quant-27)/8.0);
909          
910          /*printf ("unquant folding gain: %f\n", g);*/
911          g /= filter_ratio;
912          
913          /* High-band excitation using the low-band excitation and a gain */
914          for (i=0;i<st->subframeSize;i++)
915             exc[i]=.8*g*low_innov[offset+i];
916       } else {
917          float gc, scale;
918          int qgc = speex_bits_unpack_unsigned(bits, 4);
919          for (i=0;i<st->subframeSize;i++)
920             el+=sqr(low_exc[offset+i]);
921
922
923          gc = exp((1/3.7)*qgc-2);
924
925          scale = gc*sqrt(1+el)/filter_ratio;
926
927
928          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
929                                 bits, stack);
930          for (i=0;i<st->subframeSize;i++)
931             exc[i]*=scale;
932
933          if (SUBMODE(double_codebook)) {
934             void *tmp_stack=stack;
935             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
936             for (i=0;i<st->subframeSize;i++)
937                innov2[i]=0;
938             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
939                                 bits, tmp_stack);
940             for (i=0;i<st->subframeSize;i++)
941                innov2[i]*=scale*(1/2.5);
942             for (i=0;i<st->subframeSize;i++)
943                exc[i] += innov2[i];
944          }
945
946       }
947       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
948
949    }
950
951    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
952    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
953
954    for (i=0;i<st->full_frame_size;i++)
955       out[i]=2*(st->y0[i]-st->y1[i]);
956
957    for (i=0;i<st->lpcSize;i++)
958       st->old_qlsp[i] = st->qlsp[i];
959
960    st->first=0;
961
962    return 0;
963 }
964
965
966 void sb_encoder_ctl(void *state, int request, void *ptr)
967 {
968    SBEncState *st;
969    st=(SBEncState*)state;
970    switch(request)
971    {
972    case SPEEX_GET_FRAME_SIZE:
973       (*(int*)ptr) = st->full_frame_size;
974       break;
975    case SPEEX_SET_HIGH_MODE:
976       st->submodeID = (*(int*)ptr);
977       break;
978    case SPEEX_SET_LOW_MODE:
979       speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, ptr);
980       break;
981    case SPEEX_SET_MODE:
982       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
983       break;
984    case SPEEX_SET_VBR:
985       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
986       break;
987    case SPEEX_SET_VBR_QUALITY:
988       {
989          int q;
990          float qual = (*(float*)ptr)+.5;
991          st->vbr_quality = (*(float*)ptr);
992          if (qual>10)
993             qual=10;
994          q=(int)floor(.5+*(float*)ptr);
995          if (q>10)
996             q=10;
997          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
998          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
999          break;
1000       }
1001    case SPEEX_SET_QUALITY:
1002       {
1003          int nb_qual;
1004          int quality = (*(int*)ptr);
1005          if (quality < 0)
1006             quality = 0;
1007          if (quality > 10)
1008             quality = 10;
1009          st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1010          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1011          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1012       }
1013       break;
1014    case SPEEX_SET_COMPLEXITY:
1015       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1016       st->complexity = (*(int*)ptr);
1017       if (st->complexity<1)
1018          st->complexity=1;
1019       break;
1020    case SPEEX_GET_COMPLEXITY:
1021       (*(int*)ptr) = st->complexity;
1022       break;
1023    case SPEEX_SET_BITRATE:
1024       {
1025          int i=10, rate, target;
1026          target = (*(int*)ptr);
1027          while (i>=1)
1028          {
1029             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1030             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1031             if (rate <= target)
1032                break;
1033             i--;
1034          }
1035       }
1036       break;
1037    case SPEEX_GET_BITRATE:
1038       speex_encoder_ctl(st->st_low, request, ptr);
1039       if (st->submodes[st->submodeID])
1040          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1041       else
1042          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1043       break;
1044    case SPEEX_SET_SAMPLING_RATE:
1045       {
1046          int tmp=(*(int*)ptr);
1047          st->sampling_rate = tmp;
1048          tmp>>=1;
1049          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1050       }
1051       break;
1052    case SPEEX_GET_SAMPLING_RATE:
1053       (*(int*)ptr)=st->sampling_rate;
1054       break;
1055    case SPEEX_GET_PI_GAIN:
1056       {
1057          int i;
1058          float *g = (float*)ptr;
1059          for (i=0;i<st->nbSubframes;i++)
1060             g[i]=st->pi_gain[i];
1061       }
1062       break;
1063    case SPEEX_GET_EXC:
1064       {
1065          int i;
1066          float *e = (float*)ptr;
1067          for (i=0;i<st->full_frame_size;i++)
1068             e[i]=0;
1069          for (i=0;i<st->frame_size;i++)
1070             e[2*i]=2*st->exc[i];
1071       }
1072       break;
1073    case SPEEX_GET_INNOV:
1074       {
1075          int i;
1076          float *e = (float*)ptr;
1077          for (i=0;i<st->full_frame_size;i++)
1078             e[i]=0;
1079          for (i=0;i<st->frame_size;i++)
1080             e[2*i]=2*st->exc[i];
1081       }
1082       break;
1083    default:
1084       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1085    }
1086
1087 }
1088
1089 void sb_decoder_ctl(void *state, int request, void *ptr)
1090 {
1091    SBDecState *st;
1092    st=(SBDecState*)state;
1093    switch(request)
1094    {
1095    case SPEEX_GET_FRAME_SIZE:
1096       (*(int*)ptr) = st->full_frame_size;
1097       break;
1098    case SPEEX_SET_ENH:
1099       speex_decoder_ctl(st->st_low, request, ptr);
1100       break;
1101    case SPEEX_GET_BITRATE:
1102       speex_decoder_ctl(st->st_low, request, ptr);
1103       if (st->submodes[st->submodeID])
1104          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1105       else
1106          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1107       break;
1108    case SPEEX_SET_SAMPLING_RATE:
1109       {
1110          int tmp=(*(int*)ptr);
1111          st->sampling_rate = tmp;
1112          tmp>>=1;
1113          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1114       }
1115       break;
1116    case SPEEX_GET_SAMPLING_RATE:
1117       (*(int*)ptr)=st->sampling_rate;
1118       break;
1119    case SPEEX_SET_HANDLER:
1120       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1121       break;
1122    case SPEEX_SET_USER_HANDLER:
1123       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1124       break;
1125    case SPEEX_GET_PI_GAIN:
1126       {
1127          int i;
1128          float *g = (float*)ptr;
1129          for (i=0;i<st->nbSubframes;i++)
1130             g[i]=st->pi_gain[i];
1131       }
1132       break;
1133    case SPEEX_GET_EXC:
1134       {
1135          int i;
1136          float *e = (float*)ptr;
1137          for (i=0;i<st->full_frame_size;i++)
1138             e[i]=0;
1139          for (i=0;i<st->frame_size;i++)
1140             e[2*i]=2*st->exc[i];
1141       }
1142       break;
1143    case SPEEX_GET_INNOV:
1144       {
1145          int i;
1146          float *e = (float*)ptr;
1147          for (i=0;i<st->full_frame_size;i++)
1148             e[i]=0;
1149          for (i=0;i<st->frame_size;i++)
1150             e[2*i]=2*st->exc[i];
1151       }
1152       break;
1153    default:
1154       fprintf(stderr, "Unknown sb_ctl request: %d\n", request);
1155    }
1156
1157 }