Cleanup: getting rid of some old bits of stereo code that are no longer useful
[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 q;
425       celt_word16_t n;
426       const celt_int16_t * const *BPbits;
427       
428       int curr_balance, curr_bits;
429       
430       BPbits = m->bits;
431
432       tell = ec_enc_tell(enc, 4);
433       if (i != 0)
434          balance -= tell;
435       remaining_bits = (total_bits<<BITRES)-tell-1;
436       curr_balance = (m->nbEBands-i);
437       if (curr_balance > 3)
438          curr_balance = 3;
439       curr_balance = balance / curr_balance;
440       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
441       curr_bits = BPbits[i][q];
442       remaining_bits -= curr_bits;
443       while (remaining_bits < 0 && q > 0)
444       {
445          remaining_bits += curr_bits;
446          q--;
447          curr_bits = BPbits[i][q];
448          remaining_bits -= curr_bits;
449       }
450       balance += pulses[i] + tell;
451       
452       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
453
454       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
455       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
456       {
457          int enabled = 1;
458          pband++;
459          if (remaining_bits >= 1<<BITRES) {
460            enabled = pgains[pband] > QCONST16(.5,15);
461            ec_enc_bits(enc, enabled, 1);
462            balance += 1<<BITRES;
463          }
464          if (enabled)
465             pgains[pband] = QCONST16(.9,15);
466          else
467             pgains[pband] = 0;
468       }
469
470       /* If pitch isn't available, use intra-frame prediction */
471       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
472       {
473          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
474       } else if (pitch_used && eBands[i] < m->pitchEnd) {
475          for (j=eBands[i];j<eBands[i+1];j++)
476             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
477       } else {
478          for (j=eBands[i];j<eBands[i+1];j++)
479             P[j] = 0;
480       }
481       
482       if (q > 0)
483       {
484          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], enc);
485       } else {
486          for (j=eBands[i];j<eBands[i+1];j++)
487             X[j] = P[j];
488       }
489       for (j=eBands[i];j<eBands[i+1];j++)
490          norm[j] = MULT16_16_Q15(n,X[j]);
491    }
492    RESTORE_STACK;
493 }
494
495 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)
496 {
497    int i, j, remaining_bits, balance;
498    const celt_int16_t * restrict eBands = m->eBands;
499    celt_norm_t * restrict norm;
500    VARDECL(celt_norm_t, _norm);
501    const int C = CHANNELS(m);
502    const celt_int16_t *pBands = m->pBands;
503    int pband=-1;
504    int B;
505    celt_word16_t mid, side;
506    SAVE_STACK;
507
508    B = shortBlocks ? m->nbShortMdcts : 1;
509    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
510    norm = _norm;
511
512    balance = 0;
513    /*printf("bits left: %d\n", bits);
514    for (i=0;i<m->nbEBands;i++)
515    printf ("(%d %d) ", pulses[i], ebits[i]);
516    printf ("\n");*/
517    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
518    for (i=0;i<m->nbEBands;i++)
519    {
520       int tell;
521       int q1, q2;
522       celt_word16_t n;
523       const celt_int16_t * const *BPbits;
524       int b, qb;
525       int N;
526       int curr_balance, curr_bits;
527       int imid, iside, itheta;
528       int mbits, sbits, delta;
529       int qalloc;
530       
531       BPbits = m->bits;
532
533       N = eBands[i+1]-eBands[i];
534       tell = ec_enc_tell(enc, 4);
535       if (i != 0)
536          balance -= tell;
537       remaining_bits = (total_bits<<BITRES)-tell-1;
538       curr_balance = (m->nbEBands-i);
539       if (curr_balance > 3)
540          curr_balance = 3;
541       curr_balance = balance / curr_balance;
542       b = pulses[i]+curr_balance;
543       if (b<0)
544          b = 0;
545
546       if (N<5) {
547          
548          q1 = bits2pulses(m, BPbits[i], b/2);
549          curr_bits = 2*BPbits[i][q1];
550          remaining_bits -= curr_bits;
551          while (remaining_bits < 0 && q1 > 0)
552          {
553             remaining_bits += curr_bits;
554             q1--;
555             curr_bits = 2*BPbits[i][q1];
556             remaining_bits -= curr_bits;
557          }
558          balance += pulses[i] + tell;
559          
560          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
561          
562          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
563          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
564          {
565             int enabled = 1;
566             pband++;
567             if (remaining_bits >= 1<<BITRES) {
568                enabled = pgains[pband] > QCONST16(.5,15);
569                ec_enc_bits(enc, enabled, 1);
570                balance += 1<<BITRES;
571             }
572             if (enabled)
573                pgains[pband] = QCONST16(.9,15);
574             else
575                pgains[pband] = 0;
576          }
577
578          /* If pitch isn't available, use intra-frame prediction */
579          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
580          {
581             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
582             deinterleave(P+C*eBands[i], C*N);
583          } else if (pitch_used && eBands[i] < m->pitchEnd) {
584             deinterleave(P+C*eBands[i], C*N);
585             for (j=C*eBands[i];j<C*eBands[i+1];j++)
586                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
587          } else {
588             for (j=C*eBands[i];j<C*eBands[i+1];j++)
589                P[j] = 0;
590          }
591          deinterleave(X+C*eBands[i], C*N);
592          if (q1 > 0)
593          {
594             alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
595             alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q1, P+C*eBands[i]+N, enc);
596          } else {
597             for (j=C*eBands[i];j<C*eBands[i+1];j++)
598                X[j] = P[j];
599          }
600
601          interleave(X+C*eBands[i], C*N);
602          for (j=0;j<C*N;j++)
603             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
604
605       } else {
606       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
607       if (qb > (b>>BITRES)-1)
608          qb = (b>>BITRES)-1;
609       if (qb<0)
610          qb = 0;
611       
612       if (qb==0)
613          point_stereo_mix(m, X, bandE, i, 1);
614       else
615          stereo_band_mix(m, X, bandE, 0, i, 1);
616       
617       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
618       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
619 #ifdef FIXED_POINT
620       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
621 #else
622       itheta = floor(.5+16384*0.63662*atan2(side,mid));
623 #endif
624       qalloc = log2_frac((1<<qb)+1,4);
625       if (qb==0)
626       {
627          itheta=0;
628       } else {
629          int shift;
630          shift = 14-qb;
631          itheta = (itheta+(1<<shift>>1))>>shift;
632          ec_enc_uint(enc, itheta, (1<<qb)+1);
633          itheta <<= shift;
634       }
635       if (itheta == 0)
636       {
637          imid = 32767;
638          iside = 0;
639          delta = -10000;
640       } else if (itheta == 16384)
641       {
642          imid = 0;
643          iside = 32767;
644          delta = 10000;
645       } else {
646          imid = bitexact_cos(itheta);
647          iside = bitexact_cos(16384-itheta);
648          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
649       }
650       mbits = (b-qalloc/2-delta)/2;
651       if (mbits > b-qalloc)
652          mbits = b-qalloc;
653       if (mbits<0)
654          mbits=0;
655       sbits = b-qalloc-mbits;
656       q1 = bits2pulses(m, BPbits[i], mbits);
657       q2 = bits2pulses(m, BPbits[i], sbits);
658       curr_bits = BPbits[i][q1]+BPbits[i][q2]+qalloc;
659       remaining_bits -= curr_bits;
660       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
661       {
662          remaining_bits += curr_bits;
663          if (q1>q2)
664          {
665             q1--;
666             curr_bits = BPbits[i][q1]+BPbits[i][q2]+qalloc;
667          } else {
668             q2--;
669             curr_bits = BPbits[i][q1]+BPbits[i][q2]+qalloc;
670          }
671          remaining_bits -= curr_bits;
672       }
673       balance += pulses[i] + tell;
674       
675       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
676
677       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
678       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
679       {
680          int enabled = 1;
681          pband++;
682          if (remaining_bits >= 1<<BITRES) {
683             enabled = pgains[pband] > QCONST16(.5,15);
684             ec_enc_bits(enc, enabled, 1);
685             balance += 1<<BITRES;
686          }
687          if (enabled)
688             pgains[pband] = QCONST16(.9,15);
689          else
690             pgains[pband] = 0;
691       }
692       
693
694       /* If pitch isn't available, use intra-frame prediction */
695       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
696       {
697          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
698          if (qb==0)
699             point_stereo_mix(m, P, bandE, i, 1);
700          else
701             stereo_band_mix(m, P, bandE, 0, i, 1);
702          deinterleave(P+C*eBands[i], C*N);
703
704          /*for (j=C*eBands[i];j<C*eBands[i+1];j++)
705             P[j] = 0;*/
706       } else if (pitch_used && eBands[i] < m->pitchEnd) {
707          if (qb==0)
708             point_stereo_mix(m, P, bandE, i, 1);
709          else
710             stereo_band_mix(m, P, bandE, 0, i, 1);
711          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
712          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
713          deinterleave(P+C*eBands[i], C*N);
714          for (j=C*eBands[i];j<C*eBands[i+1];j++)
715             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
716       } else {
717          for (j=C*eBands[i];j<C*eBands[i+1];j++)
718             P[j] = 0;
719       }
720       deinterleave(X+C*eBands[i], C*N);
721       if (q1 > 0)
722          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
723       else
724          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
725             X[j] = P[j];
726       if (q2 > 0)
727          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
728       else
729          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
730             X[j] = 0;
731       /*   orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
732
733
734 #ifdef FIXED_POINT
735       mid = imid;
736       side = iside;
737 #else
738       mid = (1./32768)*imid;
739       side = (1./32768)*iside;
740 #endif
741       for (j=0;j<N;j++)
742          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
743       for (j=0;j<N;j++)
744          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
745
746       interleave(X+C*eBands[i], C*N);
747
748       stereo_band_mix(m, X, bandE, 0, i, -1);
749       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
750       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
751       for (j=0;j<C*N;j++)
752          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
753       }
754    }
755    RESTORE_STACK;
756 }
757
758 /* Decoding of the residual */
759 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)
760 {
761    int i, j, remaining_bits, balance;
762    const celt_int16_t * restrict eBands = m->eBands;
763    celt_norm_t * restrict norm;
764    VARDECL(celt_norm_t, _norm);
765    const celt_int16_t *pBands = m->pBands;
766    int pband=-1;
767    int B;
768    SAVE_STACK;
769
770    B = shortBlocks ? m->nbShortMdcts : 1;
771    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
772    norm = _norm;
773
774    balance = 0;
775    for (i=0;i<m->nbEBands;i++)
776    {
777       int tell;
778       int q;
779       celt_word16_t n;
780       const celt_int16_t * const *BPbits;
781       
782       int curr_balance, curr_bits;
783       
784       BPbits = m->bits;
785
786       tell = ec_dec_tell(dec, 4);
787       if (i != 0)
788          balance -= tell;
789       remaining_bits = (total_bits<<BITRES)-tell-1;
790       curr_balance = (m->nbEBands-i);
791       if (curr_balance > 3)
792          curr_balance = 3;
793       curr_balance = balance / curr_balance;
794       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
795       curr_bits = BPbits[i][q];
796       remaining_bits -= curr_bits;
797       while (remaining_bits < 0 && q > 0)
798       {
799          remaining_bits += curr_bits;
800          q--;
801          curr_bits = BPbits[i][q];
802          remaining_bits -= curr_bits;
803       }
804       balance += pulses[i] + tell;
805
806       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
807
808       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
809       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
810       {
811          int enabled = 1;
812          pband++;
813          if (remaining_bits >= 1<<BITRES) {
814            enabled = ec_dec_bits(dec, 1);
815            balance += 1<<BITRES;
816          }
817          if (enabled)
818             pgains[pband] = QCONST16(.9,15);
819          else
820             pgains[pband] = 0;
821       }
822
823       /* If pitch isn't available, use intra-frame prediction */
824       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
825       {
826          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
827       } else if (pitch_used && eBands[i] < m->pitchEnd) {
828          for (j=eBands[i];j<eBands[i+1];j++)
829             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
830       } else {
831          for (j=eBands[i];j<eBands[i+1];j++)
832             P[j] = 0;
833       }
834       
835       if (q > 0)
836       {
837          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
838       } else {
839          for (j=eBands[i];j<eBands[i+1];j++)
840             X[j] = P[j];
841       }
842       for (j=eBands[i];j<eBands[i+1];j++)
843          norm[j] = MULT16_16_Q15(n,X[j]);
844    }
845    RESTORE_STACK;
846 }
847
848 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)
849 {
850    int i, j, remaining_bits, balance;
851    const celt_int16_t * restrict eBands = m->eBands;
852    celt_norm_t * restrict norm;
853    VARDECL(celt_norm_t, _norm);
854    const int C = CHANNELS(m);
855    const celt_int16_t *pBands = m->pBands;
856    int pband=-1;
857    int B;
858    celt_word16_t mid, side;
859    SAVE_STACK;
860
861    B = shortBlocks ? m->nbShortMdcts : 1;
862    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
863    norm = _norm;
864
865    balance = 0;
866    /*printf("bits left: %d\n", bits);
867    for (i=0;i<m->nbEBands;i++)
868    printf ("(%d %d) ", pulses[i], ebits[i]);
869    printf ("\n");*/
870    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
871    for (i=0;i<m->nbEBands;i++)
872    {
873       int tell;
874       int q1, q2;
875       celt_word16_t n;
876       const celt_int16_t * const *BPbits;
877       int b, qb;
878       int N;
879       int curr_balance, curr_bits;
880       int imid, iside, itheta;
881       int mbits, sbits, delta;
882       int qalloc;
883       
884       BPbits = m->bits;
885
886       N = eBands[i+1]-eBands[i];
887       tell = ec_dec_tell(dec, 4);
888       if (i != 0)
889          balance -= tell;
890       remaining_bits = (total_bits<<BITRES)-tell-1;
891       curr_balance = (m->nbEBands-i);
892       if (curr_balance > 3)
893          curr_balance = 3;
894       curr_balance = balance / curr_balance;
895       b = pulses[i]+curr_balance;
896       if (b<0)
897          b = 0;
898       
899       if (N<5) {
900          
901          q1 = bits2pulses(m, BPbits[i], b/2);
902          curr_bits = 2*BPbits[i][q1];
903          remaining_bits -= curr_bits;
904          while (remaining_bits < 0 && q1 > 0)
905          {
906             remaining_bits += curr_bits;
907             q1--;
908             curr_bits = 2*BPbits[i][q1];
909             remaining_bits -= curr_bits;
910          }
911          balance += pulses[i] + tell;
912          
913          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
914          
915          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
916          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
917          {
918             int enabled = 1;
919             pband++;
920             if (remaining_bits >= 1<<BITRES) {
921                enabled = pgains[pband] > QCONST16(.5,15);
922                enabled = ec_dec_bits(dec, 1);
923                balance += 1<<BITRES;
924             }
925             if (enabled)
926                pgains[pband] = QCONST16(.9,15);
927             else
928                pgains[pband] = 0;
929          }
930          
931          /* If pitch isn't available, use intra-frame prediction */
932          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
933          {
934             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
935             deinterleave(P+C*eBands[i], C*N);
936          } else if (pitch_used && eBands[i] < m->pitchEnd) {
937             deinterleave(P+C*eBands[i], C*N);
938             for (j=C*eBands[i];j<C*eBands[i+1];j++)
939                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
940          } else {
941             for (j=C*eBands[i];j<C*eBands[i+1];j++)
942                P[j] = 0;
943          }
944          if (q1 > 0)
945          {
946             alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
947             alg_unquant(X+C*eBands[i]+N, N, q1, P+C*eBands[i]+N, dec);
948          } else {
949             for (j=C*eBands[i];j<C*eBands[i+1];j++)
950                X[j] = P[j];
951          }
952          
953          interleave(X+C*eBands[i], C*N);
954          for (j=0;j<C*N;j++)
955             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
956
957       } else {
958       
959       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
960       if (qb > (b>>BITRES)-1)
961          qb = (b>>BITRES)-1;
962       if (qb<0)
963          qb = 0;
964       qalloc = log2_frac((1<<qb)+1,4);
965       if (qb==0)
966       {
967          itheta=0;
968       } else {
969          int shift;
970          shift = 14-qb;
971          itheta = ec_dec_uint(dec, (1<<qb)+1);
972          itheta <<= shift;
973       }
974       if (itheta == 0)
975       {
976          imid = 32767;
977          iside = 0;
978          delta = -10000;
979       } else if (itheta == 16384)
980       {
981          imid = 0;
982          iside = 32767;
983          delta = 10000;
984       } else {
985          imid = bitexact_cos(itheta);
986          iside = bitexact_cos(16384-itheta);
987          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
988       }
989       mbits = (b-qalloc/2-delta)/2;
990       if (mbits > b-qalloc)
991          mbits = b-qalloc;
992       if (mbits<0)
993          mbits=0;
994       sbits = b-qalloc-mbits;
995       q1 = bits2pulses(m, BPbits[i], mbits);
996       q2 = bits2pulses(m, BPbits[i], sbits);
997       curr_bits = BPbits[i][q1]+BPbits[i][q2]+qalloc;
998       remaining_bits -= curr_bits;
999       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
1000       {
1001          remaining_bits += curr_bits;
1002          if (q1>q2)
1003          {
1004             q1--;
1005             curr_bits = BPbits[i][q1]+BPbits[i][q2]+qalloc;
1006          } else {
1007             q2--;
1008             curr_bits = BPbits[i][q1]+BPbits[i][q2]+qalloc;
1009          }
1010          remaining_bits -= curr_bits;
1011       }
1012       balance += pulses[i] + tell;
1013       
1014       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
1015
1016       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
1017       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
1018       {
1019          int enabled = 1;
1020          pband++;
1021          if (remaining_bits >= 1<<BITRES) {
1022             enabled = pgains[pband] > QCONST16(.5,15);
1023             enabled = ec_dec_bits(dec, 1);
1024             balance += 1<<BITRES;
1025          }
1026          if (enabled)
1027             pgains[pband] = QCONST16(.9,15);
1028          else
1029             pgains[pband] = 0;
1030       }
1031
1032       /* If pitch isn't available, use intra-frame prediction */
1033       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
1034       {
1035          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
1036          if (qb==0)
1037             point_stereo_mix(m, P, bandE, i, 1);
1038          else
1039             stereo_band_mix(m, P, bandE, 0, i, 1);
1040          deinterleave(P+C*eBands[i], C*N);
1041       } else if (pitch_used && eBands[i] < m->pitchEnd) {
1042          if (qb==0)
1043             point_stereo_mix(m, P, bandE, i, 1);
1044          else
1045             stereo_band_mix(m, P, bandE, 0, i, 1);
1046          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1047          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1048          deinterleave(P+C*eBands[i], C*N);
1049          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1050             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1051       } else {
1052          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1053             P[j] = 0;
1054       }
1055       deinterleave(X+C*eBands[i], C*N);
1056       if (q1 > 0)
1057          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
1058       else
1059          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
1060             X[j] = P[j];
1061       if (q2 > 0)
1062          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
1063       else
1064          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
1065             X[j] = 0;
1066       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
1067       
1068 #ifdef FIXED_POINT
1069       mid = imid;
1070       side = iside;
1071 #else
1072       mid = (1./32768)*imid;
1073       side = (1./32768)*iside;
1074 #endif
1075       for (j=0;j<N;j++)
1076          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
1077       for (j=0;j<N;j++)
1078          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1079       
1080       interleave(X+C*eBands[i], C*N);
1081
1082       stereo_band_mix(m, X, bandE, 0, i, -1);
1083       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1084       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1085       for (j=0;j<C*N;j++)
1086          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
1087       }
1088    }
1089    RESTORE_STACK;
1090 }