Note some more platforms where float-approx is tested, fix a bug in the prediction...
[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 = ec_dec_bits(dec, 1);
920                balance += 1<<BITRES;
921             }
922             if (enabled)
923                pgains[pband] = QCONST16(.9,15);
924             else
925                pgains[pband] = 0;
926          }
927          
928          /* If pitch isn't available, use intra-frame prediction */
929          if ((eBands[i] >= m->pitchEnd && fold) || q1<=0)
930          {
931             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1, norm, P+C*eBands[i], eBands[i], B);
932             deinterleave(P+C*eBands[i], C*N);
933          } else if (pitch_used && eBands[i] < m->pitchEnd) {
934             deinterleave(P+C*eBands[i], C*N);
935             for (j=C*eBands[i];j<C*eBands[i+1];j++)
936                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
937          } else {
938             for (j=C*eBands[i];j<C*eBands[i+1];j++)
939                P[j] = 0;
940          }
941          if (q1 > 0)
942          {
943             alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
944             alg_unquant(X+C*eBands[i]+N, N, q1, P+C*eBands[i]+N, dec);
945          } else {
946             for (j=C*eBands[i];j<C*eBands[i+1];j++)
947                X[j] = P[j];
948          }
949          
950          interleave(X+C*eBands[i], C*N);
951          for (j=0;j<C*N;j++)
952             norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
953
954       } else {
955       
956       qb = (b-2*(N-1)*(40-log2_frac(N,4)))/(32*(N-1));
957       if (qb > (b>>BITRES)-1)
958          qb = (b>>BITRES)-1;
959       if (qb>14)
960          qb = 14;
961       if (qb<0)
962          qb = 0;
963       qalloc = log2_frac((1<<qb)+1,4);
964       if (qb==0)
965       {
966          itheta=0;
967       } else {
968          int shift;
969          shift = 14-qb;
970          itheta = ec_dec_uint(dec, (1<<qb)+1);
971          itheta <<= shift;
972       }
973       if (itheta == 0)
974       {
975          imid = 32767;
976          iside = 0;
977          delta = -10000;
978       } else if (itheta == 16384)
979       {
980          imid = 0;
981          iside = 32767;
982          delta = 10000;
983       } else {
984          imid = bitexact_cos(itheta);
985          iside = bitexact_cos(16384-itheta);
986          delta = (N-1)*(log2_frac(iside,6)-log2_frac(imid,6))>>2;
987       }
988       mbits = (b-qalloc/2-delta)/2;
989       if (mbits > b-qalloc)
990          mbits = b-qalloc;
991       if (mbits<0)
992          mbits=0;
993       sbits = b-qalloc-mbits;
994       q1 = bits2pulses(m, BPbits[i], N, mbits);
995       q2 = bits2pulses(m, BPbits[i], N, sbits);
996       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
997       remaining_bits -= curr_bits;
998       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
999       {
1000          remaining_bits += curr_bits;
1001          if (q1>q2)
1002          {
1003             q1--;
1004             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1005          } else {
1006             q2--;
1007             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1008          }
1009          remaining_bits -= curr_bits;
1010       }
1011       balance += pulses[i] + tell;
1012       
1013       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
1014
1015       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
1016       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
1017       {
1018          int enabled = 1;
1019          pband++;
1020          if (remaining_bits >= 1<<BITRES) {
1021             enabled = ec_dec_bits(dec, 1);
1022             balance += 1<<BITRES;
1023          }
1024          if (enabled)
1025             pgains[pband] = QCONST16(.9,15);
1026          else
1027             pgains[pband] = 0;
1028       }
1029
1030       /* If pitch isn't available, use intra-frame prediction */
1031       if ((eBands[i] >= m->pitchEnd && fold) || (q1+q2)<=0)
1032       {
1033          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q1+q2, norm, P+C*eBands[i], eBands[i], B);
1034          stereo_band_mix(m, P, bandE, qb==0, i, 1);
1035          deinterleave(P+C*eBands[i], C*N);
1036       } else if (pitch_used && eBands[i] < m->pitchEnd) {
1037          stereo_band_mix(m, P, bandE, qb==0, i, 1);
1038          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1039          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1040          deinterleave(P+C*eBands[i], C*N);
1041          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1042             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1043       } else {
1044          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1045             P[j] = 0;
1046       }
1047       deinterleave(X+C*eBands[i], C*N);
1048       if (q1 > 0)
1049          alg_unquant(X+C*eBands[i], N, q1, P+C*eBands[i], dec);
1050       else
1051          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
1052             X[j] = P[j];
1053       if (q2 > 0)
1054          alg_unquant(X+C*eBands[i]+N, N, q2, P+C*eBands[i]+N, dec);
1055       else
1056          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
1057             X[j] = 0;
1058       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
1059       
1060 #ifdef FIXED_POINT
1061       mid = imid;
1062       side = iside;
1063 #else
1064       mid = (1./32768)*imid;
1065       side = (1./32768)*iside;
1066 #endif
1067       for (j=0;j<N;j++)
1068          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
1069       for (j=0;j<N;j++)
1070          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1071       
1072       interleave(X+C*eBands[i], C*N);
1073
1074       stereo_band_mix(m, X, bandE, 0, i, -1);
1075       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1076       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1077       for (j=0;j<C*N;j++)
1078          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
1079       }
1080    }
1081    RESTORE_STACK;
1082 }
1083
1084 #endif /* DISABLE_STEREO */