This fixes a bug in the new stereo code triggered only at ridiculously high
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 Jean-Marc Valin, CSIRO
2    (C) 2008-2009 Gregory Maxwell */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "bands.h"
38 #include "modes.h"
39 #include "vq.h"
40 #include "cwrs.h"
41 #include "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44 #include "rate.h"
45
46 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
47
48 #ifdef FIXED_POINT
49 /* Compute the amplitude (sqrt energy) in each of the bands */
50 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
51 {
52    int i, c;
53    const celt_int16_t *eBands = m->eBands;
54    const int C = CHANNELS(m);
55    for (c=0;c<C;c++)
56    {
57       for (i=0;i<m->nbEBands;i++)
58       {
59          int j;
60          celt_word32_t maxval=0;
61          celt_word32_t sum = 0;
62          
63          j=eBands[i]; do {
64             maxval = MAX32(maxval, X[j*C+c]);
65             maxval = MAX32(maxval, -X[j*C+c]);
66          } while (++j<eBands[i+1]);
67          
68          if (maxval > 0)
69          {
70             int shift = celt_ilog2(maxval)-10;
71             j=eBands[i]; do {
72                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j*C+c],shift)),
73                                    EXTRACT16(VSHR32(X[j*C+c],shift)));
74             } while (++j<eBands[i+1]);
75             /* We're adding one here to make damn sure we never end up with a pitch vector that's
76                larger than unity norm */
77             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
78          } else {
79             bank[i*C+c] = EPSILON;
80          }
81          /*printf ("%f ", bank[i*C+c]);*/
82       }
83    }
84    /*printf ("\n");*/
85 }
86
87 /* Normalise each band such that the energy is one. */
88 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
89 {
90    int i, c;
91    const celt_int16_t *eBands = m->eBands;
92    const int C = CHANNELS(m);
93    for (c=0;c<C;c++)
94    {
95       i=0; do {
96          celt_word16_t g;
97          int j,shift;
98          celt_word16_t E;
99          shift = celt_zlog2(bank[i*C+c])-13;
100          E = VSHR32(bank[i*C+c], shift);
101          g = EXTRACT16(celt_rcp(SHL32(E,3)));
102          j=eBands[i]; do {
103             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
104          } while (++j<eBands[i+1]);
105       } while (++i<m->nbEBands);
106    }
107 }
108
109 #else /* FIXED_POINT */
110 /* Compute the amplitude (sqrt energy) in each of the bands */
111 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
112 {
113    int i, c;
114    const celt_int16_t *eBands = m->eBands;
115    const int C = CHANNELS(m);
116    for (c=0;c<C;c++)
117    {
118       for (i=0;i<m->nbEBands;i++)
119       {
120          int j;
121          celt_word32_t sum = 1e-10;
122          for (j=eBands[i];j<eBands[i+1];j++)
123             sum += X[j*C+c]*X[j*C+c];
124          bank[i*C+c] = sqrt(sum);
125          /*printf ("%f ", bank[i*C+c]);*/
126       }
127    }
128    /*printf ("\n");*/
129 }
130
131 #ifdef EXP_PSY
132 void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, celt_ener_t *bank)
133 {
134    int i, c;
135    const celt_int16_t *eBands = m->eBands;
136    const int C = CHANNELS(m);
137    for (c=0;c<C;c++)
138    {
139       for (i=0;i<m->nbEBands;i++)
140       {
141          int j;
142          celt_word32_t sum = 1e-10;
143          for (j=eBands[i];j<eBands[i+1];j++)
144             sum += X[j*C+c]*X[j*C+c]*tonality[j];
145          bank[i*C+c] = sqrt(sum);
146          /*printf ("%f ", bank[i*C+c]);*/
147       }
148    }
149    /*printf ("\n");*/
150 }
151 #endif
152
153 /* Normalise each band such that the energy is one. */
154 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
155 {
156    int i, c;
157    const celt_int16_t *eBands = m->eBands;
158    const int C = CHANNELS(m);
159    for (c=0;c<C;c++)
160    {
161       for (i=0;i<m->nbEBands;i++)
162       {
163          int j;
164          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]);
165          for (j=eBands[i];j<eBands[i+1];j++)
166             X[j*C+c] = freq[j*C+c]*g;
167       }
168    }
169 }
170
171 #endif /* FIXED_POINT */
172
173 #ifndef DISABLE_STEREO
174 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
175 {
176    int i, c;
177    const celt_int16_t *eBands = m->eBands;
178    const int C = CHANNELS(m);
179    for (c=0;c<C;c++)
180    {
181       i=0; do {
182          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
183       } while (++i<m->nbEBands);
184    }
185 }
186 #endif /* DISABLE_STEREO */
187
188 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
189 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
190 {
191    int i, c;
192    const celt_int16_t *eBands = m->eBands;
193    const int C = CHANNELS(m);
194    if (C>2)
195       celt_fatal("denormalise_bands() not implemented for >2 channels");
196    for (c=0;c<C;c++)
197    {
198       for (i=0;i<m->nbEBands;i++)
199       {
200          int j;
201          celt_word32_t g = SHR32(bank[i*C+c],1);
202          j=eBands[i]; do {
203             freq[j*C+c] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
204          } while (++j<eBands[i+1]);
205       }
206    }
207    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
208       freq[i] = 0;
209 }
210
211
212 /* Compute the best gain for each "pitch band" */
213 int compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
214 {
215    int i;
216    int gain_sum = 0;
217    const celt_int16_t *pBands = m->pBands;
218    const int C = CHANNELS(m);
219
220    for (i=0;i<m->nbPBands;i++)
221    {
222       celt_word32_t Sxy=0, Sxx=0;
223       int j;
224       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
225       for (j=C*pBands[i];j<C*pBands[i+1];j++)
226       {
227          Sxy = MAC16_16(Sxy, X[j], P[j]);
228          Sxx = MAC16_16(Sxx, X[j], X[j]);
229       }
230       /* No negative gain allowed */
231       if (Sxy < 0)
232          Sxy = 0;
233       /* Not sure how that would happen, just making sure */
234       if (Sxy > Sxx)
235          Sxy = Sxx;
236       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
237          residual doesn't quantise well */
238       Sxy = MULT16_32_Q15(QCONST16(.99f, 15), Sxy);
239       /* gain = Sxy/Sxx */
240       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
241       if (gains[i]>QCONST16(.5,15))
242          gain_sum++;
243       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
244    }
245    /*if(rand()%10==0)
246    {
247       for (i=0;i<m->nbPBands;i++)
248          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
249       printf ("\n");
250    }*/
251    return gain_sum > 5;
252 }
253
254 static void intensity_band(celt_norm_t * restrict X, int len)
255 {
256    int j;
257    celt_word32_t E = 1e-15;
258    celt_word32_t E2 = 1e-15;
259    for (j=0;j<len;j++)
260    {
261       X[j] = X[2*j];
262       E = MAC16_16(E, X[j],X[j]);
263       E2 = MAC16_16(E2, X[2*j+1],X[2*j+1]);
264    }
265 #ifndef FIXED_POINT
266    E  = celt_sqrt(E+E2)/celt_sqrt(E);
267    for (j=0;j<len;j++)
268       X[j] *= E;
269 #endif
270    for (j=0;j<len;j++)
271       X[len+j] = 0;
272
273 }
274
275 static void dup_band(celt_norm_t * restrict X, int len)
276 {
277    int j;
278    for (j=len-1;j>=0;j--)
279    {
280       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
281       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
282    }
283 }
284
285 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
286 {
287    int i = bandID;
288    const celt_int16_t *eBands = m->eBands;
289    const int C = CHANNELS(m);
290    {
291       int j;
292       if (stereo_mode && dir <0)
293       {
294          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
295       } else {
296          celt_word16_t a1, a2;
297          if (stereo_mode==0)
298          {
299             /* Do mid-side when not doing intensity stereo */
300             a1 = QCONST16(.70711f,14);
301             a2 = dir*QCONST16(.70711f,14);
302          } else {
303             celt_word16_t left, right;
304             celt_word16_t norm;
305 #ifdef FIXED_POINT
306             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
307 #endif
308             left = VSHR32(bank[i*C],shift);
309             right = VSHR32(bank[i*C+1],shift);
310             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
311             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
312             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
313          }
314          for (j=eBands[i];j<eBands[i+1];j++)
315          {
316             celt_norm_t r, l;
317             l = X[j*C];
318             r = X[j*C+1];
319             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
320             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
321          }
322       }
323       if (stereo_mode && dir>0)
324       {
325          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
326       }
327    }
328 }
329
330 static void point_stereo_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int bandID, int dir)
331 {
332    int i = bandID;
333    const celt_int16_t *eBands = m->eBands;
334    const int C = CHANNELS(m);
335    celt_word16_t left, right;
336    celt_word16_t norm;
337    celt_word16_t a1, a2;
338    int j;
339 #ifdef FIXED_POINT
340    int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
341 #endif
342    left = VSHR32(bank[i*C],shift);
343    right = VSHR32(bank[i*C+1],shift);
344    norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
345    a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
346    a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
347    for (j=eBands[i];j<eBands[i+1];j++)
348    {
349       celt_norm_t r, l;
350       l = X[j*C];
351       r = X[j*C+1];
352       X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
353       X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
354    }
355 }
356
357 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
358 {
359    int i;
360    for (i=0;i<len-5;i++)
361       stereo_mode[i] = 0;
362    for (;i<len;i++)
363       stereo_mode[i] = 0;
364 }
365
366 void interleave(celt_norm_t *x, int N)
367 {
368    int i;
369    VARDECL(celt_norm_t, tmp);
370    SAVE_STACK;
371    ALLOC(tmp, N, celt_norm_t);
372    
373    for (i=0;i<N;i++)
374       tmp[i] = x[i];
375    for (i=0;i<N>>1;i++)
376    {
377       x[i<<1] = tmp[i];
378       x[(i<<1)+1] = tmp[i+(N>>1)];
379    }
380    RESTORE_STACK;
381 }
382
383 void deinterleave(celt_norm_t *x, int N)
384 {
385    int i;
386    VARDECL(celt_norm_t, tmp);
387    SAVE_STACK;
388    ALLOC(tmp, N, celt_norm_t);
389    
390    for (i=0;i<N;i++)
391       tmp[i] = x[i];
392    for (i=0;i<N>>1;i++)
393    {
394       x[i] = tmp[i<<1];
395       x[i+(N>>1)] = tmp[(i<<1)+1];
396    }
397    RESTORE_STACK;
398 }
399
400 /* Quantisation of the residual */
401 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
402 {
403    int i, j, remaining_bits, balance;
404    const celt_int16_t * restrict eBands = m->eBands;
405    celt_norm_t * restrict norm;
406    VARDECL(celt_norm_t, _norm);   const celt_int16_t *pBands = m->pBands;
407    int pband=-1;
408    int B;
409    SAVE_STACK;
410
411    B = shortBlocks ? m->nbShortMdcts : 1;
412    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
413    norm = _norm;
414
415    balance = 0;
416    /*printf("bits left: %d\n", bits);
417    for (i=0;i<m->nbEBands;i++)
418       printf ("(%d %d) ", pulses[i], ebits[i]);
419    printf ("\n");*/
420    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
421    for (i=0;i<m->nbEBands;i++)
422    {
423       int tell;
424       int N;
425       int q;
426       celt_word16_t n;
427       const celt_int16_t * const *BPbits;
428       
429       int curr_balance, curr_bits;
430       
431       N = eBands[i+1]-eBands[i];
432       BPbits = m->bits;
433
434       tell = ec_enc_tell(enc, 4);
435       if (i != 0)
436          balance -= tell;
437       remaining_bits = (total_bits<<BITRES)-tell-1;
438       curr_balance = (m->nbEBands-i);
439       if (curr_balance > 3)
440          curr_balance = 3;
441       curr_balance = balance / curr_balance;
442       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
443       curr_bits = pulses2bits(BPbits[i], N, q);
444       remaining_bits -= curr_bits;
445       while (remaining_bits < 0 && q > 0)
446       {
447          remaining_bits += curr_bits;
448          q--;
449          curr_bits = pulses2bits(BPbits[i], N, q);
450          remaining_bits -= curr_bits;
451       }
452       balance += pulses[i] + tell;
453       
454       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
455
456       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
457       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
458       {
459          int enabled = 1;
460          pband++;
461          if (remaining_bits >= 1<<BITRES) {
462            enabled = pgains[pband] > QCONST16(.5,15);
463            ec_enc_bits(enc, enabled, 1);
464            balance += 1<<BITRES;
465          }
466          if (enabled)
467             pgains[pband] = QCONST16(.9,15);
468          else
469             pgains[pband] = 0;
470       }
471
472       /* If pitch isn't available, use intra-frame prediction */
473       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
474       {
475          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
476       } else if (pitch_used && eBands[i] < m->pitchEnd) {
477          for (j=eBands[i];j<eBands[i+1];j++)
478             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
479       } else {
480          for (j=eBands[i];j<eBands[i+1];j++)
481             P[j] = 0;
482       }
483       
484       if (q > 0)
485       {
486          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], enc);
487       } else {
488          for (j=eBands[i];j<eBands[i+1];j++)
489             X[j] = P[j];
490       }
491       for (j=eBands[i];j<eBands[i+1];j++)
492          norm[j] = MULT16_16_Q15(n,X[j]);
493    }
494    RESTORE_STACK;
495 }
496
497 void quant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
498 {
499    int i, j, remaining_bits, balance;
500    const celt_int16_t * restrict eBands = m->eBands;
501    celt_norm_t * restrict norm;
502    VARDECL(celt_norm_t, _norm);
503    const int C = CHANNELS(m);
504    const celt_int16_t *pBands = m->pBands;
505    int pband=-1;
506    int B;
507    celt_word16_t mid, side;
508    SAVE_STACK;
509
510    B = shortBlocks ? m->nbShortMdcts : 1;
511    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
512    norm = _norm;
513
514    balance = 0;
515    /*printf("bits left: %d\n", bits);
516    for (i=0;i<m->nbEBands;i++)
517    printf ("(%d %d) ", pulses[i], ebits[i]);
518    printf ("\n");*/
519    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
520    for (i=0;i<m->nbEBands;i++)
521    {
522       int tell;
523       int q1, q2;
524       celt_word16_t n;
525       const celt_int16_t * const *BPbits;
526       int b, qb;
527       int N;
528       int curr_balance, curr_bits;
529       int imid, iside, itheta;
530       int mbits, sbits, delta;
531       int qalloc;
532       
533       BPbits = m->bits;
534
535       N = eBands[i+1]-eBands[i];
536       tell = ec_enc_tell(enc, 4);
537       if (i != 0)
538          balance -= tell;
539       remaining_bits = (total_bits<<BITRES)-tell-1;
540       curr_balance = (m->nbEBands-i);
541       if (curr_balance > 3)
542          curr_balance = 3;
543       curr_balance = balance / curr_balance;
544       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
545       if (b<0)
546          b = 0;
547
548       if (N<5) {
549          
550          q1 = bits2pulses(m, BPbits[i], N, b/2);
551          curr_bits = 2*pulses2bits(BPbits[i], N, q1);
552          remaining_bits -= curr_bits;
553          while (remaining_bits < 0 && q1 > 0)
554          {
555             remaining_bits += curr_bits;
556             q1--;
557             curr_bits = 2*pulses2bits(BPbits[i], N, q1);
558             remaining_bits -= curr_bits;
559          }
560          balance += pulses[i] + tell;
561          
562          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
563          
564          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
565          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
566          {
567             int enabled = 1;
568             pband++;
569             if (remaining_bits >= 1<<BITRES) {
570                enabled = pgains[pband] > QCONST16(.5,15);
571                ec_enc_bits(enc, enabled, 1);
572                balance += 1<<BITRES;
573             }
574             if (enabled)
575                pgains[pband] = QCONST16(.9,15);
576             else
577                pgains[pband] = 0;
578          }
579
580          /* If pitch isn't available, use intra-frame prediction */
581          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
582          {
583             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
584             deinterleave(P+C*eBands[i], C*N);
585          } else if (pitch_used && eBands[i] < m->pitchEnd) {
586             deinterleave(P+C*eBands[i], C*N);
587             for (j=C*eBands[i];j<C*eBands[i+1];j++)
588                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
589          } else {
590             for (j=C*eBands[i];j<C*eBands[i+1];j++)
591                P[j] = 0;
592          }
593          deinterleave(X+C*eBands[i], C*N);
594          if (q1 > 0)
595          {
596             alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
597             alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q1, P+C*eBands[i]+N, enc);
598          } else {
599             for (j=C*eBands[i];j<C*eBands[i+1];j++)
600                X[j] = P[j];
601          }
602
603          interleave(X+C*eBands[i], C*N);
604          for (j=0;j<C*N;j++)
605             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
606
607       } else {
608       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
609       if (qb > (b>>BITRES)-1)
610          qb = (b>>BITRES)-1;
611       if (qb<0)
612          qb = 0;
613       if (qb>14)
614          qb = 14;
615       
616       if (qb==0)
617          point_stereo_mix(m, X, bandE, i, 1);
618       else
619          stereo_band_mix(m, X, bandE, 0, i, 1);
620       
621       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
622       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
623 #ifdef FIXED_POINT
624       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
625 #else
626       itheta = floor(.5+16384*0.63662*atan2(side,mid));
627 #endif
628       qalloc = log2_frac((1<<qb)+1,4);
629       if (qb==0)
630       {
631          itheta=0;
632       } else {
633          int shift;
634          shift = 14-qb;
635          itheta = (itheta+(1<<shift>>1))>>shift;
636          ec_enc_uint(enc, itheta, (1<<qb)+1);
637          itheta <<= shift;
638       }
639       if (itheta == 0)
640       {
641          imid = 32767;
642          iside = 0;
643          delta = -10000;
644       } else if (itheta == 16384)
645       {
646          imid = 0;
647          iside = 32767;
648          delta = 10000;
649       } else {
650          imid = bitexact_cos(itheta);
651          iside = bitexact_cos(16384-itheta);
652          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
653       }
654       mbits = (b-qalloc/2-delta)/2;
655       if (mbits > b-qalloc)
656          mbits = b-qalloc;
657       if (mbits<0)
658          mbits=0;
659       sbits = b-qalloc-mbits;
660       q1 = bits2pulses(m, BPbits[i], N, mbits);
661       q2 = bits2pulses(m, BPbits[i], N, sbits);
662       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
663       remaining_bits -= curr_bits;
664       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
665       {
666          remaining_bits += curr_bits;
667          if (q1>q2)
668          {
669             q1--;
670             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
671          } else {
672             q2--;
673             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
674          }
675          remaining_bits -= curr_bits;
676       }
677       balance += pulses[i] + tell;
678       
679       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
680
681       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
682       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
683       {
684          int enabled = 1;
685          pband++;
686          if (remaining_bits >= 1<<BITRES) {
687             enabled = pgains[pband] > QCONST16(.5,15);
688             ec_enc_bits(enc, enabled, 1);
689             balance += 1<<BITRES;
690          }
691          if (enabled)
692             pgains[pband] = QCONST16(.9,15);
693          else
694             pgains[pband] = 0;
695       }
696       
697
698       /* If pitch isn't available, use intra-frame prediction */
699       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
700       {
701          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
702          if (qb==0)
703             point_stereo_mix(m, P, bandE, i, 1);
704          else
705             stereo_band_mix(m, P, bandE, 0, i, 1);
706          deinterleave(P+C*eBands[i], C*N);
707
708          /*for (j=C*eBands[i];j<C*eBands[i+1];j++)
709             P[j] = 0;*/
710       } else if (pitch_used && eBands[i] < m->pitchEnd) {
711          if (qb==0)
712             point_stereo_mix(m, P, bandE, i, 1);
713          else
714             stereo_band_mix(m, P, bandE, 0, i, 1);
715          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
716          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
717          deinterleave(P+C*eBands[i], C*N);
718          for (j=C*eBands[i];j<C*eBands[i+1];j++)
719             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
720       } else {
721          for (j=C*eBands[i];j<C*eBands[i+1];j++)
722             P[j] = 0;
723       }
724       deinterleave(X+C*eBands[i], C*N);
725       if (q1 > 0)
726          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
727       else
728          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
729             X[j] = P[j];
730       if (q2 > 0)
731          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
732       else
733          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
734             X[j] = 0;
735       /*   orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
736
737
738 #ifdef FIXED_POINT
739       mid = imid;
740       side = iside;
741 #else
742       mid = (1./32768)*imid;
743       side = (1./32768)*iside;
744 #endif
745       for (j=0;j<N;j++)
746          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
747       for (j=0;j<N;j++)
748          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
749
750       interleave(X+C*eBands[i], C*N);
751
752       stereo_band_mix(m, X, bandE, 0, i, -1);
753       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
754       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
755       for (j=0;j<C*N;j++)
756          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
757       }
758    }
759    RESTORE_STACK;
760 }
761
762 /* Decoding of the residual */
763 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
764 {
765    int i, j, remaining_bits, balance;
766    const celt_int16_t * restrict eBands = m->eBands;
767    celt_norm_t * restrict norm;
768    VARDECL(celt_norm_t, _norm);
769    const celt_int16_t *pBands = m->pBands;
770    int pband=-1;
771    int B;
772    SAVE_STACK;
773
774    B = shortBlocks ? m->nbShortMdcts : 1;
775    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
776    norm = _norm;
777
778    balance = 0;
779    for (i=0;i<m->nbEBands;i++)
780    {
781       int tell;
782       int N;
783       int q;
784       celt_word16_t n;
785       const celt_int16_t * const *BPbits;
786       
787       int curr_balance, curr_bits;
788
789       N = eBands[i+1]-eBands[i];
790       BPbits = m->bits;
791
792       tell = ec_dec_tell(dec, 4);
793       if (i != 0)
794          balance -= tell;
795       remaining_bits = (total_bits<<BITRES)-tell-1;
796       curr_balance = (m->nbEBands-i);
797       if (curr_balance > 3)
798          curr_balance = 3;
799       curr_balance = balance / curr_balance;
800       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
801       curr_bits = pulses2bits(BPbits[i], N, q);
802       remaining_bits -= curr_bits;
803       while (remaining_bits < 0 && q > 0)
804       {
805          remaining_bits += curr_bits;
806          q--;
807          curr_bits = pulses2bits(BPbits[i], N, q);
808          remaining_bits -= curr_bits;
809       }
810       balance += pulses[i] + tell;
811
812       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
813
814       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
815       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
816       {
817          int enabled = 1;
818          pband++;
819          if (remaining_bits >= 1<<BITRES) {
820            enabled = ec_dec_bits(dec, 1);
821            balance += 1<<BITRES;
822          }
823          if (enabled)
824             pgains[pband] = QCONST16(.9,15);
825          else
826             pgains[pband] = 0;
827       }
828
829       /* If pitch isn't available, use intra-frame prediction */
830       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
831       {
832          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
833       } else if (pitch_used && eBands[i] < m->pitchEnd) {
834          for (j=eBands[i];j<eBands[i+1];j++)
835             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
836       } else {
837          for (j=eBands[i];j<eBands[i+1];j++)
838             P[j] = 0;
839       }
840       
841       if (q > 0)
842       {
843          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
844       } else {
845          for (j=eBands[i];j<eBands[i+1];j++)
846             X[j] = P[j];
847       }
848       for (j=eBands[i];j<eBands[i+1];j++)
849          norm[j] = MULT16_16_Q15(n,X[j]);
850    }
851    RESTORE_STACK;
852 }
853
854 void unquant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
855 {
856    int i, j, remaining_bits, balance;
857    const celt_int16_t * restrict eBands = m->eBands;
858    celt_norm_t * restrict norm;
859    VARDECL(celt_norm_t, _norm);
860    const int C = CHANNELS(m);
861    const celt_int16_t *pBands = m->pBands;
862    int pband=-1;
863    int B;
864    celt_word16_t mid, side;
865    SAVE_STACK;
866
867    B = shortBlocks ? m->nbShortMdcts : 1;
868    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
869    norm = _norm;
870
871    balance = 0;
872    /*printf("bits left: %d\n", bits);
873    for (i=0;i<m->nbEBands;i++)
874    printf ("(%d %d) ", pulses[i], ebits[i]);
875    printf ("\n");*/
876    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
877    for (i=0;i<m->nbEBands;i++)
878    {
879       int tell;
880       int q1, q2;
881       celt_word16_t n;
882       const celt_int16_t * const *BPbits;
883       int b, qb;
884       int N;
885       int curr_balance, curr_bits;
886       int imid, iside, itheta;
887       int mbits, sbits, delta;
888       int qalloc;
889       
890       BPbits = m->bits;
891
892       N = eBands[i+1]-eBands[i];
893       tell = ec_dec_tell(dec, 4);
894       if (i != 0)
895          balance -= tell;
896       remaining_bits = (total_bits<<BITRES)-tell-1;
897       curr_balance = (m->nbEBands-i);
898       if (curr_balance > 3)
899          curr_balance = 3;
900       curr_balance = balance / curr_balance;
901       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
902       if (b<0)
903          b = 0;
904       
905       if (N<5) {
906          
907          q1 = bits2pulses(m, BPbits[i], N, b/2);
908          curr_bits = 2*pulses2bits(BPbits[i], N, q1);
909          remaining_bits -= curr_bits;
910          while (remaining_bits < 0 && q1 > 0)
911          {
912             remaining_bits += curr_bits;
913             q1--;
914             curr_bits = 2*pulses2bits(BPbits[i], N, q1);
915             remaining_bits -= curr_bits;
916          }
917          balance += pulses[i] + tell;
918          
919          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
920          
921          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
922          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
923          {
924             int enabled = 1;
925             pband++;
926             if (remaining_bits >= 1<<BITRES) {
927                enabled = pgains[pband] > QCONST16(.5,15);
928                enabled = ec_dec_bits(dec, 1);
929                balance += 1<<BITRES;
930             }
931             if (enabled)
932                pgains[pband] = QCONST16(.9,15);
933             else
934                pgains[pband] = 0;
935          }
936          
937          /* If pitch isn't available, use intra-frame prediction */
938          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
939          {
940             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
941             deinterleave(P+C*eBands[i], C*N);
942          } else if (pitch_used && eBands[i] < m->pitchEnd) {
943             deinterleave(P+C*eBands[i], C*N);
944             for (j=C*eBands[i];j<C*eBands[i+1];j++)
945                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
946          } else {
947             for (j=C*eBands[i];j<C*eBands[i+1];j++)
948                P[j] = 0;
949          }
950          if (q1 > 0)
951          {
952             alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
953             alg_unquant(X+C*eBands[i]+N, N, q1, P+C*eBands[i]+N, dec);
954          } else {
955             for (j=C*eBands[i];j<C*eBands[i+1];j++)
956                X[j] = P[j];
957          }
958          
959          interleave(X+C*eBands[i], C*N);
960          for (j=0;j<C*N;j++)
961             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
962
963       } else {
964       
965       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
966       if (qb > (b>>BITRES)-1)
967          qb = (b>>BITRES)-1;
968       if (qb>14)
969          qb = 14;
970       if (qb<0)
971          qb = 0;
972       qalloc = log2_frac((1<<qb)+1,4);
973       if (qb==0)
974       {
975          itheta=0;
976       } else {
977          int shift;
978          shift = 14-qb;
979          itheta = ec_dec_uint(dec, (1<<qb)+1);
980          itheta <<= shift;
981       }
982       if (itheta == 0)
983       {
984          imid = 32767;
985          iside = 0;
986          delta = -10000;
987       } else if (itheta == 16384)
988       {
989          imid = 0;
990          iside = 32767;
991          delta = 10000;
992       } else {
993          imid = bitexact_cos(itheta);
994          iside = bitexact_cos(16384-itheta);
995          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
996       }
997       mbits = (b-qalloc/2-delta)/2;
998       if (mbits > b-qalloc)
999          mbits = b-qalloc;
1000       if (mbits<0)
1001          mbits=0;
1002       sbits = b-qalloc-mbits;
1003       q1 = bits2pulses(m, BPbits[i], N, mbits);
1004       q2 = bits2pulses(m, BPbits[i], N, sbits);
1005       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1006       remaining_bits -= curr_bits;
1007       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
1008       {
1009          remaining_bits += curr_bits;
1010          if (q1>q2)
1011          {
1012             q1--;
1013             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1014          } else {
1015             q2--;
1016             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1017          }
1018          remaining_bits -= curr_bits;
1019       }
1020       balance += pulses[i] + tell;
1021       
1022       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
1023
1024       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
1025       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
1026       {
1027          int enabled = 1;
1028          pband++;
1029          if (remaining_bits >= 1<<BITRES) {
1030             enabled = pgains[pband] > QCONST16(.5,15);
1031             enabled = ec_dec_bits(dec, 1);
1032             balance += 1<<BITRES;
1033          }
1034          if (enabled)
1035             pgains[pband] = QCONST16(.9,15);
1036          else
1037             pgains[pband] = 0;
1038       }
1039
1040       /* If pitch isn't available, use intra-frame prediction */
1041       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
1042       {
1043          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
1044          if (qb==0)
1045             point_stereo_mix(m, P, bandE, i, 1);
1046          else
1047             stereo_band_mix(m, P, bandE, 0, i, 1);
1048          deinterleave(P+C*eBands[i], C*N);
1049       } else if (pitch_used && eBands[i] < m->pitchEnd) {
1050          if (qb==0)
1051             point_stereo_mix(m, P, bandE, i, 1);
1052          else
1053             stereo_band_mix(m, P, bandE, 0, i, 1);
1054          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1055          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1056          deinterleave(P+C*eBands[i], C*N);
1057          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1058             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1059       } else {
1060          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1061             P[j] = 0;
1062       }
1063       deinterleave(X+C*eBands[i], C*N);
1064       if (q1 > 0)
1065          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
1066       else
1067          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
1068             X[j] = P[j];
1069       if (q2 > 0)
1070          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
1071       else
1072          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
1073             X[j] = 0;
1074       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
1075       
1076 #ifdef FIXED_POINT
1077       mid = imid;
1078       side = iside;
1079 #else
1080       mid = (1./32768)*imid;
1081       side = (1./32768)*iside;
1082 #endif
1083       for (j=0;j<N;j++)
1084          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
1085       for (j=0;j<N;j++)
1086          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1087       
1088       interleave(X+C*eBands[i], C*N);
1089
1090       stereo_band_mix(m, X, bandE, 0, i, -1);
1091       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1092       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1093       for (j=0;j<C*N;j++)
1094          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
1095       }
1096    }
1097    RESTORE_STACK;
1098 }