Re-organised the special case for N==1
[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, 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    int tf_change=-1;
486
487    if (spread)
488       N_B /= spread;
489    N_B0 = N_B;
490
491    split = stereo = Y != NULL;
492
493    /* Special case for one sample */
494    if (N==1)
495    {
496       int c;
497       celt_norm *x = X;
498       for (c=0;c<1+stereo;c++)
499       {
500          int sign=0;
501          if (b>=1<<BITRES && *remaining_bits>=1<<BITRES)
502          {
503             if (encode)
504             {
505                sign = x[0]<0;
506                ec_enc_bits(ec, sign, 1);
507             } else {
508                sign = ec_dec_bits((ec_dec*)ec, 1);
509             }
510             *remaining_bits -= 1<<BITRES;
511             b-=1<<BITRES;
512          }
513          if (resynth)
514             x[0] = sign ? -NORM_SCALING : NORM_SCALING;
515          x = Y;
516       }
517       if (c==0 && lowband_out)
518          lowband_out[0] = X[0];
519       return;
520    }
521
522    /* Band recombining to increase frequency resolution */
523    if (!stereo && spread > 1 && level == 0 && tf_change>0)
524    {
525       while (spread>1 && tf_change>0)
526       {
527          spread>>=1;
528          N_B<<=1;
529          if (encode)
530             haar1(X, N_B, spread);
531          if (lowband)
532             haar1(lowband, N_B, spread);
533          recombine++;
534          tf_change--;
535       }
536       spread0=spread;
537       N_B0 = N_B;
538    }
539
540    /* Increasing the time resolution */
541    if (!stereo && spread>1 && level==0)
542    {
543       while ((N_B&1) == 0 && tf_change<0 && spread <= (1<<LM))
544       {
545          if (encode)
546             haar1(X, N_B, spread);
547          if (lowband)
548             haar1(lowband, N_B, spread);
549          spread <<= 1;
550          N_B >>= 1;
551          time_divide++;
552          tf_change++;
553       }
554       spread0 = spread;
555       N_B0 = N_B;
556       if (spread0>1)
557       {
558          if (encode)
559             deinterleave_vector(X, N_B, spread0);
560          if (lowband)
561             deinterleave_vector(lowband, N_B, spread0);
562       }
563    }
564
565    /* If we need more than 32 bits, try splitting the band in two. */
566    if (!stereo && LM != -1 && b > 32<<BITRES && N>2)
567    {
568       if (LM>0 || (N&1)==0)
569       {
570          N >>= 1;
571          Y = X+N;
572          split = 1;
573          LM -= 1;
574          spread = (spread+1)>>1;
575       }
576    }
577
578    if (split)
579    {
580       int qb;
581       int itheta=0;
582       int mbits, sbits, delta;
583       int qalloc;
584       celt_word16 mid, side;
585       int offset, N2;
586       offset = m->logN[i]+(LM<<BITRES)-QTHETA_OFFSET;
587       N2 = 2*N-1;
588       if (stereo && N>2)
589          N2--;
590       qb = (b+N2*offset)/(N2<<BITRES);
591       if (qb > (b>>(BITRES+1))-1)
592          qb = (b>>(BITRES+1))-1;
593
594       if (qb<0)
595          qb = 0;
596       if (qb>14)
597          qb = 14;
598
599       qalloc = 0;
600       if (qb!=0)
601       {
602          int shift;
603          shift = 14-qb;
604
605          if (encode)
606          {
607             if (stereo)
608                stereo_band_mix(m, X, Y, bandE, qb==0, i, 1, N);
609
610             mid = renormalise_vector(X, Q15ONE, N, 1);
611             side = renormalise_vector(Y, Q15ONE, N, 1);
612
613             /* 0.63662 = 2/pi */
614    #ifdef FIXED_POINT
615             itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
616    #else
617             itheta = floor(.5f+16384*0.63662f*atan2(side,mid));
618    #endif
619
620             itheta = (itheta+(1<<shift>>1))>>shift;
621          }
622
623          /* Entropy coding of the angle. We use a uniform pdf for the
624             first stereo split but a triangular one for the rest. */
625          if (stereo || qb>9 || spread>1)
626          {
627             if (encode)
628                ec_enc_uint(ec, itheta, (1<<qb)+1);
629             else
630                itheta = ec_dec_uint((ec_dec*)ec, (1<<qb)+1);
631             qalloc = log2_frac((1<<qb)+1,BITRES);
632          } else {
633             int fs=1, ft;
634             ft = ((1<<qb>>1)+1)*((1<<qb>>1)+1);
635             if (encode)
636             {
637                int j;
638                int fl=0;
639                j=0;
640                while(1)
641                {
642                   if (j==itheta)
643                      break;
644                   fl+=fs;
645                   if (j<(1<<qb>>1))
646                      fs++;
647                   else
648                      fs--;
649                   j++;
650                }
651                ec_encode(ec, fl, fl+fs, ft);
652             } else {
653                int fl=0;
654                int j, fm;
655                fm = ec_decode((ec_dec*)ec, ft);
656                j=0;
657                while (1)
658                {
659                   if (fm < fl+fs)
660                      break;
661                   fl+=fs;
662                   if (j<(1<<qb>>1))
663                      fs++;
664                   else
665                      fs--;
666                   j++;
667                }
668                itheta = j;
669                ec_dec_update((ec_dec*)ec, fl, fl+fs, ft);
670             }
671             qalloc = log2_frac(ft,BITRES) - log2_frac(fs,BITRES) + 1;
672          }
673          itheta <<= shift;
674       }
675
676       if (itheta == 0)
677       {
678          imid = 32767;
679          iside = 0;
680          delta = -10000;
681       } else if (itheta == 16384)
682       {
683          imid = 0;
684          iside = 32767;
685          delta = 10000;
686       } else {
687          imid = bitexact_cos(itheta);
688          iside = bitexact_cos(16384-itheta);
689          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
690       }
691
692       /* This is a special case for N=2 that only works for stereo and takes
693          advantage of the fact that mid and side are orthogonal to encode
694          the side with just one bit. */
695       if (N==2 && stereo)
696       {
697          int c, c2;
698          int sign=1;
699          celt_norm v[2], w[2];
700          celt_norm *x2, *y2;
701          mbits = b-qalloc;
702          sbits = 0;
703          if (itheta != 0 && itheta != 16384)
704             sbits = 1<<BITRES;
705          mbits -= sbits;
706          c = itheta > 8192 ? 1 : 0;
707          *remaining_bits -= qalloc+sbits;
708
709          x2 = X;
710          y2 = Y;
711          if (encode)
712          {
713             c2 = 1-c;
714
715             if (c==0)
716             {
717                v[0] = x2[0];
718                v[1] = x2[1];
719                w[0] = y2[0];
720                w[1] = y2[1];
721             } else {
722                v[0] = y2[0];
723                v[1] = y2[1];
724                w[0] = x2[0];
725                w[1] = x2[1];
726             }
727             /* Here we only need to encode a sign for the side */
728             if (v[0]*w[1] - v[1]*w[0] > 0)
729                sign = 1;
730             else
731                sign = -1;
732          }
733          quant_band(encode, m, i, v, NULL, N, mbits, spread, lowband, resynth, ec, remaining_bits, LM, lowband_out, NULL, level+1);
734          if (sbits)
735          {
736             if (encode)
737             {
738                ec_enc_bits(ec, sign==1, 1);
739             } else {
740                sign = 2*ec_dec_bits((ec_dec*)ec, 1)-1;
741             }
742          } else {
743             sign = 1;
744          }
745          w[0] = -sign*v[1];
746          w[1] = sign*v[0];
747          if (c==0)
748          {
749             x2[0] = v[0];
750             x2[1] = v[1];
751             y2[0] = w[0];
752             y2[1] = w[1];
753          } else {
754             x2[0] = w[0];
755             x2[1] = w[1];
756             y2[0] = v[0];
757             y2[1] = v[1];
758          }
759       } else
760       {
761          /* "Normal" split code */
762          celt_norm *next_lowband2=NULL;
763          celt_norm *next_lowband_out1=NULL;
764          int next_level=0;
765
766          /* Give more bits to low-energy MDCTs than they would otherwise deserve */
767          if (spread>1 && !stereo)
768             delta >>= 1;
769
770          mbits = (b-qalloc/2-delta)/2;
771          if (mbits > b-qalloc)
772             mbits = b-qalloc;
773          if (mbits<0)
774             mbits=0;
775          sbits = b-qalloc-mbits;
776          *remaining_bits -= qalloc;
777
778          if (lowband && !stereo)
779             next_lowband2 = lowband+N;
780          if (stereo)
781             next_lowband_out1 = lowband_out;
782          else
783             next_level = level+1;
784
785          quant_band(encode, m, i, X, NULL, N, mbits, spread, lowband, resynth, ec, remaining_bits, LM, next_lowband_out1, NULL, next_level);
786          quant_band(encode, m, i, Y, NULL, N, sbits, spread, next_lowband2, resynth, ec, remaining_bits, LM, NULL, NULL, level);
787       }
788
789    } else {
790       /* This is the basic no-split case */
791       q = bits2pulses(m, m->bits[LM][i], N, b);
792       curr_bits = pulses2bits(m->bits[LM][i], N, q);
793       *remaining_bits -= curr_bits;
794
795       /* Ensures we can never bust the budget */
796       while (*remaining_bits < 0 && q > 0)
797       {
798          *remaining_bits += curr_bits;
799          q--;
800          curr_bits = pulses2bits(m->bits[LM][i], N, q);
801          *remaining_bits -= curr_bits;
802       }
803
804       if (encode)
805          alg_quant(X, N, q, spread, lowband, resynth, ec);
806       else
807          alg_unquant(X, N, q, spread, lowband, (ec_dec*)ec);
808    }
809
810    if (resynth)
811    {
812       if (split)
813       {
814          int j;
815          celt_word16 mid, side;
816 #ifdef FIXED_POINT
817          mid = imid;
818          side = iside;
819 #else
820          mid = (1.f/32768)*imid;
821          side = (1.f/32768)*iside;
822 #endif
823          for (j=0;j<N;j++)
824             X[j] = MULT16_16_Q15(X[j], mid);
825          for (j=0;j<N;j++)
826             Y[j] = MULT16_16_Q15(Y[j], side);
827       }
828
829
830       if (!stereo && spread0>1 && level==0)
831       {
832          int k;
833          interleave_vector(X, N_B, spread0);
834          if (lowband)
835             interleave_vector(lowband, N_B, spread0);
836          N_B = N_B0;
837          spread = spread0;
838          for (k=0;k<time_divide;k++)
839          {
840             spread >>= 1;
841             N_B <<= 1;
842             haar1(X, N_B, spread);
843             if (lowband)
844                haar1(lowband, N_B, spread);
845          }
846       }
847
848       if (!stereo && level == 0)
849       {
850          int k;
851          spread = spread0;
852          N_B = N_B0;
853          for (k=0;k<recombine;k++)
854          {
855             haar1(X, N_B, spread);
856             if (lowband)
857                haar1(lowband, N_B, spread);
858             N_B>>=1;
859             spread <<= 1;
860          }
861       }
862
863       if (lowband_out && !stereo)
864       {
865          int j;
866          celt_word16 n;
867          n = celt_sqrt(SHL32(EXTEND32(N0),22));
868          for (j=0;j<N0;j++)
869             lowband_out[j] = MULT16_16_Q15(n,X[j]);
870       }
871
872       if (stereo)
873       {
874          stereo_band_mix(m, X, Y, bandE, 0, i, -1, N);
875          renormalise_vector(X, Q15ONE, N, 1);
876          renormalise_vector(Y, Q15ONE, N, 1);
877       }
878    }
879 }
880
881 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 resynth, int total_bits, ec_enc *ec, int LM)
882 {
883    int i, remaining_bits, balance;
884    const celt_int16 * restrict eBands = m->eBands;
885    celt_norm * restrict norm;
886    VARDECL(celt_norm, _norm);
887    int B;
888    int M;
889    int spread;
890    SAVE_STACK;
891
892    M = 1<<LM;
893    B = shortBlocks ? M : 1;
894    spread = fold ? B : 0;
895    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
896    norm = _norm;
897
898    balance = 0;
899    for (i=start;i<m->nbEBands;i++)
900    {
901       int tell;
902       int b;
903       int N;
904       int curr_balance;
905       celt_norm * restrict X, * restrict Y;
906       celt_norm *lowband;
907       
908       X = _X+M*eBands[i];
909       if (_Y!=NULL)
910          Y = _Y+M*eBands[i];
911       else
912          Y = NULL;
913       N = M*eBands[i+1]-M*eBands[i];
914       if (encode)
915          tell = ec_enc_tell(ec, BITRES);
916       else
917          tell = ec_dec_tell((ec_dec*)ec, BITRES);
918
919       if (i != start)
920          balance -= tell;
921       remaining_bits = (total_bits<<BITRES)-tell-1;
922       curr_balance = (m->nbEBands-i);
923       if (curr_balance > 3)
924          curr_balance = 3;
925       curr_balance = balance / curr_balance;
926       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
927       if (b<0)
928          b = 0;
929
930       if (M*eBands[i]-N >= M*eBands[start])
931          lowband = norm+M*eBands[i]-N;
932       else
933          lowband = NULL;
934       quant_band(encode, m, i, X, Y, N, b, spread, lowband, resynth, ec, &remaining_bits, LM, norm+M*eBands[i], bandE, 0);
935
936       balance += pulses[i] + tell;
937    }
938    RESTORE_STACK;
939 }
940