// #define DEBUGTRACE_ENABLED
#include "debugtrace.hpp"
#include "lasp_freqsmooth.h"

using rte = std::runtime_error;

vd freqSmooth(const vd& freq, const vd& X, const unsigned w) {
  DEBUGTRACE_ENTER;
  if (freq.size() < 2) {
    throw rte("Invalid frequency size");
  }
  if (freq.size() != X.size()) {
    throw rte("Sizes of freq and X do not match");
  }
  if (freq.size() > std::numeric_limits<long>::max() / 2) {
    throw rte("Frequency size limit for smoothing is 2^30");
  }
  if (w == 0) {
    throw rte("Invalid number of octaves");
  }

  // Smoothing width in unit of number of octaves
  const d Delta = 1 / d(w);

  d freq_min;
  if (freq(0) == 0) {
    freq_min = freq(1);
  } else {
    freq_min = freq(0);
  }
  const d freq_max = freq(freq.size() - 1);
  DEBUGTRACE_PRINT(freq_min);
  DEBUGTRACE_PRINT(freq_max);
  const vd freq_log =
      arma::logspace(d_log10(freq_min), d_log10(freq_max), 10 * freq.size());
  DEBUGTRACE_PRINT("freq_log = ");
//   std::cerr << freq_log << std::endl;

  const long N = freq_log.size();

  // Interpolate X to logscale
  vd X_log;
  DEBUGTRACE_PRINT("X_log = :");
  arma::interp1(freq, X, freq_log, X_log, "*linear");
  // Last point is not interpolated well.
  X_log(N - 1) = X(X.size() - 1);
//   std::cerr << X_log << std::endl;

  vd Xsm_log(freq_log.size());
  const d beta = d_log10(N) / d_log10(2) / (N - 1);
  DEBUGTRACE_PRINT(beta);

  const long mu = int(Delta / 2 / beta);
  DEBUGTRACE_PRINT(mu);
  // Long is at least 32 bits. So +/- 2M points length
  for (long k = 0; k < freq_log.size(); k++) {
    // const d fcur = freq_log(k);
    const us idx_start = std::max(k - mu, 0l);
    const us idx_stop = std::min(k + mu, N - 1);
    // DEBUGTRACE_PRINT(idx_start)
    // DEBUGTRACE_PRINT(idx_stop);

    Xsm_log(k) = arma::mean(X_log.subvec(idx_start, idx_stop));
  }
  DEBUGTRACE_PRINT("Xsm_log:");
//   std::cerr << Xsm_log << std::endl;

  // Back-interpolate to a linear scale
  vd Xsm(freq.size());
  if (freq(0) == 0) {
    Xsm(0) = X(0);
    vd Xsm_gt0;
    arma::interp1(freq_log, Xsm_log, freq.subvec(1, freq.size() - 1), Xsm_gt0,
                  "*linear");
    Xsm.subvec(1, freq.size() - 1) = Xsm_gt0;
    Xsm(1) = Xsm_log(1);

  } else {
    Xsm(0) = X(0);
    arma::interp1(freq_log, Xsm_log, freq, Xsm, "*linear");
  }
  Xsm(freq.size() - 1) = Xsm_log(N - 1);
  DEBUGTRACE_PRINT("Xsm:");
//   std::cerr << Xsm << std::endl;

  return Xsm;
}