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