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