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