#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, Vector const &_a_initial,
    Dune::BitSetVector<dim> const &_dirichletNodes,
    Function const &_dirichletFunction)
    : TimeSteppingScheme<Vector, Matrix, Function, dim>(
          _matrices, _u_initial, _v_initial, _a_initial, _dirichletNodes,
          _dirichletFunction) {}

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) {
  this->dirichletFunction.evaluate(relativeTime, this->dirichletValue);
  this->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(this->matrices.elasticity.N(),
                                 this->matrices.elasticity.M());
    indices.import(this->matrices.elasticity);
    indices.import(this->matrices.mass);
    indices.import(this->matrices.damping);
    indices.exportIdx(AM);
  }
  AM = 0.0;
  Arithmetic::addProduct(AM, 1.0 / this->tau, this->matrices.mass);
  Arithmetic::addProduct(AM, 1.0, this->matrices.damping);
  Arithmetic::addProduct(AM, this->tau, this->matrices.elasticity);

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

  iterate = this->v_o;

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

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

  this->v = iterate;

  this->u = this->u_o;
  Arithmetic::addProduct(this->u, this->tau, this->v);

  this->a = this->v;
  this->a -= this->v_o;
  this->a /= this->tau;
}

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);
}