Fixed (hopefully) once and for all the LSP root-finding problem. We now
[speexdsp.git] / libspeex / nb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: speex.c
3
4    This library is free software; you can redistribute it and/or
5    modify it under the terms of the GNU Lesser General Public
6    License as published by the Free Software Foundation; either
7    version 2.1 of the License, or (at your option) any later version.
8    
9    This library is distributed in the hope that it will be useful,
10    but WITHOUT ANY WARRANTY; without even the implied warranty of
11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12    Lesser General Public License for more details.
13    
14    You should have received a copy of the GNU Lesser General Public
15    License along with this library; if not, write to the Free Software
16    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17 */
18
19 #include <stdlib.h>
20 #include <stdio.h>
21 #include <math.h>
22 #include "nb_celp.h"
23 #include "lpc.h"
24 #include "lsp.h"
25 #include "ltp.h"
26 #include "quant_lsp.h"
27 #include "cb_search.h"
28 #include "filters.h"
29 #include "stack_alloc.h"
30 #include "vq.h"
31 #include "speex_bits.h"
32 #include "vbr.h"
33 #include "misc.h"
34
35 extern int training_weight;
36 #ifndef M_PI
37 #define M_PI           3.14159265358979323846  /* pi */
38 #endif
39
40 #define SUBMODE(x) st->submodes[st->submodeID]->x
41
42 float exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
43
44 float exc_gain_quant_scal1[2]={-0.35, 0.05};
45 /*float exc_gain_quant_scal1[2]={-0.35, 0.05};*/
46
47 #define sqr(x) ((x)*(x))
48 #define min(a,b) ((a) < (b) ? (a) : (b))
49
50 void *nb_encoder_init(SpeexMode *m)
51 {
52    EncState *st;
53    SpeexNBMode *mode;
54    int i;
55
56    mode=m->mode;
57    st = speex_alloc(sizeof(EncState));
58    st->mode=m;
59    /* Codec parameters, should eventually have several "modes"*/
60    st->frameSize = mode->frameSize;
61    st->windowSize = st->frameSize*3/2;
62    st->nbSubframes=mode->frameSize/mode->subframeSize;
63    st->subframeSize=mode->subframeSize;
64    st->lpcSize = mode->lpcSize;
65    st->bufSize = mode->bufSize;
66    st->gamma1=mode->gamma1;
67    st->gamma2=mode->gamma2;
68    st->min_pitch=mode->pitchStart;
69    st->max_pitch=mode->pitchEnd;
70    st->lag_factor=mode->lag_factor;
71    st->lpc_floor = mode->lpc_floor;
72    st->preemph = mode->preemph;
73   
74    st->submodes=mode->submodes;
75    st->submodeID=mode->defaultSubmode;
76    st->pre_mem=0;
77    st->pre_mem2=0;
78
79    /* Allocating input buffer */
80    st->inBuf = speex_alloc(st->bufSize*sizeof(float));
81    st->frame = st->inBuf + st->bufSize - st->windowSize;
82    /* Allocating excitation buffer */
83    st->excBuf = speex_alloc(st->bufSize*sizeof(float));
84    st->exc = st->excBuf + st->bufSize - st->windowSize;
85    st->swBuf = speex_alloc(st->bufSize*sizeof(float));
86    st->sw = st->swBuf + st->bufSize - st->windowSize;
87
88    st->exc2Buf = speex_alloc(st->bufSize*sizeof(float));
89    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
90
91    st->innov = speex_alloc(st->frameSize*sizeof(float));
92
93    /* Asymetric "pseudo-Hamming" window */
94    {
95       int part1, part2;
96       part1 = st->subframeSize*7/2;
97       part2 = st->subframeSize*5/2;
98       st->window = speex_alloc(st->windowSize*sizeof(float));
99       for (i=0;i<part1;i++)
100          st->window[i]=.54-.46*cos(M_PI*i/part1);
101       for (i=0;i<part2;i++)
102          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
103    }
104    /* Create the window for autocorrelation (lag-windowing) */
105    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(float));
106    for (i=0;i<st->lpcSize+1;i++)
107       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
108
109    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(float));
110
111    st->stack = speex_alloc(20000*sizeof(float));
112
113    st->buf2 = speex_alloc(st->windowSize*sizeof(float));
114
115    st->lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
116    st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
117    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
118    st->bw_lpc1 = speex_alloc((st->lpcSize+1)*sizeof(float));
119    st->bw_lpc2 = speex_alloc((st->lpcSize+1)*sizeof(float));
120
121    st->lsp = speex_alloc(st->lpcSize*sizeof(float));
122    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
123    st->old_lsp = speex_alloc(st->lpcSize*sizeof(float));
124    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
125    st->interp_lsp = speex_alloc(st->lpcSize*sizeof(float));
126    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
127    st->rc = speex_alloc(st->lpcSize*sizeof(float));
128    st->first = 1;
129
130    st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
131    st->mem_sw = speex_alloc(st->lpcSize*sizeof(float));
132    st->mem_sw_whole = speex_alloc(st->lpcSize*sizeof(float));
133    st->mem_exc = speex_alloc(st->lpcSize*sizeof(float));
134
135    st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
136
137    st->pitch = speex_alloc(st->nbSubframes*sizeof(int));
138
139    if (1) {
140       st->vbr = speex_alloc(sizeof(VBRState));
141       vbr_init(st->vbr);
142       st->vbr_quality = 8;
143       st->vbr_enabled = 0;
144    } else {
145       st->vbr = 0;
146    }
147    st->complexity=2;
148
149    return st;
150 }
151
152 void nb_encoder_destroy(void *state)
153 {
154    EncState *st=state;
155    /* Free all allocated memory */
156    speex_free(st->inBuf);
157    speex_free(st->excBuf);
158    speex_free(st->swBuf);
159    speex_free(st->exc2Buf);
160    speex_free(st->innov);
161    speex_free(st->stack);
162
163    speex_free(st->window);
164    speex_free(st->buf2);
165    speex_free(st->lpc);
166    speex_free(st->interp_lpc);
167    speex_free(st->interp_qlpc);
168    
169    speex_free(st->bw_lpc1);
170    speex_free(st->bw_lpc2);
171    speex_free(st->autocorr);
172    speex_free(st->lagWindow);
173    speex_free(st->lsp);
174    speex_free(st->qlsp);
175    speex_free(st->old_lsp);
176    speex_free(st->interp_lsp);
177    speex_free(st->old_qlsp);
178    speex_free(st->interp_qlsp);
179    speex_free(st->rc);
180
181    speex_free(st->mem_sp);
182    speex_free(st->mem_sw);
183    speex_free(st->mem_sw_whole);
184    speex_free(st->mem_exc);
185    speex_free(st->pi_gain);
186    speex_free(st->pitch);
187
188    vbr_destroy(st->vbr);
189    speex_free(st->vbr);
190
191    /*Free state memory... should be last*/
192    speex_free(st);
193 }
194
195 void nb_encode(void *state, float *in, SpeexBits *bits)
196 {
197    EncState *st;
198    int i, sub, roots;
199    float error;
200    int ol_pitch;
201    float ol_pitch_coef;
202    float ol_gain;
203    float delta_qual=0;
204
205    st=state;
206    
207    /* Copy new data in input buffer */
208    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
209    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
210    for (i=1;i<st->frameSize;i++)
211       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
212    st->pre_mem = in[st->frameSize-1];
213
214    /* Move signals 1 frame towards the past */
215    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
216    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
217    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
218
219
220    /* Window for analysis */
221    for (i=0;i<st->windowSize;i++)
222       st->buf2[i] = st->frame[i] * st->window[i];
223
224    /* Compute auto-correlation */
225    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
226
227    st->autocorr[0] += 10;        /* prevents NANs */
228    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
229
230    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
231    for (i=0;i<st->lpcSize+1;i++)
232       st->autocorr[i] *= st->lagWindow[i];
233
234    /* Levinson-Durbin */
235    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
236    st->lpc[0]=1;
237
238    /* LPC to LSPs (x-domain) transform */
239    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, st->stack);
240    if (roots!=st->lpcSize)
241    {
242       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, st->stack);
243       if (roots!=st->lpcSize) {
244          /*fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);*/
245
246          /*If we can't find all LSP's, do some damage control and use a flat filter*/
247          for (i=0;i<st->lpcSize;i++)
248          {
249             st->lsp[i]=cos(M_PI*((float)(i+1))/(st->lpcSize+1));
250          }
251       }
252    }
253
254
255    /* LSP x-domain to angle domain*/
256    for (i=0;i<st->lpcSize;i++)
257       st->lsp[i] = acos(st->lsp[i]);
258    /*print_vec(st->lsp, 10, "LSP:");*/
259    /* LSP Quantization */
260    if (st->first)
261    {
262       for (i=0;i<st->lpcSize;i++)
263          st->old_lsp[i] = st->lsp[i];
264    }
265
266    if (0) {
267       float dd=0;
268       for (i=0;i<st->lpcSize;i++)
269          dd += fabs(st->old_lsp[i] - st->lsp[i]);
270       printf ("lspdist = %f\n", dd);
271    }
272
273    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
274    {
275       for (i=0;i<st->lpcSize;i++)
276          st->interp_lsp[i] = .5*st->old_lsp[i] + .5*st->lsp[i];
277
278       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
279
280       /* Compute interpolated LPCs (unquantized) for whole frame*/
281       for (i=0;i<st->lpcSize;i++)
282          st->interp_lsp[i] = cos(st->interp_lsp[i]);
283       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
284
285       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
286       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
287
288       filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
289
290       /*Open-loop pitch*/
291       {
292          int nol_pitch[4];
293          float nol_pitch_coef[4];
294          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
295                                nol_pitch, nol_pitch_coef, 4, st->stack);
296          ol_pitch=nol_pitch[0];
297          ol_pitch_coef = nol_pitch_coef[0];
298          /*Try to remove pitch multiples*/
299          for (i=1;i<4;i++)
300          {
301             if ((nol_pitch_coef[i] > .85*ol_pitch_coef) && 
302                 (fabs(2*nol_pitch[i]-ol_pitch)<=2 || fabs(3*nol_pitch[i]-ol_pitch)<=4 || 
303                  fabs(4*nol_pitch[i]-ol_pitch)<=6 || fabs(5*nol_pitch[i]-ol_pitch)<=8))
304             {
305                /*ol_pitch_coef=nol_pitch_coef[i];*/
306                ol_pitch = nol_pitch[i];
307             }
308          }
309          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
310          /*printf ("ol_pitch: %d %f\n", ol_pitch, ol_pitch_coef);*/
311       }
312       /*Compute "real" excitation*/
313       fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);
314
315       /* Compute open-loop excitation gain */
316       ol_gain=0;
317       for (i=0;i<st->frameSize;i++)
318          ol_gain += st->exc[i]*st->exc[i];
319       
320       ol_gain=sqrt(1+ol_gain/st->frameSize);
321    }
322
323    /*Experimental VBR stuff*/
324    if (st->vbr)
325    {
326       delta_qual = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
327       /*if (delta_qual<0)*/
328          delta_qual*=.1*(3+st->vbr_quality);
329       if (st->vbr_enabled) 
330       {
331          int qual = (int)floor(st->vbr_quality+delta_qual+.5);
332          if (qual<1 && delta_qual>-3.5)
333             qual=1;
334          if (qual<0)
335             qual=0;
336          if (qual>10)
337             qual=10;
338          if (qual==10 && st->vbr_quality<10)
339             qual=9;
340          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &qual);
341       }
342    }
343    /*printf ("VBR quality = %f\n", vbr_qual);*/
344
345    /* First, transmit a zero for narrowband */
346    speex_bits_pack(bits, 0, 1);
347
348    /* Transmit the sub-mode we use for this frame */
349    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
350
351
352    /* If null mode (no transmission), just set a couple things to zero*/
353    if (st->submodes[st->submodeID] == NULL)
354    {
355       for (i=0;i<st->frameSize;i++)
356          st->exc[i]=st->exc2[i]=st->sw[i]=0;
357
358       for (i=0;i<st->lpcSize;i++)
359          st->mem_sw[i]=0;
360       st->first=1;
361
362       /* Final signal synthesis from excitation */
363       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
364
365       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
366       for (i=1;i<st->frameSize;i++)
367          in[i]=st->frame[i] + st->preemph*in[i-1];
368       st->pre_mem2=in[st->frameSize-1];
369
370       return;
371
372    }
373
374    /*Quantize LSPs*/
375 #if 1 /*0 for unquantized*/
376    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
377 #else
378    for (i=0;i<st->lpcSize;i++)
379      st->qlsp[i]=st->lsp[i];
380 #endif
381
382    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
383    if (SUBMODE(lbr_pitch)!=-1)
384    {
385       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
386    } 
387    
388    if (SUBMODE(forced_pitch_gain))
389    {
390       int quant;
391       quant = (int)floor(.5+15*ol_pitch_coef);
392       if (quant>15)
393          quant=0;
394       if (quant<0)
395          quant=0;
396       speex_bits_pack(bits, quant, 4);
397       ol_pitch_coef=0.066667*quant;
398    }
399    
400    
401    /*Quantize and transmit open-loop excitation gain*/
402    {
403       int qe = (int)(floor(3.5*log(ol_gain)));
404       if (qe<0)
405          qe=0;
406       if (qe>31)
407          qe=31;
408       ol_gain = exp(qe/3.5);
409       speex_bits_pack(bits, qe, 5);
410    }
411
412    /* Special case for first frame */
413    if (st->first)
414    {
415       for (i=0;i<st->lpcSize;i++)
416          st->old_qlsp[i] = st->qlsp[i];
417    }
418
419    /* Loop on sub-frames */
420    for (sub=0;sub<st->nbSubframes;sub++)
421    {
422       float esig, enoise, snr, tmp;
423       int   offset;
424       float *sp, *sw, *res, *exc, *target, *mem, *exc2;
425       int pitch;
426
427       /* Offset relative to start of frame */
428       offset = st->subframeSize*sub;
429       /* Original signal */
430       sp=st->frame+offset;
431       /* Excitation */
432       exc=st->exc+offset;
433       /* Weighted signal */
434       sw=st->sw+offset;
435
436       exc2=st->exc2+offset;
437
438       /* Filter response */
439       res = PUSH(st->stack, st->subframeSize);
440       /* Target signal */
441       target = PUSH(st->stack, st->subframeSize);
442       mem = PUSH(st->stack, st->lpcSize);
443
444       /* LSP interpolation (quantized and unquantized) */
445       tmp = (1.0 + sub)/st->nbSubframes;
446       for (i=0;i<st->lpcSize;i++)
447          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
448       for (i=0;i<st->lpcSize;i++)
449          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
450
451       /* Make sure the filters are stable */
452       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
453       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
454
455       /* Compute interpolated LPCs (quantized and unquantized) */
456       for (i=0;i<st->lpcSize;i++)
457          st->interp_lsp[i] = cos(st->interp_lsp[i]);
458       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
459
460       for (i=0;i<st->lpcSize;i++)
461          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
462       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
463
464       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
465       tmp=1;
466       st->pi_gain[sub]=0;
467       for (i=0;i<=st->lpcSize;i++)
468       {
469          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
470          tmp = -tmp;
471       }
472      
473
474       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
475       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
476       if (st->gamma2>=0)
477          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
478       else
479       {
480          st->bw_lpc2[0]=1;
481          st->bw_lpc2[1]=-st->preemph;
482          for (i=2;i<=st->lpcSize;i++)
483             st->bw_lpc2[i]=0;
484       }
485
486       /* Reset excitation */
487       for (i=0;i<st->subframeSize;i++)
488          exc[i]=0;
489       for (i=0;i<st->subframeSize;i++)
490          exc2[i]=0;
491
492       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
493       for (i=0;i<st->lpcSize;i++)
494          mem[i]=st->mem_sp[i];
495       iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
496       
497
498       for (i=0;i<st->lpcSize;i++)
499          mem[i]=st->mem_sw[i];
500       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
501
502       /* Compute weighted signal */
503       for (i=0;i<st->lpcSize;i++)
504          mem[i]=st->mem_sw[i];
505       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
506
507       esig=0;
508       for (i=0;i<st->subframeSize;i++)
509          esig+=sw[i]*sw[i];
510       
511       /* Compute target signal */
512       for (i=0;i<st->subframeSize;i++)
513          target[i]=sw[i]-res[i];
514
515       for (i=0;i<st->subframeSize;i++)
516          exc[i]=exc2[i]=0;
517
518       /* If we have a long-term predictor (not all sub-modes have one) */
519       if (SUBMODE(ltp_quant))
520       {
521          int pit_min, pit_max;
522          /* Long-term prediction */
523          if (SUBMODE(lbr_pitch) != -1)
524          {
525             /* Low bit-rate pitch handling */
526             int margin;
527             margin = SUBMODE(lbr_pitch);
528             if (margin)
529             {
530                if (ol_pitch < st->min_pitch+margin-1)
531                   ol_pitch=st->min_pitch+margin-1;
532                if (ol_pitch > st->max_pitch-margin)
533                   ol_pitch=st->max_pitch-margin;
534                pit_min = ol_pitch-margin+1;
535                pit_max = ol_pitch+margin;
536             } else {
537                pit_min=pit_max=ol_pitch;
538             }
539          } else {
540             pit_min = st->min_pitch;
541             pit_max = st->max_pitch;
542          }
543
544          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
545                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
546                                     st->lpcSize, st->subframeSize, bits, st->stack, 
547                                     exc2, st->complexity);
548
549          /*printf ("cl_pitch: %d\n", pitch);*/
550          st->pitch[sub]=pitch;
551       } else {
552          fprintf (stderr, "No pitch prediction, what's wrong\n");
553       }
554
555       /* Update target for adaptive codebook contribution */
556       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
557       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
558       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
559       for (i=0;i<st->subframeSize;i++)
560         target[i]-=res[i];
561
562       /* Compute noise energy and SNR */
563       enoise=0;
564       for (i=0;i<st->subframeSize;i++)
565          enoise += target[i]*target[i];
566       snr = 10*log10((esig+1)/(enoise+1));
567       /*st->pitch[sub]=(int)snr;*/
568 #ifdef DEBUG
569       printf ("pitch SNR = %f\n", snr);
570 #endif
571
572
573       /* Quantization of innovation */
574       {
575          float *innov;
576          float ener=0, ener_1;
577          /*innov=PUSH(st->stack, st->subframeSize);*/
578          innov = st->innov+sub*st->subframeSize;
579          for (i=0;i<st->subframeSize;i++)
580             innov[i]=0;
581          syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
582          residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
583          residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
584          for (i=0;i<st->subframeSize;i++)
585             ener+=st->buf2[i]*st->buf2[i];
586          ener=sqrt(.1+ener/st->subframeSize);
587
588          
589          ener /= ol_gain;
590
591          if (0)
592             printf ("ener: %f %f %f\n", ener, ol_gain, ol_pitch_coef);
593
594          if (SUBMODE(have_subframe_gain)) 
595          {
596             int qe;
597             ener=log(ener);
598             if (SUBMODE(have_subframe_gain)==3)
599             {
600                qe = vq_index(&ener, exc_gain_quant_scal3, 1, 8);
601                speex_bits_pack(bits, qe, 3);
602                ener=exc_gain_quant_scal3[qe];
603             } else {
604                qe = vq_index(&ener, exc_gain_quant_scal1, 1, 2);
605                speex_bits_pack(bits, qe, 1);
606                ener=exc_gain_quant_scal1[qe];               
607             }
608             ener=exp(ener);
609             /*printf ("encode gain: %d %f\n", qe, ener);*/
610          } else {
611             ener=1;
612          }
613
614          ener*=ol_gain;
615          /*printf ("transmit gain: %f\n", ener);*/
616          ener_1 = 1/ener;
617
618          if (0) {
619             int start=rand()%35;
620             printf ("norm_exc: ");
621             for (i=start;i<start+5;i++)
622                printf ("%f ", ener_1*st->buf2[i]);
623             printf ("\n");
624          }
625          
626          for (i=0;i<st->subframeSize;i++)
627             target[i]*=ener_1;
628          
629          if (SUBMODE(innovation_quant))
630          {
631             /* Normal quantization */
632             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
633                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
634                                       innov, bits, st->stack, st->complexity);
635             for (i=0;i<st->subframeSize;i++)
636                innov[i]*=ener;
637             for (i=0;i<st->subframeSize;i++)
638                exc[i] += innov[i];
639          } else {
640             fprintf(stderr, "No fixed codebook\n");
641          }
642
643          if (SUBMODE(double_codebook)) {
644             float *innov2 = PUSH(st->stack, st->subframeSize);
645             for (i=0;i<st->subframeSize;i++)
646                innov2[i]=0;
647             for (i=0;i<st->subframeSize;i++)
648                target[i]*=2.2;
649             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
650                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
651                                       innov2, bits, st->stack, st->complexity);
652             for (i=0;i<st->subframeSize;i++)
653                innov2[i]*=ener*(1/2.2);
654             for (i=0;i<st->subframeSize;i++)
655                exc[i] += innov2[i];
656             POP(st->stack);
657          }
658
659          /*POP(st->stack);*/
660          for (i=0;i<st->subframeSize;i++)
661             target[i]*=ener;
662
663       }
664
665       /* Compute weighted noise energy and SNR */
666       enoise=0;
667       for (i=0;i<st->subframeSize;i++)
668          enoise += target[i]*target[i];
669       snr = 10*log10((esig+1)/(enoise+1));
670 #ifdef DEBUG
671       printf ("seg SNR = %f\n", snr);
672 #endif
673
674       /*Keep the previous memory*/
675       for (i=0;i<st->lpcSize;i++)
676          mem[i]=st->mem_sp[i];
677       /* Final signal synthesis from excitation */
678       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
679
680       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
681       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
682       for (i=0;i<st->subframeSize;i++)
683          exc2[i]=exc[i];
684
685       POP(st->stack);
686       POP(st->stack);
687       POP(st->stack);
688    }
689
690    /* Store the LSPs for interpolation in the next frame */
691    for (i=0;i<st->lpcSize;i++)
692       st->old_lsp[i] = st->lsp[i];
693    for (i=0;i<st->lpcSize;i++)
694       st->old_qlsp[i] = st->qlsp[i];
695
696    /* The next frame will not be the first (Duh!) */
697    st->first = 0;
698
699    /* Replace input by synthesized speech */
700    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
701    for (i=1;i<st->frameSize;i++)
702      in[i]=st->frame[i] + st->preemph*in[i-1];
703    st->pre_mem2=in[st->frameSize-1];
704
705 }
706
707
708 void *nb_decoder_init(SpeexMode *m)
709 {
710    DecState *st;
711    SpeexNBMode *mode;
712    int i;
713
714    mode=m->mode;
715    st = speex_alloc(sizeof(DecState));
716    st->mode=m;
717
718    st->first=1;
719    /* Codec parameters, should eventually have several "modes"*/
720    st->frameSize = mode->frameSize;
721    st->windowSize = st->frameSize*3/2;
722    st->nbSubframes=mode->frameSize/mode->subframeSize;
723    st->subframeSize=mode->subframeSize;
724    st->lpcSize = mode->lpcSize;
725    st->bufSize = mode->bufSize;
726    st->gamma1=mode->gamma1;
727    st->gamma2=mode->gamma2;
728    st->min_pitch=mode->pitchStart;
729    st->max_pitch=mode->pitchEnd;
730    st->preemph = mode->preemph;
731
732    st->submodes=mode->submodes;
733    st->submodeID=mode->defaultSubmode;
734
735    st->pre_mem=0;
736    st->lpc_enh_enabled=0;
737
738    st->stack = speex_alloc(20000*sizeof(float));
739
740    st->inBuf = speex_alloc(st->bufSize*sizeof(float));
741    st->frame = st->inBuf + st->bufSize - st->windowSize;
742    st->excBuf = speex_alloc(st->bufSize*sizeof(float));
743    st->exc = st->excBuf + st->bufSize - st->windowSize;
744    for (i=0;i<st->bufSize;i++)
745       st->inBuf[i]=0;
746    for (i=0;i<st->bufSize;i++)
747       st->excBuf[i]=0;
748    st->innov = speex_alloc(st->frameSize*sizeof(float));
749
750    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
751    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
752    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
753    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
754    st->mem_sp = speex_alloc(5*st->lpcSize*sizeof(float));
755
756    st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
757    st->last_pitch = 40;
758    st->count_lost=0;
759    return st;
760 }
761
762 void nb_decoder_destroy(void *state)
763 {
764    DecState *st;
765    st=state;
766    speex_free(st->inBuf);
767    speex_free(st->excBuf);
768    speex_free(st->innov);
769    speex_free(st->interp_qlpc);
770    speex_free(st->qlsp);
771    speex_free(st->old_qlsp);
772    speex_free(st->interp_qlsp);
773    speex_free(st->stack);
774    speex_free(st->mem_sp);
775    speex_free(st->pi_gain);
776    
777    speex_free(state);
778 }
779
780 static void nb_decode_lost(DecState *st, float *out)
781 {
782    int i, sub;
783    /*float exc_ener=0,g;*/
784    /* Shift all buffers by one frame */
785    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
786    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
787
788    for (sub=0;sub<st->nbSubframes;sub++)
789    {
790       int offset;
791       float *sp, *exc;
792       float *num, *den;
793       /* Offset relative to start of frame */
794       offset = st->subframeSize*sub;
795       /* Original signal */
796       sp=st->frame+offset;
797       /* Excitation */
798       exc=st->exc+offset;
799       /* Excitation after post-filter*/
800
801       num=PUSH(st->stack, ((st->lpcSize<<1)+1));
802       den=PUSH(st->stack, ((st->lpcSize<<1)+1));
803       if (st->lpc_enh_enabled)
804       {
805          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
806                  SUBMODE(lpc_enh_k1), SUBMODE(lpc_enh_k2), st->stack);
807       } else {
808          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
809                  SUBMODE(lpc_enh_k2), SUBMODE(lpc_enh_k2), st->stack);
810       }
811         
812       for (i=0;i<st->subframeSize;i++)
813       {
814          exc[i]=st->last_pitch_gain*exc[i-st->last_pitch] + 
815          .8*st->innov[i+offset];
816       }
817
818       for (i=0;i<st->subframeSize;i++)
819          sp[i]=exc[i];
820       
821       /*pole_zero_mem(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
822                     st->mem_sp+st->lpcSize, st->stack);*/
823       filter_mem2(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
824         st->mem_sp+st->lpcSize);
825       iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
826         st->mem_sp);
827       
828       POP(st->stack);
829       POP(st->stack);
830   
831    }
832
833    out[0] = st->frame[0] + st->preemph*st->pre_mem;
834    for (i=1;i<st->frameSize;i++)
835       out[i]=st->frame[i] + st->preemph*out[i-1];
836    st->pre_mem=out[st->frameSize-1];
837    
838    st->first = 0;
839    st->count_lost++;
840 }
841
842
843 int nb_decode(void *state, SpeexBits *bits, float *out)
844 {
845    DecState *st;
846    int i, sub;
847    int pitch;
848    float pitch_gain[3];
849    float ol_gain;
850    int ol_pitch=0;
851    float ol_pitch_coef=0;
852    int best_pitch=40;
853    float best_pitch_gain=-1;
854    int wideband;
855    int m;
856
857    st=state;
858
859    if (!bits)
860    {
861       nb_decode_lost(st, out);
862       return 0;
863    }
864
865    m = speex_bits_peek_unsigned(bits, 5);
866    if (m==15)
867    {
868       return -1;
869    } else if (m==14)
870    {
871       int req_size = speex_bits_unpack_unsigned(bits, 6);
872       speex_bits_advance(bits, 8*req_size);
873    } else if (m>7)
874    {
875       return -2;
876    }
877
878    wideband = speex_bits_unpack_unsigned(bits, 1);
879    if (wideband)
880    {
881       int submode;
882       int advance;
883       submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
884       advance = submode;
885       speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
886       advance -= (SB_SUBMODE_BITS+1);
887       speex_bits_advance(bits, advance);
888       wideband = speex_bits_unpack_unsigned(bits, 1);
889       if (wideband)
890       {
891          fprintf (stderr, "Corrupted stream\n");
892       }
893    }
894
895    /* Get the sub-mode that was used */
896    st->submodeID = speex_bits_unpack_unsigned(bits, NB_SUBMODE_BITS);
897
898    /* Shift all buffers by one frame */
899    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
900    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
901
902    /* If null mode (no transmission), just set a couple things to zero*/
903    if (st->submodes[st->submodeID] == NULL)
904    {
905       for (i=0;i<st->frameSize;i++)
906          st->exc[i]=0;
907       st->first=1;
908       
909       /* Final signal synthesis from excitation */
910       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
911
912       out[0] = st->frame[0] + st->preemph*st->pre_mem;
913       for (i=1;i<st->frameSize;i++)
914          out[i]=st->frame[i] + st->preemph*out[i-1];
915       st->pre_mem=out[st->frameSize-1];
916       st->count_lost=0;
917       return 0;
918    }
919
920    /* Unquantize LSPs */
921    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
922
923    /* Handle first frame and lost-packet case */
924    if (st->first || st->count_lost)
925    {
926       for (i=0;i<st->lpcSize;i++)
927          st->old_qlsp[i] = st->qlsp[i];
928    }
929
930    /* Get open-loop pitch estimation for low bit-rate pitch coding */
931    if (SUBMODE(lbr_pitch)!=-1)
932    {
933       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
934    } 
935    
936    if (SUBMODE(forced_pitch_gain))
937    {
938       int quant;
939       quant = speex_bits_unpack_unsigned(bits, 4);
940       ol_pitch_coef=0.066667*quant;
941       /*fprintf (stderr, "unquant pitch coef: %f\n", ol_pitch_coef);*/
942    }
943    
944    /* Get global excitation gain */
945    {
946       int qe;
947       qe = speex_bits_unpack_unsigned(bits, 5);
948       ol_gain = exp(qe/3.5);
949       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
950    }
951
952    /*Loop on subframes */
953    for (sub=0;sub<st->nbSubframes;sub++)
954    {
955       int offset;
956       float *sp, *exc, tmp;
957       float *num, *den;
958       /* Offset relative to start of frame */
959       offset = st->subframeSize*sub;
960       /* Original signal */
961       sp=st->frame+offset;
962       /* Excitation */
963       exc=st->exc+offset;
964       /* Excitation after post-filter*/
965
966       /* LSP interpolation (quantized and unquantized) */
967       tmp = (1.0 + sub)/st->nbSubframes;
968       for (i=0;i<st->lpcSize;i++)
969          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
970
971       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
972
973
974       /* Compute interpolated LPCs (unquantized) */
975       for (i=0;i<st->lpcSize;i++)
976          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
977       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
978
979       num=PUSH(st->stack, ((st->lpcSize<<1)+1));
980       den=PUSH(st->stack, ((st->lpcSize<<1)+1));
981       if (st->lpc_enh_enabled)
982       {
983          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
984                  SUBMODE(lpc_enh_k1), SUBMODE(lpc_enh_k2), st->stack);
985       } else {
986          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
987                  SUBMODE(lpc_enh_k2), SUBMODE(lpc_enh_k2), st->stack);
988       }
989       /* Compute analysis filter at w=pi */
990       tmp=1;
991       st->pi_gain[sub]=0;
992       for (i=0;i<=st->lpcSize;i++)
993       {
994          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
995          tmp = -tmp;
996       }
997
998       /* Reset excitation */
999       for (i=0;i<st->subframeSize;i++)
1000          exc[i]=0;
1001
1002       /*Adaptive codebook contribution*/
1003       if (SUBMODE(ltp_unquant))
1004       {
1005          int pit_min, pit_max;
1006          if (SUBMODE(lbr_pitch) != -1)
1007          {
1008             int margin;
1009             margin = SUBMODE(lbr_pitch);
1010             if (margin)
1011             {
1012                if (ol_pitch < st->min_pitch+margin-1)
1013                   ol_pitch=st->min_pitch+margin-1;
1014                if (ol_pitch > st->max_pitch-margin)
1015                   ol_pitch=st->max_pitch-margin;
1016                pit_min = ol_pitch-margin+1;
1017                pit_max = ol_pitch+margin;
1018             } else {
1019                pit_min=pit_max=ol_pitch;
1020             }
1021          } else {
1022             pit_min = st->min_pitch;
1023             pit_max = st->max_pitch;
1024          }
1025
1026          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1027                               st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, st->count_lost);
1028          
1029          tmp = (pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1030          if (tmp>best_pitch_gain)
1031          {
1032             best_pitch = pitch;
1033             /*while (best_pitch+pitch<st->max_pitch)
1034             {
1035                best_pitch+=pitch;
1036                }*/
1037             best_pitch_gain = tmp*.9;
1038             if (best_pitch_gain>.85)
1039                best_pitch_gain=.85;
1040          }
1041       } else {
1042          fprintf (stderr, "No pitch prediction, what's wrong\n");
1043       }
1044       
1045       /* Unquantize the innovation */
1046       {
1047          int q_energy;
1048          float ener;
1049          float *innov;
1050          
1051          /*innov = PUSH(st->stack, st->subframeSize);*/
1052          innov = st->innov+sub*st->subframeSize;
1053          for (i=0;i<st->subframeSize;i++)
1054             innov[i]=0;
1055
1056          if (SUBMODE(have_subframe_gain)==3)
1057          {
1058             q_energy = speex_bits_unpack_unsigned(bits, 3);
1059             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1060          } else if (SUBMODE(have_subframe_gain)==1)
1061          {
1062             q_energy = speex_bits_unpack_unsigned(bits, 1);
1063             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1064          } else {
1065             ener = ol_gain;
1066          }
1067          
1068          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
1069          
1070          if (SUBMODE(innovation_unquant))
1071          {
1072             /*Fixed codebook contribution*/
1073             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
1074          } else {
1075             fprintf(stderr, "No fixed codebook\n");
1076          }
1077
1078          for (i=0;i<st->subframeSize;i++)
1079             innov[i]*=ener;
1080          for (i=0;i<st->subframeSize;i++)
1081             exc[i]+=innov[i];
1082
1083          if (SUBMODE(double_codebook))
1084          {
1085             float *innov2 = PUSH(st->stack, st->subframeSize);
1086             for (i=0;i<st->subframeSize;i++)
1087                innov2[i]=0;
1088             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
1089             for (i=0;i<st->subframeSize;i++)
1090                innov2[i]*=ener*(1/2.2);
1091             for (i=0;i<st->subframeSize;i++)
1092                exc[i] += innov2[i];
1093             POP(st->stack);
1094          }
1095
1096          /*POP(st->stack);*/
1097       }
1098
1099       for (i=0;i<st->subframeSize;i++)
1100          sp[i]=exc[i];
1101
1102       if (st->lpc_enh_enabled && SUBMODE(comb_gain>0))
1103          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1104                               pitch, pitch_gain, .5);
1105       /*pole_zero_mem(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
1106         st->mem_sp+st->lpcSize, st->stack);*/
1107       filter_mem2(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
1108         st->mem_sp+st->lpcSize);
1109       iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1110         st->mem_sp);
1111       
1112       POP(st->stack);
1113       POP(st->stack);
1114    }
1115    
1116    /*Copy output signal*/
1117    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1118    for (i=1;i<st->frameSize;i++)
1119      out[i]=st->frame[i] + st->preemph*out[i-1];
1120    st->pre_mem=out[st->frameSize-1];
1121
1122
1123    /* Store the LSPs for interpolation in the next frame */
1124    for (i=0;i<st->lpcSize;i++)
1125       st->old_qlsp[i] = st->qlsp[i];
1126
1127    /* The next frame will not be the first (Duh!) */
1128    st->first = 0;
1129    st->count_lost=0;
1130    st->last_pitch = best_pitch;
1131    st->last_pitch_gain = best_pitch_gain;
1132
1133    return 0;
1134 }
1135
1136 void nb_encoder_ctl(void *state, int request, void *ptr)
1137 {
1138    EncState *st;
1139    st=state;     
1140    switch(request)
1141    {
1142    case SPEEX_GET_FRAME_SIZE:
1143       (*(int*)ptr) = st->frameSize;
1144       break;
1145    case SPEEX_SET_MODE:
1146       st->submodeID = (*(int*)ptr);
1147       break;
1148    case SPEEX_GET_MODE:
1149       (*(int*)ptr) = st->submodeID;
1150       break;
1151    case SPEEX_SET_VBR:
1152       st->vbr_enabled = (*(int*)ptr);
1153       break;
1154    case SPEEX_GET_VBR:
1155       (*(int*)ptr) = st->vbr_enabled;
1156       break;
1157    case SPEEX_SET_VBR_QUALITY:
1158       st->vbr_quality = (*(int*)ptr);
1159       break;
1160    case SPEEX_GET_VBR_QUALITY:
1161       (*(int*)ptr) = st->vbr_quality;
1162       break;
1163    case SPEEX_SET_QUALITY:
1164       {
1165          int quality = (*(int*)ptr);
1166          if (quality<=0)
1167             st->submodeID = 0;
1168          else if (quality<=1)
1169             st->submodeID = 1;
1170          else if (quality<=2)
1171             st->submodeID = 2;
1172          else if (quality<=4)
1173             st->submodeID = 3;
1174          else if (quality<=6)
1175             st->submodeID = 4;
1176          else if (quality<=8)
1177             st->submodeID = 5;
1178          else if (quality<=9)
1179             st->submodeID = 6;
1180          else if (quality<=10)
1181             st->submodeID = 7;
1182          else
1183             fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);
1184       }
1185       break;
1186    case SPEEX_SET_COMPLEXITY:
1187       st->complexity = (*(int*)ptr);
1188       break;
1189    case SPEEX_GET_COMPLEXITY:
1190       (*(int*)ptr) = st->complexity;
1191       break;
1192    case SPEEX_GET_BITRATE:
1193       if (st->submodes[st->submodeID])
1194          (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
1195       else
1196          (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
1197       break;
1198    default:
1199       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1200    }
1201 }
1202
1203 void nb_decoder_ctl(void *state, int request, void *ptr)
1204 {
1205    DecState *st;
1206    st=state;
1207    switch(request)
1208    {
1209    case SPEEX_SET_ENH:
1210       st->lpc_enh_enabled = *((int*)ptr);
1211       break;
1212    case SPEEX_GET_ENH:
1213       *((int*)ptr) = st->lpc_enh_enabled;
1214       break;
1215    case SPEEX_GET_FRAME_SIZE:
1216       (*(int*)ptr) = st->frameSize;
1217       break;
1218    case SPEEX_GET_BITRATE:
1219       if (st->submodes[st->submodeID])
1220          (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
1221       else
1222          (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
1223       break;
1224    default:
1225       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1226    }
1227 }