/* #define DEBUGTRACE_ENABLED */
#include "lasp_biquadbank.h"
#include "debugtrace.hpp"
#include "lasp_thread.h"
#include <cassert>
#include <vector>

using std::cerr;
using std::endl;
using rte = std::runtime_error;
using lock = std::scoped_lock<std::mutex>;

SeriesBiquad::SeriesBiquad(const vd &filter_coefs) {
  DEBUGTRACE_ENTER;
  if (filter_coefs.n_cols != 1) {
    throw rte("Expected filter coefficients for a single SeriesBiquad as a "
              "single column with length 6 x n_filters");
  }

  if (filter_coefs.n_rows % 6 != 0) {
    cerr << "Number of rows given: " << filter_coefs.n_rows << endl;
    throw rte("filter_coefs should be multiple of 6, given: " +
              std::to_string(filter_coefs.n_rows));
  }
  us nfilters = filter_coefs.n_rows / 6;

  /// Initialize state to zero
  state = dmat(2, nfilters, arma::fill::zeros);

  sos.resize(6, nfilters);
  for (us i = 0; i < nfilters; i++) {
    sos.col(i) = filter_coefs.subvec(6 * i, 6 * (i + 1) - 1);
  }

  /// Check if third row in this matrix equals unity.
  if (!arma::approx_equal(sos.row(3), arma::rowvec(nfilters, arma::fill::ones),
                          "absdiff", 1e-9)) {
    std::cerr << "Read row: " << sos.row(3) << endl;

    throw rte(
        "Filter coefficients should have fourth element (a0) equal to 1.0");
  }
}

SeriesBiquad SeriesBiquad::firstOrderHighPass(const d fs, const d cuton_Hz) {

  if (fs <= 0) {
    throw rte("Invalid sampling frequency: " + std::to_string(fs) + " [Hz]");
  }
  if (cuton_Hz <= 0) {
    throw rte("Invalid cuton frequency: " + std::to_string(cuton_Hz) + " [Hz]");
  }
  if (cuton_Hz >= 0.98 * fs / 2) {
    throw rte(
        "Invalid cuton frequency. We limit this to 0.98* fs / 2. Given value" +
        std::to_string(cuton_Hz) + " [Hz]");
  }

  const d tau = 1 / (2 * arma::datum::pi * cuton_Hz);
  const d facnum = 2 * fs * tau / (1 + 2 * fs * tau);
  const d facden = (1 - 2 * fs * tau) / (1 + 2 * fs * tau);

  vd coefs(6);
  // b0
  coefs(0) = facnum;
  // b1
  coefs(1) = -facnum;
  // b2
  coefs(2) = 0;

  // a0
  coefs(3) = 1;

  // a1
  coefs(4) = facden;

  // a2
  coefs(5) = 0;

  return SeriesBiquad(coefs);
}

std::unique_ptr<Filter> SeriesBiquad::clone() const {
  // sos.as_col() concatenates all columns, exactly what we want.
  return std::make_unique<SeriesBiquad>(sos.as_col());
}
void SeriesBiquad::reset() {
  DEBUGTRACE_ENTER;
  state.zeros();
}
void SeriesBiquad::filter(vd &inout) {

  DEBUGTRACE_ENTER;

  /// Implementation is based on Proakis & Manolakis - Digital Signal
  /// Processing, Fourth Edition, p. 550
  for (us filterno = 0; filterno < sos.n_cols; filterno++) {
    d b0 = sos(0, filterno);
    d b1 = sos(1, filterno);
    d b2 = sos(2, filterno);
    d a1 = sos(4, filterno);
    d a2 = sos(5, filterno);

    d w1 = state(0, filterno);
    d w2 = state(1, filterno);

    for (us sample = 0; sample < inout.size(); sample++) {
      d w0 = inout(sample) - a1 * w1 - a2 * w2;
      d yn = b0 * w0 + b1 * w1 + b2 * w2;
      w2 = w1;
      w1 = w0;
      inout(sample) = yn;
    }

    state(0, filterno) = w1;
    state(1, filterno) = w2;
  }
}

BiquadBank::BiquadBank(const dmat &filters, const vd *gains) {
  DEBUGTRACE_ENTER;
  /**
   * @brief Make sure the pool is created once, such that all threads are ready
   * for use.
   */
  lock lck(_mtx);

  for (us i = 0; i < filters.n_cols; i++) {
    _filters.emplace_back(filters.col(i));
  }

  if (gains != nullptr) {
    setGains(*gains);
  } else {
    _gains = vd(_filters.size(), arma::fill::ones);
  }
}
void BiquadBank::setGains(const vd &gains) {
  DEBUGTRACE_ENTER;
  lock lck(_mtx);

  const us nfilters = _filters.size();
  if (gains.size() != nfilters) {
    throw rte("Invalid number of gain values given.");
  }
  _gains = gains;
}

void BiquadBank::filter(vd &inout) {

  lock lck(_mtx);
  std::vector<std::future<vd>> futs;

#if 1
  vd inout_cpy = inout;
  for (us i = 0; i < _filters.size(); i++) {
    futs.emplace_back(_pool.submit(
        [&](vd inout, us i) {
          _filters[i].filter(inout);
          return inout;
        },           // Launch a task to filter.
        inout_cpy, i // Column i as argument to the lambda function above.
        ));
  }

  // Zero-out in-out and sum-up the filtered values
  inout.zeros();
  for (us i = 0; i < _filters.size(); i++) {
    inout += futs[i].get() * _gains[i];
  }
#else
  /// Testing, unthreaded version
  vd inout_cpy = inout;
  inout.zeros();
  for (us i = 0; i < _filters.size(); i++) {
    vd cpy_again = inout_cpy;
    _filters[i].filter(cpy_again);
    inout += cpy_again * _gains(i);
  }
#endif
}
void BiquadBank::reset() {
  DEBUGTRACE_ENTER;
  lock lck(_mtx);
  for (auto &f : _filters) {
    f.reset();
  }
}
std::unique_ptr<Filter> BiquadBank::clone() const {
  lock lck(_mtx);
  return std::make_unique<BiquadBank>(_filters, _gains);
}