Changing some code to use BITRES directly instead of its value.
[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 #ifdef FIXED_POINT
49 /* Compute the amplitude (sqrt energy) in each of the bands */
50 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
51 {
52    int i, c, N;
53    const celt_int16_t *eBands = m->eBands;
54    const int C = CHANNELS(m);
55    N = FRAMESIZE(m);
56    for (c=0;c<C;c++)
57    {
58       for (i=0;i<m->nbEBands;i++)
59       {
60          int j;
61          celt_word32_t maxval=0;
62          celt_word32_t sum = 0;
63          
64          j=eBands[i]; do {
65             maxval = MAX32(maxval, X[j+c*N]);
66             maxval = MAX32(maxval, -X[j+c*N]);
67          } while (++j<eBands[i+1]);
68          
69          if (maxval > 0)
70          {
71             int shift = celt_ilog2(maxval)-10;
72             j=eBands[i]; do {
73                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
74                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
75             } while (++j<eBands[i+1]);
76             /* We're adding one here to make damn sure we never end up with a pitch vector that's
77                larger than unity norm */
78             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
79          } else {
80             bank[i+c*m->nbEBands] = EPSILON;
81          }
82          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
83       }
84    }
85    /*printf ("\n");*/
86 }
87
88 /* Normalise each band such that the energy is one. */
89 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
90 {
91    int i, c, N;
92    const celt_int16_t *eBands = m->eBands;
93    const int C = CHANNELS(m);
94    N = FRAMESIZE(m);
95    for (c=0;c<C;c++)
96    {
97       i=0; do {
98          celt_word16_t g;
99          int j,shift;
100          celt_word16_t E;
101          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
102          E = VSHR32(bank[i+c*m->nbEBands], shift);
103          g = EXTRACT16(celt_rcp(SHL32(E,3)));
104          j=eBands[i]; do {
105             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
106          } while (++j<eBands[i+1]);
107       } while (++i<m->nbEBands);
108    }
109 }
110
111 #else /* FIXED_POINT */
112 /* Compute the amplitude (sqrt energy) in each of the bands */
113 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
114 {
115    int i, c, N;
116    const celt_int16_t *eBands = m->eBands;
117    const int C = CHANNELS(m);
118    N = FRAMESIZE(m);
119    for (c=0;c<C;c++)
120    {
121       for (i=0;i<m->nbEBands;i++)
122       {
123          int j;
124          celt_word32_t sum = 1e-10;
125          for (j=eBands[i];j<eBands[i+1];j++)
126             sum += X[j+c*N]*X[j+c*N];
127          bank[i+c*m->nbEBands] = sqrt(sum);
128          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
129       }
130    }
131    /*printf ("\n");*/
132 }
133
134 #ifdef EXP_PSY
135 void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, celt_ener_t *bank)
136 {
137    int i, c, N;
138    const celt_int16_t *eBands = m->eBands;
139    const int C = CHANNELS(m);
140    N = FRAMESIZE(m);
141    for (c=0;c<C;c++)
142    {
143       for (i=0;i<m->nbEBands;i++)
144       {
145          int j;
146          celt_word32_t sum = 1e-10;
147          for (j=eBands[i];j<eBands[i+1];j++)
148             sum += X[j*C+c]*X[j+c*N]*tonality[j];
149          bank[i+c*m->nbEBands] = sqrt(sum);
150          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
151       }
152    }
153    /*printf ("\n");*/
154 }
155 #endif
156
157 /* Normalise each band such that the energy is one. */
158 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
159 {
160    int i, c, N;
161    const celt_int16_t *eBands = m->eBands;
162    const int C = CHANNELS(m);
163    N = FRAMESIZE(m);
164    for (c=0;c<C;c++)
165    {
166       for (i=0;i<m->nbEBands;i++)
167       {
168          int j;
169          celt_word16_t g = 1.f/(1e-10+bank[i+c*m->nbEBands]);
170          for (j=eBands[i];j<eBands[i+1];j++)
171             X[j*C+c] = freq[j+c*N]*g;
172       }
173    }
174 }
175
176 #endif /* FIXED_POINT */
177
178 #ifndef DISABLE_STEREO
179 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
180 {
181    int i, c;
182    const celt_int16_t *eBands = m->eBands;
183    const int C = CHANNELS(m);
184    for (c=0;c<C;c++)
185    {
186       i=0; do {
187          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
188       } while (++i<m->nbEBands);
189    }
190 }
191 #endif /* DISABLE_STEREO */
192
193 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
194 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
195 {
196    int i, c, N;
197    const celt_int16_t *eBands = m->eBands;
198    const int C = CHANNELS(m);
199    N = FRAMESIZE(m);
200    if (C>2)
201       celt_fatal("denormalise_bands() not implemented for >2 channels");
202    for (c=0;c<C;c++)
203    {
204       for (i=0;i<m->nbEBands;i++)
205       {
206          int j;
207          celt_word32_t g = SHR32(bank[i+c*m->nbEBands],1);
208          j=eBands[i]; do {
209             freq[j+c*N] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
210          } while (++j<eBands[i+1]);
211       }
212       for (i=eBands[m->nbEBands];i<eBands[m->nbEBands+1];i++)
213          freq[i+c*N] = 0;
214    }
215 }
216
217
218 /* Compute the best gain for each "pitch band" */
219 int compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
220 {
221    int i;
222    int gain_sum = 0;
223    const celt_int16_t *pBands = m->pBands;
224    const int C = CHANNELS(m);
225
226    for (i=0;i<m->nbPBands;i++)
227    {
228       celt_word32_t Sxy=0, Sxx=0;
229       int j;
230       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
231       for (j=C*pBands[i];j<C*pBands[i+1];j++)
232       {
233          Sxy = MAC16_16(Sxy, X[j], P[j]);
234          Sxx = MAC16_16(Sxx, X[j], X[j]);
235       }
236       Sxy = SHR32(Sxy,2);
237       Sxx = SHR32(Sxx,2);
238       /* No negative gain allowed */
239       if (Sxy < 0)
240          Sxy = 0;
241       /* Not sure how that would happen, just making sure */
242       if (Sxy > Sxx)
243          Sxy = Sxx;
244       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
245          residual doesn't quantise well */
246       Sxy = MULT16_32_Q15(QCONST16(.99f, 15), Sxy);
247       /* gain = Sxy/Sxx */
248       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
249       if (gains[i]>QCONST16(.5,15))
250          gain_sum++;
251    }
252    return gain_sum > 5;
253 }
254
255 #ifndef DISABLE_STEREO
256
257 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
258 {
259    int i = bandID;
260    const celt_int16_t *eBands = m->eBands;
261    const int C = CHANNELS(m);
262    int j;
263    celt_word16_t a1, a2;
264    if (stereo_mode==0)
265    {
266       /* Do mid-side when not doing intensity stereo */
267       a1 = QCONST16(.70711f,14);
268       a2 = dir*QCONST16(.70711f,14);
269    } else {
270       celt_word16_t left, right;
271       celt_word16_t norm;
272 #ifdef FIXED_POINT
273       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
274 #endif
275       left = VSHR32(bank[i],shift);
276       right = VSHR32(bank[i+m->nbEBands],shift);
277       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
278       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
279       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
280    }
281    for (j=eBands[i];j<eBands[i+1];j++)
282    {
283       celt_norm_t r, l;
284       l = X[j*C];
285       r = X[j*C+1];
286       X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
287       X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
288    }
289 }
290
291
292 void interleave(celt_norm_t *x, int N)
293 {
294    int i;
295    VARDECL(celt_norm_t, tmp);
296    SAVE_STACK;
297    ALLOC(tmp, N, celt_norm_t);
298    
299    for (i=0;i<N;i++)
300       tmp[i] = x[i];
301    for (i=0;i<N>>1;i++)
302    {
303       x[i<<1] = tmp[i];
304       x[(i<<1)+1] = tmp[i+(N>>1)];
305    }
306    RESTORE_STACK;
307 }
308
309 void deinterleave(celt_norm_t *x, int N)
310 {
311    int i;
312    VARDECL(celt_norm_t, tmp);
313    SAVE_STACK;
314    ALLOC(tmp, N, celt_norm_t);
315    
316    for (i=0;i<N;i++)
317       tmp[i] = x[i];
318    for (i=0;i<N>>1;i++)
319    {
320       x[i] = tmp[i<<1];
321       x[i+(N>>1)] = tmp[(i<<1)+1];
322    }
323    RESTORE_STACK;
324 }
325
326 #endif /* DISABLE_STEREO */
327
328 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
329 {
330    int i;
331    int NR=0;
332    celt_word32_t ratio = EPSILON;
333    const celt_int16_t * restrict eBands = m->eBands;
334    for (i=0;i<m->nbEBands;i++)
335    {
336       int j, N;
337       int max_i=0;
338       celt_word16_t max_val=EPSILON;
339       celt_word32_t floor_ener=EPSILON;
340       celt_norm_t * restrict x = X+eBands[i];
341       N = eBands[i+1]-eBands[i];
342       for (j=0;j<N;j++)
343       {
344          if (ABS16(x[j])>max_val)
345          {
346             max_val = ABS16(x[j]);
347             max_i = j;
348          }
349       }
350 #if 0
351       for (j=0;j<N;j++)
352       {
353          if (abs(j-max_i)>2)
354             floor_ener += x[j]*x[j];
355       }
356 #else
357       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
358       if (max_i < N-1)
359          floor_ener -= MULT16_16(x[max_i+1], x[max_i+1]);
360       if (max_i < N-2)
361          floor_ener -= MULT16_16(x[max_i+2], x[max_i+2]);
362       if (max_i > 0)
363          floor_ener -= MULT16_16(x[max_i-1], x[max_i-1]);
364       if (max_i > 1)
365          floor_ener -= MULT16_16(x[max_i-2], x[max_i-2]);
366       floor_ener = MAX32(floor_ener, EPSILON);
367 #endif
368       if (N>7 && eBands[i] >= m->pitchEnd)
369       {
370          celt_word16_t r;
371          celt_word16_t den = celt_sqrt(floor_ener);
372          den = MAX32(QCONST16(.02, 15), den);
373          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
374          ratio = ADD32(ratio, EXTEND32(r));
375          NR++;
376       }
377    }
378    if (NR>0)
379       ratio = DIV32_16(ratio, NR);
380    ratio = ADD32(HALF32(ratio), HALF32(*average));
381    if (!*last_decision)
382    {
383       *last_decision = (ratio < QCONST16(1.8,8));
384    } else {
385       *last_decision = (ratio < QCONST16(3.,8));
386    }
387    *average = EXTRACT16(ratio);
388    return *last_decision;
389 }
390
391 /* Quantisation of the residual */
392 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
393 {
394    int i, j, remaining_bits, balance;
395    const celt_int16_t * restrict eBands = m->eBands;
396    celt_norm_t * restrict norm;
397    VARDECL(celt_norm_t, _norm);   const celt_int16_t *pBands = m->pBands;
398    int pband=-1;
399    int B;
400    SAVE_STACK;
401
402    B = shortBlocks ? m->nbShortMdcts : 1;
403    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
404    norm = _norm;
405
406    balance = 0;
407    for (i=0;i<m->nbEBands;i++)
408    {
409       int tell;
410       int N;
411       int q;
412       celt_word16_t n;
413       const celt_int16_t * const *BPbits;
414       
415       int curr_balance, curr_bits;
416       
417       N = eBands[i+1]-eBands[i];
418       BPbits = m->bits;
419
420       tell = ec_enc_tell(enc, BITRES);
421       if (i != 0)
422          balance -= tell;
423       remaining_bits = (total_bits<<BITRES)-tell-1;
424       curr_balance = (m->nbEBands-i);
425       if (curr_balance > 3)
426          curr_balance = 3;
427       curr_balance = balance / curr_balance;
428       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
429       curr_bits = pulses2bits(BPbits[i], N, q);
430       remaining_bits -= curr_bits;
431       while (remaining_bits < 0 && q > 0)
432       {
433          remaining_bits += curr_bits;
434          q--;
435          curr_bits = pulses2bits(BPbits[i], N, q);
436          remaining_bits -= curr_bits;
437       }
438       balance += pulses[i] + tell;
439       
440       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
441
442       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
443       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
444       {
445          int enabled = 1;
446          pband++;
447          if (remaining_bits >= 1<<BITRES) {
448            enabled = pgains[pband] > QCONST16(.5,15);
449            ec_enc_bits(enc, enabled, 1);
450            balance += 1<<BITRES;
451          }
452          if (enabled)
453             pgains[pband] = QCONST16(.9,15);
454          else
455             pgains[pband] = 0;
456       }
457
458       /* If pitch isn't available, use intra-frame prediction */
459       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
460       {
461          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], &q, norm, P+eBands[i], eBands[i], B);
462       } else if (pitch_used && eBands[i] < m->pitchEnd) {
463          for (j=eBands[i];j<eBands[i+1];j++)
464             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
465       } else {
466          for (j=eBands[i];j<eBands[i+1];j++)
467             P[j] = 0;
468       }
469       
470       if (q > 0)
471       {
472          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], enc);
473       } else {
474          for (j=eBands[i];j<eBands[i+1];j++)
475             X[j] = P[j];
476       }
477       for (j=eBands[i];j<eBands[i+1];j++)
478          norm[j] = MULT16_16_Q15(n,X[j]);
479    }
480    RESTORE_STACK;
481 }
482
483 #ifndef DISABLE_STEREO
484
485 void quant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
486 {
487    int i, j, remaining_bits, balance;
488    const celt_int16_t * restrict eBands = m->eBands;
489    celt_norm_t * restrict norm;
490    VARDECL(celt_norm_t, _norm);
491    const int C = CHANNELS(m);
492    const celt_int16_t *pBands = m->pBands;
493    int pband=-1;
494    int B;
495    celt_word16_t mid, side;
496    SAVE_STACK;
497
498    B = shortBlocks ? m->nbShortMdcts : 1;
499    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
500    norm = _norm;
501
502    balance = 0;
503    for (i=0;i<m->nbEBands;i++)
504    {
505       int c;
506       int tell;
507       int q1, q2;
508       celt_word16_t n;
509       const celt_int16_t * const *BPbits;
510       int b, qb;
511       int N;
512       int curr_balance, curr_bits;
513       int imid, iside, itheta;
514       int mbits, sbits, delta;
515       int qalloc;
516       
517       BPbits = m->bits;
518
519       N = eBands[i+1]-eBands[i];
520       tell = ec_enc_tell(enc, BITRES);
521       if (i != 0)
522          balance -= tell;
523       remaining_bits = (total_bits<<BITRES)-tell-1;
524       curr_balance = (m->nbEBands-i);
525       if (curr_balance > 3)
526          curr_balance = 3;
527       curr_balance = balance / curr_balance;
528       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
529       if (b<0)
530          b = 0;
531
532       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
533       if (qb > (b>>BITRES)-1)
534          qb = (b>>BITRES)-1;
535       if (qb<0)
536          qb = 0;
537       if (qb>14)
538          qb = 14;
539
540       stereo_band_mix(m, X, bandE, qb==0, i, 1);
541
542       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
543       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
544 #ifdef FIXED_POINT
545       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
546 #else
547       itheta = floor(.5+16384*0.63662*atan2(side,mid));
548 #endif
549       qalloc = log2_frac((1<<qb)+1,BITRES);
550       if (qb==0)
551       {
552          itheta=0;
553       } else {
554          int shift;
555          shift = 14-qb;
556          itheta = (itheta+(1<<shift>>1))>>shift;
557          ec_enc_uint(enc, itheta, (1<<qb)+1);
558          itheta <<= shift;
559       }
560       if (itheta == 0)
561       {
562          imid = 32767;
563          iside = 0;
564          delta = -10000;
565       } else if (itheta == 16384)
566       {
567          imid = 0;
568          iside = 32767;
569          delta = 10000;
570       } else {
571          imid = bitexact_cos(itheta);
572          iside = bitexact_cos(16384-itheta);
573          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
574       }
575       mbits = (b-qalloc/2-delta)/2;
576       if (mbits > b-qalloc)
577          mbits = b-qalloc;
578       if (mbits<0)
579          mbits=0;
580       sbits = b-qalloc-mbits;
581       q1 = bits2pulses(m, BPbits[i], N, mbits);
582       q2 = bits2pulses(m, BPbits[i], N, sbits);
583       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
584       remaining_bits -= curr_bits;
585       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
586       {
587          remaining_bits += curr_bits;
588          if (q1>q2)
589          {
590             q1--;
591             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
592          } else {
593             q2--;
594             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
595          }
596          remaining_bits -= curr_bits;
597       }
598       balance += pulses[i] + tell;
599       
600       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
601
602       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
603       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
604       {
605          int enabled = 1;
606          pband++;
607          if (remaining_bits >= 1<<BITRES) {
608             enabled = pgains[pband] > QCONST16(.5,15);
609             ec_enc_bits(enc, enabled, 1);
610             balance += 1<<BITRES;
611          }
612          if (enabled)
613             pgains[pband] = QCONST16(.9,15);
614          else
615             pgains[pband] = 0;
616       }
617       
618
619       /* If pitch isn't available, use intra-frame prediction */
620       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
621       {
622          int K[2] = {q1, q2};
623          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], K, norm, P+C*eBands[i], eBands[i], B);
624          deinterleave(P+C*eBands[i], C*N);
625       } else if (pitch_used && eBands[i] < m->pitchEnd) {
626          stereo_band_mix(m, P, bandE, qb==0, i, 1);
627          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
628          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
629          deinterleave(P+C*eBands[i], C*N);
630          for (j=C*eBands[i];j<C*eBands[i+1];j++)
631             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
632       } else {
633          for (j=C*eBands[i];j<C*eBands[i+1];j++)
634             P[j] = 0;
635       }
636       deinterleave(X+C*eBands[i], C*N);
637       if (q1 > 0)
638          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
639       else
640          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
641             X[j] = P[j];
642       if (q2 > 0)
643          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
644       else
645          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
646             X[j] = 0;
647
648 #ifdef FIXED_POINT
649       mid = imid;
650       side = iside;
651 #else
652       mid = (1./32768)*imid;
653       side = (1./32768)*iside;
654 #endif
655       for (c=0;c<C;c++)
656          for (j=0;j<N;j++)
657             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
658
659       for (j=0;j<N;j++)
660          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
661       for (j=0;j<N;j++)
662          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
663
664       interleave(X+C*eBands[i], C*N);
665
666
667       stereo_band_mix(m, X, bandE, 0, i, -1);
668       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
669       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
670    }
671    RESTORE_STACK;
672 }
673 #endif /* DISABLE_STEREO */
674
675 /* Decoding of the residual */
676 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
677 {
678    int i, j, remaining_bits, balance;
679    const celt_int16_t * restrict eBands = m->eBands;
680    celt_norm_t * restrict norm;
681    VARDECL(celt_norm_t, _norm);
682    const celt_int16_t *pBands = m->pBands;
683    int pband=-1;
684    int B;
685    SAVE_STACK;
686
687    B = shortBlocks ? m->nbShortMdcts : 1;
688    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
689    norm = _norm;
690
691    balance = 0;
692    for (i=0;i<m->nbEBands;i++)
693    {
694       int tell;
695       int N;
696       int q;
697       celt_word16_t n;
698       const celt_int16_t * const *BPbits;
699       
700       int curr_balance, curr_bits;
701
702       N = eBands[i+1]-eBands[i];
703       BPbits = m->bits;
704
705       tell = ec_dec_tell(dec, BITRES);
706       if (i != 0)
707          balance -= tell;
708       remaining_bits = (total_bits<<BITRES)-tell-1;
709       curr_balance = (m->nbEBands-i);
710       if (curr_balance > 3)
711          curr_balance = 3;
712       curr_balance = balance / curr_balance;
713       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
714       curr_bits = pulses2bits(BPbits[i], N, q);
715       remaining_bits -= curr_bits;
716       while (remaining_bits < 0 && q > 0)
717       {
718          remaining_bits += curr_bits;
719          q--;
720          curr_bits = pulses2bits(BPbits[i], N, q);
721          remaining_bits -= curr_bits;
722       }
723       balance += pulses[i] + tell;
724
725       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
726
727       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
728       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
729       {
730          int enabled = 1;
731          pband++;
732          if (remaining_bits >= 1<<BITRES) {
733            enabled = ec_dec_bits(dec, 1);
734            balance += 1<<BITRES;
735          }
736          if (enabled)
737             pgains[pband] = QCONST16(.9,15);
738          else
739             pgains[pband] = 0;
740       }
741
742       /* If pitch isn't available, use intra-frame prediction */
743       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
744       {
745          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], &q, norm, P+eBands[i], eBands[i], B);
746       } else if (pitch_used && eBands[i] < m->pitchEnd) {
747          for (j=eBands[i];j<eBands[i+1];j++)
748             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
749       } else {
750          for (j=eBands[i];j<eBands[i+1];j++)
751             P[j] = 0;
752       }
753       
754       if (q > 0)
755       {
756          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
757       } else {
758          for (j=eBands[i];j<eBands[i+1];j++)
759             X[j] = P[j];
760       }
761       for (j=eBands[i];j<eBands[i+1];j++)
762          norm[j] = MULT16_16_Q15(n,X[j]);
763    }
764    RESTORE_STACK;
765 }
766
767 #ifndef DISABLE_STEREO
768
769 void unquant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
770 {
771    int i, j, remaining_bits, balance;
772    const celt_int16_t * restrict eBands = m->eBands;
773    celt_norm_t * restrict norm;
774    VARDECL(celt_norm_t, _norm);
775    const int C = CHANNELS(m);
776    const celt_int16_t *pBands = m->pBands;
777    int pband=-1;
778    int B;
779    celt_word16_t mid, side;
780    SAVE_STACK;
781
782    B = shortBlocks ? m->nbShortMdcts : 1;
783    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
784    norm = _norm;
785
786    balance = 0;
787    for (i=0;i<m->nbEBands;i++)
788    {
789       int c;
790       int tell;
791       int q1, q2;
792       celt_word16_t n;
793       const celt_int16_t * const *BPbits;
794       int b, qb;
795       int N;
796       int curr_balance, curr_bits;
797       int imid, iside, itheta;
798       int mbits, sbits, delta;
799       int qalloc;
800       
801       BPbits = m->bits;
802
803       N = eBands[i+1]-eBands[i];
804       tell = ec_dec_tell(dec, BITRES);
805       if (i != 0)
806          balance -= tell;
807       remaining_bits = (total_bits<<BITRES)-tell-1;
808       curr_balance = (m->nbEBands-i);
809       if (curr_balance > 3)
810          curr_balance = 3;
811       curr_balance = balance / curr_balance;
812       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
813       if (b<0)
814          b = 0;
815
816       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
817       if (qb > (b>>BITRES)-1)
818          qb = (b>>BITRES)-1;
819       if (qb>14)
820          qb = 14;
821       if (qb<0)
822          qb = 0;
823       qalloc = log2_frac((1<<qb)+1,BITRES);
824       if (qb==0)
825       {
826          itheta=0;
827       } else {
828          int shift;
829          shift = 14-qb;
830          itheta = ec_dec_uint(dec, (1<<qb)+1);
831          itheta <<= shift;
832       }
833       if (itheta == 0)
834       {
835          imid = 32767;
836          iside = 0;
837          delta = -10000;
838       } else if (itheta == 16384)
839       {
840          imid = 0;
841          iside = 32767;
842          delta = 10000;
843       } else {
844          imid = bitexact_cos(itheta);
845          iside = bitexact_cos(16384-itheta);
846          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
847       }
848       mbits = (b-qalloc/2-delta)/2;
849       if (mbits > b-qalloc)
850          mbits = b-qalloc;
851       if (mbits<0)
852          mbits=0;
853       sbits = b-qalloc-mbits;
854       q1 = bits2pulses(m, BPbits[i], N, mbits);
855       q2 = bits2pulses(m, BPbits[i], N, sbits);
856       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
857       remaining_bits -= curr_bits;
858       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
859       {
860          remaining_bits += curr_bits;
861          if (q1>q2)
862          {
863             q1--;
864             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
865          } else {
866             q2--;
867             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
868          }
869          remaining_bits -= curr_bits;
870       }
871       balance += pulses[i] + tell;
872       
873       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
874
875       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
876       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
877       {
878          int enabled = 1;
879          pband++;
880          if (remaining_bits >= 1<<BITRES) {
881             enabled = ec_dec_bits(dec, 1);
882             balance += 1<<BITRES;
883          }
884          if (enabled)
885             pgains[pband] = QCONST16(.9,15);
886          else
887             pgains[pband] = 0;
888       }
889
890       /* If pitch isn't available, use intra-frame prediction */
891       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
892       {
893          int K[2] = {q1, q2};
894          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], K, norm, P+C*eBands[i], eBands[i], B);
895          deinterleave(P+C*eBands[i], C*N);
896       } else if (pitch_used && eBands[i] < m->pitchEnd) {
897          stereo_band_mix(m, P, bandE, qb==0, i, 1);
898          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
899          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
900          deinterleave(P+C*eBands[i], C*N);
901          for (j=C*eBands[i];j<C*eBands[i+1];j++)
902             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
903       } else {
904          for (j=C*eBands[i];j<C*eBands[i+1];j++)
905             P[j] = 0;
906       }
907       deinterleave(X+C*eBands[i], C*N);
908       if (q1 > 0)
909          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
910       else
911          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
912             X[j] = P[j];
913       if (q2 > 0)
914          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
915       else
916          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
917             X[j] = 0;
918       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
919       
920 #ifdef FIXED_POINT
921       mid = imid;
922       side = iside;
923 #else
924       mid = (1./32768)*imid;
925       side = (1./32768)*iside;
926 #endif
927       for (c=0;c<C;c++)
928          for (j=0;j<N;j++)
929             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
930
931       for (j=0;j<N;j++)
932          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
933       for (j=0;j<N;j++)
934          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
935
936       interleave(X+C*eBands[i], C*N);
937
938       stereo_band_mix(m, X, bandE, 0, i, -1);
939       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
940       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
941    }
942    RESTORE_STACK;
943 }
944
945 #endif /* DISABLE_STEREO */