Working demo...
[speexdsp.git] / libspeex / speex.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 <string.h>
22 #include <math.h>
23 #include "speex.h"
24 #include "lpc.h"
25 #include "lsp.h"
26 #include "ltp.h"
27 #include "quant_lsp.h"
28 #include "cb_search.h"
29 #include "filters.h"
30 #include "stack_alloc.h"
31
32 extern float stoc[];
33 extern float exc_table[][8];
34 #ifndef M_PI
35 #define M_PI           3.14159265358979323846  /* pi */
36 #endif
37
38 #define sqr(x) ((x)*(x))
39 #define min(a,b) ((a) < (b) ? (a) : (b))
40
41 void encoder_init(EncState *st, SpeexMode *mode)
42 {
43    int i;
44    float tmp;
45    /* Codec parameters, should eventually have several "modes"*/
46    st->frameSize = mode->frameSize;
47    st->windowSize = mode->windowSize;
48    st->nbSubframes=mode->frameSize/mode->subframeSize;
49    st->subframeSize=mode->subframeSize;
50    st->lpcSize = mode->lpcSize;
51    st->bufSize = mode->bufSize;
52    st->gamma1=mode->gamma1;
53    st->gamma2=mode->gamma2;
54    st->min_pitch=mode->pitchStart;
55    st->max_pitch=mode->pitchEnd;
56
57    /* Over-sampling filter (fractional pitch)*/
58    st->os_fact=4;
59    st->os_filt_ord2=4*st->os_fact;
60    st->os_filt = malloc((1+2*st->os_filt_ord2)*sizeof(float));
61    st->os_filt[st->os_filt_ord2] = 1;
62    for (i=1;i<=st->os_filt_ord2;i++)
63    {
64       float x=M_PI*i/st->os_fact;
65       st->os_filt[st->os_filt_ord2-i] = st->os_filt[st->os_filt_ord2+i]=sin(x)/x*(.5+.5*cos(M_PI*i/st->os_filt_ord2));
66    }
67    /* Normalizing the over-sampling filter */
68    tmp=0;
69    for (i=0;i<2*st->os_filt_ord2+1;i++)
70       tmp += st->os_filt[i];
71    tmp=1/tmp;
72    for (i=0;i<2*st->os_filt_ord2+1;i++)
73       st->os_filt[i] *= tmp;
74
75    /*for (i=0;i<2*st->os_filt_ord2+1;i++)
76       printf ("%f ", st->os_filt[i]);
77       printf ("\n");*/
78
79    /* Allocating input buffer */
80    st->inBuf = calloc(st->bufSize,sizeof(float));
81    st->frame = st->inBuf + st->bufSize - st->windowSize;
82    /* Allocating excitation buffer */
83    st->excBuf = calloc(st->bufSize,sizeof(float));
84    st->exc = st->excBuf + st->bufSize - st->windowSize;
85    st->swBuf = calloc(st->bufSize,sizeof(float));
86    st->sw = st->swBuf + st->bufSize - st->windowSize;
87
88    /* Hanning window */
89    st->window = malloc(st->windowSize*sizeof(float));
90    for (i=0;i<st->windowSize;i++)
91       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
92
93    /* Create the window for autocorrelation (lag-windowing) */
94    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
95    for (i=0;i<st->lpcSize+1;i++)
96       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*.01*i));
97
98    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
99
100    st->stack = calloc(10000, sizeof(float));
101
102    st->buf2 = malloc(st->windowSize*sizeof(float));
103
104    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
105    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
106    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
107    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
108    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
109    st->bw_az = malloc((st->lpcSize*2+1)*sizeof(float));
110
111    st->lsp = malloc(st->lpcSize*sizeof(float));
112    st->qlsp = malloc(st->lpcSize*sizeof(float));
113    st->old_lsp = malloc(st->lpcSize*sizeof(float));
114    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
115    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
116    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
117    st->rc = malloc(st->lpcSize*sizeof(float));
118    st->first = 1;
119    
120    st->mem_sp = calloc(st->lpcSize, sizeof(float));
121    st->mem_sw = calloc(st->lpcSize, sizeof(float));
122 }
123
124 void encoder_destroy(EncState *st)
125 {
126    /* Free all allocated memory */
127    free(st->inBuf);
128    free(st->excBuf);
129    free(st->swBuf);
130    
131    free(st->stack);
132
133    free(st->window);
134    free(st->buf2);
135    free(st->lpc);
136    free(st->interp_lpc);
137    free(st->interp_qlpc);
138
139    free(st->bw_lpc1);
140    free(st->bw_lpc2);
141    free(st->bw_az);
142    free(st->autocorr);
143    free(st->lagWindow);
144    free(st->lsp);
145    free(st->qlsp);
146    free(st->old_lsp);
147    free(st->interp_lsp);
148    free(st->old_qlsp);
149    free(st->interp_qlsp);
150    free(st->rc);
151
152    free(st->mem_sp);
153    free(st->mem_sw);
154 }
155
156 void encode(EncState *st, float *in, FrameBits *bits)
157 {
158    int i, j, sub, roots;
159    float error;
160
161    /* Copy new data in input buffer */
162    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
163    for (i=0;i<st->frameSize;i++)
164       st->inBuf[st->bufSize-st->frameSize+i] = in[i];
165    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
166    memmove(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
167
168    /* Window for analysis */
169    for (i=0;i<st->windowSize;i++)
170       st->buf2[i] = st->frame[i] * st->window[i];
171
172    /* Compute auto-correlation */
173    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
174
175    st->autocorr[0] += 1;        /* prevents NANs */
176    st->autocorr[0] *= 1.0001;   /* 40 dB noise floor */
177    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
178    for (i=0;i<st->lpcSize+1;i++)
179       st->autocorr[i] *= st->lagWindow[i];
180
181    /* Levinson-Durbin */
182    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
183    st->lpc[0]=1;
184
185    /* LPC to LSPs (x-domain) transform */
186    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.02, st->stack);
187    if (roots!=st->lpcSize)
188    {
189       fprintf (stderr, "roots!=st->lpcSize\n");
190       exit(1);
191    }
192
193    /* x-domain to angle domain*/
194    for (i=0;i<st->lpcSize;i++)
195       st->lsp[i] = acos(st->lsp[i]);
196    
197    /* LSP Quantization */
198    {
199       unsigned int id;
200       for (i=0;i<st->lpcSize;i++)
201          st->qlsp[i]=st->lsp[i];
202       id=lsp_quant_nb(st->qlsp,10 );
203       lsp_unquant_nb(st->qlsp,10,id);
204    }
205
206    /*Find open-loop pitch for the whole frame*/
207    {
208       float *mem = PUSH(st->stack, st->lpcSize);
209       
210       for (i=0;i<st->lpcSize;i++)
211          st->interp_lsp[i] = .5*st->old_lsp[i] + .5*st->lsp[i];
212       for (i=0;i<st->lpcSize;i++)
213          st->interp_lsp[i] = cos(st->interp_lsp[i]);
214       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
215       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
216       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
217       for (i=0;i<st->lpcSize;i++)
218          mem[i]=st->mem_sp[i];
219       residue_mem(st->frame, st->bw_lpc1, st->sw, st->frameSize, st->lpcSize, mem);
220       for (i=0;i<st->lpcSize;i++)
221          mem[i]=st->mem_sw[i];
222          syn_filt_mem(st->sw, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, mem);
223       open_loop_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, &st->ol_pitch, &st->ol_voiced);
224       printf ("Open-loop pitch = %d\n", st->ol_pitch);
225       POP(st->stack);
226    }
227
228    /* Loop on sub-frames */
229    for (sub=0;sub<st->nbSubframes;sub++)
230    {
231       float tmp, tmp1,tmp2,gain[3];
232       float esig=0, enoise=0, snr;
233       int pitch, offset, pitch_gain_index;
234       float *sp, *sw, *res, *exc, *target, *mem;
235       
236       /* Offset relative to start of frame */
237       offset = st->subframeSize*sub;
238       /* Original signal */
239       sp=st->frame+offset;
240       /* Excitation */
241       exc=st->exc+offset;
242       /* Weighted signal */
243       sw=st->sw+offset;
244       /* Filter response */
245       res = PUSH(st->stack, st->subframeSize);
246       /* Target signal */
247       target = PUSH(st->stack, st->subframeSize);
248       mem = PUSH(st->stack, st->lpcSize);
249
250       /* LSP interpolation (quantized and unquantized) */
251       tmp = (.5 + sub)/st->nbSubframes;
252       for (i=0;i<st->lpcSize;i++)
253          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
254       for (i=0;i<st->lpcSize;i++)
255          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
256
257       /* Compute interpolated LPCs (quantized and unquantized) */
258       for (i=0;i<st->lpcSize;i++)
259          st->interp_lsp[i] = cos(st->interp_lsp[i]);
260       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
261
262       for (i=0;i<st->lpcSize;i++)
263          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
264       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
265
266       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
267       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
268       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
269       
270       /* Reset excitation */
271       for (i=0;i<st->subframeSize;i++)
272          exc[i]=0;
273
274       /* Compute zero response of A(z/g1) / ( A(z/g2) * Aq(z) ) */
275       for (i=0;i<st->lpcSize;i++)
276          mem[i]=st->mem_sp[i];
277       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
278       for (i=0;i<st->lpcSize;i++)
279          mem[i]=st->mem_sp[i];
280       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
281       for (i=0;i<st->lpcSize;i++)
282          mem[i]=st->mem_sw[i];
283       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
284
285       /* Compute weighted signal */
286       for (i=0;i<st->lpcSize;i++)
287          mem[i]=st->mem_sp[i];
288       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
289       for (i=0;i<st->lpcSize;i++)
290          mem[i]=st->mem_sw[i];
291       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
292
293       for (i=0;i<st->subframeSize;i++)
294          esig+=sw[i]*sw[i];
295       
296       /* Compute target signal */
297       for (i=0;i<st->subframeSize;i++)
298          target[i]=sw[i]-res[i];
299
300       for (i=0;i<st->subframeSize;i++)
301          exc[i]=0;
302 #if 1 /*If set to 0, we compute the excitation directly from the target, i.e. we're cheating */
303
304       /* Perform adaptive codebook search (3-tap pitch predictor) */
305       pitch = st->ol_pitch;
306 #if 0 /* 1 for fractional pitch, 0 for integer pitch */
307       closed_loop_fractional_pitch(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
308                                    exc, st->os_filt, st->os_filt_ord2, st->os_fact, 20, 147, 
309                                    &gain[0], &pitch, st->lpcSize,
310                                    st->subframeSize, st->stack);
311 #else
312       pitch_search_3tap(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
313                         exc, 20, 147, &gain[0], &pitch, &pitch_gain_index, st->lpcSize,
314                         st->subframeSize);
315       for (i=0;i<st->subframeSize;i++)
316         exc[i]=gain[0]*exc[i-pitch]+gain[1]*exc[i-pitch-1]+gain[2]*exc[i-pitch-2];
317       printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
318 #endif
319       /* Update target for adaptive codebook contribution */
320       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
321       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
322       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
323       for (i=0;i<st->subframeSize;i++)
324         target[i]-=res[i];
325
326       enoise=0;
327       for (i=0;i<st->subframeSize;i++)
328          enoise += target[i]*target[i];
329       snr = 10*log10((esig+1)/(enoise+1));
330       printf ("pitch SNR = %f\n", snr);
331 #if 0 /* 1 for stochastic excitation, 0 for split-VQ*/
332       for(j=0;j<1;j++){
333          /*float stoc2[1080];*/
334          float *stoc2 = PUSH(st->stack,1080);
335          for (i=0;i<1080;i++)
336          {
337             stoc2[i]=stoc[i];
338             if (i-(pitch-1)>=0)
339                stoc2[i] += .0*stoc[i-(pitch-1)];
340          }
341          POP(st->stack);
342       /* Perform stochastic codebook search */
343       overlap_cb_search(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
344                         stoc2, 1024, &gain[0], &pitch, st->lpcSize,
345                         st->subframeSize);
346       printf ("gain = %f index = %d energy = %f\n",gain[0], pitch, esig);
347       for (i=0;i<st->subframeSize;i++)
348          exc[i]+=gain[0]*stoc2[i+pitch];
349       
350       /* Update target for adaptive codebook contribution (Useless for now)*/
351       residue_zero(stoc2+pitch, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
352       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
353       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
354       for (i=0;i<st->subframeSize;i++)
355          target[i]-=gain[0]*res[i];
356       }
357 #else
358       split_cb_search(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
359                         exc_table, 64, &gain[0], &pitch, st->lpcSize,
360                         st->subframeSize, exc);
361 #endif
362       /* Compute weighted noise energy, SNR */
363       enoise=0;
364       for (i=0;i<st->subframeSize;i++)
365          enoise += target[i]*target[i];
366       snr = 10*log10((esig+1)/(enoise+1));
367       printf ("seg SNR = %f\n", snr);
368
369 #else
370
371 #if 1 /* Code to calculate the exact excitation after pitch prediction  */
372       for (i=0;i<st->subframeSize;i++)
373          st->buf2[i]=target[i];
374 #if 0 /* 0 for fractional pitch, 1 for integer */
375       pitch_search_3tap(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
376                                 exc, 20, 147, &gain[0], &pitch, &pitch_gain_index, st->lpcSize,
377                         st->subframeSize);
378       for (i=0;i<st->subframeSize;i++)
379         exc[i]=gain[0]*exc[i-pitch]+gain[1]*exc[i-pitch-1]+gain[2]*exc[i-pitch-2];
380       printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
381 #else
382       pitch = st->ol_pitch;
383       closed_loop_fractional_pitch(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
384                                    exc, st->os_filt, st->os_filt_ord2, st->os_fact, 20, 147, 
385                                    &gain[0], &pitch, st->lpcSize,
386                                    st->subframeSize, st->stack);
387 #endif
388       /* Update target for adaptive codebook contribution */
389       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
390       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
391       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
392       for (i=0;i<st->subframeSize;i++)
393         target[i]-=res[i];
394
395       enoise=0;
396       for (i=0;i<st->subframeSize;i++)
397          enoise += target[i]*target[i];
398       snr = 10*log10((esig+1)/(enoise+1));
399       printf ("pitch SNR = %f\n", snr);
400
401       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
402       residue_zero(res, st->interp_qlpc, exc, st->subframeSize, st->lpcSize);
403       residue_zero(exc, st->bw_lpc2, exc, st->subframeSize, st->lpcSize);
404       if (snr>5)
405       {
406          for (i=0;i<st->subframeSize;i++)
407          {
408             if (i%8==0&&i)
409                printf("\n");
410             printf ("%f ", exc[i]);
411          }
412          printf ("\n");
413       }
414       for (i=0;i<st->subframeSize;i++)
415          target[i]=st->buf2[i];
416 #endif
417
418       /* We're cheating to get perfect reconstruction */
419       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
420       residue_zero(res, st->interp_qlpc, exc, st->subframeSize, st->lpcSize);
421       residue_zero(exc, st->bw_lpc2, exc, st->subframeSize, st->lpcSize);
422 #endif
423
424       /*Keep the previous memory*/
425       for (i=0;i<st->lpcSize;i++)
426          mem[i]=st->mem_sp[i];
427       /* Final signal synthesis from excitation */
428       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
429
430       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
431       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
432       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
433
434       POP(st->stack);
435       POP(st->stack);
436       POP(st->stack);
437    }
438
439    /* Store the LSPs for interpolation in the next frame */
440    for (i=0;i<st->lpcSize;i++)
441       st->old_lsp[i] = st->lsp[i];
442    for (i=0;i<st->lpcSize;i++)
443       st->old_qlsp[i] = st->qlsp[i];
444
445    /* The next frame will not by the first (Duh!) */
446    st->first = 0;
447
448    /* Replace input by synthesized speech */
449    for (i=0;i<st->frameSize;i++)
450      in[i]=st->frame[i];
451 }
452
453
454 void decoder_init(DecState *st, SpeexMode *mode)
455 {
456    int i;
457    /* Codec parameters, should eventually have several "modes"*/
458    st->frameSize = mode->frameSize;
459    st->windowSize = mode->windowSize;
460    st->nbSubframes=mode->frameSize/mode->subframeSize;
461    st->subframeSize=mode->subframeSize;
462    st->lpcSize = mode->lpcSize;
463    st->bufSize = mode->bufSize;
464    st->gamma1=mode->gamma1;
465    st->gamma2=mode->gamma2;
466
467
468    st->inBuf = malloc(st->bufSize*sizeof(float));
469    st->frame = st->inBuf + st->bufSize - st->windowSize;
470    st->excBuf = malloc(st->bufSize*sizeof(float));
471    st->exc = st->excBuf + st->bufSize - st->windowSize;
472    for (i=0;i<st->bufSize;i++)
473       st->inBuf[i]=0;
474    for (i=0;i<st->bufSize;i++)
475       st->excBuf[i]=0;
476 }
477
478 void decoder_destroy(DecState *st)
479 {
480    free(st->inBuf);
481    free(st->excBuf);
482 }
483
484 void decode(DecState *st, FrameBits *bits, float *out)
485 {
486 }