Adds a fuzzing mode that causes the encoder to make random decisions
[opus.git] / libcelt / pitch.c
index 7e06f45..89a00f8 100644 (file)
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:
-   
+
    - Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
-   
+
    - Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
-   
-   - Neither the name of the Xiph.org Foundation nor the names of its
-   contributors may be used to endorse or promote products derived from
-   this software without specific prior written permission.
-   
+
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
 
-/*#include "_kiss_fft_guts.h"
-#include "kiss_fftr.h"*/
-#include "kfft_single.h"
-
 #include "pitch.h"
-#include "psy.h"
 #include "os_support.h"
-#include "mathops.h"
 #include "modes.h"
 #include "stack_alloc.h"
+#include "mathops.h"
 
-kiss_fftr_cfg pitch_state_alloc(int max_lag)
+static void find_best_pitch(opus_val32 *xcorr, opus_val32 maxcorr, opus_val16 *y,
+                            int yshift, int len, int max_pitch, int *best_pitch)
 {
-   return real16_fft_alloc(max_lag);
-}
+   int i, j;
+   opus_val32 Syy=1;
+   opus_val16 best_num[2];
+   opus_val32 best_den[2];
+#ifdef FIXED_POINT
+   int xshift;
 
-void pitch_state_free(kiss_fftr_cfg st)
-{
-   real16_fft_free(st);
-}
+   xshift = celt_ilog2(maxcorr)-14;
+#endif
 
-#ifdef FIXED_POINT
-static void normalise16(celt_word16_t *x, int len, celt_word16_t val)
-{
-   int i;
-   celt_word16_t maxabs;
-   maxabs = celt_maxabs16(x,len);
-   if (maxabs > val)
+   best_num[0] = -1;
+   best_num[1] = -1;
+   best_den[0] = 0;
+   best_den[1] = 0;
+   best_pitch[0] = 0;
+   best_pitch[1] = 1;
+   for (j=0;j<len;j++)
+      Syy = MAC16_16(Syy, y[j],y[j]);
+   for (i=0;i<max_pitch;i++)
    {
-      int shift = 0;
-      while (maxabs > val)
+      if (xcorr[i]>0)
       {
-         maxabs >>= 1;
-         shift++;
+         opus_val16 num;
+         opus_val32 xcorr16;
+         xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
+         num = MULT16_16_Q15(xcorr16,xcorr16);
+         if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
+         {
+            if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
+            {
+               best_num[1] = best_num[0];
+               best_den[1] = best_den[0];
+               best_pitch[1] = best_pitch[0];
+               best_num[0] = num;
+               best_den[0] = Syy;
+               best_pitch[0] = i;
+            } else {
+               best_num[1] = num;
+               best_den[1] = Syy;
+               best_pitch[1] = i;
+            }
+         }
       }
-      if (shift==0)
-         return;
-      i=0;
-      do{
-         x[i] = SHR16(x[i], shift);
-      } while (++i<len);
-   } else {
-      int shift=0;
-      if (maxabs == 0)
-         return;
-      val >>= 1;
-      while (maxabs < val)
-      {
-         val >>= 1;
-         shift++;
-      }
-      if (shift==0)
-         return;
-      i=0;
-      do{
-         x[i] = SHL16(x[i], shift);
-      } while (++i<len);
+      Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
+      Syy = MAX32(1, Syy);
    }
 }
+
+#include "plc.h"
+void pitch_downsample(celt_sig * restrict x[], opus_val16 * restrict x_lp,
+      int len, int _C)
+{
+   int i;
+   opus_val32 ac[5];
+   opus_val16 tmp=Q15ONE;
+   opus_val16 lpc[4], mem[4]={0,0,0,0};
+   const int C = CHANNELS(_C);
+   for (i=1;i<len>>1;i++)
+      x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+3);
+   x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), SIG_SHIFT+3);
+   if (C==2)
+   {
+      for (i=1;i<len>>1;i++)
+         x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+3);
+      x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+3);
+   }
+
+   _celt_autocorr(x_lp, ac, NULL, 0,
+                  4, len>>1);
+
+   /* Noise floor -40 dB */
+#ifdef FIXED_POINT
+   ac[0] += SHR32(ac[0],13);
 #else
-#define normalise16(x,len,val)
+   ac[0] *= 1.0001f;
 #endif
+   /* Lag windowing */
+   for (i=1;i<=4;i++)
+   {
+      /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
+#ifdef FIXED_POINT
+      ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
+#else
+      ac[i] -= ac[i]*(.008f*i)*(.008f*i);
+#endif
+   }
 
-#define INPUT_SHIFT 15
+   _celt_lpc(lpc, ac, 4);
+   for (i=0;i<4;i++)
+   {
+      tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
+      lpc[i] = MULT16_16_Q15(lpc[i], tmp);
+   }
+   fir(x_lp, lpc, x_lp, len>>1, 4, mem);
+
+   mem[0]=0;
+   lpc[0]=QCONST16(.8f,12);
+   fir(x_lp, lpc, x_lp, len>>1, 1, mem);
 
-void find_spectral_pitch(const CELTMode *m, kiss_fftr_cfg fft, const struct PsyDecay *decay, const celt_sig_t * restrict x, const celt_sig_t * restrict y, const celt_word16_t * restrict window, celt_word16_t * restrict spectrum, int len, int max_pitch, int *pitch, int _C)
+}
+
+void pitch_search(const opus_val16 * restrict x_lp, opus_val16 * restrict y,
+                  int len, int max_pitch, int *pitch)
 {
-   int c, i;
-   VARDECL(celt_word16_t, _X);
-   VARDECL(celt_word16_t, _Y);
-   const celt_word16_t * restrict wptr;
-#ifndef SHORTCUTS
-   VARDECL(celt_mask_t, curve);
-#endif
-   celt_word16_t * restrict X, * restrict Y;
-   celt_word16_t * restrict Xptr, * restrict Yptr;
-   const celt_sig_t * restrict yptr;
-   int n2;
-   int L2;
-   const int C = CHANNELS(_C);
-   const int overlap = OVERLAP(m);
-   const int lag = MAX_PERIOD;
+   int i, j;
+   int lag;
+   int best_pitch[2]={0,0};
+   VARDECL(opus_val16, x_lp4);
+   VARDECL(opus_val16, y_lp4);
+   VARDECL(opus_val32, xcorr);
+   opus_val32 maxcorr=1;
+   int offset;
+   int shift=0;
+
    SAVE_STACK;
-   n2 = lag>>1;
-   L2 = len>>1;
-   ALLOC(_X, lag, celt_word16_t);
-   X = _X;
-#ifndef SHORTCUTS
-   ALLOC(curve, n2, celt_mask_t);
-#endif
-   CELT_MEMSET(X,0,lag);
-   /* Sum all channels of the current frame and copy into X in bit-reverse order */
-   for (c=0;c<C;c++)
+
+   lag = len+max_pitch;
+
+   ALLOC(x_lp4, len>>2, opus_val16);
+   ALLOC(y_lp4, lag>>2, opus_val16);
+   ALLOC(xcorr, max_pitch>>1, opus_val32);
+
+   /* Downsample by 2 again */
+   for (j=0;j<len>>2;j++)
+      x_lp4[j] = x_lp[2*j];
+   for (j=0;j<lag>>2;j++)
+      y_lp4[j] = y[2*j];
+
+#ifdef FIXED_POINT
+   shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
+   if (shift>0)
    {
-      const celt_sig_t * restrict xptr = &x[c];
-      for (i=0;i<L2;i++)
-      {
-         X[2*BITREV(fft,i)] += SHR32(*xptr,INPUT_SHIFT);
-         xptr += C;
-         X[2*BITREV(fft,i)+1] += SHR32(*xptr,INPUT_SHIFT);
-         xptr += C;
-      }
+      for (j=0;j<len>>2;j++)
+         x_lp4[j] = SHR16(x_lp4[j], shift);
+      for (j=0;j<lag>>2;j++)
+         y_lp4[j] = SHR16(y_lp4[j], shift);
+      /* Use double the shift for a MAC */
+      shift *= 2;
+   } else {
+      shift = 0;
    }
-   /* Applying the window in the bit-reverse domain. It's a bit weird, but it
-      can help save memory */
-   wptr = window;
-   for (i=0;i<overlap>>1;i++)
+#endif
+
+   /* Coarse search with 4x decimation */
+
+   for (i=0;i<max_pitch>>2;i++)
    {
-      X[2*BITREV(fft,i)]        = MULT16_16_Q15(wptr[0], X[2*BITREV(fft,i)]);
-      X[2*BITREV(fft,i)+1]      = MULT16_16_Q15(wptr[1], X[2*BITREV(fft,i)+1]);
-      X[2*BITREV(fft,L2-i-1)]   = MULT16_16_Q15(wptr[1], X[2*BITREV(fft,L2-i-1)]);
-      X[2*BITREV(fft,L2-i-1)+1] = MULT16_16_Q15(wptr[0], X[2*BITREV(fft,L2-i-1)+1]);
-      wptr += 2;
+      opus_val32 sum = 0;
+      for (j=0;j<len>>2;j++)
+         sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
+      xcorr[i] = MAX32(-1, sum);
+      maxcorr = MAX32(maxcorr, sum);
    }
-   normalise16(X, lag, 8192);
-   /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
-   /* Forward real FFT (in-place) */
-   real16_fft_inplace(fft, X, lag);
+   find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
 
-   if (spectrum)
+   /* Finer search with 2x decimation */
+   maxcorr=1;
+   for (i=0;i<max_pitch>>1;i++)
    {
-      for (i=0;i<lag/4;i++)
-      {
-         spectrum[2*i] = X[4*i];
-         spectrum[2*i+1] = X[4*i+1];
-      }
+      opus_val32 sum=0;
+      xcorr[i] = 0;
+      if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
+         continue;
+      for (j=0;j<len>>1;j++)
+         sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
+      xcorr[i] = MAX32(-1, sum);
+      maxcorr = MAX32(maxcorr, sum);
    }
-#ifndef SHORTCUTS
-   compute_masking(decay, X, curve, lag);
-#endif
-   
-   /* Deferred allocation to reduce peak stack usage */
-   ALLOC(_Y, lag, celt_word16_t);
-   Y = _Y;
-   yptr = &y[0];
-   /* Copy first channel of the past audio into Y in bit-reverse order */
-   for (i=0;i<n2;i++)
+   find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
+
+   /* Refine by pseudo-interpolation */
+   if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
    {
-      Y[2*BITREV(fft,i)] = SHR32(*yptr,INPUT_SHIFT);
-      yptr += C;
-      Y[2*BITREV(fft,i)+1] = SHR32(*yptr,INPUT_SHIFT);
-      yptr += C;
+      opus_val32 a, b, c;
+      a = xcorr[best_pitch[0]-1];
+      b = xcorr[best_pitch[0]];
+      c = xcorr[best_pitch[0]+1];
+      if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
+         offset = 1;
+      else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
+         offset = -1;
+      else
+         offset = 0;
+   } else {
+      offset = 0;
    }
-   /* Add remaining channels into Y in bit-reverse order */
-   for (c=1;c<C;c++)
+   *pitch = 2*best_pitch[0]-offset;
+
+   RESTORE_STACK;
+}
+
+static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
+opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
+      int N, int *_T0, int prev_period, opus_val16 prev_gain)
+{
+   int k, i, T, T0;
+   opus_val16 g, g0;
+   opus_val16 pg;
+   opus_val32 xy,xx,yy;
+   opus_val32 xcorr[3];
+   opus_val32 best_xy, best_yy;
+   int offset;
+   int minperiod0;
+
+   minperiod0 = minperiod;
+   maxperiod /= 2;
+   minperiod /= 2;
+   *_T0 /= 2;
+   prev_period /= 2;
+   N /= 2;
+   x += maxperiod;
+   if (*_T0>=maxperiod)
+      *_T0=maxperiod-1;
+
+   T = T0 = *_T0;
+   xx=xy=yy=0;
+   for (i=0;i<N;i++)
    {
-      yptr = &y[c];
-      for (i=0;i<n2;i++)
+      xy = MAC16_16(xy, x[i], x[i-T0]);
+      xx = MAC16_16(xx, x[i], x[i]);
+      yy = MAC16_16(yy, x[i-T0],x[i-T0]);
+   }
+   best_xy = xy;
+   best_yy = yy;
+#ifdef FIXED_POINT
       {
-         Y[2*BITREV(fft,i)] += SHR32(*yptr,INPUT_SHIFT);
-         yptr += C;
-         Y[2*BITREV(fft,i)+1] += SHR32(*yptr,INPUT_SHIFT);
-         yptr += C;
+         opus_val32 x2y2;
+         int sh, t;
+         x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
+         sh = celt_ilog2(x2y2)>>1;
+         t = VSHR32(x2y2, 2*(sh-7));
+         g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
       }
-   }
-   normalise16(Y, lag, 8192);
-   /* Forward real FFT (in-place) */
-   real16_fft_inplace(fft, Y, lag);
-
-   /* Compute cross-spectrum using the inverse masking curve as weighting */
-   Xptr = &X[2];
-   Yptr = &Y[2];
-   for (i=1;i<n2;i++)
+#else
+      g = g0 = xy/celt_sqrt(1+xx*yy);
+#endif
+   /* Look for any pitch at T/k */
+   for (k=2;k<=15;k++)
    {
-      celt_word16_t Xr, Xi, n;
-      /* weight = 1/sqrt(curve) */
-      Xr = Xptr[0];
-      Xi = Xptr[1];
-#ifdef SHORTCUTS
-      /*n = SHR32(32767,(celt_ilog2(EPSILON+curve[i])>>1));*/
-      n = 1+(8192>>(celt_ilog2(1+MULT16_16(Xr,Xr)+MULT16_16(Xi,Xi))>>1));
-      /* Pre-multiply X by n, so we can keep everything in 16 bits */
-      Xr = MULT16_16_16(n, Xr);
-      Xi = MULT16_16_16(n, Xi);
+      int T1, T1b;
+      opus_val16 g1;
+      opus_val16 cont=0;
+      T1 = (2*T0+k)/(2*k);
+      if (T1 < minperiod)
+         break;
+      /* Look for another strong correlation at T1b */
+      if (k==2)
+      {
+         if (T1+T0>maxperiod)
+            T1b = T0;
+         else
+            T1b = T0+T1;
+      } else
+      {
+         T1b = (2*second_check[k]*T0+k)/(2*k);
+      }
+      xy=yy=0;
+      for (i=0;i<N;i++)
+      {
+         xy = MAC16_16(xy, x[i], x[i-T1]);
+         yy = MAC16_16(yy, x[i-T1], x[i-T1]);
+
+         xy = MAC16_16(xy, x[i], x[i-T1b]);
+         yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
+      }
+#ifdef FIXED_POINT
+      {
+         opus_val32 x2y2;
+         int sh, t;
+         x2y2 = 1+MULT32_32_Q31(xx,yy);
+         sh = celt_ilog2(x2y2)>>1;
+         t = VSHR32(x2y2, 2*(sh-7));
+         g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
+      }
 #else
-      n = celt_rsqrt(EPSILON+curve[i]);
-      /* Pre-multiply X by n, so we can keep everything in 16 bits */
-      Xr = EXTRACT16(SHR32(MULT16_16(n, Xr),3));
-      Xi = EXTRACT16(SHR32(MULT16_16(n, Xi),3));
+      g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
 #endif
-      /* Cross-spectrum between X and conj(Y) */
-      *Xptr++ = ADD16(MULT16_16_Q15(Xr, Yptr[0]), MULT16_16_Q15(Xi,Yptr[1]));
-      *Xptr++ = SUB16(MULT16_16_Q15(Xr, Yptr[1]), MULT16_16_Q15(Xi,Yptr[0]));
-      Yptr += 2;
+      if (abs(T1-prev_period)<=1)
+         cont = prev_gain;
+      else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
+         cont = HALF32(prev_gain);
+      else
+         cont = 0;
+      if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
+      {
+         best_xy = xy;
+         best_yy = yy;
+         T = T1;
+         g = g1;
+      }
    }
-   /*printf ("\n");*/
-   X[0] = X[1] = 0;
-   /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
-   normalise16(X, lag, 50);
-   /* Inverse half-complex to real FFT gives us the correlation */
-   real16_ifft(fft, X, Y, lag);
-   
-   /* The peak in the correlation gives us the pitch */
-   *pitch = find_max16(Y, max_pitch);
-   RESTORE_STACK;
+   if (best_yy <= best_xy)
+      pg = Q15ONE;
+   else
+      pg = SHR32(frac_div32(best_xy,best_yy+1),16);
+
+   for (k=0;k<3;k++)
+   {
+      int T1 = T+k-1;
+      xy = 0;
+      for (i=0;i<N;i++)
+         xy = MAC16_16(xy, x[i], x[i-T1]);
+      xcorr[k] = xy;
+   }
+   if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
+      offset = 1;
+   else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
+      offset = -1;
+   else
+      offset = 0;
+   if (pg > g)
+      pg = g;
+   *_T0 = 2*T+offset;
+
+   if (*_T0<minperiod0)
+      *_T0=minperiod0;
+   return pg;
 }