/* BseResampler - FPU and SSE optimized FIR Resampling code
 * Copyright (C) 2006 Stefan Westerfeld
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General
 * Public License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, Suite 330,
 * Boston, MA 02111-1307, USA.
 */
#ifndef __BSE_RESAMPLER_HH__
#define __BSE_RESAMPLER_HH__

#include <glib.h>
#include <vector>

namespace Bse
{

namespace Resampler
{

using std::vector;
using std::min;
using std::max;
using std::copy;

/* see: http://ds9a.nl/gcc-simd/ */
typedef float v4sf __attribute__ ((vector_size (16))); //((mode(V4SF))); // vector of four single floats
union F4Vector 
{
  v4sf v;
  float f[4];
};

/**
 * FIR filter routine
 *
 * A FIR filter has the characteristic that it has a finite impulse response,
 * and can be computed by convolution of the input signal with that finite
 * impulse response.
 *
 * Thus, we use this for computing the output of the FIR filter 
 *
 * output = input[0] * taps[0] + input[1] * taps[1] + ... + input[N-1] * taps[N-1]
 *
 * where input is the input signal, taps are the filter coefficients, in
 * other texts sometimes called h[0]..h[N-1] (impulse response) or a[0]..a[N-1]
 * (non recursive part of a digital filter), and N is the filter order.
 */
template<class Accumulator> inline Accumulator
fir_process_one_sample (const float *input,
                        const float *taps, /* [0..order-1] */
			const guint  order)
{
  Accumulator out = 0;
  for (guint i = 0; i < order; i++)
    out += input[i] * taps[i];
  return out;
}

/**
 * FIR filter routine for 4 samples simultaneously
 *
 * This routine produces (approximately) the same result as fir_process_one_sample
 * but computes four consecutive output values at once using vectorized (SSE)
 * instructions. Note that input and taps need to be 16-byte aligned here.
 *
 * Also note that taps is not a plain impulse response here, but a special
 * version that needs to be computed with fir_compute_taps_sse.
 */
inline void
fir_process_4samples_sse (const float *input, const float *taps, const guint order,
			  float *out0, float *out1, float *out2, float *out3)
{
  /* input and taps must be 16-byte aligned */
  const F4Vector *input_v = reinterpret_cast<const F4Vector *> (input);
  const F4Vector *taps_v = reinterpret_cast<const F4Vector *> (taps);
  F4Vector out0_v, out1_v, out2_v, out3_v;

  out0_v.v = input_v[0].v * taps_v[0].v;
  out1_v.v = input_v[0].v * taps_v[1].v;
  out2_v.v = input_v[0].v * taps_v[2].v;
  out3_v.v = input_v[0].v * taps_v[3].v;

  for (guint i = 1; i < (order + 6) / 4; i++)
    {
      out0_v.v += input_v[i].v * taps_v[i * 4 + 0].v;
      out1_v.v += input_v[i].v * taps_v[i * 4 + 1].v;
      out2_v.v += input_v[i].v * taps_v[i * 4 + 2].v;
      out3_v.v += input_v[i].v * taps_v[i * 4 + 3].v;
    }

  *out0 = out0_v.f[0] + out0_v.f[1] + out0_v.f[2] + out0_v.f[3];
  *out1 = out1_v.f[0] + out1_v.f[1] + out1_v.f[2] + out1_v.f[3];
  *out2 = out2_v.f[0] + out2_v.f[1] + out2_v.f[2] + out2_v.f[3];
  *out3 = out3_v.f[0] + out3_v.f[1] + out3_v.f[2] + out3_v.f[3];
}

vector<float> fir_compute_taps_sse (const vector<float>& no_sse_taps);

/* Helper class to allocate aligned memory */
template<class T, int ALIGN>
class AlignedMem {
  unsigned char *unaligned_mem;
  T *data;
  unsigned int n_elements;

  void
  allocate_aligned_data()
  {
    g_assert ((ALIGN % sizeof (T)) == 0);
    unaligned_mem = static_cast <unsigned char *> (g_malloc (n_elements * sizeof (T) + (ALIGN - 1)));
    
    unsigned char *aligned_mem = unaligned_mem;
    if (ptrdiff_t (unaligned_mem) % ALIGN)
      aligned_mem += ALIGN - ptrdiff_t (unaligned_mem) % ALIGN;

    data = reinterpret_cast<T *> (aligned_mem);
  }
  /* no copy constructor */
  AlignedMem (const AlignedMem&);
  /* no assignment operator */
  AlignedMem& operator= (const AlignedMem&);
public:
  AlignedMem (const vector<T>& elements)
    : n_elements (elements.size())
  {
    allocate_aligned_data();

    for (unsigned int i = 0; i < n_elements; i++)
      new (data + i) T(elements[i]);
  }
  AlignedMem (unsigned int n_elements)
    : n_elements (n_elements)
  {
    allocate_aligned_data();

    for (unsigned int i = 0; i < n_elements; i++)
      new (data + i) T();
  }
  ~AlignedMem()
  {
    /* C++ destruction order: last allocated element is deleted first */
    while (n_elements)
      data[--n_elements].~T();

    g_free (unaligned_mem);
  }
  T&
  operator[] (unsigned int pos)
  {
    return data[pos];
  }
  const T&
  operator[] (unsigned int pos) const
  {
    return data[pos];
  }
  unsigned int
  size()
  {
    return n_elements;
  }
};

/**
 * Interface for factor 2 resampling classes
 */
class Resampler2 {
public:
  /**
   * resample a data block
   */
  virtual void	  process_block (const float *input, unsigned int n_input_samples, float *output) = 0;

  /**
   * return FIR filter order
   */
  virtual guint	  order() const = 0;

  /**
   * virtual destructor for abstract class
   */
  virtual	 ~Resampler2();
};


/**
 * Factor 2 upsampling of a data stream
 *
 * Template arguments:
 *   ORDER    number of resampling filter coefficients
 *   USE_SSE  whether to use SSE (vectorized) instructions or not
 */
template<guint ORDER, bool USE_SSE>
class Upsampler2 : public Resampler2 {
  vector<float>        no_sse_taps;
  AlignedMem<float,16> history;
  AlignedMem<float,16> sse_taps;
protected:
  /* fast SSE optimized convolution */
  void
  process_4samples_aligned (const float *input /* aligned */, float *output)
  {
    const guint H = (ORDER / 2) - 1; /* half the filter length */

    output[0] = input[H];
    output[2] = input[H + 1];
    output[4] = input[H + 2];
    output[6] = input[H + 3];

    fir_process_4samples_sse (input, &sse_taps[0], ORDER, &output[1], &output[3], &output[5], &output[7]);
  }
  /* slow non-SSE convolution */
  void
  process_sample_unaligned (const float *input, float *output)
  {
    const guint H = (ORDER / 2) - 1; /* half the filter length */
    output[0] = input[H];
    output[1] = fir_process_one_sample<float> (&input[0], &no_sse_taps[0], ORDER);
  }
  void
  process_block_aligned (const float *input, unsigned n_input_samples, float *output)
  {
    unsigned int i = 0;
    if (USE_SSE)
      {
	while (i + 3 < n_input_samples)
	  {
	    process_4samples_aligned (&input[i], &output[i*2]);
	    i += 4;
	  }
      }
    while (i < n_input_samples)
      {
	process_sample_unaligned (&input[i], &output[2*i]);
	i++;
      }
  }
  void
  process_block_unaligned (const float *input, unsigned n_input_samples, float *output)
  {
    unsigned int i = 0;
    if (USE_SSE)
      {
	while ((reinterpret_cast<ptrdiff_t> (&input[i]) & 15) && i < n_input_samples)
	  {
	    process_sample_unaligned (&input[i], &output[2 * i]);
	    i++;
	  }
      }
    process_block_aligned (&input[i], n_input_samples - i, &output[2 * i]);
  }
public:
  /**
   * Constructor
   *
   * init_taps: coefficients for the upsampling FIR halfband filter
   */
  Upsampler2 (float *init_taps)
    : no_sse_taps (init_taps, init_taps + ORDER),
      history (2 * ORDER),
      sse_taps (fir_compute_taps_sse (no_sse_taps))
  {
    g_assert ((ORDER & 1) == 0);    /* even order filter */
  }
  /**
   * process_block takes a block of input samples and produces a block with
   * twice the length, containing interpolated output samples
   */
  void
  process_block (const float *input, unsigned int n_input_samples, float *output)
  {
    unsigned int history_todo = min (n_input_samples, ORDER);

    copy (input, input + history_todo, &history[ORDER]);
    process_block_aligned (&history[0], history_todo, output);
    if (n_input_samples >= ORDER)
      {
	process_block_unaligned (input, n_input_samples - history_todo, &output [2 * history_todo]);

	// build new history from new input
	copy (input + n_input_samples - ORDER, input + n_input_samples, &history[0]);
      }
    else
      {
	// build new history from end of old history
	// (very expensive if n_input_samples tends to be a lot smaller than ORDER often)
	g_memmove (&history[0], &history[n_input_samples], sizeof (history[0]) * ORDER);
      }
  }
  /**
   * returns the FIR filter order
   */
  guint
  order() const
  {
    return ORDER;
  }
};

/**
 * Factor 2 downsampling of a data stream
 *
 * Template arguments:
 *   ORDER    number of resampling filter coefficients
 *   USE_SSE  whether to use SSE (vectorized) instructions or not
 */
template<guint ORDER, bool USE_SSE>
class Downsampler2 : public Resampler2 {
  vector<float>        no_sse_taps;
  AlignedMem<float,16> history_even;
  AlignedMem<float,16> history_odd;
  AlignedMem<float,16> sse_taps;
  /* fast SSE optimized convolution */
  template<int ODD_STEPPING> void
  process_4samples_aligned (const float *input_even /* aligned */, const float *input_odd, float *output)
  {
    const guint H = (ORDER / 2) - 1; /* half the filter length */

    fir_process_4samples_sse (input_even, &sse_taps[0], ORDER, &output[0], &output[1], &output[2], &output[3]);

    output[0] += 0.5 * input_odd[H * ODD_STEPPING];
    output[1] += 0.5 * input_odd[(H + 1) * ODD_STEPPING];
    output[2] += 0.5 * input_odd[(H + 2) * ODD_STEPPING];
    output[3] += 0.5 * input_odd[(H + 3) * ODD_STEPPING];
  }
  /* slow non-SSE convolution */
  template<int ODD_STEPPING> float
  process_sample_unaligned (const float *input_even, const float *input_odd)
  {
    const guint H = (ORDER / 2) - 1; /* half the filter length */

    return fir_process_one_sample<float> (&input_even[0], &no_sse_taps[0], ORDER) + 0.5 * input_odd[H * ODD_STEPPING];
  }
  template<int ODD_STEPPING> void
  process_block_aligned (const float *input_even, const float *input_odd, float *output, unsigned int n_output_samples)
  {
    unsigned int i = 0;
    if (USE_SSE)
      {
	while (i + 3 < n_output_samples)
	  {
	    process_4samples_aligned<ODD_STEPPING> (&input_even[i], &input_odd[i * ODD_STEPPING], &output[i]);
	    i += 4;
	  }
      }
    while (i < n_output_samples)
      {
	output[i] = process_sample_unaligned<ODD_STEPPING> (&input_even[i], &input_odd[i * ODD_STEPPING]);
	i++;
      }
  }
  template<int ODD_STEPPING> void
  process_block_unaligned (const float *input_even, const float *input_odd, float *output, unsigned int n_output_samples)
  {
    unsigned int i = 0;
    if (USE_SSE)
      {
	while ((reinterpret_cast<ptrdiff_t> (&input_even[i]) & 15) && i < n_output_samples)
	  {
	    output[i] = process_sample_unaligned<ODD_STEPPING> (&input_even[i], &input_odd[i * ODD_STEPPING]);
	    i++;
	  }
      }
    process_block_aligned<ODD_STEPPING> (&input_even[i], &input_odd[i * ODD_STEPPING], &output[i], n_output_samples);
  }
  void
  deinterleave2 (const float *data, unsigned int n_data_values, float *output)
  {
    for (unsigned int i = 0; i < n_data_values; i += 2)
      output[i / 2] = data[i];
  }
public:
  /**
   * Constructor
   *
   * init_taps: coefficients for the downsampling FIR halfband filter
   */
  Downsampler2 (float *init_taps)
    : no_sse_taps (init_taps, init_taps + ORDER),
      history_even (2 * ORDER),
      history_odd (2 * ORDER),
      sse_taps (fir_compute_taps_sse (no_sse_taps))
  {
    g_assert ((ORDER & 1) == 0);    /* even order filter */
  }
  /**
   * process_block takes a block of input samples and produces a block with
   * half the length, containing downsampled output samples
   */
  void
  process_block (const float *input, unsigned int n_input_samples, float *output)
  {
    g_assert ((n_input_samples & 1) == 0);

    const unsigned int BLOCKSIZE = 1024;

    F4Vector  block[BLOCKSIZE / 4]; /* using F4Vector ensures 16-byte alignment */
    float    *input_even = &block[0].f[0];

    while (n_input_samples)
      {
	unsigned int n_input_todo = min (n_input_samples, BLOCKSIZE * 2);

	/*
	 * since the halfband filter contains zeros every other sample
	 * and since we're using SIMD instructions, which expect the
	 * data to be consecutively represented in memory, we prepare
	 * a block of samples containing only even-indexed samples
	 *
	 * we keep the deinterleaved data on the stack (instead of per-class
	 * allocated memory), to ensure that even running a lot of these
	 * downsampler streams will not result in cache trashing
	 */
	/*
	 * FIXME: this implementation is suboptimal for non-SSE, because it
	 * performs an extra deinterleaving step in any case, but deinterleaving
	 * is only required for SSE instructions
	 */
	deinterleave2 (input, n_input_todo, input_even);

	const float       *input_odd = input + 1; /* we process this one with a stepping of 2 */

	const unsigned int n_output_todo = n_input_todo / 2;
	const unsigned int history_todo = min (n_output_todo, ORDER);

	copy (input_even, input_even + history_todo, &history_even[ORDER]);
	deinterleave2 (input_odd, history_todo * 2, &history_odd[ORDER]);

	process_block_aligned <1> (&history_even[0], &history_odd[0], output, history_todo);
	if (n_output_todo >= ORDER)
	  {
	    process_block_unaligned<2> (input_even, input_odd, &output[history_todo], n_output_todo - history_todo);

	    // build new history from new input
	    copy (input_even + n_output_todo - ORDER, input_even + n_output_todo, &history_even[0]);
	    deinterleave2 (input_odd + n_input_todo - ORDER * 2, ORDER * 2, &history_odd[0]); /* FIXME: can be optimized */
	  }
	else
	  {
	    // build new history from end of old history
	    // (very expensive if n_output_todo tends to be a lot smaller than ORDER often)
	    g_memmove (&history_even[0], &history_even[n_output_todo], sizeof (history_even[0]) * ORDER);
	    g_memmove (&history_odd[0], &history_odd[n_output_todo], sizeof (history_odd[0]) * ORDER);
	  }

	n_input_samples -= n_input_todo;
	input += n_input_todo;
	output += n_output_todo;
      }
  }
  /**
   * returns the filter order
   */
  guint
  order() const
  {
    return ORDER;
  }
};

} /* namespace Resampler */

} /* namespace Bse */

#endif /* __BSE_RESAMPLER_HH__ */
