Now storing the band energies in de-interleaved format when doing stereo
[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*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
78          } else {
79             bank[i+c*m->nbEBands] = EPSILON;
80          }
81          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
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*m->nbEBands])-13;
100          E = VSHR32(bank[i+c*m->nbEBands], 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*m->nbEBands] = sqrt(sum);
125          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
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*m->nbEBands] = sqrt(sum);
146          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
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*m->nbEBands]);
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*m->nbEBands],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], bank[i+m->nbEBands]))-13;
309 #endif
310             left = VSHR32(bank[i],shift);
311             right = VSHR32(bank[i+m->nbEBands],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], bank[i+m->nbEBands]))-13;
343 #endif
344    left = VSHR32(bank[i],shift);
345    right = VSHR32(bank[i+m->nbEBands],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 interleave(celt_norm_t *x, int N)
360 {
361    int i;
362    VARDECL(celt_norm_t, tmp);
363    SAVE_STACK;
364    ALLOC(tmp, N, celt_norm_t);
365    
366    for (i=0;i<N;i++)
367       tmp[i] = x[i];
368    for (i=0;i<N>>1;i++)
369    {
370       x[i<<1] = tmp[i];
371       x[(i<<1)+1] = tmp[i+(N>>1)];
372    }
373    RESTORE_STACK;
374 }
375
376 void deinterleave(celt_norm_t *x, int N)
377 {
378    int i;
379    VARDECL(celt_norm_t, tmp);
380    SAVE_STACK;
381    ALLOC(tmp, N, celt_norm_t);
382    
383    for (i=0;i<N;i++)
384       tmp[i] = x[i];
385    for (i=0;i<N>>1;i++)
386    {
387       x[i] = tmp[i<<1];
388       x[i+(N>>1)] = tmp[(i<<1)+1];
389    }
390    RESTORE_STACK;
391 }
392
393 #endif /* DISABLE_STEREO */
394
395 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
396 {
397    int i;
398    int NR=0;
399    celt_word32_t ratio = EPSILON;
400    const celt_int16_t * restrict eBands = m->eBands;
401    for (i=0;i<m->nbEBands;i++)
402    {
403       int j, N;
404       int max_i=0;
405       celt_word16_t max_val=EPSILON;
406       celt_word32_t floor_ener=EPSILON;
407       celt_norm_t * restrict x = X+eBands[i];
408       N = eBands[i+1]-eBands[i];
409       for (j=0;j<N;j++)
410       {
411          if (ABS16(x[j])>max_val)
412          {
413             max_val = ABS16(x[j]);
414             max_i = j;
415          }
416       }
417 #if 0
418       for (j=0;j<N;j++)
419       {
420          if (abs(j-max_i)>2)
421             floor_ener += x[j]*x[j];
422       }
423 #else
424       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
425       if (max_i < N-1)
426          floor_ener -= MULT16_16(x[max_i+1], x[max_i+1]);
427       if (max_i < N-2)
428          floor_ener -= MULT16_16(x[max_i+2], x[max_i+2]);
429       if (max_i > 0)
430          floor_ener -= MULT16_16(x[max_i-1], x[max_i-1]);
431       if (max_i > 1)
432          floor_ener -= MULT16_16(x[max_i-2], x[max_i-2]);
433       floor_ener = MAX32(floor_ener, EPSILON);
434 #endif
435       if (N>7 && eBands[i] >= m->pitchEnd)
436       {
437          celt_word16_t r;
438          celt_word16_t den = celt_sqrt(floor_ener);
439          den = MAX32(QCONST16(.02, 15), den);
440          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
441          ratio = ADD32(ratio, EXTEND32(r));
442          NR++;
443       }
444    }
445    if (NR>0)
446       ratio = DIV32_16(ratio, NR);
447    ratio = ADD32(HALF32(ratio), HALF32(*average));
448    if (!*last_decision)
449    {
450       *last_decision = (ratio < QCONST16(1.8,8));
451    } else {
452       *last_decision = (ratio < QCONST16(3.,8));
453    }
454    *average = EXTRACT16(ratio);
455    return *last_decision;
456 }
457
458 /* Quantisation of the residual */
459 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)
460 {
461    int i, j, remaining_bits, balance;
462    const celt_int16_t * restrict eBands = m->eBands;
463    celt_norm_t * restrict norm;
464    VARDECL(celt_norm_t, _norm);   const celt_int16_t *pBands = m->pBands;
465    int pband=-1;
466    int B;
467    SAVE_STACK;
468
469    B = shortBlocks ? m->nbShortMdcts : 1;
470    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
471    norm = _norm;
472
473    balance = 0;
474    /*printf("bits left: %d\n", bits);
475    for (i=0;i<m->nbEBands;i++)
476       printf ("(%d %d) ", pulses[i], ebits[i]);
477    printf ("\n");*/
478    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
479    for (i=0;i<m->nbEBands;i++)
480    {
481       int tell;
482       int N;
483       int q;
484       celt_word16_t n;
485       const celt_int16_t * const *BPbits;
486       
487       int curr_balance, curr_bits;
488       
489       N = eBands[i+1]-eBands[i];
490       BPbits = m->bits;
491
492       tell = ec_enc_tell(enc, 4);
493       if (i != 0)
494          balance -= tell;
495       remaining_bits = (total_bits<<BITRES)-tell-1;
496       curr_balance = (m->nbEBands-i);
497       if (curr_balance > 3)
498          curr_balance = 3;
499       curr_balance = balance / curr_balance;
500       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
501       curr_bits = pulses2bits(BPbits[i], N, q);
502       remaining_bits -= curr_bits;
503       while (remaining_bits < 0 && q > 0)
504       {
505          remaining_bits += curr_bits;
506          q--;
507          curr_bits = pulses2bits(BPbits[i], N, q);
508          remaining_bits -= curr_bits;
509       }
510       balance += pulses[i] + tell;
511       
512       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
513
514       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
515       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
516       {
517          int enabled = 1;
518          pband++;
519          if (remaining_bits >= 1<<BITRES) {
520            enabled = pgains[pband] > QCONST16(.5,15);
521            ec_enc_bits(enc, enabled, 1);
522            balance += 1<<BITRES;
523          }
524          if (enabled)
525             pgains[pband] = QCONST16(.9,15);
526          else
527             pgains[pband] = 0;
528       }
529
530       /* If pitch isn't available, use intra-frame prediction */
531       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
532       {
533          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
534       } else if (pitch_used && eBands[i] < m->pitchEnd) {
535          for (j=eBands[i];j<eBands[i+1];j++)
536             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
537       } else {
538          for (j=eBands[i];j<eBands[i+1];j++)
539             P[j] = 0;
540       }
541       
542       if (q > 0)
543       {
544          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], enc);
545       } else {
546          for (j=eBands[i];j<eBands[i+1];j++)
547             X[j] = P[j];
548       }
549       for (j=eBands[i];j<eBands[i+1];j++)
550          norm[j] = MULT16_16_Q15(n,X[j]);
551    }
552    RESTORE_STACK;
553 }
554
555 #ifndef DISABLE_STEREO
556
557 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)
558 {
559    int i, j, remaining_bits, balance;
560    const celt_int16_t * restrict eBands = m->eBands;
561    celt_norm_t * restrict norm;
562    VARDECL(celt_norm_t, _norm);
563    const int C = CHANNELS(m);
564    const celt_int16_t *pBands = m->pBands;
565    int pband=-1;
566    int B;
567    celt_word16_t mid, side;
568    SAVE_STACK;
569
570    B = shortBlocks ? m->nbShortMdcts : 1;
571    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
572    norm = _norm;
573
574    balance = 0;
575    /*printf("bits left: %d\n", bits);
576    for (i=0;i<m->nbEBands;i++)
577    printf ("(%d %d) ", pulses[i], ebits[i]);
578    printf ("\n");*/
579    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
580    for (i=0;i<m->nbEBands;i++)
581    {
582       int tell;
583       int q1, q2;
584       celt_word16_t n;
585       const celt_int16_t * const *BPbits;
586       int b, qb;
587       int N;
588       int curr_balance, curr_bits;
589       int imid, iside, itheta;
590       int mbits, sbits, delta;
591       int qalloc;
592       
593       BPbits = m->bits;
594
595       N = eBands[i+1]-eBands[i];
596       tell = ec_enc_tell(enc, 4);
597       if (i != 0)
598          balance -= tell;
599       remaining_bits = (total_bits<<BITRES)-tell-1;
600       curr_balance = (m->nbEBands-i);
601       if (curr_balance > 3)
602          curr_balance = 3;
603       curr_balance = balance / curr_balance;
604       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
605       if (b<0)
606          b = 0;
607
608       if (N<5) {
609          
610          q1 = bits2pulses(m, BPbits[i], N, b/2);
611          curr_bits = 2*pulses2bits(BPbits[i], N, q1);
612          remaining_bits -= curr_bits;
613          while (remaining_bits < 0 && q1 > 0)
614          {
615             remaining_bits += curr_bits;
616             q1--;
617             curr_bits = 2*pulses2bits(BPbits[i], N, q1);
618             remaining_bits -= curr_bits;
619          }
620          balance += pulses[i] + tell;
621          
622          n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
623          
624          /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
625          if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
626          {
627             int enabled = 1;
628             pband++;
629             if (remaining_bits >= 1<<BITRES) {
630                enabled = pgains[pband] > QCONST16(.5,15);
631                ec_enc_bits(enc, enabled, 1);
632                balance += 1<<BITRES;
633             }
634             if (enabled)
635                pgains[pband] = QCONST16(.9,15);
636             else
637                pgains[pband] = 0;
638          }
639
640          /* If pitch isn't available, use intra-frame prediction */
641          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
642          {
643             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
644             deinterleave(P+C*eBands[i], C*N);
645          } else if (pitch_used && eBands[i] < m->pitchEnd) {
646             deinterleave(P+C*eBands[i], C*N);
647             for (j=C*eBands[i];j<C*eBands[i+1];j++)
648                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
649          } else {
650             for (j=C*eBands[i];j<C*eBands[i+1];j++)
651                P[j] = 0;
652          }
653          deinterleave(X+C*eBands[i], C*N);
654          if (q1 > 0)
655          {
656             alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
657             alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q1, P+C*eBands[i]+N, enc);
658          } else {
659             for (j=C*eBands[i];j<C*eBands[i+1];j++)
660                X[j] = P[j];
661          }
662
663          interleave(X+C*eBands[i], C*N);
664          for (j=0;j<C*N;j++)
665             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
666
667       } else {
668       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
669       if (qb > (b>>BITRES)-1)
670          qb = (b>>BITRES)-1;
671       if (qb<0)
672          qb = 0;
673       if (qb>14)
674          qb = 14;
675       
676       if (qb==0)
677          point_stereo_mix(m, X, bandE, i, 1);
678       else
679          stereo_band_mix(m, X, bandE, 0, i, 1);
680       
681       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
682       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
683 #ifdef FIXED_POINT
684       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
685 #else
686       itheta = floor(.5+16384*0.63662*atan2(side,mid));
687 #endif
688       qalloc = log2_frac((1<<qb)+1,4);
689       if (qb==0)
690       {
691          itheta=0;
692       } else {
693          int shift;
694          shift = 14-qb;
695          itheta = (itheta+(1<<shift>>1))>>shift;
696          ec_enc_uint(enc, itheta, (1<<qb)+1);
697          itheta <<= shift;
698       }
699       if (itheta == 0)
700       {
701          imid = 32767;
702          iside = 0;
703          delta = -10000;
704       } else if (itheta == 16384)
705       {
706          imid = 0;
707          iside = 32767;
708          delta = 10000;
709       } else {
710          imid = bitexact_cos(itheta);
711          iside = bitexact_cos(16384-itheta);
712          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
713       }
714       mbits = (b-qalloc/2-delta)/2;
715       if (mbits > b-qalloc)
716          mbits = b-qalloc;
717       if (mbits<0)
718          mbits=0;
719       sbits = b-qalloc-mbits;
720       q1 = bits2pulses(m, BPbits[i], N, mbits);
721       q2 = bits2pulses(m, BPbits[i], N, sbits);
722       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
723       remaining_bits -= curr_bits;
724       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
725       {
726          remaining_bits += curr_bits;
727          if (q1>q2)
728          {
729             q1--;
730             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
731          } else {
732             q2--;
733             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
734          }
735          remaining_bits -= curr_bits;
736       }
737       balance += pulses[i] + tell;
738       
739       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
740
741       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
742       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
743       {
744          int enabled = 1;
745          pband++;
746          if (remaining_bits >= 1<<BITRES) {
747             enabled = pgains[pband] > QCONST16(.5,15);
748             ec_enc_bits(enc, enabled, 1);
749             balance += 1<<BITRES;
750          }
751          if (enabled)
752             pgains[pband] = QCONST16(.9,15);
753          else
754             pgains[pband] = 0;
755       }
756       
757
758       /* If pitch isn't available, use intra-frame prediction */
759       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
760       {
761          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
762          if (qb==0)
763             point_stereo_mix(m, P, bandE, i, 1);
764          else
765             stereo_band_mix(m, P, bandE, 0, i, 1);
766          deinterleave(P+C*eBands[i], C*N);
767
768          /*for (j=C*eBands[i];j<C*eBands[i+1];j++)
769             P[j] = 0;*/
770       } else if (pitch_used && eBands[i] < m->pitchEnd) {
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          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
776          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
777          deinterleave(P+C*eBands[i], C*N);
778          for (j=C*eBands[i];j<C*eBands[i+1];j++)
779             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
780       } else {
781          for (j=C*eBands[i];j<C*eBands[i+1];j++)
782             P[j] = 0;
783       }
784       deinterleave(X+C*eBands[i], C*N);
785       if (q1 > 0)
786          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, P+C*eBands[i], enc);
787       else
788          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
789             X[j] = P[j];
790       if (q2 > 0)
791          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, P+C*eBands[i]+N, enc);
792       else
793          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
794             X[j] = 0;
795       /*   orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
796
797
798 #ifdef FIXED_POINT
799       mid = imid;
800       side = iside;
801 #else
802       mid = (1./32768)*imid;
803       side = (1./32768)*iside;
804 #endif
805       for (j=0;j<N;j++)
806          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
807       for (j=0;j<N;j++)
808          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
809
810       interleave(X+C*eBands[i], C*N);
811
812       stereo_band_mix(m, X, bandE, 0, i, -1);
813       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
814       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
815       for (j=0;j<C*N;j++)
816          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
817       }
818    }
819    RESTORE_STACK;
820 }
821 #endif /* DISABLE_STEREO */
822
823 /* Decoding of the residual */
824 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)
825 {
826    int i, j, remaining_bits, balance;
827    const celt_int16_t * restrict eBands = m->eBands;
828    celt_norm_t * restrict norm;
829    VARDECL(celt_norm_t, _norm);
830    const celt_int16_t *pBands = m->pBands;
831    int pband=-1;
832    int B;
833    SAVE_STACK;
834
835    B = shortBlocks ? m->nbShortMdcts : 1;
836    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
837    norm = _norm;
838
839    balance = 0;
840    for (i=0;i<m->nbEBands;i++)
841    {
842       int tell;
843       int N;
844       int q;
845       celt_word16_t n;
846       const celt_int16_t * const *BPbits;
847       
848       int curr_balance, curr_bits;
849
850       N = eBands[i+1]-eBands[i];
851       BPbits = m->bits;
852
853       tell = ec_dec_tell(dec, 4);
854       if (i != 0)
855          balance -= tell;
856       remaining_bits = (total_bits<<BITRES)-tell-1;
857       curr_balance = (m->nbEBands-i);
858       if (curr_balance > 3)
859          curr_balance = 3;
860       curr_balance = balance / curr_balance;
861       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
862       curr_bits = pulses2bits(BPbits[i], N, q);
863       remaining_bits -= curr_bits;
864       while (remaining_bits < 0 && q > 0)
865       {
866          remaining_bits += curr_bits;
867          q--;
868          curr_bits = pulses2bits(BPbits[i], N, q);
869          remaining_bits -= curr_bits;
870       }
871       balance += pulses[i] + tell;
872
873       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
874
875       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
876       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
877       {
878          int enabled = 1;
879          pband++;
880          if (remaining_bits >= 1<<BITRES) {
881            enabled = ec_dec_bits(dec, 1);
882            balance += 1<<BITRES;
883          }
884          if (enabled)
885             pgains[pband] = QCONST16(.9,15);
886          else
887             pgains[pband] = 0;
888       }
889
890       /* If pitch isn't available, use intra-frame prediction */
891       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
892       {
893          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], q, norm, P+eBands[i], eBands[i], B);
894       } else if (pitch_used && eBands[i] < m->pitchEnd) {
895          for (j=eBands[i];j<eBands[i+1];j++)
896             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
897       } else {
898          for (j=eBands[i];j<eBands[i+1];j++)
899             P[j] = 0;
900       }
901       
902       if (q > 0)
903       {
904          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, P+eBands[i], dec);
905       } else {
906          for (j=eBands[i];j<eBands[i+1];j++)
907             X[j] = P[j];
908       }
909       for (j=eBands[i];j<eBands[i+1];j++)
910          norm[j] = MULT16_16_Q15(n,X[j]);
911    }
912    RESTORE_STACK;
913 }
914
915 #ifndef DISABLE_STEREO
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 }
1162
1163 #endif /* DISABLE_STEREO */