Simplifications to the 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*N] = 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*N] = 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*N], 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    const int N = FRAMESIZE(m);
228 #ifdef FIXED_POINT
229    int shift = 0;
230    celt_word32_t maxabs=0;
231
232    for (c=0;c<C;c++)
233    {
234       for (j=0;j<len;j++)
235       {
236          maxabs = MAX32(maxabs, ABS32(X[j+c*N]));
237          maxabs = MAX32(maxabs, ABS32(P[j+c*N]));
238       }
239    }
240    shift = celt_ilog2(maxabs)-12;
241    if (shift<0)
242       shift = 0;
243 #endif
244    delta = PDIV32_16(Q15ONE, len);
245    for (c=0;c<C;c++)
246    {
247       celt_word16_t gg = Q15ONE;
248       for (j=0;j<len;j++)
249       {
250          celt_word16_t Xj, Pj;
251          Xj = EXTRACT16(SHR32(X[j+c*N], shift));
252          Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[j+c*N], shift)));
253          Sxy = MAC16_16(Sxy, Xj, Pj);
254          Sxx = MAC16_16(Sxx, Pj, Pj);
255          Syy = MAC16_16(Syy, Xj, Xj);
256          gg = SUB16(gg, delta);
257       }
258    }
259 #ifdef FIXED_POINT
260    {
261       celt_word32_t num, den;
262       celt_word16_t fact;
263       fact = MULT16_16(QCONST16(.04, 14), norm_rate);
264       if (fact < QCONST16(1., 14))
265          fact = QCONST16(1., 14);
266       num = Sxy;
267       den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03,15),Syy);
268       shift = celt_ilog2(Sxy)-16;
269       if (shift < 0)
270          shift = 0;
271       g = DIV32(SHL32(SHR32(num,shift),14),SHR32(den,shift));
272       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
273          g = 0;
274       /* This MUST round down so that we don't over-estimate the gain */
275       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
276    }
277 #else
278    {
279       float fact = .04*norm_rate;
280       if (fact < 1)
281          fact = 1;
282       g = Sxy/(.1+Sxx+.03*Syy);
283       if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
284          g = 0;
285       /* This MUST round down so that we don't over-estimate the gain */
286       *gain_id = floor(20*(g-.5));
287    }
288 #endif
289    if (*gain_id < 0)
290    {
291       *gain_id = 0;
292       return 0;
293    } else {
294       if (*gain_id > 15)
295          *gain_id = 15;
296       return 1;
297    }
298 }
299
300 void apply_pitch(const CELTMode *m, celt_sig_t *X, const celt_sig_t *P, int gain_id, int pred)
301 {
302    int j, c, N;
303    celt_word16_t gain;
304    celt_word16_t delta;
305    const int C = CHANNELS(m);
306    int len = m->pitchEnd;
307
308    N = FRAMESIZE(m);
309    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
310    delta = PDIV32_16(gain, len);
311    if (pred)
312       gain = -gain;
313    else
314       delta = -delta;
315    for (c=0;c<C;c++)
316    {
317       celt_word16_t gg = gain;
318       for (j=0;j<len;j++)
319       {
320          X[j+c*N] += SHL(MULT16_32_Q15(gg,P[j+c*N]),1);
321          gg = ADD16(gg, delta);
322       }
323    }
324 }
325
326 #ifndef DISABLE_STEREO
327
328 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)
329 {
330    int i = bandID;
331    const celt_int16_t *eBands = m->eBands;
332    int j;
333    celt_word16_t a1, a2;
334    if (stereo_mode==0)
335    {
336       /* Do mid-side when not doing intensity stereo */
337       a1 = QCONST16(.70711f,14);
338       a2 = dir*QCONST16(.70711f,14);
339    } else {
340       celt_word16_t left, right;
341       celt_word16_t norm;
342 #ifdef FIXED_POINT
343       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
344 #endif
345       left = VSHR32(bank[i],shift);
346       right = VSHR32(bank[i+m->nbEBands],shift);
347       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
348       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
349       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
350    }
351    for (j=0;j<eBands[i+1]-eBands[i];j++)
352    {
353       celt_norm_t r, l;
354       l = X[j];
355       r = Y[j];
356       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
357       Y[j] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
358    }
359 }
360
361
362 #endif /* DISABLE_STEREO */
363
364 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
365 {
366    int i, c, N0;
367    int NR=0;
368    celt_word32_t ratio = EPSILON;
369    const int C = CHANNELS(m);
370    const celt_int16_t * restrict eBands = m->eBands;
371    
372    N0 = FRAMESIZE(m);
373
374    for (c=0;c<C;c++)
375    {
376    for (i=0;i<m->nbEBands;i++)
377    {
378       int j, N;
379       int max_i=0;
380       celt_word16_t max_val=EPSILON;
381       celt_word32_t floor_ener=EPSILON;
382       celt_norm_t * restrict x = X+eBands[i]+c*N0;
383       N = eBands[i+1]-eBands[i];
384       for (j=0;j<N;j++)
385       {
386          if (ABS16(x[j])>max_val)
387          {
388             max_val = ABS16(x[j]);
389             max_i = j;
390          }
391       }
392 #if 0
393       for (j=0;j<N;j++)
394       {
395          if (abs(j-max_i)>2)
396             floor_ener += x[j]*x[j];
397       }
398 #else
399       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
400       if (max_i < N-1)
401          floor_ener -= MULT16_16(x[(max_i+1)], x[(max_i+1)]);
402       if (max_i < N-2)
403          floor_ener -= MULT16_16(x[(max_i+2)], x[(max_i+2)]);
404       if (max_i > 0)
405          floor_ener -= MULT16_16(x[(max_i-1)], x[(max_i-1)]);
406       if (max_i > 1)
407          floor_ener -= MULT16_16(x[(max_i-2)], x[(max_i-2)]);
408       floor_ener = MAX32(floor_ener, EPSILON);
409 #endif
410       if (N>7)
411       {
412          celt_word16_t r;
413          celt_word16_t den = celt_sqrt(floor_ener);
414          den = MAX32(QCONST16(.02, 15), den);
415          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
416          ratio = ADD32(ratio, EXTEND32(r));
417          NR++;
418       }
419    }
420    }
421    if (NR>0)
422       ratio = DIV32_16(ratio, NR);
423    ratio = ADD32(HALF32(ratio), HALF32(*average));
424    if (!*last_decision)
425    {
426       *last_decision = (ratio < QCONST16(1.8,8));
427    } else {
428       *last_decision = (ratio < QCONST16(3.,8));
429    }
430    *average = EXTRACT16(ratio);
431    return *last_decision;
432 }
433
434 /* Quantisation of the residual */
435 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)
436 {
437    int i, j, remaining_bits, balance;
438    const celt_int16_t * restrict eBands = m->eBands;
439    celt_norm_t * restrict norm;
440    VARDECL(celt_norm_t, _norm);
441    int B;
442    SAVE_STACK;
443
444    B = shortBlocks ? m->nbShortMdcts : 1;
445    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
446    norm = _norm;
447
448    balance = 0;
449    for (i=0;i<m->nbEBands;i++)
450    {
451       int tell;
452       int N;
453       int q;
454       celt_word16_t n;
455       const celt_int16_t * const *BPbits;
456       
457       int curr_balance, curr_bits;
458       
459       N = eBands[i+1]-eBands[i];
460       BPbits = m->bits;
461
462       tell = ec_enc_tell(enc, BITRES);
463       if (i != 0)
464          balance -= tell;
465       remaining_bits = (total_bits<<BITRES)-tell-1;
466       curr_balance = (m->nbEBands-i);
467       if (curr_balance > 3)
468          curr_balance = 3;
469       curr_balance = balance / curr_balance;
470       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
471       curr_bits = pulses2bits(BPbits[i], N, q);
472       remaining_bits -= curr_bits;
473       while (remaining_bits < 0 && q > 0)
474       {
475          remaining_bits += curr_bits;
476          q--;
477          curr_bits = pulses2bits(BPbits[i], N, q);
478          remaining_bits -= curr_bits;
479       }
480       balance += pulses[i] + tell;
481       
482       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
483
484       if (q > 0)
485       {
486          int spread = fold ? B : 0;
487          alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc);
488       } else {
489          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
490       }
491       for (j=eBands[i];j<eBands[i+1];j++)
492          norm[j] = MULT16_16_Q15(n,X[j]);
493    }
494    RESTORE_STACK;
495 }
496
497 #ifndef DISABLE_STEREO
498
499 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)
500 {
501    int i, j, remaining_bits, balance;
502    const celt_int16_t * restrict eBands = m->eBands;
503    celt_norm_t * restrict norm;
504    VARDECL(celt_norm_t, _norm);
505    int B;
506    celt_word16_t mid, side;
507    SAVE_STACK;
508
509    B = shortBlocks ? m->nbShortMdcts : 1;
510    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
511    norm = _norm;
512
513    balance = 0;
514    for (i=0;i<m->nbEBands;i++)
515    {
516       int c;
517       int tell;
518       int q1, q2;
519       celt_word16_t n;
520       const celt_int16_t * const *BPbits;
521       int b, qb;
522       int N;
523       int curr_balance, curr_bits;
524       int imid, iside, itheta;
525       int mbits, sbits, delta;
526       int qalloc;
527       celt_norm_t * restrict X, * restrict Y;
528       
529       X = _X+eBands[i];
530       Y = X+eBands[m->nbEBands+1];
531       BPbits = m->bits;
532
533       N = eBands[i+1]-eBands[i];
534       tell = ec_enc_tell(enc, BITRES);
535       if (i != 0)
536          balance -= tell;
537       remaining_bits = (total_bits<<BITRES)-tell-1;
538       curr_balance = (m->nbEBands-i);
539       if (curr_balance > 3)
540          curr_balance = 3;
541       curr_balance = balance / curr_balance;
542       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
543       if (b<0)
544          b = 0;
545
546       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
547       if (qb > (b>>BITRES)-1)
548          qb = (b>>BITRES)-1;
549       if (qb<0)
550          qb = 0;
551       if (qb>14)
552          qb = 14;
553
554       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1);
555
556       mid = renormalise_vector(X, Q15ONE, N, 1);
557       side = renormalise_vector(Y, Q15ONE, N, 1);
558 #ifdef FIXED_POINT
559       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
560 #else
561       itheta = floor(.5+16384*0.63662*atan2(side,mid));
562 #endif
563       qalloc = log2_frac((1<<qb)+1,BITRES);
564       if (qb==0)
565       {
566          itheta=0;
567       } else {
568          int shift;
569          shift = 14-qb;
570          itheta = (itheta+(1<<shift>>1))>>shift;
571          ec_enc_uint(enc, itheta, (1<<qb)+1);
572          itheta <<= shift;
573       }
574       if (itheta == 0)
575       {
576          imid = 32767;
577          iside = 0;
578          delta = -10000;
579       } else if (itheta == 16384)
580       {
581          imid = 0;
582          iside = 32767;
583          delta = 10000;
584       } else {
585          imid = bitexact_cos(itheta);
586          iside = bitexact_cos(16384-itheta);
587          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
588       }
589       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
590
591       if (N==2)
592       {
593          int c2;
594          int sign=1;
595          celt_norm_t v[2], w[2];
596          celt_norm_t *x2, *y2;
597          mbits = b-qalloc;
598          sbits = 0;
599          if (itheta != 0 && itheta != 16384)
600             sbits = 1<<BITRES;
601          mbits -= sbits;
602          c = itheta > 8192 ? 1 : 0;
603          c2 = 1-c;
604
605          x2 = X;
606          y2 = Y;
607          if (c==0)
608          {
609             v[0] = x2[0];
610             v[1] = x2[1];
611             w[0] = y2[0];
612             w[1] = y2[1];
613          } else {
614             v[0] = y2[0];
615             v[1] = y2[1];
616             w[0] = x2[0];
617             w[1] = x2[1];
618          }
619          q1 = bits2pulses(m, BPbits[i], N, mbits);
620          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
621          remaining_bits -= curr_bits;
622          while (remaining_bits < 0 && q1 > 0)
623          {
624             remaining_bits += curr_bits;
625             q1--;
626             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
627             remaining_bits -= curr_bits;
628          }
629
630          if (q1 > 0)
631          {
632             int spread = fold ? B : 0;
633             alg_quant(v, N, q1, spread, enc);
634          } else {
635             v[0] = QCONST16(1.f, 14);
636             v[1] = 0;
637          }
638          if (sbits)
639          {
640             if (v[0]*w[1] - v[1]*w[0] > 0)
641                sign = 1;
642             else
643                sign = -1;
644             ec_enc_bits(enc, sign==1, 1);
645          } else {
646             sign = 1;
647          }
648          w[0] = -sign*v[1];
649          w[1] = sign*v[0];
650          if (c==0)
651          {
652             x2[0] = v[0];
653             x2[1] = v[1];
654             y2[0] = w[0];
655             y2[1] = w[1];
656          } else {
657             x2[0] = w[0];
658             x2[1] = w[1];
659             y2[0] = v[0];
660             y2[1] = v[1];
661          }
662       } else {
663          
664          mbits = (b-qalloc/2-delta)/2;
665          if (mbits > b-qalloc)
666             mbits = b-qalloc;
667          if (mbits<0)
668             mbits=0;
669          sbits = b-qalloc-mbits;
670          q1 = bits2pulses(m, BPbits[i], N, mbits);
671          q2 = bits2pulses(m, BPbits[i], N, sbits);
672          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
673          remaining_bits -= curr_bits;
674          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
675          {
676             remaining_bits += curr_bits;
677             if (q1>q2)
678             {
679                q1--;
680                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
681             } else {
682                q2--;
683                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
684             }
685             remaining_bits -= curr_bits;
686          }
687
688          if (q1 > 0) {
689             int spread = fold ? B : 0;
690             alg_quant(X, N, q1, spread, enc);
691          } else {
692             intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
693          }
694          if (q2 > 0) {
695             int spread = fold ? B : 0;
696             alg_quant(Y, N, q2, spread, enc);
697          } else
698             for (j=0;j<N;j++)
699                Y[j] = 0;
700       }
701       
702       balance += pulses[i] + tell;
703
704 #ifdef FIXED_POINT
705       mid = imid;
706       side = iside;
707 #else
708       mid = (1./32768)*imid;
709       side = (1./32768)*iside;
710 #endif
711       for (j=0;j<N;j++)
712          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
713
714       for (j=0;j<N;j++)
715          X[j] = MULT16_16_Q15(X[j], mid);
716       for (j=0;j<N;j++)
717          Y[j] = MULT16_16_Q15(Y[j], side);
718
719       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
720       renormalise_vector(X, Q15ONE, N, 1);
721       renormalise_vector(Y, Q15ONE, N, 1);
722    }
723    RESTORE_STACK;
724 }
725 #endif /* DISABLE_STEREO */
726
727 /* Decoding of the residual */
728 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)
729 {
730    int i, j, remaining_bits, balance;
731    const celt_int16_t * restrict eBands = m->eBands;
732    celt_norm_t * restrict norm;
733    VARDECL(celt_norm_t, _norm);
734    int B;
735    SAVE_STACK;
736
737    B = shortBlocks ? m->nbShortMdcts : 1;
738    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
739    norm = _norm;
740
741    balance = 0;
742    for (i=0;i<m->nbEBands;i++)
743    {
744       int tell;
745       int N;
746       int q;
747       celt_word16_t n;
748       const celt_int16_t * const *BPbits;
749       
750       int curr_balance, curr_bits;
751
752       N = eBands[i+1]-eBands[i];
753       BPbits = m->bits;
754
755       tell = ec_dec_tell(dec, BITRES);
756       if (i != 0)
757          balance -= tell;
758       remaining_bits = (total_bits<<BITRES)-tell-1;
759       curr_balance = (m->nbEBands-i);
760       if (curr_balance > 3)
761          curr_balance = 3;
762       curr_balance = balance / curr_balance;
763       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
764       curr_bits = pulses2bits(BPbits[i], N, q);
765       remaining_bits -= curr_bits;
766       while (remaining_bits < 0 && q > 0)
767       {
768          remaining_bits += curr_bits;
769          q--;
770          curr_bits = pulses2bits(BPbits[i], N, q);
771          remaining_bits -= curr_bits;
772       }
773       balance += pulses[i] + tell;
774
775       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
776
777       if (q > 0)
778       {
779          int spread = fold ? B : 0;
780          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, dec);
781       } else {
782          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
783       }
784       for (j=eBands[i];j<eBands[i+1];j++)
785          norm[j] = MULT16_16_Q15(n,X[j]);
786    }
787    RESTORE_STACK;
788 }
789
790 #ifndef DISABLE_STEREO
791
792 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)
793 {
794    int i, j, remaining_bits, balance;
795    const celt_int16_t * restrict eBands = m->eBands;
796    celt_norm_t * restrict norm;
797    VARDECL(celt_norm_t, _norm);
798    int B;
799    celt_word16_t mid, side;
800    SAVE_STACK;
801
802    B = shortBlocks ? m->nbShortMdcts : 1;
803    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
804    norm = _norm;
805
806    balance = 0;
807    for (i=0;i<m->nbEBands;i++)
808    {
809       int c;
810       int tell;
811       int q1, q2;
812       celt_word16_t n;
813       const celt_int16_t * const *BPbits;
814       int b, qb;
815       int N;
816       int curr_balance, curr_bits;
817       int imid, iside, itheta;
818       int mbits, sbits, delta;
819       int qalloc;
820       celt_norm_t * restrict X, * restrict Y;
821       
822       X = _X+eBands[i];
823       Y = X+eBands[m->nbEBands+1];
824       BPbits = m->bits;
825
826       N = eBands[i+1]-eBands[i];
827       tell = ec_dec_tell(dec, BITRES);
828       if (i != 0)
829          balance -= tell;
830       remaining_bits = (total_bits<<BITRES)-tell-1;
831       curr_balance = (m->nbEBands-i);
832       if (curr_balance > 3)
833          curr_balance = 3;
834       curr_balance = balance / curr_balance;
835       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
836       if (b<0)
837          b = 0;
838
839       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
840       if (qb > (b>>BITRES)-1)
841          qb = (b>>BITRES)-1;
842       if (qb>14)
843          qb = 14;
844       if (qb<0)
845          qb = 0;
846       qalloc = log2_frac((1<<qb)+1,BITRES);
847       if (qb==0)
848       {
849          itheta=0;
850       } else {
851          int shift;
852          shift = 14-qb;
853          itheta = ec_dec_uint(dec, (1<<qb)+1);
854          itheta <<= shift;
855       }
856       if (itheta == 0)
857       {
858          imid = 32767;
859          iside = 0;
860          delta = -10000;
861       } else if (itheta == 16384)
862       {
863          imid = 0;
864          iside = 32767;
865          delta = 10000;
866       } else {
867          imid = bitexact_cos(itheta);
868          iside = bitexact_cos(16384-itheta);
869          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
870       }
871       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
872
873       if (N==2)
874       {
875          int c2;
876          int sign=1;
877          celt_norm_t v[2], w[2];
878          celt_norm_t *x2, *y2;
879          mbits = b-qalloc;
880          sbits = 0;
881          if (itheta != 0 && itheta != 16384)
882             sbits = 1<<BITRES;
883          mbits -= sbits;
884          c = itheta > 8192 ? 1 : 0;
885          c2 = 1-c;
886
887          x2 = X;
888          y2 = Y;
889          v[0] = x2[c];
890          v[1] = y2[c];
891          w[0] = x2[c2];
892          w[1] = y2[c2];
893          q1 = bits2pulses(m, BPbits[i], N, mbits);
894          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
895          remaining_bits -= curr_bits;
896          while (remaining_bits < 0 && q1 > 0)
897          {
898             remaining_bits += curr_bits;
899             q1--;
900             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
901             remaining_bits -= curr_bits;
902          }
903
904          if (q1 > 0)
905          {
906             int spread = fold ? B : 0;
907             alg_unquant(v, N, q1, spread, dec);
908          } else {
909             v[0] = QCONST16(1.f, 14);
910             v[1] = 0;
911          }
912          if (sbits)
913             sign = 2*ec_dec_bits(dec, 1)-1;
914          else
915             sign = 1;
916          w[0] = -sign*v[1];
917          w[1] = sign*v[0];
918          if (c==0)
919          {
920             x2[0] = v[0];
921             x2[1] = v[1];
922             y2[0] = w[0];
923             y2[1] = w[1];
924          } else {
925             x2[0] = w[0];
926             x2[1] = w[1];
927             y2[0] = v[0];
928             y2[1] = v[1];
929          }
930       } else {
931          mbits = (b-qalloc/2-delta)/2;
932          if (mbits > b-qalloc)
933             mbits = b-qalloc;
934          if (mbits<0)
935             mbits=0;
936          sbits = b-qalloc-mbits;
937          q1 = bits2pulses(m, BPbits[i], N, mbits);
938          q2 = bits2pulses(m, BPbits[i], N, sbits);
939          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
940          remaining_bits -= curr_bits;
941          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
942          {
943             remaining_bits += curr_bits;
944             if (q1>q2)
945             {
946                q1--;
947                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
948             } else {
949                q2--;
950                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
951             }
952             remaining_bits -= curr_bits;
953          }
954          
955          if (q1 > 0)
956          {
957             int spread = fold ? B : 0;
958             alg_unquant(X, N, q1, spread, dec);
959          } else
960             intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
961          if (q2 > 0)
962          {
963             int spread = fold ? B : 0;
964             alg_unquant(Y, N, q2, spread, dec);
965          } else
966             for (j=0;j<N;j++)
967                Y[j] = 0;
968             /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
969       }
970       balance += pulses[i] + tell;
971       
972 #ifdef FIXED_POINT
973       mid = imid;
974       side = iside;
975 #else
976       mid = (1./32768)*imid;
977       side = (1./32768)*iside;
978 #endif
979       for (j=0;j<N;j++)
980          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
981
982       for (j=0;j<N;j++)
983          X[j] = MULT16_16_Q15(X[j], mid);
984       for (j=0;j<N;j++)
985          Y[j] = MULT16_16_Q15(Y[j], side);
986
987       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
988       renormalise_vector(X, Q15ONE, N, 1);
989       renormalise_vector(Y, Q15ONE, N, 1);
990    }
991    RESTORE_STACK;
992 }
993
994 #endif /* DISABLE_STEREO */