/* #define DEBUGTRACE_ENABLED */
#include "lasp_streammgr.h"
#include "debugtrace.hpp"
#include "lasp_thread.h"
#include <algorithm>
#include <assert.h>
#include <functional>
#include <iostream>

using std::cerr;
using std::endl;
using rte = std::runtime_error;

InDataHandler::InDataHandler(StreamMgr &mgr) : _mgr(mgr) { DEBUGTRACE_ENTER; }
void InDataHandler::start() {
  DEBUGTRACE_ENTER;
  _mgr.addInDataHandler(*this);
}
void InDataHandler::stop() {
#if LASP_DEBUG == 1
  stopCalled = true;
#endif
  _mgr.removeInDataHandler(*this);
}
InDataHandler::~InDataHandler() {

  DEBUGTRACE_ENTER;
#if LASP_DEBUG == 1
  if (!stopCalled) {
    cerr << "************ BUG: Stop function not called while arriving at "
            "InDataHandler's destructor. Fix this by calling "
            "InDataHandler::stop() from the derived class' destructor."
         << endl;
    abort();
  }
#endif
}

StreamMgr &StreamMgr::getInstance() {

  DEBUGTRACE_ENTER;
  static StreamMgr mgr;
  return mgr;
}
StreamMgr::StreamMgr() {
  DEBUGTRACE_ENTER;
#if LASP_DEBUG == 1
  _main_thread_id = std::this_thread::get_id();
#endif
  // Trigger a scan for the available devices, in the background.
  rescanDAQDevices(true);
}
#if LASP_DEBUG == 1
void StreamMgr::checkRightThread() const {
  assert(std::this_thread::get_id() == _main_thread_id);
}
#endif

void StreamMgr::rescanDAQDevices(bool background,
                                 std::function<void()> callback) {
  DEBUGTRACE_ENTER;

  checkRightThread();
  if (!_devices_mtx.try_lock()) {
    throw rte("A background DAQ device scan is probably already running");
  }
  _devices_mtx.unlock();

  if (_inputStream || _outputStream) {
    throw rte("Rescanning DAQ devices only possible when no stream is running");
  }
  _devices.clear();
  /* auto &pool = getPool(); */
  if (!background) {
    rescanDAQDevices_impl(callback);
  } else {
    /* pool.push_task(&StreamMgr::rescanDAQDevices_impl, this, callback); */
  }
}
void StreamMgr::rescanDAQDevices_impl(std::function<void()> callback) {
  DEBUGTRACE_ENTER;
  std::scoped_lock lck(_devices_mtx);
  _devices = DeviceInfo::getDeviceInfo();
  if (callback) {
    callback();
  }
}
bool StreamMgr::inCallback(const DaqData &data) {

  DEBUGTRACE_ENTER;

  std::scoped_lock<std::mutex> lck(_inDataHandler_mtx);

  for (auto &handler : _inDataHandlers) {

    bool res = handler->inCallback(data);
    if (!res)
      return false;
  }
  return true;
}

void StreamMgr::setSiggen(std::shared_ptr<Siggen> siggen) {

  DEBUGTRACE_ENTER;
  checkRightThread();

  std::scoped_lock<std::mutex> lck(_siggen_mtx);

  // If not set to nullptr, and a stream is running, we update the signal
  // generator by resetting it.
  if (isStreamRunningOK(StreamType::output) && siggen) {
    const Daq *daq = getDaq(StreamType::output);
    assert(daq != nullptr);
    // Reset the signal generator.
    siggen->reset(daq->samplerate());
  }
  _siggen = siggen;
}

#define DEBUG_FILLDATA 0
/**
 * @brief Converts from double precision floating point to output signal in
 * non-interleaving format.
 *
 * @tparam T
 * @param data
 * @param signal
 *
 * @return
 */
template <typename T> bool fillData(DaqData &data, const vd &signal) {
  /* DEBUGTRACE_ENTER; */
  assert(data.nframes == signal.size());

  T *res = reinterpret_cast<T *>(data.raw_ptr());
  if (std::is_floating_point<T>()) {
    for (us ch = 0; ch < data.nchannels; ch++) {
      for (us frame = 0; frame < data.nframes; frame++) {
#if DEBUG_FILLDATA == 1
        DEBUGTRACE_PRINT("SLOW flt");
        data.setSlow(frame, ch,
                     reinterpret_cast<const int8_t *>(&signal[frame]));
#else
        res[ch * data.nframes + frame] = signal[frame];
#endif
      }
    }
  } else {
    for (us ch = 0; ch < data.nchannels; ch++) {
      for (us frame = 0; frame < data.nframes; frame++) {
        const T val = (signal[frame] * std::numeric_limits<T>::max());
#if DEBUG_FILLDATA == 1
        data.setSlow(frame, ch, reinterpret_cast<const int8_t *>(&val));
#else
        res[ch * data.nframes + frame] = val;
#endif
      }
    }
  }

  return true;
}
bool StreamMgr::outCallback(DaqData &data) {

  /* DEBUGTRACE_ENTER; */

  std::scoped_lock<std::mutex> lck(_siggen_mtx);

  if (_siggen) {
    vd signal = _siggen->genSignal(data.nframes);
    switch (data.dtype) {
    case (DataTypeDescriptor::DataType::dtype_fl32):
      fillData<float>(data, signal);
      break;
    case (DataTypeDescriptor::DataType::dtype_fl64):
      fillData<double>(data, signal);
      break;
    case (DataTypeDescriptor::DataType::dtype_int8):
      fillData<int8_t>(data, signal);
      break;
    case (DataTypeDescriptor::DataType::dtype_int16):
      fillData<int16_t>(data, signal);
      break;
    case (DataTypeDescriptor::DataType::dtype_int32):
      fillData<int32_t>(data, signal);
      break;
    }
  } else {
    // Set all values to 0.
    std::fill(data.raw_ptr(), data.raw_ptr() + data.size_bytes(), 0);
  }
  return true;
}

StreamMgr::~StreamMgr() {
  DEBUGTRACE_ENTER;
  checkRightThread();
  stopAllStreams();
  if (!_inDataHandlers.empty()) {
    cerr << "*** WARNING: InDataHandlers have not been all stopped, while "
            "StreamMgr destructor is called. This is a misuse BUG"
         << endl;
    abort();
  }
}
void StreamMgr::stopAllStreams() {
  DEBUGTRACE_ENTER;
  checkRightThread();
  _inputStream.reset();
  _outputStream.reset();
}

void StreamMgr::startStream(const DaqConfiguration &config) {
  DEBUGTRACE_ENTER;
  checkRightThread();

  bool isInput = std::count_if(config.inchannel_config.cbegin(),
                               config.inchannel_config.cend(),
                               [](auto &i) { return i.enabled; });

  bool isOutput = std::count_if(config.outchannel_config.cbegin(),
                                config.outchannel_config.cend(),
                                [](auto &i) { return i.enabled; });

  // Find the first device that matches with the configuration

  DeviceInfo devinfo;
  {
    std::scoped_lock lck(_devices_mtx);

    auto devinfo2 = std::find_if(
        _devices.cbegin(), _devices.cend(),
        [&config](const DeviceInfo &d) { return config.match(d); });
    if (devinfo2 == std::cend(_devices)) {
      throw rte("Could not find a device with name " + config.device_name +
                " in list of devices.");
    }
    devinfo = *devinfo2;
  }

  isInput |= config.monitorOutput && devinfo.hasInternalOutputMonitor;

  bool isDuplex = isInput && isOutput;

  if (!isInput && !isOutput) {
    throw rte("Neither input, nor output channels enabled for "
              "stream. Cannot start.");
  }

  if (isInput && _inputStream) {
    throw rte("Error: an input stream is already running. Please "
              "first stop existing stream");
  } else if (isOutput && _outputStream) {
    throw rte("Error: output stream is already running. Please "
              "first stop existing stream");
  } else if (_inputStream) {
    if (_inputStream->duplexMode() && isOutput) {
      throw rte(
          "Error: output stream is already running (in duplex mode). Please "
          "first stop existing stream");
    }
  }

  InDaqCallback inCallback;
  OutDaqCallback outCallback;

  using namespace std::placeholders;
  std::unique_ptr<Daq> daq = Daq::createDaq(devinfo, config);

  if (isInput) {
    inCallback = std::bind(&StreamMgr::inCallback, this, _1);
  }

  if (isOutput) {
    if (_siggen) {
      DEBUGTRACE_PRINT("Resetting _siggen with new samplerate of ");
      DEBUGTRACE_PRINT(daq->samplerate());
      _siggen->reset(daq->samplerate());
    }
    outCallback = std::bind(&StreamMgr::outCallback, this, _1);
  }
  DEBUGTRACE_PRINT(isInput);
  DEBUGTRACE_PRINT(isOutput);

  daq->start(inCallback, outCallback);

  if (isInput) {
    _inputStream = std::move(daq);
    for (auto &handler : _inDataHandlers) {
      handler->reset(_inputStream.get());
    }
    if (_inputStream->duplexMode()) {
      assert(!_outputStream);
    }
  } else {
    assert(isOutput);
    _outputStream = std::move(daq);
  }
}
void StreamMgr::stopStream(const StreamType t) {
  DEBUGTRACE_ENTER;
  checkRightThread();
  switch (t) {
  case (StreamType::input): {
    if (!_inputStream) {
      throw rte("Input stream is not running");
    }
    /// Kills input stream
    _inputStream = nullptr;
    /// Send reset to all in data handlers
    for (auto &handler : _inDataHandlers) {
      handler->reset(nullptr);
    }
  } break;
  case (StreamType::output): {
    if (_inputStream && _inputStream->duplexMode()) {
      _inputStream = nullptr;
    } else {

      if (!_outputStream) {
        throw rte("Output stream is not running");
      }
      _outputStream = nullptr;
    } // end else
  } break;
  default:
    throw rte("BUG");
    break;
  }
}

void StreamMgr::addInDataHandler(InDataHandler &handler) {
  DEBUGTRACE_ENTER;
  checkRightThread();
  std::scoped_lock<std::mutex> lck(_inDataHandler_mtx);
  _inDataHandlers.push_back(&handler);
  if (_inputStream) {
    handler.reset(_inputStream.get());
  } else {
    handler.reset(nullptr);
  }
}
void StreamMgr::removeInDataHandler(InDataHandler &handler) {
  DEBUGTRACE_ENTER;
  checkRightThread();
  std::scoped_lock<std::mutex> lck(_inDataHandler_mtx);
  _inDataHandlers.remove(&handler);
}

Daq::StreamStatus StreamMgr::getStreamStatus(const StreamType type) const {
  /* DEBUGTRACE_ENTER; */

  checkRightThread();
  // Default constructor, says stream is not running, but also no errors

  const Daq *daq = getDaq(type);
  if (daq) {
    return daq->getStreamStatus();
  } else {
    return Daq::StreamStatus();
  }
}

const Daq *StreamMgr::getDaq(StreamType type) const {

  checkRightThread();

  if (type == StreamType::input) {
    return _inputStream.get();
  } else {
    // Output stream. If input runs in duplex mode, this is also the output
    // stream. In that case, we return the outputstram
    if (_inputStream && _inputStream->duplexMode()) {
      return _inputStream.get();
    }
    return _outputStream.get();
  }
}