Cleaned up vocoder mode...
[speexdsp.git] / libspeex / ltp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: ltp.c
3    Lont-Term Prediction functions
4
5    This library is free software; you can redistribute it and/or
6    modify it under the terms of the GNU Lesser General Public
7    License as published by the Free Software Foundation; either
8    version 2.1 of the License, or (at your option) any later version.
9    
10    This library is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13    Lesser General Public License for more details.
14    
15    You should have received a copy of the GNU Lesser General Public
16    License along with this library; if not, write to the Free Software
17    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18 */
19
20 #include <math.h>
21 #include <stdio.h>
22 #include "ltp.h"
23 #include "stack_alloc.h"
24 #include "filters.h"
25 #include "speex_bits.h"
26
27 #define abs(x) ((x)<0 ? -(x) : (x))
28
29
30 void open_loop_nbest_pitch(float *sw, int start, int end, int len, int *pitch, float *gain, int N, float *stack)
31 {
32    int i,j,k;
33    float corr;
34    float energy;
35    float score, *best_score;
36    float e0;
37
38    best_score = PUSH(stack,N);
39    for (i=0;i<N;i++)
40         best_score[i]=-1;
41    energy=xcorr(sw-start, sw-start, len);
42    e0=xcorr(sw, sw, len);
43    for (i=start;i<=end;i++)
44    {
45       corr=xcorr(sw, sw-i, len);
46       score=corr*corr/(energy+1);
47       if (score>best_score[N-1])
48       {
49          float g1, g;
50          g1 = corr/(energy+10);
51          g = sqrt(corr/(e0+10));
52          if (g>g1)
53             g=g1;
54          if (g<0)
55             g=0;
56          for (j=0;j<N;j++)
57          {
58             if (score > best_score[j])
59             {
60                for (k=N-1;k>j;k--)
61                {
62                   best_score[k]=best_score[k-1];
63                   pitch[k]=pitch[k-1];
64                   gain[k] = gain[k-1];
65                }
66                best_score[j]=score;
67                pitch[j]=i;
68                gain[j]=g;
69                break;
70             }
71          }
72       }
73       /* Update energy for next pitch*/
74       energy+=sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
75    }
76
77    POP(stack);
78 }
79
80
81
82
83 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
84 float pitch_gain_search_3tap(
85 float target[],                 /* Target vector */
86 float ak[],                     /* LPCs for this subframe */
87 float awk1[],                   /* Weighted LPCs #1 for this subframe */
88 float awk2[],                   /* Weighted LPCs #2 for this subframe */
89 float exc[],                    /* Excitation */
90 void *par,
91 int   pitch,                    /* Pitch value */
92 int   p,                        /* Number of LPC coeffs */
93 int   nsf,                      /* Number of samples in subframe */
94 SpeexBits *bits,
95 float *stack,
96 float *exc2,
97 int  *cdbk_index
98 )
99 {
100    int i,j;
101    float *tmp, *tmp2;
102    float *x[3];
103    float *e[3];
104    float corr[3];
105    float A[3][3];
106    float gain[3];
107    int   gain_cdbk_size;
108    float *gain_cdbk;
109    float err1,err2;
110    ltp_params *params;
111    params = (ltp_params*) par;
112    gain_cdbk=params->gain_cdbk;
113    gain_cdbk_size=1<<params->gain_bits;
114    tmp = PUSH(stack, 3*nsf);
115    tmp2 = PUSH(stack, 3*nsf);
116
117    x[0]=tmp;
118    x[1]=tmp+nsf;
119    x[2]=tmp+2*nsf;
120
121    e[0]=tmp2;
122    e[1]=tmp2+nsf;
123    e[2]=tmp2+2*nsf;
124    
125    for (i=0;i<3;i++)
126    {
127       int pp=pitch+1-i;
128       for (j=0;j<nsf;j++)
129       {
130          if (j-pp<0)
131             e[i][j]=exc2[j-pp];
132          else if (j-pp-pitch<0)
133             e[i][j]=exc2[j-pp-pitch];
134          else
135             e[i][j]=0;
136       }
137
138       if (p==10)
139       {
140          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p);
141       } else {
142          residue_zero(e[i],awk1,x[i],nsf,p);
143          syn_filt_zero(x[i],ak,x[i],nsf,p);
144          syn_filt_zero(x[i],awk2,x[i],nsf,p);
145       }
146    }
147
148    for (i=0;i<3;i++)
149       corr[i]=xcorr(x[i],target,nsf);
150    
151    for (i=0;i<3;i++)
152       for (j=0;j<=i;j++)
153          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
154    
155    {
156       int j;
157       float C[9];
158       float *ptr=gain_cdbk;
159       int best_cdbk=0;
160       float best_sum=0;
161       C[0]=corr[2];
162       C[1]=corr[1];
163       C[2]=corr[0];
164       C[3]=A[1][2];
165       C[4]=A[0][1];
166       C[5]=A[0][2];
167       C[6]=A[2][2];
168       C[7]=A[1][1];
169       C[8]=A[0][0];
170       
171       for (i=0;i<gain_cdbk_size;i++)
172       {
173          float sum=0;
174          ptr = gain_cdbk+12*i;
175          for (j=0;j<9;j++)
176             sum+=C[j]*ptr[j+3];
177          if (0) {
178             float tot=ptr[0]+ptr[1]+ptr[2];
179             if (tot < 1.1)
180                sum *= 1+.15*tot;
181          }
182          if (sum>best_sum || i==0)
183          {
184             best_sum=sum;
185             best_cdbk=i;
186          }
187       }
188       gain[0] = gain_cdbk[best_cdbk*12];
189       gain[1] = gain_cdbk[best_cdbk*12+1];
190       gain[2] = gain_cdbk[best_cdbk*12+2];
191
192       *cdbk_index=best_cdbk;
193    }
194    /* Calculate gains by matrix inversion... (unquantized) */
195    if (0) {
196       float tmp;
197       float B[3][3];
198       A[0][0]+=1;
199       A[1][1]+=1;
200       A[2][2]+=1;
201       
202       for (i=0;i<3;i++)
203          for (j=0;j<3;j++)
204             B[i][j]=A[i][j];
205
206
207       tmp=A[1][0]/A[0][0];
208       for (i=0;i<3;i++)
209          A[1][i] -= tmp*A[0][i];
210       corr[1] -= tmp*corr[0];
211
212       tmp=A[2][0]/A[0][0];
213       for (i=0;i<3;i++)
214          A[2][i] -= tmp*A[0][i];
215       corr[2] -= tmp*corr[0];
216       
217       tmp=A[2][1]/A[1][1];
218       A[2][2] -= tmp*A[1][2];
219       corr[2] -= tmp*corr[1];
220
221       corr[2] /= A[2][2];
222       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
223       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
224       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
225
226    
227       /* Put gains in right order */
228       gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];
229
230       {
231          float gain_sum = gain[0]+gain[1]+gain[2];
232          if (fabs(gain_sum)>2.5)
233          {
234             float fact = 2.5/gain_sum;
235             for (i=0;i<3;i++)
236                gain[i]*=fact;
237          }
238       }
239       
240    }
241    
242    for (i=0;i<nsf;i++)
243       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
244 #ifdef DEBUG
245    printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
246 #endif
247    
248    err1=0;
249    err2=0;
250    for (i=0;i<nsf;i++)
251       err1+=target[i]*target[i];
252    for (i=0;i<nsf;i++)
253       err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
254       * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
255 #ifdef DEBUG
256    printf ("prediction gain = %f\n",err1/(err2+1));
257 #endif
258
259    POP(stack);
260    POP(stack);
261    return err2;
262 }
263
264
265 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
266 int pitch_search_3tap(
267 float target[],                 /* Target vector */
268 float *sw,
269 float ak[],                     /* LPCs for this subframe */
270 float awk1[],                   /* Weighted LPCs #1 for this subframe */
271 float awk2[],                   /* Weighted LPCs #2 for this subframe */
272 float exc[],                    /* Excitation */
273 void *par,
274 int   start,                    /* Smallest pitch value allowed */
275 int   end,                      /* Largest pitch value allowed */
276 float pitch_coef,               /* Voicing (pitch) coefficient */
277 int   p,                        /* Number of LPC coeffs */
278 int   nsf,                      /* Number of samples in subframe */
279 SpeexBits *bits,
280 float *stack,
281 float *exc2,
282 int complexity
283 )
284 {
285    int i,j;
286    int cdbk_index, pitch=0, best_gain_index=0;
287    float *best_exc;
288    int best_pitch=0;
289    float err, best_err=-1;
290    int N=3;
291    ltp_params *params;
292    int *nbest;
293    float *gains;
294
295    N=complexity;
296    if (N<1)
297       N=1;
298    if (N>10)
299       N=10;
300
301    nbest=(int*)PUSH(stack, N);
302    gains = PUSH(stack, N);
303    params = (ltp_params*) par;
304    
305    best_exc=PUSH(stack,nsf);
306    
307    if (N>end-start+1)
308       N=end-start+1;
309    open_loop_nbest_pitch(sw, start, end, nsf, nbest, gains, N, stack);
310    for (i=0;i<N;i++)
311    {
312       pitch=nbest[i];
313       for (j=0;j<nsf;j++)
314          exc[j]=0;
315       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
316                                  bits, stack, exc2, &cdbk_index);
317       if (err<best_err || best_err<0)
318       {
319          for (j=0;j<nsf;j++)
320             best_exc[j]=exc[j];
321          best_err=err;
322          best_pitch=pitch;
323          best_gain_index=cdbk_index;
324       }
325    }
326    
327    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
328    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
329    speex_bits_pack(bits, best_gain_index, params->gain_bits);
330    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
331    for (i=0;i<nsf;i++)
332       exc[i]=best_exc[i];
333
334    POP(stack);
335    POP(stack);
336    POP(stack);
337    return pitch;
338 }
339
340
341 void pitch_unquant_3tap(
342 float exc[],                    /* Excitation */
343 int   start,                    /* Smallest pitch value allowed */
344 int   end,                      /* Largest pitch value allowed */
345 float pitch_coef,               /* Voicing (pitch) coefficient */
346 void *par,
347 int   nsf,                      /* Number of samples in subframe */
348 int *pitch_val,
349 float *gain_val,
350 SpeexBits *bits,
351 float *stack,
352 int lost)
353 {
354    int i;
355    int pitch;
356    int gain_index;
357    float gain[3];
358    float *gain_cdbk;
359    ltp_params *params;
360    params = (ltp_params*) par;
361    gain_cdbk=params->gain_cdbk;
362
363    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
364    pitch += start;
365    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
366    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
367    gain[0] = gain_cdbk[gain_index*12];
368    gain[1] = gain_cdbk[gain_index*12+1];
369    gain[2] = gain_cdbk[gain_index*12+2];
370    if (lost)
371    {
372       float gain_sum;
373       /*Put everything in one tap*/
374       gain[1]+=gain[0]+gain[2];
375       gain[0]=gain[2]=0;
376       gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
377       if (gain_sum>.85)
378       {
379          float fact = .85/gain_sum;
380          for (i=0;i<3;i++)
381             gain[i]*=fact;
382       }
383       /*gain[1]=.8;*/
384    }
385
386    *pitch_val = pitch;
387    /**gain_val = gain[0]+gain[1]+gain[2];*/
388    gain_val[0]=gain[0];
389    gain_val[1]=gain[1];
390    gain_val[2]=gain[2];
391
392 #ifdef DEBUG
393    printf ("unquantized pitch: %d %f %f %f\n", pitch, gain[0], gain[1], gain[2]);
394 #endif
395    for(i=0;i<nsf;i++)
396       exc[i]=0;
397
398    {
399       float *e[3];
400       float *tmp2;
401       tmp2=PUSH(stack, 3*nsf);
402       e[0]=tmp2;
403       e[1]=tmp2+nsf;
404       e[2]=tmp2+2*nsf;
405       
406       for (i=0;i<3;i++)
407       {
408          int j;
409          int pp=pitch+1-i;
410          for (j=0;j<nsf;j++)
411          {
412             if (j-pp<0)
413                e[i][j]=exc[j-pp];
414             else
415                e[i][j]=exc[j-pp-pitch];
416          }
417       }
418       for (i=0;i<nsf;i++)
419          exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
420       
421       POP(stack);
422    }
423 }
424
425
426 /** Forced pitch delay and gain */
427 int forced_pitch_quant(
428 float target[],                 /* Target vector */
429 float *sw,
430 float ak[],                     /* LPCs for this subframe */
431 float awk1[],                   /* Weighted LPCs #1 for this subframe */
432 float awk2[],                   /* Weighted LPCs #2 for this subframe */
433 float exc[],                    /* Excitation */
434 void *par,
435 int   start,                    /* Smallest pitch value allowed */
436 int   end,                      /* Largest pitch value allowed */
437 float pitch_coef,               /* Voicing (pitch) coefficient */
438 int   p,                        /* Number of LPC coeffs */
439 int   nsf,                      /* Number of samples in subframe */
440 SpeexBits *bits,
441 float *stack,
442 float *exc2,
443 int complexity
444 )
445 {
446    int i;
447    if (pitch_coef>.9)
448       pitch_coef=.9;
449    for (i=0;i<nsf;i++)
450    {
451       exc[i]=exc[i-start]*pitch_coef;
452    }
453    return start;
454 }
455
456 /** Unquantize forced pitch delay and gain */
457 void forced_pitch_unquant(
458 float exc[],                    /* Excitation */
459 int   start,                    /* Smallest pitch value allowed */
460 int   end,                      /* Largest pitch value allowed */
461 float pitch_coef,               /* Voicing (pitch) coefficient */
462 void *par,
463 int   nsf,                      /* Number of samples in subframe */
464 int *pitch_val,
465 float *gain_val,
466 SpeexBits *bits,
467 float *stack,
468 int lost)
469 {
470    int i;
471    /*pitch_coef=.9;*/
472    if (pitch_coef>.9)
473       pitch_coef=.9;
474    for (i=0;i<nsf;i++)
475    {
476       exc[i]=exc[i-start]*pitch_coef;
477    }
478    *pitch_val = start;
479    *gain_val = pitch_coef;
480 }