Fixed "forced intensity stereo"
[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 /* This is a cos() approximation designed to be bit-exact on any platform. Bit exactness
49    with this approximation is important because it has an impact on the bit allocation */
50 static celt_int16 bitexact_cos(celt_int16 x)
51 {
52    celt_int32 tmp;
53    celt_int16 x2;
54    tmp = (4096+((celt_int32)(x)*(x)))>>13;
55    if (tmp > 32767)
56       tmp = 32767;
57    x2 = tmp;
58    x2 = (32767-x2) + FRAC_MUL16(x2, (-7651 + FRAC_MUL16(x2, (8277 + FRAC_MUL16(-626, x2)))));
59    if (x2 > 32766)
60       x2 = 32766;
61    return 1+x2;
62 }
63
64
65 #ifdef FIXED_POINT
66 /* Compute the amplitude (sqrt energy) in each of the bands */
67 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int end, int _C, int M)
68 {
69    int i, c, N;
70    const celt_int16 *eBands = m->eBands;
71    const int C = CHANNELS(_C);
72    N = M*m->shortMdctSize;
73    for (c=0;c<C;c++)
74    {
75       for (i=0;i<end;i++)
76       {
77          int j;
78          celt_word32 maxval=0;
79          celt_word32 sum = 0;
80          
81          j=M*eBands[i]; do {
82             maxval = MAX32(maxval, X[j+c*N]);
83             maxval = MAX32(maxval, -X[j+c*N]);
84          } while (++j<M*eBands[i+1]);
85          
86          if (maxval > 0)
87          {
88             int shift = celt_ilog2(maxval)-10;
89             j=M*eBands[i]; do {
90                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
91                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
92             } while (++j<M*eBands[i+1]);
93             /* We're adding one here to make damn sure we never end up with a pitch vector that's
94                larger than unity norm */
95             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
96          } else {
97             bank[i+c*m->nbEBands] = EPSILON;
98          }
99          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
100       }
101    }
102    /*printf ("\n");*/
103 }
104
105 /* Normalise each band such that the energy is one. */
106 void normalise_bands(const CELTMode *m, const celt_sig * restrict freq, celt_norm * restrict X, const celt_ener *bank, int end, int _C, int M)
107 {
108    int i, c, N;
109    const celt_int16 *eBands = m->eBands;
110    const int C = CHANNELS(_C);
111    N = M*m->shortMdctSize;
112    for (c=0;c<C;c++)
113    {
114       i=0; do {
115          celt_word16 g;
116          int j,shift;
117          celt_word16 E;
118          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
119          E = VSHR32(bank[i+c*m->nbEBands], shift);
120          g = EXTRACT16(celt_rcp(SHL32(E,3)));
121          j=M*eBands[i]; do {
122             X[j+c*N] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
123          } while (++j<M*eBands[i+1]);
124       } while (++i<end);
125    }
126 }
127
128 #else /* FIXED_POINT */
129 /* Compute the amplitude (sqrt energy) in each of the bands */
130 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int end, int _C, int M)
131 {
132    int i, c, N;
133    const celt_int16 *eBands = m->eBands;
134    const int C = CHANNELS(_C);
135    N = M*m->shortMdctSize;
136    for (c=0;c<C;c++)
137    {
138       for (i=0;i<end;i++)
139       {
140          int j;
141          celt_word32 sum = 1e-10;
142          for (j=M*eBands[i];j<M*eBands[i+1];j++)
143             sum += X[j+c*N]*X[j+c*N];
144          bank[i+c*m->nbEBands] = sqrt(sum);
145          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
146       }
147    }
148    /*printf ("\n");*/
149 }
150
151 /* Normalise each band such that the energy is one. */
152 void normalise_bands(const CELTMode *m, const celt_sig * restrict freq, celt_norm * restrict X, const celt_ener *bank, int end, int _C, int M)
153 {
154    int i, c, N;
155    const celt_int16 *eBands = m->eBands;
156    const int C = CHANNELS(_C);
157    N = M*m->shortMdctSize;
158    for (c=0;c<C;c++)
159    {
160       for (i=0;i<end;i++)
161       {
162          int j;
163          celt_word16 g = 1.f/(1e-10f+bank[i+c*m->nbEBands]);
164          for (j=M*eBands[i];j<M*eBands[i+1];j++)
165             X[j+c*N] = freq[j+c*N]*g;
166       }
167    }
168 }
169
170 #endif /* FIXED_POINT */
171
172 void renormalise_bands(const CELTMode *m, celt_norm * restrict X, int end, int _C, int M)
173 {
174    int i, c;
175    const celt_int16 *eBands = m->eBands;
176    const int C = CHANNELS(_C);
177    for (c=0;c<C;c++)
178    {
179       i=0; do {
180          renormalise_vector(X+M*eBands[i]+c*M*m->shortMdctSize, Q15ONE, M*eBands[i+1]-M*eBands[i], 1);
181       } while (++i<end);
182    }
183 }
184
185 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
186 void denormalise_bands(const CELTMode *m, const celt_norm * restrict X, celt_sig * restrict freq, const celt_ener *bank, int end, int _C, int M)
187 {
188    int i, c, N;
189    const celt_int16 *eBands = m->eBands;
190    const int C = CHANNELS(_C);
191    N = M*m->shortMdctSize;
192    celt_assert2(C<=2, "denormalise_bands() not implemented for >2 channels");
193    for (c=0;c<C;c++)
194    {
195       celt_sig * restrict f;
196       const celt_norm * restrict x;
197       f = freq+c*N;
198       x = X+c*N;
199       for (i=0;i<end;i++)
200       {
201          int j, band_end;
202          celt_word32 g = SHR32(bank[i+c*m->nbEBands],1);
203          j=M*eBands[i];
204          band_end = M*eBands[i+1];
205          do {
206             *f++ = SHL32(MULT16_32_Q15(*x, g),2);
207             x++;
208          } while (++j<band_end);
209       }
210       for (i=M*eBands[m->nbEBands];i<N;i++)
211          *f++ = 0;
212    }
213 }
214
215 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)
216 {
217    int j, c;
218    celt_word16 g;
219    celt_word16 delta;
220    const int C = CHANNELS(_C);
221    celt_word32 Sxy=0, Sxx=0, Syy=0;
222    int len = M*m->pitchEnd;
223    int N = M*m->shortMdctSize;
224 #ifdef FIXED_POINT
225    int shift = 0;
226    celt_word32 maxabs=0;
227
228    for (c=0;c<C;c++)
229    {
230       for (j=0;j<len;j++)
231       {
232          maxabs = MAX32(maxabs, ABS32(X[j+c*N]));
233          maxabs = MAX32(maxabs, ABS32(P[j+c*N]));
234       }
235    }
236    shift = celt_ilog2(maxabs)-12;
237    if (shift<0)
238       shift = 0;
239 #endif
240    delta = PDIV32_16(Q15ONE, len);
241    for (c=0;c<C;c++)
242    {
243       celt_word16 gg = Q15ONE;
244       for (j=0;j<len;j++)
245       {
246          celt_word16 Xj, Pj;
247          Xj = EXTRACT16(SHR32(X[j+c*N], shift));
248          Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[j+c*N], shift)));
249          Sxy = MAC16_16(Sxy, Xj, Pj);
250          Sxx = MAC16_16(Sxx, Pj, Pj);
251          Syy = MAC16_16(Syy, Xj, Xj);
252          gg = SUB16(gg, delta);
253       }
254    }
255 #ifdef FIXED_POINT
256    {
257       celt_word32 num, den;
258       celt_word16 fact;
259       fact = MULT16_16(QCONST16(.04f, 14), norm_rate);
260       if (fact < QCONST16(1.f, 14))
261          fact = QCONST16(1.f, 14);
262       num = Sxy;
263       den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03f,15),Syy);
264       shift = celt_zlog2(Sxy)-16;
265       if (shift < 0)
266          shift = 0;
267       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
268          g = 0;
269       else
270          g = DIV32(SHL32(SHR32(num,shift),14),ADD32(EPSILON,SHR32(den,shift)));
271
272       /* This MUST round down so that we don't over-estimate the gain */
273       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5f,14))),14));
274    }
275 #else
276    {
277       float fact = .04f*norm_rate;
278       if (fact < 1)
279          fact = 1;
280       g = Sxy/(.1f+Sxx+.03f*Syy);
281       if (Sxy < .5f*fact*celt_sqrt(1+Sxx*Syy))
282          g = 0;
283       /* This MUST round down so that we don't over-estimate the gain */
284       *gain_id = floor(20*(g-.5f));
285    }
286 #endif
287    /* This prevents the pitch gain from being above 1.0 for too long by bounding the 
288       maximum error amplification factor to 2.0 */
289    g = ADD16(QCONST16(.5f,14), MULT16_16_16(QCONST16(.05f,14),*gain_id));
290    *gain_prod = MAX16(QCONST32(1.f, 13), MULT16_16_Q14(*gain_prod,g));
291    if (*gain_prod>QCONST32(2.f, 13))
292    {
293       *gain_id=9;
294       *gain_prod = QCONST32(2.f, 13);
295    }
296
297    if (*gain_id < 0)
298    {
299       *gain_id = 0;
300       return 0;
301    } else {
302       if (*gain_id > 15)
303          *gain_id = 15;
304       return 1;
305    }
306 }
307
308 void apply_pitch(const CELTMode *m, celt_sig *X, const celt_sig *P, int gain_id, int pred, int _C, int M)
309 {
310    int j, c, N;
311    celt_word16 gain;
312    celt_word16 delta;
313    const int C = CHANNELS(_C);
314    int len = M*m->pitchEnd;
315
316    N = M*m->shortMdctSize;
317    gain = ADD16(QCONST16(.5f,14), MULT16_16_16(QCONST16(.05f,14),gain_id));
318    delta = PDIV32_16(gain, len);
319    if (pred)
320       gain = -gain;
321    else
322       delta = -delta;
323    for (c=0;c<C;c++)
324    {
325       celt_word16 gg = gain;
326       for (j=0;j<len;j++)
327       {
328          X[j+c*N] += SHL32(MULT16_32_Q15(gg,P[j+c*N]),1);
329          gg = ADD16(gg, delta);
330       }
331    }
332 }
333
334 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 N)
335 {
336    int i = bandID;
337    int j;
338    celt_word16 a1, a2;
339    if (stereo_mode==0)
340    {
341       /* Do mid-side when not doing intensity stereo */
342       a1 = QCONST16(.70711f,14);
343       a2 = dir*QCONST16(.70711f,14);
344    } else {
345       celt_word16 left, right;
346       celt_word16 norm;
347 #ifdef FIXED_POINT
348       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
349 #endif
350       left = VSHR32(bank[i],shift);
351       right = VSHR32(bank[i+m->nbEBands],shift);
352       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
353       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
354       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
355    }
356    for (j=0;j<N;j++)
357    {
358       celt_norm r, l;
359       l = X[j];
360       r = Y[j];
361       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
362       Y[j] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
363    }
364 }
365
366 /* Decide whether we should spread the pulses in the current frame */
367 int folding_decision(const CELTMode *m, celt_norm *X, celt_word16 *average, int *last_decision, int end, int _C, int M)
368 {
369    int i, c, N0;
370    int NR=0;
371    celt_word32 ratio = EPSILON;
372    const int C = CHANNELS(_C);
373    const celt_int16 * restrict eBands = m->eBands;
374    
375    N0 = M*m->shortMdctSize;
376
377    for (c=0;c<C;c++)
378    {
379    for (i=0;i<end;i++)
380    {
381       int j, N;
382       int max_i=0;
383       celt_word16 max_val=EPSILON;
384       celt_word32 floor_ener=EPSILON;
385       celt_norm * restrict x = X+M*eBands[i]+c*N0;
386       N = M*eBands[i+1]-M*eBands[i];
387       for (j=0;j<N;j++)
388       {
389          if (ABS16(x[j])>max_val)
390          {
391             max_val = ABS16(x[j]);
392             max_i = j;
393          }
394       }
395 #if 0
396       for (j=0;j<N;j++)
397       {
398          if (abs(j-max_i)>2)
399             floor_ener += x[j]*x[j];
400       }
401 #else
402       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
403       if (max_i < N-1)
404          floor_ener -= MULT16_16(x[(max_i+1)], x[(max_i+1)]);
405       if (max_i < N-2)
406          floor_ener -= MULT16_16(x[(max_i+2)], x[(max_i+2)]);
407       if (max_i > 0)
408          floor_ener -= MULT16_16(x[(max_i-1)], x[(max_i-1)]);
409       if (max_i > 1)
410          floor_ener -= MULT16_16(x[(max_i-2)], x[(max_i-2)]);
411       floor_ener = MAX32(floor_ener, EPSILON);
412 #endif
413       if (N>7)
414       {
415          celt_word16 r;
416          celt_word16 den = celt_sqrt(floor_ener);
417          den = MAX32(QCONST16(.02f, 15), den);
418          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
419          ratio = ADD32(ratio, EXTEND32(r));
420          NR++;
421       }
422    }
423    }
424    if (NR>0)
425       ratio = DIV32_16(ratio, NR);
426    ratio = ADD32(HALF32(ratio), HALF32(*average));
427    if (!*last_decision)
428    {
429       *last_decision = (ratio < QCONST16(1.8f,8));
430    } else {
431       *last_decision = (ratio < QCONST16(3.f,8));
432    }
433    *average = EXTRACT16(ratio);
434    return *last_decision;
435 }
436
437 #ifdef MEASURE_NORM_MSE
438
439 float MSE[30] = {0};
440 int nbMSEBands = 0;
441 int MSECount[30] = {0};
442
443 void dump_norm_mse(void)
444 {
445    int i;
446    for (i=0;i<nbMSEBands;i++)
447    {
448       printf ("%f ", MSE[i]/MSECount[i]);
449    }
450    printf ("\n");
451 }
452
453 void measure_norm_mse(const CELTMode *m, float *X, float *X0, float *bandE, float *bandE0, int M, int N, int C)
454 {
455    static int init = 0;
456    int i;
457    if (!init)
458    {
459       atexit(dump_norm_mse);
460       init = 1;
461    }
462    for (i=0;i<m->nbEBands;i++)
463    {
464       int j;
465       int c;
466       float g;
467       if (bandE0[i]<1 || (C==2 && bandE0[i+m->nbEBands]<1))
468          continue;
469       for (c=0;c<C;c++)
470       {
471          g = bandE[i+c*m->nbEBands]/(1e-15+bandE0[i+c*m->nbEBands]);
472          for (j=M*m->eBands[i];j<M*m->eBands[i+1];j++)
473             MSE[i] += (g*X[j+c*N]-X0[j+c*N])*(g*X[j+c*N]-X0[j+c*N]);
474       }
475       MSECount[i]+=C;
476    }
477    nbMSEBands = m->nbEBands;
478 }
479
480 #endif
481
482 static void interleave_vector(celt_norm *X, int N0, int stride)
483 {
484    int i,j;
485    VARDECL(celt_norm, tmp);
486    int N;
487    SAVE_STACK;
488    N = N0*stride;
489    ALLOC(tmp, N, celt_norm);
490    for (i=0;i<stride;i++)
491       for (j=0;j<N0;j++)
492          tmp[j*stride+i] = X[i*N0+j];
493    for (j=0;j<N;j++)
494       X[j] = tmp[j];
495    RESTORE_STACK;
496 }
497
498 static void deinterleave_vector(celt_norm *X, int N0, int stride)
499 {
500    int i,j;
501    VARDECL(celt_norm, tmp);
502    int N;
503    SAVE_STACK;
504    N = N0*stride;
505    ALLOC(tmp, N, celt_norm);
506    for (i=0;i<stride;i++)
507       for (j=0;j<N0;j++)
508          tmp[i*N0+j] = X[j*stride+i];
509    for (j=0;j<N;j++)
510       X[j] = tmp[j];
511    RESTORE_STACK;
512 }
513
514 static void haar1(celt_norm *X, int N0, int stride)
515 {
516    int i, j;
517    N0 >>= 1;
518    for (i=0;i<stride;i++)
519       for (j=0;j<N0;j++)
520       {
521          celt_norm tmp = X[stride*2*j+i];
522          X[stride*2*j+i] = MULT16_16_Q15(QCONST16(.7070678f,15), X[stride*2*j+i] + X[stride*(2*j+1)+i]);
523          X[stride*(2*j+1)+i] = MULT16_16_Q15(QCONST16(.7070678f,15), tmp - X[stride*(2*j+1)+i]);
524       }
525 }
526
527 /* This function is responsible for encoding and decoding a band for both
528    the mono and stereo case. Even in the mono case, it can split the band
529    in two and transmit the energy difference with the two half-bands. It
530    can be called recursively so bands can end up being split in 8 parts. */
531 static void quant_band(int encode, const CELTMode *m, int i, celt_norm *X, celt_norm *Y,
532       int N, int b, int spread, int B, int tf_change, celt_norm *lowband, int resynth, void *ec,
533       celt_int32 *remaining_bits, int LM, celt_norm *lowband_out, const celt_ener *bandE, int level, celt_int32 *seed)
534 {
535    int q;
536    int curr_bits;
537    int stereo, split;
538    int imid=0, iside=0;
539    int N0=N;
540    int N_B=N;
541    int N_B0;
542    int B0=B;
543    int time_divide=0;
544    int recombine=0;
545
546    N_B /= B;
547    N_B0 = N_B;
548
549    split = stereo = Y != NULL;
550
551    /* Special case for one sample */
552    if (N==1)
553    {
554       int c;
555       celt_norm *x = X;
556       for (c=0;c<1+stereo;c++)
557       {
558          int sign=0;
559          if (b>=1<<BITRES && *remaining_bits>=1<<BITRES)
560          {
561             if (encode)
562             {
563                sign = x[0]<0;
564                ec_enc_bits((ec_enc*)ec, sign, 1);
565             } else {
566                sign = ec_dec_bits((ec_dec*)ec, 1);
567             }
568             *remaining_bits -= 1<<BITRES;
569             b-=1<<BITRES;
570          }
571          if (resynth)
572             x[0] = sign ? -NORM_SCALING : NORM_SCALING;
573          x = Y;
574       }
575       if (lowband_out)
576          lowband_out[0] = X[0];
577       return;
578    }
579
580    /* Band recombining to increase frequency resolution */
581    if (!stereo && B > 1 && level == 0 && tf_change>0)
582    {
583       while (B>1 && tf_change>0)
584       {
585          B>>=1;
586          N_B<<=1;
587          if (encode)
588             haar1(X, N_B, B);
589          if (lowband)
590             haar1(lowband, N_B, B);
591          recombine++;
592          tf_change--;
593       }
594       B0=B;
595       N_B0 = N_B;
596    }
597
598    /* Increasing the time resolution */
599    if (!stereo && level==0)
600    {
601       while ((N_B&1) == 0 && tf_change<0 && B <= (1<<LM))
602       {
603          if (encode)
604             haar1(X, N_B, B);
605          if (lowband)
606             haar1(lowband, N_B, B);
607          B <<= 1;
608          N_B >>= 1;
609          time_divide++;
610          tf_change++;
611       }
612       B0 = B;
613       N_B0 = N_B;
614    }
615
616    /* Reorganize the samples in time order instead of frequency order */
617    if (!stereo && B0>1 && level==0)
618    {
619       if (encode)
620          deinterleave_vector(X, N_B, B0);
621       if (lowband)
622          deinterleave_vector(lowband, N_B, B0);
623    }
624
625    /* If we need more than 32 bits, try splitting the band in two. */
626    if (!stereo && LM != -1 && b > 32<<BITRES && N>2)
627    {
628       if (LM>0 || (N&1)==0)
629       {
630          N >>= 1;
631          Y = X+N;
632          split = 1;
633          LM -= 1;
634          B = (B+1)>>1;
635       }
636    }
637
638    if (split)
639    {
640       int qb;
641       int itheta=0;
642       int mbits, sbits, delta;
643       int qalloc;
644       celt_word16 mid, side;
645       int offset, N2;
646       offset = ((m->logN[i]+(LM<<BITRES))>>1) - (stereo ? QTHETA_OFFSET_STEREO : QTHETA_OFFSET);
647
648       /* Decide on the resolution to give to the split parameter theta */
649       N2 = 2*N-1;
650       if (stereo && N==2)
651          N2--;
652       qb = (b+N2*offset)/(N2<<BITRES);
653       if (qb > (b>>(BITRES+1))-1)
654          qb = (b>>(BITRES+1))-1;
655
656       if (qb<0)
657          qb = 0;
658       if (qb>14)
659          qb = 14;
660
661       qalloc = 0;
662       if (qb!=0)
663       {
664          int shift;
665          shift = 14-qb;
666
667          if (encode)
668          {
669             if (stereo)
670                stereo_band_mix(m, X, Y, bandE, 0, i, 1, N);
671
672             mid = renormalise_vector(X, Q15ONE, N, 1);
673             side = renormalise_vector(Y, Q15ONE, N, 1);
674
675             /* theta is the atan() of the ratio between the (normalized)
676                side and mid. With just that parameter, we can re-scale both
677                mid and side because we know that 1) they have unit norm and
678                2) they are orthogonal. */
679    #ifdef FIXED_POINT
680             /* 0.63662 = 2/pi */
681             itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
682    #else
683             itheta = floor(.5f+16384*0.63662f*atan2(side,mid));
684    #endif
685
686             itheta = (itheta+(1<<shift>>1))>>shift;
687          }
688
689          /* Entropy coding of the angle. We use a uniform pdf for the
690             first stereo split but a triangular one for the rest. */
691          if (stereo || qb>8 || B>1)
692          {
693             if (encode)
694                ec_enc_uint((ec_enc*)ec, itheta, (1<<qb)+1);
695             else
696                itheta = ec_dec_uint((ec_dec*)ec, (1<<qb)+1);
697             qalloc = log2_frac((1<<qb)+1,BITRES);
698          } else {
699             int fs=1, ft;
700             ft = ((1<<qb>>1)+1)*((1<<qb>>1)+1);
701             if (encode)
702             {
703                int fl;
704
705                fs = itheta <= (1<<qb>>1) ? itheta + 1 : (1<<qb) + 1 - itheta;
706                fl = itheta <= (1<<qb>>1) ? itheta*(itheta + 1)>>1 :
707                 ft - (((1<<qb) + 1 - itheta)*((1<<qb) + 2 - itheta)>>1);
708
709                ec_encode((ec_enc*)ec, fl, fl+fs, ft);
710             } else {
711                int fl=0;
712                int fm;
713                fm = ec_decode((ec_dec*)ec, ft);
714
715                if (fm < ((1<<qb>>1)*((1<<qb>>1) + 1)>>1))
716                {
717                   itheta = (isqrt32(8*(celt_uint32)fm + 1) - 1)>>1;
718                   fs = itheta + 1;
719                   fl = itheta*(itheta + 1)>>1;
720                }
721                else
722                {
723                   itheta = (2*((1<<qb) + 1)
724                    - isqrt32(8*(celt_uint32)(ft - fm - 1) + 1))>>1;
725                   fs = (1<<qb) + 1 - itheta;
726                   fl = ft - (((1<<qb) + 1 - itheta)*((1<<qb) + 2 - itheta)>>1);
727                }
728
729                ec_dec_update((ec_dec*)ec, fl, fl+fs, ft);
730             }
731             qalloc = log2_frac(ft,BITRES) - log2_frac(fs,BITRES) + 1;
732          }
733          itheta <<= shift;
734       } else {
735          if (stereo && encode)
736             stereo_band_mix(m, X, Y, bandE, 1, i, 1, N);
737       }
738
739       if (itheta == 0)
740       {
741          imid = 32767;
742          iside = 0;
743          delta = -10000;
744       } else if (itheta == 16384)
745       {
746          imid = 0;
747          iside = 32767;
748          delta = 10000;
749       } else {
750          imid = bitexact_cos(itheta);
751          iside = bitexact_cos(16384-itheta);
752          /* This is the mid vs side allocation that minimizes squared error
753             in that band. */
754          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
755       }
756
757       /* This is a special case for N=2 that only works for stereo and takes
758          advantage of the fact that mid and side are orthogonal to encode
759          the side with just one bit. */
760       if (N==2 && stereo)
761       {
762          int c, c2;
763          int sign=1;
764          celt_norm v[2], w[2];
765          celt_norm *x2, *y2;
766          mbits = b-qalloc;
767          sbits = 0;
768          /* Only need one bit for the side */
769          if (itheta != 0 && itheta != 16384)
770             sbits = 1<<BITRES;
771          mbits -= sbits;
772          c = itheta > 8192 ? 1 : 0;
773          *remaining_bits -= qalloc+sbits;
774
775          x2 = X;
776          y2 = Y;
777          if (encode)
778          {
779             c2 = 1-c;
780
781             /* v is the largest vector between mid and side. w is the other */
782             if (c==0)
783             {
784                v[0] = x2[0];
785                v[1] = x2[1];
786                w[0] = y2[0];
787                w[1] = y2[1];
788             } else {
789                v[0] = y2[0];
790                v[1] = y2[1];
791                w[0] = x2[0];
792                w[1] = x2[1];
793             }
794             /* Here we only need to encode a sign for the side */
795             if (v[0]*w[1] - v[1]*w[0] > 0)
796                sign = 1;
797             else
798                sign = -1;
799          }
800          quant_band(encode, m, i, v, NULL, N, mbits, spread, B, tf_change, lowband, resynth, ec, remaining_bits, LM, lowband_out, NULL, level+1, seed);
801          if (sbits)
802          {
803             if (encode)
804             {
805                ec_enc_bits((ec_enc*)ec, sign==1, 1);
806             } else {
807                sign = 2*ec_dec_bits((ec_dec*)ec, 1)-1;
808             }
809          } else {
810             sign = 1;
811          }
812          w[0] = -sign*v[1];
813          w[1] = sign*v[0];
814          if (c==0)
815          {
816             x2[0] = v[0];
817             x2[1] = v[1];
818             y2[0] = w[0];
819             y2[1] = w[1];
820          } else {
821             x2[0] = w[0];
822             x2[1] = w[1];
823             y2[0] = v[0];
824             y2[1] = v[1];
825          }
826       } else
827       {
828          /* "Normal" split code */
829          celt_norm *next_lowband2=NULL;
830          celt_norm *next_lowband_out1=NULL;
831          int next_level=0;
832
833          /* Give more bits to low-energy MDCTs than they would otherwise deserve */
834          if (B>1 && !stereo)
835             delta >>= 1;
836
837          mbits = (b-qalloc-delta)/2;
838          if (mbits > b-qalloc)
839             mbits = b-qalloc;
840          if (mbits<0)
841             mbits=0;
842          sbits = b-qalloc-mbits;
843          *remaining_bits -= qalloc;
844
845          if (lowband && !stereo)
846             next_lowband2 = lowband+N; /* >32-bit split case */
847
848          /* Only stereo needs to pass on lowband_out. Otherwise, it's handled at the end */
849          if (stereo)
850             next_lowband_out1 = lowband_out;
851          else
852             next_level = level+1;
853
854          quant_band(encode, m, i, X, NULL, N, mbits, spread, B, tf_change, lowband, resynth, ec, remaining_bits, LM, next_lowband_out1, NULL, next_level, seed);
855          quant_band(encode, m, i, Y, NULL, N, sbits, spread, B, tf_change, next_lowband2, resynth, ec, remaining_bits, LM, NULL, NULL, level, seed);
856       }
857
858    } else {
859       /* This is the basic no-split case */
860       q = bits2pulses(m, m->bits[LM][i], N, b);
861       curr_bits = pulses2bits(m->bits[LM][i], N, q);
862       *remaining_bits -= curr_bits;
863
864       /* Ensures we can never bust the budget */
865       while (*remaining_bits < 0 && q > 0)
866       {
867          *remaining_bits += curr_bits;
868          q--;
869          curr_bits = pulses2bits(m->bits[LM][i], N, q);
870          *remaining_bits -= curr_bits;
871       }
872
873       /* Finally do the actual quantization */
874       if (encode)
875          alg_quant(X, N, q, spread, B, lowband, resynth, (ec_enc*)ec, seed);
876       else
877          alg_unquant(X, N, q, spread, B, lowband, (ec_dec*)ec, seed);
878    }
879
880    /* This code is used by the decoder and by the resynthesis-enabled encoder */
881    if (resynth)
882    {
883       int k;
884
885       if (split)
886       {
887          int j;
888          celt_word16 mid, side;
889 #ifdef FIXED_POINT
890          mid = imid;
891          side = iside;
892 #else
893          mid = (1.f/32768)*imid;
894          side = (1.f/32768)*iside;
895 #endif
896          for (j=0;j<N;j++)
897             X[j] = MULT16_16_Q15(X[j], mid);
898          for (j=0;j<N;j++)
899             Y[j] = MULT16_16_Q15(Y[j], side);
900       }
901
902       /* Undo the sample reorganization going from time order to frequency order */
903       if (!stereo && B0>1 && level==0)
904       {
905          interleave_vector(X, N_B, B0);
906          if (lowband)
907             interleave_vector(lowband, N_B, B0);
908       }
909
910       /* Undo time-freq changes that we did earlier */
911       N_B = N_B0;
912       B = B0;
913       for (k=0;k<time_divide;k++)
914       {
915          B >>= 1;
916          N_B <<= 1;
917          haar1(X, N_B, B);
918          if (lowband)
919             haar1(lowband, N_B, B);
920       }
921
922       for (k=0;k<recombine;k++)
923       {
924          haar1(X, N_B, B);
925          if (lowband)
926             haar1(lowband, N_B, B);
927          N_B>>=1;
928          B <<= 1;
929       }
930
931       /* Scale output for later folding */
932       if (lowband_out && !stereo)
933       {
934          int j;
935          celt_word16 n;
936          n = celt_sqrt(SHL32(EXTEND32(N0),22));
937          for (j=0;j<N0;j++)
938             lowband_out[j] = MULT16_16_Q15(n,X[j]);
939       }
940
941       if (stereo)
942       {
943          stereo_band_mix(m, X, Y, bandE, 0, i, -1, N);
944          /* We only need to renormalize because quantization may not
945             have preserved orthogonality of mid and side */
946          renormalise_vector(X, Q15ONE, N, 1);
947          renormalise_vector(Y, Q15ONE, N, 1);
948       }
949    }
950 }
951
952 void quant_all_bands(int encode, const CELTMode *m, int start, int end, celt_norm *_X, celt_norm *_Y, const celt_ener *bandE, int *pulses, int shortBlocks, int fold, int *tf_res, int resynth, int total_bits, void *ec, int LM)
953 {
954    int i, balance;
955    celt_int32 remaining_bits;
956    const celt_int16 * restrict eBands = m->eBands;
957    celt_norm * restrict norm;
958    VARDECL(celt_norm, _norm);
959    int B;
960    int M;
961    int spread;
962    celt_int32 seed;
963    celt_norm *lowband;
964    int update_lowband = 1;
965    int C = _Y != NULL ? 2 : 1;
966    SAVE_STACK;
967
968    M = 1<<LM;
969    B = shortBlocks ? M : 1;
970    spread = fold ? B : 0;
971    ALLOC(_norm, M*eBands[m->nbEBands], celt_norm);
972    norm = _norm;
973
974    if (encode)
975       seed = ((ec_enc*)ec)->rng;
976    else
977       seed = ((ec_dec*)ec)->rng;
978    balance = 0;
979    lowband = NULL;
980    for (i=start;i<end;i++)
981    {
982       int tell;
983       int b;
984       int N;
985       int curr_balance;
986       celt_norm * restrict X, * restrict Y;
987       int tf_change=0;
988       celt_norm *effective_lowband;
989       
990       X = _X+M*eBands[i];
991       if (_Y!=NULL)
992          Y = _Y+M*eBands[i];
993       else
994          Y = NULL;
995       N = M*eBands[i+1]-M*eBands[i];
996       if (encode)
997          tell = ec_enc_tell((ec_enc*)ec, BITRES);
998       else
999          tell = ec_dec_tell((ec_dec*)ec, BITRES);
1000
1001       /* Compute how many bits we want to allocate to this band */
1002       if (i != start)
1003          balance -= tell;
1004       remaining_bits = (total_bits<<BITRES)-tell-1;
1005       curr_balance = (end-i);
1006       if (curr_balance > 3)
1007          curr_balance = 3;
1008       curr_balance = balance / curr_balance;
1009       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
1010       if (b<0)
1011          b = 0;
1012       /* Prevents ridiculous bit depths */
1013       if (b > C*16*N<<BITRES)
1014          b = C*16*N<<BITRES;
1015
1016       if (M*eBands[i]-N >= M*eBands[start] && (update_lowband || lowband==NULL))
1017             lowband = norm+M*eBands[i]-N;
1018
1019       tf_change = tf_res[i];
1020       if (i>=m->effEBands)
1021       {
1022          X=norm;
1023          if (_Y!=NULL)
1024             Y = norm;
1025       }
1026
1027       if (tf_change==0 && !shortBlocks && fold)
1028          effective_lowband = NULL;
1029       else
1030          effective_lowband = lowband;
1031       quant_band(encode, m, i, X, Y, N, b, fold, B, tf_change, effective_lowband, resynth, ec, &remaining_bits, LM, norm+M*eBands[i], bandE, 0, &seed);
1032
1033       balance += pulses[i] + tell;
1034
1035       /* Update the folding position only as long as we have 2 bit/sample depth */
1036       update_lowband = (b>>BITRES)>2*N;
1037    }
1038    RESTORE_STACK;
1039 }
1040