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