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