Allocation adjustment code in quand_band().
[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, int N, int bits,
469                  int spread, celt_norm *lowband, ec_dec *dec,
470                  celt_int32 *remaining_bits, int LM)
471 {
472    int q;
473    int curr_bits;
474
475    q = bits2pulses(m, m->bits[LM][i], N, bits);
476    curr_bits = pulses2bits(m->bits[LM][i], N, q);
477    *remaining_bits -= curr_bits;
478    while (*remaining_bits < 0 && q > 0)
479    {
480       *remaining_bits += curr_bits;
481       q--;
482       curr_bits = pulses2bits(m->bits[LM][i], N, q);
483       *remaining_bits -= curr_bits;
484    }
485    alg_unquant(X, N, q, spread, lowband, dec);
486 }
487
488 /* Quantisation of the residual */
489 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)
490 {
491    int i, j, remaining_bits, balance;
492    const celt_int16 * restrict eBands = m->eBands;
493    celt_norm * restrict norm;
494    VARDECL(celt_norm, _norm);
495    int B;
496    int M;
497    int spread;
498    SAVE_STACK;
499
500    M = 1<<LM;
501    B = shortBlocks ? M : 1;
502    spread = fold ? B : 0;
503    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
504    norm = _norm;
505    /* Just in case the first bands attempts to fold -- shouldn't really happen */
506    for (i=0;i<M;i++)
507       norm[i] = 0;
508
509    balance = 0;
510    for (i=start;i<m->nbEBands;i++)
511    {
512       int tell;
513       int N;
514       int curr_balance;
515       
516       N = M*eBands[i+1]-M*eBands[i];
517
518       tell = ec_enc_tell(enc, BITRES);
519       if (i != start)
520          balance -= tell;
521       remaining_bits = (total_bits<<BITRES)-tell-1;
522       curr_balance = (m->nbEBands-i);
523       if (curr_balance > 3)
524          curr_balance = 3;
525       curr_balance = balance / curr_balance;
526
527       quant_band(m, i, X+M*eBands[i], N, pulses[i]+curr_balance, spread, norm+eBands[start], resynth, enc, &remaining_bits, LM);
528
529       balance += pulses[i] + tell;
530       if (resynth)
531       {
532          celt_word16 n;
533          n = celt_sqrt(SHL32(EXTEND32(N),22));
534          for (j=M*eBands[i];j<M*eBands[i+1];j++)
535             norm[j] = MULT16_16_Q15(n,X[j]);
536       }
537    }
538    RESTORE_STACK;
539 }
540
541 /* Decoding of the residual */
542 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)
543 {
544    int i, j, remaining_bits, balance;
545    const celt_int16 * restrict eBands = m->eBands;
546    celt_norm * restrict norm;
547    VARDECL(celt_norm, _norm);
548    int B;
549    int M;
550    int spread;
551    SAVE_STACK;
552
553    M = 1<<LM;
554    B = shortBlocks ? M : 1;
555    spread = fold ? B : 0;
556    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
557    norm = _norm;
558    /* Just in case the first bands attempts to fold -- shouldn't really happen */
559    for (i=0;i<M;i++)
560       norm[i] = 0;
561
562    balance = 0;
563    for (i=start;i<m->nbEBands;i++)
564    {
565       int tell;
566       celt_word16 n;
567       int N;
568       int curr_balance;
569
570       N = M*eBands[i+1]-M*eBands[i];
571
572       tell = ec_dec_tell(dec, BITRES);
573       if (i != start)
574          balance -= tell;
575       remaining_bits = (total_bits<<BITRES)-tell-1;
576       curr_balance = (m->nbEBands-i);
577       if (curr_balance > 3)
578          curr_balance = 3;
579       curr_balance = balance / curr_balance;
580
581       unquant_band(m, i, X+M*eBands[i], N, pulses[i]+curr_balance, spread, norm+eBands[start], dec, &remaining_bits, LM);
582
583       balance += pulses[i] + tell;
584       n = celt_sqrt(SHL32(EXTEND32(N),22));
585       for (j=M*eBands[i];j<M*eBands[i+1];j++)
586          norm[j] = MULT16_16_Q15(n,X[j]);
587    }
588    RESTORE_STACK;
589 }
590
591 #ifndef DISABLE_STEREO
592
593 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)
594 {
595    int i, j, remaining_bits, balance;
596    const celt_int16 * restrict eBands = m->eBands;
597    celt_norm * restrict norm;
598    VARDECL(celt_norm, _norm);
599    int B;
600    celt_word16 mid, side;
601    int M;
602    int spread;
603    SAVE_STACK;
604
605    M = 1<<LM;
606    B = shortBlocks ? M : 1;
607    spread = fold ? B : 0;
608    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
609    norm = _norm;
610    /* Just in case the first bands attempts to fold -- not that rare for stereo */
611    for (i=0;i<M;i++)
612       norm[i] = 0;
613
614    balance = 0;
615    for (i=start;i<m->nbEBands;i++)
616    {
617       int tell;
618       int q1, q2;
619       const celt_int16 * const *BPbits;
620       int b, qb;
621       int N;
622       int curr_balance, curr_bits;
623       int imid, iside, itheta;
624       int mbits, sbits, delta;
625       int qalloc;
626       celt_norm * restrict X, * restrict Y;
627       
628       X = _X+M*eBands[i];
629       Y = X+M*eBands[m->nbEBands+1];
630       BPbits = m->bits[LM];
631
632       N = M*eBands[i+1]-M*eBands[i];
633       tell = ec_enc_tell(enc, BITRES);
634       if (i != start)
635          balance -= tell;
636       remaining_bits = (total_bits<<BITRES)-tell-1;
637       curr_balance = (m->nbEBands-i);
638       if (curr_balance > 3)
639          curr_balance = 3;
640       curr_balance = balance / curr_balance;
641       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
642       if (b<0)
643          b = 0;
644
645       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
646       if (qb > (b>>BITRES)-1)
647          qb = (b>>BITRES)-1;
648       if (qb<0)
649          qb = 0;
650       if (qb>14)
651          qb = 14;
652
653       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1, M);
654
655       mid = renormalise_vector(X, Q15ONE, N, 1);
656       side = renormalise_vector(Y, Q15ONE, N, 1);
657 #ifdef FIXED_POINT
658       itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
659 #else
660       itheta = floor(.5f+16384*0.63662f*atan2(side,mid));
661 #endif
662       qalloc = log2_frac((1<<qb)+1,BITRES);
663       if (qb==0)
664       {
665          itheta=0;
666       } else {
667          int shift;
668          shift = 14-qb;
669          itheta = (itheta+(1<<shift>>1))>>shift;
670          ec_enc_uint(enc, itheta, (1<<qb)+1);
671          itheta <<= shift;
672       }
673       if (itheta == 0)
674       {
675          imid = 32767;
676          iside = 0;
677          delta = -10000;
678       } else if (itheta == 16384)
679       {
680          imid = 0;
681          iside = 32767;
682          delta = 10000;
683       } else {
684          imid = bitexact_cos(itheta);
685          iside = bitexact_cos(16384-itheta);
686          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
687       }
688 #if 1
689       if (N==2)
690       {
691          int c, c2;
692          int sign=1;
693          celt_norm v[2], w[2];
694          celt_norm *x2, *y2;
695          mbits = b-qalloc;
696          sbits = 0;
697          if (itheta != 0 && itheta != 16384)
698             sbits = 1<<BITRES;
699          mbits -= sbits;
700          c = itheta > 8192 ? 1 : 0;
701          c2 = 1-c;
702
703          x2 = X;
704          y2 = Y;
705          if (c==0)
706          {
707             v[0] = x2[0];
708             v[1] = x2[1];
709             w[0] = y2[0];
710             w[1] = y2[1];
711          } else {
712             v[0] = y2[0];
713             v[1] = y2[1];
714             w[0] = x2[0];
715             w[1] = x2[1];
716          }
717          q1 = bits2pulses(m, BPbits[i], N, mbits);
718          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
719          remaining_bits -= curr_bits;
720          while (remaining_bits < 0 && q1 > 0)
721          {
722             remaining_bits += curr_bits;
723             q1--;
724             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
725             remaining_bits -= curr_bits;
726          }
727
728          alg_quant(v, N, q1, spread, norm+eBands[start], resynth, enc);
729          if (sbits)
730          {
731             if (v[0]*w[1] - v[1]*w[0] > 0)
732                sign = 1;
733             else
734                sign = -1;
735             ec_enc_bits(enc, sign==1, 1);
736          } else {
737             sign = 1;
738          }
739          w[0] = -sign*v[1];
740          w[1] = sign*v[0];
741          if (c==0)
742          {
743             x2[0] = v[0];
744             x2[1] = v[1];
745             y2[0] = w[0];
746             y2[1] = w[1];
747          } else {
748             x2[0] = w[0];
749             x2[1] = w[1];
750             y2[0] = v[0];
751             y2[1] = v[1];
752          }
753       } else 
754 #endif
755       {
756          
757          mbits = (b-qalloc/2-delta)/2;
758          if (mbits > b-qalloc)
759             mbits = b-qalloc;
760          if (mbits<0)
761             mbits=0;
762          sbits = b-qalloc-mbits;
763          q1 = bits2pulses(m, BPbits[i], N, mbits);
764          q2 = bits2pulses(m, BPbits[i], N, sbits);
765          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
766          remaining_bits -= curr_bits;
767          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
768          {
769             remaining_bits += curr_bits;
770             if (q1>q2)
771             {
772                q1--;
773                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
774             } else {
775                q2--;
776                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
777             }
778             remaining_bits -= curr_bits;
779          }
780
781          alg_quant(X, N, q1, spread, norm+eBands[start], resynth, enc);
782          alg_quant(Y, N, q2, spread, NULL, resynth, enc);
783       }
784       
785       balance += pulses[i] + tell;
786
787       if (resynth)
788       {
789          celt_word16 n;
790 #ifdef FIXED_POINT
791          mid = imid;
792          side = iside;
793 #else
794          mid = (1.f/32768)*imid;
795          side = (1.f/32768)*iside;
796 #endif
797          n = celt_sqrt(SHL32(EXTEND32(N),22));
798          for (j=0;j<N;j++)
799             norm[M*eBands[i]+j] = MULT16_16_Q15(n,X[j]);
800
801          for (j=0;j<N;j++)
802             X[j] = MULT16_16_Q15(X[j], mid);
803          for (j=0;j<N;j++)
804             Y[j] = MULT16_16_Q15(Y[j], side);
805
806          stereo_band_mix(m, X, Y, bandE, 0, i, -1, M);
807          renormalise_vector(X, Q15ONE, N, 1);
808          renormalise_vector(Y, Q15ONE, N, 1);
809       }
810    }
811    RESTORE_STACK;
812 }
813 #endif /* DISABLE_STEREO */
814
815
816 #ifndef DISABLE_STEREO
817
818 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)
819 {
820    int i, j, remaining_bits, balance;
821    const celt_int16 * restrict eBands = m->eBands;
822    celt_norm * restrict norm;
823    VARDECL(celt_norm, _norm);
824    int B;
825    celt_word16 mid, side;
826    int M;
827    int spread;
828    SAVE_STACK;
829
830    M = 1<<LM;
831    B = shortBlocks ? M : 1;
832    spread = fold ? B : 0;
833    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
834    norm = _norm;
835    /* Just in case the first bands attempts to fold -- not that rare for stereo */
836    for (i=0;i<M;i++)
837       norm[i] = 0;
838
839    balance = 0;
840    for (i=start;i<m->nbEBands;i++)
841    {
842       int tell;
843       int q1, q2;
844       celt_word16 n;
845       const celt_int16 * const *BPbits;
846       int b, qb;
847       int N;
848       int curr_balance, curr_bits;
849       int imid, iside, itheta;
850       int mbits, sbits, delta;
851       int qalloc;
852       celt_norm * restrict X, * restrict Y;
853       
854       X = _X+M*eBands[i];
855       Y = X+M*eBands[m->nbEBands+1];
856       BPbits = m->bits[LM];
857
858       N = M*eBands[i+1]-M*eBands[i];
859       tell = ec_dec_tell(dec, BITRES);
860       if (i != start)
861          balance -= tell;
862       remaining_bits = (total_bits<<BITRES)-tell-1;
863       curr_balance = (m->nbEBands-i);
864       if (curr_balance > 3)
865          curr_balance = 3;
866       curr_balance = balance / curr_balance;
867       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
868       if (b<0)
869          b = 0;
870
871       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
872       if (qb > (b>>BITRES)-1)
873          qb = (b>>BITRES)-1;
874       if (qb>14)
875          qb = 14;
876       if (qb<0)
877          qb = 0;
878       qalloc = log2_frac((1<<qb)+1,BITRES);
879       if (qb==0)
880       {
881          itheta=0;
882       } else {
883          int shift;
884          shift = 14-qb;
885          itheta = ec_dec_uint(dec, (1<<qb)+1);
886          itheta <<= shift;
887       }
888       if (itheta == 0)
889       {
890          imid = 32767;
891          iside = 0;
892          delta = -10000;
893       } else if (itheta == 16384)
894       {
895          imid = 0;
896          iside = 32767;
897          delta = 10000;
898       } else {
899          imid = bitexact_cos(itheta);
900          iside = bitexact_cos(16384-itheta);
901          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
902       }
903       n = celt_sqrt(SHL32(EXTEND32(N),22));
904
905 #if 1
906       if (N==2)
907       {
908          int c, c2;
909          int sign=1;
910          celt_norm v[2], w[2];
911          celt_norm *x2, *y2;
912          mbits = b-qalloc;
913          sbits = 0;
914          if (itheta != 0 && itheta != 16384)
915             sbits = 1<<BITRES;
916          mbits -= sbits;
917          c = itheta > 8192 ? 1 : 0;
918          c2 = 1-c;
919
920          x2 = X;
921          y2 = Y;
922          v[0] = x2[c];
923          v[1] = y2[c];
924          w[0] = x2[c2];
925          w[1] = y2[c2];
926          q1 = bits2pulses(m, BPbits[i], N, mbits);
927          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
928          remaining_bits -= curr_bits;
929          while (remaining_bits < 0 && q1 > 0)
930          {
931             remaining_bits += curr_bits;
932             q1--;
933             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
934             remaining_bits -= curr_bits;
935          }
936
937          alg_unquant(v, N, q1, spread, norm+eBands[start], dec);
938          if (sbits)
939             sign = 2*ec_dec_bits(dec, 1)-1;
940          else
941             sign = 1;
942          w[0] = -sign*v[1];
943          w[1] = sign*v[0];
944          if (c==0)
945          {
946             x2[0] = v[0];
947             x2[1] = v[1];
948             y2[0] = w[0];
949             y2[1] = w[1];
950          } else {
951             x2[0] = w[0];
952             x2[1] = w[1];
953             y2[0] = v[0];
954             y2[1] = v[1];
955          }
956       } else
957 #endif
958       {
959          mbits = (b-qalloc/2-delta)/2;
960          if (mbits > b-qalloc)
961             mbits = b-qalloc;
962          if (mbits<0)
963             mbits=0;
964          sbits = b-qalloc-mbits;
965          q1 = bits2pulses(m, BPbits[i], N, mbits);
966          q2 = bits2pulses(m, BPbits[i], N, sbits);
967          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
968          remaining_bits -= curr_bits;
969          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
970          {
971             remaining_bits += curr_bits;
972             if (q1>q2)
973             {
974                q1--;
975                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
976             } else {
977                q2--;
978                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
979             }
980             remaining_bits -= curr_bits;
981          }
982          
983          alg_unquant(X, N, q1, spread, norm+eBands[start], dec);
984          alg_unquant(Y, N, q2, spread, NULL, dec);
985       }
986       balance += pulses[i] + tell;
987       
988 #ifdef FIXED_POINT
989       mid = imid;
990       side = iside;
991 #else
992       mid = (1.f/32768)*imid;
993       side = (1.f/32768)*iside;
994 #endif
995       for (j=0;j<N;j++)
996          norm[M*eBands[i]+j] = MULT16_16_Q15(n,X[j]);
997
998       for (j=0;j<N;j++)
999          X[j] = MULT16_16_Q15(X[j], mid);
1000       for (j=0;j<N;j++)
1001          Y[j] = MULT16_16_Q15(Y[j], side);
1002
1003       stereo_band_mix(m, X, Y, bandE, 0, i, -1, M);
1004       renormalise_vector(X, Q15ONE, N, 1);
1005       renormalise_vector(Y, Q15ONE, N, 1);
1006    }
1007    RESTORE_STACK;
1008 }
1009
1010 #endif /* DISABLE_STEREO */