/* #define DEBUGTRACE_ENABLED */
#include "debugtrace.hpp"
#include "lasp_slm.h"
#include "lasp_thread.h"
#include <algorithm>
#include <cmath>
#include <future>
#include <memory>

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

SLM::SLM(const d fs, const d Lref, const us downsampling_fac, const d tau,
         std::unique_ptr<Filter> pre_filter,
         std::vector<std::unique_ptr<Filter>> bandpass)
    : _pre_filter(std::move(pre_filter)), _bandpass(std::move(bandpass)),
      _alpha(exp(-1 / (fs * tau))),
      _sp_storage(_bandpass.size(), arma::fill::zeros), // Storage for
      // components of
      // single pole low pass
      // filter
      Lrefsq(Lref * Lref), // Reference level
      downsampling_fac(downsampling_fac),

      // Initalize mean square
      Pm(_bandpass.size(), arma::fill::zeros),

      // Initalize max
      Pmax(_bandpass.size(), arma::fill::zeros),

      // Initalize peak
      Ppeak(_bandpass.size(), arma::fill::zeros)

{
  DEBUGTRACE_ENTER;
  DEBUGTRACE_PRINT(_alpha);

  // Make sure thread pool is running
  getPool();

  if (Lref <= 0) {
    throw rte("Invalid reference level");
  }
  if (tau < 0) {
    throw rte("Invalid time constant for Single pole lowpass filter");
  }
  if (fs <= 0) {
    throw rte("Invalid sampling frequency");
  }
}
SLM::~SLM() {}

/**
 * @brief Create set bandpass filters from filter coefficients
 *
 * @param coefs
 *
 * @return
 */
std::vector<unique_ptr<Filter>> createBandPass(const dmat &coefs) {
  DEBUGTRACE_ENTER;
  std::vector<unique_ptr<Filter>> bf;
  for (us colno = 0; colno < coefs.n_cols; colno++) {
    bf.emplace_back(std::make_unique<SeriesBiquad>(coefs.col(colno)));
  }
  return bf;
}
us SLM::suggestedDownSamplingFac(const d fs, const d tau) {
  if (fs <= 0)
    throw rte("Invalid sampling frequency");
  // A reasonable 'framerate' for the sound level meter, based on the
  // filtering time constant.
  if (tau > 0) {
    d fs_slm = 10 / tau;
    if (fs_slm < 30) {
      fs_slm = 30;
    }
    return std::max((us)1, static_cast<us>(fs / fs_slm));
  } else {
    return 1;
  }
}

SLM SLM::fromBiquads(const d fs, const d Lref, const us downsampling_fac,
                     const d tau, const vd &pre_filter_coefs,
                     const dmat &bandpass_coefs) {
  DEBUGTRACE_ENTER;

  return SLM(fs, Lref, downsampling_fac, tau,
             std::make_unique<SeriesBiquad>(pre_filter_coefs),
             createBandPass(bandpass_coefs));
}
SLM SLM::fromBiquads(const d fs, const d Lref, const us downsampling_fac,
                     const d tau, const dmat &bandpass_coefs) {

  DEBUGTRACE_ENTER;

  return SLM(fs, Lref, downsampling_fac, tau,
             nullptr,                       // Pre-filter
             createBandPass(bandpass_coefs) // Bandpass coefficients
  );
}

vd SLM::run_single(vd work, const us i) {

  // Filter input in-place
  _bandpass[i]->filter(work);

  /* cerr << "Filter done" << endl; */

  // Square input --> Signal powers
  /* work.transform([](d j) { return j * j; }); */
  work %= work;

  // Compute peak level, that is before single-pole low pass filter
  Ppeak(i) = std::max(Ppeak(i), arma::max(work));

  // Create copy of N, as we run this in multiple threads.
  us N_local = N;
  DEBUGTRACE_PRINT(N);

  // Obtain storage of single_pole low pass filter
  d cur_storage = _sp_storage(i);

  for (us j = 0; j < work.n_rows; j++) {
    // Update mean square of signal, work is here still signal power
    Pm(i) = (Pm(i) * static_cast<d>(N_local) + work(j)) /
            (static_cast<d>(N_local) + 1);

    N_local++;

    cur_storage = _alpha * cur_storage + (1 - _alpha) * work(j);

    // Now work is single-pole lowpassed signal power
    work(j) = cur_storage;
  }

  // And update storage of low-pass filter
  _sp_storage(i) = cur_storage;

  Pmax(i) = std::max(Pmax(i), arma::max(work));

  // Convert to levels in dB
  work = 10 * arma::log10((work + arma::datum::eps) / Lrefsq);

  return work;
}

dmat SLM::run(const vd &input_orig) {

  DEBUGTRACE_ENTER;
  vd input = input_orig;

  // _pre_filter filters in-place
  if (_pre_filter) {
    _pre_filter->filter(input);
  }

  // Fan out over multiple threads, as it is typically a heavy load
  dmat res(input.n_rows, _bandpass.size());

  // Perform operations in-place.

#pragma omp parallel for
  for (us i = 0; i < _bandpass.size(); i++) {
    res.col(i) = run_single(input, i);
    /* DEBUGTRACE_PRINT(_bandpass.size()); */
    /* DEBUGTRACE_PRINT(res.n_cols); */
    /* DEBUGTRACE_PRINT(res.n_rows); */
    /* DEBUGTRACE_PRINT(futs.size()); */

    // Update the total number of samples harvested so far. NOTE: *This should
    // be done AFTER the threads are done!!!*
  }
  N += input.n_rows;

  // Downsample, if applicable
  if (downsampling_fac > 1) {
    dmat res_ds;
    us rowno = 0;
    while (cur_offset < res.n_rows) {
      res_ds.insert_rows(rowno, res.row(cur_offset));
      rowno++;

      cur_offset += downsampling_fac;
    }
    cur_offset -= res.n_rows;
    // Instead, return a downsampled version
    return res_ds;
  }

  return res;
}
void SLM::reset() {
  Pm.zeros();
  Pmax.zeros();
  Ppeak.zeros();
  for (auto &f : _bandpass) {
    f.reset();
  }
  if (_pre_filter) {
    _pre_filter->reset();
  }
  _sp_storage.zeros();
  cur_offset = 0;

  N = 0;
}