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