/*

FPUDownsampler2x.h
Copyright(c) 2005 Laurent de Soras

Downsamples the input signal by a factor 2, using FPU.

Template parameters:
  - NC: number of coefficients, > 0

  --- Legal stuff ---

This program is free software. It comes without any warranty, to
the extent permitted by applicable law. You can redistribute it
and/or modify it under the terms of the Do What The Fuck You Want
To Public License, Version 2, as published by Sam Hocevar. See
http://sam.zoy.org/wtfpl/COPYING for more details.

*/

#include <array>
#include "FPUStageProc.h"

namespace hiir
{
template <int NC, typename T>
class Downsampler2xFPU
{
public:
  enum { NBR_COEFS = NC };
  Downsampler2xFPU();

  /*
  Name: set_coefs
  Description:
  Sets filter coefficients. Generate them with the PolyphaseIir2Designer
  class.
  Call this function before doing any processing.
  Input parameters:
  - coef_arr: Array of coefficients. There should be as many coefficients as
  mentioned in the class template parameter.
  */
  void set_coefs(const double coef_arr[]);

  /*
  Name: process_sample
  Description:
  Downsamples (x2) one pair of samples, to generate one output sample.
  Input parameters:
  - in_ptr: pointer on the two samples to decimate
  Returns: Samplerate-reduced sample.
  */
  inline T process_sample(const T in_ptr [2]);

  /*
  Name: process_block
  Description:
  Downsamples (x2) a block of samples.
  Input and output blocks may overlap, see assert() for details.
  Input parameters:
  - in_ptr: Input array, containing nbr_spl * 2 samples.
  - nbr_spl: Number of samples to output, > 0
  Output parameters:
  - out_ptr: Array for the output samples, capacity: nbr_spl samples.
  */
  void process_block(T out_ptr[], const T in_ptr[], long nbr_spl);
  
  /*
   Name: process_sample_split
   Description:
   Split (spectrum-wise) in half a pair of samples. The lower part of the
   spectrum is a classic downsampling, equivalent to the output of
   process_sample().
   The higher part is the complementary signal: original filter response
   is flipped from left to right, becoming a high-pass filter with the same
   cutoff frequency. This signal is then critically sampled (decimation by 2),
   flipping the spectrum: Fs/4...Fs/2 becomes Fs/4...0.
   Input parameters:
   - in_ptr: pointer on the pair of input samples
   Output parameters:
   - low: output sample, lower part of the spectrum (downsampling)
   - high: output sample, higher part of the spectrum.
   */
  inline void process_sample_split(T &low, T &high, const T in_ptr[2]);

  /*
  Name: process_block_split
  Description:
   Split (spectrum-wise) in half a block of samples. The lower part of the
   spectrum is a classic downsampling, equivalent to the output of
   process_block().
   The higher part is the complementary signal: original filter response
   is flipped from left to right, becoming a high-pass filter with the same
   cutoff frequency. This signal is then critically sampled (decimation by 2),
   flipping the spectrum: Fs/4...Fs/2 becomes Fs/4...0.
  Input and output blocks may overlap, see assert() for details.
  Input parameters:
  - in_ptr: Input array, containing nbr_spl * 2 samples.
  - nbr_spl: Number of samples for each output, > 0
  Output parameters:
  - out_l_ptr: Array for the output samples, lower part of the spectrum
      (downsampling). Capacity: nbr_spl samples.
  - out_h_ptr: Array for the output samples, higher part of the spectrum.
      Capacity: nbr_spl samples.
  */
  void process_block_split(T out_l_ptr[], T out_h_ptr[], const T in_ptr[], long nbr_spl);

  /*
  Name: clear_buffers
  Description:
  Clears filter memory, as if it processed silence since an infinite amount
  of time.
  */
  void clear_buffers();

private:
  std::array<T, NBR_COEFS> _coef;
  std::array<T, NBR_COEFS> _x;
  std::array<T, NBR_COEFS> _y;

private:
  bool operator == (const Downsampler2xFPU &other);
  bool operator != (const Downsampler2xFPU &other);

};  // class Downsampler2xFPU


template <int NC, typename T>
Downsampler2xFPU <NC, T>::Downsampler2xFPU ()
: _coef ()
, _x ()
, _y ()
{
  for (int i = 0; i < NBR_COEFS; ++i)
  {
    _coef [i] = 0;
  }
  clear_buffers ();
}

template <int NC, typename T>
void  Downsampler2xFPU <NC, T>::set_coefs (const double coef_arr[])
{
  assert (coef_arr != 0);

  for (int i = 0; i < NBR_COEFS; ++i)
  {
    _coef [i] = static_cast <T> (coef_arr [i]);
  }
}

template <int NC, typename T>
T Downsampler2xFPU <NC, T>::process_sample (const T in_ptr [2])
{
  assert (in_ptr != 0);

  T spl_0 (in_ptr [1]);
  T spl_1 (in_ptr [0]);

  #if defined (_MSC_VER)
    #pragma inline_depth (255)
  #endif  // _MSC_VER

  StageProcFPU <NBR_COEFS, T>::process_sample_pos (
    NBR_COEFS,
    spl_0,
    spl_1,
    &_coef [0],
    &_x [0],
    &_y [0]
  );

  return (0.5f * (spl_0 + spl_1));
}


template <int NC, typename T>
void Downsampler2xFPU <NC, T>::process_block (T out_ptr[], const T in_ptr[], long nbr_spl)
{
  assert (in_ptr != 0);
  assert (out_ptr != 0);
  assert (out_ptr <= in_ptr || out_ptr >= in_ptr + nbr_spl * 2);
  assert (nbr_spl > 0);

  long pos = 0;
  do
  {
    out_ptr [pos] = process_sample (&in_ptr [pos * 2]);
    ++pos;
  }
  while (pos < nbr_spl);
}

template <int NC, typename T>
void Downsampler2xFPU <NC, T>::process_sample_split (T &low, T &high, const T in_ptr [2])
{
  assert (&low != 0);
  assert (&high != 0);
  assert (in_ptr != 0);

  T       spl_0 = in_ptr [1];
  T       spl_1 = in_ptr [0];

  #if defined (_MSC_VER)
    #pragma inline_depth (255)
  #endif  // _MSC_VER

  StageProcFPU <NBR_COEFS, T>::process_sample_pos (
    NBR_COEFS,
    spl_0,
    spl_1,
    &_coef [0],
    &_x [0],
    &_y [0]
  );

  low = (spl_0 + spl_1) * 0.5f;
  high = spl_0 - low; // (spl_0 - spl_1) * 0.5f;
}

template <int NC, typename T>
void Downsampler2xFPU <NC, T>::process_block_split (T out_l_ptr[], T out_h_ptr[], const T in_ptr[], long nbr_spl)
{
  assert (in_ptr != 0);
  assert (out_l_ptr != 0);
  assert (out_l_ptr <= in_ptr || out_l_ptr >= in_ptr + nbr_spl * 2);
  assert (out_h_ptr != 0);
  assert (out_h_ptr <= in_ptr || out_h_ptr >= in_ptr + nbr_spl * 2);
  assert (out_h_ptr != out_l_ptr);
  assert (nbr_spl > 0);

  long pos = 0;
  do
  {
    process_sample_split
    (
      out_l_ptr [pos],
      out_h_ptr [pos],
      &in_ptr [pos * 2]
    );
    ++pos;
  }
  while (pos < nbr_spl);
}

template <int NC, typename T>
void Downsampler2xFPU <NC, T>::clear_buffers ()
{
  for (int i = 0; i < NBR_COEFS; ++i)
  {
    _x [i] = 0;
    _y [i] = 0;
  }
}

} // namespace hiir