#include "Numerical.hpp"
#include <algorithm>
#include <cassert>
#include <cmath>
#include <numeric>

namespace
{
constexpr double PI = 3.141592653589793238463;
}  // namespace

std::vector<double> schneide::gauss(
  std::vector<double> const& data, double sigma)
{
  assert(sigma > 0.0);

  // Build a kernel for the gauss filter
  auto kernelExtends = static_cast<int>(std::ceil(sigma * 2.0));
  std::vector<double> kernel(1 + kernelExtends * 2);

  const auto twoSigmaSquared = 2.0 * sigma * sigma;
  auto scale = 1.0 / std::sqrt(PI * twoSigmaSquared);
  for (int i = 0; i <= kernelExtends; ++i)
  {
    double delta = (kernelExtends - i);
    double v = scale * std::exp(-(delta * delta / twoSigmaSquared));
    kernel[i] = v;
    kernel[kernel.size() - 1 - i] = v;
  }

  // Normalize the kernel
  auto sum = std::accumulate(kernel.begin(), kernel.end(), 0.0);
  for (auto& each : kernel) each /= sum;

  return convolve(data, kernel);
}

std::vector<double> schneide::convolve(
  std::vector<double> const& data, std::vector<double> const& kernel)
{
  if (data.empty())
    return data;

  // Apply a convolution with clampeing border handling
  assert(kernel.size() > 0 && kernel.size() & 1);
  int delta = kernel.size() / 2;
  auto N = static_cast<int>(data.size());
  auto M = static_cast<int>(kernel.size());
  auto highest = N - 1;
  std::vector<double> result(data.size());
  for (int i = 0; i < N; ++i)
  {
    double sum = 0.0;

    for (int k = 0; k < M; ++k)
    {
      int sourceIndex = i + k - delta;
      if (sourceIndex < 0)
        sourceIndex = 0;
      else if (sourceIndex > highest)
        sourceIndex = highest;

      sum += data[sourceIndex] * kernel[k];
    }

    result[i] = sum;
  }
  return result;
}

double schneide::interpolate(std::vector<double> const& data, double x)
{
  if (data.empty())
    throw std::invalid_argument("Empty value list for interpolation");

  if (x < 0.0)
    return data.front();

  double begin = std::floor(x);
  std::size_t i = static_cast<std::size_t>(begin);
  std::size_t j = static_cast<std::size_t>(std::ceil(x));
  if (j >= data.size())
    return data.back();

  if (i == j)
    return data[i];

  double lambda = x - begin;
  return data[i] + lambda * (data[j] - data[i]);
}

double schneide::findRelative(
  std::vector<double> const& ascendingValues, double which)
{
  if (ascendingValues.empty())
  {
    throw std::invalid_argument("Given empty array to find value in");
  }
  if (which <= ascendingValues.front())
    return 0.0;
  if (which >= ascendingValues.back())
    return static_cast<double>(ascendingValues.size() - 1);

  auto right =
    std::lower_bound(ascendingValues.begin(), ascendingValues.end(), which);
  auto left = right - 1;

  auto begin = *left;
  auto end = *right;

  auto relative = (which - begin) / (end - begin);

  return relative + (left - ascendingValues.begin());
}
