Removing the 64-bit part of the range coder.
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 Jean-Marc Valin, CSIRO
2 */
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(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
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 /* Normalise each band such that the energy is one. */
132 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const 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_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
143          for (j=eBands[i];j<eBands[i+1];j++)
144             X[j*C+c] = freq[j*C+c]*g;
145       }
146    }
147 }
148
149 #endif /* FIXED_POINT */
150
151 #ifndef DISABLE_STEREO
152 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
153 {
154    int i, c;
155    const celt_int16_t *eBands = m->eBands;
156    const int C = CHANNELS(m);
157    for (c=0;c<C;c++)
158    {
159       i=0; do {
160          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
161       } while (++i<m->nbEBands);
162    }
163 }
164 #endif /* DISABLE_STEREO */
165
166 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
167 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
168 {
169    int i, c;
170    const celt_int16_t *eBands = m->eBands;
171    const int C = CHANNELS(m);
172    if (C>2)
173       celt_fatal("denormalise_bands() not implemented for >2 channels");
174    for (c=0;c<C;c++)
175    {
176       for (i=0;i<m->nbEBands;i++)
177       {
178          int j;
179          celt_word32_t g = MULT16_32_Q15(sqrtC_1[C-1],bank[i*C+c]);
180          j=eBands[i]; do {
181             freq[j*C+c] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
182          } while (++j<eBands[i+1]);
183       }
184    }
185    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
186       freq[i] = 0;
187 }
188
189
190 /* Compute the best gain for each "pitch band" */
191 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
192 {
193    int i;
194    const celt_int16_t *pBands = m->pBands;
195    const int C = CHANNELS(m);
196
197    for (i=0;i<m->nbPBands;i++)
198    {
199       celt_word32_t Sxy=0, Sxx=0;
200       int j;
201       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
202       for (j=C*pBands[i];j<C*pBands[i+1];j++)
203       {
204          Sxy = MAC16_16(Sxy, X[j], P[j]);
205          Sxx = MAC16_16(Sxx, X[j], X[j]);
206       }
207       /* No negative gain allowed */
208       if (Sxy < 0)
209          Sxy = 0;
210       /* Not sure how that would happen, just making sure */
211       if (Sxy > Sxx)
212          Sxy = Sxx;
213       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
214          residual doesn't quantise well */
215       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
216       /* gain = Sxy/Sxx */
217       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
218       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
219    }
220    /*if(rand()%10==0)
221    {
222       for (i=0;i<m->nbPBands;i++)
223          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
224       printf ("\n");
225    }*/
226 }
227
228 /* Apply the (quantised) gain to each "pitch band" */
229 void pitch_quant_bands(const CELTMode *m, celt_norm_t * restrict P, const celt_pgain_t * restrict gains)
230 {
231    int i;
232    const celt_int16_t *pBands = m->pBands;
233    const int C = CHANNELS(m);
234    for (i=0;i<m->nbPBands;i++)
235    {
236       int j;
237       for (j=C*pBands[i];j<C*pBands[i+1];j++)
238          P[j] = MULT16_16_Q15(gains[i], P[j]);
239       /*printf ("%f ", gain);*/
240    }
241    for (i=C*pBands[m->nbPBands];i<C*pBands[m->nbPBands+1];i++)
242       P[i] = 0;
243 }
244
245 static void intensity_band(celt_norm_t * restrict X, int len)
246 {
247    int j;
248    celt_word32_t E = 1e-15;
249    celt_word32_t E2 = 1e-15;
250    for (j=0;j<len;j++)
251    {
252       X[j] = X[2*j];
253       E = MAC16_16(E, X[j],X[j]);
254       E2 = MAC16_16(E2, X[2*j+1],X[2*j+1]);
255    }
256 #ifndef FIXED_POINT
257    E  = celt_sqrt(E+E2)/celt_sqrt(E);
258    for (j=0;j<len;j++)
259       X[j] *= E;
260 #endif
261    for (j=0;j<len;j++)
262       X[len+j] = 0;
263
264 }
265
266 static void dup_band(celt_norm_t * restrict X, int len)
267 {
268    int j;
269    for (j=len-1;j>=0;j--)
270    {
271       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
272       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
273    }
274 }
275
276 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)
277 {
278    int i = bandID;
279    const celt_int16_t *eBands = m->eBands;
280    const int C = CHANNELS(m);
281    {
282       int j;
283       if (stereo_mode[i] && dir <0)
284       {
285          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
286       } else {
287          celt_word16_t a1, a2;
288          if (stereo_mode[i]==0)
289          {
290             /* Do mid-side when not doing intensity stereo */
291             a1 = QCONST16(.70711f,14);
292             a2 = dir*QCONST16(.70711f,14);
293          } else {
294             celt_word16_t left, right;
295             celt_word16_t norm;
296 #ifdef FIXED_POINT
297             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
298 #endif
299             left = VSHR32(bank[i*C],shift);
300             right = VSHR32(bank[i*C+1],shift);
301             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
302             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
303             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
304          }
305          for (j=eBands[i];j<eBands[i+1];j++)
306          {
307             celt_norm_t r, l;
308             l = X[j*C];
309             r = X[j*C+1];
310             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
311             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
312          }
313       }
314       if (stereo_mode[i] && dir>0)
315       {
316          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
317       }
318    }
319 }
320
321 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
322 {
323    int i;
324    for (i=0;i<len-5;i++)
325       stereo_mode[i] = 0;
326    for (;i<len;i++)
327       stereo_mode[i] = 0;
328 }
329
330
331
332 /* Quantisation of the residual */
333 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int total_bits, ec_enc *enc)
334 {
335    int i, j, remaining_bits, balance;
336    const celt_int16_t * restrict eBands = m->eBands;
337    celt_norm_t * restrict norm;
338    VARDECL(celt_norm_t, _norm);
339    const int C = CHANNELS(m);
340    int B;
341    SAVE_STACK;
342
343    B = shortBlocks ? m->nbShortMdcts : 1;
344    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
345    norm = _norm;
346
347    balance = 0;
348    /*printf("bits left: %d\n", bits);
349    for (i=0;i<m->nbEBands;i++)
350       printf ("(%d %d) ", pulses[i], ebits[i]);
351    printf ("\n");*/
352    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
353    for (i=0;i<m->nbEBands;i++)
354    {
355       int tell;
356       int q;
357       celt_word16_t n;
358       const celt_int16_t * const *BPbits;
359       
360       int curr_balance, curr_bits;
361       
362       if (C>1 && stereo_mode[i]==0)
363          BPbits = m->bits_stereo;
364       else
365          BPbits = m->bits;
366
367       tell = ec_enc_tell(enc, 4);
368       if (i != 0)
369          balance -= tell;
370       remaining_bits = (total_bits<<BITRES)-tell-2;
371       curr_balance = (m->nbEBands-i);
372       if (curr_balance > 3)
373          curr_balance = 3;
374       curr_balance = balance / curr_balance;
375       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
376       curr_bits = BPbits[i][q];
377       remaining_bits -= curr_bits;
378       if (remaining_bits < 0)
379       {
380          q--;
381          remaining_bits += curr_bits;
382          curr_bits = BPbits[i][q];
383          remaining_bits -= curr_bits;
384       }
385       balance += pulses[i] + tell;
386       
387       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
388
389       /* If pitch isn't available, use intra-frame prediction */
390       if (eBands[i] >= m->pitchEnd || q<=0)
391       {
392          q -= 1;
393          if (q<0)
394             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
395          else
396             intra_prediction(m, X+C*eBands[i], W+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B, enc);
397       }
398       
399       if (q > 0)
400       {
401          int ch=C;
402          if (C==2 && stereo_mode[i]==1)
403             ch = 1;
404          if (C==2)
405          {
406             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
407             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
408          }
409          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
410          if (C==2)
411             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
412       } else {
413          for (j=C*eBands[i];j<C*eBands[i+1];j++)
414             X[j] = P[j];
415       }
416       for (j=C*eBands[i];j<C*eBands[i+1];j++)
417          norm[j] = MULT16_16_Q15(n,X[j]);
418    }
419    RESTORE_STACK;
420 }
421
422 /* Decoding of the residual */
423 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int total_bits, ec_dec *dec)
424 {
425    int i, j, remaining_bits, balance;
426    const celt_int16_t * restrict eBands = m->eBands;
427    celt_norm_t * restrict norm;
428    VARDECL(celt_norm_t, _norm);
429    const int C = CHANNELS(m);
430    int B;
431    SAVE_STACK;
432
433    B = shortBlocks ? m->nbShortMdcts : 1;
434    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
435    norm = _norm;
436
437    balance = 0;
438    for (i=0;i<m->nbEBands;i++)
439    {
440       int tell;
441       int q;
442       celt_word16_t n;
443       const celt_int16_t * const *BPbits;
444       
445       int curr_balance, curr_bits;
446       
447       if (C>1 && stereo_mode[i]==0)
448          BPbits = m->bits_stereo;
449       else
450          BPbits = m->bits;
451
452       tell = ec_dec_tell(dec, 4);
453       if (i != 0)
454          balance -= tell;
455       remaining_bits = (total_bits<<BITRES)-tell-2;
456       curr_balance = (m->nbEBands-i);
457       if (curr_balance > 3)
458          curr_balance = 3;
459       curr_balance = balance / curr_balance;
460       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
461       curr_bits = BPbits[i][q];
462       remaining_bits -= curr_bits;
463       if (remaining_bits < 0)
464       {
465          q--;
466          remaining_bits += curr_bits;
467          curr_bits = BPbits[i][q];
468          remaining_bits -= curr_bits;
469       }
470       balance += pulses[i] + tell;
471
472       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
473
474       /* If pitch isn't available, use intra-frame prediction */
475       if (eBands[i] >= m->pitchEnd || q<=0)
476       {
477          q -= 1;
478          if (q<0)
479             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
480          else
481             intra_unquant(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B, dec);
482       }
483       
484       if (q > 0)
485       {
486          int ch=C;
487          if (C==2 && stereo_mode[i]==1)
488             ch = 1;
489          if (C==2)
490             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
491          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
492          if (C==2)
493             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
494       } else {
495          for (j=C*eBands[i];j<C*eBands[i+1];j++)
496             X[j] = P[j];
497       }
498       for (j=C*eBands[i];j<C*eBands[i+1];j++)
499          norm[j] = MULT16_16_Q15(n,X[j]);
500    }
501    RESTORE_STACK;
502 }
503