Minor stuff: preventing float underflow in celt_exp2(), preventing the use of
[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       /* No negative gain allowed */
237       if (Sxy < 0)
238          Sxy = 0;
239       /* Not sure how that would happen, just making sure */
240       if (Sxy > Sxx)
241          Sxy = Sxx;
242       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
243          residual doesn't quantise well */
244       Sxy = MULT16_32_Q15(QCONST16(.99f, 15), Sxy);
245       /* gain = Sxy/Sxx */
246       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
247       if (gains[i]>QCONST16(.5,15))
248          gain_sum++;
249       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
250    }
251    /*if(rand()%10==0)
252    {
253       for (i=0;i<m->nbPBands;i++)
254          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
255       printf ("\n");
256    }*/
257    return gain_sum > 5;
258 }
259
260 #ifndef DISABLE_STEREO
261
262 static void intensity_band(celt_norm_t * restrict X, int len)
263 {
264    int j;
265    celt_word32_t E = 1e-15;
266    celt_word32_t E2 = 1e-15;
267    for (j=0;j<len;j++)
268    {
269       X[j] = X[2*j];
270       E = MAC16_16(E, X[j],X[j]);
271       E2 = MAC16_16(E2, X[2*j+1],X[2*j+1]);
272    }
273 #ifndef FIXED_POINT
274    E  = celt_sqrt(E+E2)/celt_sqrt(E);
275    for (j=0;j<len;j++)
276       X[j] *= E;
277 #endif
278    for (j=0;j<len;j++)
279       X[len+j] = 0;
280
281 }
282
283 static void dup_band(celt_norm_t * restrict X, int len)
284 {
285    int j;
286    for (j=len-1;j>=0;j--)
287    {
288       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
289       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
290    }
291 }
292
293 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
294 {
295    int i = bandID;
296    const celt_int16_t *eBands = m->eBands;
297    const int C = CHANNELS(m);
298    {
299       int j;
300       if (stereo_mode && dir <0)
301       {
302          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
303       } else {
304          celt_word16_t a1, a2;
305          if (stereo_mode==0)
306          {
307             /* Do mid-side when not doing intensity stereo */
308             a1 = QCONST16(.70711f,14);
309             a2 = dir*QCONST16(.70711f,14);
310          } else {
311             celt_word16_t left, right;
312             celt_word16_t norm;
313 #ifdef FIXED_POINT
314             int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
315 #endif
316             left = VSHR32(bank[i],shift);
317             right = VSHR32(bank[i+m->nbEBands],shift);
318             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
319             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
320             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
321          }
322          for (j=eBands[i];j<eBands[i+1];j++)
323          {
324             celt_norm_t r, l;
325             l = X[j*C];
326             r = X[j*C+1];
327             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
328             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
329          }
330       }
331       if (stereo_mode && dir>0)
332       {
333          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
334       }
335    }
336 }
337
338 static void point_stereo_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int bandID, int dir)
339 {
340    int i = bandID;
341    const celt_int16_t *eBands = m->eBands;
342    const int C = CHANNELS(m);
343    celt_word16_t left, right;
344    celt_word16_t norm;
345    celt_word16_t a1, a2;
346    int j;
347 #ifdef FIXED_POINT
348    int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
349 #endif
350    left = VSHR32(bank[i],shift);
351    right = VSHR32(bank[i+m->nbEBands],shift);
352    norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
353    a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
354    a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
355    for (j=eBands[i];j<eBands[i+1];j++)
356    {
357       celt_norm_t r, l;
358       l = X[j*C];
359       r = X[j*C+1];
360       X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
361       X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
362    }
363 }
364
365 void interleave(celt_norm_t *x, int N)
366 {
367    int i;
368    VARDECL(celt_norm_t, tmp);
369    SAVE_STACK;
370    ALLOC(tmp, N, celt_norm_t);
371    
372    for (i=0;i<N;i++)
373       tmp[i] = x[i];
374    for (i=0;i<N>>1;i++)
375    {
376       x[i<<1] = tmp[i];
377       x[(i<<1)+1] = tmp[i+(N>>1)];
378    }
379    RESTORE_STACK;
380 }
381
382 void deinterleave(celt_norm_t *x, int N)
383 {
384    int i;
385    VARDECL(celt_norm_t, tmp);
386    SAVE_STACK;
387    ALLOC(tmp, N, celt_norm_t);
388    
389    for (i=0;i<N;i++)
390       tmp[i] = x[i];
391    for (i=0;i<N>>1;i++)
392    {
393       x[i] = tmp[i<<1];
394       x[i+(N>>1)] = tmp[(i<<1)+1];
395    }
396    RESTORE_STACK;
397 }
398
399 #endif /* DISABLE_STEREO */
400
401 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
402 {
403    int i;
404    int NR=0;
405    celt_word32_t ratio = EPSILON;
406    const celt_int16_t * restrict eBands = m->eBands;
407    for (i=0;i<m->nbEBands;i++)
408    {
409       int j, N;
410       int max_i=0;
411       celt_word16_t max_val=EPSILON;
412       celt_word32_t floor_ener=EPSILON;
413       celt_norm_t * restrict x = X+eBands[i];
414       N = eBands[i+1]-eBands[i];
415       for (j=0;j<N;j++)
416       {
417          if (ABS16(x[j])>max_val)
418          {
419             max_val = ABS16(x[j]);
420             max_i = j;
421          }
422       }
423 #if 0
424       for (j=0;j<N;j++)
425       {
426          if (abs(j-max_i)>2)
427             floor_ener += x[j]*x[j];
428       }
429 #else
430       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
431       if (max_i < N-1)
432          floor_ener -= MULT16_16(x[max_i+1], x[max_i+1]);
433       if (max_i < N-2)
434          floor_ener -= MULT16_16(x[max_i+2], x[max_i+2]);
435       if (max_i > 0)
436          floor_ener -= MULT16_16(x[max_i-1], x[max_i-1]);
437       if (max_i > 1)
438          floor_ener -= MULT16_16(x[max_i-2], x[max_i-2]);
439       floor_ener = MAX32(floor_ener, EPSILON);
440 #endif
441       if (N>7 && eBands[i] >= m->pitchEnd)
442       {
443          celt_word16_t r;
444          celt_word16_t den = celt_sqrt(floor_ener);
445          den = MAX32(QCONST16(.02, 15), den);
446          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
447          ratio = ADD32(ratio, EXTEND32(r));
448          NR++;
449       }
450    }
451    if (NR>0)
452       ratio = DIV32_16(ratio, NR);
453    ratio = ADD32(HALF32(ratio), HALF32(*average));
454    if (!*last_decision)
455    {
456       *last_decision = (ratio < QCONST16(1.8,8));
457    } else {
458       *last_decision = (ratio < QCONST16(3.,8));
459    }
460    *average = EXTRACT16(ratio);
461    return *last_decision;
462 }
463
464 /* Quantisation of the residual */
465 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)
466 {
467    int i, j, remaining_bits, balance;
468    const celt_int16_t * restrict eBands = m->eBands;
469    celt_norm_t * restrict norm;
470    VARDECL(celt_norm_t, _norm);   const celt_int16_t *pBands = m->pBands;
471    int pband=-1;
472    int B;
473    SAVE_STACK;
474
475    B = shortBlocks ? m->nbShortMdcts : 1;
476    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
477    norm = _norm;
478
479    balance = 0;
480    /*printf("bits left: %d\n", bits);
481    for (i=0;i<m->nbEBands;i++)
482       printf ("(%d %d) ", pulses[i], ebits[i]);
483    printf ("\n");*/
484    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
485    for (i=0;i<m->nbEBands;i++)
486    {
487       int tell;
488       int N;
489       int q;
490       celt_word16_t n;
491       const celt_int16_t * const *BPbits;
492       
493       int curr_balance, curr_bits;
494       
495       N = eBands[i+1]-eBands[i];
496       BPbits = m->bits;
497
498       tell = ec_enc_tell(enc, 4);
499       if (i != 0)
500          balance -= tell;
501       remaining_bits = (total_bits<<BITRES)-tell-1;
502       curr_balance = (m->nbEBands-i);
503       if (curr_balance > 3)
504          curr_balance = 3;
505       curr_balance = balance / curr_balance;
506       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
507       curr_bits = pulses2bits(BPbits[i], N, q);
508       remaining_bits -= curr_bits;
509       while (remaining_bits < 0 && q > 0)
510       {
511          remaining_bits += curr_bits;
512          q--;
513          curr_bits = pulses2bits(BPbits[i], N, q);
514          remaining_bits -= curr_bits;
515       }
516       balance += pulses[i] + tell;
517       
518       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
519
520       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
521       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
522       {
523          int enabled = 1;
524          pband++;
525          if (remaining_bits >= 1<<BITRES) {
526            enabled = pgains[pband] > QCONST16(.5,15);
527            ec_enc_bits(enc, enabled, 1);
528            balance += 1<<BITRES;
529          }
530          if (enabled)
531             pgains[pband] = QCONST16(.9,15);
532          else
533             pgains[pband] = 0;
534       }
535
536       /* If pitch isn't available, use intra-frame prediction */
537       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
538       {
539          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
540       } else if (pitch_used && eBands[i] < m->pitchEnd) {
541          for (j=eBands[i];j<eBands[i+1];j++)
542             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
543       } else {
544          for (j=eBands[i];j<eBands[i+1];j++)
545             P[j] = 0;
546       }
547       
548       if (q > 0)
549       {
550          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], enc);
551       } else {
552          for (j=eBands[i];j<eBands[i+1];j++)
553             X[j] = P[j];
554       }
555       for (j=eBands[i];j<eBands[i+1];j++)
556          norm[j] = MULT16_16_Q15(n,X[j]);
557    }
558    RESTORE_STACK;
559 }
560
561 #ifndef DISABLE_STEREO
562
563 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)
564 {
565    int i, j, remaining_bits, balance;
566    const celt_int16_t * restrict eBands = m->eBands;
567    celt_norm_t * restrict norm;
568    VARDECL(celt_norm_t, _norm);
569    const int C = CHANNELS(m);
570    const celt_int16_t *pBands = m->pBands;
571    int pband=-1;
572    int B;
573    celt_word16_t mid, side;
574    SAVE_STACK;
575
576    B = shortBlocks ? m->nbShortMdcts : 1;
577    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
578    norm = _norm;
579
580    balance = 0;
581    /*printf("bits left: %d\n", bits);
582    for (i=0;i<m->nbEBands;i++)
583    printf ("(%d %d) ", pulses[i], ebits[i]);
584    printf ("\n");*/
585    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
586    for (i=0;i<m->nbEBands;i++)
587    {
588       int tell;
589       int q1, q2;
590       celt_word16_t n;
591       const celt_int16_t * const *BPbits;
592       int b, qb;
593       int N;
594       int curr_balance, curr_bits;
595       int imid, iside, itheta;
596       int mbits, sbits, delta;
597       int qalloc;
598       
599       BPbits = m->bits;
600
601       N = eBands[i+1]-eBands[i];
602       tell = ec_enc_tell(enc, 4);
603       if (i != 0)
604          balance -= tell;
605       remaining_bits = (total_bits<<BITRES)-tell-1;
606       curr_balance = (m->nbEBands-i);
607       if (curr_balance > 3)
608          curr_balance = 3;
609       curr_balance = balance / curr_balance;
610       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
611       if (b<0)
612          b = 0;
613
614       if (N<5) {
615          
616          q1 = bits2pulses(m, BPbits[i], N, b/2);
617          curr_bits = 2*pulses2bits(BPbits[i], N, q1);
618          remaining_bits -= curr_bits;
619          while (remaining_bits < 0 && q1 > 0)
620          {
621             remaining_bits += curr_bits;
622             q1--;
623             curr_bits = 2*pulses2bits(BPbits[i], N, q1);
624             remaining_bits -= curr_bits;
625          }
626          balance += pulses[i] + tell;
627          
628          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
629          
630          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
631          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
632          {
633             int enabled = 1;
634             pband++;
635             if (remaining_bits >= 1<<BITRES) {
636                enabled = pgains[pband] > QCONST16(.5,15);
637                ec_enc_bits(enc, enabled, 1);
638                balance += 1<<BITRES;
639             }
640             if (enabled)
641                pgains[pband] = QCONST16(.9,15);
642             else
643                pgains[pband] = 0;
644          }
645
646          /* If pitch isn't available, use intra-frame prediction */
647          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
648          {
649             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
650             deinterleave(P+C*eBands[i], C*N);
651          } else if (pitch_used && eBands[i] < m->pitchEnd) {
652             deinterleave(P+C*eBands[i], C*N);
653             for (j=C*eBands[i];j<C*eBands[i+1];j++)
654                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
655          } else {
656             for (j=C*eBands[i];j<C*eBands[i+1];j++)
657                P[j] = 0;
658          }
659          deinterleave(X+C*eBands[i], C*N);
660          if (q1 > 0)
661          {
662             alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
663             alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q1, P+C*eBands[i]+N, enc);
664          } else {
665             for (j=C*eBands[i];j<C*eBands[i+1];j++)
666                X[j] = P[j];
667          }
668
669          interleave(X+C*eBands[i], C*N);
670          for (j=0;j<C*N;j++)
671             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
672
673       } else {
674       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
675       if (qb > (b>>BITRES)-1)
676          qb = (b>>BITRES)-1;
677       if (qb<0)
678          qb = 0;
679       if (qb>14)
680          qb = 14;
681       
682       if (qb==0)
683          point_stereo_mix(m, X, bandE, i, 1);
684       else
685          stereo_band_mix(m, X, bandE, 0, i, 1);
686       
687       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
688       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
689 #ifdef FIXED_POINT
690       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
691 #else
692       itheta = floor(.5+16384*0.63662*atan2(side,mid));
693 #endif
694       qalloc = log2_frac((1<<qb)+1,4);
695       if (qb==0)
696       {
697          itheta=0;
698       } else {
699          int shift;
700          shift = 14-qb;
701          itheta = (itheta+(1<<shift>>1))>>shift;
702          ec_enc_uint(enc, itheta, (1<<qb)+1);
703          itheta <<= shift;
704       }
705       if (itheta == 0)
706       {
707          imid = 32767;
708          iside = 0;
709          delta = -10000;
710       } else if (itheta == 16384)
711       {
712          imid = 0;
713          iside = 32767;
714          delta = 10000;
715       } else {
716          imid = bitexact_cos(itheta);
717          iside = bitexact_cos(16384-itheta);
718          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
719       }
720       mbits = (b-qalloc/2-delta)/2;
721       if (mbits > b-qalloc)
722          mbits = b-qalloc;
723       if (mbits<0)
724          mbits=0;
725       sbits = b-qalloc-mbits;
726       q1 = bits2pulses(m, BPbits[i], N, mbits);
727       q2 = bits2pulses(m, BPbits[i], N, sbits);
728       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
729       remaining_bits -= curr_bits;
730       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
731       {
732          remaining_bits += curr_bits;
733          if (q1>q2)
734          {
735             q1--;
736             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
737          } else {
738             q2--;
739             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
740          }
741          remaining_bits -= curr_bits;
742       }
743       balance += pulses[i] + tell;
744       
745       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
746
747       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
748       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
749       {
750          int enabled = 1;
751          pband++;
752          if (remaining_bits >= 1<<BITRES) {
753             enabled = pgains[pband] > QCONST16(.5,15);
754             ec_enc_bits(enc, enabled, 1);
755             balance += 1<<BITRES;
756          }
757          if (enabled)
758             pgains[pband] = QCONST16(.9,15);
759          else
760             pgains[pband] = 0;
761       }
762       
763
764       /* If pitch isn't available, use intra-frame prediction */
765       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
766       {
767          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
768          if (qb==0)
769             point_stereo_mix(m, P, bandE, i, 1);
770          else
771             stereo_band_mix(m, P, bandE, 0, i, 1);
772          deinterleave(P+C*eBands[i], C*N);
773
774          /*for (j=C*eBands[i];j<C*eBands[i+1];j++)
775             P[j] = 0;*/
776       } else if (pitch_used && eBands[i] < m->pitchEnd) {
777          if (qb==0)
778             point_stereo_mix(m, P, bandE, i, 1);
779          else
780             stereo_band_mix(m, P, bandE, 0, i, 1);
781          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
782          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
783          deinterleave(P+C*eBands[i], C*N);
784          for (j=C*eBands[i];j<C*eBands[i+1];j++)
785             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
786       } else {
787          for (j=C*eBands[i];j<C*eBands[i+1];j++)
788             P[j] = 0;
789       }
790       deinterleave(X+C*eBands[i], C*N);
791       if (q1 > 0)
792          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
793       else
794          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
795             X[j] = P[j];
796       if (q2 > 0)
797          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
798       else
799          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
800             X[j] = 0;
801       /*   orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
802
803
804 #ifdef FIXED_POINT
805       mid = imid;
806       side = iside;
807 #else
808       mid = (1./32768)*imid;
809       side = (1./32768)*iside;
810 #endif
811       for (j=0;j<N;j++)
812          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
813       for (j=0;j<N;j++)
814          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
815
816       interleave(X+C*eBands[i], C*N);
817
818       stereo_band_mix(m, X, bandE, 0, i, -1);
819       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
820       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
821       for (j=0;j<C*N;j++)
822          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
823       }
824    }
825    RESTORE_STACK;
826 }
827 #endif /* DISABLE_STEREO */
828
829 /* Decoding of the residual */
830 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)
831 {
832    int i, j, remaining_bits, balance;
833    const celt_int16_t * restrict eBands = m->eBands;
834    celt_norm_t * restrict norm;
835    VARDECL(celt_norm_t, _norm);
836    const celt_int16_t *pBands = m->pBands;
837    int pband=-1;
838    int B;
839    SAVE_STACK;
840
841    B = shortBlocks ? m->nbShortMdcts : 1;
842    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
843    norm = _norm;
844
845    balance = 0;
846    for (i=0;i<m->nbEBands;i++)
847    {
848       int tell;
849       int N;
850       int q;
851       celt_word16_t n;
852       const celt_int16_t * const *BPbits;
853       
854       int curr_balance, curr_bits;
855
856       N = eBands[i+1]-eBands[i];
857       BPbits = m->bits;
858
859       tell = ec_dec_tell(dec, 4);
860       if (i != 0)
861          balance -= tell;
862       remaining_bits = (total_bits<<BITRES)-tell-1;
863       curr_balance = (m->nbEBands-i);
864       if (curr_balance > 3)
865          curr_balance = 3;
866       curr_balance = balance / curr_balance;
867       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
868       curr_bits = pulses2bits(BPbits[i], N, q);
869       remaining_bits -= curr_bits;
870       while (remaining_bits < 0 && q > 0)
871       {
872          remaining_bits += curr_bits;
873          q--;
874          curr_bits = pulses2bits(BPbits[i], N, q);
875          remaining_bits -= curr_bits;
876       }
877       balance += pulses[i] + tell;
878
879       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
880
881       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
882       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
883       {
884          int enabled = 1;
885          pband++;
886          if (remaining_bits >= 1<<BITRES) {
887            enabled = ec_dec_bits(dec, 1);
888            balance += 1<<BITRES;
889          }
890          if (enabled)
891             pgains[pband] = QCONST16(.9,15);
892          else
893             pgains[pband] = 0;
894       }
895
896       /* If pitch isn't available, use intra-frame prediction */
897       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
898       {
899          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
900       } else if (pitch_used && eBands[i] < m->pitchEnd) {
901          for (j=eBands[i];j<eBands[i+1];j++)
902             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
903       } else {
904          for (j=eBands[i];j<eBands[i+1];j++)
905             P[j] = 0;
906       }
907       
908       if (q > 0)
909       {
910          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
911       } else {
912          for (j=eBands[i];j<eBands[i+1];j++)
913             X[j] = P[j];
914       }
915       for (j=eBands[i];j<eBands[i+1];j++)
916          norm[j] = MULT16_16_Q15(n,X[j]);
917    }
918    RESTORE_STACK;
919 }
920
921 #ifndef DISABLE_STEREO
922
923 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)
924 {
925    int i, j, remaining_bits, balance;
926    const celt_int16_t * restrict eBands = m->eBands;
927    celt_norm_t * restrict norm;
928    VARDECL(celt_norm_t, _norm);
929    const int C = CHANNELS(m);
930    const celt_int16_t *pBands = m->pBands;
931    int pband=-1;
932    int B;
933    celt_word16_t mid, side;
934    SAVE_STACK;
935
936    B = shortBlocks ? m->nbShortMdcts : 1;
937    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
938    norm = _norm;
939
940    balance = 0;
941    /*printf("bits left: %d\n", bits);
942    for (i=0;i<m->nbEBands;i++)
943    printf ("(%d %d) ", pulses[i], ebits[i]);
944    printf ("\n");*/
945    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
946    for (i=0;i<m->nbEBands;i++)
947    {
948       int tell;
949       int q1, q2;
950       celt_word16_t n;
951       const celt_int16_t * const *BPbits;
952       int b, qb;
953       int N;
954       int curr_balance, curr_bits;
955       int imid, iside, itheta;
956       int mbits, sbits, delta;
957       int qalloc;
958       
959       BPbits = m->bits;
960
961       N = eBands[i+1]-eBands[i];
962       tell = ec_dec_tell(dec, 4);
963       if (i != 0)
964          balance -= tell;
965       remaining_bits = (total_bits<<BITRES)-tell-1;
966       curr_balance = (m->nbEBands-i);
967       if (curr_balance > 3)
968          curr_balance = 3;
969       curr_balance = balance / curr_balance;
970       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
971       if (b<0)
972          b = 0;
973       
974       if (N<5) {
975          
976          q1 = bits2pulses(m, BPbits[i], N, b/2);
977          curr_bits = 2*pulses2bits(BPbits[i], N, q1);
978          remaining_bits -= curr_bits;
979          while (remaining_bits < 0 && q1 > 0)
980          {
981             remaining_bits += curr_bits;
982             q1--;
983             curr_bits = 2*pulses2bits(BPbits[i], N, q1);
984             remaining_bits -= curr_bits;
985          }
986          balance += pulses[i] + tell;
987          
988          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
989          
990          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
991          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
992          {
993             int enabled = 1;
994             pband++;
995             if (remaining_bits >= 1<<BITRES) {
996                enabled = pgains[pband] > QCONST16(.5,15);
997                enabled = ec_dec_bits(dec, 1);
998                balance += 1<<BITRES;
999             }
1000             if (enabled)
1001                pgains[pband] = QCONST16(.9,15);
1002             else
1003                pgains[pband] = 0;
1004          }
1005          
1006          /* If pitch isn't available, use intra-frame prediction */
1007          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
1008          {
1009             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
1010             deinterleave(P+C*eBands[i], C*N);
1011          } else if (pitch_used && eBands[i] < m->pitchEnd) {
1012             deinterleave(P+C*eBands[i], C*N);
1013             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1014                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1015          } else {
1016             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1017                P[j] = 0;
1018          }
1019          if (q1 > 0)
1020          {
1021             alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
1022             alg_unquant(X+C*eBands[i]+N, N, q1, P+C*eBands[i]+N, dec);
1023          } else {
1024             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1025                X[j] = P[j];
1026          }
1027          
1028          interleave(X+C*eBands[i], C*N);
1029          for (j=0;j<C*N;j++)
1030             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
1031
1032       } else {
1033       
1034       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
1035       if (qb > (b>>BITRES)-1)
1036          qb = (b>>BITRES)-1;
1037       if (qb>14)
1038          qb = 14;
1039       if (qb<0)
1040          qb = 0;
1041       qalloc = log2_frac((1<<qb)+1,4);
1042       if (qb==0)
1043       {
1044          itheta=0;
1045       } else {
1046          int shift;
1047          shift = 14-qb;
1048          itheta = ec_dec_uint(dec, (1<<qb)+1);
1049          itheta <<= shift;
1050       }
1051       if (itheta == 0)
1052       {
1053          imid = 32767;
1054          iside = 0;
1055          delta = -10000;
1056       } else if (itheta == 16384)
1057       {
1058          imid = 0;
1059          iside = 32767;
1060          delta = 10000;
1061       } else {
1062          imid = bitexact_cos(itheta);
1063          iside = bitexact_cos(16384-itheta);
1064          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
1065       }
1066       mbits = (b-qalloc/2-delta)/2;
1067       if (mbits > b-qalloc)
1068          mbits = b-qalloc;
1069       if (mbits<0)
1070          mbits=0;
1071       sbits = b-qalloc-mbits;
1072       q1 = bits2pulses(m, BPbits[i], N, mbits);
1073       q2 = bits2pulses(m, BPbits[i], N, sbits);
1074       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1075       remaining_bits -= curr_bits;
1076       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
1077       {
1078          remaining_bits += curr_bits;
1079          if (q1>q2)
1080          {
1081             q1--;
1082             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1083          } else {
1084             q2--;
1085             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1086          }
1087          remaining_bits -= curr_bits;
1088       }
1089       balance += pulses[i] + tell;
1090       
1091       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
1092
1093       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
1094       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
1095       {
1096          int enabled = 1;
1097          pband++;
1098          if (remaining_bits >= 1<<BITRES) {
1099             enabled = pgains[pband] > QCONST16(.5,15);
1100             enabled = ec_dec_bits(dec, 1);
1101             balance += 1<<BITRES;
1102          }
1103          if (enabled)
1104             pgains[pband] = QCONST16(.9,15);
1105          else
1106             pgains[pband] = 0;
1107       }
1108
1109       /* If pitch isn't available, use intra-frame prediction */
1110       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
1111       {
1112          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
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          deinterleave(P+C*eBands[i], C*N);
1118       } else if (pitch_used && eBands[i] < m->pitchEnd) {
1119          if (qb==0)
1120             point_stereo_mix(m, P, bandE, i, 1);
1121          else
1122             stereo_band_mix(m, P, bandE, 0, i, 1);
1123          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1124          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1125          deinterleave(P+C*eBands[i], C*N);
1126          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1127             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1128       } else {
1129          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1130             P[j] = 0;
1131       }
1132       deinterleave(X+C*eBands[i], C*N);
1133       if (q1 > 0)
1134          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
1135       else
1136          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
1137             X[j] = P[j];
1138       if (q2 > 0)
1139          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
1140       else
1141          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
1142             X[j] = 0;
1143       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
1144       
1145 #ifdef FIXED_POINT
1146       mid = imid;
1147       side = iside;
1148 #else
1149       mid = (1./32768)*imid;
1150       side = (1./32768)*iside;
1151 #endif
1152       for (j=0;j<N;j++)
1153          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
1154       for (j=0;j<N;j++)
1155          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1156       
1157       interleave(X+C*eBands[i], C*N);
1158
1159       stereo_band_mix(m, X, bandE, 0, i, -1);
1160       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1161       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1162       for (j=0;j<C*N;j++)
1163          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
1164       }
1165    }
1166    RESTORE_STACK;
1167 }
1168
1169 #endif /* DISABLE_STEREO */