Cleaning up leftovers of "freq" in celt_decode_with_ec()
[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 static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
43 {
44    int i;
45    opus_val16 ms;
46    celt_norm *Xptr;
47    Xptr = X;
48    ms = NEG16(s);
49    for (i=0;i<len-stride;i++)
50    {
51       celt_norm x1, x2;
52       x1 = Xptr[0];
53       x2 = Xptr[stride];
54       Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
55       *Xptr++      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
56    }
57    Xptr = &X[len-2*stride-1];
58    for (i=len-2*stride-1;i>=0;i--)
59    {
60       celt_norm x1, x2;
61       x1 = Xptr[0];
62       x2 = Xptr[stride];
63       Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
64       *Xptr--      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
65    }
66 }
67
68 static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K, int spread)
69 {
70    static const int SPREAD_FACTOR[3]={15,10,5};
71    int i;
72    opus_val16 c, s;
73    opus_val16 gain, theta;
74    int stride2=0;
75    int factor;
76
77    if (2*K>=len || spread==SPREAD_NONE)
78       return;
79    factor = SPREAD_FACTOR[spread-1];
80
81    gain = celt_div((opus_val32)MULT16_16(Q15_ONE,len),(opus_val32)(len+factor*K));
82    theta = HALF16(MULT16_16_Q15(gain,gain));
83
84    c = celt_cos_norm(EXTEND32(theta));
85    s = celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */
86
87    if (len>=8*stride)
88    {
89       stride2 = 1;
90       /* This is just a simple (equivalent) way of computing sqrt(len/stride) with rounding.
91          It's basically incrementing long as (stride2+0.5)^2 < len/stride. */
92       while ((stride2*stride2+stride2)*stride + (stride>>2) < len)
93          stride2++;
94    }
95    /*NOTE: As a minor optimization, we could be passing around log2(B), not B, for both this and for
96       extract_collapse_mask().*/
97    len /= stride;
98    for (i=0;i<stride;i++)
99    {
100       if (dir < 0)
101       {
102          if (stride2)
103             exp_rotation1(X+i*len, len, stride2, s, c);
104          exp_rotation1(X+i*len, len, 1, c, s);
105       } else {
106          exp_rotation1(X+i*len, len, 1, c, -s);
107          if (stride2)
108             exp_rotation1(X+i*len, len, stride2, s, -c);
109       }
110    }
111 }
112
113 /** Takes the pitch vector and the decoded residual vector, computes the gain
114     that will give ||p+g*y||=1 and mixes the residual with the pitch. */
115 static void normalise_residual(int * OPUS_RESTRICT iy, celt_norm * OPUS_RESTRICT X,
116       int N, opus_val32 Ryy, opus_val16 gain)
117 {
118    int i;
119 #ifdef FIXED_POINT
120    int k;
121 #endif
122    opus_val32 t;
123    opus_val16 g;
124
125 #ifdef FIXED_POINT
126    k = celt_ilog2(Ryy)>>1;
127 #endif
128    t = VSHR32(Ryy, 2*(k-7));
129    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
130
131    i=0;
132    do
133       X[i] = EXTRACT16(PSHR32(MULT16_16(g, iy[i]), k+1));
134    while (++i < N);
135 }
136
137 static unsigned extract_collapse_mask(int *iy, int N, int B)
138 {
139    unsigned collapse_mask;
140    int N0;
141    int i;
142    if (B<=1)
143       return 1;
144    /*NOTE: As a minor optimization, we could be passing around log2(B), not B, for both this and for
145       exp_rotation().*/
146    N0 = N/B;
147    collapse_mask = 0;
148    i=0; do {
149       int j;
150       j=0; do {
151          collapse_mask |= (iy[i*N0+j]!=0)<<i;
152       } while (++j<N0);
153    } while (++i<B);
154    return collapse_mask;
155 }
156
157 unsigned alg_quant(celt_norm *X, int N, int K, int spread, int B, ec_enc *enc
158 #ifdef RESYNTH
159    , opus_val16 gain
160 #endif
161    )
162 {
163    VARDECL(celt_norm, y);
164    VARDECL(int, iy);
165    VARDECL(opus_val16, signx);
166    int i, j;
167    opus_val16 s;
168    int pulsesLeft;
169    opus_val32 sum;
170    opus_val32 xy;
171    opus_val16 yy;
172    unsigned collapse_mask;
173    SAVE_STACK;
174
175    celt_assert2(K>0, "alg_quant() needs at least one pulse");
176    celt_assert2(N>1, "alg_quant() needs at least two dimensions");
177
178    ALLOC(y, N, celt_norm);
179    ALLOC(iy, N, int);
180    ALLOC(signx, N, opus_val16);
181
182    exp_rotation(X, N, 1, B, K, spread);
183
184    /* Get rid of the sign */
185    sum = 0;
186    j=0; do {
187       if (X[j]>0)
188          signx[j]=1;
189       else {
190          signx[j]=-1;
191          X[j]=-X[j];
192       }
193       iy[j] = 0;
194       y[j] = 0;
195    } while (++j<N);
196
197    xy = yy = 0;
198
199    pulsesLeft = K;
200
201    /* Do a pre-search by projecting on the pyramid */
202    if (K > (N>>1))
203    {
204       opus_val16 rcp;
205       j=0; do {
206          sum += X[j];
207       }  while (++j<N);
208
209       /* If X is too small, just replace it with a pulse at 0 */
210 #ifdef FIXED_POINT
211       if (sum <= K)
212 #else
213       /* Prevents infinities and NaNs from causing too many pulses
214          to be allocated. 64 is an approximation of infinity here. */
215       if (!(sum > EPSILON && sum < 64))
216 #endif
217       {
218          X[0] = QCONST16(1.f,14);
219          j=1; do
220             X[j]=0;
221          while (++j<N);
222          sum = QCONST16(1.f,14);
223       }
224       rcp = EXTRACT16(MULT16_32_Q16(K-1, celt_rcp(sum)));
225       j=0; do {
226 #ifdef FIXED_POINT
227          /* It's really important to round *towards zero* here */
228          iy[j] = MULT16_16_Q15(X[j],rcp);
229 #else
230          iy[j] = (int)floor(rcp*X[j]);
231 #endif
232          y[j] = (celt_norm)iy[j];
233          yy = MAC16_16(yy, y[j],y[j]);
234          xy = MAC16_16(xy, X[j],y[j]);
235          y[j] *= 2;
236          pulsesLeft -= iy[j];
237       }  while (++j<N);
238    }
239    celt_assert2(pulsesLeft>=1, "Allocated too many pulses in the quick pass");
240
241    /* This should never happen, but just in case it does (e.g. on silence)
242       we fill the first bin with pulses. */
243 #ifdef FIXED_POINT_DEBUG
244    celt_assert2(pulsesLeft<=N+3, "Not enough pulses in the quick pass");
245 #endif
246    if (pulsesLeft > N+3)
247    {
248       opus_val16 tmp = (opus_val16)pulsesLeft;
249       yy = MAC16_16(yy, tmp, tmp);
250       yy = MAC16_16(yy, tmp, y[0]);
251       iy[0] += pulsesLeft;
252       pulsesLeft=0;
253    }
254
255    s = 1;
256    for (i=0;i<pulsesLeft;i++)
257    {
258       int best_id;
259       opus_val32 best_num = -VERY_LARGE16;
260       opus_val16 best_den = 0;
261 #ifdef FIXED_POINT
262       int rshift;
263 #endif
264 #ifdef FIXED_POINT
265       rshift = 1+celt_ilog2(K-pulsesLeft+i+1);
266 #endif
267       best_id = 0;
268       /* The squared magnitude term gets added anyway, so we might as well
269          add it outside the loop */
270       yy = ADD32(yy, 1);
271       j=0;
272       do {
273          opus_val16 Rxy, Ryy;
274          /* Temporary sums of the new pulse(s) */
275          Rxy = EXTRACT16(SHR32(ADD32(xy, EXTEND32(X[j])),rshift));
276          /* We're multiplying y[j] by two so we don't have to do it here */
277          Ryy = ADD16(yy, y[j]);
278
279          /* Approximate score: we maximise Rxy/sqrt(Ryy) (we're guaranteed that
280             Rxy is positive because the sign is pre-computed) */
281          Rxy = MULT16_16_Q15(Rxy,Rxy);
282          /* The idea is to check for num/den >= best_num/best_den, but that way
283             we can do it without any division */
284          /* OPT: Make sure to use conditional moves here */
285          if (MULT16_16(best_den, Rxy) > MULT16_16(Ryy, best_num))
286          {
287             best_den = Ryy;
288             best_num = Rxy;
289             best_id = j;
290          }
291       } while (++j<N);
292
293       /* Updating the sums of the new pulse(s) */
294       xy = ADD32(xy, EXTEND32(X[best_id]));
295       /* We're multiplying y[j] by two so we don't have to do it here */
296       yy = ADD16(yy, y[best_id]);
297
298       /* Only now that we've made the final choice, update y/iy */
299       /* Multiplying y[j] by 2 so we don't have to do it everywhere else */
300       y[best_id] += 2*s;
301       iy[best_id]++;
302    }
303
304    /* Put the original sign back */
305    j=0;
306    do {
307       X[j] = MULT16_16(signx[j],X[j]);
308       if (signx[j] < 0)
309          iy[j] = -iy[j];
310    } while (++j<N);
311    encode_pulses(iy, N, K, enc);
312
313 #ifdef RESYNTH
314    normalise_residual(iy, X, N, yy, gain);
315    exp_rotation(X, N, -1, B, K, spread);
316 #endif
317
318    collapse_mask = extract_collapse_mask(iy, N, B);
319    RESTORE_STACK;
320    return collapse_mask;
321 }
322
323 /** Decode pulse vector and combine the result with the pitch vector to produce
324     the final normalised signal in the current band. */
325 unsigned alg_unquant(celt_norm *X, int N, int K, int spread, int B,
326       ec_dec *dec, opus_val16 gain)
327 {
328    int i;
329    opus_val32 Ryy;
330    unsigned collapse_mask;
331    VARDECL(int, iy);
332    SAVE_STACK;
333
334    celt_assert2(K>0, "alg_unquant() needs at least one pulse");
335    celt_assert2(N>1, "alg_unquant() needs at least two dimensions");
336    ALLOC(iy, N, int);
337    decode_pulses(iy, N, K, dec);
338    Ryy = 0;
339    i=0;
340    do {
341       Ryy = MAC16_16(Ryy, iy[i], iy[i]);
342    } while (++i < N);
343    normalise_residual(iy, X, N, Ryy, gain);
344    exp_rotation(X, N, -1, B, K, spread);
345    collapse_mask = extract_collapse_mask(iy, N, B);
346    RESTORE_STACK;
347    return collapse_mask;
348 }
349
350 void renormalise_vector(celt_norm *X, int N, opus_val16 gain)
351 {
352    int i;
353 #ifdef FIXED_POINT
354    int k;
355 #endif
356    opus_val32 E;
357    opus_val16 g;
358    opus_val32 t;
359    celt_norm *xptr;
360    E = EPSILON + celt_inner_prod(X, X, N);
361 #ifdef FIXED_POINT
362    k = celt_ilog2(E)>>1;
363 #endif
364    t = VSHR32(E, 2*(k-7));
365    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
366
367    xptr = X;
368    for (i=0;i<N;i++)
369    {
370       *xptr = EXTRACT16(PSHR32(MULT16_16(g, *xptr), k+1));
371       xptr++;
372    }
373    /*return celt_sqrt(E);*/
374 }
375
376 int stereo_itheta(const celt_norm *X, const celt_norm *Y, int stereo, int N)
377 {
378    int i;
379    int itheta;
380    opus_val16 mid, side;
381    opus_val32 Emid, Eside;
382
383    Emid = Eside = EPSILON;
384    if (stereo)
385    {
386       for (i=0;i<N;i++)
387       {
388          celt_norm m, s;
389          m = ADD16(SHR16(X[i],1),SHR16(Y[i],1));
390          s = SUB16(SHR16(X[i],1),SHR16(Y[i],1));
391          Emid = MAC16_16(Emid, m, m);
392          Eside = MAC16_16(Eside, s, s);
393       }
394    } else {
395       Emid += celt_inner_prod(X, X, N);
396       Eside += celt_inner_prod(Y, Y, N);
397    }
398    mid = celt_sqrt(Emid);
399    side = celt_sqrt(Eside);
400 #ifdef FIXED_POINT
401    /* 0.63662 = 2/pi */
402    itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
403 #else
404    itheta = (int)floor(.5f+16384*0.63662f*atan2(side,mid));
405 #endif
406
407    return itheta;
408 }