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

using std::cerr;
using std::endl;
using Lck = std::scoped_lock<std::mutex>;

RtAps::RtAps(SmgrHandle mgr, const Filter *freqWeightingFilter,
    const us nfft,
    const Window::WindowType w,
    const d overlap_percentage, const d time_constant)
  : ThreadedInDataHandler(mgr),
  _ps(nfft, w, overlap_percentage, time_constant) {

    if (freqWeightingFilter != nullptr) {
      _filterPrototype = freqWeightingFilter->clone();
    }

    startThread();
  }
RtAps::~RtAps() {
  stopThread();
}
void RtAps::inCallback(const DaqData &data) {

  DEBUGTRACE_ENTER;

  Lck lck(_ps_mtx);

  dmat fltdata = data.toFloat();
  const us nchannels = fltdata.n_cols;
  if(nchannels != _sens.size()) {
    cerr << "**** Error: sensitivity size does not match! *****" << endl;
    return;
  }
  fltdata.each_row() /= _sens.as_row();

  if (_filterPrototype) {

    // Adjust number of filters, if necessary
    if (nchannels > _freqWeightingFilters.size()) {
      while (nchannels > _freqWeightingFilters.size()) {
        _freqWeightingFilters.emplace_back(_filterPrototype->clone());
      }

      for (auto &filter : _freqWeightingFilters) {
        filter->reset();
      }
    }

    // Apply filtering
    #pragma omp parallel for
    for (us i = 0; i < nchannels; i++) {
      vd col = fltdata.col(i);
      _freqWeightingFilters.at(i)->filter(col);
      fltdata.col(i) = col;
    }
  } // End of if(_filterPrototype)

  _ps.compute(fltdata);

}
void RtAps::reset(const Daq *daq) {

  DEBUGTRACE_ENTER;
  Lck lck(_ps_mtx);
  for (auto &filter : _freqWeightingFilters) {
    filter->reset();
  }
  if(daq) {
    _sens.resize(daq->neninchannels());
    us i = 0;
    for(const auto& ch: daq->enabledInChannels()) {
      _sens[i] = ch.sensitivity;
      i++;
    }
  }

  _ps.reset();
}

ccube RtAps::getCurrentValue() const {

  DEBUGTRACE_ENTER;
  Lck lck(_ps_mtx);
  return _ps.get_est();

}