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