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