trying some ideas for soft-decision DTD based on residual-to-signal ratio
[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 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include <math.h>
38 #include "ltp.h"
39 #include "stack_alloc.h"
40 #include "filters.h"
41 #include <speex/speex_bits.h>
42 #include "math_approx.h"
43
44 #ifndef NULL
45 #define NULL 0
46 #endif
47
48
49 #ifdef _USE_SSE
50 #include "ltp_sse.h"
51 #elif defined (ARM4_ASM) || defined(ARM5E_ASM)
52 #include "ltp_arm4.h"
53 #else
54
55 static spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
56 {
57    spx_word32_t sum=0;
58    len >>= 2;
59    while(len--)
60    {
61       spx_word32_t part=0;
62       part = MAC16_16(part,*x++,*y++);
63       part = MAC16_16(part,*x++,*y++);
64       part = MAC16_16(part,*x++,*y++);
65       part = MAC16_16(part,*x++,*y++);
66       /* HINT: If you had a 40-bit accumulator, you could shift only at the end */
67       sum = ADD32(sum,SHR32(part,6));
68    }
69    return sum;
70 }
71
72 #if 0 /* HINT: Enable this for machines with enough registers (i.e. not x86) */
73 static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
74 {
75    int i,j;
76    for (i=0;i<nb_pitch;i+=4)
77    {
78       /* Compute correlation*/
79       //corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);
80       spx_word32_t sum1=0;
81       spx_word32_t sum2=0;
82       spx_word32_t sum3=0;
83       spx_word32_t sum4=0;
84       const spx_word16_t *y = _y+i;
85       const spx_word16_t *x = _x;
86       spx_word16_t y0, y1, y2, y3;
87       //y0=y[0];y1=y[1];y2=y[2];y3=y[3];
88       y0=*y++;
89       y1=*y++;
90       y2=*y++;
91       y3=*y++;
92       for (j=0;j<len;j+=4)
93       {
94          spx_word32_t part1;
95          spx_word32_t part2;
96          spx_word32_t part3;
97          spx_word32_t part4;
98          part1 = MULT16_16(*x,y0);
99          part2 = MULT16_16(*x,y1);
100          part3 = MULT16_16(*x,y2);
101          part4 = MULT16_16(*x,y3);
102          x++;
103          y0=*y++;
104          part1 = MAC16_16(part1,*x,y1);
105          part2 = MAC16_16(part2,*x,y2);
106          part3 = MAC16_16(part3,*x,y3);
107          part4 = MAC16_16(part4,*x,y0);
108          x++;
109          y1=*y++;
110          part1 = MAC16_16(part1,*x,y2);
111          part2 = MAC16_16(part2,*x,y3);
112          part3 = MAC16_16(part3,*x,y0);
113          part4 = MAC16_16(part4,*x,y1);
114          x++;
115          y2=*y++;
116          part1 = MAC16_16(part1,*x,y3);
117          part2 = MAC16_16(part2,*x,y0);
118          part3 = MAC16_16(part3,*x,y1);
119          part4 = MAC16_16(part4,*x,y2);
120          x++;
121          y3=*y++;
122          
123          sum1 = ADD32(sum1,SHR32(part1,6));
124          sum2 = ADD32(sum2,SHR32(part2,6));
125          sum3 = ADD32(sum3,SHR32(part3,6));
126          sum4 = ADD32(sum4,SHR32(part4,6));
127       }
128       corr[nb_pitch-1-i]=sum1;
129       corr[nb_pitch-2-i]=sum2;
130       corr[nb_pitch-3-i]=sum3;
131       corr[nb_pitch-4-i]=sum4;
132    }
133
134 }
135 #else
136 static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
137 {
138    int i;
139    for (i=0;i<nb_pitch;i++)
140    {
141       /* Compute correlation*/
142       corr[nb_pitch-1-i]=inner_prod(_x, _y+i, len);
143    }
144
145 }
146 #endif
147
148
149
150 #endif
151
152 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
153 {
154    int i,j,k;
155    VARDECL(spx_word32_t *best_score);
156    spx_word32_t e0;
157    VARDECL(spx_word32_t *corr);
158    VARDECL(spx_word32_t *energy);
159    VARDECL(spx_word32_t *score);
160    VARDECL(spx_word16_t *swn2);
161    spx_word16_t *swn;
162
163    ALLOC(best_score, N, spx_word32_t);
164    ALLOC(corr, end-start+1, spx_word32_t);
165    ALLOC(energy, end-start+2, spx_word32_t);
166    ALLOC(score, end-start+1, spx_word32_t);
167
168 #ifdef FIXED_POINT
169    ALLOC(swn2, end+len, spx_word16_t);
170    normalize16(sw-end, swn2, 16384, end+len);
171    swn = swn2 + end;
172 #else
173    swn = sw;
174 #endif
175
176    for (i=0;i<N;i++)
177    {
178         best_score[i]=-1;
179         pitch[i]=start;
180    }
181
182
183    energy[0]=inner_prod(swn-start, swn-start, len);
184    e0=inner_prod(swn, swn, len);
185    for (i=start;i<=end;i++)
186    {
187       /* Update energy for next pitch*/
188       energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(swn[-i-1],swn[-i-1]),6)), SHR32(MULT16_16(swn[-i+len-1],swn[-i+len-1]),6));
189    }
190
191    pitch_xcorr(swn, swn-end, corr, len, end-start+1, stack);
192
193 #ifdef FIXED_POINT
194    {
195       VARDECL(spx_word16_t *corr16);
196       VARDECL(spx_word16_t *ener16);
197       ALLOC(corr16, end-start+1, spx_word16_t);
198       ALLOC(ener16, end-start+1, spx_word16_t);
199       normalize16(corr, corr16, 16384, end-start+1);
200       normalize16(energy, ener16, 16384, end-start+1);
201
202       for (i=start;i<=end;i++)
203       {
204          spx_word16_t g;
205          spx_word32_t tmp;
206          tmp = corr16[i-start];
207          if (tmp>0)
208          {
209             if (SHR16(corr16[i-start],4)>ener16[i-start])
210                tmp = SHL32(EXTEND32(ener16[i-start]),14);
211             else if (-SHR16(corr16[i-start],4)>ener16[i-start])
212                tmp = -SHL32(EXTEND32(ener16[i-start]),14);
213             else
214                tmp = SHL32(tmp,10);
215             g = DIV32_16(tmp, 8+ener16[i-start]);
216             score[i-start] = MULT16_16(corr16[i-start],g);
217          } else
218          {
219             score[i-start] = 1;
220          }
221       }
222    }
223 #else
224    for (i=start;i<=end;i++)
225    {
226       float g = corr[i-start]/(1+energy[i-start]);
227       if (g>16)
228          g = 16;
229       else if (g<-16)
230          g = -16;
231       score[i-start] = g*corr[i-start];
232    }
233 #endif
234
235    /* Extract best scores */
236    for (i=start;i<=end;i++)
237    {
238       if (score[i-start]>best_score[N-1])
239       {
240          for (j=0;j<N;j++)
241          {
242             if (score[i-start] > best_score[j])
243             {
244                for (k=N-1;k>j;k--)
245                {
246                   best_score[k]=best_score[k-1];
247                   pitch[k]=pitch[k-1];
248                }
249                best_score[j]=score[i-start];
250                pitch[j]=i;
251                break;
252             }
253          }
254       }
255    }
256
257    /* Compute open-loop gain */
258    if (gain)
259    {
260        for (j=0;j<N;j++)
261        {
262           spx_word16_t g;
263           i=pitch[j];
264           g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
265           /* FIXME: g = max(g,corr/energy) */
266                    if (g<0)
267                    g = 0;
268              gain[j]=g;
269        }
270    }
271 }
272
273
274 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
275 static spx_word64_t pitch_gain_search_3tap(
276 const spx_sig_t target[],       /* Target vector */
277 const spx_coef_t ak[],          /* LPCs for this subframe */
278 const spx_coef_t awk1[],        /* Weighted LPCs #1 for this subframe */
279 const spx_coef_t awk2[],        /* Weighted LPCs #2 for this subframe */
280 spx_sig_t exc[],                /* Excitation */
281 const void *par,
282 int   pitch,                    /* Pitch value */
283 int   p,                        /* Number of LPC coeffs */
284 int   nsf,                      /* Number of samples in subframe */
285 SpeexBits *bits,
286 char *stack,
287 const spx_sig_t *exc2,
288 const spx_word16_t *r,
289 spx_sig_t *new_target,
290 int  *cdbk_index,
291 int cdbk_offset,
292 int plc_tuning
293 )
294 {
295    int i,j;
296    VARDECL(spx_sig_t *tmp1);
297    VARDECL(spx_sig_t *tmp2);
298    spx_sig_t *x[3];
299    spx_sig_t *e[3];
300    spx_word32_t corr[3];
301    spx_word32_t A[3][3];
302    int   gain_cdbk_size;
303    const signed char *gain_cdbk;
304    spx_word16_t gain[3];
305    spx_word64_t err;
306
307    const ltp_params *params;
308    params = (const ltp_params*) par;
309    gain_cdbk_size = 1<<params->gain_bits;
310    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
311    ALLOC(tmp1, 3*nsf, spx_sig_t);
312    ALLOC(tmp2, 3*nsf, spx_sig_t);
313
314    x[0]=tmp1;
315    x[1]=tmp1+nsf;
316    x[2]=tmp1+2*nsf;
317    
318    e[0]=tmp2;
319    e[1]=tmp2+nsf;
320    e[2]=tmp2+2*nsf;
321    for (i=2;i>=0;i--)
322    {
323       int pp=pitch+1-i;
324       for (j=0;j<nsf;j++)
325       {
326          if (j-pp<0)
327             e[i][j]=exc2[j-pp];
328          else if (j-pp-pitch<0)
329             e[i][j]=exc2[j-pp-pitch];
330          else
331             e[i][j]=0;
332       }
333
334       if (i==2)
335          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
336       else {
337          for (j=0;j<nsf-1;j++)
338             x[i][j+1]=x[i+1][j];
339          x[i][0]=0;
340          for (j=0;j<nsf;j++)
341          {
342             x[i][j]=ADD32(x[i][j],SHL32(MULT16_32_Q15(r[j], e[i][0]),1));
343          }
344       }
345    }
346
347 #ifdef FIXED_POINT
348    {
349       /* If using fixed-point, we need to normalize the signals first */
350       spx_word16_t *y[3];
351       VARDECL(spx_word16_t *ytmp);
352       VARDECL(spx_word16_t *t);
353
354       spx_sig_t max_val=1;
355       int sig_shift;
356       
357       ALLOC(ytmp, 3*nsf, spx_word16_t);
358 #if 0
359       ALLOC(y[0], nsf, spx_word16_t);
360       ALLOC(y[1], nsf, spx_word16_t);
361       ALLOC(y[2], nsf, spx_word16_t);
362 #else
363       y[0] = ytmp;
364       y[1] = ytmp+nsf;
365       y[2] = ytmp+2*nsf;
366 #endif
367       ALLOC(t, nsf, spx_word16_t);
368       for (j=0;j<3;j++)
369       {
370          for (i=0;i<nsf;i++)
371          {
372             spx_sig_t tmp = x[j][i];
373             if (tmp<0)
374                tmp = -tmp;
375             if (tmp > max_val)
376                max_val = tmp;
377          }
378       }
379       for (i=0;i<nsf;i++)
380       {
381          spx_sig_t tmp = target[i];
382          if (tmp<0)
383             tmp = -tmp;
384          if (tmp > max_val)
385             max_val = tmp;
386       }
387
388       sig_shift=0;
389       while (max_val>16384)
390       {
391          sig_shift++;
392          max_val >>= 1;
393       }
394
395       for (j=0;j<3;j++)
396       {
397          for (i=0;i<nsf;i++)
398          {
399             y[j][i] = EXTRACT16(SHR32(x[j][i],sig_shift));
400          }
401       }
402       for (i=0;i<nsf;i++)
403       {
404          t[i] = EXTRACT16(SHR32(target[i],sig_shift));
405       }
406
407       for (i=0;i<3;i++)
408          corr[i]=inner_prod(y[i],t,nsf);
409       
410       for (i=0;i<3;i++)
411          for (j=0;j<=i;j++)
412             A[i][j]=A[j][i]=inner_prod(y[i],y[j],nsf);
413    }
414 #else
415    {
416       for (i=0;i<3;i++)
417          corr[i]=inner_prod(x[i],target,nsf);
418       
419       for (i=0;i<3;i++)
420          for (j=0;j<=i;j++)
421             A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
422    }
423 #endif
424
425    {
426       spx_word32_t C[9];
427       const signed char *ptr=gain_cdbk;
428       int best_cdbk=0;
429       spx_word32_t best_sum=0;
430       C[0]=corr[2];
431       C[1]=corr[1];
432       C[2]=corr[0];
433       C[3]=A[1][2];
434       C[4]=A[0][1];
435       C[5]=A[0][2];      
436       C[6]=A[2][2];
437       C[7]=A[1][1];
438       C[8]=A[0][0];
439       
440       /*plc_tuning *= 2;*/
441       if (plc_tuning<2)
442          plc_tuning=2;
443 #ifdef FIXED_POINT
444       C[0] = MAC16_32_Q15(C[0],MULT16_16_16(plc_tuning,-327),C[0]);
445       C[1] = MAC16_32_Q15(C[1],MULT16_16_16(plc_tuning,-327),C[1]);
446       C[2] = MAC16_32_Q15(C[2],MULT16_16_16(plc_tuning,-327),C[2]);
447 #else
448       C[0]*=1-.01*plc_tuning;
449       C[1]*=1-.01*plc_tuning;
450       C[2]*=1-.01*plc_tuning;
451       C[6]*=.5*(1+.01*plc_tuning);
452       C[7]*=.5*(1+.01*plc_tuning);
453       C[8]*=.5*(1+.01*plc_tuning);
454 #endif
455       for (i=0;i<gain_cdbk_size;i++)
456       {
457          spx_word32_t sum=0;
458          spx_word16_t g0,g1,g2;
459          spx_word16_t pitch_control=64;
460          spx_word16_t gain_sum;
461          
462          ptr = gain_cdbk+3*i;
463          g0=ADD16((spx_word16_t)ptr[0],32);
464          g1=ADD16((spx_word16_t)ptr[1],32);
465          g2=ADD16((spx_word16_t)ptr[2],32);
466
467          gain_sum = g1;
468          if (g0>0)
469             gain_sum += g0;
470          if (g2>0)
471             gain_sum += g2;
472          if (gain_sum > 64)
473          {
474             gain_sum = SUB16(gain_sum, 64);
475             if (gain_sum > 127)
476                gain_sum = 127;
477 #ifdef FIXED_POINT
478             pitch_control =  SUB16(64,EXTRACT16(PSHR32(MULT16_16(64,MULT16_16_16(plc_tuning, gain_sum)),10)));
479 #else
480             pitch_control = 64*(1.-.001*plc_tuning*gain_sum);
481 #endif
482             if (pitch_control < 0)
483                pitch_control = 0;
484          }
485          
486          sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g0,pitch_control),C[0]));
487          sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g1,pitch_control),C[1]));
488          sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g2,pitch_control),C[2]));
489          sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g0,g1),C[3]));
490          sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g1),C[4]));
491          sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g0),C[5]));
492          sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g0,g0),C[6]));
493          sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g1,g1),C[7]));
494          sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g2,g2),C[8]));
495          /* We could force "safe" pitch values to handle packet loss better */
496
497          if (sum>best_sum || i==0)
498          {
499             best_sum=sum;
500             best_cdbk=i;
501          }
502       }
503 #ifdef FIXED_POINT
504       gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3]);
505       gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+1]);
506       gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+2]);
507       /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
508 #else
509       gain[0] = 0.015625*gain_cdbk[best_cdbk*3]  + .5;
510       gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
511       gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
512 #endif
513       *cdbk_index=best_cdbk;
514    }
515
516 #ifdef FIXED_POINT
517    for (i=0;i<nsf;i++)
518      exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])),
519                         MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2);
520    
521    err=0;
522    for (i=0;i<nsf;i++)
523    {
524       spx_word16_t perr2;
525       spx_sig_t tmp = SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),x[2][i]),MULT16_32_Q15(SHL16(gain[1],7),x[1][i])),
526                                   MULT16_32_Q15(SHL16(gain[2],7),x[0][i])),2);
527       spx_sig_t perr=SUB32(target[i],tmp);
528       new_target[i] = SUB32(target[i], tmp);
529       perr2 = EXTRACT16(PSHR32(perr,15));
530       err = ADD64(err,MULT16_16(perr2,perr2));
531       
532    }
533 #else
534    for (i=0;i<nsf;i++)
535       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
536    
537    err=0;
538    for (i=0;i<nsf;i++)
539    {
540       spx_sig_t tmp = gain[2]*x[0][i]+gain[1]*x[1][i]+gain[0]*x[2][i];
541       new_target[i] = target[i] - tmp;
542       err+=new_target[i]*new_target[i];
543    }
544 #endif
545
546    return err;
547 }
548
549
550 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
551 int pitch_search_3tap(
552 spx_sig_t target[],                 /* Target vector */
553 spx_sig_t *sw,
554 spx_coef_t ak[],                     /* LPCs for this subframe */
555 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
556 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
557 spx_sig_t exc[],                    /* Excitation */
558 const void *par,
559 int   start,                    /* Smallest pitch value allowed */
560 int   end,                      /* Largest pitch value allowed */
561 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
562 int   p,                        /* Number of LPC coeffs */
563 int   nsf,                      /* Number of samples in subframe */
564 SpeexBits *bits,
565 char *stack,
566 spx_sig_t *exc2,
567 spx_word16_t *r,
568 int complexity,
569 int cdbk_offset,
570 int plc_tuning
571 )
572 {
573    int i,j;
574    int cdbk_index, pitch=0, best_gain_index=0;
575    VARDECL(spx_sig_t *best_exc);
576    VARDECL(spx_sig_t *new_target);
577    VARDECL(spx_sig_t *best_target);
578    int best_pitch=0;
579    spx_word64_t err, best_err=-1;
580    int N;
581    const ltp_params *params;
582    VARDECL(int *nbest);
583
584    N=complexity;
585    if (N>10)
586       N=10;
587    if (N<1)
588       N=1;
589
590    ALLOC(nbest, N, int);
591    params = (const ltp_params*) par;
592
593    if (end<start)
594    {
595       speex_bits_pack(bits, 0, params->pitch_bits);
596       speex_bits_pack(bits, 0, params->gain_bits);
597       for (i=0;i<nsf;i++)
598          exc[i]=0;
599       return start;
600    }
601    
602    ALLOC(best_exc, nsf, spx_sig_t);
603    ALLOC(new_target, nsf, spx_sig_t);
604    ALLOC(best_target, nsf, spx_sig_t);
605    
606    if (N>end-start+1)
607       N=end-start+1;
608    open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack);
609    for (i=0;i<N;i++)
610    {
611       pitch=nbest[i];
612       for (j=0;j<nsf;j++)
613          exc[j]=0;
614       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
615                                  bits, stack, exc2, r, new_target, &cdbk_index, cdbk_offset, plc_tuning);
616       if (err<best_err || best_err<0)
617       {
618          for (j=0;j<nsf;j++)
619             best_exc[j]=exc[j];
620          for (j=0;j<nsf;j++)
621             best_target[j]=new_target[j];
622          best_err=err;
623          best_pitch=pitch;
624          best_gain_index=cdbk_index;
625       }
626    }
627    
628    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
629    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
630    speex_bits_pack(bits, best_gain_index, params->gain_bits);
631    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
632    for (i=0;i<nsf;i++)
633       exc[i]=best_exc[i];
634    for (i=0;i<nsf;i++)
635       target[i]=best_target[i];
636
637    return pitch;
638 }
639
640 void pitch_unquant_3tap(
641 spx_sig_t exc[],                    /* Excitation */
642 int   start,                    /* Smallest pitch value allowed */
643 int   end,                      /* Largest pitch value allowed */
644 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
645 const void *par,
646 int   nsf,                      /* Number of samples in subframe */
647 int *pitch_val,
648 spx_word16_t *gain_val,
649 SpeexBits *bits,
650 char *stack,
651 int count_lost,
652 int subframe_offset,
653 spx_word16_t last_pitch_gain,
654 int cdbk_offset
655 )
656 {
657    int i;
658    int pitch;
659    int gain_index;
660    spx_word16_t gain[3];
661    const signed char *gain_cdbk;
662    int gain_cdbk_size;
663    const ltp_params *params;
664
665    params = (const ltp_params*) par;
666    gain_cdbk_size = 1<<params->gain_bits;
667    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
668
669    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
670    pitch += start;
671    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
672    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
673 #ifdef FIXED_POINT
674    gain[0] = 32+(spx_word16_t)gain_cdbk[gain_index*3];
675    gain[1] = 32+(spx_word16_t)gain_cdbk[gain_index*3+1];
676    gain[2] = 32+(spx_word16_t)gain_cdbk[gain_index*3+2];
677 #else
678    gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5;
679    gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5;
680    gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5;
681 #endif
682
683    if (count_lost && pitch > subframe_offset)
684    {
685       float gain_sum;
686       if (1) {
687          float tmp = count_lost < 4 ? GAIN_SCALING_1*last_pitch_gain : 0.4 * GAIN_SCALING_1 * last_pitch_gain;
688          if (tmp>.95)
689             tmp=.95;
690          gain_sum = GAIN_SCALING_1*gain_3tap_to_1tap(gain);
691
692          if (gain_sum > tmp) {
693             float fact = tmp/gain_sum;
694             for (i=0;i<3;i++)
695                gain[i]*=fact;
696
697          }
698
699       }
700
701    }
702
703    *pitch_val = pitch;
704    gain_val[0]=gain[0];
705    gain_val[1]=gain[1];
706    gain_val[2]=gain[2];
707
708    {
709       spx_sig_t *e[3];
710       VARDECL(spx_sig_t *tmp2);
711       ALLOC(tmp2, 3*nsf, spx_sig_t);
712       e[0]=tmp2;
713       e[1]=tmp2+nsf;
714       e[2]=tmp2+2*nsf;
715       
716       for (i=0;i<3;i++)
717       {
718          int j;
719          int pp=pitch+1-i;
720 #if 0
721          for (j=0;j<nsf;j++)
722          {
723             if (j-pp<0)
724                e[i][j]=exc[j-pp];
725             else if (j-pp-pitch<0)
726                e[i][j]=exc[j-pp-pitch];
727             else
728                e[i][j]=0;
729          }
730 #else
731          {
732             int tmp1, tmp3;
733             tmp1=nsf;
734             if (tmp1>pp)
735                tmp1=pp;
736             for (j=0;j<tmp1;j++)
737                e[i][j]=exc[j-pp];
738             tmp3=nsf;
739             if (tmp3>pp+pitch)
740                tmp3=pp+pitch;
741             for (j=tmp1;j<tmp3;j++)
742                e[i][j]=exc[j-pp-pitch];
743             for (j=tmp3;j<nsf;j++)
744                e[i][j]=0;
745          }
746 #endif
747       }
748
749 #ifdef FIXED_POINT
750       {
751          for (i=0;i<nsf;i++)
752             exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])),
753                                MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2);
754       }
755 #else
756       for (i=0;i<nsf;i++)
757          exc[i]=VERY_SMALL+gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
758 #endif
759    }
760 }
761
762
763 /** Forced pitch delay and gain */
764 int forced_pitch_quant(
765 spx_sig_t target[],                 /* Target vector */
766 spx_sig_t *sw,
767 spx_coef_t ak[],                     /* LPCs for this subframe */
768 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
769 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
770 spx_sig_t exc[],                    /* Excitation */
771 const void *par,
772 int   start,                    /* Smallest pitch value allowed */
773 int   end,                      /* Largest pitch value allowed */
774 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
775 int   p,                        /* Number of LPC coeffs */
776 int   nsf,                      /* Number of samples in subframe */
777 SpeexBits *bits,
778 char *stack,
779 spx_sig_t *exc2,
780 spx_word16_t *r,
781 int complexity,
782 int cdbk_offset,
783 int plc_tuning
784 )
785 {
786    int i;
787    float coef = GAIN_SCALING_1*pitch_coef;
788    if (coef>.99)
789       coef=.99;
790    for (i=0;i<nsf;i++)
791    {
792       exc[i]=exc[i-start]*coef;
793    }
794    return start;
795 }
796
797 /** Unquantize forced pitch delay and gain */
798 void forced_pitch_unquant(
799 spx_sig_t exc[],                    /* Excitation */
800 int   start,                    /* Smallest pitch value allowed */
801 int   end,                      /* Largest pitch value allowed */
802 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
803 const void *par,
804 int   nsf,                      /* Number of samples in subframe */
805 int *pitch_val,
806 spx_word16_t *gain_val,
807 SpeexBits *bits,
808 char *stack,
809 int count_lost,
810 int subframe_offset,
811 spx_word16_t last_pitch_gain,
812 int cdbk_offset
813 )
814 {
815    int i;
816    float coef = GAIN_SCALING_1*pitch_coef;
817    if (coef>.99)
818       coef=.99;
819    for (i=0;i<nsf;i++)
820    {
821       exc[i]=exc[i-start]*coef;
822    }
823    *pitch_val = start;
824    gain_val[0]=gain_val[2]=0;
825    gain_val[1] = pitch_coef;
826 }