Pitch now quantised at the band level, got rid of all the VQ code.
[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 #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]*sqrt(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 = MULT16_32_Q15(sqrtC_1[C-1],bank[i*C+c]);
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 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
331 {
332    int i;
333    for (i=0;i<len-5;i++)
334       stereo_mode[i] = 0;
335    for (;i<len;i++)
336       stereo_mode[i] = 0;
337 }
338
339
340
341 /* Quantisation of the residual */
342 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)
343 {
344    int i, j, remaining_bits, balance;
345    const celt_int16_t * restrict eBands = m->eBands;
346    celt_norm_t * restrict norm;
347    VARDECL(celt_norm_t, _norm);
348    const int C = CHANNELS(m);
349    const celt_int16_t *pBands = m->pBands;
350    int pband=-1;
351    int B;
352    SAVE_STACK;
353
354    B = shortBlocks ? m->nbShortMdcts : 1;
355    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
356    norm = _norm;
357
358    balance = 0;
359    /*printf("bits left: %d\n", bits);
360    for (i=0;i<m->nbEBands;i++)
361       printf ("(%d %d) ", pulses[i], ebits[i]);
362    printf ("\n");*/
363    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
364    for (i=0;i<m->nbEBands;i++)
365    {
366       int tell;
367       int q;
368       celt_word16_t n;
369       const celt_int16_t * const *BPbits;
370       
371       int curr_balance, curr_bits;
372       
373       if (C>1 && stereo_mode[i]==0)
374          BPbits = m->bits_stereo;
375       else
376          BPbits = m->bits;
377
378       tell = ec_enc_tell(enc, 4);
379       if (i != 0)
380          balance -= tell;
381       remaining_bits = (total_bits<<BITRES)-tell-1;
382       curr_balance = (m->nbEBands-i);
383       if (curr_balance > 3)
384          curr_balance = 3;
385       curr_balance = balance / curr_balance;
386       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
387       curr_bits = BPbits[i][q];
388       remaining_bits -= curr_bits;
389       if (remaining_bits < 0)
390       {
391          remaining_bits += curr_bits;
392          if (q>0) {
393            q--;
394            curr_bits = BPbits[i][q];
395            remaining_bits -= curr_bits;
396          }
397       }
398       balance += pulses[i] + tell;
399       
400       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
401
402       /* If pitch isn't available, use intra-frame prediction */
403       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
404       {
405          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B);
406       } else if (pitch_used && eBands[i] < m->pitchEnd)
407       {
408          if (eBands[i] == pBands[pband+1])
409          {
410             int enabled = 0;
411             pband++;
412             if (pgains[pband] > QCONST16(.5,15))
413                enabled = 1;
414             ec_enc_bits(enc, enabled, 1);
415             if (enabled)
416                pgains[pband] = QCONST16(.9,15);
417             else
418                pgains[pband] = 0;
419          }
420          for (j=C*eBands[i];j<C*eBands[i+1];j++)
421             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
422       } else {
423          for (j=C*eBands[i];j<C*eBands[i+1];j++)
424             P[j] = 0;
425       }
426       
427       if (q > 0)
428       {
429          int ch=C;
430          if (C==2 && stereo_mode[i]==1)
431             ch = 1;
432          if (C==2)
433          {
434             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
435             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
436          }
437          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
438          if (C==2)
439             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
440       } else {
441          for (j=C*eBands[i];j<C*eBands[i+1];j++)
442             X[j] = P[j];
443       }
444       for (j=C*eBands[i];j<C*eBands[i+1];j++)
445          norm[j] = MULT16_16_Q15(n,X[j]);
446    }
447    RESTORE_STACK;
448 }
449
450 /* Decoding of the residual */
451 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)
452 {
453    int i, j, remaining_bits, balance;
454    const celt_int16_t * restrict eBands = m->eBands;
455    celt_norm_t * restrict norm;
456    VARDECL(celt_norm_t, _norm);
457    const int C = CHANNELS(m);
458    const celt_int16_t *pBands = m->pBands;
459    int pband=-1;
460    int B;
461    SAVE_STACK;
462
463    B = shortBlocks ? m->nbShortMdcts : 1;
464    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
465    norm = _norm;
466
467    balance = 0;
468    for (i=0;i<m->nbEBands;i++)
469    {
470       int tell;
471       int q;
472       celt_word16_t n;
473       const celt_int16_t * const *BPbits;
474       
475       int curr_balance, curr_bits;
476       
477       if (C>1 && stereo_mode[i]==0)
478          BPbits = m->bits_stereo;
479       else
480          BPbits = m->bits;
481
482       tell = ec_dec_tell(dec, 4);
483       if (i != 0)
484          balance -= tell;
485       remaining_bits = (total_bits<<BITRES)-tell-1;
486       curr_balance = (m->nbEBands-i);
487       if (curr_balance > 3)
488          curr_balance = 3;
489       curr_balance = balance / curr_balance;
490       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
491       curr_bits = BPbits[i][q];
492       remaining_bits -= curr_bits;
493       if (remaining_bits < 0)
494       {
495          remaining_bits += curr_bits;
496          if (q>0) {
497            q--;
498            curr_bits = BPbits[i][q];
499            remaining_bits -= curr_bits;
500          }
501       }
502       balance += pulses[i] + tell;
503
504       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
505
506       /* If pitch isn't available, use intra-frame prediction */
507       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
508       {
509          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B);
510       } else if (pitch_used && eBands[i] < m->pitchEnd)
511       {
512          if (eBands[i] == pBands[pband+1])
513          {
514             int enabled = 0;
515             pband++;
516             enabled = ec_dec_bits(dec, 1);
517             if (enabled)
518                pgains[pband] = QCONST16(.9,15);
519             else
520                pgains[pband] = 0;
521          }
522          for (j=C*eBands[i];j<C*eBands[i+1];j++)
523             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
524       } else {
525          for (j=C*eBands[i];j<C*eBands[i+1];j++)
526             P[j] = 0;
527       }
528       
529       if (q > 0)
530       {
531          int ch=C;
532          if (C==2 && stereo_mode[i]==1)
533             ch = 1;
534          if (C==2)
535             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
536          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
537          if (C==2)
538             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
539       } else {
540          for (j=C*eBands[i];j<C*eBands[i+1];j++)
541             X[j] = P[j];
542       }
543       for (j=C*eBands[i];j<C*eBands[i+1];j++)
544          norm[j] = MULT16_16_Q15(n,X[j]);
545    }
546    RESTORE_STACK;
547 }
548