/**
 * @file ConsensusCluster.hpp
 * @brief Implementation of consensus clustering using Armadillo.
 * @author Sriram P. Chockalingam <srirampc@gatech.edu>
 * @version 1.0
 * @date 2021-01-05
 *
 * Copyright 2021 Georgia Institute of Technology
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef DETAIL_CONSENSUSCLUSTER_HPP_
#define DETAIL_CONSENSUSCLUSTER_HPP_

#include "utils/Logging.hpp"

#include <armadillo>

template <typename MatType>
MatType
load_csv_mat(
  const char *file_name
)
{
  // load matrix from csv file
  MatType X;
  X.load(file_name, arma::csv_ascii);
  return X;
}

template <bool Norm, typename RandomIt, typename MatType>
double
clusterScore(const MatType&, const RandomIt, const RandomIt);

template <bool Norm, typename RandomIt>
double
clusterScore(
  const arma::mat& A,
  const RandomIt begin,
  const RandomIt end
)
{
  if (begin == end) {
    return 0.0;
  }
  arma::colvec v = arma::zeros<arma::colvec>(A.n_rows);
  for (auto it = begin; it != end; ++it) {
    v(*it) = 1.0;
  }
  auto w = A * v;
  auto score = arma::dot(v, w);
  auto denom = static_cast<double>(Norm ? pow(arma::norm(v), 2) :
                                          std::distance(begin, end));
  return score / denom;
}

template <bool Norm, typename RandomIt>
double
clusterScore(
  const arma::sp_mat& A,
  const RandomIt begin,
  const RandomIt end
)
{
  if (begin == end) {
    return 0.0;
  }
  arma::sp_mat v(A.n_rows, 1);
  for (auto it = begin; it != end; ++it) {
    v(*it, 0) = 1.0;
  }
  auto w = A * v;
  auto score = arma::dot(v, w);
  auto denom = static_cast<double>(Norm ? pow(arma::norm(v), 2) :
                                          std::distance(begin, end));
  return score / denom;
}

// Identify the cluster with best score by finding the top k
// vertices with the highest values of eigen vector that can maximize the
// cluster score
template <typename Var, typename MatType>
std::pair<std::vector<Var>, double>
bestCluster(
  const MatType& A,
  const arma::sp_mat&& v,
  const double tolerance
)
{
  LOG_MESSAGE_IF(v.n_cols != 1, error, "Perron vector should have one column");
  LOG_MESSAGE_IF(v.n_rows != A.n_rows, error, "Mismatch between the given row vector and matrix rows");
  LOG_MESSAGE_IF(v.n_rows != A.n_cols, error, "Mismatch between the given row vector and matrix columns");
  // Store non-zero values in sorted descending order
  std::set<std::pair<double, Var>, std::greater<std::pair<double, Var>>> vals;
  for (auto vit = v.begin_col(0); vit != v.end_col(0); ++vit) {
    // We only want to retain the elements which are greater than tolerance
    if (std::isgreater(*vit, tolerance)) {
      vals.insert(std::make_pair(*vit, vit.row()));
    }
  }
  // Now, copy all the eligible indices in the sorted order
  std::vector<Var> sortedIdx(vals.size());
  std::transform(vals.begin(), vals.end(), sortedIdx.begin(),
                 [] (const std::pair<Var, double>& vl)
                    { return vl.second; });

  auto maxScore = 0.0;
  auto numElements = 0u;
  // Find the top k vertices in the sorted order that maximzes the
  // cluster score
  const auto first = sortedIdx.cbegin();
  auto last = first + 1;
  for (auto k = 0u; k < sortedIdx.size(); ++k, ++last) {
    auto thisScore = clusterScore<true>(A, first, last);
    if (std::isgreaterequal(thisScore, maxScore)) {
      maxScore = thisScore;
      numElements = k + 1;
    }
  }
  LOG_MESSAGE_IF(numElements > 0, info, "Score: %g; Cluster Size: %u",
                                        clusterScore<false>(A, first, std::next(first, numElements)), numElements);
  sortedIdx.resize(numElements);
  return std::make_pair(sortedIdx, maxScore);
}

// Compute dominant eigen vector using the power method
template <typename MatType>
arma::sp_mat
perronVector(
  const MatType& A,
  const double tolerance,
  const uint32_t maxSteps
)
{
  // Initial vector : unit vector with 1.0 at vertex w. the maximum weight
  MatType rx = arma::sum(A, 1);
  arma::sp_mat v(rx.n_rows, 1);
  auto i = rx.index_max();
  v(i, 0) = 1.0;

  auto mu = 1.0;
  auto diff = 1.0;
  auto step = maxSteps;
  for (step = 0u; (step < maxSteps) && (diff > tolerance); ++step) {
    // Matrix - vector multiplication
    v = A * v;
    auto muNext = arma::norm(v);
    // normalize
    v = arma::normalise(v); // , p = 2);
    // Convergence parameter
    diff = std::abs(1.0 - muNext / mu);
    // Update mu
    mu = muNext;
  }
  LOG_MESSAGE(info, "Number of Steps / Max Steps: %u / %u", step, maxSteps);
  LOG_MESSAGE_IF(step == maxSteps, info, "Maximum number of steps reached with error = %g", diff);
  return v;
}

template <typename Var, typename MatType>
MatType
getSubmatrix(const MatType&, const std::vector<Var>&);

template <typename Var>
arma::mat
getSubmatrix(
  const arma::mat& Ain,
  const std::vector<Var>& indices
)
{
  // TODO : find a better way ?
  arma::mat Aout(indices.size(), indices.size());
  for (auto ix = 0u; ix < Aout.n_rows; ++ix) {
    for (auto jx = 0u; jx < Aout.n_cols; ++jx) {
      Aout(ix, jx) = Ain(indices[ix], indices[jx]);
    }
  }
  return Aout;
}

template <typename Var>
arma::sp_mat
getSubmatrix(
  const arma::sp_mat& Ain,
  const std::vector<Var>& indices
)
{
  // TODO : find a better way ?
  const auto nvertices = Ain.n_rows;
  arma::uvec map_idx(nvertices);
  map_idx.fill(nvertices);
  for (uint32_t ix = 0; ix < indices.size(); ++ix) {
    map_idx[indices[ix]] = ix;
  }
  uint32_t submat_size = 0;
  for (auto it = Ain.begin(); it != Ain.end(); ++it) {
    if (map_idx[it.row()] < nvertices && map_idx[it.col()] < nvertices) {
      ++submat_size;
    }
  }
  arma::umat locations(2, submat_size, arma::fill::zeros);
  arma::vec values(submat_size, arma::fill::zeros);
  uint32_t idx = 0;
  for (auto it = Ain.begin(); it != Ain.end(); ++it) {
    if ((map_idx[it.row()] < nvertices) && (map_idx[it.col()] < nvertices)) {
      locations(0, idx) = map_idx[it.row()];
      locations(1, idx) = map_idx[it.col()];
      values(idx) = *it; ++idx;
    }
  }
  return arma::sp_mat(locations, values, indices.size(), indices.size());
}

// Lemon tree cluster tightening algorithm
template <typename Var, typename MatType>
std::multimap<Var, Var>
perronCluster(
  MatType&& A,
  const double tolerance,
  const uint32_t maxSteps,
  const uint32_t minClustSize,
  const double minClustScore
)
{
  // set diagonal to ones as done by Lemon Tree
  A.diag().ones();
  // (to keep track of the original vertex id when we construct sub matrix)
  std::vector<Var> vertexMapping(A.n_rows);
  for (Var i = 0; i < A.n_rows; ++i) {
    vertexMapping[i] = i;
  }
  std::multimap<Var, Var> vertexClusters;
  Var clusterId = 0;
  auto numRemaining = A.n_rows;
  while (numRemaining >= minClustSize) {
    // Compute PF vector and best cluster
    auto v = perronVector(A, tolerance,  maxSteps);
    auto best = bestCluster<Var>(A, std::move(v), tolerance);
    auto& clusterElements = best.first;
    numRemaining = A.n_rows - clusterElements.size();
    // Identify remaining rows/cols
    std::vector<Var> currElements(A.n_rows);
    for (Var i = 0; i < A.n_rows; ++i) {
      currElements[i] = i;
    }
    std::sort(clusterElements.begin(), clusterElements.end());
    std::vector<Var> remainingElements(numRemaining);
    // XXX: We can possibly do better here
    std::set_difference(currElements.begin(), currElements.end(),
                        clusterElements.begin(), clusterElements.end(),
                        remainingElements.begin());
    A = getSubmatrix(A, remainingElements);
    if ((clusterElements.size() >= minClustSize) &&
        (best.second >= minClustScore)) {
      // Update the cluster ids of the clusters
      for (const auto ce : clusterElements) {
        auto vid = vertexMapping[ce];
        vertexClusters.emplace(clusterId, vid);
      }
      ++clusterId;
    }
    // Resize matrix and update mapping
    vertexMapping.resize(numRemaining);
    for (auto i = 0u; i < remainingElements.size(); ++i) {
      auto vid = vertexMapping[remainingElements[i]];
      vertexMapping[i] = vid;
    }
  }

  // XXX: Assign unique ids to the rest of the vertices ?
  return vertexClusters;
}

template <typename MatType, typename Set, typename Var>
void
fillCoclusteringWeights(
  MatType& C,
  const std::list<std::list<Set>>&& sampledClusters,
  const Var n,
  const double minWeight
)
{
  std::list<std::unordered_map<Var, uint32_t>> varClusterMaps;
  for (const auto& varClusters : sampledClusters) {
    std::unordered_map<Var, uint32_t> thisMap;
    auto c = 0u;
    for (auto cit = varClusters.begin(); cit != varClusters.end(); ++cit, ++c) {
      for (const auto var : *cit) {
        thisMap[var] = c;
      }
    }
    varClusterMaps.push_back(thisMap);
  }
  for (auto u = 0u; u < n; ++u) {
    for (auto v = u + 1; v < n; ++v) {
      auto cooccurrence = 0u;
      for (const auto& varCluster : varClusterMaps) {
        if (varCluster.at(u) == varCluster.at(v)) {
          ++cooccurrence;
        }
      }
      auto weight = static_cast<double>(cooccurrence) / sampledClusters.size();
      if (std::isgreater(weight, minWeight)) {
        C(u, v) = weight;
        C(v, u) = weight;
      }
    }
  }
}

template <typename Var, typename Set>
std::multimap<Var, Var>
consensusCluster(
  const std::list<std::list<Set>>&& sampledClusters,
  const Var numVars,
  const double minWeight,
  const double tolerance,
  const uint32_t maxSteps,
  const uint32_t minClustSize,
  const double minClustScore,
  const bool sparse = true
)
{
  if (sparse) {
    arma::sp_mat C(numVars, numVars);
    fillCoclusteringWeights(C, std::move(sampledClusters), numVars, minWeight);
    return perronCluster<Var>(std::move(C), tolerance, maxSteps, minClustSize, minClustScore);
  }
  else {
    arma::mat C(numVars, numVars, arma::fill::zeros);
    fillCoclusteringWeights(C, std::move(sampledClusters), numVars, minWeight);
    return perronCluster<Var>(std::move(C), tolerance, maxSteps, minClustSize, minClustScore);
  }
}

#endif // DETAIL_CONSENSUSCLUSTER_HPP_


//
// Copyright (c) 2019-2020 CNRS INRIA
//

#include <boost/python.hpp>

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/utils/namespace.hpp"
#include "pinocchio/math/rpy.hpp"

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobian_overload, rpy::computeRpyJacobian, 1, 2)
    BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianInverse_overload, rpy::computeRpyJacobianInverse, 1, 2)
    BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianTimeDerivative_overload, rpy::computeRpyJacobianTimeDerivative, 2, 3)

    Eigen::Matrix3d rotate(const std::string & axis, const double ang)
    {
      if(axis.length() != 1U)
          throw std::invalid_argument(std::string("Invalid axis: ").append(axis));
      Eigen::Vector3d u;
      u.setZero();
      const char axis_ = axis[0];
      switch(axis_)
      {
        case 'x': u[0] = 1.; break;
        case 'y': u[1] = 1.; break;
        case 'z': u[2] = 1.; break;
        default: throw std::invalid_argument(std::string("Invalid axis: ").append(1U,axis_));
      }

      return Eigen::AngleAxisd(ang, u).matrix();
    }

    void exposeRpy()
    {
      using namespace Eigen;
      using namespace pinocchio::rpy;

      {
        // using the rpy scope
        bp::scope current_scope = getOrCreatePythonNamespace("rpy");

        bp::def("rpyToMatrix",
                static_cast<Matrix3d (*)(const double&, const double&, const double&)>(&rpyToMatrix),
                bp::args("roll", "pitch", "yaw"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("rpyToMatrix",
                static_cast<Matrix3d (*)(const MatrixBase<Vector3d>&)>(&rpyToMatrix),
                bp::arg("rpy"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("matrixToRpy",
                &matrixToRpy<Matrix3d>,
                bp::arg("R"),
                "Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a."
                " The angles are guaranteed to be in the ranges: r in [-pi,pi],"
                " p in[-pi/2,pi/2], y in [-pi,pi]");

        bp::def("rotate",
                &rotate,
                bp::args("axis", "ang"),
                "Rotation matrix corresponding to a rotation about x, y or z"
                " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis");

        bp::def("computeRpyJacobian",
                &computeRpyJacobian<Vector3d>,
                computeRpyJacobian_overload(
                    bp::args("rpy","reference_frame"),
                    "Compute the Jacobian of the Roll-Pitch-Yaw conversion"
                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
                    " and reference frame F (either LOCAL or WORLD),"
                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
                    " where omega_F is the angular velocity expressed in frame F"
                    " and J_F is the Jacobian computed with reference frame F"
                    "\nParameters:\n"
                    "\trpy Roll-Pitch-Yaw vector"
                    "\treference_frame  Reference frame in which the angular velocity is expressed."
                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                )
        );

        bp::def("computeRpyJacobianInverse",
                &computeRpyJacobianInverse<Vector3d>,
                computeRpyJacobianInverse_overload(
                    bp::args("rpy","reference_frame"),
                    "Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion"
                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
                    " and reference frame F (either LOCAL or WORLD),"
                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
                    " where omega_F is the angular velocity expressed in frame F"
                    " and J_F is the Jacobian computed with reference frame F"
                    "\nParameters:\n"
                    "\trpy Roll-Pitch-Yaw vector"
                    "\treference_frame  Reference frame in which the angular velocity is expressed."
                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                )
        );

        bp::def("computeRpyJacobianTimeDerivative",
                &computeRpyJacobianTimeDerivative<Vector3d, Vector3d>,
                computeRpyJacobianTimeDerivative_overload(
                    bp::args("rpy", "rpydot", "reference_frame"),
                    "Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion"
                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
                    " and reference frame F (either LOCAL or WORLD),"
                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
                    " where omega_F is the angular velocity expressed in frame F"
                    " and J_F is the Jacobian computed with reference frame F"
                    "\nParameters:\n"
                    "\trpy Roll-Pitch-Yaw vector"
                    "\treference_frame  Reference frame in which the angular velocity is expressed."
                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                )
        );
      }

    }

  } // namespace python
} // namespace pinocchio


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include "ActivationFunctions.h"
#include <boost/algorithm/string.hpp>

// -------------------------------------------------------------------------
template< class _TScl >
const std::string& ActivationFunctions::Function< _TScl >::
name( ) const
{
  return( this->m_N );
}

// -------------------------------------------------------------------------
template< class _TScl >
const typename ActivationFunctions::Function< _TScl >::TScalar&
ActivationFunctions::Function< _TScl >::
threshold( ) const
{
  return( this->m_T );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Function< _TScl >::
TMatrix ActivationFunctions::Function< _TScl >::
t( const TMatrix& z ) const
{
  return(
    TMatrix( ( z.array( ) >= this->m_T ).template cast< TScalar >( ) )
    );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ArcTan< _TScl >::
TMatrix ActivationFunctions::ArcTan< _TScl >::
f( const TMatrix& z ) const
{
  return( Eigen::atan( z.array( ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ArcTan< _TScl >::
TMatrix ActivationFunctions::ArcTan< _TScl >::
d( const TMatrix& z ) const
{
  return( TScalar( 1 ) / ( Eigen::pow( z.array( ), 2 ) + TScalar( 1 ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::BinaryStep< _TScl >::
TMatrix ActivationFunctions::BinaryStep< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r = TMatrix::Ones( z.rows( ), z.cols( ) );
  r.array( ) *= ( z.array( ) >= 0 ).template cast< TScalar >( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::BinaryStep< _TScl >::
TMatrix ActivationFunctions::BinaryStep< _TScl >::
d( const TMatrix& z ) const
{
  return( TMatrix::Zero( z.rows( ), z.cols( ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ELU< _TScl >::
TMatrix ActivationFunctions::ELU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) =
    ( c * z.array( ) ) +
    (
      ( ( TScalar( 1 ) - c ) * this->m_Alpha ) *
      ( Eigen::exp( z.array( ) ) - TScalar( 1 ) )
      );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ELU< _TScl >::
TMatrix ActivationFunctions::ELU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix e = this->f( z );
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) < 0 ).template cast< TScalar >( );
  r.array( ) = ( c * ( e.array( ) + this->m_Alpha ) ) + ( TScalar( 1 ) - c );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Identity< _TScl >::
TMatrix ActivationFunctions::Identity< _TScl >::
f( const TMatrix& z ) const
{
  return( z );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Identity< _TScl >::
TMatrix ActivationFunctions::Identity< _TScl >::
d( const TMatrix& z ) const
{
  return( TMatrix::Ones( z.rows( ), z.cols( ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::LeakyReLU< _TScl >::
TMatrix ActivationFunctions::LeakyReLU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) =
    ( c * z.array( ) ) +
    ( ( TScalar( 1 ) - c ) * TScalar( 1e-2 ) ) * z.array( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::LeakyReLU< _TScl >::
TMatrix ActivationFunctions::LeakyReLU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) = c + ( ( TScalar( 1 ) - c ) * TScalar( 1e-2 ) );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Logistic< _TScl >::
TMatrix ActivationFunctions::Logistic< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  r.array( ) = TScalar( 1 ) / ( TScalar( 1 ) + Eigen::exp( -z.array( ) ) );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Logistic< _TScl >::
TMatrix ActivationFunctions::Logistic< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix e = this->f( z );
  TMatrix r( z.rows( ), z.cols( ) );
  r.array( ) = e.array( ) * ( TScalar( 1 ) - e.array( ) );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::OutTanh< _TScl >::
TMatrix ActivationFunctions::OutTanh< _TScl >::
f( const TMatrix& z ) const
{
  return( ( z.array( ).tanh( ) + TScalar( 1 ) ) / TScalar( 2 ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::OutTanh< _TScl >::
TMatrix ActivationFunctions::OutTanh< _TScl >::
d( const TMatrix& z ) const
{
  return( ( TScalar( 1 ) - z.array( ).tanh( ).square( ) ) / TScalar( 2 ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::RandomizedReLU< _TScl >::
TMatrix ActivationFunctions::RandomizedReLU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) =
    ( c * z.array( ) ) +
    ( ( TScalar( 1 ) - c ) * this->m_Alpha ) * z.array( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::RandomizedReLU< _TScl >::
TMatrix ActivationFunctions::RandomizedReLU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix r( z.rows( ), z.cols( ) );
  auto c = ( z.array( ) >= 0 ).template cast< TScalar >( );
  r.array( ) = c + ( ( TScalar( 1 ) - c ) * this->m_Alpha );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ReLU< _TScl >::
TMatrix ActivationFunctions::ReLU< _TScl >::
f( const TMatrix& z ) const
{
  TMatrix r = z;
  r.array( ) *= ( z.array( ) >= 0 ).template cast< TScalar >( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::ReLU< _TScl >::
TMatrix ActivationFunctions::ReLU< _TScl >::
d( const TMatrix& z ) const
{
  TMatrix r = TMatrix::Ones( z.rows( ), z.cols( ) );
  r.array( ) *= ( z.array( ) >= 0 ).template cast< TScalar >( );
  return( r );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::SoftPlus< _TScl >::
TMatrix ActivationFunctions::SoftPlus< _TScl >::
f( const TMatrix& z ) const
{
  return( Eigen::log( Eigen::exp( z.array( ) ) + TScalar( 1 ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::SoftPlus< _TScl >::
TMatrix ActivationFunctions::SoftPlus< _TScl >::
d( const TMatrix& z ) const
{
  return( TScalar( 1 ) / ( TScalar( 1 ) + Eigen::exp( -z.array( ) ) ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Tanh< _TScl >::
TMatrix ActivationFunctions::Tanh< _TScl >::
f( const TMatrix& z ) const
{
  return( z.array( ).tanh( ) );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Tanh< _TScl >::
TMatrix ActivationFunctions::Tanh< _TScl >::
d( const TMatrix& z ) const
{
  return( TScalar( 1 ) - this->f( z ).array( ).square( ) );
}

// Instances
#define _PUJ_ML_ActivationFunction_Instances( _n_, _t_ )        \
  template class ActivationFunctions::_n_< _t_ >

#define PUJ_ML_ActivationFunction_Instances( _n_ )             \
  _PUJ_ML_ActivationFunction_Instances( _n_, float );          \
  _PUJ_ML_ActivationFunction_Instances( _n_, double );         \
  _PUJ_ML_ActivationFunction_Instances( _n_, long double )

PUJ_ML_ActivationFunction_Instances( Function );
PUJ_ML_ActivationFunction_Instances( ArcTan );
PUJ_ML_ActivationFunction_Instances( BinaryStep );
PUJ_ML_ActivationFunction_Instances( ELU );
PUJ_ML_ActivationFunction_Instances( Identity );
PUJ_ML_ActivationFunction_Instances( LeakyReLU );
PUJ_ML_ActivationFunction_Instances( Logistic );
PUJ_ML_ActivationFunction_Instances( OutTanh );
PUJ_ML_ActivationFunction_Instances( RandomizedReLU );
PUJ_ML_ActivationFunction_Instances( ReLU );
PUJ_ML_ActivationFunction_Instances( SoftPlus );
PUJ_ML_ActivationFunction_Instances( Tanh );

// -------------------------------------------------------------------------
template< class _TScl >
ActivationFunctions::Factory< _TScl >::
Factory( )
{
  this->reg_cre( "arctan", &ActivationFunctions::ArcTan< _TScl >::create );
  this->reg_cre( "binarystep", &ActivationFunctions::BinaryStep< _TScl >::create );
  this->reg_cre( "elu", &ActivationFunctions::ELU< _TScl >::create );
  this->reg_cre( "identity", &ActivationFunctions::Identity< _TScl >::create );
  this->reg_cre( "leakyrelu", &ActivationFunctions::LeakyReLU< _TScl >::create );
  this->reg_cre( "logistic", &ActivationFunctions::Logistic< _TScl >::create );
  this->reg_cre( "outtanh", &ActivationFunctions::OutTanh< _TScl >::create );
  this->reg_cre( "randomizedrelu", &ActivationFunctions::RandomizedReLU< _TScl >::create );
  this->reg_cre( "relu", &ActivationFunctions::ReLU< _TScl >::create );
  this->reg_cre( "softplus", &ActivationFunctions::SoftPlus< _TScl >::create );
  this->reg_cre( "tanh", &ActivationFunctions::Tanh< _TScl >::create );
}

// -------------------------------------------------------------------------
template< class _TScl >
ActivationFunctions::Factory< _TScl >::
Factory( const Self& other )
{
}

// -------------------------------------------------------------------------
template< class _TScl >
ActivationFunctions::Factory< _TScl >::
~Factory( )
{
  this->m_FactoryMap.clear( );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Factory< _TScl >::
Self& ActivationFunctions::Factory< _TScl >::
operator=( const Self& other )
{
  return( *this );
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Factory< _TScl >::
Self* ActivationFunctions::Factory< _TScl >::
get( )
{
  static Self instance;
  return( &instance );
}

// -------------------------------------------------------------------------
template< class _TScl >
void ActivationFunctions::Factory< _TScl >::
reg_cre( const std::string& n, TCreator c )
{
  this->m_FactoryMap[ n ] = c;
}

// -------------------------------------------------------------------------
template< class _TScl >
typename ActivationFunctions::Factory< _TScl >::
TFunction* ActivationFunctions::Factory< _TScl >::
create( const std::string& n ) const
{
  typename TMap::const_iterator i =
    this->m_FactoryMap.find( boost::algorithm::to_lower_copy( n ) );
  if( i != this->m_FactoryMap.end( ) )
    return( i->second( ) );
  else
    return( nullptr );
}

// -------------------------------------------------------------------------
PUJ_ML_ActivationFunction_Instances( Factory );

// eof - $RCSfile$


/*!
 * @file
 * calculating value of pi by MC trials
 *
 * command to run:
 * mpirung -n 2 ./bin/pi 100000
 *
 * The command line argument is the total number of trials.
 *
 * benchmarks at the bottom
 * */
#include <random>
#include <stdexcept>

#include <boost/mpi.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/filters.hpp>
#include <ezl/algorithms/io.hpp>
#include <ezl/algorithms/reduces.hpp>

// to facilitate random number generation
template <class T>
struct RandReal {
public:
  RandReal(T min, T max) : dis{min, max} {
    std::random_device rd;
    gen.seed(rd());
  }
  double operator () () {
    return dis(gen);
  }
private:
  std::mt19937 gen;
  std::uniform_real_distribution<T> dis;
};

void valueOfPi(int argc, char* argv[]) {
  if (argc < 2) {
    std::cout<<"Please provide number of MC trials as argument\n";
    return;
  }
  auto trials = std::stoll(argv[1]);

  RandReal<double> rand01{0.0,1.0};

  ezl::rise(ezl::kick(trials).split())
    .map([&rand01] {
      auto x = rand01();
      auto y = rand01();
      return x*x + y*y;
    })
    .filter(ezl::lt(1.))
    .reduce(ezl::count(), 0LL).inprocess()
    .reduce(ezl::sum(), 0LL)
    .map([trials](long long res) {
      return (4.0 * res / trials);
    }).colsTransform().dump("", "pi in " + std::to_string(trials) + " trials:")
    .run();
}

int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    valueOfPi(argc, argv);
  } catch (const std::exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}

/*!
 * The benchmarks are using rand() function for random.
 * benchmark results: i7(hdd); input: 4 x 10^9; units: secs
 *  *nprocs* | 1   | 2   | 4    |
 *  ---      |---  |---  |---   |
 *  *time(s)*| 111 | 56  | 39   |
 *
 * benchmark results: Linux(nfs-3); input: variable; units: secs
 *  *nprocs* | 1x12      | 2x12      | 4x12      | 8x12      |  16x12   |
 *  ---      |---        |---        |---        | ---       |          |
 *  *trials* | 1/8x10^11 | 1/4x10^11 | 1/2x10^11 | 1x10^11   |  2x10^11 |
 *  *time(s)*| 48        | 55        | 58        | 57.5      |  59      |
 */


#include <vtkImageData.h>
#include <vtkDICOMImageReader.h>
#include <vtkImageReader.h>
#include <vtkImageGaussianSmooth.h>
#include <vtkDemandDrivenPipeline.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>

#include <CGAL/Gray_image_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include <CGAL/Image_3.h>
#include <CGAL/read_vtk_image_data.h>

#include <boost/lexical_cast.hpp>

typedef short Image_word_type;

// Domain
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Gray_image_mesh_domain_3<CGAL::Image_3,K,
                                       Image_word_type,
                                       std::binder1st< std::less<Image_word_type> > > Mesh_domain;

// Triangulation
typedef CGAL::Mesh_triangulation_3<Mesh_domain>::type Tr;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

// Criteria
typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;

// To avoid verbose function and named parameters call
using namespace CGAL::parameters;

int main(int argc, char* argv[])
{
  // Loads image
  if(argc == 1){
    std::cerr << "Usage:  " << argv[0] << " <directory with dicom data> iso_level=1  facet_size=1  facet_distance=0.1  cell_size=1\n";
    return 0;
  }

  Image_word_type iso = (argc>2)? boost::lexical_cast<Image_word_type>(argv[2]): 1;
  double fs = (argc>3)? boost::lexical_cast<double>(argv[3]): 1;
  double fd = (argc>4)? boost::lexical_cast<double>(argv[4]): 0.1;
  double cs = (argc>5)? boost::lexical_cast<double>(argv[5]): 1;

  vtkDICOMImageReader*dicom_reader = vtkDICOMImageReader::New();
  dicom_reader->SetDirectoryName(argv[1]);

  vtkDemandDrivenPipeline*executive =
    vtkDemandDrivenPipeline::SafeDownCast(dicom_reader->GetExecutive());
  if (executive)
    {
      executive->SetReleaseDataFlag(0, 0); // where 0 is the port index
    }

  vtkImageGaussianSmooth* smoother = vtkImageGaussianSmooth::New();
  smoother->SetStandardDeviations(1., 1., 1.);
  smoother->SetInputConnection(dicom_reader->GetOutputPort());
  smoother->Update();
  vtkImageData* vtk_image = smoother->GetOutput();
  vtk_image->Print(std::cerr);

  CGAL::Image_3 image = CGAL::read_vtk_image_data(vtk_image);
  if(image.image() == 0){
    std::cerr << "could not create a CGAL::Image_3 from the vtk image\n";
    return 0;
  }
  // Domain
  Mesh_domain domain(image, std::bind1st(std::less<Image_word_type>(), iso), 0);

  // Mesh criteria
  Mesh_criteria criteria(facet_angle=30, facet_size=fs, facet_distance=fd,
                         cell_radius_edge_ratio=3, cell_size=cs);

  // Meshing
  C3t3 c3t3 = CGAL::make_mesh_3<C3t3>(domain, criteria);

  // Output
  std::ofstream medit_file("out.mesh");
  c3t3.output_to_medit(medit_file);

  return 0;
}


#ifndef _DUAL_FCC_ISOSURFACE_H_
#define _DUAL_FCC_ISOSURFACE_H_

#include <sisl/sisl.hpp>
#include <sisl/sparse_array.hpp>
#include <sisl/utility/ply_writer.hpp>

#include <Eigen/Dense>

#include <tuple>
#include <unordered_map>
#include <map>

namespace sisl{
namespace utility{
using namespace std;

template<class T>
class dualfcc_isosurface{
public:
	struct cell_vertex
	{
		cell_vertex(){}
		std::vector<vector3<T> > touching;
		vector3<T> vertex;
		int vertexId;
	};

	dualfcc_isosurface() : face_hash_table(1000,1000,1000, {}){
		this->faceList.clear();
	}

	template<class L, class I, class O>
	void contour(
			L *f,
			const O &isoValue,
			const I &scalingParameter,
			sisl::vector3<I> origin,
			sisl::vector3<I> boundary ){

		I dh = scalingParameter;
		int res = int(1./(dh));

		// Go over every lattice point
		#pragma omp parallel for
		for(unsigned int i = 2; i < res-4; i+=2){
			/*
			 * We keep this local to each worker, so we only have to delve into
			 * a critical section at the end of each loop
			 */
			std::vector<std::vector<vector3<int>>> localFaceList;

			for(unsigned int j = 2; j < res - 2; j++)
				for(unsigned int k = 2; k < res - 2; k++) {
					int ii = i + ((j&1) ^ (k&1));
					int jj = j;
					int kk = k;

					O value = f->f(dh*ii, dh*jj, dh*kk) - isoValue;

					// For each face in the minimal amount of faces of
					// the polyhedron
					for(auto idx : minimal_face_set) {
						auto polyhedron_vertex = polyhedron_vertices[idx];
						int x = polyhedron_vertex.i + ii,
							y = polyhedron_vertex.j + jj,
							z = polyhedron_vertex.k + kk;
						O next_value = f->f(dh*x, dh*y, dh*z) - isoValue;
						I zero_solution = 0.5;

						vector3<T> pv, n;

						// No sign change?
						if((next_value > 0 && value > 0) || (next_value <0 && value < 0))
							continue; // Whatever

						// Find the sign change.
						zero_solution = ((value - 0)/(value - next_value));
						pv = n = vector3<T>(x,y,z) - vector3<T>(ii,jj,kk);
						pv = vector3<T>(ii,jj,kk) + pv * zero_solution;
						n = n * (value > next_value ? -1 : 1);

						// Lookup all the dual points that touch this vertex
						std::vector<int> adj = adj_index[idx - 1];
						std::vector<std::vector<int>> luf = triangle_lookup[idx - 1];

						// Push all the faces into our local face list.
						for(auto triangle : luf) {
							vector3<int>
									hash1 = center_hash_offsets[triangle[0]] + vector3<int>(ii*2, jj*2, kk*2),
									hash2 = center_hash_offsets[triangle[1]] + vector3<int>(ii*2, jj*2, kk*2),
									hash3 = center_hash_offsets[triangle[2]] + vector3<int>(ii*2, jj*2, kk*2);

							vector3<int> t = (hash2 - hash1)%(hash3 - hash1);
							vector3<T> dir(t.i, t.j, t.k);
							if(dir * n > 0) localFaceList.push_back((std::vector<vector3<int>>){hash1, hash2, hash3});
							else localFaceList.push_back((std::vector<vector3<int>>){hash3, hash2, hash1});
						}

						// Mark the hashed dual vertex as having seen this primal vertex
						for(auto jdx : adj) {
							vector3<int> hash = center_hash_offsets[jdx] + vector3<int>(ii*2, jj*2, kk*2);

							#pragma omp critical (hash_bash_bcc)
							{
								face_hash_table(hash.i, hash.j, hash.k).touching.push_back({pv.i, pv.j, pv.k});
							}
						}

					}
				}
				// Merge the faces back in to global face list
				#pragma omp critical (lizst_cyst)
				{
					faceList.reserve(faceList.size() + localFaceList.size());
					faceList.insert(faceList.end(), localFaceList.begin(), localFaceList.end());
				}
		}
		processVertices<L,I,O>(f, dh);
		processFaces();
	}

	bool writeSurface(const std::string &out) const {
		return output_mesh.writePly(out);
	}

private:
	template<class L, class I, class O>
	void processVertices(L *f, const I &dh){
		// Calculate the vertex for each cell
		for (auto it = face_hash_table.siteMap.begin(); it != face_hash_table.siteMap.end(); ++it) {
			auto hash = it->first;
			auto vcache = it->second;

			std::vector<vector3<T>> normals;
			for(auto v : vcache.touching)
				normals.push_back(f->grad_f(v*dh).normalize());

			auto pavg = optimize_for_feature(vcache.touching, normals) * dh;
			auto normal = f->grad_f(pavg).normalize();

			face_hash_table.siteMap[hash].vertexId = output_mesh.addVertex({pavg, normal});
		}
	}

	void processFaces(){
		/* Build the final face list */
		for(auto face : faceList) {
			std::vector<int> index_face;
			for(auto hash : face) {
				index_face.push_back(face_hash_table(hash.i, hash.j, hash.k).vertexId);
			}
			output_mesh.addPolygon(index_face);
		}
	}

	std::vector<std::vector<vector3<int>>> faceList;
	sisl::sparse_array3<cell_vertex> face_hash_table;
	utility::ply_writer<T> output_mesh;

	const std::vector<vector3<int>> polyhedron_vertices = {
		{0,0,0}, {-1,0,1}, {0,1,1}, {1,0,1}, {0,-1,1},
		{-1,-1,0}, {1,-1,0}, {1,1,0}, {-1,1,0}, {0,-1,-1},
		{1,0,-1}, {0,1,-1},  {-1,0,-1}
	};

	const std::vector<vector3<int>> center_hash_offsets = {
		{ 2, 0, 0}, {-2, 0, 0}, { 0, 2, 0}, { 0,-2, 0},
		{ 0, 0, 2}, { 0, 0,-2}, { 1, 1, 1}, { 1, 1,-1},
		{ 1,-1, 1}, { 1,-1,-1}, {-1, 1, 1}, {-1, 1,-1},
		{-1,-1, 1}, {-1,-1,-1},
	};

	const std::vector<std::vector<int>> adj_index = {
		{4, 10, 12, 1}, {4, 10, 6, 2}, {4, 6, 8, 0}, {4, 8, 12, 3},
		{3, 13, 12, 1}, {0, 8, 9, 3}, {2,6,7,0}, {1,10,11,2},
		{3,9,13, 5}, {0,7,9,5}, {2,11,7, 5}, {1,13,11,5}
	};

	const std::vector<std::vector<std::vector<int>>> triangle_lookup = {
		{{4, 10, 12}, {10, 12, 1}}, {{4, 10, 6}, {10, 6, 2}},
		{{4, 6, 8}, {6, 8, 0}}, {{4, 8, 12}, {8, 12, 3}},
		{{3, 13, 12}, {8, 12, 3}}, {{0, 8, 9}, {8, 9, 3}},
		{{2,6,7}, {6,7,0}}, {{1,10,11}, {10,11,2}},
		{{3,9,13}, {9,13,5}}, {{0,7,9}, {7,9,5}},
		{{2,11,7}, {11,7,5}}, {{1,13,11}, {13,11,5}}
	};

	const std::vector<int> minimal_face_set = {1 , 2, 3, 4, 6, 7, 8};

	vector3<T> optimize_for_feature(
			const std::vector<vector3<T>> &points,
			const std::vector<vector3<T>> &normals,
			const T &threshold = 0.1,
			const bool &optimize = true) {
		using namespace Eigen;
		typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> EMatrix;
		vector3<T> center(0,0,0);
		EMatrix A(points.size(), 3), b(points.size(), 1);
		unsigned int i = 0;

		// Calculate the center and setup the matix
		for(auto v : points) {
			center += v;

			A(i, 0) = v.i;
			A(i, 1) = v.j;
			A(i, 2) = v.k;

			b(i, 0) = points[i] * normals[i];
			i++;
		}

		center = center * (1./(T(points.size())));

		return center;
	}

};
};
};

#endif // _DUAL_CC_ISOSURFACE_H_


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2012-2014 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SDC_HH
#define SDC_HH

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/istl/operators.hh"
#include "dune/istl/solvers.hh"

#include "dune/common/dynmatrix.hh"
#include "dune/common/dynvector.hh"

#include "linalg/dynamicMatrix.hh"

namespace Kaskade {

  /**
   * \ingroup timestepping
   * \brief Abstract base class of time grids for (block) spectral defect correction methods.
   *
   * This class represents a time grid on \f$ [t_0,t_n]\f$ with \f$ n \f$ subintervals and \f$ n+1 \f$
   * time grid points (including the end points).
   */
  class SDCTimeGrid
  {
  public:
    /**
     * \brief The type used for real vectors.
     */
    typedef Dune::DynamicVector<double> RealVector;

    /**
     * \brief The type used for real (dense) matrices.
     */
    typedef Dune::DynamicMatrix<double> RealMatrix;

    /**
     * \brief Time points in the time step
     *
     * The time step \f$ [t_0, t_n] \f$ contains \f$ n+1 \f$ time points \f$ t_i \f$,
     * including the end points. Those are provided here. The time points are stored in increasing
     * order.
     */
    virtual RealVector const& points() const = 0;

    /**
     * \brief Integration matrix \f$ S \f$
     *
     * On the time interval, the time grid defines an interpolation scheme, such that given \f$ u(t_i) \f$
     * for all time points, a continuous function \f$ u(t) \f$ can be evaluated in the whole interval.
     *
     * The Lagrangian interpolation functions \f$ L_k \f$ are defined by \f$ L_(t_i) = \delta_{ik} \f$.
     * The matrix \f$ S \in \mathbb{R}^{n\times n+1}\f$ contains the values
     * \f[ S_{ik} = \int_{\tau=t_i}^{t_{i+1}} L_k(\tau) \, d\tau. \f]
     * This way, if \f$ u \f$ is defined in terms of its function values \f$ v_i = u(t_i) \f$, the
     * integrals can be evaluated by a matrix-vector multiplication:
     * \f[ \int_{\tau=t_i}^{t_{i+1}} u(\tau) \, d\tau = (Sv)_i \f]
     *
     * Note that not necessarily all time points \f$ t_i \f$ are used in formulating the Lagrangian interpolation
     * functions. E.g., on Radau points, the first point \f$ t_0 \f$ is omitted (leading to a zero column in \f$ S \f$).
     */
    virtual RealMatrix const& integrationMatrix() const = 0;

    /**
     * \brief Differentiation matrix \f$ D \f$
     *
     * On the time interval, the time grid defines an interpolation scheme, such that given \f$ u(t_i) \f$
     * for all time points, a continuous function \f$ u(t) \f$ can be evaluated in the whole interval.
     *
     * The Lagrangian interpolation functions \f$ L_k \f$ are defined by \f$ L_k(t_i) = \delta_{ik} \f$.
     * The matrix \f$ D \in \mathbb{R}^{n+1\times n+1}\f$ contains the values
     * \f[ D_{ik} = \dot L_k(\tau_i)  \f]
     * This way, if \f$ u \f$ is defined in terms of its function values \f$ v_i = u(t_i) \f$, its
     * derivatives can be evaluated by a matrix-vector multiplication:
     * \f[ \dot u(\tau_i) = (Dv)_i \f]
     *
     * Note that not necessarily all time points \f$ t_i \f$ are used in formulating the Lagrangian interpolation
     * functions. E.g., on Radau points, the first point \f$ t_0 \f$ is omitted (leading to a zero column in \f$ D \f$).
     */
    virtual RealMatrix const& differentiationMatrix() const = 0;

    /**
     * \brief Perform refinement of the grid, filling the prolongation matrix.
     *
     * If the function representation is not sufficiently accurate, a finer grid of time points can be tried.
     * This method refines the grid to \f$ m+1 \f$ time points \f$ s_i \f$, \f$ m>n \f$,
     * and fills a prolongation matrix \f$ P\in \mathbb{R}^{m+1\times n+1} \f$ such that with \f$ v_i = u(t_i) \f$
     * and \f$ w=Pv \f$ it holds that \f$ w_i = u(s_i) \f$.
     *
     * Different derived classes may implement this in different ways, or provide their own, more flexible
     * ways of refining the grid.
     *
     * \param[out] p the prolongation matrix
     */
    virtual void refine(RealMatrix& p) = 0;

    /**
     * \brief Compute interpolation coefficients.
     *
     * Returns a matrix \f$ w \in \mathbb{R}^{m+1\times n+1} \f$, such that the interpolation polynomial \f$ p \f$ to the values \f$ y_i \f$ at
     * grid points \f$ t_i \f$ can be evaluated as
     * \f[ p(x_i) = \sum_{j=0}^n w_{ij} y_j, \quad i=0,\dots,m. \f]
     */
    virtual RealMatrix interpolate(RealVector const& x) const = 0;
  };

  // ---------------------------------------------------------------------------------------------------

  /**
   * \ingroup timestepping
   * \brief Triangular approximate integration matrix \f$ \hat S \f$ corresponding to Euler integrator
   *
   * This computes a lower "triangular" matrix \f$ \hat{S} \in \mathbb{R}^{n\times n+1} \f$ with \f$ \hat{S}_{ij} = 0 \f$
   * for \f$ i \le j \f$ that can be used to formulate SDC sweeps.
   *
   * This is \f$ \hat{S}_{i,i+1} = t_{i+1}-t_i \f$, all other entries zero. In an SDC sweep,
   * this corresponds to an implicit Euler scheme for integrating the defect equation.
   */
  void eulerIntegrationMatrix(SDCTimeGrid const& grid, SDCTimeGrid::RealMatrix& Shat);

  /**
   * \ingroup timestepping
   * \brief Triangular approximate integration matrix \f$ \hat S \f$ corresponding to specialized multi-step integrators
   *
   * This computes a lower "triangular" matrix \f$ \hat{S} \in \mathbb{R}^{n\times n+1} \f$ with \f$ \hat{S}_{ij} = 0 \f$
   * for \f$ i \le j \f$ that can be used to formulate SDC sweeps.
   *
   * If \f$ S^T = LU \f$, this is \f$ \hat S = U^T \f$, which, in an SDC sweep
   * corresponds to a cascade of tailor-made multi-step integrators. The consequence is that on the Dahlquist
   * test equation \f$ \dot u = \lambda u \f$, in the limit \f$ \lambda \to \infty \f$ the SDC iteration matrix
   * \f$ G = I - \hat S^{-1} S \f$ is nilpotent and hence has a spectral radius of 0, which yields fast convergence.
   */
  void luIntegrationMatrix(SDCTimeGrid const& grid, SDCTimeGrid::RealMatrix& Shat);

  /**
   * \ingroup timestepping
   * \brief Types of weight functions for optimized integration matrices.
   */
  enum IntegrationMatrixOptimizationWeight { OW_FLAT };

  /**
   * \ingroup timestepping
   * \brief Triangular approximate differentiation and integration matrix \f$ \hat D, \hat S \f$ corresponding to specialized multi-step integrators
   *
   * \param grid the time grid
   * \param k    the sweep number
   * \param w    the type of weight function for the optimization
   *
   * This returns one of a set of precomputed, optimized integration matrices, if available for the given grid. If no precomputed
   * matrix is available, a lookup exception is thrown.
   */
  std::pair<SDCTimeGrid::RealMatrix,SDCTimeGrid::RealMatrix>
  optimizedMatrices(SDCTimeGrid const& grid, int k, IntegrationMatrixOptimizationWeight w=OW_FLAT);

  /**
   * \ingroup timestepping
   * \brief Triangular approximate differentiation and integration matrix \f$ \hat D, \hat S \f$ corresponding to specialized multi-step integrators
   *
   * \param grid     the time grid
   * \param k        the sweep number
   * \param Df       the differentiation matrix returned in case no precomputed optimized matrix is available
   * \param Sf       the integration matrix returned in case no precomputed optimized matrix is available
   * \param w        the type of weight function for the optimization
   *
   * This returns one of a set of precomputed, optimized integration matrices, if available for the given grid. If no precomputed
   * matrix is available, the provided fallback matrix is returned. This is a convenience overload.
   */
  std::pair<SDCTimeGrid::RealMatrix,SDCTimeGrid::RealMatrix>
  optimizedIntegrationMatrix(SDCTimeGrid const& grid, int k, SDCTimeGrid::RealMatrix const& Df, SDCTimeGrid::RealMatrix const& Sf,
                             IntegrationMatrixOptimizationWeight w=OW_FLAT);

  /**
   * \ingroup timestepping
   * \brief spectral time grid for defect correction methods with Lobatto points
   *
   * Note that collocation with Lobatto points is A-stable but not L-stable, and therefore SDC methods
   * on Lobatto grids may be a suboptimal choice for highly stiff problems (e.g., parabolic equations or Dirichlet
   * b.c. realized as quadratic penalty). Consider using RadauTimeGrid in these cases.
   */
  class LobattoTimeGrid: public SDCTimeGrid
  {
  public:
    /**
     * \brief constructs a Lobatto grid with \f$ n+1 \f$ points on \f$ [a,b] \f$
     *
     * This may throw LinearAlgebraException.
     */
    LobattoTimeGrid(int n, double a, double b);

    virtual RealVector const& points() const
    {
      return pts;
    }

    virtual RealMatrix const& integrationMatrix() const
    {
      return integ;
    }

    virtual RealMatrix const& differentiationMatrix() const
    {
      return diff;
    }

    /**
     * \brief perform refinement of the grid, filling the prolongation matrix
     *
     * This may throw LinearAlgebraException.
     */
    virtual void refine(RealMatrix& p);

    virtual RealMatrix interpolate(RealVector const& x) const;

  private:
    RealVector pts;
    RealMatrix integ;
    RealMatrix diff;
  };

  /**
   * \ingroup timestepping
   * \brief spectral time grid for defect correction methods with Radau points
   *
   * Note that collocation with Radau points is L-stable, and therefore SDC methods
   * on Radau grids are best for highly stiff problems (e.g., parabolic equations or Dirichlet
   * b.c. realized as quadratic penalty). Consider using LobattoTimeGrid in other cases.
   */
  class RadauTimeGrid: public SDCTimeGrid
  {
  public:
    /**
     * \brief constructs a Radau grid with \f$ n+1 \f$ points on \f$ [a,b] \f$
     *
     * This may throw LinearAlgebraException.
     */
    RadauTimeGrid(int n, double a, double b);

    virtual RealVector const& points() const
    {
      return pts;
    }

    virtual RealMatrix const& integrationMatrix() const
    {
      return integ;
    }

    virtual RealMatrix const& differentiationMatrix() const
    {
      return diff;
    }

    /**
     * \brief perform refinement of the grid, filling the prolongation matrix
     *
     * This may throw LinearAlgebraException.
     */
    virtual void refine(RealMatrix& p);

    virtual RealMatrix interpolate(RealVector const& x) const;

  private:
    RealVector pts;
    RealMatrix integ;
    RealMatrix diff;

    void computeMatrices();
  };

  /**
   * \ingroup timestepping
   * \brief A single spectral defect correction iteration sweep
   *
   * This function performs one spectral defect correction (SDC) iteration for the abstract reaction
   * diffusion equation
   * \f[ M \dot u = A u + M f(u) \f]
   * using the linearly implicit Euler method on the given time grid. The solution interpolation
   * vector \arg u contains the approximate values of \f$ u \f$ at the time nodes \f$ t_i \f$ in a
   * Lagrangian FE basis.
   * Here, \arg A and \arg M are fixed matrices, such that locally we perform a method of lines
   * in this time step.
   *
   * SDC iterations are inexact Newton iterations for the Fredholm collocation time discretization
   * of an ODE on a time grid \f$ t_0, \dots, t_n \f$:
   * \f[ M (u_{i+1} - u_i) = \int_{\tau=t_i}^{t_{i+1}} p(\tau) \,d\tau \quad i=0,\dots,n-1, \f]
   * where \f$ p(t_i) = r(u_i) = Au_i + M f(u_i) \f$ represents the (probably polynomial) interpolant
   * of the right hand side. By linearity, the integral can be expressed as a linear combination of
   * the right hand side values:
   * \f[ M (u_{i+1} - u_i) = \sum_{j=0}^n S_{ij} r(u_j) \f]
   * Applying Newton's method to \f$ F(u) = M (u_{i+1} - u_i) - \sum_{j=0}^n S_{ij} r(u_j) \f$ yields
   * \f$ F'(u^k)\delta u^k = -F(u^k), \quad \delta u_i^k = u_i^{k+1} - u_i^k \f$, or, more elaborate,
   * \f[ M(\delta u_{i+1}^k - \delta u_i^k) - \sum_{j=0}^n S_{ij} r'(u_j^k) \delta u_j^k
   *     = -M (u_{i+1}^k - u_i^k) + \sum_{j=0}^n S_{ij} r(u_j^k) =: R_i, \quad i=0,\dots,n-1. \f]
   * The time-global coupling makes this system difficult to solve. An approximation of the
   * quadrature coefficients \f$ S_{ij} \f$ by a triangular \f$ \hat S_{ij} \f$  yields the simpler system
   * \f[ M(\delta u_{i+1}^k - \delta u_i^k) - \hat{S}_{i,i+1} r'(u_{i+1}^k) \delta u_{i+1}^k
   *     =  R_i + \sum_{j=0}^i \hat{S}_{i,j} r'(u_{j}^k) \delta u_j^k, \quad i=0,\dots,n-1, \f]
   * starting with \f$ \delta u_0^k = 0 \f$, or, equivalently,
   * \f[ (M- \hat{S}_{i,i+1} r'(u_{i+1}^k))(\delta u_{i+1}^k - \delta u_i^k)
   *     =  \sum_{j=0}^{i}\hat{S}_{i,j}r'(u_{j}^k) \delta u_j^k +  \hat{S}_{i,i+1}r'(u_{i+1}^k) \delta u_i^k + R_i, \quad i=0,\dots,n-1, \f]
   *
   * The norm of the correction \f$ \delta u^k \f$ is returned, i.e.
   * \f[ \left( (t_n-t_0)^{-1}\sum_{i=0}^{n-1} (t_{i+1}-t_i)(\delta u_i^k)^T (M- \hat{S}_{i,i+1} r'(u_{i+1}^k)) \delta u_i^k \right)^{1/2}. \f]
   *
   * \tparam Matrix a sparse matrix type, usually Dune::BCRSMatrix or a NumaBCRSMatrix
   * \tparam Vectors a container type with elements from the domain type of Matrix
   * \tparam Solver
   * \tparam ReactionDerivatives
   *
   * \param[in] grid the collocation time grid
   * \param[in] Shat the triangular quadrature matrix approximation \f$ \hat S \f$
   * \param[in] solve a callable that supports solve(A,x,b) giving an approximative solution of \f$ Ax = b \f$, where A is of type Matrix
   * \param[in] M the mass matrix
   * \param[in] Stiff the stiffness matrix \f$ A \f$ (usually negative semidefinite)
   * \param[in] rUi the right hand sides at the collocation times
   * \param[in] rDu the reaction term derivatives (a collection of vectors representing the diagonals of the derivatives)
   * \param[in] u the current iterate (values of \f$ u \f$ at the collocation times)
   * \param[out] du the approximate Newton correction \f$ \delta u \f$
   */
  template <class Matrix, class Vectors, class ReactionDerivatives, class Solver>
  typename Matrix::field_type sdcIterationStep(SDCTimeGrid const& grid, SDCTimeGrid::RealMatrix const& Shat, Solver const& solve,
                                               Matrix const& M, Matrix const& Stiff,
                                               Vectors const& rUi, ReactionDerivatives const& rDu, Vectors const& u, Vectors& du)
  {
    auto const& pts = grid.points();
    int const n = pts.size()-1;      // number of subintervals

    assert(u.size()==n+1); // including start point
    typedef typename Vectors::value_type Vector;

    Vector tmp = u[0];
    std::vector<Vector> Mdu(n,tmp);
    for (int i=0; i<n; ++i)
    {
      // compute M (u_{i}-u_{i+1})  TODO: loop fusion
      tmp = u[i];
      tmp.axpy(-1.0,u[i+1]);
      M.mv(tmp,Mdu[i]);
    }
    return sdcIterationStep2(grid,Shat,solve,M,Stiff,rUi,rDu,Mdu,du);
  }

 /**
   * \ingroup timestepping
   * \brief A single spectral defect correction iteration sweep
   *
   * This is an overload with slightly different interface (required for some more or less arcane algorithmic variants). The only interface difference
   * is that instead of values \f$ u_i \f$, the actually required differences \f$ M (u_{i+1}-u_i) \f$ are provided. Those are computed explicitly in
   * \ref sdcIterationStep.
   *
   * \tparam Matrix a sparse matrix type, usually Dune::BCRSMatrix or a NumaBCRSMatrix
   * \tparam Vectors a container type with elements from the domain type of Matrix
   * \tparam ReactionDerivatives a container type with sparse matrix elements (usually BCRSMatrix or NumaBCRSMatrix)
   * \tparam Solver
   *
   * \param[in] grid the collocation time grid with n+1 points
   * \param[in] Shat the triangular quadrature matrix approximation \f$ \hat S \f$
   * \param[in] solve a callable that supports solve(A,x,b) giving an approximative solution of \f$ Ax = b \f$, where A is of type Matrix
   * \param[in] M the mass matrix
   * \param[in] Stiff the stiffness matrix (usually negative semidefinite)
   * \param[in] rUi the right hand sides at the collocation times (including interval start point), size n+1
   * \param[in] rDu the reaction term derivatives (a collection of sparse matrices, the sparsity pattern of which is a subset of that of A and M), size n+1
   * \param[in] Mdu the current iterate differences (values of \f$ M(u_{i}-u_{i+1}) \f$ for \f$ i=0,\dots,n-1 \f$)
   * \param[out] du the approximate Newton correction \f$ \delta u \f$
   *
   * \return the energy norm of the correction du
   */
  template <class Matrix, class Vectors, class ReactionDerivatives, class Solver>
  typename Matrix::field_type sdcIterationStep2(SDCTimeGrid const& grid, SDCTimeGrid::RealMatrix const& Shat, Solver const& solve,
                                                Matrix const& M, Matrix const& Stiff,
                                                Vectors const& rUi, ReactionDerivatives const& rDu, Vectors const& Mdu, Vectors& du)
  {
    auto const& pts = grid.points();
    int const n = pts.size()-1;      // number of subintervals

    assert(Mdu.size()>=n);
    assert(du.size()>=n+1);  // including start point

    size_t const dofs = Mdu[0].size();

    // compute exact integration matrix
    auto const& S = grid.integrationMatrix();

    // initialize correction at starting point to zero
    du[0] = 0.0;

    typedef typename Vectors::value_type Vector;
    Vector rhs(dofs), tmp(dofs);  // declare here to prevent frequent reallocation

    // perform n Euler steps
    typename Matrix::field_type norm = 0;
    Matrix J = M;
    for (int i=1; i<=n; i++)
    {
      // matrix J = M - Shat_i-1,i*(A+f_u)
      for (size_t row=0; row<J.N(); ++row)
      {
        auto colJ = J[row].begin();
        auto end = J[row].end();
        auto colM = M[row].begin();
        auto colA = Stiff[row].begin();
        auto colR = rDu[i][row].begin();
        auto endR = rDu[i][row].end();

        while (colJ != end)
        {
          *colJ = *colM - Shat[i-1][i] * *colA;

          if (colR != endR && colJ.index() == colR.index()) // f_u can have subset of sparsity pattern
          {
            *colJ -= std::min(0.5* *colM, Shat[i-1][i] * *colR); // guarantee M - Shat*fu is nonnegative -- reduce by at most 50%
            ++colR;
          }
          ++colJ; ++colM; ++colA;
        }
      }

      //  right-hand side for linear system

      // M * ( u_i^{k} - u_{i+1}^k + du_i)
      rhs = Mdu[i-1];
      M.umv(du[i-1],rhs);

      // add sum_j S_ij r_j to right hand side
      for (int j=0; j<=n; ++j)
        rhs.axpy(S[i-1][j],rUi[j]);

      // add sum_j Shat_ij r'(u_j) du_j with r' = A + f_u
      tmp = 0;
      for (int j=0; j<i; ++j) // TODO: start at 1 instead of 0? du[0] is zero anyway...
      {
        rDu[j].usmv(Shat[i-1][j],du[j],rhs);
        tmp.axpy(Shat[i-1][j],du[j]);
      }
      Stiff.umv(tmp,rhs);

      // solve linear system
      du[i] = du[i-1]; // previous increment is probably a good starting value
      solve(J,du[i],rhs);

      // evaluate norm of correction
      norm += (pts[i]-pts[i-1]) * (du[i]*rhs);
    }   // end i - loop

    return std::sqrt(norm/(pts[n]-pts[0]));
  }

  template <class Matrix, class Vectors, class Reaction, class Solver>
  typename Matrix::field_type sdcIterationStep3(SDCTimeGrid const& grid, SDCTimeGrid::RealMatrix const& Shat, Solver const& solve,
                                                Matrix const& M, Matrix const& Stiff,
                                                Vectors const& rUi, Reaction const& fAt, Vectors const& Mdu, Vectors& du,
                                                int nReactionSweeps)
  {
    auto const& pts = grid.points();
    int const n = pts.size()-1;      // number of subintervals

    assert(Mdu.size()>=n);
    assert(du.size()>=n+1);  // including start point

    size_t const dofs = Mdu[0].size();

    // compute exact integration matrix
    auto const& S = grid.integrationMatrix();
// auto const& S = Shat;

    // initialize correction at starting point to zero
    du[0] = 0.0;

    typedef typename Vectors::value_type Vector;
    Vector rhs(dofs), tmp(dofs);  // declare here to prevent frequent reallocation

    // perform n basic steps
    typename Matrix::field_type norm = 0;
    Matrix J = M;
    for (int i=1; i<=n; i++)
    {
      // Each basic step consists of a splitting method, separating a linearly implicit Euler step
      // from the pointwise nonlinearity of the right hand side. First we perform the linearly implicit
      // Euler step.

      //  right-hand side for linear system

      // M * ( u_{i-1}^{k} - u_{i}^k + du_{i-1})
      rhs = Mdu[i-1];
      M.umv(du[i-1],rhs);

      // add sum_j S_ij r_j to right hand side
      for (int j=0; j<=n; ++j)
        rhs.axpy(S[i-1][j],rUi[j]);

      // add sum_j Shat_ij r'(u_j) du_j with r' = A + f_u, i.e. A (sum_j Shat_ij du_j) + sum_j Shat_ij f_uj du_j
      // addition of f_u is postponed to matrix loop
      tmp = 0;
      for (int j=0; j<i; ++j) // TODO: start at 1 instead of 0? du[0] is zero anyway...
        tmp.axpy(Shat[i-1][j],du[j]);
      Stiff.umv(tmp,rhs);

      // matrix J = M - Shat_i-1,i*(A+f_u)
      auto const f = fAt( pts[i] );
      for (size_t row=0; row<J.N(); ++row)
      {
        auto colJ = J[row].begin();
        auto end = J[row].end();
        auto colM = M[row].begin();
        auto colA = Stiff[row].begin();

        while (colJ != end)
        {
          auto fu = *colM * (f(row,0.0,1) + f(colJ.index(),0.0,1)) / 2;
//           *colJ = *colM - Shat[i-1][i] * (*colA + fu); // exact Newton, but for large step sizes the Jacobi matrix becomes singular...
//           *colJ = *colM - Shat[i-1][i] * (*colA + std::min(0.0,fu));  // this modification guarantees an invertible Jacobian
          *colJ = *colM - Shat[i-1][i] * (*colA);         // explicit reaction
          if (colJ.index()==row)
            *colJ -= Shat[i-1][i] * fu; // only diagonal of reaction mass matrix

//           // add f_u contribution to right hand side (no-op for Euler SDC as Shat[i-1][j]=0 for j<i)
//           for (int j=0; j<i; ++j) // TODO: start at 1 instead of 0? du[0] is zero anyway...
//           {
//             auto f = fAt(pts[j]);
//             rhs[row] += *colM*Shat[i-1][j]*(f(row,0.0,1)+f(colJ.index(),0.0,1))/2 * du[j][colJ.index()];
//           }

          ++colJ; ++colM; ++colA;
        }
      }

      // solve linear system
      du[i] = du[i-1]; // previous increment is probably a good starting value
      solve(J,du[i],rhs);

std::cerr << "du=" << du[i] << "\n";
// std::cerr << "|du| = " << du[i].two_norm2() << "  du[i]=" << du[i][0] << "  du[i-1]=" << du[i-1][0] << "\n";
      // Second part is the remaining nonlinearity, which we address by a subsequent run of a couple of Euler steps
      double const tau = pts[i]-pts[i-1];
      double snorm = 0;
      for (size_t row=0; row<J.N(); ++row)
      {
        auto fend = fAt(pts[i]);
        double dfdu = std::min(0.0,fend(row,0.0,1)) * du[i][row];

        double s = 0;
        int l = nReactionSweeps;
        for (int j=0; j<l; ++j)
        {
          double theta = 0.0; // where in the subinterval to evaluate (theta=0 explicit Euler, theta=0.5 lin. impl. midpoint, theta=1 lin. impl Euler)
          auto f = fAt(pts[i-1]+(j+theta)*tau/l);
          double dut = ((l-j-theta)*du[i-1][row]+(j+theta)*du[i][row])/l;

// if (row==0)
// std::cerr << "j=" << j << " du+s=" << dut+s << "   f(u+du+s)=" <<  f(row,dut+s,0) << "  f(u)=" <<  f(row,0.0,0) << "  dfdu=" <<  dfdu  <<  "  sum rhs=" << (f(row,dut+s,0) - f(row,0.0,0) - dfdu);
//           s += tau/l * (f(row,dut+s,0) - f(row,0.0,0) - dfdu) / (1-1.0*tau/l*std::min(0.0,f(row,dut+s,1)));
if (row==0)
std::cerr << "j=" << j << " du+s=" << dut+s << "   f(u+du+s)=" <<  f(row,dut+s,0) << "  f(u)=" <<  f(row,0.0,0) << "  dfdu=" <<  f(row,dut+s,1)  <<  "  sum rhs=" << (f(row,dut+s,0) - f(row,0.0,0));
          s += tau/l * (f(row,dut+s,0) - f(row,0.0,0) - f(row,0.0,1)*(dut+s)) / (1-theta*tau/l*std::min(0.0,f(row,dut+s,1)));
if (row==0) std::cerr << "  -> s=" << s << "\n";
        }
        du[i][row] += s;
        snorm += s*s;
      }
// std::cerr << "|s| = " << snorm << "\n";

      // evaluate norm of correction
      norm += (pts[i]-pts[i-1]) * (du[i]*rhs);
    }   // end i - loop

    return std::sqrt(norm/(pts[n]-pts[0]));
  }

  /**
   * \ingroup timestepping
   * \brief A single waveform relaxation iteration
   *
   * This function performs one linearized waveform relaxation iteration for
   * \f[ M \dot u = A u + M f(u) \f]
   * using the Jacobi method. It approximately solves the linear defect system
   * \f[ M \dot{\delta u} = A \delta u + M f'(u)\delta u + r, \quad r=Au+f(u)-\dot u \f]
   * for the correction \f$ \delta u \f$ to be added to the provided approximation \f$ u \f$. The time evolution
   * is discretized by a collocation method on the given time grid. Thus, for each entry \f$ u_i \f$, the collocation
   * solution of the scalar ODE
   * \f[ M_{ii}\dot{\delta u_i} = A_{ii} \delta u_i + M_{ii}f'(u_i) \delta u_i + r_i. \f]
   *
   * The solution interpolation
   * vector \arg u contains the approximate values of \f$ u \f$ at the time nodes \f$ t_i \f$ in a
   * Lagrangian FE basis.
   * Here, \arg A and \arg M are fixed matrices, such that locally we perform a method of lines
   * in this time step.
   *
   * Linearized waveform relaxation works here as
   *
   *
   *
   * The norm of the correction \f$ \delta u^k \f$ is returned, i.e.
   * \f[  \f]
   *
   * \tparam Matrix a sparse matrix type, usually Dune::BCRSMatrix or a NumaBCRSMatrix
   * \tparam Vectors a container type with elements from the domain type of Matrix
   * \tparam ReactionDerivatives
   *
   * \param[in] grid the collocation time grid
   * \param[in] Shat the triangular quadrature matrix approximation \f$ \hat S \f$
   * \param[in] solve a callable that supports solve(A,x,b) giving an approximative solution of \f$ Ax = b \f$, where A is of type Matrix
   * \param[in] M the mass matrix
   * \param[in] A the stiffness matrix (usually negative semidefinite)
   * \param[in] rUi the right hand sides at the collocation times
   * \param[in] rDu the reaction term derivatives (a container of sparse matrices, the sparsity pattern of which is a subset of that of A and M), size n+1
   * \param[in] Mdu the current iterate differences (values of \f$ M(u_{i}-u_{i+1}) \f$ for \f$ i=0,\dots,n-1 \f$)
   * \param[out] du the approximate Newton correction \f$ \delta u \f$
   *
   * \return the energy norm of the correction du
   */
  template <class Matrix, class Vectors, class ReactionDerivatives>
  double waveformRelaxationStep2(SDCTimeGrid const& grid,
                                 Matrix const& M, Matrix const& A,
                                 Vectors const& rUi, ReactionDerivatives const& rDu, Vectors& du)
  {
    auto const& pts = grid.points();
    int const n = pts.size()-1;      // number of subintervals -- left interval boundary always included in points

    assert(rUi.size()>=n);
    assert(du.size()>=n+1);  // including start point - usually 0

    size_t const dofs = rUi[0].size();

    // We need to solve the scalar ODE
    // Mii y_t = Aii y + Rii y + ri, y(0) = 0
    // We approximate y_t by the spectral differentiation matrix D as Dy and obtain, as system in the
    // collocation points (excluding the initial value 0)
    // (MiiD - diag(Aii+Rii)) y = r.
    // The system matrix is called S here.

    Dune::DynamicMatrix<double> S(n,n);  // n x n system matrix TODO: use Kaskade::DynamicMatrix?
    Dune::DynamicVector<double> r(n), y(n);
    auto const& D = grid.differentiationMatrix();

    std::cerr << "using D = \n" << D << "\n";

    // a function to extract diagonal entries
    auto diagonal = [](Matrix const& M, size_t i) -> double
    {
      auto const& arow = M[i];
      auto first = arow.begin();
      auto last = arow.end();
      while (first!=last && first.index()<i)
        ++first;
      if (first==last)
        std::cerr << "first==last\n"; std::cerr.flush();
      return *first;
    };

    double retval = 0;

    for (size_t i=0; i<dofs; ++i)
    {
      std::cerr.flush();
      auto mii = diagonal(M,i);
      auto aii = diagonal(A,i);

      for (int j=0; j<n; ++j)
      {
        for (int k=0; k<n; ++k)
          S[j][k] = mii*D[j+1][k+1]; // remember D starts with left interval point
        S[j][j] -= aii+diagonal(rDu[j+1],i);
        r[j] = rUi[j+1][i];
      }
      S.solve(y,r);
      for (int j=0; j<n; ++j)
        du[j+1][i] = 0.5*y[j]; // Jacobi damping (otherwise we get spatially high-frequent errors)
      retval += 0.25 * y.two_norm2();
    }

    return std::sqrt(retval);
  }

}

#endif