combine.hh

#ifndef __SDR_COMBINE_HH__
#define __SDR_COMBINE_HH__

#include "config.hh"
#include "traits.hh"
#include "node.hh"
#include "buffer.hh"
#include "logger.hh"
#include <limits>

namespace sdr {

template <class Scalar> class Combine;

template <class Scalar>
class CombineSink: public Sink<Scalar>
{
public:
  CombineSink(Combine<Scalar> *combine, size_t index, RingBuffer<Scalar> &buffer)
    : Sink<Scalar>(), _index(index), _parent(combine), _buffer(buffer)
  {
    // Pass...
  }
  virtual ~CombineSink() {
    // pass...
  }

  virtual void config(const Config &src_cfg) {
    // Requires sample rate and type
    if (!src_cfg.hasType() || !src_cfg.hasSampleRate()) { return; }
    // Check type
    if (Config::typeId<Scalar>() != src_cfg.type()) {
      ConfigError err;
      err << "Can not configure CombinSink: Invalid source type " << src_cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // Notify parent
    _parent->notifyConfig(_index, src_cfg);
  }

  virtual void process(const Buffer<Scalar> &buffer, bool allow_overwrite) {
    // Copy data into ring buffer and notify parent
    _buffer.put(buffer);
    _parent->notifyData(_index);
  }

protected:
  size_t _index;
  Combine<Scalar> *_parent;
  RingBuffer<Scalar> &_buffer;
};


template <class Scalar>
class Combine
{
public:
  Combine(size_t N) {
    _buffers.reserve(N); _sinks.reserve(N);
    for (size_t i=0; i<N; i++) {
      _buffers.push_back(RingBuffer<Scalar>());
      _sinks.push_back(new CombineSink<Scalar>(this, i, _buffers.back()));
    }
  }

  virtual ~Combine() {
    // Unref all buffers and free sinks
    for (size_t i=0; i<_sinks.size(); i++) {
      delete _sinks[i];
      _buffers[i].unref();
    }
    _buffers.clear();
    _sinks.clear();
  }

  virtual void config(const Config &cfg) = 0;
  virtual void process(std::vector< RingBuffer<Scalar> > &buffers, size_t N) = 0;


protected:
  void notifyConfig(size_t idx, const Config &cfg)
  {
    // Requires type, sampleRate and buffer size
    if (!cfg.hasType() || !cfg.hasSampleRate() || !cfg.hasBufferSize()) { return; }
    // check or unify type
    if (!_config.hasType()) { _config.setType(cfg.type()); }
    else if (_config.type() != cfg.type()) {
      ConfigError err;
      err << "Can not configure Combine node: Invalid type of sink #" << idx
          << " " << cfg.type() << ", expected " << _config.type();
      throw err;
    }
    // Check sample rate
    if (!_config.hasSampleRate()) { _config.setSampleRate(cfg.sampleRate()); }
    else if (_config.sampleRate() != cfg.sampleRate()) {
      ConfigError err;
      err << "Can ont configure Combine node: Invalid sample rate of sink #" << idx
          << " " << cfg.sampleRate() << ", expected " << _config.sampleRate();
      throw err;
    }
    // Determine max buffer size
    if (!_config.hasBufferSize()) { _config.setBufferSize(cfg.bufferSize()); }
    else {
      // Take maximum:
      _config.setBufferSize(std::max(_config.bufferSize(), cfg.bufferSize()));
    }
    // Reallocate buffers
    for (size_t i=0; i<_sinks.size(); i++) {
      _buffers[i] = RingBuffer<Scalar>(_config.bufferSize());
    }
    // Propergate config
    this->config(_config);
  }

  void notifyData(size_t idx) {
    // Determine minimum size of available data
    size_t N = std::numeric_limits<size_t>::max();
    for (size_t i=0; i<_sinks.size(); i++) {
      N = std::min(N, _buffers[i].stored());
    }
    if (N > 0) { this->process(_buffers, N); }
  }

protected:
  std::vector< RingBuffer<Scalar> > _buffers;
  std::vector< CombineSink<Scalar> *> _sinks;
  Config _config;

  friend class CombineSink<Scalar>;
};


template <class Scalar>
class Interleave : public Combine<Scalar>, public Source
{
public:
  Interleave(size_t N)
    : Combine<Scalar>(N), Source(), _N(N), _buffer()
  {
    // pass...
  }

  Sink<Scalar> *sink(size_t i) {
    if (i >= _N) {
      RuntimeError err;
      err << "Interleave: Sink index " << i << " out of range [0," << _N << ")";
      throw err;
    }
    return Combine<Scalar>::_sinks[i];
  }

  virtual void config(const Config &cfg) {
    //Requres type & buffer size
    if (!cfg.hasType() || !cfg.hasBufferSize()) { return; }
    // Check type
    if (Config::typeId<Scalar>() != cfg.type()) {
      ConfigError err;
      err << "Can not configure Interleave node: Invalid source type " << cfg.type()
          << ", expected " << Config::typeId<Scalar>();
      throw err;
    }
    // Allocate buffer:
    _buffer = Buffer<Scalar>(_N*cfg.bufferSize());
    // Propergate config
    this->setConfig(Config(Config::typeId<Scalar>(), cfg.sampleRate(), _buffer.size(), 1));
  }

  virtual void process(std::vector<RingBuffer<Scalar> > &buffers, size_t N) {
    if (0 == N) { return; }
    if (! _buffer.isUnused()) {
#ifdef SDR_DEBUG
      LogMessage msg(LOG_WARNING);
      msg << "Interleave: Output buffer in use: Drop " << _N << "x" << N
          << " input values";
      Logger::get().log(msg);
#endif
      for (size_t i=0; i<buffers.size(); i++) { buffers[i].drop(N); }
      return;
    }
    size_t num = std::min(_buffer.size()/_N,N);
    // Interleave data
    size_t idx = 0;
    for (size_t i=0; i<num; i++) {
      for (size_t j=0; j<_N; j++, idx++) {
        _buffer[idx] = buffers[j][i];
      }
    }
    // Drop num elements from all ring buffers
    for (size_t i=0; i<_N; i++) {
      buffers[i].drop(num);
    }
    // Send buffer
    this->send(_buffer.head(num*_N));
  }

protected:
  size_t _N;
  Buffer<Scalar> _buffer;
};

}
#endif // __SDR_COMBINE_HH__