Fixes the recombining stride and the deinterleaving stride
[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          if (encode)
577             haar1(X, N>>k, 1<<k);
578          if (lowband)
579             haar1(lowband, N>>k, 1<<k);
580       }
581       B>>=recombine;
582       N_B<<=recombine;
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>>recombine, B0<<recombine, longBlocks);
604          if (lowband)
605             deinterleave_hadamard(lowband, N_B>>recombine, B0<<recombine, 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       celt_int32 tell;
630
631       /* Decide on the resolution to give to the split parameter theta */
632       offset = ((m->logN[i]+(LM<<BITRES))>>1) - (stereo ? QTHETA_OFFSET_STEREO : QTHETA_OFFSET);
633       qn = compute_qn(N, b, offset, stereo);
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       tell = encode ? ec_enc_tell(ec, BITRES) : ec_dec_tell(ec, BITRES);
645       if (qn!=1)
646       {
647          if (encode)
648             itheta = (itheta*qn+8192)>>14;
649
650          /* Entropy coding of the angle. We use a uniform pdf for the
651             first stereo split but a triangular one for the rest. */
652          if (stereo || B0>1)
653          {
654             if (encode)
655                ec_enc_uint((ec_enc*)ec, itheta, qn+1);
656             else
657                itheta = ec_dec_uint((ec_dec*)ec, qn+1);
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          }
692          itheta = (celt_int32)itheta*16384/qn;
693          if (encode && stereo)
694          {
695             if (itheta==0)
696                intensity_stereo(m, X, Y, bandE, i, N);
697             else
698                stereo_split(X, Y, N);
699          }
700          /* TODO: Renormalising X and Y *may* help fixed-point a bit at very high rate.
701                   Let's do that at higher complexity */
702       } else if (stereo) {
703          if (encode)
704          {
705             inv = itheta > 8192;
706             if (inv)
707             {
708                int j;
709                for (j=0;j<N;j++)
710                   Y[j] = -Y[j];
711             }
712             intensity_stereo(m, X, Y, bandE, i, N);
713          }
714          if (b>2<<BITRES && *remaining_bits > 2<<BITRES)
715          {
716             if (encode)
717                ec_enc_bit_logp(ec, inv, 2);
718             else
719                inv = ec_dec_bit_logp(ec, 2);
720          } else
721             inv = 0;
722          itheta = 0;
723       }
724       qalloc = (encode ? ec_enc_tell(ec, BITRES) : ec_dec_tell(ec, BITRES))
725                - tell;
726
727       if (itheta == 0)
728       {
729          imid = 32767;
730          iside = 0;
731          delta = -16384;
732       } else if (itheta == 16384)
733       {
734          imid = 0;
735          iside = 32767;
736          delta = 16384;
737       } else {
738          imid = bitexact_cos(itheta);
739          iside = bitexact_cos(16384-itheta);
740          /* This is the mid vs side allocation that minimizes squared error
741             in that band. */
742          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
743       }
744
745 #ifdef FIXED_POINT
746       mid = imid;
747       side = iside;
748 #else
749       mid = (1.f/32768)*imid;
750       side = (1.f/32768)*iside;
751 #endif
752
753       /* This is a special case for N=2 that only works for stereo and takes
754          advantage of the fact that mid and side are orthogonal to encode
755          the side with just one bit. */
756       if (N==2 && stereo)
757       {
758          int c;
759          int sign=0;
760          celt_norm *x2, *y2;
761          mbits = b-qalloc;
762          sbits = 0;
763          /* Only need one bit for the side */
764          if (itheta != 0 && itheta != 16384)
765             sbits = 1<<BITRES;
766          mbits -= sbits;
767          c = itheta > 8192;
768          *remaining_bits -= qalloc+sbits;
769
770          x2 = c ? Y : X;
771          y2 = c ? X : Y;
772          if (sbits)
773          {
774             if (encode)
775             {
776                /* Here we only need to encode a sign for the side */
777                sign = x2[0]*y2[1] - x2[1]*y2[0] < 0;
778                ec_enc_bits((ec_enc*)ec, sign, 1);
779             } else {
780                sign = ec_dec_bits((ec_dec*)ec, 1);
781             }
782          }
783          sign = 1-2*sign;
784          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);
785          y2[0] = -sign*x2[1];
786          y2[1] = sign*x2[0];
787          if (resynth)
788          {
789             celt_norm tmp;
790             X[0] = MULT16_16_Q15(mid, X[0]);
791             X[1] = MULT16_16_Q15(mid, X[1]);
792             Y[0] = MULT16_16_Q15(side, Y[0]);
793             Y[1] = MULT16_16_Q15(side, Y[1]);
794             tmp = X[0];
795             X[0] = SUB16(tmp,Y[0]);
796             Y[0] = ADD16(tmp,Y[0]);
797             tmp = X[1];
798             X[1] = SUB16(tmp,Y[1]);
799             Y[1] = ADD16(tmp,Y[1]);
800          }
801       } else {
802          /* "Normal" split code */
803          celt_norm *next_lowband2=NULL;
804          celt_norm *next_lowband_out1=NULL;
805          int next_level=0;
806
807          /* Give more bits to low-energy MDCTs than they would otherwise deserve */
808          if (B0>1 && !stereo)
809          {
810             if (itheta > 8192)
811                /* Rough approximation for pre-echo masking */
812                delta -= delta>>(4-LM);
813             else
814                /* Corresponds to a forward-masking slope of 1.5 dB per 10 ms */
815                delta = IMIN(0, delta + (N<<BITRES>>(5-LM)));
816          }
817          mbits = (b-qalloc-delta)/2;
818          if (mbits > b-qalloc)
819             mbits = b-qalloc;
820          if (mbits<0)
821             mbits=0;
822          sbits = b-qalloc-mbits;
823          *remaining_bits -= qalloc;
824
825          if (lowband && !stereo)
826             next_lowband2 = lowband+N; /* >32-bit split case */
827
828          /* Only stereo needs to pass on lowband_out. Otherwise, it's
829             handled at the end */
830          if (stereo)
831             next_lowband_out1 = lowband_out;
832          else
833             next_level = level+1;
834
835          quant_band(encode, m, i, X, NULL, N, mbits, spread, B, intensity, tf_change,
836                lowband, resynth, ec, remaining_bits, LM, next_lowband_out1,
837                NULL, next_level, seed, MULT16_16_P15(gain,mid), lowband_scratch, fill);
838          quant_band(encode, m, i, Y, NULL, N, sbits, spread, B, intensity, tf_change,
839                next_lowband2, resynth, ec, remaining_bits, LM, NULL,
840                NULL, next_level, seed, MULT16_16_P15(gain,side), NULL, fill && !stereo);
841       }
842
843    } else {
844       /* This is the basic no-split case */
845       q = bits2pulses(m, i, LM, b);
846       curr_bits = pulses2bits(m, i, LM, q);
847       *remaining_bits -= curr_bits;
848
849       /* Ensures we can never bust the budget */
850       while (*remaining_bits < 0 && q > 0)
851       {
852          *remaining_bits += curr_bits;
853          q--;
854          curr_bits = pulses2bits(m, i, LM, q);
855          *remaining_bits -= curr_bits;
856       }
857
858       if (q!=0)
859       {
860          int K = get_pulses(q);
861
862          /* Finally do the actual quantization */
863          if (encode)
864             alg_quant(X, N, K, spread, B, lowband, resynth, (ec_enc*)ec, seed, gain);
865          else
866             alg_unquant(X, N, K, spread, B, lowband, (ec_dec*)ec, seed, gain);
867       } else {
868          /* If there's no pulse, fill the band anyway */
869          int j;
870          if (resynth)
871          {
872             if (!fill)
873             {
874                for (j=0;j<N;j++)
875                   X[j] = 0;
876             } else {
877                if (lowband == NULL || (spread==SPREAD_AGGRESSIVE && B<=1))
878                {
879                   /* Noise */
880                   for (j=0;j<N;j++)
881                   {
882                      *seed = lcg_rand(*seed);
883                      X[j] = (celt_int32)(*seed)>>20;
884                   }
885                } else {
886                   /* Folded spectrum */
887                   for (j=0;j<N;j++)
888                      X[j] = lowband[j];
889                }
890                renormalise_vector(X, N, gain);
891             }
892          }
893       }
894    }
895
896    /* This code is used by the decoder and by the resynthesis-enabled encoder */
897    if (resynth)
898    {
899       if (stereo)
900       {
901          if (N!=2)
902             stereo_merge(X, Y, mid, N);
903          if (inv)
904          {
905             int j;
906             for (j=0;j<N;j++)
907                Y[j] = -Y[j];
908          }
909       } else if (level == 0)
910       {
911          int k;
912
913          /* Undo the sample reorganization going from time order to frequency order */
914          if (B0>1)
915             interleave_hadamard(X, N_B>>recombine, B0<<recombine, longBlocks);
916
917          /* Undo time-freq changes that we did earlier */
918          N_B = N_B0;
919          B = B0;
920          for (k=0;k<time_divide;k++)
921          {
922             B >>= 1;
923             N_B <<= 1;
924             haar1(X, N_B, B);
925          }
926
927          for (k=0;k<recombine;k++)
928             haar1(X, N0>>k, 1<<k);
929          B<<=recombine;
930          N_B>>=recombine;
931
932          /* Scale output for later folding */
933          if (lowband_out)
934          {
935             int j;
936             celt_word16 n;
937             n = celt_sqrt(SHL32(EXTEND32(N0),22));
938             for (j=0;j<N0;j++)
939                lowband_out[j] = MULT16_16_Q15(n,X[j]);
940          }
941       }
942    }
943 }
944
945 void quant_all_bands(int encode, const CELTMode *m, int start, int end,
946       celt_norm *_X, celt_norm *_Y, const celt_ener *bandE, int *pulses,
947       int shortBlocks, int spread, int dual_stereo, int intensity, int *tf_res, int resynth,
948       int total_bits, void *ec, int LM, int codedBands)
949 {
950    int i;
951    celt_int32 balance;
952    celt_int32 remaining_bits;
953    const celt_int16 * restrict eBands = m->eBands;
954    celt_norm * restrict norm, * restrict norm2;
955    VARDECL(celt_norm, _norm);
956    VARDECL(celt_norm, lowband_scratch);
957    int B;
958    int M;
959    celt_int32 seed;
960    int lowband_offset;
961    int update_lowband = 1;
962    int C = _Y != NULL ? 2 : 1;
963    SAVE_STACK;
964
965    M = 1<<LM;
966    B = shortBlocks ? M : 1;
967    ALLOC(_norm, C*M*eBands[m->nbEBands], celt_norm);
968    ALLOC(lowband_scratch, M*(eBands[m->nbEBands]-eBands[m->nbEBands-1]), celt_norm);
969    norm = _norm;
970    norm2 = norm + M*eBands[m->nbEBands];
971
972    if (encode)
973       seed = ((ec_enc*)ec)->rng;
974    else
975       seed = ((ec_dec*)ec)->rng;
976    balance = 0;
977    lowband_offset = -1;
978    for (i=start;i<end;i++)
979    {
980       celt_int32 tell;
981       int b;
982       int N;
983       celt_int32 curr_balance;
984       int effective_lowband=-1;
985       celt_norm * restrict X, * restrict Y;
986       int tf_change=0;
987       
988       X = _X+M*eBands[i];
989       if (_Y!=NULL)
990          Y = _Y+M*eBands[i];
991       else
992          Y = NULL;
993       N = M*eBands[i+1]-M*eBands[i];
994       if (encode)
995          tell = ec_enc_tell((ec_enc*)ec, BITRES);
996       else
997          tell = ec_dec_tell((ec_dec*)ec, BITRES);
998
999       /* Compute how many bits we want to allocate to this band */
1000       if (i != start)
1001          balance -= tell;
1002       remaining_bits = ((celt_int32)total_bits<<BITRES)-tell-1;
1003       if (i <= codedBands-1)
1004       {
1005          curr_balance = (codedBands-i);
1006          if (curr_balance > 3)
1007             curr_balance = 3;
1008          curr_balance = balance / curr_balance;
1009          b = IMIN(16384, IMIN(remaining_bits+1,pulses[i]+curr_balance));
1010          if (b<0)
1011             b = 0;
1012       } else {
1013          b = 0;
1014       }
1015
1016       if (M*eBands[i]-N >= M*eBands[start] && (update_lowband || lowband_offset==-1))
1017             lowband_offset = M*eBands[i];
1018
1019       tf_change = tf_res[i];
1020       if (i>=m->effEBands)
1021       {
1022          X=norm;
1023          if (_Y!=NULL)
1024             Y = norm;
1025       }
1026
1027       /* This ensures we never repeat spectral content within one band */
1028       if (lowband_offset != -1)
1029       {
1030          effective_lowband = lowband_offset-N;
1031          if (effective_lowband < M*eBands[start])
1032             effective_lowband = M*eBands[start];
1033       }
1034       if (dual_stereo && i==intensity)
1035       {
1036          int j;
1037
1038          /* Switch off dual stereo to do intensity */
1039          dual_stereo = 0;
1040          for (j=M*eBands[start];j<M*eBands[i];j++)
1041             norm[j] = HALF32(norm[j]+norm2[j]);
1042       }
1043       if (dual_stereo)
1044       {
1045          quant_band(encode, m, i, X, NULL, N, b/2, spread, B, intensity, tf_change,
1046                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1047                norm+M*eBands[i], bandE, 0, &seed, Q15ONE, lowband_scratch, 1);
1048          quant_band(encode, m, i, Y, NULL, N, b/2, spread, B, intensity, tf_change,
1049                effective_lowband != -1 ? norm2+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1050                norm2+M*eBands[i], bandE, 0, &seed, Q15ONE, lowband_scratch, 1);
1051       } else {
1052          quant_band(encode, m, i, X, Y, N, b, spread, B, intensity, tf_change,
1053                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1054                norm+M*eBands[i], bandE, 0, &seed, Q15ONE, lowband_scratch, 1);
1055       }
1056       balance += pulses[i] + tell;
1057
1058       /* Update the folding position only as long as we have 1 bit/sample depth */
1059       update_lowband = (b>>BITRES)>N;
1060    }
1061    RESTORE_STACK;
1062 }
1063