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