Fixes bitrate statistics for decode-only operation and inband FEC
[opus.git] / celt / vq.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2009 Xiph.Org Foundation
3    Written by Jean-Marc Valin */
4 /*
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15
16    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
20    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
21    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
22    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28
29 #ifdef HAVE_CONFIG_H
30 #include "config.h"
31 #endif
32
33 #include "mathops.h"
34 #include "cwrs.h"
35 #include "vq.h"
36 #include "arch.h"
37 #include "os_support.h"
38 #include "bands.h"
39 #include "rate.h"
40 #include "pitch.h"
41
42 #if defined(MIPSr1_ASM)
43 #include "mips/vq_mipsr1.h"
44 #endif
45
46 #ifndef OVERRIDE_vq_exp_rotation1
47 static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
48 {
49    int i;
50    opus_val16 ms;
51    celt_norm *Xptr;
52    Xptr = X;
53    ms = NEG16(s);
54    for (i=0;i<len-stride;i++)
55    {
56       celt_norm x1, x2;
57       x1 = Xptr[0];
58       x2 = Xptr[stride];
59       Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
60       *Xptr++      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
61    }
62    Xptr = &X[len-2*stride-1];
63    for (i=len-2*stride-1;i>=0;i--)
64    {
65       celt_norm x1, x2;
66       x1 = Xptr[0];
67       x2 = Xptr[stride];
68       Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
69       *Xptr--      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
70    }
71 }
72 #endif /* OVERRIDE_vq_exp_rotation1 */
73
74 static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K, int spread)
75 {
76    static const int SPREAD_FACTOR[3]={15,10,5};
77    int i;
78    opus_val16 c, s;
79    opus_val16 gain, theta;
80    int stride2=0;
81    int factor;
82
83    if (2*K>=len || spread==SPREAD_NONE)
84       return;
85    factor = SPREAD_FACTOR[spread-1];
86
87    gain = celt_div((opus_val32)MULT16_16(Q15_ONE,len),(opus_val32)(len+factor*K));
88    theta = HALF16(MULT16_16_Q15(gain,gain));
89
90    c = celt_cos_norm(EXTEND32(theta));
91    s = celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */
92
93    if (len>=8*stride)
94    {
95       stride2 = 1;
96       /* This is just a simple (equivalent) way of computing sqrt(len/stride) with rounding.
97          It's basically incrementing long as (stride2+0.5)^2 < len/stride. */
98       while ((stride2*stride2+stride2)*stride + (stride>>2) < len)
99          stride2++;
100    }
101    /*NOTE: As a minor optimization, we could be passing around log2(B), not B, for both this and for
102       extract_collapse_mask().*/
103    len = celt_udiv(len, stride);
104    for (i=0;i<stride;i++)
105    {
106       if (dir < 0)
107       {
108          if (stride2)
109             exp_rotation1(X+i*len, len, stride2, s, c);
110          exp_rotation1(X+i*len, len, 1, c, s);
111       } else {
112          exp_rotation1(X+i*len, len, 1, c, -s);
113          if (stride2)
114             exp_rotation1(X+i*len, len, stride2, s, -c);
115       }
116    }
117 }
118
119 /** Takes the pitch vector and the decoded residual vector, computes the gain
120     that will give ||p+g*y||=1 and mixes the residual with the pitch. */
121 static void normalise_residual(int * OPUS_RESTRICT iy, celt_norm * OPUS_RESTRICT X,
122       int N, opus_val32 Ryy, opus_val16 gain)
123 {
124    int i;
125 #ifdef FIXED_POINT
126    int k;
127 #endif
128    opus_val32 t;
129    opus_val16 g;
130
131 #ifdef FIXED_POINT
132    k = celt_ilog2(Ryy)>>1;
133 #endif
134    t = VSHR32(Ryy, 2*(k-7));
135    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
136
137    i=0;
138    do
139       X[i] = EXTRACT16(PSHR32(MULT16_16(g, iy[i]), k+1));
140    while (++i < N);
141 }
142
143 static unsigned extract_collapse_mask(int *iy, int N, int B)
144 {
145    unsigned collapse_mask;
146    int N0;
147    int i;
148    if (B<=1)
149       return 1;
150    /*NOTE: As a minor optimization, we could be passing around log2(B), not B, for both this and for
151       exp_rotation().*/
152    N0 = celt_udiv(N, B);
153    collapse_mask = 0;
154    i=0; do {
155       int j;
156       unsigned tmp=0;
157       j=0; do {
158          tmp |= iy[i*N0+j];
159       } while (++j<N0);
160       collapse_mask |= (tmp!=0)<<i;
161    } while (++i<B);
162    return collapse_mask;
163 }
164
165 unsigned alg_quant(celt_norm *X, int N, int K, int spread, int B, ec_enc *enc
166 #ifdef RESYNTH
167    , opus_val16 gain
168 #endif
169    )
170 {
171    VARDECL(celt_norm, y);
172    VARDECL(int, iy);
173    VARDECL(opus_val16, signx);
174    int i, j;
175    opus_val16 s;
176    int pulsesLeft;
177    opus_val32 sum;
178    opus_val32 xy;
179    opus_val16 yy;
180    unsigned collapse_mask;
181    SAVE_STACK;
182
183    celt_assert2(K>0, "alg_quant() needs at least one pulse");
184    celt_assert2(N>1, "alg_quant() needs at least two dimensions");
185
186    ALLOC(y, N, celt_norm);
187    ALLOC(iy, N, int);
188    ALLOC(signx, N, opus_val16);
189
190    exp_rotation(X, N, 1, B, K, spread);
191
192    /* Get rid of the sign */
193    sum = 0;
194    j=0; do {
195       if (X[j]>0)
196          signx[j]=1;
197       else {
198          signx[j]=-1;
199          X[j]=-X[j];
200       }
201       iy[j] = 0;
202       y[j] = 0;
203    } while (++j<N);
204
205    xy = yy = 0;
206
207    pulsesLeft = K;
208
209    /* Do a pre-search by projecting on the pyramid */
210    if (K > (N>>1))
211    {
212       opus_val16 rcp;
213       j=0; do {
214          sum += X[j];
215       }  while (++j<N);
216
217       /* If X is too small, just replace it with a pulse at 0 */
218 #ifdef FIXED_POINT
219       if (sum <= K)
220 #else
221       /* Prevents infinities and NaNs from causing too many pulses
222          to be allocated. 64 is an approximation of infinity here. */
223       if (!(sum > EPSILON && sum < 64))
224 #endif
225       {
226          X[0] = QCONST16(1.f,14);
227          j=1; do
228             X[j]=0;
229          while (++j<N);
230          sum = QCONST16(1.f,14);
231       }
232       rcp = EXTRACT16(MULT16_32_Q16(K-1, celt_rcp(sum)));
233       j=0; do {
234 #ifdef FIXED_POINT
235          /* It's really important to round *towards zero* here */
236          iy[j] = MULT16_16_Q15(X[j],rcp);
237 #else
238          iy[j] = (int)floor(rcp*X[j]);
239 #endif
240          y[j] = (celt_norm)iy[j];
241          yy = MAC16_16(yy, y[j],y[j]);
242          xy = MAC16_16(xy, X[j],y[j]);
243          y[j] *= 2;
244          pulsesLeft -= iy[j];
245       }  while (++j<N);
246    }
247    celt_assert2(pulsesLeft>=1, "Allocated too many pulses in the quick pass");
248
249    /* This should never happen, but just in case it does (e.g. on silence)
250       we fill the first bin with pulses. */
251 #ifdef FIXED_POINT_DEBUG
252    celt_assert2(pulsesLeft<=N+3, "Not enough pulses in the quick pass");
253 #endif
254    if (pulsesLeft > N+3)
255    {
256       opus_val16 tmp = (opus_val16)pulsesLeft;
257       yy = MAC16_16(yy, tmp, tmp);
258       yy = MAC16_16(yy, tmp, y[0]);
259       iy[0] += pulsesLeft;
260       pulsesLeft=0;
261    }
262
263    s = 1;
264    for (i=0;i<pulsesLeft;i++)
265    {
266       int best_id;
267       opus_val32 best_num = -VERY_LARGE16;
268       opus_val16 best_den = 0;
269 #ifdef FIXED_POINT
270       int rshift;
271 #endif
272 #ifdef FIXED_POINT
273       rshift = 1+celt_ilog2(K-pulsesLeft+i+1);
274 #endif
275       best_id = 0;
276       /* The squared magnitude term gets added anyway, so we might as well
277          add it outside the loop */
278       yy = ADD32(yy, 1);
279       j=0;
280       do {
281          opus_val16 Rxy, Ryy;
282          /* Temporary sums of the new pulse(s) */
283          Rxy = EXTRACT16(SHR32(ADD32(xy, EXTEND32(X[j])),rshift));
284          /* We're multiplying y[j] by two so we don't have to do it here */
285          Ryy = ADD16(yy, y[j]);
286
287          /* Approximate score: we maximise Rxy/sqrt(Ryy) (we're guaranteed that
288             Rxy is positive because the sign is pre-computed) */
289          Rxy = MULT16_16_Q15(Rxy,Rxy);
290          /* The idea is to check for num/den >= best_num/best_den, but that way
291             we can do it without any division */
292          /* OPT: Make sure to use conditional moves here */
293          if (MULT16_16(best_den, Rxy) > MULT16_16(Ryy, best_num))
294          {
295             best_den = Ryy;
296             best_num = Rxy;
297             best_id = j;
298          }
299       } while (++j<N);
300
301       /* Updating the sums of the new pulse(s) */
302       xy = ADD32(xy, EXTEND32(X[best_id]));
303       /* We're multiplying y[j] by two so we don't have to do it here */
304       yy = ADD16(yy, y[best_id]);
305
306       /* Only now that we've made the final choice, update y/iy */
307       /* Multiplying y[j] by 2 so we don't have to do it everywhere else */
308       y[best_id] += 2*s;
309       iy[best_id]++;
310    }
311
312    /* Put the original sign back */
313    j=0;
314    do {
315       X[j] = MULT16_16(signx[j],X[j]);
316       if (signx[j] < 0)
317          iy[j] = -iy[j];
318    } while (++j<N);
319    encode_pulses(iy, N, K, enc);
320
321 #ifdef RESYNTH
322    normalise_residual(iy, X, N, yy, gain);
323    exp_rotation(X, N, -1, B, K, spread);
324 #endif
325
326    collapse_mask = extract_collapse_mask(iy, N, B);
327    RESTORE_STACK;
328    return collapse_mask;
329 }
330
331 /** Decode pulse vector and combine the result with the pitch vector to produce
332     the final normalised signal in the current band. */
333 unsigned alg_unquant(celt_norm *X, int N, int K, int spread, int B,
334       ec_dec *dec, opus_val16 gain)
335 {
336    opus_val32 Ryy;
337    unsigned collapse_mask;
338    VARDECL(int, iy);
339    SAVE_STACK;
340
341    celt_assert2(K>0, "alg_unquant() needs at least one pulse");
342    celt_assert2(N>1, "alg_unquant() needs at least two dimensions");
343    ALLOC(iy, N, int);
344    Ryy = decode_pulses(iy, N, K, dec);
345    normalise_residual(iy, X, N, Ryy, gain);
346    exp_rotation(X, N, -1, B, K, spread);
347    collapse_mask = extract_collapse_mask(iy, N, B);
348    RESTORE_STACK;
349    return collapse_mask;
350 }
351
352 #ifndef OVERRIDE_renormalise_vector
353 void renormalise_vector(celt_norm *X, int N, opus_val16 gain, int arch)
354 {
355    int i;
356 #ifdef FIXED_POINT
357    int k;
358 #endif
359    opus_val32 E;
360    opus_val16 g;
361    opus_val32 t;
362    celt_norm *xptr;
363    E = EPSILON + celt_inner_prod(X, X, N, arch);
364 #ifdef FIXED_POINT
365    k = celt_ilog2(E)>>1;
366 #endif
367    t = VSHR32(E, 2*(k-7));
368    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
369
370    xptr = X;
371    for (i=0;i<N;i++)
372    {
373       *xptr = EXTRACT16(PSHR32(MULT16_16(g, *xptr), k+1));
374       xptr++;
375    }
376    /*return celt_sqrt(E);*/
377 }
378 #endif /* OVERRIDE_renormalise_vector */
379
380 int stereo_itheta(const celt_norm *X, const celt_norm *Y, int stereo, int N, int arch)
381 {
382    int i;
383    int itheta;
384    opus_val16 mid, side;
385    opus_val32 Emid, Eside;
386
387    Emid = Eside = EPSILON;
388    if (stereo)
389    {
390       for (i=0;i<N;i++)
391       {
392          celt_norm m, s;
393          m = ADD16(SHR16(X[i],1),SHR16(Y[i],1));
394          s = SUB16(SHR16(X[i],1),SHR16(Y[i],1));
395          Emid = MAC16_16(Emid, m, m);
396          Eside = MAC16_16(Eside, s, s);
397       }
398    } else {
399       Emid += celt_inner_prod(X, X, N, arch);
400       Eside += celt_inner_prod(Y, Y, N, arch);
401    }
402    mid = celt_sqrt(Emid);
403    side = celt_sqrt(Eside);
404 #ifdef FIXED_POINT
405    /* 0.63662 = 2/pi */
406    itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
407 #else
408    itheta = (int)floor(.5f+16384*0.63662f*atan2(side,mid));
409 #endif
410
411    return itheta;
412 }