fixed-point: conversion of the open-loop pitch analysis
[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 #include <stdio.h>
40
41 #ifdef FIXED_POINT
42
43 static spx_word32_t inner_prod2(spx_word16_t *x, spx_word16_t *y, int len)
44 {
45    int i;
46    spx_word32_t sum=0;
47    for (i=0;i<len;i+=4)
48    {
49       spx_word32_t part=0;
50       part += MULT16_16(x[i],y[i]);
51       part += MULT16_16(x[i+1],y[i+1]);
52       part += MULT16_16(x[i+2],y[i+2]);
53       part += MULT16_16(x[i+3],y[i+3]);
54       sum += SHR(part,6);
55    }
56    return sum;
57 }
58
59
60 static float inner_prod(spx_sig_t *x, spx_sig_t *y, int len)
61 {
62    int i;
63    float sum1=0,sum2=0,sum3=0,sum4=0;
64    for (i=0;i<len;)
65    {
66       sum1 += x[i]*y[i];
67       sum2 += x[i+1]*y[i+1];
68       sum3 += x[i+2]*y[i+2];
69       sum4 += x[i+3]*y[i+3];
70       i+=4;
71    }
72    return sum1+sum2+sum3+sum4;
73 }
74
75 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, float *gain, int N, char *stack)
76 {
77    int i,j,k;
78    /*float corr=0;*/
79    /*float energy;*/
80    float *best_score;
81    float e0;
82    spx_word32_t *corr, *energy;
83    float *score;
84
85    spx_word16_t *swn;
86    spx_sig_t max_sw=1;
87    int sw_shift=0;
88
89    best_score = PUSH(stack,N, float);
90    corr = PUSH(stack,end-start+1, spx_word32_t);
91    energy = PUSH(stack,end-start+2, spx_word32_t);
92    score = PUSH(stack,end-start+1, float);
93
94    swn = PUSH(stack, end+len, spx_word16_t);
95    for (i=-end;i<len;i++)
96    {
97       spx_sig_t tmp = sw[i];
98       if (tmp<0)
99          tmp = -tmp;
100       if (tmp > max_sw)
101          max_sw = tmp;
102    }
103    while (max_sw>16384)
104    {
105       sw_shift++;
106       max_sw>>=1;
107    }
108    for (i=0;i<end+len;i++)
109       swn[i] = SHR(sw[i-end],sw_shift);
110    
111    swn += end;
112
113
114    for (i=0;i<N;i++)
115    {
116         best_score[i]=-1;
117         gain[i]=0;
118    }
119
120
121    energy[0]=inner_prod2(swn-start, swn-start, len);
122    e0=inner_prod2(swn, swn, len);
123    for (i=start;i<=end;i++)
124    {
125       /* Update energy for next pitch*/
126       energy[i-start+1] = energy[i-start] + SHR(MULT16_16(swn[-i-1],swn[-i-1]) - MULT16_16(swn[-i+len-1],swn[-i+len-1]),6);
127    }
128    for (i=start;i<=end;i++)
129    {
130       corr[i-start]=0;
131       score[i-start]=0;
132    }
133
134    for (i=start;i<=end;i++)
135    {
136       /* Compute correlation*/
137       corr[i-start]=inner_prod2(swn, swn-i, len);
138       score[i-start]=1.*corr[i-start]*corr[i-start]/(energy[i-start]+1.);
139    }
140    for (i=start;i<=end;i++)
141    {
142       if (score[i-start]>best_score[N-1])
143       {
144          float g1, g;
145          g1 = corr[i-start]/(energy[i-start]+10.);
146          g = sqrt(g1*corr[i-start]/(e0+10.));
147          if (g>g1)
148             g=g1;
149          if (g<0)
150             g=0;
151          for (j=0;j<N;j++)
152          {
153             if (score[i-start] > best_score[j])
154             {
155                for (k=N-1;k>j;k--)
156                {
157                   best_score[k]=best_score[k-1];
158                   pitch[k]=pitch[k-1];
159                   gain[k] = gain[k-1];
160                }
161                best_score[j]=score[i-start];
162                pitch[j]=i;
163                gain[j]=g;
164                break;
165             }
166          }
167       }
168    }
169
170 }
171
172 #else
173
174
175 #ifdef _USE_SSE
176 #include "ltp_sse.h"
177 #else
178 static float inner_prod(spx_sig_t *x, spx_sig_t *y, int len)
179 {
180    int i;
181    float sum1=0,sum2=0,sum3=0,sum4=0;
182    for (i=0;i<len;)
183    {
184       sum1 += x[i]*y[i];
185       sum2 += x[i+1]*y[i+1];
186       sum3 += x[i+2]*y[i+2];
187       sum4 += x[i+3]*y[i+3];
188       i+=4;
189    }
190    return sum1+sum2+sum3+sum4;
191 }
192 #endif
193
194 /*Original, non-optimized version*/
195 /*static float inner_prod(float *x, float *y, int len)
196 {
197    int i;
198    float sum=0;
199    for (i=0;i<len;i++)
200       sum += x[i]*y[i];
201    return sum;
202 }
203 */
204
205
206 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, float *gain, int N, char *stack)
207 {
208    int i,j,k;
209    /*float corr=0;*/
210    /*float energy;*/
211    float *best_score;
212    float e0;
213    float *corr, *energy, *score;
214
215    best_score = PUSH(stack,N, float);
216    corr = PUSH(stack,end-start+1, float);
217    energy = PUSH(stack,end-start+2, float);
218    score = PUSH(stack,end-start+1, float);
219    for (i=0;i<N;i++)
220    {
221         best_score[i]=-1;
222         gain[i]=0;
223    }
224    energy[0]=inner_prod(sw-start, sw-start, len);
225    e0=inner_prod(sw, sw, len);
226    for (i=start;i<=end;i++)
227    {
228       /* Update energy for next pitch*/
229       energy[i-start+1] = energy[i-start] + sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
230    }
231    for (i=start;i<=end;i++)
232    {
233       corr[i-start]=0;
234       score[i-start]=0;
235    }
236
237    for (i=start;i<=end;i++)
238    {
239       /* Compute correlation*/
240       corr[i-start]=inner_prod(sw, sw-i, len);
241       score[i-start]=corr[i-start]*corr[i-start]/(energy[i-start]+1);
242    }
243    for (i=start;i<=end;i++)
244    {
245       if (score[i-start]>best_score[N-1])
246       {
247          float g1, g;
248          g1 = corr[i-start]/(energy[i-start]+10);
249          g = sqrt(g1*corr[i-start]/(e0+10));
250          if (g>g1)
251             g=g1;
252          if (g<0)
253             g=0;
254          for (j=0;j<N;j++)
255          {
256             if (score[i-start] > best_score[j])
257             {
258                for (k=N-1;k>j;k--)
259                {
260                   best_score[k]=best_score[k-1];
261                   pitch[k]=pitch[k-1];
262                   gain[k] = gain[k-1];
263                }
264                best_score[j]=score[i-start];
265                pitch[j]=i;
266                gain[j]=g;
267                break;
268             }
269          }
270       }
271    }
272
273 }
274 #endif
275
276
277
278 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
279 static float pitch_gain_search_3tap(
280 spx_sig_t target[],                 /* Target vector */
281 spx_coef_t ak[],                     /* LPCs for this subframe */
282 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
283 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
284 spx_sig_t exc[],                    /* Excitation */
285 void *par,
286 int   pitch,                    /* Pitch value */
287 int   p,                        /* Number of LPC coeffs */
288 int   nsf,                      /* Number of samples in subframe */
289 SpeexBits *bits,
290 char *stack,
291 spx_sig_t *exc2,
292 spx_sig_t *r,
293 int  *cdbk_index,
294 int cdbk_offset
295 )
296 {
297    int i,j;
298    spx_sig_t *tmp, *tmp2;
299    spx_sig_t *x[3];
300    spx_sig_t *e[3];
301    float corr[3];
302    float A[3][3];
303    float gain[3];
304    int   gain_cdbk_size;
305    signed char *gain_cdbk;
306    float err1,err2;
307
308    ltp_params *params;
309    params = (ltp_params*) par;
310    gain_cdbk_size = 1<<params->gain_bits;
311    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
312    tmp = PUSH(stack, 3*nsf, spx_sig_t);
313    tmp2 = PUSH(stack, 3*nsf, spx_sig_t);
314
315    x[0]=tmp;
316    x[1]=tmp+nsf;
317    x[2]=tmp+2*nsf;
318
319    e[0]=tmp2;
320    e[1]=tmp2+nsf;
321    e[2]=tmp2+2*nsf;
322    
323    for (i=2;i>=0;i--)
324    {
325       int pp=pitch+1-i;
326       for (j=0;j<nsf;j++)
327       {
328          if (j-pp<0)
329             e[i][j]=exc2[j-pp];
330          else if (j-pp-pitch<0)
331             e[i][j]=exc2[j-pp-pitch];
332          else
333             e[i][j]=0;
334       }
335
336       if (i==2)
337          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
338       else {
339          for (j=0;j<nsf-1;j++)
340             x[i][j+1]=x[i+1][j];
341          x[i][0]=0;
342          for (j=0;j<nsf;j++)
343             x[i][j]+=e[i][0]*r[j]/SIG_SCALING;
344       }
345    }
346
347    for (i=0;i<3;i++)
348       corr[i]=inner_prod(x[i],target,nsf);
349    
350    for (i=0;i<3;i++)
351       for (j=0;j<=i;j++)
352          A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
353    
354    {
355       float C[9];
356       signed char *ptr=gain_cdbk;
357       int best_cdbk=0;
358       float best_sum=0;
359       C[0]=corr[2];
360       C[1]=corr[1];
361       C[2]=corr[0];
362       C[3]=A[1][2];
363       C[4]=A[0][1];
364       C[5]=A[0][2];
365       C[6]=A[2][2];
366       C[7]=A[1][1];
367       C[8]=A[0][0];
368       
369       for (i=0;i<gain_cdbk_size;i++)
370       {
371          float sum=0;
372          float g0,g1,g2;
373          ptr = gain_cdbk+3*i;
374          g0=0.015625*ptr[0]+.5;
375          g1=0.015625*ptr[1]+.5;
376          g2=0.015625*ptr[2]+.5;
377
378          sum += C[0]*g0;
379          sum += C[1]*g1;
380          sum += C[2]*g2;
381          sum -= C[3]*g0*g1;
382          sum -= C[4]*g2*g1;
383          sum -= C[5]*g2*g0;
384          sum -= .5*C[6]*g0*g0;
385          sum -= .5*C[7]*g1*g1;
386          sum -= .5*C[8]*g2*g2;
387
388          /* If 1, force "safe" pitch values to handle packet loss better */
389          if (0) {
390             float tot = fabs(ptr[1]);
391             if (ptr[0]>0)
392                tot+=ptr[0];
393             if (ptr[2]>0)
394                tot+=ptr[2];
395             if (tot>1)
396                continue;
397          }
398
399          if (sum>best_sum || i==0)
400          {
401             best_sum=sum;
402             best_cdbk=i;
403          }
404       }
405       gain[0] = 0.015625*gain_cdbk[best_cdbk*3]  + .5;
406       gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
407       gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
408
409       *cdbk_index=best_cdbk;
410    }
411    
412    for (i=0;i<nsf;i++)
413       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
414    
415    err1=0;
416    err2=0;
417    for (i=0;i<nsf;i++)
418       err1+=target[i]*target[i];
419    for (i=0;i<nsf;i++)
420       err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
421       * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
422
423    return err2;
424 }
425
426
427 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
428 int pitch_search_3tap(
429 spx_sig_t target[],                 /* Target vector */
430 spx_sig_t *sw,
431 spx_coef_t ak[],                     /* LPCs for this subframe */
432 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
433 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
434 spx_sig_t exc[],                    /* Excitation */
435 void *par,
436 int   start,                    /* Smallest pitch value allowed */
437 int   end,                      /* Largest pitch value allowed */
438 float pitch_coef,               /* Voicing (pitch) coefficient */
439 int   p,                        /* Number of LPC coeffs */
440 int   nsf,                      /* Number of samples in subframe */
441 SpeexBits *bits,
442 char *stack,
443 spx_sig_t *exc2,
444 spx_sig_t *r,
445 int complexity,
446 int cdbk_offset
447 )
448 {
449    int i,j;
450    int cdbk_index, pitch=0, best_gain_index=0;
451    float *best_exc;
452    int best_pitch=0;
453    float err, best_err=-1;
454    int N;
455    ltp_params *params;
456    int *nbest;
457    float *gains;
458
459    N=complexity;
460    if (N>10)
461       N=10;
462
463    nbest=PUSH(stack, N, int);
464    gains = PUSH(stack, N, float);
465    params = (ltp_params*) par;
466
467    if (N==0 || end<start)
468    {
469       speex_bits_pack(bits, 0, params->pitch_bits);
470       speex_bits_pack(bits, 0, params->gain_bits);
471       for (i=0;i<nsf;i++)
472          exc[i]=0;
473       return start;
474    }
475    
476    best_exc=PUSH(stack,nsf, float);
477    
478    if (N>end-start+1)
479       N=end-start+1;
480    open_loop_nbest_pitch(sw, start, end, nsf, nbest, gains, N, stack);
481    for (i=0;i<N;i++)
482    {
483       pitch=nbest[i];
484       for (j=0;j<nsf;j++)
485          exc[j]=0;
486       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
487                                  bits, stack, exc2, r, &cdbk_index, cdbk_offset);
488       if (err<best_err || best_err<0)
489       {
490          for (j=0;j<nsf;j++)
491             best_exc[j]=exc[j];
492          best_err=err;
493          best_pitch=pitch;
494          best_gain_index=cdbk_index;
495       }
496    }
497    
498    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
499    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
500    speex_bits_pack(bits, best_gain_index, params->gain_bits);
501    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
502    for (i=0;i<nsf;i++)
503       exc[i]=best_exc[i];
504
505    return pitch;
506 }
507
508 void pitch_unquant_3tap(
509 spx_sig_t exc[],                    /* Excitation */
510 int   start,                    /* Smallest pitch value allowed */
511 int   end,                      /* Largest pitch value allowed */
512 float pitch_coef,               /* Voicing (pitch) coefficient */
513 void *par,
514 int   nsf,                      /* Number of samples in subframe */
515 int *pitch_val,
516 float *gain_val,
517 SpeexBits *bits,
518 char *stack,
519 int count_lost,
520 int subframe_offset,
521 float last_pitch_gain,
522 int cdbk_offset
523 )
524 {
525    int i;
526    int pitch;
527    int gain_index;
528    float gain[3];
529    signed char *gain_cdbk;
530    int gain_cdbk_size;
531    ltp_params *params;
532    params = (ltp_params*) par;
533    gain_cdbk_size = 1<<params->gain_bits;
534    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
535
536    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
537    pitch += start;
538    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
539    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
540    gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5;
541    gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5;
542    gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5;
543
544    if (count_lost && pitch > subframe_offset)
545    {
546       float gain_sum;
547       if (1) {
548          float tmp = count_lost < 4 ? last_pitch_gain : 0.4 * last_pitch_gain;
549          if (tmp>.95)
550             tmp=.95;
551          gain_sum = fabs(gain[1]);
552          if (gain[0]>0)
553             gain_sum += gain[0];
554          else
555             gain_sum -= .5*gain[0];
556          if (gain[2]>0)
557             gain_sum += gain[2];
558          else
559             gain_sum -= .5*gain[2];
560          if (gain_sum > tmp) {
561             float fact = tmp/gain_sum;
562             for (i=0;i<3;i++)
563                gain[i]*=fact;
564
565          }
566
567       }
568
569       if (0) {
570       gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
571          if (gain_sum>.95) {
572          float fact = .95/gain_sum;
573          for (i=0;i<3;i++)
574             gain[i]*=fact;
575       }
576    }
577    }
578
579    *pitch_val = pitch;
580    /**gain_val = gain[0]+gain[1]+gain[2];*/
581    gain_val[0]=gain[0];
582    gain_val[1]=gain[1];
583    gain_val[2]=gain[2];
584
585    {
586       spx_sig_t *e[3];
587       spx_sig_t *tmp2;
588       tmp2=PUSH(stack, 3*nsf, spx_sig_t);
589       e[0]=tmp2;
590       e[1]=tmp2+nsf;
591       e[2]=tmp2+2*nsf;
592       
593       for (i=0;i<3;i++)
594       {
595          int j;
596          int pp=pitch+1-i;
597 #if 0
598          for (j=0;j<nsf;j++)
599          {
600             if (j-pp<0)
601                e[i][j]=exc[j-pp];
602             else if (j-pp-pitch<0)
603                e[i][j]=exc[j-pp-pitch];
604             else
605                e[i][j]=0;
606          }
607 #else
608          {
609             int tmp1, tmp3;
610             tmp1=nsf;
611             if (tmp1>pp)
612                tmp1=pp;
613             for (j=0;j<tmp1;j++)
614                e[i][j]=exc[j-pp];
615             tmp3=nsf;
616             if (tmp3>pp+pitch)
617                tmp3=pp+pitch;
618             for (j=tmp1;j<tmp3;j++)
619                e[i][j]=exc[j-pp-pitch];
620             for (j=tmp3;j<nsf;j++)
621                e[i][j]=0;
622          }
623 #endif
624       }
625       for (i=0;i<nsf;i++)
626            exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
627    }
628 }
629
630
631 /** Forced pitch delay and gain */
632 int forced_pitch_quant(
633 spx_sig_t target[],                 /* Target vector */
634 spx_sig_t *sw,
635 spx_coef_t ak[],                     /* LPCs for this subframe */
636 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
637 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
638 spx_sig_t exc[],                    /* Excitation */
639 void *par,
640 int   start,                    /* Smallest pitch value allowed */
641 int   end,                      /* Largest pitch value allowed */
642 float pitch_coef,               /* Voicing (pitch) coefficient */
643 int   p,                        /* Number of LPC coeffs */
644 int   nsf,                      /* Number of samples in subframe */
645 SpeexBits *bits,
646 char *stack,
647 spx_sig_t *exc2,
648 spx_sig_t *r,
649 int complexity,
650 int cdbk_offset
651 )
652 {
653    int i;
654    if (pitch_coef>.99)
655       pitch_coef=.99;
656    for (i=0;i<nsf;i++)
657    {
658       exc[i]=exc[i-start]*pitch_coef;
659    }
660    return start;
661 }
662
663 /** Unquantize forced pitch delay and gain */
664 void forced_pitch_unquant(
665 spx_sig_t exc[],                    /* Excitation */
666 int   start,                    /* Smallest pitch value allowed */
667 int   end,                      /* Largest pitch value allowed */
668 float pitch_coef,               /* Voicing (pitch) coefficient */
669 void *par,
670 int   nsf,                      /* Number of samples in subframe */
671 int *pitch_val,
672 float *gain_val,
673 SpeexBits *bits,
674 char *stack,
675 int count_lost,
676 int subframe_offset,
677 float last_pitch_gain,
678 int cdbk_offset
679 )
680 {
681    int i;
682    /*pitch_coef=.9;*/
683    if (pitch_coef>.99)
684       pitch_coef=.99;
685    for (i=0;i<nsf;i++)
686    {
687       exc[i]=exc[i-start]*pitch_coef;
688    }
689    *pitch_val = start;
690    gain_val[0]=gain_val[2]=0;
691    gain_val[1] = pitch_coef;
692 }