fixed-point: converted all signals to spx_sig_t
[speexdsp.git] / libspeex / ltp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: ltp.c
3    Long-Term Prediction functions
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <math.h>
34 #include "ltp.h"
35 #include "stack_alloc.h"
36 #include "filters.h"
37 #include "speex_bits.h"
38
39 #ifdef _USE_SSE
40 #include "ltp_sse.h"
41 #else
42 static float inner_prod(spx_sig_t *x, spx_sig_t *y, int len)
43 {
44    int i;
45    float sum1=0,sum2=0,sum3=0,sum4=0;
46    for (i=0;i<len;)
47    {
48       sum1 += x[i]*y[i];
49       sum2 += x[i+1]*y[i+1];
50       sum3 += x[i+2]*y[i+2];
51       sum4 += x[i+3]*y[i+3];
52       i+=4;
53    }
54    return sum1+sum2+sum3+sum4;
55 }
56 #endif
57
58 /*Original, non-optimized version*/
59 /*static float inner_prod(float *x, float *y, int len)
60 {
61    int i;
62    float sum=0;
63    for (i=0;i<len;i++)
64       sum += x[i]*y[i];
65    return sum;
66 }
67 */
68
69
70 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, float *gain, int N, char *stack)
71 {
72    int i,j,k;
73    /*float corr=0;*/
74    /*float energy;*/
75    float *best_score;
76    float e0;
77    float *corr, *energy, *score;
78
79    best_score = PUSH(stack,N, float);
80    corr = PUSH(stack,end-start+1, float);
81    energy = PUSH(stack,end-start+2, float);
82    score = PUSH(stack,end-start+1, float);
83    for (i=0;i<N;i++)
84    {
85         best_score[i]=-1;
86         gain[i]=0;
87    }
88    energy[0]=inner_prod(sw-start, sw-start, len);
89    e0=inner_prod(sw, sw, len);
90    for (i=start;i<=end;i++)
91    {
92       /* Update energy for next pitch*/
93       energy[i-start+1] = energy[i-start] + sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
94    }
95    for (i=start;i<=end;i++)
96    {
97       corr[i-start]=0;
98       score[i-start]=0;
99    }
100
101    for (i=start;i<=end;i++)
102    {
103       /* Compute correlation*/
104       corr[i-start]=inner_prod(sw, sw-i, len);
105       score[i-start]=corr[i-start]*corr[i-start]/(energy[i-start]+1);
106    }
107    for (i=start;i<=end;i++)
108    {
109       if (score[i-start]>best_score[N-1])
110       {
111          float g1, g;
112          g1 = corr[i-start]/(energy[i-start]+10);
113          g = sqrt(g1*corr[i-start]/(e0+10));
114          if (g>g1)
115             g=g1;
116          if (g<0)
117             g=0;
118          for (j=0;j<N;j++)
119          {
120             if (score[i-start] > best_score[j])
121             {
122                for (k=N-1;k>j;k--)
123                {
124                   best_score[k]=best_score[k-1];
125                   pitch[k]=pitch[k-1];
126                   gain[k] = gain[k-1];
127                }
128                best_score[j]=score[i-start];
129                pitch[j]=i;
130                gain[j]=g;
131                break;
132             }
133          }
134       }
135    }
136
137 }
138
139
140
141
142 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
143 static float pitch_gain_search_3tap(
144 spx_sig_t target[],                 /* Target vector */
145 spx_coef_t ak[],                     /* LPCs for this subframe */
146 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
147 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
148 spx_sig_t exc[],                    /* Excitation */
149 void *par,
150 int   pitch,                    /* Pitch value */
151 int   p,                        /* Number of LPC coeffs */
152 int   nsf,                      /* Number of samples in subframe */
153 SpeexBits *bits,
154 char *stack,
155 spx_sig_t *exc2,
156 spx_sig_t *r,
157 int  *cdbk_index,
158 int cdbk_offset
159 )
160 {
161    int i,j;
162    spx_sig_t *tmp, *tmp2;
163    spx_sig_t *x[3];
164    spx_sig_t *e[3];
165    float corr[3];
166    float A[3][3];
167    float gain[3];
168    int   gain_cdbk_size;
169    signed char *gain_cdbk;
170    float err1,err2;
171
172    ltp_params *params;
173    params = (ltp_params*) par;
174    gain_cdbk_size = 1<<params->gain_bits;
175    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
176    tmp = PUSH(stack, 3*nsf, spx_sig_t);
177    tmp2 = PUSH(stack, 3*nsf, spx_sig_t);
178
179    x[0]=tmp;
180    x[1]=tmp+nsf;
181    x[2]=tmp+2*nsf;
182
183    e[0]=tmp2;
184    e[1]=tmp2+nsf;
185    e[2]=tmp2+2*nsf;
186    
187    for (i=2;i>=0;i--)
188    {
189       int pp=pitch+1-i;
190       for (j=0;j<nsf;j++)
191       {
192          if (j-pp<0)
193             e[i][j]=exc2[j-pp];
194          else if (j-pp-pitch<0)
195             e[i][j]=exc2[j-pp-pitch];
196          else
197             e[i][j]=0;
198       }
199
200       if (i==2)
201          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
202       else {
203          for (j=0;j<nsf-1;j++)
204             x[i][j+1]=x[i+1][j];
205          x[i][0]=0;
206          for (j=0;j<nsf;j++)
207             x[i][j]+=e[i][0]*r[j]/SIG_SCALING;
208       }
209    }
210
211    for (i=0;i<3;i++)
212       corr[i]=inner_prod(x[i],target,nsf);
213    
214    for (i=0;i<3;i++)
215       for (j=0;j<=i;j++)
216          A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
217    
218    {
219       float C[9];
220       signed char *ptr=gain_cdbk;
221       int best_cdbk=0;
222       float best_sum=0;
223       C[0]=corr[2];
224       C[1]=corr[1];
225       C[2]=corr[0];
226       C[3]=A[1][2];
227       C[4]=A[0][1];
228       C[5]=A[0][2];
229       C[6]=A[2][2];
230       C[7]=A[1][1];
231       C[8]=A[0][0];
232       
233       for (i=0;i<gain_cdbk_size;i++)
234       {
235          float sum=0;
236          float g0,g1,g2;
237          ptr = gain_cdbk+3*i;
238          g0=0.015625*ptr[0]+.5;
239          g1=0.015625*ptr[1]+.5;
240          g2=0.015625*ptr[2]+.5;
241
242          sum += C[0]*g0;
243          sum += C[1]*g1;
244          sum += C[2]*g2;
245          sum -= C[3]*g0*g1;
246          sum -= C[4]*g2*g1;
247          sum -= C[5]*g2*g0;
248          sum -= .5*C[6]*g0*g0;
249          sum -= .5*C[7]*g1*g1;
250          sum -= .5*C[8]*g2*g2;
251
252          /* If 1, force "safe" pitch values to handle packet loss better */
253          if (0) {
254             float tot = fabs(ptr[1]);
255             if (ptr[0]>0)
256                tot+=ptr[0];
257             if (ptr[2]>0)
258                tot+=ptr[2];
259             if (tot>1)
260                continue;
261          }
262
263          if (sum>best_sum || i==0)
264          {
265             best_sum=sum;
266             best_cdbk=i;
267          }
268       }
269       gain[0] = 0.015625*gain_cdbk[best_cdbk*3]  + .5;
270       gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
271       gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
272
273       *cdbk_index=best_cdbk;
274    }
275    
276    for (i=0;i<nsf;i++)
277       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
278    
279    err1=0;
280    err2=0;
281    for (i=0;i<nsf;i++)
282       err1+=target[i]*target[i];
283    for (i=0;i<nsf;i++)
284       err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
285       * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
286
287    return err2;
288 }
289
290
291 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
292 int pitch_search_3tap(
293 spx_sig_t target[],                 /* Target vector */
294 spx_sig_t *sw,
295 spx_coef_t ak[],                     /* LPCs for this subframe */
296 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
297 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
298 spx_sig_t exc[],                    /* Excitation */
299 void *par,
300 int   start,                    /* Smallest pitch value allowed */
301 int   end,                      /* Largest pitch value allowed */
302 float pitch_coef,               /* Voicing (pitch) coefficient */
303 int   p,                        /* Number of LPC coeffs */
304 int   nsf,                      /* Number of samples in subframe */
305 SpeexBits *bits,
306 char *stack,
307 spx_sig_t *exc2,
308 spx_sig_t *r,
309 int complexity,
310 int cdbk_offset
311 )
312 {
313    int i,j;
314    int cdbk_index, pitch=0, best_gain_index=0;
315    float *best_exc;
316    int best_pitch=0;
317    float err, best_err=-1;
318    int N;
319    ltp_params *params;
320    int *nbest;
321    float *gains;
322
323    N=complexity;
324    if (N>10)
325       N=10;
326
327    nbest=PUSH(stack, N, int);
328    gains = PUSH(stack, N, float);
329    params = (ltp_params*) par;
330
331    if (N==0 || end<start)
332    {
333       speex_bits_pack(bits, 0, params->pitch_bits);
334       speex_bits_pack(bits, 0, params->gain_bits);
335       for (i=0;i<nsf;i++)
336          exc[i]=0;
337       return start;
338    }
339    
340    best_exc=PUSH(stack,nsf, float);
341    
342    if (N>end-start+1)
343       N=end-start+1;
344    open_loop_nbest_pitch(sw, start, end, nsf, nbest, gains, N, stack);
345    for (i=0;i<N;i++)
346    {
347       pitch=nbest[i];
348       for (j=0;j<nsf;j++)
349          exc[j]=0;
350       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
351                                  bits, stack, exc2, r, &cdbk_index, cdbk_offset);
352       if (err<best_err || best_err<0)
353       {
354          for (j=0;j<nsf;j++)
355             best_exc[j]=exc[j];
356          best_err=err;
357          best_pitch=pitch;
358          best_gain_index=cdbk_index;
359       }
360    }
361    
362    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
363    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
364    speex_bits_pack(bits, best_gain_index, params->gain_bits);
365    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
366    for (i=0;i<nsf;i++)
367       exc[i]=best_exc[i];
368
369    return pitch;
370 }
371
372 void pitch_unquant_3tap(
373 spx_sig_t exc[],                    /* Excitation */
374 int   start,                    /* Smallest pitch value allowed */
375 int   end,                      /* Largest pitch value allowed */
376 float pitch_coef,               /* Voicing (pitch) coefficient */
377 void *par,
378 int   nsf,                      /* Number of samples in subframe */
379 int *pitch_val,
380 float *gain_val,
381 SpeexBits *bits,
382 char *stack,
383 int count_lost,
384 int subframe_offset,
385 float last_pitch_gain,
386 int cdbk_offset
387 )
388 {
389    int i;
390    int pitch;
391    int gain_index;
392    float gain[3];
393    signed char *gain_cdbk;
394    int gain_cdbk_size;
395    ltp_params *params;
396    params = (ltp_params*) par;
397    gain_cdbk_size = 1<<params->gain_bits;
398    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
399
400    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
401    pitch += start;
402    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
403    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
404    gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5;
405    gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5;
406    gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5;
407
408    if (count_lost && pitch > subframe_offset)
409    {
410       float gain_sum;
411
412       if (1) {
413          float tmp = count_lost < 4 ? last_pitch_gain : 0.4 * last_pitch_gain;
414          if (tmp>.95)
415             tmp=.95;
416          gain_sum = fabs(gain[1]);
417          if (gain[0]>0)
418             gain_sum += gain[0];
419          else
420             gain_sum -= .5*gain[0];
421          if (gain[2]>0)
422             gain_sum += gain[2];
423          else
424             gain_sum -= .5*gain[2];
425          if (gain_sum > tmp) {
426             float fact = tmp/gain_sum;
427             for (i=0;i<3;i++)
428                gain[i]*=fact;
429
430          }
431
432       }
433
434       if (0) {
435       gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
436          if (gain_sum>.95) {
437          float fact = .95/gain_sum;
438          for (i=0;i<3;i++)
439             gain[i]*=fact;
440       }
441    }
442    }
443
444    *pitch_val = pitch;
445    /**gain_val = gain[0]+gain[1]+gain[2];*/
446    gain_val[0]=gain[0];
447    gain_val[1]=gain[1];
448    gain_val[2]=gain[2];
449
450    {
451       float *e[3];
452       float *tmp2;
453       tmp2=PUSH(stack, 3*nsf, float);
454       e[0]=tmp2;
455       e[1]=tmp2+nsf;
456       e[2]=tmp2+2*nsf;
457       
458       for (i=0;i<3;i++)
459       {
460          int j;
461          int pp=pitch+1-i;
462 #if 0
463          for (j=0;j<nsf;j++)
464          {
465             if (j-pp<0)
466                e[i][j]=exc[j-pp];
467             else if (j-pp-pitch<0)
468                e[i][j]=exc[j-pp-pitch];
469             else
470                e[i][j]=0;
471          }
472 #else
473          {
474             int tmp1, tmp3;
475             tmp1=nsf;
476             if (tmp1>pp)
477                tmp1=pp;
478             for (j=0;j<tmp1;j++)
479                e[i][j]=exc[j-pp];
480             tmp3=nsf;
481             if (tmp3>pp+pitch)
482                tmp3=pp+pitch;
483             for (j=tmp1;j<tmp3;j++)
484                e[i][j]=exc[j-pp-pitch];
485             for (j=tmp3;j<nsf;j++)
486                e[i][j]=0;
487          }
488 #endif
489       }
490       for (i=0;i<nsf;i++)
491            exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
492    }
493 }
494
495
496 /** Forced pitch delay and gain */
497 int forced_pitch_quant(
498 spx_sig_t target[],                 /* Target vector */
499 spx_sig_t *sw,
500 spx_coef_t ak[],                     /* LPCs for this subframe */
501 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
502 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
503 spx_sig_t exc[],                    /* Excitation */
504 void *par,
505 int   start,                    /* Smallest pitch value allowed */
506 int   end,                      /* Largest pitch value allowed */
507 float pitch_coef,               /* Voicing (pitch) coefficient */
508 int   p,                        /* Number of LPC coeffs */
509 int   nsf,                      /* Number of samples in subframe */
510 SpeexBits *bits,
511 char *stack,
512 spx_sig_t *exc2,
513 spx_sig_t *r,
514 int complexity,
515 int cdbk_offset
516 )
517 {
518    int i;
519    if (pitch_coef>.99)
520       pitch_coef=.99;
521    for (i=0;i<nsf;i++)
522    {
523       exc[i]=exc[i-start]*pitch_coef;
524    }
525    return start;
526 }
527
528 /** Unquantize forced pitch delay and gain */
529 void forced_pitch_unquant(
530 spx_sig_t exc[],                    /* Excitation */
531 int   start,                    /* Smallest pitch value allowed */
532 int   end,                      /* Largest pitch value allowed */
533 float pitch_coef,               /* Voicing (pitch) coefficient */
534 void *par,
535 int   nsf,                      /* Number of samples in subframe */
536 int *pitch_val,
537 float *gain_val,
538 SpeexBits *bits,
539 char *stack,
540 int count_lost,
541 int subframe_offset,
542 float last_pitch_gain,
543 int cdbk_offset
544 )
545 {
546    int i;
547    /*pitch_coef=.9;*/
548    if (pitch_coef>.99)
549       pitch_coef=.99;
550    for (i=0;i<nsf;i++)
551    {
552       exc[i]=exc[i-start]*pitch_coef;
553    }
554    *pitch_val = start;
555    gain_val[0]=gain_val[2]=0;
556    gain_val[1] = pitch_coef;
557 }