/* Copyright (C) 2007-2008 Jean-Marc Valin

   Copyright (C) 2008      Thorvald Natvig

   File: resample.c

   Arbitrary resampling code

   Redistribution and use in source and binary forms, with or without

   modification, are permitted provided that the following conditions are

   met:

   1. Redistributions of source code must retain the above copyright notice,

   this list of conditions and the following disclaimer.

   2. 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.

   3. The name of the author may not be used to endorse or promote products

   derived from this software without specific prior written permission.

   THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR

   IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES

   OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

   DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,

   INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

   (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR

   SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)

   HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,

   STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

   POSSIBILITY OF SUCH DAMAGE.

*/

/*

   The design goals of this code are:

      - Very fast algorithm

      - SIMD-friendly algorithm

      - Low memory requirement

      - Good *perceptual* quality (and not best SNR)

   Warning: This resampler is relatively new. Although I think I got rid of 

   all the major bugs and I don't expect the API to change anymore, there

   may be something I've missed. So use with caution.

   This algorithm is based on this original resampling algorithm:

   Smith, Julius O. Digital Audio Resampling Home Page

   Center for Computer Research in Music and Acoustics (CCRMA), 

   Stanford University, 2007.

   Web published at http://www-ccrma.stanford.edu/~jos/resample/.

   There is one main difference, though. This resampler uses cubic 

   interpolation instead of linear interpolation in the above paper. This

   makes the table much smaller and makes it possible to compute that table

   on a per-stream basis. In turn, being able to tweak the table for each 

   stream makes it possible to both reduce complexity on simple ratios 

   (e.g. 2/3), and get rid of the rounding operations in the inner loop. 

   The latter both reduces CPU time and makes the algorithm more SIMD-friendly.

*/

#ifdef HAVE_CONFIG_H

#include "config.h"

#endif

#ifdef OUTSIDE_SPEEX

#include <stdlib.h>

#include <glib.h>

#include <glibconfig.h>

#include <e32def.h>

#ifndef __SYMBIAN32__

#define EXPORT EXPORT_C

#else

#define EXPORT G_GNUC_INTERNAL

#endif

static inline void *

speex_alloc (int size)

{

  return g_malloc0 (size);

}

static inline void *

speex_realloc (void *ptr, int size)

{

  return g_realloc (ptr, size);

}

static inline void

speex_free (void *ptr)

{

  g_free (ptr);

}

#include "speex_resampler.h"

#include "arch_int.h"

#else /* OUTSIDE_SPEEX */

#include "speex_resampler.h"

#include "arch.h"

//#include "os_support.h"

#endif /* OUTSIDE_SPEEX */

#include <math.h>

#ifndef M_PI

#define M_PI 3.14159263

#endif

#ifdef FIXED_POINT

#define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))

#else

#define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))

#endif

#define IMAX(a,b) ((a) > (b) ? (a) : (b))

#define IMIN(a,b) ((a) < (b) ? (a) : (b))

#ifndef NULL

#define NULL 0

#endif

#ifdef _USE_SSE

#include "resample_sse.h"

#endif

/* Numer of elements to allocate on the stack */

#ifdef VAR_ARRAYS

#define FIXED_STACK_ALLOC 8192

#else

#define FIXED_STACK_ALLOC 1024

#endif

typedef int (*resampler_basic_func) (SpeexResamplerState *, spx_uint32_t,

    const spx_word16_t *, spx_uint32_t *, spx_word16_t *, spx_uint32_t *);

struct SpeexResamplerState_

{

  spx_uint32_t in_rate;

  spx_uint32_t out_rate;

  spx_uint32_t num_rate;

  spx_uint32_t den_rate;

  int quality;

  spx_uint32_t nb_channels;

  spx_uint32_t filt_len;

  spx_uint32_t mem_alloc_size;

  spx_uint32_t buffer_size;

  int int_advance;

  int frac_advance;

  float cutoff;

  spx_uint32_t oversample;

  int initialised;

  int started;

  /* These are per-channel */

  spx_int32_t *last_sample;

  spx_uint32_t *samp_frac_num;

  spx_uint32_t *magic_samples;

  spx_word16_t *mem;

  spx_word16_t *sinc_table;

  spx_uint32_t sinc_table_length;

  resampler_basic_func resampler_ptr;

  int in_stride;

  int out_stride;

};

static double kaiser12_table[68] = {

  0.99859849, 1.00000000, 0.99859849, 0.99440475, 0.98745105, 0.97779076,

  0.96549770, 0.95066529, 0.93340547, 0.91384741, 0.89213598, 0.86843014,

  0.84290116, 0.81573067, 0.78710866, 0.75723148, 0.72629970, 0.69451601,

  0.66208321, 0.62920216, 0.59606986, 0.56287762, 0.52980938, 0.49704014,

  0.46473455, 0.43304576, 0.40211431, 0.37206735, 0.34301800, 0.31506490,

  0.28829195, 0.26276832, 0.23854851, 0.21567274, 0.19416736, 0.17404546,

  0.15530766, 0.13794294, 0.12192957, 0.10723616, 0.09382272, 0.08164178,

  0.07063950, 0.06075685, 0.05193064, 0.04409466, 0.03718069, 0.03111947,

  0.02584161, 0.02127838, 0.01736250, 0.01402878, 0.01121463, 0.00886058,

  0.00691064, 0.00531256, 0.00401805, 0.00298291, 0.00216702, 0.00153438,

  0.00105297, 0.00069463, 0.00043489, 0.00025272, 0.00013031, 0.0000527734,

  0.00001000, 0.00000000

};

/*

static double kaiser12_table[36] = {

   0.99440475, 1.00000000, 0.99440475, 0.97779076, 0.95066529, 0.91384741,

   0.86843014, 0.81573067, 0.75723148, 0.69451601, 0.62920216, 0.56287762,

   0.49704014, 0.43304576, 0.37206735, 0.31506490, 0.26276832, 0.21567274,

   0.17404546, 0.13794294, 0.10723616, 0.08164178, 0.06075685, 0.04409466,

   0.03111947, 0.02127838, 0.01402878, 0.00886058, 0.00531256, 0.00298291,

   0.00153438, 0.00069463, 0.00025272, 0.0000527734, 0.00000500, 0.00000000};

*/

static double kaiser10_table[36] = {

  0.99537781, 1.00000000, 0.99537781, 0.98162644, 0.95908712, 0.92831446,

  0.89005583, 0.84522401, 0.79486424, 0.74011713, 0.68217934, 0.62226347,

  0.56155915, 0.50119680, 0.44221549, 0.38553619, 0.33194107, 0.28205962,

  0.23636152, 0.19515633, 0.15859932, 0.12670280, 0.09935205, 0.07632451,

  0.05731132, 0.04193980, 0.02979584, 0.02044510, 0.01345224, 0.00839739,

  0.00488951, 0.00257636, 0.00115101, 0.00035515, 0.00000000, 0.00000000

};

static double kaiser8_table[36] = {

  0.99635258, 1.00000000, 0.99635258, 0.98548012, 0.96759014, 0.94302200,

  0.91223751, 0.87580811, 0.83439927, 0.78875245, 0.73966538, 0.68797126,

  0.63451750, 0.58014482, 0.52566725, 0.47185369, 0.41941150, 0.36897272,

  0.32108304, 0.27619388, 0.23465776, 0.19672670, 0.16255380, 0.13219758,

  0.10562887, 0.08273982, 0.06335451, 0.04724088, 0.03412321, 0.02369490,

  0.01563093, 0.00959968, 0.00527363, 0.00233883, 0.00050000, 0.00000000

};

static double kaiser6_table[36] = {

  0.99733006, 1.00000000, 0.99733006, 0.98935595, 0.97618418, 0.95799003,

  0.93501423, 0.90755855, 0.87598009, 0.84068475, 0.80211977, 0.76076565,

  0.71712752, 0.67172623, 0.62508937, 0.57774224, 0.53019925, 0.48295561,

  0.43647969, 0.39120616, 0.34752997, 0.30580127, 0.26632152, 0.22934058,

  0.19505503, 0.16360756, 0.13508755, 0.10953262, 0.08693120, 0.06722600,

  0.05031820, 0.03607231, 0.02432151, 0.01487334, 0.00752000, 0.00000000

};

struct FuncDef

{

  double *table;

  int oversample;

};

static struct FuncDef _KAISER12 = { kaiser12_table, 64 };

#define KAISER12 (&_KAISER12)

/*static struct FuncDef _KAISER12 = {kaiser12_table, 32};

#define KAISER12 (&_KAISER12)*/

static struct FuncDef _KAISER10 = { kaiser10_table, 32 };

#define KAISER10 (&_KAISER10)

static struct FuncDef _KAISER8 = { kaiser8_table, 32 };

#define KAISER8 (&_KAISER8)

static struct FuncDef _KAISER6 = { kaiser6_table, 32 };

#define KAISER6 (&_KAISER6)

struct QualityMapping

{

  int base_length;

  int oversample;

  float downsample_bandwidth;

  float upsample_bandwidth;

  struct FuncDef *window_func;

};

/* This table maps conversion quality to internal parameters. There are two

   reasons that explain why the up-sampling bandwidth is larger than the 

   down-sampling bandwidth:

   1) When up-sampling, we can assume that the spectrum is already attenuated

      close to the Nyquist rate (from an A/D or a previous resampling filter)

   2) Any aliasing that occurs very close to the Nyquist rate will be masked

      by the sinusoids/noise just below the Nyquist rate (guaranteed only for

      up-sampling).

*/

static const struct QualityMapping quality_map[11] = {

  {8, 4, 0.830f, 0.860f, KAISER6},      /* Q0 */

  {16, 4, 0.850f, 0.880f, KAISER6},     /* Q1 */

  {32, 4, 0.882f, 0.910f, KAISER6},     /* Q2 *//* 82.3% cutoff ( ~60 dB stop) 6  */

  {48, 8, 0.895f, 0.917f, KAISER8},     /* Q3 *//* 84.9% cutoff ( ~80 dB stop) 8  */

  {64, 8, 0.921f, 0.940f, KAISER8},     /* Q4 *//* 88.7% cutoff ( ~80 dB stop) 8  */

  {80, 16, 0.922f, 0.940f, KAISER10},   /* Q5 *//* 89.1% cutoff (~100 dB stop) 10 */

  {96, 16, 0.940f, 0.945f, KAISER10},   /* Q6 *//* 91.5% cutoff (~100 dB stop) 10 */

  {128, 16, 0.950f, 0.950f, KAISER10},  /* Q7 *//* 93.1% cutoff (~100 dB stop) 10 */

  {160, 16, 0.960f, 0.960f, KAISER10},  /* Q8 *//* 94.5% cutoff (~100 dB stop) 10 */

  {192, 32, 0.968f, 0.968f, KAISER12},  /* Q9 *//* 95.5% cutoff (~100 dB stop) 10 */

  {256, 32, 0.975f, 0.975f, KAISER12},  /* Q10 *//* 96.6% cutoff (~100 dB stop) 10 */

};

/*8,24,40,56,80,104,128,160,200,256,320*/

#ifdef DOUBLE_PRECISION

static double

compute_func (double x, struct FuncDef *func)

{

  double y, frac;

#else

static double

compute_func (float x, struct FuncDef *func)

{

  float y, frac;

#endif

  double interp[4];

  int ind;

  y = x * func->oversample;

  ind = (int) floor (y);

  frac = (y - ind);

  /* CSE with handle the repeated powers */

  interp[3] = -0.1666666667 * frac + 0.1666666667 * (frac * frac * frac);

  interp[2] = frac + 0.5 * (frac * frac) - 0.5 * (frac * frac * frac);

  /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac; */

  interp[0] =

      -0.3333333333 * frac + 0.5 * (frac * frac) -

      0.1666666667 * (frac * frac * frac);

  /* Just to make sure we don't have rounding problems */

  interp[1] = 1.f - interp[3] - interp[2] - interp[0];

  /*sum = frac*accum[1] + (1-frac)*accum[2]; */

  return interp[0] * func->table[ind] + interp[1] * func->table[ind + 1] +

      interp[2] * func->table[ind + 2] + interp[3] * func->table[ind + 3];

}

#if 0

#include <stdio.h>

int

main (int argc, char **argv)

{

  int i;

  for (i = 0; i < 256; i++) {

    printf ("%f\n", compute_func (i / 256., KAISER12));

  }

  return 0;

}

#endif

#ifdef FIXED_POINT

/* The slow way of computing a sinc for the table. Should improve that some day */

static spx_word16_t

sinc (float cutoff, float x, int N, struct FuncDef *window_func)

{

  /*fprintf (stderr, "%f ", x); */

  float xx = x * cutoff;

  if (fabs (x) < 1e-6f)

    return WORD2INT (32768. * cutoff);

  else if (fabs (x) > .5f * N)

    return 0;

  /*FIXME: Can it really be any slower than this? */

  return WORD2INT (32768. * cutoff * sin (M_PI * xx) / (M_PI * xx) *

      compute_func (fabs (2. * x / N), window_func));

}

#else

/* The slow way of computing a sinc for the table. Should improve that some day */

#ifdef DOUBLE_PRECISION

static spx_word16_t

sinc (double cutoff, double x, int N, struct FuncDef *window_func)

{

  /*fprintf (stderr, "%f ", x); */

  double xx = x * cutoff;

#else

static spx_word16_t

sinc (float cutoff, float x, int N, struct FuncDef *window_func)

{

  /*fprintf (stderr, "%f ", x); */

  float xx = x * cutoff;

#endif

  if (fabs (x) < 1e-6)

    return cutoff;

  else if (fabs (x) > .5 * N)

    return 0;

  /*FIXME: Can it really be any slower than this? */

  return cutoff * sin (M_PI * xx) / (M_PI * xx) * compute_func (fabs (2. * x /

          N), window_func);

}

#endif

#ifdef FIXED_POINT

static void

cubic_coef (spx_word16_t x, spx_word16_t interp[4])

{

  /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation

     but I know it's MMSE-optimal on a sinc */

  spx_word16_t x2, x3;

  x2 = MULT16_16_P15 (x, x);

  x3 = MULT16_16_P15 (x, x2);

  interp[0] =

      PSHR32 (MULT16_16 (QCONST16 (-0.16667f, 15),

          x) + MULT16_16 (QCONST16 (0.16667f, 15), x3), 15);

  interp[1] =

      EXTRACT16 (EXTEND32 (x) + SHR32 (SUB32 (EXTEND32 (x2), EXTEND32 (x3)),

          1));

  interp[3] =

      PSHR32 (MULT16_16 (QCONST16 (-0.33333f, 15),

          x) + MULT16_16 (QCONST16 (.5f, 15),

          x2) - MULT16_16 (QCONST16 (0.16667f, 15), x3), 15);

  /* Just to make sure we don't have rounding problems */

  interp[2] = Q15_ONE - interp[0] - interp[1] - interp[3];

  if (interp[2] < 32767)

    interp[2] += 1;

}

#else

static void

cubic_coef (spx_word16_t frac, spx_word16_t interp[4])

{

  /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation

     but I know it's MMSE-optimal on a sinc */

  interp[0] = -0.16667f * frac + 0.16667f * frac * frac * frac;

  interp[1] = frac + 0.5f * frac * frac - 0.5f * frac * frac * frac;

  /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac; */

  interp[3] =

      -0.33333f * frac + 0.5f * frac * frac - 0.16667f * frac * frac * frac;

  /* Just to make sure we don't have rounding problems */

  interp[2] = 1. - interp[0] - interp[1] - interp[3];

}

#endif

#ifndef DOUBLE_PRECISION

static int

resampler_basic_direct_single (SpeexResamplerState * st,

    spx_uint32_t channel_index, const spx_word16_t * in, spx_uint32_t * in_len,

    spx_word16_t * out, spx_uint32_t * out_len)

{

  const int N = st->filt_len;

  int out_sample = 0;

  int last_sample = st->last_sample[channel_index];

  spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];

  const spx_word16_t *sinc_table = st->sinc_table;

  const int out_stride = st->out_stride;

  const int int_advance = st->int_advance;

  const int frac_advance = st->frac_advance;

  const spx_uint32_t den_rate = st->den_rate;

  spx_word32_t sum;

  int j;

  while (!(last_sample >= (spx_int32_t) * in_len

          || out_sample >= (spx_int32_t) * out_len)) {

    const spx_word16_t *sinc = &sinc_table[samp_frac_num * N];

    const spx_word16_t *iptr = &in[last_sample];

#ifndef OVERRIDE_INNER_PRODUCT_SINGLE

    float accum[4] = { 0, 0, 0, 0 };

    for (j = 0; j < N; j += 4) {

      accum[0] += sinc[j] * iptr[j];

      accum[1] += sinc[j + 1] * iptr[j + 1];

      accum[2] += sinc[j + 2] * iptr[j + 2];

      accum[3] += sinc[j + 3] * iptr[j + 3];

    }

    sum = accum[0] + accum[1] + accum[2] + accum[3];

#else

    sum = inner_product_single (sinc, iptr, N);

#endif

    out[out_stride * out_sample++] = PSHR32 (sum, 15);

    last_sample += int_advance;

    samp_frac_num += frac_advance;

    if (samp_frac_num >= den_rate) {

      samp_frac_num -= den_rate;

      last_sample++;

    }

  }

  st->last_sample[channel_index] = last_sample;

  st->samp_frac_num[channel_index] = samp_frac_num;

  return out_sample;

}

#endif

#ifdef FIXED_POINT

#else

/* This is the same as the previous function, except with a double-precision accumulator */

static int

resampler_basic_direct_double (SpeexResamplerState * st,

    spx_uint32_t channel_index, const spx_word16_t * in, spx_uint32_t * in_len,

    spx_word16_t * out, spx_uint32_t * out_len)

{

  const int N = st->filt_len;

  int out_sample = 0;

  int last_sample = st->last_sample[channel_index];

  spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];

  const spx_word16_t *sinc_table = st->sinc_table;

  const int out_stride = st->out_stride;

  const int int_advance = st->int_advance;

  const int frac_advance = st->frac_advance;

  const spx_uint32_t den_rate = st->den_rate;

  double sum;

  int j;

  while (!(last_sample >= (spx_int32_t) * in_len

          || out_sample >= (spx_int32_t) * out_len)) {

    const spx_word16_t *sinc = &sinc_table[samp_frac_num * N];

    const spx_word16_t *iptr = &in[last_sample];

#ifndef OVERRIDE_INNER_PRODUCT_DOUBLE

    double accum[4] = { 0, 0, 0, 0 };

    for (j = 0; j < N; j += 4) {

      accum[0] += sinc[j] * iptr[j];

      accum[1] += sinc[j + 1] * iptr[j + 1];

      accum[2] += sinc[j + 2] * iptr[j + 2];

      accum[3] += sinc[j + 3] * iptr[j + 3];

    }

    sum = accum[0] + accum[1] + accum[2] + accum[3];

#else

    sum = inner_product_double (sinc, iptr, N);

#endif

    out[out_stride * out_sample++] = PSHR32 (sum, 15);

    last_sample += int_advance;

    samp_frac_num += frac_advance;

    if (samp_frac_num >= den_rate) {

      samp_frac_num -= den_rate;

      last_sample++;

    }

  }

  st->last_sample[channel_index] = last_sample;

  st->samp_frac_num[channel_index] = samp_frac_num;

  return out_sample;

}

#endif

#ifndef DOUBLE_PRECISION

static int

resampler_basic_interpolate_single (SpeexResamplerState * st,

    spx_uint32_t channel_index, const spx_word16_t * in, spx_uint32_t * in_len,

    spx_word16_t * out, spx_uint32_t * out_len)

{

  const int N = st->filt_len;

  int out_sample = 0;

  int last_sample = st->last_sample[channel_index];

  spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];

  const int out_stride = st->out_stride;

  const int int_advance = st->int_advance;

  const int frac_advance = st->frac_advance;

  const spx_uint32_t den_rate = st->den_rate;

  int j;

  spx_word32_t sum;

  while (!(last_sample >= (spx_int32_t) * in_len

          || out_sample >= (spx_int32_t) * out_len)) {

    const spx_word16_t *iptr = &in[last_sample];

    const int offset = samp_frac_num * st->oversample / st->den_rate;

#ifdef FIXED_POINT

    const spx_word16_t frac =