simplifications to the stereo code
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 Jean-Marc Valin, CSIRO
2    (C) 2008-2009 Gregory Maxwell */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "bands.h"
38 #include "modes.h"
39 #include "vq.h"
40 #include "cwrs.h"
41 #include "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44 #include "rate.h"
45
46 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
47
48
49
50 #ifdef FIXED_POINT
51 /* Compute the amplitude (sqrt energy) in each of the bands */
52 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
53 {
54    int i, c, N;
55    const celt_int16_t *eBands = m->eBands;
56    const int C = CHANNELS(m);
57    N = FRAMESIZE(m);
58    for (c=0;c<C;c++)
59    {
60       for (i=0;i<m->nbEBands;i++)
61       {
62          int j;
63          celt_word32_t maxval=0;
64          celt_word32_t sum = 0;
65          
66          j=eBands[i]; do {
67             maxval = MAX32(maxval, X[j+c*N]);
68             maxval = MAX32(maxval, -X[j+c*N]);
69          } while (++j<eBands[i+1]);
70          
71          if (maxval > 0)
72          {
73             int shift = celt_ilog2(maxval)-10;
74             j=eBands[i]; do {
75                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
76                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
77             } while (++j<eBands[i+1]);
78             /* We're adding one here to make damn sure we never end up with a pitch vector that's
79                larger than unity norm */
80             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
81          } else {
82             bank[i+c*m->nbEBands] = EPSILON;
83          }
84          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
85       }
86    }
87    /*printf ("\n");*/
88 }
89
90 /* Normalise each band such that the energy is one. */
91 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
92 {
93    int i, c, N;
94    const celt_int16_t *eBands = m->eBands;
95    const int C = CHANNELS(m);
96    N = FRAMESIZE(m);
97    for (c=0;c<C;c++)
98    {
99       i=0; do {
100          celt_word16_t g;
101          int j,shift;
102          celt_word16_t E;
103          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
104          E = VSHR32(bank[i+c*m->nbEBands], shift);
105          g = EXTRACT16(celt_rcp(SHL32(E,3)));
106          j=eBands[i]; do {
107             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
108          } while (++j<eBands[i+1]);
109       } while (++i<m->nbEBands);
110    }
111 }
112
113 #else /* FIXED_POINT */
114 /* Compute the amplitude (sqrt energy) in each of the bands */
115 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
116 {
117    int i, c, N;
118    const celt_int16_t *eBands = m->eBands;
119    const int C = CHANNELS(m);
120    N = FRAMESIZE(m);
121    for (c=0;c<C;c++)
122    {
123       for (i=0;i<m->nbEBands;i++)
124       {
125          int j;
126          celt_word32_t sum = 1e-10;
127          for (j=eBands[i];j<eBands[i+1];j++)
128             sum += X[j+c*N]*X[j+c*N];
129          bank[i+c*m->nbEBands] = sqrt(sum);
130          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
131       }
132    }
133    /*printf ("\n");*/
134 }
135
136 #ifdef EXP_PSY
137 void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, celt_ener_t *bank)
138 {
139    int i, c, N;
140    const celt_int16_t *eBands = m->eBands;
141    const int C = CHANNELS(m);
142    N = FRAMESIZE(m);
143    for (c=0;c<C;c++)
144    {
145       for (i=0;i<m->nbEBands;i++)
146       {
147          int j;
148          celt_word32_t sum = 1e-10;
149          for (j=eBands[i];j<eBands[i+1];j++)
150             sum += X[j*C+c]*X[j+c*N]*tonality[j];
151          bank[i+c*m->nbEBands] = sqrt(sum);
152          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
153       }
154    }
155    /*printf ("\n");*/
156 }
157 #endif
158
159 /* Normalise each band such that the energy is one. */
160 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
161 {
162    int i, c, N;
163    const celt_int16_t *eBands = m->eBands;
164    const int C = CHANNELS(m);
165    N = FRAMESIZE(m);
166    for (c=0;c<C;c++)
167    {
168       for (i=0;i<m->nbEBands;i++)
169       {
170          int j;
171          celt_word16_t g = 1.f/(1e-10+bank[i+c*m->nbEBands]);
172          for (j=eBands[i];j<eBands[i+1];j++)
173             X[j*C+c] = freq[j+c*N]*g;
174       }
175    }
176 }
177
178 #endif /* FIXED_POINT */
179
180 #ifndef DISABLE_STEREO
181 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
182 {
183    int i, c;
184    const celt_int16_t *eBands = m->eBands;
185    const int C = CHANNELS(m);
186    for (c=0;c<C;c++)
187    {
188       i=0; do {
189          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
190       } while (++i<m->nbEBands);
191    }
192 }
193 #endif /* DISABLE_STEREO */
194
195 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
196 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
197 {
198    int i, c, N;
199    const celt_int16_t *eBands = m->eBands;
200    const int C = CHANNELS(m);
201    N = FRAMESIZE(m);
202    if (C>2)
203       celt_fatal("denormalise_bands() not implemented for >2 channels");
204    for (c=0;c<C;c++)
205    {
206       for (i=0;i<m->nbEBands;i++)
207       {
208          int j;
209          celt_word32_t g = SHR32(bank[i+c*m->nbEBands],1);
210          j=eBands[i]; do {
211             freq[j+c*N] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
212          } while (++j<eBands[i+1]);
213       }
214       for (i=eBands[m->nbEBands];i<eBands[m->nbEBands+1];i++)
215          freq[i+c*N] = 0;
216    }
217 }
218
219 int compute_pitch_gain(const CELTMode *m, const celt_sig_t *X, const celt_sig_t *P, int norm_rate, int *gain_id)
220 {
221    int j, c;
222    celt_word16_t g;
223    celt_word16_t delta;
224    const int C = CHANNELS(m);
225    celt_word32_t Sxy=0, Sxx=0, Syy=0;
226    int len = m->pitchEnd;
227 #ifdef FIXED_POINT
228    int shift = 0;
229    celt_word32_t maxabs=0;
230    for (j=0;j<len;j++)
231    {
232       maxabs = MAX32(maxabs, ABS32(X[j]));
233       maxabs = MAX32(maxabs, ABS32(P[j]));
234    }
235    shift = celt_ilog2(maxabs)-12;
236    if (shift<0)
237       shift = 0;
238 #endif
239    delta = PDIV32_16(Q15ONE, len);
240    for (c=0;c<C;c++)
241    {
242       celt_word16_t gg = Q15ONE;
243       for (j=0;j<len;j++)
244       {
245          celt_word16_t Xj, Pj;
246          Xj = EXTRACT16(SHR32(X[C*j+c], shift));
247          Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[C*j+c], shift)));
248          Sxy = MAC16_16(Sxy, Xj, Pj);
249          Sxx = MAC16_16(Sxx, Pj, Pj);
250          Syy = MAC16_16(Syy, Xj, Xj);
251          gg = SUB16(gg, delta);
252       }
253    }
254 #ifdef FIXED_POINT
255    {
256       celt_word32_t num, den;
257       celt_word16_t fact;
258       fact = MULT16_16(QCONST16(.04, 14), norm_rate);
259       if (fact < QCONST16(1., 14))
260          fact = QCONST16(1., 14);
261       num = Sxy;
262       den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03,15),Syy);
263       shift = celt_ilog2(Sxy)-16;
264       if (shift < 0)
265          shift = 0;
266       g = DIV32(SHL32(SHR32(num,shift),14),SHR32(den,shift));
267       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
268          g = 0;
269       /* This MUST round down so that we don't over-estimate the gain */
270       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
271    }
272 #else
273    {
274       float fact = .04*norm_rate;
275       if (fact < 1)
276          fact = 1;
277       g = Sxy/(.1+Sxx+.03*Syy);
278       if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
279          g = 0;
280       /* This MUST round down so that we don't over-estimate the gain */
281       *gain_id = floor(20*(g-.5));
282    }
283 #endif
284    if (*gain_id < 0)
285    {
286       *gain_id = 0;
287       return 0;
288    } else {
289       if (*gain_id > 15)
290          *gain_id = 15;
291       return 1;
292    }
293 }
294
295 void apply_pitch(const CELTMode *m, celt_sig_t *X, const celt_sig_t *P, int gain_id, int pred)
296 {
297    int j, c;
298    celt_word16_t gain;
299    celt_word16_t delta;
300    const int C = CHANNELS(m);
301    int len = m->pitchEnd;
302    
303    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
304    delta = PDIV32_16(gain, len);
305    if (pred)
306       gain = -gain;
307    else
308       delta = -delta;
309    for (c=0;c<C;c++)
310    {
311       celt_word16_t gg = gain;
312       for (j=0;j<len;j++)
313       {
314          X[C*j+c] += SHL(MULT16_32_Q15(gg,P[C*j+c]),1);
315          gg = ADD16(gg, delta);
316       }
317    }
318 }
319
320 #ifndef DISABLE_STEREO
321
322 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)
323 {
324    int i = bandID;
325    const celt_int16_t *eBands = m->eBands;
326    int j;
327    celt_word16_t a1, a2;
328    if (stereo_mode==0)
329    {
330       /* Do mid-side when not doing intensity stereo */
331       a1 = QCONST16(.70711f,14);
332       a2 = dir*QCONST16(.70711f,14);
333    } else {
334       celt_word16_t left, right;
335       celt_word16_t norm;
336 #ifdef FIXED_POINT
337       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
338 #endif
339       left = VSHR32(bank[i],shift);
340       right = VSHR32(bank[i+m->nbEBands],shift);
341       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
342       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
343       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
344    }
345    for (j=eBands[i];j<eBands[i+1];j++)
346    {
347       celt_norm_t r, l;
348       l = X[j];
349       r = Y[j];
350       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
351       Y[j] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
352    }
353 }
354
355
356 void interleave(celt_norm_t *x, int N)
357 {
358    int i;
359    VARDECL(celt_norm_t, tmp);
360    SAVE_STACK;
361    ALLOC(tmp, N, celt_norm_t);
362    
363    for (i=0;i<N;i++)
364       tmp[i] = x[i];
365    for (i=0;i<N>>1;i++)
366    {
367       x[i<<1] = tmp[i];
368       x[(i<<1)+1] = tmp[i+(N>>1)];
369    }
370    RESTORE_STACK;
371 }
372
373 void deinterleave(celt_norm_t *x, int N)
374 {
375    int i;
376    VARDECL(celt_norm_t, tmp);
377    SAVE_STACK;
378    ALLOC(tmp, N, celt_norm_t);
379    
380    for (i=0;i<N;i++)
381       tmp[i] = x[i];
382    for (i=0;i<N>>1;i++)
383    {
384       x[i] = tmp[i<<1];
385       x[i+(N>>1)] = tmp[(i<<1)+1];
386    }
387    RESTORE_STACK;
388 }
389
390 #endif /* DISABLE_STEREO */
391
392 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
393 {
394    int i, c;
395    int NR=0;
396    celt_word32_t ratio = EPSILON;
397    const int C = CHANNELS(m);
398    const celt_int16_t * restrict eBands = m->eBands;
399    for (c=0;c<C;c++)
400    {
401    for (i=0;i<m->nbEBands;i++)
402    {
403       int j, N;
404       int max_i=0;
405       celt_word16_t max_val=EPSILON;
406       celt_word32_t floor_ener=EPSILON;
407       celt_norm_t * restrict x = X+C*eBands[i]+c;
408       N = eBands[i+1]-eBands[i];
409       for (j=0;j<N;j++)
410       {
411          if (ABS16(x[C*j])>max_val)
412          {
413             max_val = ABS16(x[C*j]);
414             max_i = j;
415          }
416       }
417 #if 0
418       for (j=0;j<N;j++)
419       {
420          if (abs(j-max_i)>2)
421             floor_ener += x[j]*x[j];
422       }
423 #else
424       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
425       if (max_i < N-1)
426          floor_ener -= MULT16_16(x[C*(max_i+1)], x[C*(max_i+1)]);
427       if (max_i < N-2)
428          floor_ener -= MULT16_16(x[C*(max_i+2)], x[C*(max_i+2)]);
429       if (max_i > 0)
430          floor_ener -= MULT16_16(x[C*(max_i-1)], x[C*(max_i-1)]);
431       if (max_i > 1)
432          floor_ener -= MULT16_16(x[C*(max_i-2)], x[C*(max_i-2)]);
433       floor_ener = MAX32(floor_ener, EPSILON);
434 #endif
435       if (N>7)
436       {
437          celt_word16_t r;
438          celt_word16_t den = celt_sqrt(floor_ener);
439          den = MAX32(QCONST16(.02, 15), den);
440          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
441          ratio = ADD32(ratio, EXTEND32(r));
442          NR++;
443       }
444    }
445    }
446    if (NR>0)
447       ratio = DIV32_16(ratio, NR);
448    ratio = ADD32(HALF32(ratio), HALF32(*average));
449    if (!*last_decision)
450    {
451       *last_decision = (ratio < QCONST16(1.8,8));
452    } else {
453       *last_decision = (ratio < QCONST16(3.,8));
454    }
455    *average = EXTRACT16(ratio);
456    return *last_decision;
457 }
458
459 /* Quantisation of the residual */
460 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, ec_enc *enc)
461 {
462    int i, j, remaining_bits, balance;
463    const celt_int16_t * restrict eBands = m->eBands;
464    celt_norm_t * restrict norm;
465    VARDECL(celt_norm_t, _norm);
466    int B;
467    SAVE_STACK;
468
469    B = shortBlocks ? m->nbShortMdcts : 1;
470    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
471    norm = _norm;
472
473    balance = 0;
474    for (i=0;i<m->nbEBands;i++)
475    {
476       int tell;
477       int N;
478       int q;
479       celt_word16_t n;
480       const celt_int16_t * const *BPbits;
481       
482       int curr_balance, curr_bits;
483       
484       N = eBands[i+1]-eBands[i];
485       BPbits = m->bits;
486
487       tell = ec_enc_tell(enc, BITRES);
488       if (i != 0)
489          balance -= tell;
490       remaining_bits = (total_bits<<BITRES)-tell-1;
491       curr_balance = (m->nbEBands-i);
492       if (curr_balance > 3)
493          curr_balance = 3;
494       curr_balance = balance / curr_balance;
495       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
496       curr_bits = pulses2bits(BPbits[i], N, q);
497       remaining_bits -= curr_bits;
498       while (remaining_bits < 0 && q > 0)
499       {
500          remaining_bits += curr_bits;
501          q--;
502          curr_bits = pulses2bits(BPbits[i], N, q);
503          remaining_bits -= curr_bits;
504       }
505       balance += pulses[i] + tell;
506       
507       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
508
509       if (q > 0)
510       {
511          int spread = fold ? B : 0;
512          alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc);
513       } else {
514          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
515       }
516       for (j=eBands[i];j<eBands[i+1];j++)
517          norm[j] = MULT16_16_Q15(n,X[j]);
518    }
519    RESTORE_STACK;
520 }
521
522 #ifndef DISABLE_STEREO
523
524 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)
525 {
526    int i, j, remaining_bits, balance;
527    const celt_int16_t * restrict eBands = m->eBands;
528    celt_norm_t * restrict norm, *X, *Y;
529    VARDECL(celt_norm_t, _norm);
530    const int C = CHANNELS(m);
531    int B;
532    celt_word16_t mid, side;
533    SAVE_STACK;
534
535    B = shortBlocks ? m->nbShortMdcts : 1;
536    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
537    norm = _norm;
538
539    balance = 0;
540    deinterleave(_X, C*eBands[m->nbEBands+1]);
541    X = _X;
542    Y = _X+eBands[m->nbEBands+1];
543    for (i=0;i<m->nbEBands;i++)
544    {
545       int c;
546       int tell;
547       int q1, q2;
548       celt_word16_t n;
549       const celt_int16_t * const *BPbits;
550       int b, qb;
551       int N;
552       int curr_balance, curr_bits;
553       int imid, iside, itheta;
554       int mbits, sbits, delta;
555       int qalloc;
556       
557       BPbits = m->bits;
558
559       N = eBands[i+1]-eBands[i];
560       tell = ec_enc_tell(enc, BITRES);
561       if (i != 0)
562          balance -= tell;
563       remaining_bits = (total_bits<<BITRES)-tell-1;
564       curr_balance = (m->nbEBands-i);
565       if (curr_balance > 3)
566          curr_balance = 3;
567       curr_balance = balance / curr_balance;
568       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
569       if (b<0)
570          b = 0;
571
572       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
573       if (qb > (b>>BITRES)-1)
574          qb = (b>>BITRES)-1;
575       if (qb<0)
576          qb = 0;
577       if (qb>14)
578          qb = 14;
579
580       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1);
581
582       mid = renormalise_vector(X+eBands[i], Q15ONE, N, 1);
583       side = renormalise_vector(Y+eBands[i], Q15ONE, N, 1);
584 #ifdef FIXED_POINT
585       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
586 #else
587       itheta = floor(.5+16384*0.63662*atan2(side,mid));
588 #endif
589       qalloc = log2_frac((1<<qb)+1,BITRES);
590       if (qb==0)
591       {
592          itheta=0;
593       } else {
594          int shift;
595          shift = 14-qb;
596          itheta = (itheta+(1<<shift>>1))>>shift;
597          ec_enc_uint(enc, itheta, (1<<qb)+1);
598          itheta <<= shift;
599       }
600       if (itheta == 0)
601       {
602          imid = 32767;
603          iside = 0;
604          delta = -10000;
605       } else if (itheta == 16384)
606       {
607          imid = 0;
608          iside = 32767;
609          delta = 10000;
610       } else {
611          imid = bitexact_cos(itheta);
612          iside = bitexact_cos(16384-itheta);
613          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
614       }
615       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
616
617       if (N==2)
618       {
619          int c2;
620          int sign=1;
621          celt_norm_t v[2], w[2];
622          celt_norm_t *x2, *y2;
623          mbits = b-qalloc;
624          sbits = 0;
625          if (itheta != 0 && itheta != 16384)
626             sbits = 1<<BITRES;
627          mbits -= sbits;
628          c = itheta > 8192 ? 1 : 0;
629          c2 = 1-c;
630
631          x2 = X+eBands[i];
632          y2 = Y+eBands[i];
633          if (c==0)
634          {
635             v[0] = x2[0];
636             v[1] = x2[1];
637             w[0] = y2[0];
638             w[1] = y2[1];
639          } else {
640             v[0] = y2[0];
641             v[1] = y2[1];
642             w[0] = x2[0];
643             w[1] = x2[1];
644          }
645          q1 = bits2pulses(m, BPbits[i], N, mbits);
646          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
647          remaining_bits -= curr_bits;
648          while (remaining_bits < 0 && q1 > 0)
649          {
650             remaining_bits += curr_bits;
651             q1--;
652             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
653             remaining_bits -= curr_bits;
654          }
655
656          if (q1 > 0)
657          {
658             int spread = fold ? B : 0;
659             alg_quant(v, N, q1, spread, enc);
660          } else {
661             v[0] = QCONST16(1.f, 14);
662             v[1] = 0;
663          }
664          if (sbits)
665          {
666             if (v[0]*w[1] - v[1]*w[0] > 0)
667                sign = 1;
668             else
669                sign = -1;
670             ec_enc_bits(enc, sign==1, 1);
671          } else {
672             sign = 1;
673          }
674          w[0] = -sign*v[1];
675          w[1] = sign*v[0];
676          if (c==0)
677          {
678             x2[0] = v[0];
679             x2[1] = v[1];
680             y2[0] = w[0];
681             y2[1] = w[1];
682          } else {
683             x2[0] = w[0];
684             x2[1] = w[1];
685             y2[0] = v[0];
686             y2[1] = v[1];
687          }
688       } else {
689          
690          mbits = (b-qalloc/2-delta)/2;
691          if (mbits > b-qalloc)
692             mbits = b-qalloc;
693          if (mbits<0)
694             mbits=0;
695          sbits = b-qalloc-mbits;
696          q1 = bits2pulses(m, BPbits[i], N, mbits);
697          q2 = bits2pulses(m, BPbits[i], N, sbits);
698          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
699          remaining_bits -= curr_bits;
700          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
701          {
702             remaining_bits += curr_bits;
703             if (q1>q2)
704             {
705                q1--;
706                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
707             } else {
708                q2--;
709                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
710             }
711             remaining_bits -= curr_bits;
712          }
713
714          if (q1 > 0) {
715             int spread = fold ? B : 0;
716             alg_quant(X+eBands[i], N, q1, spread, enc);
717          } else {
718             intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
719          }
720          if (q2 > 0) {
721             int spread = fold ? B : 0;
722             alg_quant(Y+eBands[i], N, q2, spread, enc);
723          } else
724             for (j=eBands[i];j<eBands[i+1];j++)
725                Y[j] = 0;
726       }
727       
728       balance += pulses[i] + tell;
729
730 #ifdef FIXED_POINT
731       mid = imid;
732       side = iside;
733 #else
734       mid = (1./32768)*imid;
735       side = (1./32768)*iside;
736 #endif
737       for (j=0;j<N;j++)
738          norm[eBands[i]+j] = MULT16_16_Q15(n,X[eBands[i]+j]);
739
740       for (j=0;j<N;j++)
741          X[eBands[i]+j] = MULT16_16_Q15(X[eBands[i]+j], mid);
742       for (j=0;j<N;j++)
743          Y[eBands[i]+j] = MULT16_16_Q15(Y[eBands[i]+j], side);
744
745       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
746       renormalise_vector(X+eBands[i], Q15ONE, N, 1);
747       renormalise_vector(Y+eBands[i], Q15ONE, N, 1);
748    }
749    interleave(_X, C*eBands[m->nbEBands+1]);
750    RESTORE_STACK;
751 }
752 #endif /* DISABLE_STEREO */
753
754 /* Decoding of the residual */
755 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
756 {
757    int i, j, remaining_bits, balance;
758    const celt_int16_t * restrict eBands = m->eBands;
759    celt_norm_t * restrict norm;
760    VARDECL(celt_norm_t, _norm);
761    int B;
762    SAVE_STACK;
763
764    B = shortBlocks ? m->nbShortMdcts : 1;
765    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
766    norm = _norm;
767
768    balance = 0;
769    for (i=0;i<m->nbEBands;i++)
770    {
771       int tell;
772       int N;
773       int q;
774       celt_word16_t n;
775       const celt_int16_t * const *BPbits;
776       
777       int curr_balance, curr_bits;
778
779       N = eBands[i+1]-eBands[i];
780       BPbits = m->bits;
781
782       tell = ec_dec_tell(dec, BITRES);
783       if (i != 0)
784          balance -= tell;
785       remaining_bits = (total_bits<<BITRES)-tell-1;
786       curr_balance = (m->nbEBands-i);
787       if (curr_balance > 3)
788          curr_balance = 3;
789       curr_balance = balance / curr_balance;
790       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
791       curr_bits = pulses2bits(BPbits[i], N, q);
792       remaining_bits -= curr_bits;
793       while (remaining_bits < 0 && q > 0)
794       {
795          remaining_bits += curr_bits;
796          q--;
797          curr_bits = pulses2bits(BPbits[i], N, q);
798          remaining_bits -= curr_bits;
799       }
800       balance += pulses[i] + tell;
801
802       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
803
804       if (q > 0)
805       {
806          int spread = fold ? B : 0;
807          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, dec);
808       } else {
809          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
810       }
811       for (j=eBands[i];j<eBands[i+1];j++)
812          norm[j] = MULT16_16_Q15(n,X[j]);
813    }
814    RESTORE_STACK;
815 }
816
817 #ifndef DISABLE_STEREO
818
819 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)
820 {
821    int i, j, remaining_bits, balance;
822    const celt_int16_t * restrict eBands = m->eBands;
823    celt_norm_t * restrict norm, *X, *Y;
824    VARDECL(celt_norm_t, _norm);
825    const int C = CHANNELS(m);
826    int B;
827    celt_word16_t mid, side;
828    SAVE_STACK;
829
830    B = shortBlocks ? m->nbShortMdcts : 1;
831    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
832    norm = _norm;
833
834    balance = 0;
835    X = _X;
836    Y = _X+eBands[m->nbEBands+1];
837    for (i=0;i<m->nbEBands;i++)
838    {
839       int c;
840       int tell;
841       int q1, q2;
842       celt_word16_t n;
843       const celt_int16_t * const *BPbits;
844       int b, qb;
845       int N;
846       int curr_balance, curr_bits;
847       int imid, iside, itheta;
848       int mbits, sbits, delta;
849       int qalloc;
850       
851       BPbits = m->bits;
852
853       N = eBands[i+1]-eBands[i];
854       tell = ec_dec_tell(dec, BITRES);
855       if (i != 0)
856          balance -= tell;
857       remaining_bits = (total_bits<<BITRES)-tell-1;
858       curr_balance = (m->nbEBands-i);
859       if (curr_balance > 3)
860          curr_balance = 3;
861       curr_balance = balance / curr_balance;
862       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
863       if (b<0)
864          b = 0;
865
866       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
867       if (qb > (b>>BITRES)-1)
868          qb = (b>>BITRES)-1;
869       if (qb>14)
870          qb = 14;
871       if (qb<0)
872          qb = 0;
873       qalloc = log2_frac((1<<qb)+1,BITRES);
874       if (qb==0)
875       {
876          itheta=0;
877       } else {
878          int shift;
879          shift = 14-qb;
880          itheta = ec_dec_uint(dec, (1<<qb)+1);
881          itheta <<= shift;
882       }
883       if (itheta == 0)
884       {
885          imid = 32767;
886          iside = 0;
887          delta = -10000;
888       } else if (itheta == 16384)
889       {
890          imid = 0;
891          iside = 32767;
892          delta = 10000;
893       } else {
894          imid = bitexact_cos(itheta);
895          iside = bitexact_cos(16384-itheta);
896          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
897       }
898       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
899
900       if (N==2)
901       {
902          int c2;
903          int sign=1;
904          celt_norm_t v[2], w[2];
905          celt_norm_t *x2, *y2;
906          mbits = b-qalloc;
907          sbits = 0;
908          if (itheta != 0 && itheta != 16384)
909             sbits = 1<<BITRES;
910          mbits -= sbits;
911          c = itheta > 8192 ? 1 : 0;
912          c2 = 1-c;
913
914          x2 = X+eBands[i];
915          y2 = Y+eBands[i];
916          v[0] = x2[c];
917          v[1] = y2[c];
918          w[0] = x2[c2];
919          w[1] = y2[c2];
920          q1 = bits2pulses(m, BPbits[i], N, mbits);
921          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
922          remaining_bits -= curr_bits;
923          while (remaining_bits < 0 && q1 > 0)
924          {
925             remaining_bits += curr_bits;
926             q1--;
927             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
928             remaining_bits -= curr_bits;
929          }
930
931          if (q1 > 0)
932          {
933             int spread = fold ? B : 0;
934             alg_unquant(v, N, q1, spread, dec);
935          } else {
936             v[0] = QCONST16(1.f, 14);
937             v[1] = 0;
938          }
939          if (sbits)
940             sign = 2*ec_dec_bits(dec, 1)-1;
941          else
942             sign = 1;
943          w[0] = -sign*v[1];
944          w[1] = sign*v[0];
945          if (c==0)
946          {
947             x2[0] = v[0];
948             x2[1] = v[1];
949             y2[0] = w[0];
950             y2[1] = w[1];
951          } else {
952             x2[0] = w[0];
953             x2[1] = w[1];
954             y2[0] = v[0];
955             y2[1] = v[1];
956          }
957       } else {
958          mbits = (b-qalloc/2-delta)/2;
959          if (mbits > b-qalloc)
960             mbits = b-qalloc;
961          if (mbits<0)
962             mbits=0;
963          sbits = b-qalloc-mbits;
964          q1 = bits2pulses(m, BPbits[i], N, mbits);
965          q2 = bits2pulses(m, BPbits[i], N, sbits);
966          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
967          remaining_bits -= curr_bits;
968          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
969          {
970             remaining_bits += curr_bits;
971             if (q1>q2)
972             {
973                q1--;
974                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
975             } else {
976                q2--;
977                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
978             }
979             remaining_bits -= curr_bits;
980          }
981          
982          if (q1 > 0)
983          {
984             int spread = fold ? B : 0;
985             alg_unquant(X+eBands[i], N, q1, spread, dec);
986          } else
987             intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
988          if (q2 > 0)
989          {
990             int spread = fold ? B : 0;
991             alg_unquant(Y+eBands[i], N, q2, spread, dec);
992          } else
993             for (j=eBands[i];j<eBands[i+1];j++)
994                Y[j] = 0;
995             /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
996       }
997       balance += pulses[i] + tell;
998       
999 #ifdef FIXED_POINT
1000       mid = imid;
1001       side = iside;
1002 #else
1003       mid = (1./32768)*imid;
1004       side = (1./32768)*iside;
1005 #endif
1006       for (j=0;j<N;j++)
1007          norm[eBands[i]+j] = MULT16_16_Q15(n,X[eBands[i]+j]);
1008
1009       for (j=0;j<N;j++)
1010          X[eBands[i]+j] = MULT16_16_Q15(X[eBands[i]+j], mid);
1011       for (j=0;j<N;j++)
1012          Y[eBands[i]+j] = MULT16_16_Q15(Y[eBands[i]+j], side);
1013
1014       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
1015       renormalise_vector(X+eBands[i], Q15ONE, N, 1);
1016       renormalise_vector(Y+eBands[i], Q15ONE, N, 1);
1017    }
1018    interleave(_X, C*eBands[m->nbEBands+1]);
1019    RESTORE_STACK;
1020 }
1021
1022 #endif /* DISABLE_STEREO */