/* #define DEBUGTRACE_ENABLED */
#include "debugtrace.hpp"
#include "lasp_clip.h"
#include "lasp_daqdata.h"
#include "lasp_daq.h"
#include <mutex>

using std::cerr;
using std::endl;

using Lck = std::scoped_lock<std::mutex>;
using rte = std::runtime_error;

ClipHandler::ClipHandler(SmgrHandle mgr)
  : ThreadedInDataHandler(mgr){

    DEBUGTRACE_ENTER;
    startThread();
  }

void ClipHandler::inCallback(const DaqData &d) {

  DEBUGTRACE_ENTER;
  Lck lck(_mtx);

  dmat data = d.toFloat();

  const us nchannels = d.nchannels;
  assert(data.n_cols == nchannels);

  if (nchannels != _clip_time.size()) {
    DEBUGTRACE_PRINT("Resizing clip indication");
    _clip_time = vd(nchannels, arma::fill::value(-1));
  }

  /// Obtain max abs values for each column, convert this row-vec to a column
  /// vector, and scale with the max-range for each channel
  vd maxabs = arma::max(arma::abs(data), 0).as_col() / _max_range;

  /// Find indices for channels that have a clip
  arma::uvec clips = maxabs > clip_point;

  for (us i = 0; i < nchannels; i++) {
    if (clips(i)) {
      /// Reset clip counter
      _clip_time(i) = 0;
    } else if (_clip_time(i) > clip_indication_time) {
      /// Reset to 'unclipped'
      _clip_time(i) = -1;
    } else if (_clip_time(i) >= 0) {
      /// Add a bit of clip time
      _clip_time(i) += _dt;
    }
  }
}

arma::uvec ClipHandler::getCurrentValue() const {

  DEBUGTRACE_ENTER;
  Lck lck(_mtx);

  arma::uvec clips(_clip_time.size(), arma::fill::zeros);
  clips.elem(arma::find(_clip_time >= 0)).fill(1);

  return {clips};
}

void ClipHandler::reset(const Daq *daq) {

  DEBUGTRACE_ENTER;
  Lck lck(_mtx);

  if (daq) {

    const us nchannels = daq->neninchannels();
    _max_range.resize(nchannels);

    dvec ranges = daq->inputRangeForEnabledChannels();
    assert(ranges.size() == nchannels);
    for(us i=0;i<daq->neninchannels();i++) {
      _max_range[i] = ranges[i];
    }

    _clip_time.fill(-1);

    const d fs = daq->samplerate();
    /* DEBUGTRACE_PRINT(fs); */
    _dt = daq->framesPerBlock() / fs;
  }
}

ClipHandler::~ClipHandler() {
  DEBUGTRACE_ENTER;
  stopThread();
}