#include <dune/solvers/common/arithmetic.hh>

template <class Vector, class Matrix, class Function, size_t dim>
BackwardEuler<Vector, Matrix, Function, dim>::BackwardEuler(
    Matrices<Matrix> const &_matrices, Vector const &_u_initial,
    Vector const &_v_initial, Dune::BitSetVector<dim> const &_dirichletNodes,
    Function const &_dirichletFunction)
    : matrices(_matrices),
      u(_u_initial),
      v(_v_initial),
      dirichletNodes(_dirichletNodes),
      dirichletFunction(_dirichletFunction) {}

template <class Vector, class Matrix, class Function, size_t dim>
void BackwardEuler<Vector, Matrix, Function, dim>::nextTimeStep() {
  v_o = v;
  u_o = u;
}

template <class Vector, class Matrix, class Function, size_t dim>
void BackwardEuler<Vector, Matrix, Function, dim>::setup(
    Vector const &ell, double _tau, double relativeTime, Vector &rhs,
    Vector &iterate, Matrix &AM) {
  postProcessCalled = false;
  dirichletFunction.evaluate(relativeTime, dirichletValue);
  tau = _tau;

  /* We start out with the formulation

       M a + C v + A u = ell

     Backward Euler means

       a1 = 1.0/tau ( v1 - v0 )
       u1 = tau v1 + u0

     in summary, we get at time t=1

       M [1.0/tau ( v1 - v0 )] + C v1
       + A [tau v1 + u0] = ell

     or

       1.0/tau M v1 + C v1 + tau A v1
       = [1.0/tau M + C + tau A] v1
       = ell + 1.0/tau M v0 - A u0
  */

  // set up LHS (for fixed tau, we'd only really have to do this once)
  {
    Dune::MatrixIndexSet indices(matrices.elasticity.N(),
                                 matrices.elasticity.M());
    indices.import(matrices.elasticity);
    indices.import(matrices.mass);
    indices.import(matrices.damping);
    indices.exportIdx(AM);
  }
  AM = 0.0;
  Arithmetic::addProduct(AM, 1.0 / tau, matrices.mass);
  Arithmetic::addProduct(AM, 1.0, matrices.damping);
  Arithmetic::addProduct(AM, tau, matrices.elasticity);

  // set up RHS
  {
    rhs = ell;
    Arithmetic::addProduct(rhs, 1.0 / tau, matrices.mass, v_o);
    Arithmetic::subtractProduct(rhs, matrices.elasticity, u_o);
  }

  iterate = v_o;

  for (size_t i = 0; i < dirichletNodes.size(); ++i)
    for (size_t j = 0; j < dim; ++j)
      if (dirichletNodes[i][j])
        iterate[i][j] = (j == 0) ? dirichletValue : 0;
}

template <class Vector, class Matrix, class Function, size_t dim>
void BackwardEuler<Vector, Matrix, Function, dim>::postProcess(
    Vector const &iterate) {
  postProcessCalled = true;

  v = iterate;

  u = u_o;
  Arithmetic::addProduct(u, tau, v);
}

template <class Vector, class Matrix, class Function, size_t dim>
void BackwardEuler<Vector, Matrix, Function, dim>::extractDisplacement(
    Vector &displacement) const {
  if (!postProcessCalled)
    DUNE_THROW(Dune::Exception, "It seems you forgot to call postProcess!");

  displacement = u;
}

template <class Vector, class Matrix, class Function, size_t dim>
void BackwardEuler<Vector, Matrix, Function, dim>::extractVelocity(
    Vector &velocity) const {
  if (!postProcessCalled)
    DUNE_THROW(Dune::Exception, "It seems you forgot to call postProcess!");

  velocity = v;
}

template <class Vector, class Matrix, class Function, size_t dim>
void BackwardEuler<Vector, Matrix, Function, dim>::extractOldVelocity(
    Vector &velocity) const {
  velocity = v_o;
}

template <class Vector, class Matrix, class Function, size_t dim>
std::shared_ptr<TimeSteppingScheme<Vector, Matrix, Function, dim>>
BackwardEuler<Vector, Matrix, Function, dim>::clone() const {
  return std::make_shared<BackwardEuler<Vector, Matrix, Function, dim>>(*this);
}