added encoder complexity option
[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 "post_filter.h"
33 #include "vbr.h"
34 #include "misc.h"
35
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_scal[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
43
44 #define sqr(x) ((x)*(x))
45 #define min(a,b) ((a) < (b) ? (a) : (b))
46
47 void *nb_encoder_init(SpeexMode *m)
48 {
49    EncState *st;
50    SpeexNBMode *mode;
51    int i;
52
53    mode=m->mode;
54    st = speex_alloc(sizeof(EncState));
55    st->mode=m;
56    /* Codec parameters, should eventually have several "modes"*/
57    st->frameSize = mode->frameSize;
58    st->windowSize = st->frameSize*3/2;
59    st->nbSubframes=mode->frameSize/mode->subframeSize;
60    st->subframeSize=mode->subframeSize;
61    st->lpcSize = mode->lpcSize;
62    st->bufSize = mode->bufSize;
63    st->gamma1=mode->gamma1;
64    st->gamma2=mode->gamma2;
65    st->min_pitch=mode->pitchStart;
66    st->max_pitch=mode->pitchEnd;
67    st->lag_factor=mode->lag_factor;
68    st->lpc_floor = mode->lpc_floor;
69    st->preemph = mode->preemph;
70   
71    st->submodes=mode->submodes;
72    st->submodeID=mode->defaultSubmode;
73    st->pre_mem=0;
74    st->pre_mem2=0;
75
76    /* Allocating input buffer */
77    st->inBuf = speex_alloc(st->bufSize*sizeof(float));
78    st->frame = st->inBuf + st->bufSize - st->windowSize;
79    /* Allocating excitation buffer */
80    st->excBuf = speex_alloc(st->bufSize*sizeof(float));
81    st->exc = st->excBuf + st->bufSize - st->windowSize;
82    st->swBuf = speex_alloc(st->bufSize*sizeof(float));
83    st->sw = st->swBuf + st->bufSize - st->windowSize;
84
85    st->exc2Buf = speex_alloc(st->bufSize*sizeof(float));
86    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
87
88    /* Asymetric "pseudo-Hamming" window */
89    {
90       int part1, part2;
91       part1 = st->subframeSize*7/2;
92       part2 = st->subframeSize*5/2;
93       st->window = speex_alloc(st->windowSize*sizeof(float));
94       for (i=0;i<part1;i++)
95          st->window[i]=.54-.46*cos(M_PI*i/part1);
96       for (i=0;i<part2;i++)
97          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
98    }
99    /* Create the window for autocorrelation (lag-windowing) */
100    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(float));
101    for (i=0;i<st->lpcSize+1;i++)
102       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
103
104    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(float));
105
106    st->stack = speex_alloc(20000*sizeof(float));
107
108    st->buf2 = speex_alloc(st->windowSize*sizeof(float));
109
110    st->lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
111    st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
112    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
113    st->bw_lpc1 = speex_alloc((st->lpcSize+1)*sizeof(float));
114    st->bw_lpc2 = speex_alloc((st->lpcSize+1)*sizeof(float));
115
116    st->lsp = speex_alloc(st->lpcSize*sizeof(float));
117    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
118    st->old_lsp = speex_alloc(st->lpcSize*sizeof(float));
119    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
120    st->interp_lsp = speex_alloc(st->lpcSize*sizeof(float));
121    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
122    st->rc = speex_alloc(st->lpcSize*sizeof(float));
123    st->first = 1;
124
125    st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
126    st->mem_sw = speex_alloc(st->lpcSize*sizeof(float));
127
128    st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
129
130    st->pitch = speex_alloc(st->nbSubframes*sizeof(int));
131
132    if (1) {
133       st->vbr = speex_alloc(sizeof(VBRState));
134       vbr_init(st->vbr);
135       st->vbr_quality = 8;
136       st->vbr_enabled = 0;
137    } else {
138       st->vbr = 0;
139    }
140    st->complexity=2;
141
142    return st;
143 }
144
145 void nb_encoder_destroy(void *state)
146 {
147    EncState *st=state;
148    /* Free all allocated memory */
149    speex_free(st->inBuf);
150    speex_free(st->excBuf);
151    speex_free(st->swBuf);
152    speex_free(st->exc2Buf);
153    speex_free(st->stack);
154
155    speex_free(st->window);
156    speex_free(st->buf2);
157    speex_free(st->lpc);
158    speex_free(st->interp_lpc);
159    speex_free(st->interp_qlpc);
160    
161    speex_free(st->bw_lpc1);
162    speex_free(st->bw_lpc2);
163    speex_free(st->autocorr);
164    speex_free(st->lagWindow);
165    speex_free(st->lsp);
166    speex_free(st->qlsp);
167    speex_free(st->old_lsp);
168    speex_free(st->interp_lsp);
169    speex_free(st->old_qlsp);
170    speex_free(st->interp_qlsp);
171    speex_free(st->rc);
172
173    speex_free(st->mem_sp);
174    speex_free(st->mem_sw);
175    speex_free(st->pi_gain);
176    speex_free(st->pitch);
177
178    vbr_destroy(st->vbr);
179    speex_free(st->vbr);
180
181    /*Free state memory... should be last*/
182    speex_free(st);
183 }
184
185 void nb_encode(void *state, float *in, SpeexBits *bits)
186 {
187    EncState *st;
188    int i, sub, roots;
189    float error;
190    int ol_pitch;
191    float ol_pitch_coef;
192    float ol_gain;
193    float delta_qual=0;
194
195    st=state;
196    
197    /* Copy new data in input buffer */
198    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
199    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
200    for (i=1;i<st->frameSize;i++)
201       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
202    st->pre_mem = in[st->frameSize-1];
203
204    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
205    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
206    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
207
208    /* Window for analysis */
209    for (i=0;i<st->windowSize;i++)
210       st->buf2[i] = st->frame[i] * st->window[i];
211
212    /* Compute auto-correlation */
213    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
214
215    st->autocorr[0] += 1;        /* prevents NANs */
216    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
217    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
218    for (i=0;i<st->lpcSize+1;i++)
219       st->autocorr[i] *= st->lagWindow[i];
220
221    /* Levinson-Durbin */
222    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
223    st->lpc[0]=1;
224
225    /* LPC to LSPs (x-domain) transform */
226    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
227    if (roots!=st->lpcSize)
228    {
229       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
230       exit(1);
231    }
232
233    /* x-domain to angle domain*/
234    for (i=0;i<st->lpcSize;i++)
235       st->lsp[i] = acos(st->lsp[i]);
236    /*print_vec(st->lsp, 10, "LSP:");*/
237    /* LSP Quantization */
238    if (st->first)
239    {
240       for (i=0;i<st->lpcSize;i++)
241          st->old_lsp[i] = st->lsp[i];
242    }
243
244
245    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
246    {
247       for (i=0;i<st->lpcSize;i++)
248          st->interp_lsp[i] = .5*st->old_lsp[i] + .5*st->lsp[i];
249
250       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
251
252       /* Compute interpolated LPCs (unquantized) for whole frame*/
253       for (i=0;i<st->lpcSize;i++)
254          st->interp_lsp[i] = cos(st->interp_lsp[i]);
255       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
256
257       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
258       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
259
260       residue(st->frame, st->bw_lpc1, st->exc, st->frameSize, st->lpcSize);
261       syn_filt(st->exc, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize);
262       
263       /*Open-loop pitch*/
264       open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
265                             &ol_pitch, &ol_pitch_coef, 1, st->stack);
266
267       /*Compute "real" excitation*/
268       residue(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize);
269
270       /* Compute open-loop excitation gain */
271       ol_gain=0;
272       for (i=0;i<st->frameSize;i++)
273          ol_gain += st->exc[i]*st->exc[i];
274       
275       ol_gain=sqrt(1+ol_gain/st->frameSize);
276    }
277
278    /*Experimental VBR stuff*/
279    if (st->vbr)
280    {
281       delta_qual = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
282       if (delta_qual<0)
283          delta_qual*=.1*(4+st->vbr_quality);
284       if (st->vbr_enabled) 
285       {
286          int qual = (int)floor(st->vbr_quality+delta_qual+.5);
287          if (qual<0)
288             qual=0;
289          if (qual>10)
290             qual=10;
291          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &qual);
292       }
293    }
294    /*printf ("VBR quality = %f\n", vbr_qual);*/
295
296    /* First, transmit the sub-mode we use for this frame */
297    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
298
299
300    /*Quantize LSPs*/
301 #if 1 /*0 for unquantized*/
302    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
303 #else
304    for (i=0;i<st->lpcSize;i++)
305      st->qlsp[i]=st->lsp[i];
306 #endif
307
308    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
309    if (SUBMODE(lbr_pitch)!=-1 && SUBMODE(ltp_params))
310    {
311       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
312    } else if (SUBMODE(lbr_pitch)==0)
313    {
314       int quant;
315       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
316       quant = (int)floor(.5+15*ol_pitch_coef);
317       if (quant>15)
318          quant=0;
319       if (quant<0)
320          quant=0;
321       speex_bits_pack(bits, quant, 4);
322       ol_pitch_coef=0.066667*quant;
323    }
324    
325    
326    /*Quantize and transmit open-loop excitation gain*/
327    {
328       int qe = (int)(floor(3.5*log(ol_gain)));
329       if (qe<0)
330          qe=0;
331       if (qe>31)
332          qe=31;
333       ol_gain = exp(qe/3.5);
334       speex_bits_pack(bits, qe, 5);
335    }
336
337    /* Special case for first frame */
338    if (st->first)
339    {
340       for (i=0;i<st->lpcSize;i++)
341          st->old_qlsp[i] = st->qlsp[i];
342    }
343
344    /* Loop on sub-frames */
345    for (sub=0;sub<st->nbSubframes;sub++)
346    {
347       float esig, enoise, snr, tmp;
348       int   offset;
349       float *sp, *sw, *res, *exc, *target, *mem, *exc2;
350       int pitch;
351
352       /* Offset relative to start of frame */
353       offset = st->subframeSize*sub;
354       /* Original signal */
355       sp=st->frame+offset;
356       /* Excitation */
357       exc=st->exc+offset;
358       /* Weighted signal */
359       sw=st->sw+offset;
360
361       exc2=st->exc2+offset;
362
363       /* Filter response */
364       res = PUSH(st->stack, st->subframeSize);
365       /* Target signal */
366       target = PUSH(st->stack, st->subframeSize);
367       mem = PUSH(st->stack, st->lpcSize);
368
369       /* LSP interpolation (quantized and unquantized) */
370       tmp = (1.0 + sub)/st->nbSubframes;
371       for (i=0;i<st->lpcSize;i++)
372          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
373       for (i=0;i<st->lpcSize;i++)
374          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
375
376       /* Make sure the filters are stable */
377       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
378       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
379
380       /* Compute interpolated LPCs (quantized and unquantized) */
381       for (i=0;i<st->lpcSize;i++)
382          st->interp_lsp[i] = cos(st->interp_lsp[i]);
383       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
384
385       for (i=0;i<st->lpcSize;i++)
386          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
387       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
388
389       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
390       tmp=1;
391       st->pi_gain[sub]=0;
392       for (i=0;i<=st->lpcSize;i++)
393       {
394          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
395          tmp = -tmp;
396       }
397      
398
399       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
400       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
401       if (st->gamma2>=0)
402          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
403       else
404       {
405          st->bw_lpc2[0]=1;
406          st->bw_lpc2[1]=-st->preemph;
407          for (i=2;i<=st->lpcSize;i++)
408             st->bw_lpc2[i]=0;
409       }
410
411       /* Reset excitation */
412       for (i=0;i<st->subframeSize;i++)
413          exc[i]=0;
414       for (i=0;i<st->subframeSize;i++)
415          exc2[i]=0;
416
417       /* Compute zero response of A(z/g1) / ( A(z/g2) * Aq(z) ) */
418       for (i=0;i<st->lpcSize;i++)
419          mem[i]=st->mem_sp[i];
420       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
421       for (i=0;i<st->lpcSize;i++)
422          mem[i]=st->mem_sp[i];
423       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
424       for (i=0;i<st->lpcSize;i++)
425          mem[i]=st->mem_sw[i];
426       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
427
428       /* Compute weighted signal */
429       for (i=0;i<st->lpcSize;i++)
430          mem[i]=st->mem_sp[i];
431       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
432       for (i=0;i<st->lpcSize;i++)
433          mem[i]=st->mem_sw[i];
434       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
435       
436       esig=0;
437       for (i=0;i<st->subframeSize;i++)
438          esig+=sw[i]*sw[i];
439       
440       /* Compute target signal */
441       for (i=0;i<st->subframeSize;i++)
442          target[i]=sw[i]-res[i];
443
444       for (i=0;i<st->subframeSize;i++)
445          exc[i]=exc2[i]=0;
446
447       /* If we have a long-term predictor (not all sub-modes have one) */
448       if (SUBMODE(ltp_params))
449       {
450          /* Long-term prediction */
451          if (SUBMODE(lbr_pitch) != -1)
452          {
453             /* Low bit-rate pitch handling */
454             int pit_min, pit_max;
455             int margin;
456             margin = SUBMODE(lbr_pitch);
457             if (margin)
458             {
459                if (ol_pitch < st->min_pitch+margin-1)
460                   ol_pitch=st->min_pitch+margin-1;
461                if (ol_pitch > st->max_pitch-margin)
462                   ol_pitch=st->max_pitch-margin;
463                pit_min = ol_pitch-margin+1;
464                pit_max = ol_pitch+margin;
465             } else {
466                pit_min=pit_max=ol_pitch;
467             }
468             pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
469                                        exc, SUBMODE(ltp_params), pit_min, pit_max, 
470                                        st->lpcSize, st->subframeSize, bits, st->stack, 
471                                        exc2, st->complexity);
472          } else {
473             /* Normal pitch handling */
474             pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
475                                        exc, SUBMODE(ltp_params), st->min_pitch, st->max_pitch, 
476                                        st->lpcSize, st->subframeSize, bits, st->stack, 
477                                        exc2, st->complexity);
478          }
479          /*printf ("cl_pitch: %d\n", pitch);*/
480          st->pitch[sub]=pitch;
481       } else if (SUBMODE(lbr_pitch==0)) {
482          for (i=0;i<st->subframeSize;i++)
483          {
484             exc[i]=exc[i-ol_pitch]*ol_pitch_coef;
485          }
486       }
487
488       /* Update target for adaptive codebook contribution */
489       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
490       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
491       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
492       for (i=0;i<st->subframeSize;i++)
493         target[i]-=res[i];
494
495       /* Compute noise energy and SNR */
496       enoise=0;
497       for (i=0;i<st->subframeSize;i++)
498          enoise += target[i]*target[i];
499       snr = 10*log10((esig+1)/(enoise+1));
500       /*st->pitch[sub]=(int)snr;*/
501 #ifdef DEBUG
502       printf ("pitch SNR = %f\n", snr);
503 #endif
504
505
506 #if 0 /*If set to 1, compute "real innovation" i.e. cheat to get perfect reconstruction*/
507       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
508       residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
509       residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
510       /*if (1||(snr>9 && (rand()%6==0)))
511       {
512          float ener=0;
513          printf ("exc ");
514          for (i=0;i<st->subframeSize;i++)
515          {
516             ener+=st->buf2[i]*st->buf2[i];
517             if (i && i%5==0)
518                printf ("\nexc ");
519             printf ("%f ", st->buf2[i]);
520          }
521          printf ("\n");
522       printf ("innovation_energy = %f\n", ener);
523       }*/
524       if (rand()%5==0 && snr>5)
525       {
526          float ener=0, sign=1;
527          if (rand()%2)
528             sign=-1;
529          for (i=0;i<st->subframeSize;i++)
530          {
531             ener+=st->buf2[i]*st->buf2[i];
532          }
533          ener=sign/sqrt(.01+ener/st->subframeSize);
534          for (i=0;i<st->subframeSize;i++)
535          {
536             if (i%10==0)
537                printf ("\nexc ");
538             printf ("%f ", ener*st->buf2[i]);
539          }
540          printf ("\n");
541       }
542
543       for (i=0;i<st->subframeSize;i++)
544          exc[i]+=st->buf2[i];
545 #else
546       /* Quantization of innovation */
547       {
548          float *innov;
549          float ener=0, ener_1;
550          innov=PUSH(st->stack, st->subframeSize);
551          for (i=0;i<st->subframeSize;i++)
552             innov[i]=0;
553          syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
554          residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
555          residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
556          for (i=0;i<st->subframeSize;i++)
557             ener+=st->buf2[i]*st->buf2[i];
558          ener=sqrt(.1+ener/st->subframeSize);
559
560          ener /= ol_gain;
561          if (SUBMODE(have_subframe_gain)) 
562          {
563             int qe;
564             ener=log(ener);
565             qe = vq_index(&ener, exc_gain_quant_scal, 1, 8);
566             speex_bits_pack(bits, qe, 3);
567             ener=exc_gain_quant_scal[qe];
568             ener=exp(ener);
569             /*printf ("encode gain: %d %f\n", qe, ener);*/
570          } else {
571             ener=1;
572          }
573          ener*=ol_gain;
574          /*printf ("transmit gain: %f\n", ener);*/
575          ener_1 = 1/ener;
576          
577          for (i=0;i<st->subframeSize;i++)
578             target[i]*=ener_1;
579          
580          if (SUBMODE(innovation_quant))
581          {
582             /* Normal quantization */
583             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
584                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
585                                       innov, bits, st->stack, st->complexity);
586             
587             for (i=0;i<st->subframeSize;i++)
588                exc[i] += innov[i]*ener;
589          } else {
590             /* This is the "real" (cheating) excitation in the encoder but the decoder will
591                use white noise */
592             for (i=0;i<st->subframeSize;i++)
593                exc[i] += st->buf2[i];
594          }
595          POP(st->stack);
596          for (i=0;i<st->subframeSize;i++)
597             target[i]*=ener;
598
599       }
600 #endif
601       /* Compute weighted noise energy and SNR */
602       enoise=0;
603       for (i=0;i<st->subframeSize;i++)
604          enoise += target[i]*target[i];
605       snr = 10*log10((esig+1)/(enoise+1));
606 #ifdef DEBUG
607       printf ("seg SNR = %f\n", snr);
608 #endif
609
610       /*Keep the previous memory*/
611       for (i=0;i<st->lpcSize;i++)
612          mem[i]=st->mem_sp[i];
613       /* Final signal synthesis from excitation */
614       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
615
616       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
617       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
618       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
619
620 #if 0
621       /*for (i=0;i<st->subframeSize;i++)
622         exc2[i]=.75*exc[i]+.2*exc[i-pitch]+.05*exc[i-2*pitch];*/
623       {
624          float max_exc=0;
625          for (i=0;i<st->subframeSize;i++)
626             if (fabs(exc[i])>max_exc)
627                max_exc=fabs(exc[i]);
628          max_exc=1/(max_exc+.01);
629          for (i=0;i<st->subframeSize;i++)
630          {
631             float xx=max_exc*exc[i];
632             exc2[i]=exc[i]*(1-exp(-100*xx*xx));
633          }
634       }
635 #else
636       for (i=0;i<st->subframeSize;i++)
637          exc2[i]=exc[i];
638 #endif
639       POP(st->stack);
640       POP(st->stack);
641       POP(st->stack);
642    }
643
644    /* Store the LSPs for interpolation in the next frame */
645    for (i=0;i<st->lpcSize;i++)
646       st->old_lsp[i] = st->lsp[i];
647    for (i=0;i<st->lpcSize;i++)
648       st->old_qlsp[i] = st->qlsp[i];
649
650    /* The next frame will not be the first (Duh!) */
651    st->first = 0;
652
653    /* Replace input by synthesized speech */
654    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
655    for (i=1;i<st->frameSize;i++)
656      in[i]=st->frame[i] + st->preemph*in[i-1];
657    st->pre_mem2=in[st->frameSize-1];
658
659 }
660
661
662 void *nb_decoder_init(SpeexMode *m)
663 {
664    DecState *st;
665    SpeexNBMode *mode;
666    int i;
667
668    mode=m->mode;
669    st = speex_alloc(sizeof(DecState));
670    st->mode=m;
671
672    st->first=1;
673    /* Codec parameters, should eventually have several "modes"*/
674    st->frameSize = mode->frameSize;
675    st->windowSize = st->frameSize*3/2;
676    st->nbSubframes=mode->frameSize/mode->subframeSize;
677    st->subframeSize=mode->subframeSize;
678    st->lpcSize = mode->lpcSize;
679    st->bufSize = mode->bufSize;
680    st->gamma1=mode->gamma1;
681    st->gamma2=mode->gamma2;
682    st->min_pitch=mode->pitchStart;
683    st->max_pitch=mode->pitchEnd;
684    st->preemph = mode->preemph;
685
686    st->submodes=mode->submodes;
687    st->submodeID=mode->defaultSubmode;
688
689    st->pre_mem=0;
690    st->pf_enabled=0;
691
692    st->stack = speex_alloc(10000*sizeof(float));
693
694    st->inBuf = speex_alloc(st->bufSize*sizeof(float));
695    st->frame = st->inBuf + st->bufSize - st->windowSize;
696    st->excBuf = speex_alloc(st->bufSize*sizeof(float));
697    st->exc = st->excBuf + st->bufSize - st->windowSize;
698    st->exc2Buf = speex_alloc(st->bufSize*sizeof(float));
699    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
700    for (i=0;i<st->bufSize;i++)
701       st->inBuf[i]=0;
702    for (i=0;i<st->bufSize;i++)
703       st->excBuf[i]=0;
704    for (i=0;i<st->bufSize;i++)
705       st->exc2Buf[i]=0;
706
707    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
708    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
709    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
710    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
711    st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
712    st->mem_pf = speex_alloc(st->lpcSize*sizeof(float));
713    st->mem_pf2 = speex_alloc(st->lpcSize*sizeof(float));
714
715    st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
716    st->last_pitch = 40;
717    st->count_lost=0;
718    return st;
719 }
720
721 void nb_decoder_destroy(void *state)
722 {
723    DecState *st;
724    st=state;
725    speex_free(st->inBuf);
726    speex_free(st->excBuf);
727    speex_free(st->exc2Buf);
728    speex_free(st->interp_qlpc);
729    speex_free(st->qlsp);
730    speex_free(st->old_qlsp);
731    speex_free(st->interp_qlsp);
732    speex_free(st->stack);
733    speex_free(st->mem_sp);
734    speex_free(st->mem_pf);
735    speex_free(st->mem_pf2);
736    speex_free(st->pi_gain);
737    
738    speex_free(state);
739 }
740
741 void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
742 {
743    DecState *st;
744    int i, sub;
745    int pitch;
746    float pitch_gain[3];
747    float ol_gain;
748    int ol_pitch=0;
749    float ol_pitch_coef=0;
750    int best_pitch=40;
751    float best_pitch_gain=-1;
752    st=state;
753
754    /* Get the sub-mode that was used */
755    st->submodeID = speex_bits_unpack_unsigned(bits, NB_SUBMODE_BITS);
756
757    /* Shift all buffers by one frame */
758    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
759    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
760    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
761
762    /* Unquantize LSPs */
763    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
764
765    /* Handle first frame and lost-packet case */
766    if (st->first || st->count_lost)
767    {
768       for (i=0;i<st->lpcSize;i++)
769          st->old_qlsp[i] = st->qlsp[i];
770    }
771
772    /* Get open-loop pitch estimation for low bit-rate pitch coding */
773    if (SUBMODE(lbr_pitch)!=-1 && SUBMODE(ltp_params))
774    {
775       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
776    } else if (SUBMODE(lbr_pitch)==0)
777    {
778       int quant;
779       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
780       quant = speex_bits_unpack_unsigned(bits, 4);
781       ol_pitch_coef=0.066667*quant;
782    }
783    
784    /* Get global excitation gain */
785    {
786       int qe;
787       qe = speex_bits_unpack_unsigned(bits, 5);
788       ol_gain = exp(qe/3.5);
789       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
790    }
791
792    /*Loop on subframes */
793    for (sub=0;sub<st->nbSubframes;sub++)
794    {
795       int offset;
796       float *sp, *exc, *exc2, tmp;
797       
798       /* Offset relative to start of frame */
799       offset = st->subframeSize*sub;
800       /* Original signal */
801       sp=st->frame+offset;
802       /* Excitation */
803       exc=st->exc+offset;
804       /* Excitation after post-filter*/
805       exc2=st->exc2+offset;
806
807       /* LSP interpolation (quantized and unquantized) */
808       tmp = (1.0 + sub)/st->nbSubframes;
809       for (i=0;i<st->lpcSize;i++)
810          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
811
812       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
813
814
815       /* Compute interpolated LPCs (unquantized) */
816       for (i=0;i<st->lpcSize;i++)
817          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
818       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
819
820
821       /* Compute analysis filter at w=pi */
822       tmp=1;
823       st->pi_gain[sub]=0;
824       for (i=0;i<=st->lpcSize;i++)
825       {
826          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
827          tmp = -tmp;
828       }
829
830       /* Reset excitation */
831       for (i=0;i<st->subframeSize;i++)
832          exc[i]=0;
833
834       /*Adaptive codebook contribution*/
835       if (SUBMODE(ltp_unquant))
836       {
837          if (SUBMODE(lbr_pitch) != -1)
838          {
839             int pit_min, pit_max;
840             int margin;
841             margin = SUBMODE(lbr_pitch);
842             if (margin)
843             {
844                if (ol_pitch < st->min_pitch+margin-1)
845                   ol_pitch=st->min_pitch+margin-1;
846                if (ol_pitch > st->max_pitch-margin)
847                   ol_pitch=st->max_pitch-margin;
848                pit_min = ol_pitch-margin+1;
849                pit_max = ol_pitch+margin;
850             } else {
851                pit_min=pit_max=ol_pitch;
852             }
853             SUBMODE(ltp_unquant)(exc, pit_min, pit_max, SUBMODE(ltp_params), st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, 0);
854          } else {
855             SUBMODE(ltp_unquant)(exc, st->min_pitch, st->max_pitch, SUBMODE(ltp_params), st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, 0);
856          }
857          
858          if (!lost)
859          {
860             /* If the frame was not lost... */
861             tmp = fabs(pitch_gain[0])+fabs(pitch_gain[1])+fabs(pitch_gain[2]);
862             tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
863             if (tmp>best_pitch_gain)
864             {
865                best_pitch = pitch;
866                while (best_pitch+pitch<st->max_pitch)
867                {
868                   best_pitch+=pitch;
869                }
870                best_pitch_gain = tmp*.9;
871                if (best_pitch_gain>.85)
872                   best_pitch_gain=.85;
873             }
874          } else {
875             /* What to do with pitch if we lost the frame */
876             for (i=0;i<st->subframeSize;i++)
877                exc[i]=0;
878             /*printf ("best_pitch: %d %f\n", st->last_pitch, st->last_pitch_gain);*/
879             for (i=0;i<st->subframeSize;i++)
880                exc[i]=st->last_pitch_gain*exc[i-st->last_pitch];
881          }
882       } else if (SUBMODE(lbr_pitch==0)) {
883          for (i=0;i<st->subframeSize;i++)
884          {
885             exc[i]=exc[i-ol_pitch]*ol_pitch_coef;
886          }
887       }
888       
889       /* Unquantize the innovation */
890       {
891          int q_energy;
892          float ener;
893          float *innov;
894          
895          innov = PUSH(st->stack, st->subframeSize);
896          for (i=0;i<st->subframeSize;i++)
897             innov[i]=0;
898
899          if (SUBMODE(have_subframe_gain))
900          {
901             q_energy = speex_bits_unpack_unsigned(bits, 3);
902             ener = ol_gain*exp(exc_gain_quant_scal[q_energy]);
903          } else {
904             ener = ol_gain;
905          }
906          
907          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
908          
909          if (SUBMODE(innovation_unquant))
910          {
911             /*Fixed codebook contribution*/
912             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
913          } else {
914             float scale;
915             scale = 3*sqrt(1.2-ol_pitch_coef);
916             for (i=0;i<st->subframeSize;i++)
917                innov[i] = scale*((((float)rand())/RAND_MAX)-.5);
918             
919          }
920
921          if (st->count_lost)
922             ener*=pow(.8,st->count_lost);
923
924          for (i=0;i<st->subframeSize;i++)
925             exc[i]+=ener*innov[i];
926
927          POP(st->stack);
928       }
929
930       for (i=0;i<st->subframeSize;i++)
931          exc2[i]=exc[i];
932
933       /* Apply post-filter */
934       if (st->pf_enabled && SUBMODE(post_filter_func))
935          SUBMODE(post_filter_func)(exc, exc2, st->interp_qlpc, st->lpcSize, st->subframeSize,
936                               pitch, pitch_gain, SUBMODE(post_filter_params), st->mem_pf, 
937                               st->mem_pf2, st->stack);
938       
939       /* Apply synthesis filter */
940       syn_filt_mem(exc2, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
941
942    }
943    
944    /*Copy output signal*/
945    for (i=0;i<st->frameSize;i++)
946       out[i]=st->frame[i];
947
948    out[0] = st->frame[0] + st->preemph*st->pre_mem;
949    for (i=1;i<st->frameSize;i++)
950      out[i]=st->frame[i] + st->preemph*out[i-1];
951    st->pre_mem=out[st->frameSize-1];
952
953
954    /* Store the LSPs for interpolation in the next frame */
955    for (i=0;i<st->lpcSize;i++)
956       st->old_qlsp[i] = st->qlsp[i];
957
958    /* The next frame will not be the first (Duh!) */
959    st->first = 0;
960    if (!lost)
961       st->count_lost=0;
962    else
963       st->count_lost++;
964    if (!lost)
965    {
966       st->last_pitch = best_pitch;
967       st->last_pitch_gain = best_pitch_gain;
968    }
969 }
970
971 void nb_encoder_ctl(void *state, int request, void *ptr)
972 {
973    EncState *st;
974    st=state;     
975    switch(request)
976    {
977    case SPEEX_GET_FRAME_SIZE:
978       (*(int*)ptr) = st->frameSize;
979       break;
980    case SPEEX_SET_MODE:
981       st->submodeID = (*(int*)ptr);
982       break;
983    case SPEEX_GET_MODE:
984       (*(int*)ptr) = st->submodeID;
985       break;
986    case SPEEX_SET_VBR:
987       st->vbr_enabled = (*(int*)ptr);
988       break;
989    case SPEEX_GET_VBR:
990       (*(int*)ptr) = st->vbr_enabled;
991       break;
992    case SPEEX_SET_VBR_QUALITY:
993       st->vbr_quality = (*(int*)ptr);
994       break;
995    case SPEEX_GET_VBR_QUALITY:
996       (*(int*)ptr) = st->vbr_quality;
997       break;
998    case SPEEX_SET_QUALITY:
999       {
1000          int quality = (*(int*)ptr);
1001          if (quality<=0)
1002             st->submodeID = 1;
1003          else if (quality<=1)
1004             st->submodeID = 1;
1005          else if (quality<=2)
1006             st->submodeID = 2;
1007          else if (quality<=4)
1008             st->submodeID = 3;
1009          else if (quality<=6)
1010             st->submodeID = 4;
1011          else if (quality<=8)
1012             st->submodeID = 5;
1013          else if (quality<=10)
1014             st->submodeID = 6;
1015          else
1016             fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);
1017       }
1018       break;
1019    case SPEEX_SET_COMPLEXITY:
1020       st->complexity = (*(int*)ptr);
1021       break;
1022    case SPEEX_GET_COMPLEXITY:
1023       (*(int*)ptr) = st->complexity;
1024       break;
1025    default:
1026       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1027    }
1028 }
1029
1030 void nb_decoder_ctl(void *state, int request, void *ptr)
1031 {
1032    DecState *st;
1033    st=state;
1034    switch(request)
1035    {
1036    case SPEEX_SET_PF:
1037       st->pf_enabled = *((int*)ptr);
1038       break;
1039    case SPEEX_GET_FRAME_SIZE:
1040       (*(int*)ptr) = st->frameSize;
1041       break;
1042    default:
1043       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1044    }
1045 }