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