#include 
#include 
#include 

#include "csaps.h"

namespace csaps
{

DoubleArray Diff(const DoubleArray &vec)
{
  const Size n = vec.size() - 1;
  return vec.tail(n) - vec.head(n);
}

IndexArray Digitize(const DoubleArray &arr, const DoubleArray &bins)
{
  // This code works if `arr` and `bins` are monotonically increasing

  IndexArray indexes = IndexArray::Zero(arr.size());

  auto IsInsideBin = [arr, bins](Index item, Index index)
  {
    const double prc = 1.e-8;

    double a = arr(item);
    double bl = bins(index - 1);
    double br = bins(index);

    // bins[i-1]  bl || std::abs(a - bl) < std::abs(std::min(a, bl)) * prc) && a < br;
  };

  Index kstart = 1;

  for (Index i = 0; i < arr.size(); ++i) {
    for (Index k = kstart; k < bins.size(); ++k) {
      if (IsInsideBin(i, k)) {
        indexes(i) = k;
        kstart = k;
        break;
      }
    }
  }

  return indexes;
}

DoubleSparseMatrix MakeSparseDiagMatrix(const DoubleArray2D& diags, const IndexArray& offsets, Size rows, Size cols)
{
  auto GetNumElemsAndIndex = [rows, cols](Index offset, Index &i, Index &j)
  {
    if (offset < 0) {
      i = -offset;
      j = 0;
    }
    else {
      i = 0;
      j = offset;
    }

    return std::min(rows - i, cols - j);
  };

  DoubleSparseMatrix m(rows, cols);

  for (Index k = 0; k < offsets.size(); ++k) {
    Index offset = offsets(k);
    Index i, j;

    Index n = GetNumElemsAndIndex(offset, i, j);

    // When rows == cols or rows > cols, the function takes elements of the 
    // super-diagonal from the lower part of the corresponding diag array, and 
    // elements of the sub-diagonal from the upper part of the corresponding diag array.
    //
    // When rows < cols, the function does the opposite, taking elements of the 
    // super-diagonal from the upper part of the corresponding diag array, and 
    // elements of the sub-diagonal from the lower part of the corresponding diag array.
    DoubleArray diag(n);

    if (offset < 0) {
      if (rows >= cols) {
        diag = diags.row(k).head(n);
      }
      else {
        diag = diags.row(k).tail(n);
      }
    }
    else {
      if (rows >= cols) {
        diag = diags.row(k).tail(n);
      }
      else {
        diag = diags.row(k).head(n);
      }
    }

    for (Index l = 0; l < n; ++l) {
      m.insert(i+l, j+l) = diag(l);
    }
  }

  return m;
}

csaps::DoubleArray SolveLinearSystem(const DoubleSparseMatrix &A, const DoubleArray &b)
{
  Eigen::SparseLU solver;

  // Compute the ordering permutation vector from the structural pattern of A
  solver.analyzePattern(A);

  // Compute the numerical factorization
  solver.factorize(A);
  
  // Use the factors to solve the linear system
  DoubleArray x = solver.solve(b.matrix()).array();

  return x;
}

UnivariateCubicSmoothingSpline::UnivariateCubicSmoothingSpline(const DoubleArray &xdata, const DoubleArray &ydata)
  : UnivariateCubicSmoothingSpline(xdata, ydata, DoubleArray(), -1.0)
{
}

UnivariateCubicSmoothingSpline::UnivariateCubicSmoothingSpline(const DoubleArray &xdata, const DoubleArray &ydata, const DoubleArray &weights)
  : UnivariateCubicSmoothingSpline(xdata, ydata, weights, -1.0)
{
}

UnivariateCubicSmoothingSpline::UnivariateCubicSmoothingSpline(const DoubleArray &xdata, const DoubleArray &ydata, double smooth)
  : UnivariateCubicSmoothingSpline(xdata, ydata, DoubleArray(), smooth)
{
}

UnivariateCubicSmoothingSpline::UnivariateCubicSmoothingSpline(const DoubleArray &xdata, const DoubleArray &ydata, const DoubleArray &weights, double smooth)
  : m_xdata(xdata)
  , m_ydata(ydata)
  , m_weights(weights)
  , m_smooth(smooth)
{
  if (m_xdata.size() < 2) {
    throw std::runtime_error("There must be at least 2 data points");
  }

  if (m_weights.size() == 0) {
    m_weights = DoubleArray::Constant(m_xdata.size(), 1.0);
  }

  if (m_smooth > 1.0) {
    throw std::runtime_error("Smoothing parameter must be less than or equal 1.0");
  }

  if (m_xdata.size() != m_ydata.size() || m_xdata.size() != m_weights.size()) {
    throw std::runtime_error("Lenghts of the input data vectors are not equal");
  }

  MakeSpline();
}

DoubleArray UnivariateCubicSmoothingSpline::operator()(const DoubleArray &xidata)
{
  if (xidata.size() < 2) {
    throw std::runtime_error("There must be at least 2 data points");
  }

  return Evaluate(xidata);
}

DoubleArray UnivariateCubicSmoothingSpline::operator()(const Size pcount, DoubleArray &xidata)
{
  if (pcount < 2) {
    throw std::runtime_error("There must be at least 2 data points");
  }

  xidata.resize(pcount);
  xidata  2) {
    // Create diagonal sparse matrices
    const Size n = dx.size() - 1;

    DoubleArray2D diags(3, n);

    DoubleArray head_r = dx.head(n);
    DoubleArray tail_r = dx.tail(n);

    diags.row(0) = tail_r;
    diags.row(1) = 2 * (tail_r + head_r);
    diags.row(2) = head_r;
    
    IndexArray offsets(3); 
    
    offsets