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