utils.hh

#ifndef __SDR_UTILS_HH__
#define __SDR_UTILS_HH__

#include "config.hh"
#include "node.hh"
#include "traits.hh"
#include "operators.hh"
#include "logger.hh"
#include <ctime>


namespace sdr {

template <class Scalar>
class RealImagPart: public Sink< std::complex<Scalar> >, public Source
{
public:
  RealImagPart(bool select_real, double scale=1.0)
    : Sink< std::complex<Scalar> >(), Source(), _buffer(), _select_real(select_real), _scale(scale)
  {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    // Needs full config
    if ((Config::Type_UNDEFINED==src_cfg.type()) || (0 == src_cfg.bufferSize())) { return; }
    // Assert complex type
    if (src_cfg.type() != Config::typeId< std::complex<Scalar> >()) {
      ConfigError err;
      err << "Can not configure sink of RealPart: Invalid buffer type " << src_cfg.type()
          << " expected " << Config::typeId< std::complex<Scalar> >();
      throw err;
    }
    // Resize buffer
    _buffer = Buffer<Scalar>(src_cfg.bufferSize());
    // propergate config
    this->setConfig(Config(Config::typeId< Scalar >(), src_cfg.sampleRate(),
                           src_cfg.bufferSize(), 1));

    if (_select_real) {
      LogMessage msg(LOG_DEBUG);
      msg << "Configured RealPart node:" << std::endl
          << " type: " << Config::typeId<Scalar>() << std::endl
          << " sample-rate: " << src_cfg.sampleRate() << std::endl
          << " buffer-size: " << src_cfg.bufferSize();
      Logger::get().log(msg);
    }
  }

  virtual void process(const Buffer<std::complex<Scalar> > &buffer, bool allow_overwrite) {
    // Convert
    if (_select_real) {
      for (size_t i=0; i<buffer.size(); i++) {
        _buffer[i] = _scale*buffer[i].real();
      }
    } else {
      for (size_t i=0; i<buffer.size(); i++) {
        _buffer[i] = _scale*buffer[i].imag();
      }
    }
    this->send(_buffer);
  }

protected:
  Buffer<Scalar> _buffer;
  bool _select_real;
  double _scale;
};


template <class Scalar>
class RealPart: public RealImagPart<Scalar>
{
public:
  RealPart(double scale=1.0)
    : RealImagPart<Scalar>(true, scale)
  {
    // pass...
  }
};


template <class Scalar>
class ImagPart: public RealImagPart<Scalar>
{
public:
  ImagPart(double scale=1.0)
    : RealImagPart<Scalar>(false, scale)
  {
    // pass...
  }
};


template <class Scalar>
class IQBalance: public Sink< std::complex<Scalar> >, public Source
{
public:
  typedef typename Traits<Scalar>::SScalar SScalar;

public:
  IQBalance(double balance=0.0)
    : Sink< std::complex<Scalar> >(), Source(), _realFact(1), _imagFact(1)
  {
    if (balance < 0) {
      // scale real part
      balance = std::min(1.,-balance);
      _realFact = (1-balance)*(1<<8);
      _imagFact = (1<<8);
    } else { // scale imag part
      balance = std::min(1., balance);
      _realFact = (1<<8);
      _imagFact = (1-balance)*(1<<8);
    }
  }

  virtual ~IQBalance() {
    _buffer.unref();
  }

  double balance() const {
    if (_realFact != (1<<8)) {
      return (double(_realFact)/(1<<8)-1);
    }
    return (1-double(_imagFact)/(1<<8));
  }

  void setBalance(double balance) {
    if (balance < 0) {
      // scale real part
      balance = std::min(1.,-balance);
      _realFact = balance*(1<<8);
      _imagFact = (1<<8);
    } else { // scale imag part
      balance = std::min(1., balance);
      _realFact = (1<<8);
      _imagFact = balance*(1<<8);
    }
  }

  virtual void config(const Config &src_cfg) {
    // Check if config is complete
    if (! src_cfg.hasBufferSize()) { return; }
    // Allocate buffer
    _buffer = Buffer< std::complex<Scalar> >(src_cfg.bufferSize());
    // Forward config
    Config cfg(src_cfg); cfg.setNumBuffers(1);
    this->setConfig(cfg);
  }

  virtual void process(const Buffer<std::complex<Scalar> > &buffer, bool allow_overwrite) {
    if (allow_overwrite) {
      _process(buffer, buffer);
      this->send(buffer);
    } else {
      _process(buffer, _buffer);
      this->send(_buffer.head(buffer.size()));
    }
  }

protected:
  void _process(const Buffer< std::complex<Scalar> > &in, const Buffer< std::complex<Scalar> > &out) {
    for (size_t i=0; i<in.size(); i++) {
      out[i] = std::complex<Scalar>((_realFact*SScalar(in[i].real()))/(1<<8),
                                    (_imagFact*SScalar(in[i].imag()))/(1<<8));
    }
  }

protected:
  int32_t _realFact;
  int32_t _imagFact;
  Buffer< std::complex<Scalar> > _buffer;
};


template <class iScalar, class oScalar=iScalar>
class ToComplex: public Sink<iScalar>, public Source
{
public:
  ToComplex(double scale=1.0)
    : Sink<iScalar>(), Source(), _scale(scale)
  {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    // Requires at least type & buffer size
    if (!src_cfg.hasType() || !src_cfg.hasBufferSize()) { return; }
    // Check input type
    if (Config::typeId<iScalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure ToComplex node: Invalid buffer type " << src_cfg.type()
          << ", expected " << Config::typeId<iScalar>();
      throw err;
    }
    // Allocate buffer:
    _buffer = Buffer< std::complex<oScalar> >(src_cfg.bufferSize());
    // Propergate config
    this->setConfig(
          Config(Config::typeId< std::complex<oScalar> >(),
                 src_cfg.sampleRate(), src_cfg.bufferSize(), src_cfg.numBuffers()));
  }

  virtual void process(const Buffer<iScalar> &buffer, bool allow_overwrite) {
    if (1.0 == _scale) {
      for (size_t i=0; i<buffer.size(); i++) {
        _buffer[i] = std::complex<oScalar>(oScalar(buffer[i]));
      }
    } else  {
      for (size_t i=0; i<buffer.size(); i++) {
          _buffer[i] = std::complex<oScalar>(_scale*oScalar(buffer[i]));
      }
    }
    // propergate buffer
    this->send(_buffer.head(buffer.size()));
  }

protected:
  double _scale;
  Buffer< std::complex<oScalar> > _buffer;
};



template <class iScalar, class oScalar>
class Cast : public Sink<iScalar>, public Source
{
public:
  typedef typename Traits<iScalar>::SScalar iSScalar;
  typedef typename Traits<oScalar>::SScalar oSScalar;

public:
  Cast(oScalar scale=1, iScalar shift=0)
    : Sink<iScalar>(), Source(), _can_overwrite(false), _do_scale(false),
      _scale(scale), _shift(shift), _buffer()
  {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    // only check type
    if (!src_cfg.hasType()) { return; }
    if (src_cfg.type() != Config::typeId<iScalar>()) {
      ConfigError err;
      err << "Can not configure Cast: Invalid input type " << src_cfg.type()
          << ", expected " << Config::typeId<iScalar>();
      throw err;
    }

    // unreference buffer if non-empty
    if (! _buffer.isEmpty()) { _buffer.unref(); }

    // allocate buffer
    _buffer = Buffer<oScalar>(src_cfg.bufferSize());

    LogMessage msg(LOG_DEBUG);
    msg << "Configure Cast node:" << std::endl
        << " conversion: "<< Config::typeId<iScalar>()
        << " -> " << Config::typeId<oScalar>() << std::endl
        << " in-place " << (_can_overwrite ? "true" : "false") << std::endl
        << " scale: " << _scale;
    Logger::get().log(msg);

    _can_overwrite = sizeof(iScalar) >= sizeof(oScalar);
    _do_scale = (_scale != oScalar(0));

    // forward config
    this->setConfig(Config(Config::typeId<oScalar>(), src_cfg.sampleRate(),
                           src_cfg.bufferSize(), 1));
  }

  virtual void process(const Buffer<iScalar> &buffer, bool allow_overwrite) {
    if (allow_overwrite && _can_overwrite) {
      _process(buffer, Buffer<oScalar>(buffer));
    } else if (_buffer.isUnused()) {
      _process(buffer, _buffer);
    } else {
#ifdef SDR_DEBUG
      std::cerr << "Cast: Drop buffer: Output buffer is still in use by "
                << _buffer.refCount() << std::endl;
#endif
    }
  }

  inline double scale() const { return _scale; }

  void setScale(double scale) {
    _scale = scale;
    _do_scale = (0 != _scale);
  }

protected:
  inline void _process(const Buffer<iScalar> &in, const Buffer<oScalar> &out) {
    if (_do_scale) {
      for (size_t i=0; i<in.size(); i++) {
        out[i] = _scale*( cast<iScalar,oScalar>(in[i]) + cast<iScalar, oScalar>(_shift) );
      }
    } else {
      for (size_t i=0; i<in.size(); i++) {
        out[i] = in[i]+_shift;
      }
    }
    this->send(out.head(in.size()));
  }

protected:
  bool _can_overwrite;
  bool _do_scale;
  oScalar _scale;
  iScalar _shift;
  Buffer<oScalar> _buffer;
};



class UnsignedToSigned: public SinkBase, public Source
{
public:
  UnsignedToSigned(float scale=1.0);
  virtual ~UnsignedToSigned();

  virtual void config(const Config &src_cfg);
  virtual void handleBuffer(const RawBuffer &buffer, bool allow_overwrite);

protected:
  void _process_int8(const RawBuffer &in, const RawBuffer &out);
  void _process_int16(const RawBuffer &in, const RawBuffer &out);

protected:
  void (UnsignedToSigned::*_process)(const RawBuffer &in, const RawBuffer &out);
  RawBuffer _buffer;
  float _scale;
};



class SignedToUnsigned: public SinkBase, public Source
{
public:
  SignedToUnsigned();
  virtual ~SignedToUnsigned();

  virtual void config(const Config &src_cfg);
  virtual void handleBuffer(const RawBuffer &buffer, bool allow_overwrite);

protected:
  void _process_int8(const RawBuffer &in, const RawBuffer &out);
  void _process_int16(const RawBuffer &in, const RawBuffer &out);

protected:
  void (SignedToUnsigned::*_process)(const RawBuffer &in, const RawBuffer &out);
  RawBuffer _buffer;
};


template <class Scalar>
class FreqShift: public Sink< std::complex<Scalar> >, public Source
{
public:
  FreqShift(double shift, Scalar scale=1.0)
    : Sink< std::complex<Scalar> >(), Source(),
      _shift(shift), _scale(scale), _factor(1), _sample_rate(0), _delta(1)
  {
    // pass...
  }

  virtual ~FreqShift() {
    // pass...
  }

  inline double shift() const { return _shift; }

  void setShift(double shift) {
    // Update delta
    _shift = shift;
    _delta  = exp(std::complex<double>(0,2*M_PI*_shift/_sample_rate));
  }

  virtual void config(const Config &src_cfg) {
    // Requires type, samplerate & buffersize
    if ((Config::Type_UNDEFINED==src_cfg.type()) || (0==src_cfg.sampleRate()) || (0==src_cfg.bufferSize())) { return; }
    // Assert type
    if (src_cfg.type() != Config::typeId< std::complex<Scalar> >()) {
      ConfigError err;
      err << "Can not configure FreqShift node: Invalid source type " << src_cfg.type()
          << ", expected " << Config::typeId< std::complex<Scalar> >();
      throw err;
    }
    // Allocate buffer
    _buffer = Buffer< std::complex<Scalar> >(src_cfg.bufferSize());
    // Store sample rate
    _sample_rate = src_cfg.sampleRate();
    // Precalc delta
    _delta  = exp(std::complex<double>(0,2*M_PI*_shift/_sample_rate));
    // reset factor
    _factor = 1;

    LogMessage msg(LOG_DEBUG);
    msg << "Configure FreqShift node:" << std::endl
        << " shift: " << _shift << std::endl
        << " scale: " << _scale << std::endl
        << " sample-rate: " << src_cfg.sampleRate() << std::endl
        << " buffer-suize: " << src_cfg.bufferSize();
    Logger::get().log(msg);

    // Propergate config
    this->setConfig(Config(Config::typeId< std::complex<Scalar> >(), src_cfg.sampleRate(),
                           src_cfg.bufferSize(), 1));
  }

  virtual void process(const Buffer<std::complex<Scalar> > &buffer, bool allow_overwrite) {
    // Shift freq:
    for (size_t i=0; i<buffer.size(); i++) {
      _buffer[i] = (double(_scale)*_factor)*buffer[i]; _factor *= _delta;
    }
    // Send buffer
    this->send(_buffer);
  }

protected:
  Buffer< std::complex<Scalar> > _buffer;
  double _shift;
  Scalar _scale;
  std::complex<double> _factor;
  double _sample_rate;
  std::complex<double> _delta;
};


template<class Scalar>
class StreamSource: public Source
{
public:
  StreamSource(std::istream &stream, double sample_rate=1, size_t buffersize=1024)
    : Source(), _stream(stream), _buffer(buffersize)
  {
    // Assemble config for stream:
    this->_config = Config(Config::typeId<Scalar>(), sample_rate, buffersize, 1);
  }

  void next() {
    if(_stream.eof()) { Queue::get().stop(); return; }
    int len = _stream.read(_buffer.ptr(), _buffer.storageSize());
    if (len > 0) {
      this->send(_buffer.head(len/sizeof(Scalar)));
    }
  }

protected:
  std::istream &_stream;
  Buffer<Scalar> _buffer;
};


template <class Scalar>
class StreamSink: public Sink<Scalar>
{
public:
  StreamSink(std::ostream &stream)
    : Sink<Scalar>(), _stream(stream)
  {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    if (Config::Type_UNDEFINED == src_cfg.type()) { return; }
    if (src_cfg.type() != Config::typeId<Scalar>()) {
      ConfigError err;
      err << "Can not configure StreamSink: Invalid buffer type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // done...
  }

  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite) {
    _stream.write(buffer.data(), buffer.size()*sizeof(Scalar));
  }

protected:
  std::ostream &_stream;
};




template <class Scalar>
class Scale : public Sink<Scalar>, public Source
{
public:
  Scale(float scale=1, Scalar shift=0)
    : Sink<Scalar>(), Source(), _scale(scale), _shift(shift)
  {
    // pass...
  }

  virtual ~Scale() {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    // Check for type & buffer size
    if (!src_cfg.hasType() || !src_cfg.bufferSize()) { return; }
    // Check type
    if (Config::typeId<Scalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure Scale node: Invalid type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // Allocate buffer
    _buffer = Buffer<Scalar>(src_cfg.bufferSize());
    // Done, propergate config
    this->setConfig(src_cfg);
  }

  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite) {
    if ((1 == _scale) && (Scalar(0) == _shift)) {
      this->send(buffer, allow_overwrite);
    } else if (allow_overwrite) {
      // Scale inplace
      for (size_t i=0; i<buffer.size(); i++) { buffer[i] *= _scale; }
      this->send(buffer, allow_overwrite);
    } else if (_buffer.isUnused()) {
      // Scale out-of-place
      for (size_t i=0; i<buffer.size(); i++) { _buffer[i] = _scale*_buffer[i]; }
      this->send(_buffer.head(buffer.size()), true);
    }
    // else if buffer is still in use -> drop buffer...
  }

protected:
  Buffer<Scalar> _buffer;
  float _scale;
  Scalar _shift;
};



template <class Scalar>
class AGC: public Sink<Scalar>, public Source
{
public:
  AGC(double tau=0.1, double target=0)
    : Sink<Scalar>(), Source(), _enabled(true), _tau(tau), _lambda(0), _sd(0),
      _target(target), _gain(1), _sample_rate(0)
  {
    if (0 == target) {
      // Determine target by scalar type
      switch (Config::typeId<Scalar>()) {
      case Config::Type_u8:
      case Config::Type_s8:
      case Config::Type_cu8:
      case Config::Type_cs8:
        _target = 64; break;
      case Config::Type_u16:
      case Config::Type_s16:
      case Config::Type_cu16:
      case Config::Type_cs16:
        _target = 16000; break;
      case Config::Type_f32:
      case Config::Type_f64:
      case Config::Type_cf32:
      case Config::Type_cf64:
        _target = 0.5; break;
      case Config::Type_UNDEFINED: {
        ConfigError err; err << "Can not configure AGC node: Unsupported type."; throw err;
      }
      }
    }
    _sd = _target;
  }

  inline bool enabled() const {
    return _enabled;
  }

  inline void enable(bool enabled) {
    _enabled = enabled;
  }

  inline float gain() const {
    return _gain;
  }

  inline void setGain(float gain) {
    _gain = gain;
  }

  inline double tau() const {
    return _tau;
  }

  inline void setTau(double tau) {
    // calc decay factor
    _tau = tau;
    _lambda = exp(-1./(_tau*_sample_rate));
  }

  virtual void config(const Config &src_cfg) {
    // Need sample rate & type
    if (!src_cfg.hasType() || !src_cfg.hasSampleRate() || !src_cfg.hasBufferSize()) { return; }
    if (Config::typeId<Scalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure AGC node: Invalid type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }

    // calc decay factor
    _sample_rate = src_cfg.sampleRate();
    _lambda = exp(-1./(_tau*src_cfg.sampleRate()));
    // reset variance
    _sd = _target;

    // Allocate buffer
    _buffer = Buffer<Scalar>(src_cfg.bufferSize());

    LogMessage msg(LOG_DEBUG);
    msg << "Configured AGC:" << std::endl
        << " type: " << src_cfg.type() << std::endl
        << " sample-rate: " << src_cfg.sampleRate() << std::endl
        << " tau: " << _tau << std::endl
        << " lambda [1/s]: " << pow(_lambda, src_cfg.sampleRate()) << std::endl
        << " lambda [1/sam]: " << _lambda << std::endl
        << " target value: " << _target;
    Logger::get().log(msg);

    // Propergate config
    this->setConfig(Config(Config::typeId<Scalar>(), src_cfg.sampleRate(), 
                    src_cfg.bufferSize(), 1));
  }


  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite) {
    // Simply forward buffer if disabled
    if ((! _enabled) && (0 == _gain) ) {
      this->send(buffer, allow_overwrite); return;
    }
    // Update signal ampl
    for (size_t i=0; i<buffer.size(); i++) {
      _sd = _lambda*_sd + (1-_lambda)*std::abs(buffer[i]);
      if (_enabled) { _gain = _target/(4*_sd); }
      _buffer[i] = _gain*buffer[i];
    }
    this->send(_buffer);
  }


protected:
  bool _enabled;
  float _tau;
  float _lambda;
  float _sd;
  float _target;
  float _gain;
  double _sample_rate;
  Buffer<Scalar> _buffer;
};


template <class Scalar>
class DebugStore: public Sink<Scalar>
{
public:
  DebugStore() : Sink<Scalar>(), _buffer(), _view() { }
  virtual ~DebugStore() {
    _buffer.unref();
  }

  virtual void config(const Config &src_cfg) {
    // Requires type and buffer size
    if (!src_cfg.hasType() || !src_cfg.hasBufferSize()) { return; }
    // Check type
    if (Config::typeId<Scalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure DebugStore node: Invalid input type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // Allocate buffer
    _buffer = Buffer<Scalar>(src_cfg.bufferSize());
  }

  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite) {
    size_t N = std::min(buffer.size(), _buffer.size());
    memcpy(_buffer.ptr(), buffer.data(), N*sizeof(Scalar));
    // Store view
    _view = _buffer.head(N);
  }

  inline const Buffer<Scalar> &buffer() const { return _view; }
  inline void clear() { _view = Buffer<Scalar>(); }

public:
  Buffer<Scalar> _buffer;
  Buffer<Scalar> _view;
};


template <class Scalar>
class DebugDump: public Sink<Scalar>
{
public:
  DebugDump(std::ostream &stream=std::cerr)
    : Sink<Scalar>(), _stream(stream)
  {
    // pass...
  }

  virtual ~DebugDump() {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    // Requires type
    if (!src_cfg.hasType()) { return; }
    // check type
    if (Config::typeId<Scalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure DebugDump sink: Invalid input type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // Done...
  }

  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite) {
    _stream << buffer << std::endl;
  }

protected:
  std::ostream &_stream;
};

class TextDump : public Sink<uint8_t>
{
public:
  TextDump(std::ostream &stream=std::cerr);
  virtual void config(const Config &src_cfg);
  virtual void process(const Buffer<uint8_t> &buffer, bool allow_overwrite);

protected:
  std::ostream &_stream;
};


template <class Scalar>
class GWNSource: public Source
{
public:
  GWNSource(double sample_rate, size_t buffer_size=1024)
    : Source(), _sample_rate(sample_rate), _buffer_size(buffer_size), _buffer(_buffer_size),
      _mean(0)
  {
    // Seed RNG
    std::srand(std::time(0));
    // Determine mean
    switch (Config::typeId<Scalar>()) {
    case Config::Type_u8:
    case Config::Type_cu8:
    case Config::Type_u16:
    case Config::Type_cu16:
      _mean = 1;
      break;
    default:
      _mean = 0;
      break;
    }
    // Propergate config
    setConfig(Config(Config::typeId<Scalar>(), _sample_rate, _buffer_size, 1));
  }

  virtual ~GWNSource() {
    _buffer.unref();
  }

  void next() {
    double a,b;
    for (size_t i=0; i<_buffer_size; i+=2) {
      _getNormal(a,b);
      _buffer[i] = Scalar(Traits<Scalar>::scale*(a+_mean));
      _buffer[i+1] = Scalar(Traits<Scalar>::scale*(b+_mean));
    }
    if (_buffer_size%2) {
      _getNormal(a,b); _buffer[_buffer_size-1] = Scalar(Traits<Scalar>::scale*(a+_mean));
    }
    this->send(_buffer, true);
  }


protected:
  void _getNormal(double &a, double &b) {
    // Obtain pair of std. normal floating point values
    double x = 2*(double(std::rand())/RAND_MAX) - 1;
    double y = 2*(double(std::rand())/RAND_MAX) - 1;
    double s = std::sqrt(x*x + y*y);
    while (s >= 1) {
      x = 2*(double(std::rand())/RAND_MAX) - 1;
      y = 2*(double(std::rand())/RAND_MAX) - 1;
      s = std::sqrt(x*x + y*y);
    }
    a = x*std::sqrt(-2*log(s)/s);
    b = y*std::sqrt(-2*log(s)/s);
  }

protected:
  double _sample_rate;
  size_t _buffer_size;
  Buffer<Scalar> _buffer;
  double _mean;
};

}

#endif // __SDR_UTILS_HH__