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