bands.c 26.3 KB
Newer Older
Jean-Marc Valin's avatar
Jean-Marc Valin committed
1
2
3
4
/* Copyright (c) 2007-2008 CSIRO
   Copyright (c) 2007-2009 Xiph.Org Foundation
   Copyright (c) 2008-2009 Gregory Maxwell 
   Written by Jean-Marc Valin and Gregory Maxwell */
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
/*
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
   
   - Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
   
   - Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
   
   - Neither the name of the Xiph.org Foundation nor the names of its
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.
   
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

34
35
36
37
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

38
39
#include <math.h>
#include "bands.h"
40
#include "modes.h"
41
#include "vq.h"
Jean-Marc Valin's avatar
Jean-Marc Valin committed
42
#include "cwrs.h"
43
#include "stack_alloc.h"
44
#include "os_support.h"
45
#include "mathops.h"
46
#include "rate.h"
47

48

49
#ifdef FIXED_POINT
50
/* Compute the amplitude (sqrt energy) in each of the bands */
51
void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank, int _C)
52
{
53
   int i, c, N;
54
   const celt_int16 *eBands = m->eBands;
55
   const int C = CHANNELS(_C);
56
   N = FRAMESIZE(m);
57
   for (c=0;c<C;c++)
58
   {
59
60
61
      for (i=0;i<m->nbEBands;i++)
      {
         int j;
62
63
         celt_word32_t maxval=0;
         celt_word32_t sum = 0;
64
65
         
         j=eBands[i]; do {
66
67
            maxval = MAX32(maxval, X[j+c*N]);
            maxval = MAX32(maxval, -X[j+c*N]);
68
69
         } while (++j<eBands[i+1]);
         
70
71
         if (maxval > 0)
         {
72
            int shift = celt_ilog2(maxval)-10;
73
            j=eBands[i]; do {
74
75
               sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
                                   EXTRACT16(VSHR32(X[j+c*N],shift)));
76
            } while (++j<eBands[i+1]);
77
78
            /* We're adding one here to make damn sure we never end up with a pitch vector that's
               larger than unity norm */
79
            bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
80
         } else {
81
            bank[i+c*m->nbEBands] = EPSILON;
82
         }
83
         /*printf ("%f ", bank[i+c*m->nbEBands]);*/
84
85
86
87
88
89
      }
   }
   /*printf ("\n");*/
}

/* Normalise each band such that the energy is one. */
90
void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank, int _C)
91
{
92
   int i, c, N;
93
   const celt_int16 *eBands = m->eBands;
94
   const int C = CHANNELS(_C);
95
   N = FRAMESIZE(m);
96
97
   for (c=0;c<C;c++)
   {
Jean-Marc Valin's avatar
Jean-Marc Valin committed
98
      i=0; do {
99
100
101
         celt_word16_t g;
         int j,shift;
         celt_word16_t E;
102
103
         shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
         E = VSHR32(bank[i+c*m->nbEBands], shift);
104
         g = EXTRACT16(celt_rcp(SHL32(E,3)));
105
         j=eBands[i]; do {
106
            X[j+c*N] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
107
         } while (++j<eBands[i+1]);
Jean-Marc Valin's avatar
Jean-Marc Valin committed
108
      } while (++i<m->nbEBands);
109
110
111
   }
}

112
#else /* FIXED_POINT */
113
/* Compute the amplitude (sqrt energy) in each of the bands */
114
void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank, int _C)
115
{
116
   int i, c, N;
117
   const celt_int16 *eBands = m->eBands;
118
   const int C = CHANNELS(_C);
119
   N = FRAMESIZE(m);
120
121
122
123
124
   for (c=0;c<C;c++)
   {
      for (i=0;i<m->nbEBands;i++)
      {
         int j;
125
         celt_word32_t sum = 1e-10;
126
         for (j=eBands[i];j<eBands[i+1];j++)
127
            sum += X[j+c*N]*X[j+c*N];
128
129
         bank[i+c*m->nbEBands] = sqrt(sum);
         /*printf ("%f ", bank[i+c*m->nbEBands]);*/
130
131
132
133
134
      }
   }
   /*printf ("\n");*/
}

135
#ifdef EXP_PSY
136
void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, celt_ener_t *bank, int _C)
137
{
138
   int i, c, N;
139
   const celt_int16 *eBands = m->eBands;
140
   const int C = CHANNELS(_C);
141
   N = FRAMESIZE(m);
142
143
144
145
146
147
148
   for (c=0;c<C;c++)
   {
      for (i=0;i<m->nbEBands;i++)
      {
         int j;
         celt_word32_t sum = 1e-10;
         for (j=eBands[i];j<eBands[i+1];j++)
149
            sum += X[j*C+c]*X[j+c*N]*tonality[j];
150
151
         bank[i+c*m->nbEBands] = sqrt(sum);
         /*printf ("%f ", bank[i+c*m->nbEBands]);*/
152
153
154
155
156
157
      }
   }
   /*printf ("\n");*/
}
#endif

158
/* Normalise each band such that the energy is one. */
159
void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank, int _C)
160
{
161
   int i, c, N;
162
   const celt_int16 *eBands = m->eBands;
163
   const int C = CHANNELS(_C);
164
   N = FRAMESIZE(m);
165
166
167
168
169
   for (c=0;c<C;c++)
   {
      for (i=0;i<m->nbEBands;i++)
      {
         int j;
170
         celt_word16_t g = 1.f/(1e-10+bank[i+c*m->nbEBands]);
171
         for (j=eBands[i];j<eBands[i+1];j++)
172
            X[j+c*N] = freq[j+c*N]*g;
173
174
175
176
      }
   }
}

177
178
#endif /* FIXED_POINT */

179
void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X, int _C)
180
{
181
   int i, c;
182
   const celt_int16 *eBands = m->eBands;
183
   const int C = CHANNELS(_C);
184
185
186
   for (c=0;c<C;c++)
   {
      i=0; do {
187
         renormalise_vector(X+eBands[i]+c*m->eBands[m->nbEBands+1], Q15ONE, eBands[i+1]-eBands[i], 1);
188
189
      } while (++i<m->nbEBands);
   }
190
191
}

192
/* De-normalise the energy to produce the synthesis from the unit-energy bands */
193
void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank, int _C)
194
{
195
   int i, c, N;
196
   const celt_int16 *eBands = m->eBands;
197
   const int C = CHANNELS(_C);
198
   N = FRAMESIZE(m);
199
200
   if (C>2)
      celt_fatal("denormalise_bands() not implemented for >2 channels");
201
   for (c=0;c<C;c++)
202
   {
203
204
205
      for (i=0;i<m->nbEBands;i++)
      {
         int j;
206
         celt_word32_t g = SHR32(bank[i+c*m->nbEBands],1);
207
         j=eBands[i]; do {
208
            freq[j+c*N] = SHL32(MULT16_32_Q15(X[j+c*N], g),2);
209
         } while (++j<eBands[i+1]);
210
      }
211
212
      for (i=eBands[m->nbEBands];i<eBands[m->nbEBands+1];i++)
         freq[i+c*N] = 0;
213
214
215
   }
}

216
int compute_pitch_gain(const CELTMode *m, const celt_sig_t *X, const celt_sig_t *P, int norm_rate, int *gain_id, int _C, celt_word16_t *gain_prod)
217
{
218
   int j, c;
219
   celt_word16_t g;
220
   celt_word16_t delta;
221
   const int C = CHANNELS(_C);
222
   celt_word32_t Sxy=0, Sxx=0, Syy=0;
223
   int len = m->pitchEnd;
224
   const int N = FRAMESIZE(m);
225
226
227
#ifdef FIXED_POINT
   int shift = 0;
   celt_word32_t maxabs=0;
228
229

   for (c=0;c<C;c++)
230
   {
231
232
233
234
235
      for (j=0;j<len;j++)
      {
         maxabs = MAX32(maxabs, ABS32(X[j+c*N]));
         maxabs = MAX32(maxabs, ABS32(P[j+c*N]));
      }
236
237
238
239
   }
   shift = celt_ilog2(maxabs)-12;
   if (shift<0)
      shift = 0;
240
#endif
241
242
   delta = PDIV32_16(Q15ONE, len);
   for (c=0;c<C;c++)
243
   {
244
245
246
247
      celt_word16_t gg = Q15ONE;
      for (j=0;j<len;j++)
      {
         celt_word16_t Xj, Pj;
248
249
         Xj = EXTRACT16(SHR32(X[j+c*N], shift));
         Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[j+c*N], shift)));
250
251
252
253
254
         Sxy = MAC16_16(Sxy, Xj, Pj);
         Sxx = MAC16_16(Sxx, Pj, Pj);
         Syy = MAC16_16(Syy, Xj, Xj);
         gg = SUB16(gg, delta);
      }
255
   }
256
#ifdef FIXED_POINT
257
258
   {
      celt_word32_t num, den;
259
260
261
262
      celt_word16_t fact;
      fact = MULT16_16(QCONST16(.04, 14), norm_rate);
      if (fact < QCONST16(1., 14))
         fact = QCONST16(1., 14);
263
264
265
266
267
268
      num = Sxy;
      den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03,15),Syy);
      shift = celt_ilog2(Sxy)-16;
      if (shift < 0)
         shift = 0;
      g = DIV32(SHL32(SHR32(num,shift),14),SHR32(den,shift));
269
      if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
270
         g = 0;
271
      /* This MUST round down so that we don't over-estimate the gain */
272
273
      *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
   }
274
#else
275
276
277
278
279
280
281
   {
      float fact = .04*norm_rate;
      if (fact < 1)
         fact = 1;
      g = Sxy/(.1+Sxx+.03*Syy);
      if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
         g = 0;
282
      /* This MUST round down so that we don't over-estimate the gain */
283
284
      *gain_id = floor(20*(g-.5));
   }
285
#endif
286
287
288
289
290
291
292
293
294
295
   /* This prevents the pitch gain from being above 1.0 for too long by bounding the 
      maximum error amplification factor to 2.0 */
   g = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),*gain_id));
   *gain_prod = MAX16(QCONST32(1., 13), MULT16_16_Q14(*gain_prod,g));
   if (*gain_prod>QCONST32(2., 13))
   {
      *gain_id=9;
      *gain_prod = QCONST32(2., 13);
   }

296
297
298
299
300
301
302
303
304
305
306
   if (*gain_id < 0)
   {
      *gain_id = 0;
      return 0;
   } else {
      if (*gain_id > 15)
         *gain_id = 15;
      return 1;
   }
}

307
void apply_pitch(const CELTMode *m, celt_sig_t *X, const celt_sig_t *P, int gain_id, int pred, int _C)
308
{
309
   int j, c, N;
310
   celt_word16_t gain;
311
   celt_word16_t delta;
312
   const int C = CHANNELS(_C);
313
   int len = m->pitchEnd;
314
315

   N = FRAMESIZE(m);
316
   gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
317
   delta = PDIV32_16(gain, len);
318
319
   if (pred)
      gain = -gain;
320
321
322
   else
      delta = -delta;
   for (c=0;c<C;c++)
323
   {
324
325
326
      celt_word16_t gg = gain;
      for (j=0;j<len;j++)
      {
327
         X[j+c*N] += SHL32(MULT16_32_Q15(gg,P[j+c*N]),1);
328
329
         gg = ADD16(gg, delta);
      }
330
331
   }
}
332

333
334
#ifndef DISABLE_STEREO

335
static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, celt_norm_t *Y, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
336
337
{
   int i = bandID;
338
   const celt_int16 *eBands = m->eBands;
339
340
341
   int j;
   celt_word16_t a1, a2;
   if (stereo_mode==0)
342
   {
343
344
345
346
347
348
      /* Do mid-side when not doing intensity stereo */
      a1 = QCONST16(.70711f,14);
      a2 = dir*QCONST16(.70711f,14);
   } else {
      celt_word16_t left, right;
      celt_word16_t norm;
349
#ifdef FIXED_POINT
350
      int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
351
#endif
352
353
354
355
356
      left = VSHR32(bank[i],shift);
      right = VSHR32(bank[i+m->nbEBands],shift);
      norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
      a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
      a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
357
   }
Jean-Marc Valin's avatar
Jean-Marc Valin committed
358
   for (j=0;j<eBands[i+1]-eBands[i];j++)
359
360
   {
      celt_norm_t r, l;
361
362
363
364
      l = X[j];
      r = Y[j];
      X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
      Y[j] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
365
366
367
   }
}

368

369
370
#endif /* DISABLE_STEREO */

371
int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision, int _C)
372
{
373
   int i, c, N0;
374
375
   int NR=0;
   celt_word32_t ratio = EPSILON;
376
   const int C = CHANNELS(_C);
377
   const celt_int16 * restrict eBands = m->eBands;
378
379
380
   
   N0 = FRAMESIZE(m);

381
382
   for (c=0;c<C;c++)
   {
383
384
385
386
387
388
   for (i=0;i<m->nbEBands;i++)
   {
      int j, N;
      int max_i=0;
      celt_word16_t max_val=EPSILON;
      celt_word32_t floor_ener=EPSILON;
389
      celt_norm_t * restrict x = X+eBands[i]+c*N0;
390
391
392
      N = eBands[i+1]-eBands[i];
      for (j=0;j<N;j++)
      {
393
         if (ABS16(x[j])>max_val)
394
         {
395
            max_val = ABS16(x[j]);
396
397
398
399
400
401
402
403
404
405
406
407
            max_i = j;
         }
      }
#if 0
      for (j=0;j<N;j++)
      {
         if (abs(j-max_i)>2)
            floor_ener += x[j]*x[j];
      }
#else
      floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
      if (max_i < N-1)
408
         floor_ener -= MULT16_16(x[(max_i+1)], x[(max_i+1)]);
409
      if (max_i < N-2)
410
         floor_ener -= MULT16_16(x[(max_i+2)], x[(max_i+2)]);
411
      if (max_i > 0)
412
         floor_ener -= MULT16_16(x[(max_i-1)], x[(max_i-1)]);
413
      if (max_i > 1)
414
         floor_ener -= MULT16_16(x[(max_i-2)], x[(max_i-2)]);
415
416
      floor_ener = MAX32(floor_ener, EPSILON);
#endif
417
      if (N>7)
418
419
420
421
422
423
424
425
426
      {
         celt_word16_t r;
         celt_word16_t den = celt_sqrt(floor_ener);
         den = MAX32(QCONST16(.02, 15), den);
         r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
         ratio = ADD32(ratio, EXTEND32(r));
         NR++;
      }
   }
427
   }
428
429
430
431
432
433
434
435
436
437
438
439
440
   if (NR>0)
      ratio = DIV32_16(ratio, NR);
   ratio = ADD32(HALF32(ratio), HALF32(*average));
   if (!*last_decision)
   {
      *last_decision = (ratio < QCONST16(1.8,8));
   } else {
      *last_decision = (ratio < QCONST16(3.,8));
   }
   *average = EXTRACT16(ratio);
   return *last_decision;
}

441
/* Quantisation of the residual */
442
void quant_bands(const CELTMode *m, celt_norm_t * restrict X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, int encode, void *enc_dec)
443
{
444
   int i, j, remaining_bits, balance;
445
   const celt_int16 * restrict eBands = m->eBands;
446
   celt_norm_t * restrict norm;
447
   VARDECL(celt_norm_t, _norm);
448
   int B;
449
450
   SAVE_STACK;

451
   B = shortBlocks ? m->nbShortMdcts : 1;
452
   ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
453
   norm = _norm;
454

455
   balance = 0;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
456
   for (i=0;i<m->nbEBands;i++)
457
   {
458
      int tell;
459
      int N;
460
      int q;
461
      celt_word16_t n;
462
      const celt_int16 * const *BPbits;
463
464
465
      
      int curr_balance, curr_bits;
      
466
      N = eBands[i+1]-eBands[i];
467
      BPbits = m->bits;
468

469
470
471
472
      if (encode)
         tell = ec_enc_tell(enc_dec, BITRES);
      else
         tell = ec_dec_tell(enc_dec, BITRES);
473
474
      if (i != 0)
         balance -= tell;
475
      remaining_bits = (total_bits<<BITRES)-tell-1;
476
477
478
479
      curr_balance = (m->nbEBands-i);
      if (curr_balance > 3)
         curr_balance = 3;
      curr_balance = balance / curr_balance;
480
481
      q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
      curr_bits = pulses2bits(BPbits[i], N, q);
482
      remaining_bits -= curr_bits;
483
      while (remaining_bits < 0 && q > 0)
484
485
      {
         remaining_bits += curr_bits;
486
         q--;
487
         curr_bits = pulses2bits(BPbits[i], N, q);
488
         remaining_bits -= curr_bits;
489
490
491
      }
      balance += pulses[i] + tell;
      
492
      n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
493

494
      if (q > 0)
495
      {
496
         int spread = fold ? B : 0;
497
498
499
500
         if (encode)
            alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc_dec);
         else
            alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc_dec);
501
      } else {
502
         intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
503
      }
504
      for (j=eBands[i];j<eBands[i+1];j++)
505
         norm[j] = MULT16_16_Q15(n,X[j]);
506
   }
507
   RESTORE_STACK;
508
509
}

510
511
#ifndef DISABLE_STEREO

512
void quant_bands_stereo(const CELTMode *m, celt_norm_t *_X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
513
514
{
   int i, j, remaining_bits, balance;
515
   const celt_int16 * restrict eBands = m->eBands;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
516
   celt_norm_t * restrict norm;
517
518
519
520
521
522
   VARDECL(celt_norm_t, _norm);
   int B;
   celt_word16_t mid, side;
   SAVE_STACK;

   B = shortBlocks ? m->nbShortMdcts : 1;
523
   ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
524
525
526
527
528
   norm = _norm;

   balance = 0;
   for (i=0;i<m->nbEBands;i++)
   {
529
      int c;
530
531
532
      int tell;
      int q1, q2;
      celt_word16_t n;
533
      const celt_int16 * const *BPbits;
534
535
536
537
538
539
      int b, qb;
      int N;
      int curr_balance, curr_bits;
      int imid, iside, itheta;
      int mbits, sbits, delta;
      int qalloc;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
540
      celt_norm_t * restrict X, * restrict Y;
541
      
Jean-Marc Valin's avatar
Jean-Marc Valin committed
542
543
      X = _X+eBands[i];
      Y = X+eBands[m->nbEBands+1];
544
545
546
      BPbits = m->bits;

      N = eBands[i+1]-eBands[i];
547
      tell = ec_enc_tell(enc, BITRES);
548
549
550
551
552
553
554
      if (i != 0)
         balance -= tell;
      remaining_bits = (total_bits<<BITRES)-tell-1;
      curr_balance = (m->nbEBands-i);
      if (curr_balance > 3)
         curr_balance = 3;
      curr_balance = balance / curr_balance;
555
      b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
556
557
558
      if (b<0)
         b = 0;

559
      qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
560
561
562
563
      if (qb > (b>>BITRES)-1)
         qb = (b>>BITRES)-1;
      if (qb<0)
         qb = 0;
564
565
      if (qb>14)
         qb = 14;
566

567
      stereo_band_mix(m, X, Y, bandE, qb==0, i, 1);
568

Jean-Marc Valin's avatar
Jean-Marc Valin committed
569
570
      mid = renormalise_vector(X, Q15ONE, N, 1);
      side = renormalise_vector(Y, Q15ONE, N, 1);
571
572
573
574
575
#ifdef FIXED_POINT
      itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
#else
      itheta = floor(.5+16384*0.63662*atan2(side,mid));
#endif
576
      qalloc = log2_frac((1<<qb)+1,BITRES);
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
      if (qb==0)
      {
         itheta=0;
      } else {
         int shift;
         shift = 14-qb;
         itheta = (itheta+(1<<shift>>1))>>shift;
         ec_enc_uint(enc, itheta, (1<<qb)+1);
         itheta <<= shift;
      }
      if (itheta == 0)
      {
         imid = 32767;
         iside = 0;
         delta = -10000;
      } else if (itheta == 16384)
      {
         imid = 0;
         iside = 32767;
         delta = 10000;
      } else {
         imid = bitexact_cos(itheta);
         iside = bitexact_cos(16384-itheta);
600
         delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
601
      }
602
603
604
605
606
607
608
      n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);

      if (N==2)
      {
         int c2;
         int sign=1;
         celt_norm_t v[2], w[2];
609
         celt_norm_t *x2, *y2;
610
611
612
613
614
615
616
617
         mbits = b-qalloc;
         sbits = 0;
         if (itheta != 0 && itheta != 16384)
            sbits = 1<<BITRES;
         mbits -= sbits;
         c = itheta > 8192 ? 1 : 0;
         c2 = 1-c;

Jean-Marc Valin's avatar
Jean-Marc Valin committed
618
619
         x2 = X;
         y2 = Y;
620
621
622
623
624
625
626
627
628
629
630
631
         if (c==0)
         {
            v[0] = x2[0];
            v[1] = x2[1];
            w[0] = y2[0];
            w[1] = y2[1];
         } else {
            v[0] = y2[0];
            v[1] = y2[1];
            w[0] = x2[0];
            w[1] = x2[1];
         }
632
633
634
635
636
637
638
639
640
641
642
643
         q1 = bits2pulses(m, BPbits[i], N, mbits);
         curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
         remaining_bits -= curr_bits;
         while (remaining_bits < 0 && q1 > 0)
         {
            remaining_bits += curr_bits;
            q1--;
            curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
            remaining_bits -= curr_bits;
         }

         if (q1 > 0)
644
         {
645
            int spread = fold ? B : 0;
646
            alg_quant(v, N, q1, spread, enc);
647
         } else {
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
            v[0] = QCONST16(1.f, 14);
            v[1] = 0;
         }
         if (sbits)
         {
            if (v[0]*w[1] - v[1]*w[0] > 0)
               sign = 1;
            else
               sign = -1;
            ec_enc_bits(enc, sign==1, 1);
         } else {
            sign = 1;
         }
         w[0] = -sign*v[1];
         w[1] = sign*v[0];
         if (c==0)
         {
            x2[0] = v[0];
            x2[1] = v[1];
667
668
            y2[0] = w[0];
            y2[1] = w[1];
669
670
671
         } else {
            x2[0] = w[0];
            x2[1] = w[1];
672
673
            y2[0] = v[0];
            y2[1] = v[1];
674
675
676
         }
      } else {
         
677
678
679
680
681
682
683
684
685
686
687
         mbits = (b-qalloc/2-delta)/2;
         if (mbits > b-qalloc)
            mbits = b-qalloc;
         if (mbits<0)
            mbits=0;
         sbits = b-qalloc-mbits;
         q1 = bits2pulses(m, BPbits[i], N, mbits);
         q2 = bits2pulses(m, BPbits[i], N, sbits);
         curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
         remaining_bits -= curr_bits;
         while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
688
         {
689
690
691
692
693
694
695
696
697
698
            remaining_bits += curr_bits;
            if (q1>q2)
            {
               q1--;
               curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
            } else {
               q2--;
               curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
            }
            remaining_bits -= curr_bits;
699
700
         }

701
702
         if (q1 > 0) {
            int spread = fold ? B : 0;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
703
            alg_quant(X, N, q1, spread, enc);
704
         } else {
Jean-Marc Valin's avatar
Jean-Marc Valin committed
705
            intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
706
707
708
         }
         if (q2 > 0) {
            int spread = fold ? B : 0;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
709
            alg_quant(Y, N, q2, spread, enc);
710
         } else
Jean-Marc Valin's avatar
Jean-Marc Valin committed
711
            for (j=0;j<N;j++)
712
               Y[j] = 0;
713
714
715
      }
      
      balance += pulses[i] + tell;
716
717
718
719
720
721
722
723

#ifdef FIXED_POINT
      mid = imid;
      side = iside;
#else
      mid = (1./32768)*imid;
      side = (1./32768)*iside;
#endif
724
      for (j=0;j<N;j++)
Jean-Marc Valin's avatar
Jean-Marc Valin committed
725
         norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
726

727
      for (j=0;j<N;j++)
Jean-Marc Valin's avatar
Jean-Marc Valin committed
728
         X[j] = MULT16_16_Q15(X[j], mid);
729
      for (j=0;j<N;j++)
Jean-Marc Valin's avatar
Jean-Marc Valin committed
730
         Y[j] = MULT16_16_Q15(Y[j], side);
731

732
      stereo_band_mix(m, X, Y, bandE, 0, i, -1);
Jean-Marc Valin's avatar
Jean-Marc Valin committed
733
734
      renormalise_vector(X, Q15ONE, N, 1);
      renormalise_vector(Y, Q15ONE, N, 1);
735
736
737
   }
   RESTORE_STACK;
}
738
#endif /* DISABLE_STEREO */
739

740

741
742
#ifndef DISABLE_STEREO

743
void unquant_bands_stereo(const CELTMode *m, celt_norm_t *_X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
744
745
{
   int i, j, remaining_bits, balance;
746
   const celt_int16 * restrict eBands = m->eBands;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
747
   celt_norm_t * restrict norm;
748
749
750
751
752
753
   VARDECL(celt_norm_t, _norm);
   int B;
   celt_word16_t mid, side;
   SAVE_STACK;

   B = shortBlocks ? m->nbShortMdcts : 1;
754
   ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
755
756
757
758
759
   norm = _norm;

   balance = 0;
   for (i=0;i<m->nbEBands;i++)
   {
760
      int c;
761
762
763
      int tell;
      int q1, q2;
      celt_word16_t n;
764
      const celt_int16 * const *BPbits;
765
766
767
768
769
770
      int b, qb;
      int N;
      int curr_balance, curr_bits;
      int imid, iside, itheta;
      int mbits, sbits, delta;
      int qalloc;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
771
      celt_norm_t * restrict X, * restrict Y;
772
      
Jean-Marc Valin's avatar
Jean-Marc Valin committed
773
774
      X = _X+eBands[i];
      Y = X+eBands[m->nbEBands+1];
775
776
777
      BPbits = m->bits;

      N = eBands[i+1]-eBands[i];
778
      tell = ec_dec_tell(dec, BITRES);
779
780
781
782
783
784
785
      if (i != 0)
         balance -= tell;
      remaining_bits = (total_bits<<BITRES)-tell-1;
      curr_balance = (m->nbEBands-i);
      if (curr_balance > 3)
         curr_balance = 3;
      curr_balance = balance / curr_balance;
786
      b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
787
788
789
      if (b<0)
         b = 0;

790
      qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
791
792
      if (qb > (b>>BITRES)-1)
         qb = (b>>BITRES)-1;
793
794
      if (qb>14)
         qb = 14;
795
796
      if (qb<0)
         qb = 0;
797
      qalloc = log2_frac((1<<qb)+1,BITRES);
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
      if (qb==0)
      {
         itheta=0;
      } else {
         int shift;
         shift = 14-qb;
         itheta = ec_dec_uint(dec, (1<<qb)+1);
         itheta <<= shift;
      }
      if (itheta == 0)
      {
         imid = 32767;
         iside = 0;
         delta = -10000;
      } else if (itheta == 16384)
      {
         imid = 0;
         iside = 32767;
         delta = 10000;
      } else {
         imid = bitexact_cos(itheta);
         iside = bitexact_cos(16384-itheta);
820
         delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
821
      }
822
823
824
825
826
827
828
      n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);

      if (N==2)
      {
         int c2;
         int sign=1;
         celt_norm_t v[2], w[2];
829
         celt_norm_t *x2, *y2;
830
831
832
833
834
835
836
837
         mbits = b-qalloc;
         sbits = 0;
         if (itheta != 0 && itheta != 16384)
            sbits = 1<<BITRES;
         mbits -= sbits;
         c = itheta > 8192 ? 1 : 0;
         c2 = 1-c;

Jean-Marc Valin's avatar
Jean-Marc Valin committed
838
839
         x2 = X;
         y2 = Y;
840
         v[0] = x2[c];
841
         v[1] = y2[c];
842
         w[0] = x2[c2];
843
         w[1] = y2[c2];
844
845
846
847
848
849
850
851
852
853
854
855
         q1 = bits2pulses(m, BPbits[i], N, mbits);
         curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
         remaining_bits -= curr_bits;
         while (remaining_bits < 0 && q1 > 0)
         {
            remaining_bits += curr_bits;
            q1--;
            curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
            remaining_bits -= curr_bits;
         }

         if (q1 > 0)
856
         {
857
            int spread = fold ? B : 0;
858
            alg_unquant(v, N, q1, spread, dec);
859
         } else {
860
861
862
863
864
865
866
867
868
869
870
871
872
            v[0] = QCONST16(1.f, 14);
            v[1] = 0;
         }
         if (sbits)
            sign = 2*ec_dec_bits(dec, 1)-1;
         else
            sign = 1;
         w[0] = -sign*v[1];
         w[1] = sign*v[0];
         if (c==0)
         {
            x2[0] = v[0];
            x2[1] = v[1];
873
874
            y2[0] = w[0];
            y2[1] = w[1];
875
876
877
         } else {
            x2[0] = w[0];
            x2[1] = w[1];
878
879
            y2[0] = v[0];
            y2[1] = v[1];
880
881
         }
      } else {
882
883
884
885
886
887
888
889
890
891
892
         mbits = (b-qalloc/2-delta)/2;
         if (mbits > b-qalloc)
            mbits = b-qalloc;
         if (mbits<0)
            mbits=0;
         sbits = b-qalloc-mbits;
         q1 = bits2pulses(m, BPbits[i], N, mbits);
         q2 = bits2pulses(m, BPbits[i], N, sbits);
         curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
         remaining_bits -= curr_bits;
         while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
893
         {
894
895
896
897
898
899
900
901
902
903
            remaining_bits += curr_bits;
            if (q1>q2)
            {
               q1--;
               curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
            } else {
               q2--;
               curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
            }
            remaining_bits -= curr_bits;
904
         }
905
906
907
908
         
         if (q1 > 0)
         {
            int spread = fold ? B : 0;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
909
            alg_unquant(X, N, q1, spread, dec);
910
         } else
Jean-Marc Valin's avatar
Jean-Marc Valin committed
911
            intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
912
913
914
         if (q2 > 0)
         {
            int spread = fold ? B : 0;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
915
            alg_unquant(Y, N, q2, spread, dec);
916
         } else
Jean-Marc Valin's avatar
Jean-Marc Valin committed
917
            for (j=0;j<N;j++)
918
919
               Y[j] = 0;
            /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
920
921
      }
      balance += pulses[i] + tell;
922
      
923
924
925
926
927
928
929
#ifdef FIXED_POINT
      mid = imid;
      side = iside;
#else
      mid = (1./32768)*imid;
      side = (1./32768)*iside;
#endif
930
      for (j=0;j<N;j++)
Jean-Marc Valin's avatar
Jean-Marc Valin committed
931
         norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
932

933
      for (j=0;j<N;j++)
Jean-Marc Valin's avatar
Jean-Marc Valin committed
934
         X[j] = MULT16_16_Q15(X[j], mid);
935
      for (j=0;j<N;j++)
Jean-Marc Valin's avatar
Jean-Marc Valin committed
936
         Y[j] = MULT16_16_Q15(Y[j], side);
937

938
      stereo_band_mix(m, X, Y, bandE, 0, i, -1);
Jean-Marc Valin's avatar
Jean-Marc Valin committed
939
940
      renormalise_vector(X, Q15ONE, N, 1);
      renormalise_vector(Y, Q15ONE, N, 1);
941
942
943
   }
   RESTORE_STACK;
}