baseband.hh

#ifndef __SDR_BASEBAND_HH__
#define __SDR_BASEBAND_HH__

#include "node.hh"
#include "config.hh"
#include "utils.hh"
#include "traits.hh"
#include "operators.hh"
#include "freqshift.hh"

namespace sdr {


template <class Scalar>
class IQBaseBand: public Sink< std::complex<Scalar> >, public Source, public FreqShiftBase<Scalar>
{
public:
  typedef std::complex<Scalar> CScalar;
  typedef int32_t SScalar;
  typedef std::complex<SScalar> CSScalar;

public:
  IQBaseBand(double Fc, double width, size_t order, size_t sub_sample, double oFs=0.0)
    : Sink<CScalar>(), Source(), FreqShiftBase<Scalar>(Fc, 0),
      _Fc(Fc), _Ff(Fc), _Fs(0), _width(width), _order(std::max(size_t(1), order)),
      _sub_sample(sub_sample), _oFs(oFs), _ring_offset(0), _sample_count(0),
      _last(0), _kernel(_order)
  {
    // Allocate and reset ring buffer:
    _ring = Buffer<CSScalar>(_order);
    for (size_t i=0; i<_order; i++) { _ring[i] = 0; }
  }

  IQBaseBand(double Fc, double Ff, double width, size_t order, size_t sub_sample, double oFs=0.0)
    : Sink<CScalar>(), Source(), FreqShiftBase<Scalar>(Fc, 0),
      _Fc(Fc), _Ff(Ff), _Fs(0), _width(width), _order(std::max(size_t(1), order)),
      _sub_sample(sub_sample), _oFs(oFs), _ring_offset(0), _sample_count(0),
      _last(0), _kernel(_order)
  {
    // Allocate and reset ring buffer:
    _ring = Buffer<CSScalar>(_order);
    for (size_t i=0; i<_order; i++) { _ring[i] = 0; }
  }

  virtual ~IQBaseBand() {
    // Free buffers
    _buffer.unref();
    _kernel.unref();
    _ring.unref();
  }

  inline size_t order() const { return _order; }
  void setOrder(size_t o) {
    // ensure filter order >= 1
    o = (o<1) ? 1 : o;
    // Reallocate ring buffer and kernel
    if (! _kernel.isEmpty()) { _kernel.unref(); }
    if (! _ring.isEmpty()) { _ring.unref(); }
    _kernel = Buffer<CSScalar>(o);
    _ring = Buffer<CSScalar>(o);
    // Update filter kernel
    _order = o; _update_filter_kernel();
  }

  inline double centerFrequency() const { return _Fc; }
  void setCenterFrequency(double Fc) {
    _Fc = Fc; this->setFrequencyShift(_Fc);
  }

  inline double filterFrequency() const { return _Ff; }
  void setFilterFrequency(double Ff) {
    _Ff = Ff; _update_filter_kernel();
  }

  inline double filterWidth() const { return _width; }
  void setFilterWidth(double width) {
    _width = width; _update_filter_kernel();
  }

  size_t subSample() const { return _sub_sample; }
  void setSubsample(size_t sub_sample) {
    _sub_sample = std::max(size_t(1), sub_sample); _reconfigure();
  }
  void setOutputSampleRate(double Fs) {
    _oFs = Fs; _reconfigure();
  }

  virtual void config(const Config &src_cfg)
  {
    // Requires type, sample rate & buffer size
    if (!src_cfg.hasType() || !src_cfg.hasSampleRate() || !src_cfg.hasBufferSize()) { return; }
    // Check buffer type
    if (Config::typeId<CScalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure IQBaseBand: Invalid type " << src_cfg.type()
          << ", expected " << Config::typeId<CScalar>();
      throw err;
    }
    // Store sample rate
    _Fs = src_cfg.sampleRate();
    // Store source buffer size
    _sourceBs = src_cfg.bufferSize();

    _reconfigure();
  }


  virtual void process(const Buffer< std::complex<Scalar> > &buffer, bool allow_overwrite)
  {
    if (allow_overwrite) {
      // Perform in-place if @c allow_overwrite
      _process(buffer, buffer);
    } else if (_buffer.isUnused()) {
      // Otherwise store results into output buffer.
      _process(buffer, _buffer);
    } else {
#ifdef SDR_DEBUG
      LogMessage msg(LOG_WARNING);
      msg << "IQBaseBand: Drop buffer: Output buffer still in use.";
      Logger::get().log(msg);
#endif
    }
  }


protected:
  void _reconfigure()
  {
    // If _oFs > 0 -> set _sub_sample to match the sample rate (approx).
    if (_oFs > 0) {
      _sub_sample = _Fs/_oFs;
      if (_sub_sample < 1) { _sub_sample=1; }
    }

    // Update filter kernel
    _update_filter_kernel();
    // Update freq shift operator:
    this->setSampleRate(_Fs);
    // Calc output buffer size
    size_t buffer_size = _sourceBs/_sub_sample;
    if (_sourceBs%_sub_sample) { buffer_size += 1; }
    // Allocate output buffer
    _buffer = Buffer<CScalar>(buffer_size);

    // Reset internal state
    _last = 0;
    _sample_count = 0;
    _ring_offset = 0;

    LogMessage msg(LOG_DEBUG);
    msg << "Configured IQBaseBand node:" << std::endl
        << " type " << Traits< std::complex<Scalar> >::scalarId << std::endl
        << " sample-rate " << _Fs << "Hz" << std::endl
        << " center freq " << _Fc << "Hz" << std::endl
        << " width " << _width << "Hz" << std::endl
        << " kernel " << _kernel << std::endl
        << " in buffer size " << _sourceBs << std::endl
        << " sub-sample by " << _sub_sample << std::endl
        << " out buffer size " << buffer_size;
    Logger::get().log(msg);

     // Propergate config
    this->setConfig(Config(Traits< std::complex<Scalar> >::scalarId,
                           _Fs/_sub_sample, buffer_size, 1));
  }

  inline void _process(const Buffer<CScalar> &in, const Buffer<CScalar> &out) {
    size_t i=0, j=0;
    for (; i<in.size(); i++, _sample_count++) {
      // Store sample in ring buffer
      _ring[_ring_offset] = in[i];

      // Apply filter on ring-buffer and shift freq
      _last += this->applyFrequencyShift(_filter_ring());

      // _ring_offset modulo _order
      _ring_offset++;
      if (_order == _ring_offset) { _ring_offset = 0; }

      // If _sample_count samples have been averaged:
      if (_sub_sample == _sample_count) {
        // Store average in output buffer
        CSScalar value = _last/CSScalar(_sub_sample);
        out[j] = value;
        // reset average, sample count and increment output buffer index j
        _last = 0; _sample_count=0; j++;
      } else if (_sub_sample == 1) {
        out[j] = _last; _last = 0; _sample_count = 0; j++;
      }
    }
    this->send(out.head(j), true);
  }

  inline CSScalar _filter_ring() const
  {
    CSScalar res = 0;
    size_t idx = _ring_offset+1;
    if (_order == idx) { idx = 0; }
    for (size_t i=0; i<_order; i++, idx++) {
      if (_order == idx) { idx = 0; }
      res += _kernel[i]*_ring[idx];
    }
    return (res>>14);
  }

  void _update_filter_kernel() {
    // First, create a filter kernel of a low-pass filter with _width/2 cut-off
    Buffer< std::complex<double> > alpha(_order);
    double w = (M_PI*_width)/(_Fs);
    double M = double(_order)/2.;
    double norm = 0;

    //std::complex<double> phi(0.0, (2*M_PI*_Ff)/_Fs); phi = std::exp(phi);
    for (size_t i=0; i<_order; i++) {
      if (_order == 2*i) { alpha[i] = 4*(w/M_PI); }
      else { alpha[i] = std::sin(w*(i-M))/(w*(i-M)); }
      // Shift freq by +_Ff:
      //alpha[i] = phi*alpha[i]; phi = phi*phi;
      alpha[i] *= std::exp(std::complex<double>(0.0, (-2*M_PI*_Ff*i)/_Fs));
      // apply window function
      alpha[i] *= (0.42 - 0.5*cos((2*M_PI*i)/_order) + 0.08*cos((4*M_PI*i)/_order));
      // Calc norm
      norm += std::abs(alpha[i]);
    }
    // Normalize filter coeffs and store in _kernel:
    for (size_t i=0; i<_order; i++) {
      _kernel[i] = (double(1<<14) * alpha[i]) / norm;
    }
  }

protected:
  int32_t _Fc;
  int32_t _Ff;
  int32_t _Fs;
  int32_t _width;
  size_t _order;
  size_t _sub_sample;
  double _oFs;
  size_t _ring_offset;
  size_t _sample_count;
  CSScalar _last;
  size_t _sourceBs;

  Buffer<CSScalar> _kernel;
  Buffer<CSScalar> _ring;
  Buffer<CScalar> _buffer;
};




template <class Scalar>
class BaseBand: public Sink<Scalar>, public Source, public FreqShiftBase<Scalar>
{
public:
  typedef typename FreqShiftBase<Scalar>::CScalar  CScalar;
  typedef typename FreqShiftBase<Scalar>::SScalar  SScalar;
  typedef typename FreqShiftBase<Scalar>::CSScalar CSScalar;

public:
  BaseBand(double Fc, double width, size_t order, size_t sub_sample)
    : Sink<Scalar>(), Source(), FreqShiftBase<Scalar>(Fc, 0),
      _Ff(Fc), _width(width), _order(std::max(size_t(1), order)),
      _sub_sample(sub_sample), _ring_offset(0), _sample_count(0),
      _last(0), _kernel(_order)
  {
    // Allocate and reset ring buffer:
    _ring = Buffer<SScalar>(_order);
    for (size_t i=0; i<_order; i++) { _ring[i] = 0; }
  }

  BaseBand(double Fc, double Ff, double width, size_t order, size_t sub_sample)
    : Sink<Scalar>(), Source(), FreqShiftBase<Scalar>(Fc, 0),
      _Ff(Ff), _width(width), _order(std::max(size_t(1), order)),
      _sub_sample(sub_sample), _ring_offset(0), _sample_count(0),
      _last(0), _kernel(_order)
  {
    // Allocate and reset ring buffer:
    _ring = Buffer<SScalar>(_order);
    for (size_t i=0; i<_order; i++) { _ring[i] = 0; }
  }

  virtual ~BaseBand() {
    // Free buffers
    _buffer.unref();
    _kernel.unref();
    _ring.unref();
  }

  virtual void config(const Config &src_cfg) {
    // Requires type, sample rate & buffer size
    if (!src_cfg.hasType() || !src_cfg.hasSampleRate() || !src_cfg.hasBufferSize()) { return; }
    // Check buffer type
    if (Config::typeId<Scalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure BaseBand: Invalid type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // Store sample rate
    this->setSampleRate(src_cfg.sampleRate());

    // Calc output buffer size
    size_t buffer_size = src_cfg.bufferSize()/_sub_sample;
    if (src_cfg.bufferSize()%_sub_sample) { buffer_size += 1; }
    // Allocate output buffer
    _buffer = Buffer<CScalar>(buffer_size);

    _last = 0;
    _sample_count = 0;
    _ring_offset = 0;

    LogMessage msg(LOG_DEBUG);
    msg << "Configured BaseBand node:" << std::endl
        << " type " << Traits< std::complex<Scalar> >::scalarId << std::endl
        << " sample-rate " << FreqShiftBase<Scalar>::sampleRate() << "Hz" << std::endl
        << " center freq " << FreqShiftBase<Scalar>::frequencyShift() << "Hz" << std::endl
        << " width " << _width << "Hz" << std::endl
        << " kernel " << _kernel << std::endl
        << " in buffer size " << src_cfg.bufferSize() << std::endl
        << " sub-sample by " << _sub_sample << std::endl
        << " out buffer size " << buffer_size;
    Logger::get().log(msg);

     // Propergate config
    this->setConfig(Config(Traits< std::complex<Scalar> >::scalarId,
                           FreqShiftBase<Scalar>::sampleRate()/_sub_sample, buffer_size, 1));
  }

  virtual void setSampleRate(double Fs) {
    FreqShiftBase<Scalar>::setSampleRate(Fs);
    _update_filter_kernel();
  }

  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite)
  {
    if (_buffer.isUnused()) {
      _process(buffer, _buffer);
    } else {
#ifdef SDR_DEBUG
      LogMessage msg(LOG_WARNING);
      msg << "BaseBand: Drop buffer: Output buffer still in use.";
      Logger::get().log(msg);
#endif
    }
  }


protected:
  inline void _process(const Buffer<Scalar> &in, const Buffer<CScalar> &out) {
    size_t i=0, j=0;
    for (; i<in.size(); i++) {
      // Store sample in ring buffer
      _ring[_ring_offset] = in[i];

      _last += this->applyFrequencyShift(_filter_ring());
      _sample_count++;

      // _ring_offset modulo _order
      _ring_offset++;
      if (_order == _ring_offset) { _ring_offset = 0; }

      // If _sample_count samples have been averaged:
      if (_sub_sample == _sample_count) {
        // Store average in output buffer
        out[j] = _last/CSScalar(_sub_sample);;
        // reset average, sample count and increment output buffer index j
        _last = 0; _sample_count=0; j++;
      }
    }
    this->send(out.head(j), true);
  }

  inline CSScalar _filter_ring()
  {
    CSScalar res = 0;
    size_t idx = _ring_offset+1;
    if (_order == idx) { idx = 0; }
    for (size_t i=0; i<_order; i++, idx++) {
      if (_order == idx) { idx = 0; }
      res += _kernel[i]*_ring[idx];
    }
    return (res>>Traits<Scalar>::shift);
  }

  void _update_filter_kernel() {
    // First, create a filter kernel of a low-pass filter with
    // _width/2 cut-off
    Buffer< std::complex<double> > alpha(_order);
    double w = (2*M_PI*_width)/(2*FreqShiftBase<Scalar>::sampleRate());
    double M = double(_order)/2;
    double norm = 0;
    for (size_t i=0; i<_order; i++) {
      if (_order == (2*i)) { alpha[i] = 1; }
      else { alpha[i] = std::sin(w*(i-M))/(w*(i-M)); }
    }
    // Shift filter by _Fc, apply window function and calc norm of filter
    std::complex<double> phi(0.0, (2*M_PI*_Ff)/FreqShiftBase<Scalar>::sampleRate()); phi = std::exp(phi);
    for (size_t i=0; i<_order; i++) {
      // Shift filter kernel
      alpha[i] = alpha[i]*std::exp(std::complex<double>(0, (2*M_PI*_Ff*i)/FreqShiftBase<Scalar>::sampleRate()));
      // apply window function
      alpha[i] *= (0.42 - 0.5*cos((2*M_PI*(i+1))/(_order+2)) +
                   0.08*cos((4*M_PI*(i+1))/(_order+2)));
      // Calc norm
      norm += std::abs(alpha[i]);
    }
    // Normalize filter coeffs and store in _kernel:
    for (size_t i=0; i<_order; i++) {
      _kernel[i] = ((double(1<<Traits<Scalar>::shift) * alpha[i]) / norm);
    }
    // free alpha
    alpha.unref();
  }

protected:
  double _Ff;
  double _width;
  size_t _order;
  size_t _sub_sample;
  size_t _ring_offset;

  size_t _sample_count;
  CSScalar _last;

  bool _shift_freq;
  Buffer< CSScalar > _lut;
  size_t _lut_inc;
  size_t _lut_count;

  Buffer<CSScalar> _kernel;
  Buffer<SScalar> _ring;
  Buffer<CScalar> _buffer;

protected:
  static const size_t _lut_size=128;
};

}

#endif // __SDR_BASEBAND_HH__