/* #define DEBUGTRACE_ENABLED */
#include "lasp_ppm.h"
#include "debugtrace.hpp"
#include <mutex>

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

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

PPMHandler::PPMHandler(StreamMgr &mgr, const d decay_dBps)
  : ThreadedInDataHandler(mgr), _decay_dBps(decay_dBps) {
    DEBUGTRACE_ENTER;
    // This results in a deadlock!
    /* std::scoped_lock<std::mutex> lck(_mtx); */
    start();
  }
bool PPMHandler::inCallback_threaded(const DaqData &d) {
  /* DEBUGTRACE_ENTER; */
  std::scoped_lock<std::mutex> lck(_mtx);
  dmat data = d.toFloat();

  const us nchannels = _cur_max.size();

  vrd maxabs = arma::max(arma::abs(data));

  arma::uvec clip_indices = arma::find(maxabs > clip_point);
  arma::uvec clip(nchannels, arma::fill::zeros);
  clip.elem(clip_indices).fill(1);

  arma::uvec update_max_idx = arma::find(maxabs > _cur_max);
  arma::uvec update_max(nchannels, arma::fill::zeros);
  update_max.elem(update_max_idx).fill(1);

  assert(_cur_max.size() == _clip_time.size());

  for (us i = 0; i < nchannels; i++) {
    if (clip(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;
    }

    /* cerr << "maxabs(i)" << maxabs(i) << endl; */
    /* cerr << "curmax(i)" << _cur_max(i) << endl; */
    if (update_max(i)) {
      _cur_max(i) = maxabs(i);
    } else {
      _cur_max(i) *= _alpha;
    }
  }
  return true;
}

std::tuple<vd, arma::uvec> PPMHandler::getCurrentValue() const {

  /* DEBUGTRACE_ENTER; */
  std::scoped_lock<std::mutex> lck(_mtx);

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

  return {20 * arma::log10(_cur_max + arma::datum::eps).as_col(), clips};
}

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

  DEBUGTRACE_ENTER;
  std::scoped_lock<std::mutex> lck(_mtx);

  if (daq) {

    _cur_max = vrd(daq->neninchannels(), arma::fill::zeros);

    _clip_time = vd(daq->neninchannels(), arma::fill::value(-1));
    const d fs = daq->samplerate();
    DEBUGTRACE_PRINT(fs);
    _dt = daq->framesPerBlock() / fs;

    _alpha = std::max<d>(d_pow(10, -_dt * _decay_dBps / (20)), 0);
    DEBUGTRACE_PRINT(_alpha);

  } else {
    _cur_max.clear();
    _clip_time.clear();
  }
}

PPMHandler::~PPMHandler() {
  DEBUGTRACE_ENTER;
  std::scoped_lock<std::mutex> lck(_mtx);
  stop();
}