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