#ifndef DUNE_TECTONIC_LOCALFRICTION_HH
#define DUNE_TECTONIC_LOCALFRICTION_HH

#include <cmath>
#include <limits>

#include <dune/common/fvector.hh>
#include <dune/common/fmatrix.hh>

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

#include "frictionpotential.hh"

template <size_t dimension> class LocalFriction {
public:
  virtual ~LocalFriction() {}

  using VectorType = Dune::FieldVector<double, dimension>;
  using MatrixType = Dune::FieldMatrix<double, dimension, dimension>;

  double virtual operator()(VectorType const &x) const = 0;
  void virtual updateAlpha(double alpha) = 0;
  double virtual regularity(VectorType const &x) const = 0;
  double virtual coefficientOfFriction(VectorType const &x) const = 0;
  void virtual directionalSubDiff(VectorType const &x, VectorType const &v,
                                  Dune::Solvers::Interval<double> &D) const = 0;

  void virtual addHessian(VectorType const &x, MatrixType &A) const = 0;

  void virtual addGradient(VectorType const &x, VectorType &y) const = 0;

  void virtual directionalDomain(
      VectorType const &, VectorType const &,
      Dune::Solvers::Interval<double> &dom) const = 0;
};

template <size_t dimension, class ScalarFriction>
class WrappedScalarFriction : public LocalFriction<dimension> {
  using VectorType = typename LocalFriction<dimension>::VectorType;
  using MatrixType = typename LocalFriction<dimension>::MatrixType;

private:
    VectorType removeNormal(const VectorType& x) {
        VectorType res = x;
        res[0] = 0.0;
        return res;
    }

public:
  template <typename... Args>
  WrappedScalarFriction(Args... args)
      : func_(args...) {}

  double operator()(VectorType const &x) const override {
    auto tangential_x = removeNormal(x);
    return func_.evaluate(tangential_x.two_norm());
  }

  void updateAlpha(double alpha) override { func_.updateAlpha(alpha); }

  double regularity(VectorType const &x) const override {
    auto tangential_x = removeNormal(x);
    double xnorm = tangential_x.two_norm();
    if (xnorm < 0.0) {
        return std::numeric_limits<double>::infinity();
    }

    return func_.regularity(xnorm);
  }

  double coefficientOfFriction(VectorType const &x) const override {
    auto tangential_x = removeNormal(x);
    return func_.coefficientOfFriction(tangential_x.two_norm());
  }

  // directional subdifferential: at u on the line u + t*v
  // u and v are assumed to be non-zero
  void directionalSubDiff(VectorType const &x, VectorType const &v,
                          Dune::Solvers::Interval<double> &D) const override {

    auto tangential_x = removeNormal(x);
    double const xnorm = tangential_x.two_norm();
    if (xnorm <= 0.0)
      D[0] = D[1] = func_.differential(0.0) * v.two_norm();
    else
      D[0] = D[1] = func_.differential(xnorm) * (tangential_x * v) / xnorm;
  }

  /** Formula for the derivative:

      \f{align*}{
      \frac {d^2}{dz^2} H(|z|)
      &= \frac d{dz} \left( H'(|z|) \otimes \frac z{|z|} \right)\\
      &= H''(|z|) \frac z{|z|} \otimes \frac z{|z|}
      + H'(|z|) \left( \frac {|z| \operatorname{id} - z \otimes z/|z|}{|z|^2}
     \right)\\
      &= \frac {H''(|z|)}{|z|^2} z \otimes z
      + \frac {H'(|z|)}{|z|} \operatorname{id}
      - \frac {H'(|z|)}{|z|^3} z \otimes z\\
      &= \left( \frac {H''(|z|)}{|z|^2} - \frac {H'(|z|)}{|z|^3} \right) z
     \otimes z
      + \frac {H'(|z|)}{|z|} \operatorname{id}
      \f}
  */
  void addHessian(VectorType const &x, MatrixType &A) const override {
    //std::cout << A << std::endl;
    //std::cout << x << std::endl;

 /*   double const xnorm2 = x.two_norm2();
    double const xnorm = std::sqrt(xnorm2);
    if (xnorm2 <= 0.0)
      return;

    //std::cout << xnorm << " " << xnorm2 << std::endl;

    double const H1 = func_.differential(xnorm);
    double const H2 = func_.second_deriv(xnorm);

    //std::cout << H1 << " " << H2 << std::endl;

    double const tensorweight = (H2 - H1 / xnorm) / xnorm2;
    double const idweight = H1 / xnorm;

    //std::cout << tensorweight << " " << idweight << std::endl;

    for (size_t i = 0; i < dimension; ++i)
      for (size_t j = 0; j < i; ++j) {
        double const entry = tensorweight * x[i] * x[j];
        A[i][j] += entry;
        A[j][i] += entry;
      }

    for (size_t k = 0; k < dimension; ++k) {
      double const entry = tensorweight * x[k] * x[k];
      A[k][k] += entry + idweight;
    }*/
    auto tangential_x = removeNormal(x);
    const double xnorm = tangential_x.two_norm();

    if (std::isinf(func_.regularity(xnorm)) or xnorm<=0.0)
      return;

    VectorType y = tangential_x;
    y /= xnorm;

    double const H1 = func_.differential(xnorm);
    double const H2 = func_.second_deriv(xnorm);

    double const tensorweight = (H2 - H1 / xnorm);
    double const idweight = H1 / xnorm;

    //std::cout << "weights: " << tensorweight << " " << idweight << std::endl;

    for (size_t i = 1; i < dimension; ++i)
      for (size_t j = 1; j < i; ++j) {
        double const entry = tensorweight * y[i] * y[j];
        A[i][j] += entry;
        A[j][i] += entry;
      }

    for (size_t k = 1; k < dimension; ++k) {
      double const entry = tensorweight * y[k] * y[k];
      A[k][k] += entry + idweight;
    }

    //std::cout << A << std::endl;
  }

  void addGradient(VectorType const &x, VectorType &y) const override {
    auto tangential_x = removeNormal(x);
    double const xnorm = tangential_x.two_norm();
    if (std::isinf(func_.regularity(xnorm)))
      return;

    if (xnorm > 0.0)
      Arithmetic::addProduct(y, func_.differential(xnorm) / xnorm, tangential_x);
  }

  void directionalDomain(VectorType const &, VectorType const &,
                         Dune::Solvers::Interval<double> &dom) const override {
    dom[0] = -std::numeric_limits<double>::max();
    dom[1] = std::numeric_limits<double>::max();
  }

private:
  ScalarFriction func_;
};
#endif