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