18e8d1298150a55a37e3497890a3512c6a5795db
[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];
623          K[0] = q1;
624          K[1] = q2;
625          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], K, norm, P+C*eBands[i], eBands[i], B);
626          deinterleave(P+C*eBands[i], C*N);
627       } else if (pitch_used && eBands[i] < m->pitchEnd) {
628          stereo_band_mix(m, P, bandE, qb==0, i, 1);
629          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
630          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
631          deinterleave(P+C*eBands[i], C*N);
632          for (j=C*eBands[i];j<C*eBands[i+1];j++)
633             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
634       } else {
635          for (j=C*eBands[i];j<C*eBands[i+1];j++)
636             P[j] = 0;
637       }
638       deinterleave(X+C*eBands[i], C*N);
639       if (q1 > 0)
640          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
641       else
642          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
643             X[j] = P[j];
644       if (q2 > 0)
645          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
646       else
647          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
648             X[j] = 0;
649
650 #ifdef FIXED_POINT
651       mid = imid;
652       side = iside;
653 #else
654       mid = (1./32768)*imid;
655       side = (1./32768)*iside;
656 #endif
657       for (c=0;c<C;c++)
658          for (j=0;j<N;j++)
659             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
660
661       for (j=0;j<N;j++)
662          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
663       for (j=0;j<N;j++)
664          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
665
666       interleave(X+C*eBands[i], C*N);
667
668
669       stereo_band_mix(m, X, bandE, 0, i, -1);
670       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
671       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
672    }
673    RESTORE_STACK;
674 }
675 #endif /* DISABLE_STEREO */
676
677 /* Decoding of the residual */
678 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)
679 {
680    int i, j, remaining_bits, balance;
681    const celt_int16_t * restrict eBands = m->eBands;
682    celt_norm_t * restrict norm;
683    VARDECL(celt_norm_t, _norm);
684    const celt_int16_t *pBands = m->pBands;
685    int pband=-1;
686    int B;
687    SAVE_STACK;
688
689    B = shortBlocks ? m->nbShortMdcts : 1;
690    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
691    norm = _norm;
692
693    balance = 0;
694    for (i=0;i<m->nbEBands;i++)
695    {
696       int tell;
697       int N;
698       int q;
699       celt_word16_t n;
700       const celt_int16_t * const *BPbits;
701       
702       int curr_balance, curr_bits;
703
704       N = eBands[i+1]-eBands[i];
705       BPbits = m->bits;
706
707       tell = ec_dec_tell(dec, BITRES);
708       if (i != 0)
709          balance -= tell;
710       remaining_bits = (total_bits<<BITRES)-tell-1;
711       curr_balance = (m->nbEBands-i);
712       if (curr_balance > 3)
713          curr_balance = 3;
714       curr_balance = balance / curr_balance;
715       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
716       curr_bits = pulses2bits(BPbits[i], N, q);
717       remaining_bits -= curr_bits;
718       while (remaining_bits < 0 && q > 0)
719       {
720          remaining_bits += curr_bits;
721          q--;
722          curr_bits = pulses2bits(BPbits[i], N, q);
723          remaining_bits -= curr_bits;
724       }
725       balance += pulses[i] + tell;
726
727       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
728
729       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
730       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
731       {
732          int enabled = 1;
733          pband++;
734          if (remaining_bits >= 1<<BITRES) {
735            enabled = ec_dec_bits(dec, 1);
736            balance += 1<<BITRES;
737          }
738          if (enabled)
739             pgains[pband] = QCONST16(.9,15);
740          else
741             pgains[pband] = 0;
742       }
743
744       /* If pitch isn't available, use intra-frame prediction */
745       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
746       {
747          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], &q, norm, P+eBands[i], eBands[i], B);
748       } else if (pitch_used && eBands[i] < m->pitchEnd) {
749          for (j=eBands[i];j<eBands[i+1];j++)
750             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
751       } else {
752          for (j=eBands[i];j<eBands[i+1];j++)
753             P[j] = 0;
754       }
755       
756       if (q > 0)
757       {
758          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
759       } else {
760          for (j=eBands[i];j<eBands[i+1];j++)
761             X[j] = P[j];
762       }
763       for (j=eBands[i];j<eBands[i+1];j++)
764          norm[j] = MULT16_16_Q15(n,X[j]);
765    }
766    RESTORE_STACK;
767 }
768
769 #ifndef DISABLE_STEREO
770
771 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)
772 {
773    int i, j, remaining_bits, balance;
774    const celt_int16_t * restrict eBands = m->eBands;
775    celt_norm_t * restrict norm;
776    VARDECL(celt_norm_t, _norm);
777    const int C = CHANNELS(m);
778    const celt_int16_t *pBands = m->pBands;
779    int pband=-1;
780    int B;
781    celt_word16_t mid, side;
782    SAVE_STACK;
783
784    B = shortBlocks ? m->nbShortMdcts : 1;
785    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
786    norm = _norm;
787
788    balance = 0;
789    for (i=0;i<m->nbEBands;i++)
790    {
791       int c;
792       int tell;
793       int q1, q2;
794       celt_word16_t n;
795       const celt_int16_t * const *BPbits;
796       int b, qb;
797       int N;
798       int curr_balance, curr_bits;
799       int imid, iside, itheta;
800       int mbits, sbits, delta;
801       int qalloc;
802       
803       BPbits = m->bits;
804
805       N = eBands[i+1]-eBands[i];
806       tell = ec_dec_tell(dec, BITRES);
807       if (i != 0)
808          balance -= tell;
809       remaining_bits = (total_bits<<BITRES)-tell-1;
810       curr_balance = (m->nbEBands-i);
811       if (curr_balance > 3)
812          curr_balance = 3;
813       curr_balance = balance / curr_balance;
814       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
815       if (b<0)
816          b = 0;
817
818       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
819       if (qb > (b>>BITRES)-1)
820          qb = (b>>BITRES)-1;
821       if (qb>14)
822          qb = 14;
823       if (qb<0)
824          qb = 0;
825       qalloc = log2_frac((1<<qb)+1,BITRES);
826       if (qb==0)
827       {
828          itheta=0;
829       } else {
830          int shift;
831          shift = 14-qb;
832          itheta = ec_dec_uint(dec, (1<<qb)+1);
833          itheta <<= shift;
834       }
835       if (itheta == 0)
836       {
837          imid = 32767;
838          iside = 0;
839          delta = -10000;
840       } else if (itheta == 16384)
841       {
842          imid = 0;
843          iside = 32767;
844          delta = 10000;
845       } else {
846          imid = bitexact_cos(itheta);
847          iside = bitexact_cos(16384-itheta);
848          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
849       }
850       mbits = (b-qalloc/2-delta)/2;
851       if (mbits > b-qalloc)
852          mbits = b-qalloc;
853       if (mbits<0)
854          mbits=0;
855       sbits = b-qalloc-mbits;
856       q1 = bits2pulses(m, BPbits[i], N, mbits);
857       q2 = bits2pulses(m, BPbits[i], N, sbits);
858       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
859       remaining_bits -= curr_bits;
860       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
861       {
862          remaining_bits += curr_bits;
863          if (q1>q2)
864          {
865             q1--;
866             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
867          } else {
868             q2--;
869             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
870          }
871          remaining_bits -= curr_bits;
872       }
873       balance += pulses[i] + tell;
874       
875       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
876
877       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
878       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
879       {
880          int enabled = 1;
881          pband++;
882          if (remaining_bits >= 1<<BITRES) {
883             enabled = ec_dec_bits(dec, 1);
884             balance += 1<<BITRES;
885          }
886          if (enabled)
887             pgains[pband] = QCONST16(.9,15);
888          else
889             pgains[pband] = 0;
890       }
891
892       /* If pitch isn't available, use intra-frame prediction */
893       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
894       {
895          int K[2];
896          K[0] = q1;
897          K[1] = q2;
898          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], K, norm, P+C*eBands[i], eBands[i], B);
899          deinterleave(P+C*eBands[i], C*N);
900       } else if (pitch_used && eBands[i] < m->pitchEnd) {
901          stereo_band_mix(m, P, bandE, qb==0, i, 1);
902          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
903          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
904          deinterleave(P+C*eBands[i], C*N);
905          for (j=C*eBands[i];j<C*eBands[i+1];j++)
906             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
907       } else {
908          for (j=C*eBands[i];j<C*eBands[i+1];j++)
909             P[j] = 0;
910       }
911       deinterleave(X+C*eBands[i], C*N);
912       if (q1 > 0)
913          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
914       else
915          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
916             X[j] = P[j];
917       if (q2 > 0)
918          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
919       else
920          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
921             X[j] = 0;
922       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
923       
924 #ifdef FIXED_POINT
925       mid = imid;
926       side = iside;
927 #else
928       mid = (1./32768)*imid;
929       side = (1./32768)*iside;
930 #endif
931       for (c=0;c<C;c++)
932          for (j=0;j<N;j++)
933             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
934
935       for (j=0;j<N;j++)
936          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
937       for (j=0;j<N;j++)
938          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
939
940       interleave(X+C*eBands[i], C*N);
941
942       stereo_band_mix(m, X, bandE, 0, i, -1);
943       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
944       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
945    }
946    RESTORE_STACK;
947 }
948
949 #endif /* DISABLE_STEREO */