Using MS stereo for all bands, fixing a few bugs in the stereo folding
[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, 4);
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 tell;
506       int q1, q2;
507       celt_word16_t n;
508       const celt_int16_t * const *BPbits;
509       int b, qb;
510       int N;
511       int curr_balance, curr_bits;
512       int imid, iside, itheta;
513       int mbits, sbits, delta;
514       int qalloc;
515       
516       BPbits = m->bits;
517
518       N = eBands[i+1]-eBands[i];
519       tell = ec_enc_tell(enc, 4);
520       if (i != 0)
521          balance -= tell;
522       remaining_bits = (total_bits<<BITRES)-tell-1;
523       curr_balance = (m->nbEBands-i);
524       if (curr_balance > 3)
525          curr_balance = 3;
526       curr_balance = balance / curr_balance;
527       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
528       if (b<0)
529          b = 0;
530
531       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
532       if (qb > (b>>BITRES)-1)
533          qb = (b>>BITRES)-1;
534       if (qb<0)
535          qb = 0;
536       if (qb>14)
537          qb = 14;
538
539       stereo_band_mix(m, X, bandE, qb==0, i, 1);
540
541       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
542       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
543 #ifdef FIXED_POINT
544       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
545 #else
546       itheta = floor(.5+16384*0.63662*atan2(side,mid));
547 #endif
548       qalloc = log2_frac((1<<qb)+1,4);
549       if (qb==0)
550       {
551          itheta=0;
552       } else {
553          int shift;
554          shift = 14-qb;
555          itheta = (itheta+(1<<shift>>1))>>shift;
556          ec_enc_uint(enc, itheta, (1<<qb)+1);
557          itheta <<= shift;
558       }
559       if (itheta == 0)
560       {
561          imid = 32767;
562          iside = 0;
563          delta = -10000;
564       } else if (itheta == 16384)
565       {
566          imid = 0;
567          iside = 32767;
568          delta = 10000;
569       } else {
570          imid = bitexact_cos(itheta);
571          iside = bitexact_cos(16384-itheta);
572          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
573       }
574       mbits = (b-qalloc/2-delta)/2;
575       if (mbits > b-qalloc)
576          mbits = b-qalloc;
577       if (mbits<0)
578          mbits=0;
579       sbits = b-qalloc-mbits;
580       q1 = bits2pulses(m, BPbits[i], N, mbits);
581       q2 = bits2pulses(m, BPbits[i], N, sbits);
582       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
583       remaining_bits -= curr_bits;
584       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
585       {
586          remaining_bits += curr_bits;
587          if (q1>q2)
588          {
589             q1--;
590             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
591          } else {
592             q2--;
593             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
594          }
595          remaining_bits -= curr_bits;
596       }
597       balance += pulses[i] + tell;
598       
599       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
600
601       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
602       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
603       {
604          int enabled = 1;
605          pband++;
606          if (remaining_bits >= 1<<BITRES) {
607             enabled = pgains[pband] > QCONST16(.5,15);
608             ec_enc_bits(enc, enabled, 1);
609             balance += 1<<BITRES;
610          }
611          if (enabled)
612             pgains[pband] = QCONST16(.9,15);
613          else
614             pgains[pband] = 0;
615       }
616       
617
618       /* If pitch isn't available, use intra-frame prediction */
619       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
620       {
621          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
622          deinterleave(P+C*eBands[i], C*N);
623       } else if (pitch_used && eBands[i] < m->pitchEnd) {
624          stereo_band_mix(m, P, bandE, qb==0, i, 1);
625          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
626          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
627          deinterleave(P+C*eBands[i], C*N);
628          for (j=C*eBands[i];j<C*eBands[i+1];j++)
629             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
630       } else {
631          for (j=C*eBands[i];j<C*eBands[i+1];j++)
632             P[j] = 0;
633       }
634       deinterleave(X+C*eBands[i], C*N);
635       if (q1 > 0)
636          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
637       else
638          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
639             X[j] = P[j];
640       if (q2 > 0)
641          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
642       else
643          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
644             X[j] = 0;
645
646 #ifdef FIXED_POINT
647       mid = imid;
648       side = iside;
649 #else
650       mid = (1./32768)*imid;
651       side = (1./32768)*iside;
652 #endif
653       for (j=0;j<N;j++)
654          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
655       for (j=0;j<N;j++)
656          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
657
658       interleave(X+C*eBands[i], C*N);
659       for (j=0;j<C*N;j++)
660          norm[C*eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
661
662
663       stereo_band_mix(m, X, bandE, 0, i, -1);
664       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
665       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
666    }
667    RESTORE_STACK;
668 }
669 #endif /* DISABLE_STEREO */
670
671 /* Decoding of the residual */
672 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)
673 {
674    int i, j, remaining_bits, balance;
675    const celt_int16_t * restrict eBands = m->eBands;
676    celt_norm_t * restrict norm;
677    VARDECL(celt_norm_t, _norm);
678    const celt_int16_t *pBands = m->pBands;
679    int pband=-1;
680    int B;
681    SAVE_STACK;
682
683    B = shortBlocks ? m->nbShortMdcts : 1;
684    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
685    norm = _norm;
686
687    balance = 0;
688    for (i=0;i<m->nbEBands;i++)
689    {
690       int tell;
691       int N;
692       int q;
693       celt_word16_t n;
694       const celt_int16_t * const *BPbits;
695       
696       int curr_balance, curr_bits;
697
698       N = eBands[i+1]-eBands[i];
699       BPbits = m->bits;
700
701       tell = ec_dec_tell(dec, 4);
702       if (i != 0)
703          balance -= tell;
704       remaining_bits = (total_bits<<BITRES)-tell-1;
705       curr_balance = (m->nbEBands-i);
706       if (curr_balance > 3)
707          curr_balance = 3;
708       curr_balance = balance / curr_balance;
709       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
710       curr_bits = pulses2bits(BPbits[i], N, q);
711       remaining_bits -= curr_bits;
712       while (remaining_bits < 0 && q > 0)
713       {
714          remaining_bits += curr_bits;
715          q--;
716          curr_bits = pulses2bits(BPbits[i], N, q);
717          remaining_bits -= curr_bits;
718       }
719       balance += pulses[i] + tell;
720
721       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
722
723       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
724       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
725       {
726          int enabled = 1;
727          pband++;
728          if (remaining_bits >= 1<<BITRES) {
729            enabled = ec_dec_bits(dec, 1);
730            balance += 1<<BITRES;
731          }
732          if (enabled)
733             pgains[pband] = QCONST16(.9,15);
734          else
735             pgains[pband] = 0;
736       }
737
738       /* If pitch isn't available, use intra-frame prediction */
739       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
740       {
741          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
742       } else if (pitch_used && eBands[i] < m->pitchEnd) {
743          for (j=eBands[i];j<eBands[i+1];j++)
744             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
745       } else {
746          for (j=eBands[i];j<eBands[i+1];j++)
747             P[j] = 0;
748       }
749       
750       if (q > 0)
751       {
752          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
753       } else {
754          for (j=eBands[i];j<eBands[i+1];j++)
755             X[j] = P[j];
756       }
757       for (j=eBands[i];j<eBands[i+1];j++)
758          norm[j] = MULT16_16_Q15(n,X[j]);
759    }
760    RESTORE_STACK;
761 }
762
763 #ifndef DISABLE_STEREO
764
765 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)
766 {
767    int i, j, remaining_bits, balance;
768    const celt_int16_t * restrict eBands = m->eBands;
769    celt_norm_t * restrict norm;
770    VARDECL(celt_norm_t, _norm);
771    const int C = CHANNELS(m);
772    const celt_int16_t *pBands = m->pBands;
773    int pband=-1;
774    int B;
775    celt_word16_t mid, side;
776    SAVE_STACK;
777
778    B = shortBlocks ? m->nbShortMdcts : 1;
779    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
780    norm = _norm;
781
782    balance = 0;
783    for (i=0;i<m->nbEBands;i++)
784    {
785       int tell;
786       int q1, q2;
787       celt_word16_t n;
788       const celt_int16_t * const *BPbits;
789       int b, qb;
790       int N;
791       int curr_balance, curr_bits;
792       int imid, iside, itheta;
793       int mbits, sbits, delta;
794       int qalloc;
795       
796       BPbits = m->bits;
797
798       N = eBands[i+1]-eBands[i];
799       tell = ec_dec_tell(dec, 4);
800       if (i != 0)
801          balance -= tell;
802       remaining_bits = (total_bits<<BITRES)-tell-1;
803       curr_balance = (m->nbEBands-i);
804       if (curr_balance > 3)
805          curr_balance = 3;
806       curr_balance = balance / curr_balance;
807       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
808       if (b<0)
809          b = 0;
810
811       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
812       if (qb > (b>>BITRES)-1)
813          qb = (b>>BITRES)-1;
814       if (qb>14)
815          qb = 14;
816       if (qb<0)
817          qb = 0;
818       qalloc = log2_frac((1<<qb)+1,4);
819       if (qb==0)
820       {
821          itheta=0;
822       } else {
823          int shift;
824          shift = 14-qb;
825          itheta = ec_dec_uint(dec, (1<<qb)+1);
826          itheta <<= shift;
827       }
828       if (itheta == 0)
829       {
830          imid = 32767;
831          iside = 0;
832          delta = -10000;
833       } else if (itheta == 16384)
834       {
835          imid = 0;
836          iside = 32767;
837          delta = 10000;
838       } else {
839          imid = bitexact_cos(itheta);
840          iside = bitexact_cos(16384-itheta);
841          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
842       }
843       mbits = (b-qalloc/2-delta)/2;
844       if (mbits > b-qalloc)
845          mbits = b-qalloc;
846       if (mbits<0)
847          mbits=0;
848       sbits = b-qalloc-mbits;
849       q1 = bits2pulses(m, BPbits[i], N, mbits);
850       q2 = bits2pulses(m, BPbits[i], N, sbits);
851       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
852       remaining_bits -= curr_bits;
853       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
854       {
855          remaining_bits += curr_bits;
856          if (q1>q2)
857          {
858             q1--;
859             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
860          } else {
861             q2--;
862             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
863          }
864          remaining_bits -= curr_bits;
865       }
866       balance += pulses[i] + tell;
867       
868       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
869
870       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
871       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
872       {
873          int enabled = 1;
874          pband++;
875          if (remaining_bits >= 1<<BITRES) {
876             enabled = ec_dec_bits(dec, 1);
877             balance += 1<<BITRES;
878          }
879          if (enabled)
880             pgains[pband] = QCONST16(.9,15);
881          else
882             pgains[pband] = 0;
883       }
884
885       /* If pitch isn't available, use intra-frame prediction */
886       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
887       {
888          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
889          deinterleave(P+C*eBands[i], C*N);
890       } else if (pitch_used && eBands[i] < m->pitchEnd) {
891          stereo_band_mix(m, P, bandE, qb==0, i, 1);
892          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
893          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
894          deinterleave(P+C*eBands[i], C*N);
895          for (j=C*eBands[i];j<C*eBands[i+1];j++)
896             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
897       } else {
898          for (j=C*eBands[i];j<C*eBands[i+1];j++)
899             P[j] = 0;
900       }
901       deinterleave(X+C*eBands[i], C*N);
902       if (q1 > 0)
903          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
904       else
905          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
906             X[j] = P[j];
907       if (q2 > 0)
908          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
909       else
910          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
911             X[j] = 0;
912       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
913       
914 #ifdef FIXED_POINT
915       mid = imid;
916       side = iside;
917 #else
918       mid = (1./32768)*imid;
919       side = (1./32768)*iside;
920 #endif
921       for (j=0;j<N;j++)
922          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
923       for (j=0;j<N;j++)
924          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
925       
926       interleave(X+C*eBands[i], C*N);
927       for (j=0;j<C*N;j++)
928          norm[C*eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
929
930
931       stereo_band_mix(m, X, bandE, 0, i, -1);
932       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
933       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
934    }
935    RESTORE_STACK;
936 }
937
938 #endif /* DISABLE_STEREO */