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