4b00a8928b444aa9be108cde342cff2b9c89cb98
[speexdsp.git] / libspeex / sb_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
20 #include "speex.h"
21 #include "sb_celp.h"
22 #include "stdlib.h"
23 #include "filters.h"
24 #include <math.h>
25 #include "lpc.h"
26 #include "lsp.h"
27 #include <stdio.h>
28 #include "stack_alloc.h"
29 #include "cb_search.h"
30
31 #ifndef M_PI
32 #define M_PI           3.14159265358979323846  /* pi */
33 #endif
34
35 extern float stoc[];
36
37 #define sqr(x) ((x)*(x))
38
39 #if 0
40 #define QMF_ORDER 32
41 static float h0[32] = {
42    0.0006910579, -0.001403793,
43    -0.001268303, 0.004234195,
44    0.001414246, -0.009458318,
45    -0.0001303859, 0.01798145,
46    -0.004187483, -0.03123862,
47    0.01456844, 0.05294745,
48    -0.03934878, -0.09980243,
49    0.1285579, 0.4664053,
50    0.4664053, 0.1285579,
51    -0.09980243, -0.03934878,
52    0.05294745, 0.01456844,
53    -0.03123862, -0.004187483,
54    0.01798145, -0.0001303859,
55    -0.009458318, 0.001414246,
56    0.004234195, -0.001268303,
57    -0.001403793, 0.0006910579
58 };
59
60 static float h1[32] = {
61    0.0006910579, 0.001403793,
62    -0.001268303, -0.004234195,
63    0.001414246, 0.009458318,
64    -0.0001303859, -0.01798145,
65    -0.004187483, 0.03123862,
66    0.01456844, -0.05294745,
67    -0.03934878, 0.09980243,
68    0.1285579, -0.4664053,
69    0.4664053, -0.1285579,
70    -0.09980243, 0.03934878,
71    0.05294745, -0.01456844,
72    -0.03123862, 0.004187483,
73    0.01798145, 0.0001303859,
74    -0.009458318, -0.001414246,
75    0.004234195, 0.001268303,
76    -0.001403793, -0.0006910579
77 };
78 #else 
79 #define QMF_ORDER 64
80 static float h0[64] = {
81    3.596189e-05, -0.0001123515,
82    -0.0001104587, 0.0002790277,
83    0.0002298438, -0.0005953563,
84    -0.0003823631, 0.00113826,
85    0.0005308539, -0.001986177,
86    -0.0006243724, 0.003235877,
87    0.0005743159, -0.004989147,
88    -0.0002584767, 0.007367171,
89    -0.0004857935, -0.01050689,
90    0.001894714, 0.01459396,
91    -0.004313674, -0.01994365,
92    0.00828756, 0.02716055,
93    -0.01485397, -0.03764973,
94    0.026447, 0.05543245,
95    -0.05095487, -0.09779096,
96    0.1382363, 0.4600981,
97    0.4600981, 0.1382363,
98    -0.09779096, -0.05095487,
99    0.05543245, 0.026447,
100    -0.03764973, -0.01485397,
101    0.02716055, 0.00828756,
102    -0.01994365, -0.004313674,
103    0.01459396, 0.001894714,
104    -0.01050689, -0.0004857935,
105    0.007367171, -0.0002584767,
106    -0.004989147, 0.0005743159,
107    0.003235877, -0.0006243724,
108    -0.001986177, 0.0005308539,
109    0.00113826, -0.0003823631,
110    -0.0005953563, 0.0002298438,
111    0.0002790277, -0.0001104587,
112    -0.0001123515, 3.596189e-05
113 };
114
115 static float h1[64] = {
116    3.596189e-05, 0.0001123515,
117    -0.0001104587, -0.0002790277,
118    0.0002298438, 0.0005953563,
119    -0.0003823631, -0.00113826,
120    0.0005308539, 0.001986177,
121    -0.0006243724, -0.003235877,
122    0.0005743159, 0.004989147,
123    -0.0002584767, -0.007367171,
124    -0.0004857935, 0.01050689,
125    0.001894714, -0.01459396,
126    -0.004313674, 0.01994365,
127    0.00828756, -0.02716055,
128    -0.01485397, 0.03764973,
129    0.026447, -0.05543245,
130    -0.05095487, 0.09779096,
131    0.1382363, -0.4600981,
132    0.4600981, -0.1382363,
133    -0.09779096, 0.05095487,
134    0.05543245, -0.026447,
135    -0.03764973, 0.01485397,
136    0.02716055, -0.00828756,
137    -0.01994365, 0.004313674,
138    0.01459396, -0.001894714,
139    -0.01050689, 0.0004857935,
140    0.007367171, 0.0002584767,
141    -0.004989147, -0.0005743159,
142    0.003235877, 0.0006243724,
143    -0.001986177, -0.0005308539,
144    0.00113826, 0.0003823631,
145    -0.0005953563, -0.0002298438,
146    0.0002790277, 0.0001104587,
147    -0.0001123515, -3.596189e-05
148 };
149 #endif
150
151 void sb_encoder_init(SBEncState *st, SpeexMode *mode)
152 {
153    int i;
154    encoder_init(&st->st_low, mode);
155    st->full_frame_size = 2*st->st_low.frameSize;
156    st->frame_size = st->st_low.frameSize;
157    st->subframeSize = 40;
158    st->nbSubframes = 4;
159    st->windowSize = mode->windowSize;
160    st->lpcSize=8;
161
162    st->lag_factor = .002;
163    st->lpc_floor = 1.0001;
164    st->gamma1=.9;
165    st->gamma2=.6;
166    st->first=1;
167    st->stack = calloc(10000, sizeof(float));
168
169    st->x0=calloc(st->full_frame_size, sizeof(float));
170    st->x1=calloc(st->full_frame_size, sizeof(float));
171    st->x0d=calloc(st->frame_size, sizeof(float));
172    st->x1d=calloc(st->frame_size, sizeof(float));
173    st->high=calloc(st->full_frame_size, sizeof(float));
174    st->y0=calloc(st->full_frame_size, sizeof(float));
175    st->y1=calloc(st->full_frame_size, sizeof(float));
176
177    st->h0_mem=calloc(QMF_ORDER, sizeof(float));
178    st->h1_mem=calloc(QMF_ORDER, sizeof(float));
179    st->g0_mem=calloc(QMF_ORDER, sizeof(float));
180    st->g1_mem=calloc(QMF_ORDER, sizeof(float));
181
182    st->buf=calloc(st->windowSize, sizeof(float));
183    st->excBuf=calloc(2*st->frame_size, sizeof(float));
184    st->exc=st->excBuf+st->frame_size;
185    st->exc_alias=calloc(st->frame_size, sizeof(float));
186
187    st->res=calloc(st->frame_size, sizeof(float));
188    st->sw=calloc(st->frame_size, sizeof(float));
189    st->target=calloc(st->frame_size, sizeof(float));
190    st->window=calloc(st->windowSize, sizeof(float));
191    for (i=0;i<st->windowSize;i++)
192       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
193
194    st->exc_window=calloc(st->frame_size, sizeof(float));
195    for (i=0;i<st->frame_size;i++)
196       st->exc_window[i]=.5*(1-cos(2*M_PI*i/st->frame_size));
197
198    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
199    for (i=0;i<st->lpcSize+1;i++)
200       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
201
202    st->rc = malloc(st->lpcSize*sizeof(float));
203    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
204    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
205    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
206    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
207    st->lsp = malloc(st->lpcSize*sizeof(float));
208    st->qlsp = malloc(st->lpcSize*sizeof(float));
209    st->old_lsp = malloc(st->lpcSize*sizeof(float));
210    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
211    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
212    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
213    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
214    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
215
216    st->mem_sp = calloc(st->lpcSize, sizeof(float));
217    st->mem_sp2 = calloc(st->lpcSize, sizeof(float));
218    st->mem_sw = calloc(st->lpcSize, sizeof(float));
219    st->mem_exc = calloc(st->lpcSize, sizeof(float));
220
221 }
222
223 void sb_encoder_destroy(SBEncState *st)
224 {
225    encoder_destroy(&st->st_low);
226    free(st->x0);
227    free(st->x0d);
228    free(st->x1);
229    free(st->x1d);
230    free(st->high);
231    free(st->y0);
232    free(st->y1);
233    free(st->h0_mem);
234    free(st->h1_mem);
235    free(st->g0_mem);
236    free(st->g1_mem);
237    
238    free(st->buf);
239    free(st->window);
240    free(st->excBuf);
241    free(st->sw);
242    free(st->res);
243    free(st->target);
244    free(st->lagWindow);
245    free(st->rc);
246    free(st->autocorr);
247    free(st->lpc);
248    free(st->bw_lpc1);
249    free(st->bw_lpc2);
250    free(st->lsp);
251    free(st->qlsp);
252    free(st->old_lsp);
253    free(st->old_qlsp);
254    free(st->interp_lsp);
255    free(st->interp_qlsp);
256    free(st->interp_lpc);
257    free(st->interp_qlpc);
258
259    free(st->mem_sp);
260    free(st->mem_sw);
261
262    free(st->stack);
263    
264 }
265
266
267 void sb_encode(SBEncState *st, float *in, FrameBits *bits)
268 {
269    int i, roots, sub;
270    /* Compute the two sub-bands by filtering with h0 and h1*/
271    fir_mem(in, h0, st->x0, st->full_frame_size, QMF_ORDER, st->h0_mem);
272    fir_mem(in, h1, st->x1, st->full_frame_size, QMF_ORDER, st->h1_mem);
273    /* Down-sample x0 and x1 */
274    for (i=0;i<st->frame_size;i++)
275    {
276       st->x0d[i]=st->x0[i<<1];
277       st->x1d[i]=st->x1[i<<1];
278    }
279    /* Encode the narrowband part*/
280    encode(&st->st_low, st->x0d, bits);
281
282    /* High-band buffering / sync with low band */
283    for (i=0;i<st->frame_size;i++)
284    {
285       st->excBuf[i]=st->exc[i];
286       st->high[i]=st->high[st->frame_size+i];
287       st->high[st->frame_size+i]=st->x1d[i];
288    }
289    
290    /* Start encoding the high-band */
291
292    for (i=0;i<st->windowSize;i++)
293       st->buf[i] = st->high[i] * st->window[i];
294
295    /* Compute auto-correlation */
296    autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
297
298    st->autocorr[0] += 1;        /* prevents NANs */
299    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
300    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
301    for (i=0;i<st->lpcSize+1;i++)
302       st->autocorr[i] *= st->lagWindow[i];
303
304    /* Levinson-Durbin */
305    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
306    st->lpc[0]=1;
307
308    /* LPC to LSPs (x-domain) transform */
309    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
310    if (roots!=st->lpcSize)
311    {
312       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
313       exit(1);
314    }
315
316    {
317       for (i=0;i<st->frame_size;i++)
318          st->buf[i] = st->st_low.exc[i] * st->exc_window[i];
319       
320       /* Compute auto-correlation */
321       autocorr(st->buf, st->autocorr, st->lpcSize+1, st->frame_size);
322       
323       st->autocorr[0] += 1;        /* prevents NANs */
324       st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
325       /* Lag windowing: equivalent to filtering in the power-spectrum domain */
326       for (i=0;i<st->lpcSize+1;i++)
327          st->autocorr[i] *= st->lagWindow[i];
328       
329       /* Levinson-Durbin */
330       wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
331       st->lpc[0]=1;
332       printf ("exc_lpc: ");
333       for(i=0;i<=st->lpcSize;i++)
334          printf ("%f ", st->lpc[i]);
335       printf ("\n");
336       residue_mem(st->st_low.exc, st->lpc, st->exc_alias, st->frame_size, st->lpcSize, st->mem_exc);
337    }
338
339
340    /* x-domain to angle domain*/
341    for (i=0;i<st->lpcSize;i++)
342       st->lsp[i] = acos(st->lsp[i]);
343    
344    /* FIXME: Need to really quantize the LSPs*/
345    for (i=0;i<st->lpcSize;i++)
346       st->qlsp[i]=st->lsp[i];
347    if (st->first)
348    {
349       for (i=0;i<st->lpcSize;i++)
350          st->old_lsp[i] = st->lsp[i];
351       for (i=0;i<st->lpcSize;i++)
352          st->old_qlsp[i] = st->qlsp[i];
353    }
354    
355    for (sub=0;sub<st->nbSubframes;sub++)
356    {
357       float *exc, *sp, *mem, *res, *target, *sw, tmp;
358       int offset;
359       
360       offset = st->subframeSize*sub;
361       sp=st->high+offset;
362       exc=st->excBuf+offset;
363       res=st->res+offset;
364       target=st->target+offset;
365       sw=st->sw+offset;
366
367       mem=PUSH(st->stack, st->lpcSize);
368       
369       /* LSP interpolation (quantized and unquantized) */
370       tmp = (.5 + 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       /* Compute interpolated LPCs (quantized and unquantized) */
377       for (i=0;i<st->lpcSize;i++)
378          st->interp_lsp[i] = cos(st->interp_lsp[i]);
379       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
380
381       for (i=0;i<st->lpcSize;i++)
382          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
383       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
384
385       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
386       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
387 #if 0 /* 1 for spectral folding excitation, 0 for stochastic */
388       for (i=0;i<st->lpcSize;i++)
389          mem[i]=st->mem_sp[i];
390       residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp);
391       {
392          float el=0,eh=0,g;
393          printf ("exca");
394          for (i=0;i<st->subframeSize;i++)
395             printf (" %f", exc[i]);
396          printf ("\n");
397          for (i=0;i<st->subframeSize;i++)
398             eh+=sqr(exc[i]);
399          /*for (i=0;i<st->subframeSize;i++)
400             el+=sqr(st->exc_alias[offset+i]);*/
401          for (i=0;i<st->subframeSize;i++)
402            el+=sqr(st->st_low.exc[offset+i]);
403          g=eh/(.01+el);
404          g=sqrt(g);
405          for (i=0;i<st->subframeSize;i++)
406            exc[i]=g*st->st_low.exc[offset+i];
407          /*for (i=0;i<st->subframeSize;i++)
408            exc[i]=g*st->exc_alias[offset+i];*/
409          printf ("excb");
410          for (i=0;i<st->subframeSize;i++)
411             printf (" %f", exc[i]);
412          printf ("\n");
413       }
414       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, mem);
415
416 #else
417       /* Reset excitation */
418       for (i=0;i<st->subframeSize;i++)
419          exc[i]=0;
420
421       /* Compute zero response of A(z/g1) / ( A(z/g2) * Aq(z) ) */
422       for (i=0;i<st->lpcSize;i++)
423          mem[i]=st->mem_sp[i];
424       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
425       for (i=0;i<st->lpcSize;i++)
426          mem[i]=st->mem_sp[i];
427       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
428       for (i=0;i<st->lpcSize;i++)
429          mem[i]=st->mem_sw[i];
430       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
431
432       /* Compute weighted signal */
433       for (i=0;i<st->lpcSize;i++)
434          mem[i]=st->mem_sp[i];
435       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
436       for (i=0;i<st->lpcSize;i++)
437          mem[i]=st->mem_sw[i];
438       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
439
440       /* Compute target signal */
441       for (i=0;i<st->subframeSize;i++)
442          target[i]=sw[i]-res[i];
443       {
444          int ind;
445          float gain;
446 #if 0
447          
448          float el=0,eh=0,g;
449          residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
450          
451          for (i=0;i<st->subframeSize;i++)
452             eh+=sqr(exc[i]);
453          overlap_cb_search(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
454                            &stoc[0], 512, &gain, &ind, st->lpcSize,
455                            st->subframeSize);
456          for (i=0;i<st->subframeSize;i++)
457             exc[i]=gain*stoc[ind+i];
458          for (i=0;i<st->subframeSize;i++)
459             el+=sqr(exc[i]);
460          g=sqrt(eh/(el+.001));
461          for (i=0;i<st->subframeSize;i++)
462             exc[i]*=g;
463          
464 #else
465          int k,N=2;
466          float el=0,eh=0,g;
467          residue_mem(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
468          
469          for (i=0;i<st->subframeSize;i++)
470             eh+=sqr(exc[i]);
471
472          for (i=0;i<st->subframeSize;i++)
473             exc[i]=0;
474          for (k=0;k<N;k++)
475          {
476             int of=k*st->subframeSize/N;
477          overlap_cb_search(target+of, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
478                            &stoc[0], 512, &gain, &ind, st->lpcSize,
479                            st->subframeSize/N);
480          for (i=0;i<st->subframeSize;i++)
481             res[i]=0;
482          for (i=0;i<st->subframeSize/N;i++)
483             res[of+i]=gain*stoc[ind+i];
484          residue_zero(res, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
485          syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
486          syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
487          for (i=0;i<st->subframeSize;i++)
488             target[i]-=res[i];
489          for (i=0;i<st->subframeSize/N;i++)
490             exc[of+i]+=gain*stoc[ind+i];
491          }
492          for (i=0;i<st->subframeSize;i++)
493             el+=sqr(exc[i]);
494          g=sqrt(eh/(el+.001));
495          for (i=0;i<st->subframeSize;i++)
496             exc[i]*=g;
497
498 #endif
499       }
500
501       /*Keep the previous memory*/
502       for (i=0;i<st->lpcSize;i++)
503          mem[i]=st->mem_sp[i];
504       /* Final signal synthesis from excitation */
505       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
506        
507       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
508       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
509       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
510 #endif
511
512       POP(st->stack);
513    }
514
515    /* Up-sample coded low-band and high-band*/
516    for (i=0;i<st->frame_size;i++)
517    {
518       st->x0[(i<<1)]=st->x0d[i];
519       st->x1[(i<<1)]=st->high[i];
520       st->x0[(i<<1)+1]=0;
521       st->x1[(i<<1)+1]=0;
522    }
523    /* Reconstruct the original */
524    fir_mem(st->x0, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem);
525    fir_mem(st->x1, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem);
526    for (i=0;i<st->full_frame_size;i++)
527       in[i]=2*(st->y0[i]-st->y1[i]);
528
529    for (i=0;i<st->lpcSize;i++)
530       st->old_lsp[i] = st->lsp[i];
531    for (i=0;i<st->lpcSize;i++)
532       st->old_qlsp[i] = st->qlsp[i];
533
534    st->first=0;
535 }