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