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

#ifndef ISTLINTERFACE_HH
#define ISTLINTERFACE_HH

#include <memory>
#include <type_traits>

#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/sequence.hpp>

#include <dune/istl/operators.hh>
#include "dune/istl/preconditioners.hh"

#include "fem/fixdune.hh"
#include "fem/functionspace.hh"

#include "linalg/triplet.hh"
#include "linalg/crsutil.hh"
#include "linalg/factorization.hh"
#include "linalg/symmetricOperators.hh"
#include "linalg/threadedMatrix.hh"

#include "utilities/duneInterface.hh"

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

namespace Kaskade
{
  /**
   * \cond internals
   */
  namespace IstlInterfaceDetail {

    template <class T>
    typename T::iterator begin(T& t)
    {
      return t.begin();
    }

    template <class T>
    typename T::const_iterator begin(T const& t)
    {
      return t.begin();
    }

    template <class Scalar>
    typename std::vector<Scalar>::iterator begin(std::vector<Scalar>& vec) { return vec.begin(); }

    template <class Scalar>
    typename std::vector<Scalar>::const_iterator begin(std::vector<Scalar> const& vec) { return vec.begin(); }

    using namespace boost::fusion;

    /**
     * \brief This is a lightweight matrix block class, holding a reference to the actual matrix that is owned by the matrix block in the
     *        assembler as well as a NUMA-aware copy.
     * \tparam ElementType          the type of block matrix elements
     * \tparam rid the row index of the block
     * \tparam cid the column index of the block
     * \tparam sym whether the block is symmetric or not
     * \tparam trans whether the block is mirrored
     *
     * If the block is mirrored, the assembler matrix may have a different element type than this block (m x n instead of n x m).
     */
    template <class ElementType, int rid, int cid, bool sym, bool trans>
    struct Block
    {
      static int const rowId = rid;          // row index of the block
      static int const colId = cid;          // col index of the block
      static bool const symmetric = sym;     // symmetry
      static bool const transposed = trans;  // whether only its mirror image is available in the assembler

      typedef ElementType BlockType;
      typedef typename BlockType::field_type Scalar;
      typedef NumaBCRSMatrix<typename Transpose<BlockType,transposed>::type> Matrix; // Matrix type in assembler
      typedef Dune::BlockVector<Dune::FieldVector<Scalar,BlockType::cols>> Domain; // take care of transposition
      typedef Dune::BlockVector<Dune::FieldVector<Scalar,BlockType::rows>> Range;
      typedef NumaBCRSMatrix<BlockType> TMatrix;

      explicit Block(std::shared_ptr<Matrix const> const& m): matrix(m) {}

      std::shared_ptr<Matrix const> matrix;
      std::shared_ptr<TMatrix> threadedMatrix;
    };

    // Provides a heterogeneous sequence of all matrix blocks appearing
    // conceptually in the Galerkin operator, even if they are not stored
    // explicitly in the assembler because of symmetry.
    template <class GOP>
    class AllBlocks
    {
      struct Copy
      {
        template <class MBlock>
        auto operator()(MBlock const& mb) const {
          typedef std::remove_reference_t<MBlock> MB;
          typedef Block<typename MB::BlockType,MB::rowId,MB::colId,MB::symmetric,false> type;
          return type(mb.globalMatrixPointer());
        }
      };

      // Filters out flagged blocks with row index -1
      struct Cleanup {
        template <class MB> struct apply { typedef boost::mpl::bool_<MB::rowId>=0> type; };
      };

      // Blocks not to be mirrored are flagged with row index -1 to be filtered out later by Cleanup.
      struct Mirror
      {
        template <class T> struct result {};

        template <class MBlock>
        struct result<Mirror(MBlock)> {
          typedef typename std::remove_reference<MBlock>::type MB;
          typedef typename Transpose<typename MB::BlockType,true>::type ElementType;
          typedef Block<ElementType,
              MB::mirror?MB::colId:-1, MB::rowId, MB::symmetric, true> type;
        };

        template <class MB>
        typename result<Mirror(MB)>::type operator()(MB const& mb) const {
          return typename result<Mirror(MB)>::type(mb.globalMatrixPointer());
        }
      };

      // It would be more natural to filter the mirror blocks before
      // actually mirroring them, but this does frequently result in empty
      // filter lists. For some reasons (don't know why), the filter view
      // produces wrong type results when empty lists occur.

      typedef typename result_of::transform<typename GOP::MatrixBlockArray const,Copy>::type CopiedBlocks;
      typedef typename result_of::transform<typename GOP::MatrixBlockArray const,Mirror>::type MirroredBlocks;
      typedef typename result_of::join<CopiedBlocks const,MirroredBlocks const>::type JointBlocks;

    public:
      typedef typename result_of::filter_if<JointBlocks const,Cleanup>::type const A;
      typedef typename result_of::as_vector<A>::type result;

      static result apply(GOP const& op) {
        return filter_if<Cleanup>(join(transform(op.getMatrix(),Copy()),transform(op.getMatrix(),Mirror())));
      }
    };

    // A boost fusion predicate that is true for some matrix block if it
    // lies within the half-open subrange given by the template
    // parameters.
    template <int firstRow, int lastRow, int firstCol, int lastCol>
    struct RangeBlockSelector
    {
      template <class Block> struct apply
      {
        typedef boost::mpl::bool_<firstRow<=Block::rowId && Block::rowId<lastRow && firstCol<=Block::colId && Block::colId<lastCol> type;
      };
    };

    // Shifts row and column indices of matrix blocks downwards and initializes the threaded matrix held by this block.
    template <int firstRow, int firstCol>
    struct Translate {
      // Constructor takes the number of threads onto which the threaded matrix shall be distributed.
      // As a conservative default, this is 1 (sequential).
      Translate(int nThreads_=1): nThreads(nThreads_)
      {
      }

      template <class T> struct result {};

      template <class Blk>
      struct result<Translate(Blk)> {
        typedef typename std::remove_reference<Blk>::type Bl;
        typedef Block<typename Bl::BlockType,Bl::rowId-firstRow,Bl::colId-firstCol,Bl::symmetric,Bl::transposed> type;
      };

      template <class Bl>
      auto operator()(Bl const& bl) const
      {
        Block<typename Bl::BlockType,Bl::rowId-firstRow,Bl::colId-firstCol,Bl::symmetric,Bl::transposed>  blk(bl.matrix);
        blk.threadedMatrix.reset(new typename Bl::TMatrix(*blk.matrix,bl.symmetric,bl.transposed,false));
        return blk;
      }

    private:
      int nThreads;
    };

    /// Coordinate mapping.
    /**
     * Enabled/disabled via std::enable_if on return type.
     */
    template <class FSElement, class Vector>
    typename std::enable_if<!std::is_same<FSElement,Vector>::value,void>::type
    toVector(FSElement const& x, Vector& coefficients)
    {
      coefficients.resize(x.dim());
      vectorToSequence(x,begin(coefficients));
    }

    /// Coordinate mapping is the identity if range_type = vector_type
    template <class FSElement, class Vector>
    typename std::enable_if<std::is_same<FSElement,Vector>::value,void>::type
    toVector(FSElement const& x, Vector& coefficients)
    {
      coefficients = x;
    }

    /// Coordinate mapping
    template <class FSElement, class Vector>
    typename std::enable_if<!std::is_same<FSElement,Vector>::value,void>::type
    fromVector(Vector const& coefficients, FSElement& x)
    {
      assert(coefficients.size()>=x.dim());
      vectorFromSequence(x,begin(coefficients));
    }

    /// Coordinate mapping is the identity if range_type = vector_type
    template <class FSElement, class Vector>
    typename std::enable_if<std::is_same<FSElement,Vector>::value,void>::type
    fromVector(Vector const& coefficients, FSElement& x)
    {
      x = coefficients;
    }

    template <class T>
    void toFile(std::vector<T> const& v, std::string const& filename, size_t precision = 10)
    {
      std::ofstream os;
      os.precision(precision);
      os.open(filename);

      for(T t : v) os << t << "\t";
      os << std::endl;

      os.close();
    }

    // Container storing information on blocks
    template <int firstRow_, int lastRow_, int firstCol_,int lastCol_>
    struct BlockInfo
    {
      static int const firstRow = firstRow_;
      static int const lastRow = lastRow_;
      static int const firstCol = firstCol_;
      static int const lastCol = lastCol_;
    };
  } // End of namespace IstlInterfaceDetail

  /**
   * \endcond
   */

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

/*  template <class Operator>
  class TransposedOperator
  {
  public:
    typedef typename Operator::Domain Range;
    typedef typename Operator::Range Domain;
    typedef typename Operator::Scalar Scalar;

    explicit TransposedOperator(Operator const& A_) : A(A_) {}

    void apply(const Domain &x, Range &b) const { return A.applyTransposed(x,b); }

    void applyscaleadd(Scalar alpha, Domain const& x, Range& b) const { return A.applyscaleaddTransposed(alpha,x,b); }

    Operator const& getOperator() const { return A; }

  private:
    Operator const& A;
  };
*/

  /**
   * \ingroup linalg
   *
   * \brief Operator with matrix-representation and coordinate mappings.
   *
   * Note that the operator creates a copy of the passed matrix (if not passed as rvalue).
   * This allows the implementation of a rvalue-constructor and assures that further manipulation
   * of the matrix data does not change the operator's behaviour.
   *
   * Implements the MatrixRepresentedOperator concept.
   *
   * \todo docme: How does this relate to Dune::MatrixAdapter and Dune::AssembledLinearOperator?
   */
  template <class Matrix_, class Domain_, class Range_> class MatrixRepresentedOperator;

  template <class Scalar_, class SparseInt, class Domain_, class Range_>
  class MatrixRepresentedOperator<MatrixAsTriplet<Scalar_,SparseInt>,Domain_,Range_>
      : public Dune::LinearOperator<Domain_,Range_>
  {
  public:
    typedef MatrixAsTriplet<Scalar_,SparseInt> Matrix;
    typedef Matrix matrix_type; // for compatibility with Dune::MatrixAdapter
    typedef Domain_ Domain;
    typedef Range_ Range;
    typedef Scalar_ Scalar;

    static int const category = Dune::SolverCategory::sequential;

    MatrixRepresentedOperator() = default;

    explicit MatrixRepresentedOperator(Matrix const& A_) : A(A_)
    {}

    explicit MatrixRepresentedOperator(Matrix&& A_) : A(A_)
    {}

    MatrixAsTriplet<Scalar,SparseInt> getTriplet() const { return get<MatrixAsTriplet<Scalar,SparseInt> >(); }

    /// Access matrix
    template <class OtherMatrix=Matrix>
    typename std::enable_if<std::is_same<Matrix,OtherMatrix>::value, Matrix const&>::type get() const { return A; }

    Matrix& get_non_const() { return A; }

    /// Access matrix in another matrix format specified by OtherMatrix.
    /**
     * Note that OtherMatrix must be constructible from Matrix.
     */
    template <class OtherMatrix>
    typename std::enable_if<!std::is_same<Matrix,OtherMatrix>::value, OtherMatrix>::type get() const { return OtherMatrix(A); }

    /// compute \f$b=Ax\f$
    void apply(Domain const& x, Range& b) const
    {
//      A.mv(x,b);
            std::vector<Scalar> tmpx, tmpb;
            domainToVector(x,tmpx);

            A.mv(tmpx,tmpb);

            vectorToRange(tmpb,b);
    }

    void applyTransposed(Range const& x, Domain& b) const
    {
//      A.mv(x,b);
            std::vector<Scalar> tmpx, tmpb;
            domainToVector(x,tmpx);

            A.mtv(tmpx,tmpb);

            vectorToRange(tmpb,b);
    }

    /// Access matrix via unique_ptr. Use if Matrix does not support move-semantics.
    template <class OtherMatrix=Matrix>
    typename std::enable_if<std::is_same<OtherMatrix,Matrix>::value,std::unique_ptr<Matrix> >::type getPointer() const
    {
      return std::unique_ptr<Matrix>(new Matrix(A));
    }

    /// Access matrix via unique_ptr. Use if Matrix does not support move-semantics.
    template <class OtherMatrix>
    typename std::enable_if<std::is_same<OtherMatrix,Dune::BCRSMatrix<Dune::FieldMatrix<Scalar,1,1> > >::value,std::unique_ptr<OtherMatrix> >::type getPointer() const
    {
      return A.toBCRS();
    }

    /// compute \f$b=b+\alpha Ax\f$
    void applyscaleadd(Scalar alpha, Domain const& x, Range& b) const
    {
//      A.usmv(alpha,x,b);
            std::vector<Scalar> tmpx, tmpb;
            domainToVector(x,tmpx);
            rangeToVector(b,tmpb);

            A.usmv(alpha,tmpx, tmpb);

            vectorToRange(tmpb,b);
    }

    void applyscaleaddTransposed(Scalar alpha, Range const& x, Domain& b) const
    {
//      A.usmv(alpha,x,b);
            std::vector<Scalar> tmpx, tmpb;
            domainToVector(x,tmpx);
            rangeToVector(b,tmpb);

            A.usmtv(alpha,tmpx, tmpb);

            vectorToRange(tmpb,b);
    }
    /// Get coefficient vector \f$ \mathrm{coefficients}\in\mathbb{K}^m\f$ from \f$y\in Y\f$.
    /**
     * Apply \f$S_Y\f$  to \f$y\in Y\f$: \f$\mathrm{coefficients}=S_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector>
    void rangeToVector(Range const& y, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(y,coefficients);
    }

    /// Get coefficients vector \f$\mathrm{coefficients}\in\mathbb{K}^n\f$ from \f$x\in X\f$.
    /**
     * Apply \f$S_X\f$  to \f$x\in X\f$: \f$\mathrm{coefficients}=S_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector>
    void domainToVector(Domain const& x, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(x, coefficients);
    }

    /// Get \f$x\in X\f$ from coefficients vector \f$\mathrm{coefficients}\in\mathbb{K}^n\f$
    /**
     * Apply \f$S^{-1}_X\f$ to \f$\mathrm{coefficients}\f$: \f$x=S^{-1}_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToDomain(Vector const& coefficients, Domain& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    /// Get \f$y\in Y\f$ from coefficient vector \f$\mathrm{coefficients}\in\mathbb{K}^m\f$
    /**
     * Apply \f$S^{-1}_Y\f$ to \f$\mathrm{coefficients}\f$: \f$x=S^{-1}_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToRange(Vector const& coefficients, Range& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    MatrixRepresentedOperator& operator=(MatrixRepresentedOperator const& other) = default;

    MatrixRepresentedOperator& operator=(Matrix const& B)
    {
      A = B;   return *this;
    }

    MatrixRepresentedOperator& operator*= (Scalar alpha)
    {
      A *= alpha;   return *this;
    }

    MatrixRepresentedOperator& operator/= (Scalar alpha)
    {
      A /= alpha;   return *this;
    }

    MatrixRepresentedOperator& operator+= (MatrixRepresentedOperator const& B)
    {
      A += B;   return *this;
    }

    MatrixRepresentedOperator& operator+= (Matrix const& B)
    {
      A += B;   return *this;
    }

    MatrixRepresentedOperator& operator-= (MatrixRepresentedOperator const& B)
    {
      A -= B;   return *this;
    }

    MatrixRepresentedOperator& operator-= (Matrix const& B)
    {
      A -= B;   return *this;
    }

  private:
    Matrix A;
  };

  template <class MatrixBlock, class Allocator, class Domain_, class Range_>
  class MatrixRepresentedOperator<Dune::BCRSMatrix<MatrixBlock,Allocator>,Domain_,Range_>
      : public Dune::LinearOperator<Domain_,Range_>
  {
  public:
    typedef Dune::BCRSMatrix<MatrixBlock,Allocator> Matrix;
    typedef Domain_ Domain;
    typedef Range_ Range;
    typedef typename GetScalar<Domain>::type Scalar;

    static int const category = Dune::SolverCategory::sequential;

    MatrixRepresentedOperator() = default;

    explicit MatrixRepresentedOperator(Matrix const& A_) : A(A_)
    {}

    explicit MatrixRepresentedOperator(Matrix&& A_) : A(A_)
    {}

    MatrixAsTriplet<Scalar> getTriplet() const { return get<MatrixAsTriplet<Scalar> >(); }

    /// Access matrix
    template <class OtherMatrix=Matrix>
    typename std::enable_if<std::is_same<Matrix,OtherMatrix>::value, Matrix const&>::type get() const { return A; }

    Matrix& get_non_const() { return A; }

    /// Access matrix in another matrix format specified by OtherMatrix.
    /**
     * Note that OtherMatrix must be constructible from Matrix.
     */
    template <class OtherMatrix>
    typename std::enable_if<!std::is_same<Matrix,OtherMatrix>::value, OtherMatrix>::type get() const { return OtherMatrix(A); }

    /// compute \f$b=Ax\f$
    void apply(Domain const& x, Range& b) const
    {
      A.mv(x,b);
    }

    void applyTransposed(Range const& x, Domain& b) const
    {
      A.mtv(x,b);
    }

    /// Access matrix via unique_ptr. Use if Matrix does not support move-semantics.
    template <class OtherMatrix=Matrix>
    typename std::enable_if<std::is_same<OtherMatrix,Matrix>::value,std::unique_ptr<Matrix> >::type getPointer() const
    {
      return std::unique_ptr<Matrix>(new Matrix(A));
    }

    /// Access matrix via unique_ptr. Use if Matrix does not support move-semantics.
    template <class OtherMatrix>
    typename std::enable_if<std::is_same<OtherMatrix,Dune::BCRSMatrix<Dune::FieldMatrix<Scalar,1,1> > >::value,std::unique_ptr<OtherMatrix> >::type getPointer() const
    {
      return A.toBCRS();
    }

    /// compute \f$b=b+\alpha Ax\f$
    void applyscaleadd(Scalar alpha, Domain const& x, Range& b) const
    {
      A.usmv(alpha,x,b);
    }

    void applyscaleaddTransposed(Scalar alpha, Range const& x, Domain& b) const
    {
      A.usmtv(alpha,x,b);
    }

    /// Get coefficient vector \f$ \mathrm{coefficients}\in\mathbb{K}^m\f$ from \f$y\in Y\f$.
    /**
     * Apply \f$S_Y\f$  to \f$y\in Y\f$: \f$\mathrm{coefficients}=S_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector> void rangeToVector(Range const& y, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(y,coefficients);
    }

    /// Get coefficients vector \f$\mathrm{coefficients}\in\mathbb{K}^n\f$ from \f$x\in X\f$.
    /**
     * Apply \f$S_X\f$  to \f$x\in X\f$: \f$\mathrm{coefficients}=S_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector> void domainToVector(Domain const& x, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(x, coefficients);
    }

    /// Get \f$x\in X\f$ from coefficients vector \f$\mathrm{coefficients}\in\mathbb{K}^n\f$
    /**
     * Apply \f$S^{-1}_X\f$ to \f$\mathrm{coefficients}\f$: \f$x=S^{-1}_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToDomain(Vector const& coefficients, Domain& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    /// Get \f$y\in Y\f$ from coefficient vector \f$\mathrm{coefficients}\in\mathbb{K}^m\f$
    /**
     * Apply \f$S^{-1}_Y\f$ to \f$\mathrm{coefficients}\f$: \f$x=S^{-1}_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToRange(Vector const& coefficients, Range& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    MatrixRepresentedOperator& operator=(MatrixRepresentedOperator const& other) = default;

    MatrixRepresentedOperator& operator=(Matrix const& B)
    {
      A = B;   return *this;
    }

    MatrixRepresentedOperator& operator*= (Scalar alpha)
    {
      A *= alpha;   return *this;
    }

    MatrixRepresentedOperator& operator/= (Scalar alpha)
    {
      A /= alpha;   return *this;
    }

    MatrixRepresentedOperator& operator+= (MatrixRepresentedOperator const& B)
    {
      A += B;   return *this;
    }

    MatrixRepresentedOperator& operator+= (Matrix const& B)
    {
      A += B;   return *this;
    }

    MatrixRepresentedOperator& operator-= (MatrixRepresentedOperator const& B)
    {
      A -= B;   return *this;
    }

    MatrixRepresentedOperator& operator-= (Matrix const& B)
    {
      A -= B;   return *this;
    }

  private:
    Matrix A;
  };

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

  /// \cond internals
  namespace IstlInterfaceDetail {

    template <class Assembler,
              int firstRow, int lastRow,
              int firstCol, int lastCol,
              bool symmetric=false>
    struct Base
    {
      typedef typename Assembler::AnsatzVariableSet::template CoefficientVectorRepresentation<firstCol,lastCol>::type Domain;
      typedef typename Assembler::TestVariableSet::template CoefficientVectorRepresentation<firstRow,lastRow>::type   Range;

      static bool const inferredSymmetry = (lastRow-firstRow == lastCol-firstCol)              // make sure the number of domain/range variables is the same
                                                &&
                                                ((Assembler::Functional::type==VariationalFunctional       // a diagonally-centered block of a symmetric block matrix
                                                    && firstRow==firstCol && lastRow==lastCol)
                                                    || (firstRow+1==lastRow && firstCol+1==lastCol && false)); /* true if this one block is symmetric */  // TODO: implement the symmetry check

      typedef typename std::conditional<symmetric,
          SymmetricLinearOperator<Domain,Range>,
          Dune::LinearOperator<Domain,Range>>::type type;
    };
  } // end of namespace IstlInterfaceDetail
  /// \endcond

  /**
   * \ingroup linalg
   * \brief An adaptor presenting a Dune::LinearOperator <domain_type,range_type> interface to a
   * contiguous sub-blockmatrix of an assembled heterogeneous Galerkin operator.
   *
   * \tparam Assembler the \ref VariationalFunctionalAssembler containing the Galerkin matrix
   * \tparam firstRow the first block row to be included
   * \tparam lastRow one behind the final row to be included (half-open range)
   * \tparam firstCol the first block column to be included
   * \tparam lastCol one behind the final column to be included (half-open range)
   * \tparam BlockFilter a boost::fusion predicate working on the assembler's matrix blocks, defining which blocks to include
   * \tparam symmetric whether the resulting operator is symmetric. The default is a conservative check whether the operator is symmetric or not.
   *
   * Due to data sharing, the GalerkinOperator is still valid
   * even if the referenced assembler is deleted or otherwise
   * invalidated. However, reassembling without invalidation of the
   * assembler's data structures also modifies the values in the
   * GalerkinOperator.
   */
  template <class Assembler_,
            int firstRow=0, int lastRow=Assembler_::TestVariableSet::noOfVariables,
            int firstCol=0, int lastCol=Assembler_::AnsatzVariableSet::noOfVariables,
            class BlockFilter=IstlInterfaceDetail::RangeBlockSelector<firstRow,lastRow,firstCol,lastCol>,
            bool symmetric=IstlInterfaceDetail::Base<Assembler_,firstRow,lastRow,firstCol,lastCol>::inferredSymmetry>
  class AssembledGalerkinOperator: public IstlInterfaceDetail::Base<Assembler_,firstRow,lastRow,firstCol,lastCol,symmetric>::type
  {
  public:
    typedef typename IstlInterfaceDetail::Base<Assembler_,firstRow,lastRow,firstCol,lastCol,symmetric>::type Base;
    typedef Assembler_ Assembler;
    typedef typename Assembler::AnsatzVariableSet::template CoefficientVectorRepresentation<firstCol,lastCol>::type Domain;
    typedef typename Assembler::TestVariableSet::template CoefficientVectorRepresentation<firstRow,lastRow>::type   Range;
    typedef typename Assembler::Scalar                                                                              Scalar;

    typedef IstlInterfaceDetail::BlockInfo<firstRow,lastRow,firstCol,lastCol> BlockInfo;

    // do we need this?
    static int const category = Dune::SolverCategory::sequential;

    /**
     * \brief Constructor.
     * \param op the variational assembler.
     * \param onlyLowerTriangle if true, only the lower triangle is returned if a matrix is requested
     * \param nThreads use given number of threads (or a machine-dependent default if nThreads<=0).
     */
    explicit AssembledGalerkinOperator(Assembler const& assembler_, bool onlyLowerTriangle_=false, int nThreads_=0):
        onlyLowerTriangle(onlyLowerTriangle_),
        assembler(assembler_),
        blocks(boost::fusion::transform(boost::fusion::filter_if<BlockFilter>(IstlInterfaceDetail::AllBlocks<Assembler>::apply(assembler)),
                                        IstlInterfaceDetail::Translate<firstRow,firstCol>(nThreads_))),
        nThreads(nThreads_)
    { }

    virtual ~AssembledGalerkinOperator(){}

    /// update operator if grid has changed or assemble(...) has been called.
    void update()
    {
      blocks = boost::fusion::transform(boost::fusion::filter_if<BlockFilter>(IstlInterfaceDetail::AllBlocks<Assembler>::apply(assembler)),
                                        IstlInterfaceDetail::Translate<firstRow,firstCol>(nThreads));
    }

    virtual MatrixAsTriplet<Scalar> getTriplet() const { return get<MatrixAsTriplet<Scalar> >(); }

    /// Access matrix. Use if Matrix does support move-semantics.
    template <class Matrix>
    Matrix get() const
    {
      return assembler.template get<Matrix>(onlyLowerTriangle, firstRow, lastRow, firstCol, lastCol);
    }

    /// Access matrix via unique_ptr. Use if Matrix does not support move-semantics.
    template <class Matrix>
    std::unique_ptr<Matrix> getPointer() const
    {
      return assembler.template getPointer<Matrix,firstRow,lastRow,firstCol,lastCol>(onlyLowerTriangle);
    }

    /**
     * \brief returns a reference to the matrix
     */
    MatrixAsTriplet<Scalar> getmat() const __attribute__((deprecated));

    /// Get coefficient vector \f$coefficients\in\mathbb{K}^m\f$ from \f$y\in Y\f$.
    /**
     * Apply \f$S_Y\f$  to \f$y\in Y\f$: \f$coefficients=S_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector>
    void rangeToVector(Range const& y, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(y, coefficients);
    }

    /// Get coefficients vector \f$coefficients\in\mathbb{K}^n\f$ from \f$x\in X\f$.
    /**
     * Apply \f$S_X\f$  to \f$x\in X\f$: \f$coefficients=S_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector>
    void domainToVector(Domain const& x, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(x, coefficients);
    }

    /// Get \f$x\in X\f$ from coefficients vector \f$coefficients\in\mathbb{K}^n\f$
    /**
     * Apply \f$S^{-1}_X\f$ to \f$coefficients\f$: \f$x=S^{-1}_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToDomain(Vector const& coefficients, Domain& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    /**
     * \brief Get \f$ y\in Y \f$ from coefficient vector \f$coefficients\in\mathbb{K}^m\f$
     * Apply \f$S^{-1}_Y\f$ to \f$coefficients\f$: \f$x=S^{-1}_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToRange(Vector const& coefficients, Range& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    /// compute \f$ b \leftarrow Ax \f$
    virtual void apply(Domain const& x, Range& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      boost::fusion::for_each(blocks,ApplyScaleAdd(assembler,1.0,x,b,true));
    }

    void applyTransposed(Range const& x, Domain& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      boost::fusion::for_each(blocks,TransposedApplyScaleAdd(assembler,1.0,x,b,true));
    }

    /**
     * \brief Computes \f$ b \leftarrow Ax \f$ and, in case \f$ A \f$ is symmetric, also \f$ \langle x, b \rangle \f$
     *
     * If \f$ A \f$ is not symmetric, zero is returned.
     */
    virtual Scalar applyDp(Domain const& x, Range& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      Scalar dp = 0;
      boost::fusion::for_each(blocks,ApplyScaleAdd(assembler,1.0,x,b,true,&dp));
      return symmetric? dp: 0;
    }

    virtual Scalar dp(Domain const& x, Range const& y) const
    {
      std::cerr << "AssembledGalerkinOperator::dp not yet implemented!\n"; abort();
      return 0; // TODO: fixme
    }

    /**
     * \brief  Compute \f$b=b+\alpha Ax\f$
     * Note that x and b must not refer to the same memory locations (in case Domain==Range).
     */
    virtual void applyscaleadd(Scalar alpha, Domain const& x, Range& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      boost::fusion::for_each(blocks,ApplyScaleAdd(assembler,alpha,x,b,false));
    }

    virtual void applyscaleaddTransposed(Scalar alpha, Range const& x, Domain& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      boost::fusion::for_each(blocks,TransposedApplyScaleAdd(assembler,alpha,x,b,false));
    }

    /**
     * \brief Provides access to the underlying assembler.
     */
    Assembler const& getAssembler() const
    {
      return assembler;
    }

    bool getOnlyLowerTriangle() const { return onlyLowerTriangle; }

  protected:
    typedef typename boost::fusion::result_of::as_vector<
        typename IstlInterfaceDetail::AllBlocks<Assembler>::result
        >::type allblocks;

    typedef typename boost::fusion::result_of::as_vector<
        typename boost::fusion::result_of::filter_if<allblocks,BlockFilter>::type
        >::type filtered;

    bool onlyLowerTriangle;
    Assembler const& assembler;

    typename boost::fusion::result_of::as_vector<
    typename boost::fusion::result_of::transform<filtered,
    IstlInterfaceDetail::Translate<firstRow,firstCol>>::type
    >::type blocks;
    bool isSymmetric;

    struct ApplyScaleAdd
    {
      Assembler const& assembler;
      // Constructs the functor for matrix-vector multiplication and addition. If init is true, the first block
      // encountered in each row is called with apply (using alpha=1 and initializing the rhs to zero), otherwise
      // applyscaleadd is used. This prevents a double access of the right hand side, first for setting to zero,
      // then for adding the MV product.
      ApplyScaleAdd(Assembler const& as, Scalar a_, Domain const& x_, Range& y_, bool init, Scalar* dp_=0): assembler(as), a(a_), x(x_), y(y_), dp(dp_? dp_: &dummy)
      {
        for (int i=0; i<lastRow; ++i)
          doInit[i] = init;
        *dp = 0;
      }

      template <class Block>
      void operator()(Block const& b) const {
        using namespace boost::fusion;

        typename result_of::at_c<typename Domain::Functions const,Block::colId>::type xi = at_c<Block::colId>(x.data);
        typename result_of::at_c<typename Range::Functions,Block::rowId>::type  yi = at_c<Block::rowId>(y.data);
        if (!b.threadedMatrix.get())
          abort();

        if (doInit[Block::rowId])
        {
          *dp += b.threadedMatrix->mv(xi,yi);
          doInit[Block::rowId] = false;
        }
        else
          *dp += b.threadedMatrix->usmv(a,xi,yi);

        return;
      }

    private:
      Scalar a;
      Domain const& x;
      Range& y;
      Scalar* dp;
      Scalar dummy;                 // something to point to in case no target pointer has been provided
      mutable bool doInit[lastRow]; // ugly, but boost::fusion::for_each only works with const functors
    };

    struct TransposedApplyScaleAdd
    {
      Assembler const& assembler;
      // Constructs the functor for matrix-vector multiplication and addition. If init is true, the first block
      // encountered in each row is called with apply (using alpha=1 and initializing the rhs to zero), otherwise
      // applyscaleadd is used. This prevents a double access of the right hand side, first for setting to zero,
      // then for adding the MV product.
      TransposedApplyScaleAdd(Assembler const& as, Scalar a_, Range const& x_, Domain& y_, bool init, Scalar* dp_=0): assembler(as), a(a_), x(x_), y(y_), dp(dp_? dp_: &dummy)
      {
        for (int i=0; i<lastCol; ++i)
          doInit[i] = init;
        *dp = 0;
      }

      template <class Block>
      void operator()(Block const& b) const {
        using namespace boost::fusion;

        typename result_of::at_c<typename Range::Functions const,Block::rowId>::type xi = at_c<Block::rowId>(x.data);
        typename result_of::at_c<typename Domain::Functions,Block::colId>::type  yi = at_c<Block::colId>(y.data);
        if (!b.threadedMatrix.get())
          abort();

        if (doInit[Block::colId])
        {
          *dp += b.threadedMatrix->mv(xi,yi);
          doInit[Block::colId] = false;
        }
        else
          *dp += b.threadedMatrix->usmv(a,xi,yi);

        return;
      }

    private:
      Scalar a;
      Range const& x;
      Domain& y;
      Scalar* dp;
      Scalar dummy;                 // something to point to in case no target pointer has been provided
      mutable bool doInit[lastCol]; // ugly, but boost::fusion::for_each only works with const functors
    };

    int nThreads;
  };

  /**
   * \todo docme
   */
  template <class NormalStepAssembler, class TangentialStepAssembler, int stateId=1, int controlId=0, int adjointId=2>
  class LagrangeOperator: public Dune::LinearOperator<typename NormalStepAssembler::AnsatzVariableSet::template CoefficientVectorRepresentation<>::type,typename NormalStepAssembler::TestVariableSet::template CoefficientVectorRepresentation<>::type>
  {
    typedef typename NormalStepAssembler::AnsatzVariableSet::template CoefficientVectorRepresentation<>::type Domain;
    typedef typename NormalStepAssembler::TestVariableSet::template CoefficientVectorRepresentation<>::type Range;
    static constexpr int firstRow = 0;
    static constexpr int firstCol = 0;
    static constexpr int lastRow = NormalStepAssembler::TestVariableSet::noOfVariables;
    static constexpr int lastCol = NormalStepAssembler::AnsatzVariableSet::noOfVariables;
    typedef IstlInterfaceDetail::RangeBlockSelector<firstRow,lastRow,firstCol,lastCol> NormalStepBlockFilter;
    typedef IstlInterfaceDetail::RangeBlockSelector<firstRow,adjointId,firstCol,adjointId> TangentialStepBlockFilter;

  public:
    typedef typename NormalStepAssembler::Scalar Scalar;
    typedef MatrixAsTriplet<Scalar> Triplet;

    typedef IstlInterfaceDetail::BlockInfo<firstRow,lastRow,firstCol,lastCol> BlockInfo;

    static int const category = Dune::SolverCategory::sequential;

    /**
     * \param op the variational assembler.
     * \param onlyLowerTriangle
     * \param nThreads use given number of threads (or a machine-dependent default if nThreads<=0).
     */
    LagrangeOperator(NormalStepAssembler const& normalStepAssembler_, TangentialStepAssembler const& tangentialStepAssembler_,  bool onlyLowerTriangle_=false, int nThreads_=0):
      onlyLowerTriangle(onlyLowerTriangle_),
      normalStepAssembler(normalStepAssembler_), tangentialStepAssembler(tangentialStepAssembler_),
      normalStepBlocks(boost::fusion::transform(boost::fusion::filter_if<NormalStepBlockFilter>(IstlInterfaceDetail::AllBlocks<NormalStepAssembler>::apply(normalStepAssembler)),IstlInterfaceDetail::Translate<firstRow,firstCol>(nThreads_))),
      tangentialStepBlocks(boost::fusion::transform(boost::fusion::filter_if<TangentialStepBlockFilter>(IstlInterfaceDetail::AllBlocks<TangentialStepAssembler>::apply(tangentialStepAssembler)),IstlInterfaceDetail::Translate<firstRow,firstCol>(nThreads_))),
      nThreads(nThreads_)
    {}

    virtual ~LagrangeOperator(){}

    /// update operator if grid has changed or assemble(...) has been called.
    void update()
    {
      assert(false);
      //      blocks = boost::fusion::transform(boost::fusion::filter_if<BlockFilter>(IstlInterfaceDetail::AllBlocks<Assembler>::apply(assembler)),
      //          IstlInterfaceDetail::Translate<firstRow,firstCol>(nThreads));
    }

    virtual MatrixAsTriplet<Scalar> getTriplet() const
    {
      std::vector<size_t> offset(3,0);
      Triplet result, tmp;
      for(size_t i=0; i<3; ++i)
      {
        for(size_t j=0; j<3; ++j)
        {
          if(i==adjointId || j== adjointId || (i==controlId && j==controlId)) tmp = normalStepAssembler.template get<Triplet>(onlyLowerTriangle,i,i+1,j,j+1);
          else tmp = tangentialStepAssembler.template get<Triplet>(onlyLowerTriangle,i,i+1,j,j+1);
          tmp.shiftIndices(offset[i],offset[j]);
          result += tmp;
          if(i==0)
          {
            if(j==0) offset[1] = tmp.nrows();
            if(j==1) offset[2] = tmp.nrows();
          }
        }
      }

      return result;
    }

    //    /// Access matrix. Use if Matrix does support move-semantics.
    //    template <class Matrix> Matrix get() const
    //    {
    //      return assembler.template get<Matrix>(onlyLowerTriangle, firstRow, lastRow, firstCol, lastCol);
    //    }

    //    /// Access matrix via unique_ptr. Use if Matrix does not support move-semantics.
    //    template <class Matrix> std::unique_ptr<Matrix> getPointer() const
    //        {
    //      return assembler.template getPointer<Matrix,firstRow,lastRow,firstCol,lastCol>(onlyLowerTriangle);
    //  }

    /**
     * \brief returns a reference to the matrix
     */
    //    MatrixAsTriplet<Scalar> getmat() const __attribute__((deprecated));

    /// Get coefficient vector \f$coefficients\in\mathbb{K}^m\f$ from \f$y\in Y\f$.
    /**
     * Apply \f$S_Y\f$  to \f$y\in Y\f$: \f$coefficients=S_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector> void rangeToVector(Range const& y, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(y, coefficients);
    }

    /// Get coefficients vector \f$coefficients\in\mathbb{K}^n\f$ from \f$x\in X\f$.
    /**
     * Apply \f$S_X\f$  to \f$x\in X\f$: \f$coefficients=S_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::iterator std::begin(Vector&);
     */
    template <class Vector> void domainToVector(Domain const& x, Vector& coefficients) const
    {
      IstlInterfaceDetail::toVector(x, coefficients);
    }

    /// Get \f$x\in X\f$ from coefficients vector \f$coefficients\in\mathbb{K}^n\f$
    /**
     * Apply \f$S^{-1}_X\f$ to \f$coefficients\f$: \f$x=S^{-1}_X(x)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToDomain(Vector const& coefficients, Domain& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    /**
     * \brief Get \f$ y\in Y \f$ from coefficient vector \f$coefficients\in\mathbb{K}^m\f$
     * Apply \f$S^{-1}_Y\f$ to \f$coefficients\f$: \f$x=S^{-1}_Y(y)\f$.
     *
     * The used vector type Vector must provide:
     * - its iterator type via typename Vector::const_iterator
     * - (possibly overloads of) the free functions:
     *    - typename Vector::const_iterator std::begin(Vector const&);
     */
    template <class Vector>
    void vectorToRange(Vector const& coefficients, Range& x) const
    {
      IstlInterfaceDetail::fromVector(coefficients, x);
    }

    /// compute \f$ b \leftarrow Ax \f$
    virtual void apply(Domain const& x, Range& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      boost::fusion::for_each(normalStepBlocks,NormalStepApplyScaleAdd(normalStepAssembler,1.0,x,b,true));
      boost::fusion::for_each(tangentialStepBlocks,TangentialStepApplyScaleAdd(tangentialStepAssembler,1.0,x,b,false));
    }

    /**
     * \brief Computes \f$ b \leftarrow Ax \f$ and, in case \f$ A \f$ is symmetric, also \f$ \langle x, b \rangle \f$
     *
     * If \f$ A \f$ is not symmetric, zero is returned.
     */
    virtual Scalar applyDp(Domain const& x, Range& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      std::cerr << "AssembledGalerkinOperator::dp not yet implemented!\n"; abort();
      return 0;
    }

    virtual Scalar dp(Domain const& x, Range const& y) const
    {
      std::cerr << "AssembledGalerkinOperator::dp not yet implemented!\n"; abort();
      return 0; // TODO: fixme
    }

    /**
     * \brief  Compute \f$b=b+\alpha Ax\f$
     * Note that x and b must not refer to the same memory locations (in case Domain==Range).
     */
    virtual void applyscaleadd(Scalar alpha, Domain const& x, Range& b) const
    {
      assert(static_cast<void const*>(&x) != static_cast<void const*>(&b));
      boost::fusion::for_each(normalStepBlocks,NormalStepApplyScaleAdd(normalStepAssembler,1.0,x,b,false));
      boost::fusion::for_each(tangentialStepBlocks,TangentialStepApplyScaleAdd(tangentialStepAssembler,1.0,x,b,false));
    }

    /**
     * \brief Provides access to the underlying assembler.
     */
    //    Assembler const& getAssembler() const
    //    {
    //      return assembler;
    //    }
    //
    //    bool getOnlyLowerTriangle() const { return onlyLowerTriangle; }

  protected:
    typedef typename IstlInterfaceDetail::AllBlocks<NormalStepAssembler>::result AllNormalStepBlocksSeq;
    typedef typename IstlInterfaceDetail::AllBlocks<TangentialStepAssembler>::result AllTangentialStepBlocksSeq;
    typedef typename boost::fusion::result_of::filter_if<AllNormalStepBlocksSeq,NormalStepBlockFilter>::type FilteredNormalStepBlocksSeq;
    typedef typename boost::fusion::result_of::filter_if<AllTangentialStepBlocksSeq,TangentialStepBlockFilter>::type FilteredTangentialStepBlocksSeq;

    typedef typename boost::fusion::result_of::as_vector<AllNormalStepBlocksSeq>::type AllNormalStepBlocks;
    typedef typename boost::fusion::result_of::as_vector<AllTangentialStepBlocksSeq>::type AllTangentialStepBlocks;
    typedef typename boost::fusion::result_of::as_vector<FilteredNormalStepBlocksSeq>::type FilteredNormalStepBlocks;
    typedef typename boost::fusion::result_of::as_vector<FilteredTangentialStepBlocksSeq>::type FilteredTangentialStepBlocks;

    typedef typename boost::fusion::result_of::as_vector<typename boost::fusion::result_of::transform<FilteredNormalStepBlocks,IstlInterfaceDetail::Translate<firstRow,firstCol>>::type>::type NormalBlocks;
    typedef typename boost::fusion::result_of::as_vector<typename boost::fusion::result_of::transform<FilteredTangentialStepBlocks,IstlInterfaceDetail::Translate<0,0>>::type>::type TangentialBlocks;

    bool onlyLowerTriangle;
    NormalStepAssembler const& normalStepAssembler;
    TangentialStepAssembler const& tangentialStepAssembler;

    NormalBlocks  normalStepBlocks;
    TangentialBlocks tangentialStepBlocks;

    bool isSymmetric;
    int nThreads;

    struct NormalStepApplyScaleAdd
    {
      NormalStepAssembler const& assembler;
      // Constructs the functor for matrix-vector multiplication and addition. If init is true, the first block
      // encountered in each row is called with apply (using alpha=1 and initializing the rhs to zero), otherwise
      // applyscaleadd is used. This prevents a double access of the right hand side, first for setting to zero,
      // then for adding the MV product.
      NormalStepApplyScaleAdd(NormalStepAssembler const& as, Scalar a_, Domain const& x_, Range& y_, bool init, Scalar* dp_=0): assembler(as), a(a_), x(x_), y(y_), dp(dp_? dp_: &dummy)
      {
        for (int i=0; i<lastRow; ++i)
          doInit[i] = init;
        *dp = 0;
      }

      template <class Block>
      void operator()(Block const& b) const {
        if( ( (Block::rowId==stateId && Block::colId==stateId) || (Block::rowId==stateId && Block::colId==controlId) || (Block::colId==stateId && Block::rowId==controlId) ) ) return;
//        if( (Block::rowId==stateId && Block::colId==stateId) ) return;
        using namespace boost::fusion;

        typename result_of::at_c<typename Domain::Functions const,Block::colId>::type xi = at_c<Block::colId>(x.data);
        typename result_of::at_c<typename Range::Functions,Block::rowId>::type  yi = at_c<Block::rowId>(y.data);
        if (!b.threadedMatrix.get())
          abort();

        if (doInit[Block::rowId])
        {
          *dp += b.threadedMatrix->mv(xi,yi);
          doInit[Block::rowId] = false;
        }
        else
          *dp += b.threadedMatrix->usmv(a,xi,yi);

        return;
      }

    private:
      Scalar a;
      Domain const& x;
      Range& y;
      Scalar* dp;
      Scalar dummy;                 // something to point to in case no target pointer has been provided
      mutable bool doInit[lastRow]; // ugly, but boost::fusion::for_each only works with const functors
    };

    struct TangentialStepApplyScaleAdd
    {
      TangentialStepAssembler const& assembler;
      // Constructs the functor for matrix-vector multiplication and addition. If init is true, the first block
      // encountered in each row is called with apply (using alpha=1 and initializing the rhs to zero), otherwise
      // applyscaleadd is used. This prevents a double access of the right hand side, first for setting to zero,
      // then for adding the MV product.
      TangentialStepApplyScaleAdd(TangentialStepAssembler const& as, Scalar a_, Domain const& x_, Range& y_, bool init, Scalar* dp_=0): assembler(as), a(a_), x(x_), y(y_), dp(dp_? dp_: &dummy)
      {
        for (int i=0; i<lastRow; ++i)
          doInit[i] = init;
        *dp = 0;
      }

      template <class Block>
      void operator()(Block const& b) const {
        if( !( (Block::rowId==stateId && Block::colId==stateId) || (Block::rowId==stateId && Block::colId==controlId) || (Block::colId==stateId && Block::rowId==controlId) ) ) return;
        using namespace boost::fusion;

        typename result_of::at_c<typename Domain::Functions const,Block::colId>::type xi = at_c<Block::colId>(x.data);
        typename result_of::at_c<typename Range::Functions,Block::rowId>::type  yi = at_c<Block::rowId>(y.data);
        if (!b.threadedMatrix.get())
          abort();

        if (doInit[Block::rowId])
        {
          *dp += b.threadedMatrix->mv(xi,yi);
          doInit[Block::rowId] = false;
        }
        else
          *dp += b.threadedMatrix->usmv(a,xi,yi);

        return;
      }

    private:
      Scalar a;
      Domain const& x;
      Range& y;
      Scalar* dp;
      Scalar dummy;                 // something to point to in case no target pointer has been provided
      mutable bool doInit[lastRow]; // ugly, but boost::fusion::for_each only works with const functors
    };
  };

  template <class Operator>
  class TransposedOperator : public Dune::LinearOperator<typename Operator::Range,typename Operator::Domain>
  {
  public:
    typedef typename Operator::Scalar Scalar;
    typedef typename Operator::Range Domain;
    typedef typename Operator::Domain Range;
    typedef typename Operator::Assembler Assembler;

    explicit TransposedOperator(Operator const& A_) : A(A_.getTriplet().transpose()){}

    virtual ~TransposedOperator(){}

    virtual void apply(Domain const& x, Range& b) const
    {
      A.apply(x,b);
    }

    virtual void applyscaleadd(Scalar alpha, Domain const& x, Range& b) const
    {
      A.applyscaleadd(alpha,x,b);
    }

    MatrixAsTriplet<Scalar> getTriplet() const { return A.getTriplet(); }

  private:
    MatrixRepresentedOperator<MatrixAsTriplet<double>,Domain,Range> A;
  };

} // end of namespace Kaskade

#endif


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Geomlib_TriMeshEdgeSplit.h"

#include <iostream>
#include <vector>

#include <Eigen/Core>

#pragma warning(push, 0)
#include <igl/edge_flaps.h>
#include <igl/is_edge_manifold.h>
#include <igl/is_vertex_manifold.h>
#include <igl/is_border_vertex.h>
#pragma warning(pop)

#include <Urho3D/IO/Log.h>

#include "TriMesh.h"
#include "ConversionUtilities.h"

#pragma warning(disable : 4244)

using Urho3D::Variant;

namespace {

/*==============================================================================
ASSUMPTIONS:
Original mesh was OV, OF, OE, EMAP.
Looped over edges in OE, and edges to be split were marked.
Sizes of new vertex and face matrices were determined, and
V, F, E were initialized to the right size, matching OV, OF, OE in
their starting blocks.

e:       index into OE of original edge to split
i:       e is the ith edge to be split (starting at 0)
V, F, E: partially updated matrices before edge e is split

Outputs:
V, F, E: partially updated matrices after edge e is split
==============================================================================*/
void split_edge(
	int e,
	int count,
	int orig_vcount,
	int orig_fcount,
	int orig_ecount,
	Eigen::MatrixXf& V,
	Eigen::MatrixXi& F,
	Eigen::MatrixXi& E,
	Eigen::MatrixXi& EMAP,
	Eigen::MatrixXi& EF,
	Eigen::MatrixXi& EI
)
{
	int i = E(e, 0);
	int j = E(e, 1);

	int f0 = EF(e, 0); // face opposite e=(i-->j)
	int f1 = EF(e, 1); // face opposite e=(j-->i)

	int k = F(f0, EI(e, 0));
	int l = F(f1, EI(e, 1));

	int r = orig_vcount + count;
	V.row(r) = 0.5f * (V.row(i) + V.row(j));

	int f = orig_fcount + 2 * count;
	// update row f0
	int v = EI(e, 0);
	int ejtok = EMAP(f0, (v + 1) % 3);
	F(f0, (v + 2) % 3) = r;
	// update row f1
	int w = EI(e, 1);
	int eltoj = EMAP(f1, (w + 2) % 3);

	F(f1, (w + 1) % 3) = r;
	// fill row f
	F.row(f) = Eigen::RowVector3i(j, k, r);
	// fill row f + 1
	F.row(f + 1) = Eigen::RowVector3i(l, j, r);

	int s = orig_ecount + 3 * count;
	// update row e from e=(i-->j) to e=(i-->r)
	E(e, 1) = r;
	// fill row s with (r-->j)
	E.row(s) = Eigen::RowVector2i(r, j);
	// fill row s + 1
	E.row(s + 1) = Eigen::RowVector2i(k, r);
	// fill row s + 2
	E.row(s + 2) = Eigen::RowVector2i(r, l);

	// EF
	// EF.row(e) does not require update
	// s=(r-->j)
	EF.row(s) = Eigen::RowVector2i(f + 1, f);
	EF.row(s + 1) = Eigen::RowVector2i(f, f0);
	EF.row(s + 2) = Eigen::RowVector2i(f + 1, f1);

	if (EF(ejtok, 0) == f0) {
		EF(ejtok, 0) = f;
		EI(ejtok, 0) = 2;
	}
	else {
		EF(ejtok, 1) = f;
		EI(ejtok, 1) = 2;
	}
	if (EF(eltoj, 0) == f1) {
		EF(eltoj, 0) = f + 1;
		EI(eltoj, 0) = 2;
	}
	else {
		EF(eltoj, 1) = f + 1;
		EI(eltoj, 1) = 2;
	}

	// s used for EI
	// EI.row(e) does not require update
	EI.row(s) = Eigen::RowVector2i(1, 0); // refers to lines 65, 67 respectively
	EI.row(s + 1) = Eigen::RowVector2i(0, (v + 1) % 3);
	EI.row(s + 2) = Eigen::RowVector2i(1, (w + 2) % 3);

	// where it used to say j->k should now be r->k i.e. edge s+1
	EMAP(f0, (v + 1) % 3) = s + 1;
	// where it used to say l->j should now be l->r i.e. edge s+2
	EMAP(f1, (w + 2) % 3) = s + 2;
	// ? is j-->k
	EMAP.row(f) = Eigen::RowVector3i(s + 1, s, ejtok);
	// ? is l-->j
	EMAP.row(f + 1) = Eigen::RowVector3i(s, s + 2, eltoj);
}

void split_edges_above_length(
	const Eigen::MatrixXf& OV,
	const Eigen::MatrixXi& OF,
	Eigen::MatrixXf& NV,
	Eigen::MatrixXi& NF,
	float L
)
{
	Eigen::MatrixXi OE, EF, EI;
	Eigen::VectorXi EMAP;

	igl::edge_flaps(OF, OE, EMAP, EF, EI);
	Eigen::MatrixXi NE = OE;

	NV = OV;
	NF = OF;

	/***********************/

	std::vector<bool> border_verts = igl::is_border_vertex(NV, NF);

	/***********************/

	std::vector<int> edges_to_split;

	for (int e = 0; e < OE.rows(); ++e) {

		bool totally_interior_edge = false;
		if (
			border_verts[OE(e, 0)] == false &&
			border_verts[OE(e, 1)] == false
			)
		{
			totally_interior_edge = true;
		}

		// after we see if this is faster, convert to SQUAREDNORM!
		float len = (OV.row(OE(e, 0)) - OV.row(OE(e, 1))).norm();
		if (len > L && totally_interior_edge) {
			edges_to_split.push_back(e);
		}
	}

	int split_count = edges_to_split.size();

	/***********************/

	int orig_vcount = NV.rows();
	int orig_fcount = NF.rows();
	int orig_ecount = NE.rows();

	NV.conservativeResize(NV.rows() + split_count, Eigen::NoChange);
	NF.conservativeResize(NF.rows() + 2 * split_count, Eigen::NoChange);
	NE.conservativeResize(NE.rows() + 3 * split_count, Eigen::NoChange);
	EF.conservativeResize(EF.rows() + 3 * split_count, Eigen::NoChange);
	EI.conservativeResize(EI.rows() + 3 * split_count, Eigen::NoChange);

	Eigen::MatrixXi NEMAP(NF.rows(), 3);
	NEMAP.block(0, 0, OF.rows(), 1) = EMAP.block(0, 0, OF.rows(), 1);
	NEMAP.block(0, 1, OF.rows(), 1) = EMAP.block(OF.rows(), 0, OF.rows(), 1);
	NEMAP.block(0, 2, OF.rows(), 1) = EMAP.block(2 * OF.rows(), 0, OF.rows(), 1);

	for (int count = 0; count < edges_to_split.size(); ++count) {
		int e = edges_to_split[count];
		split_edge(
			e,
			count,
			orig_vcount,
			orig_fcount,
			orig_ecount,
			NV,
			NF,
			NE,
			NEMAP,
			EF,
			EI
		);
	}
}

} // namespace

Urho3D::Variant Geomlib::TriMesh_SplitLongEdges(
	const Urho3D::Variant& tri_mesh,
	float split_threshold
)
{
	if (!TriMesh_Verify(tri_mesh)) {
		return Variant();
	}

	Eigen::MatrixXf V, NV;
	Eigen::MatrixXi F, NF;
	bool igl_success = IglMeshToMatrices(tri_mesh, V, F);
	if (!igl_success) {
		return Variant();
	}

	if (!igl::is_edge_manifold(F)) {
		return Variant();
	}

	Eigen::VectorXi B;
	if (!igl::is_vertex_manifold(F, B)) {
		URHO3D_LOGINFO("TriMeshEdgeSplit --- not a vertex manifold, exiting");
		return Variant();
	}

	split_edges_above_length(V, F, NV, NF, split_threshold);

	return TriMesh_Make(NV, NF);
}


#include "RBGL.hpp"
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/wavefront.hpp>
#include <boost/graph/minimum_degree_ordering.hpp>
#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/king_ordering.hpp>

extern "C"
{
	// These are more easily done in R alone, no point to use BGL
	// copy_graph
	// transpose_graph

	SEXP BGL_isomorphism(
		SEXP num_verts_in1, SEXP num_edges_in1, SEXP R_edges_in1,
		SEXP num_verts_in2, SEXP num_edges_in2, SEXP R_edges_in2
			 )
	{
		using namespace boost;

		bool r = FALSE;

		const int NV1 = Rf_asInteger(num_verts_in1);
		const int NV2 = Rf_asInteger(num_verts_in2);
		const int NE1 = Rf_asInteger(num_edges_in1);
		const int NE2 = Rf_asInteger(num_edges_in2);

		if ( NV1 == NV2 )
		{
		typedef adjacency_list < vecS, listS, undirectedS,
			property < vertex_index_t, int> > VEGraph;

		VEGraph g1(NV1), g2(NV2);

		std::vector<graph_traits<VEGraph>::vertex_descriptor> v1(NV1), v2(NV2);
		property_map<VEGraph, vertex_index_t>::type
			v1_index_map = get(vertex_index, g1),
			v2_index_map = get(vertex_index, g2);

		graph_traits<VEGraph>::vertex_iterator i, end;
		int id = 0;
		for (tie(i, end) = vertices(g1); i != end; ++i, ++id)
		{
		      put(v1_index_map, *i, id);
		      v1[id] = *i;
		}
		id = 0;
		for (tie(i, end) = vertices(g2); i != end; ++i, ++id)
		{
		      put(v2_index_map, *i, id);
		      v2[id] = *i;
		}

		int* edges_in = INTEGER(R_edges_in1);
		for ( int i = 0; i < NE1; i++, edges_in += 2 )
			add_edge(v1[*edges_in], v1[*(edges_in+1)], g1);

		edges_in = INTEGER(R_edges_in2);
		for ( int i = 0; i < NE2; i++, edges_in += 2 )
			add_edge(v2[*edges_in], v2[*(edges_in+1)], g2);

		std::vector<graph_traits<VEGraph>::vertex_descriptor> f(NV1);

		signal(SIGABRT, sigabrt_handler);
		r = isomorphism(g1, g2, isomorphism_map
			(make_iterator_property_map(f.begin(), v1_index_map, f[0])));
		}
		signal(SIGABRT, SIG_DFL);

		SEXP ansList, conn;
		PROTECT(ansList = Rf_allocVector(VECSXP,1));
		PROTECT(conn = Rf_allocVector(LGLSXP, 1));

		LOGICAL(conn)[0] = r;

		SET_VECTOR_ELT(ansList,0,conn);
		UNPROTECT(2);
		return(ansList);
	}

	SEXP BGL_cuthill_mckee_ordering(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in)
	{
		using namespace boost;

		typedef graph_traits<Graph_ud>::vertex_descriptor Vertex;
		typedef graph_traits<Graph_ud>::vertices_size_type size_type;

		const int N = Rf_asInteger(num_verts_in);
		std::vector<Vertex>    inv_perm(N);
		std::vector<size_type> perm(N);

		Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

		cuthill_mckee_ordering(g, inv_perm.rbegin(), get(vertex_color, g), make_degree_map(g));

		SEXP ansList, invpermList, robw, rbw;
		PROTECT(ansList = Rf_allocVector(VECSXP,3));
		PROTECT(invpermList = Rf_allocVector(INTSXP,N));
		PROTECT(robw = Rf_allocVector(INTSXP, 1));
		PROTECT(rbw = Rf_allocVector(INTSXP, 1));

		property_map<Graph_ud, vertex_index_t>::type index_map = get(vertex_index, g);
		std::vector<Vertex>::const_iterator i;
		int j = 0;

		for ( i = inv_perm.begin(); i != inv_perm.end(); i++ )
			INTEGER(invpermList)[j++] = index_map[*i];
		for ( size_type c = 0; c != inv_perm.size(); ++c )
			perm[index_map[inv_perm[c]]] = c;

		INTEGER(robw)[0] = bandwidth(g);
		INTEGER(rbw)[0] = bandwidth(g, make_iterator_property_map(&perm[0], index_map, perm[0]));

		SET_VECTOR_ELT(ansList,0,invpermList);
		SET_VECTOR_ELT(ansList,1,robw);
		SET_VECTOR_ELT(ansList,2,rbw);

		UNPROTECT(4);
		return(ansList);
	}

	SEXP BGL_min_degree_ordering(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in,   SEXP R_delta)
	{
		using namespace boost;

		int delta = Rf_asInteger(R_delta);
		const int NV = Rf_asInteger(num_verts_in);
		typedef graph_traits<Graph_dd>::vertex_descriptor Vertex;
		Graph_dd g(num_verts_in, num_edges_in, R_edges_in);
		std::vector<int> inverse_perm(NV, 0);
		std::vector<int> perm(NV, 0);
		std::vector<int> degree(NV, 0);
		std::vector<int> supernode_sizes(NV, 1);
		property_map<Graph_dd, vertex_index_t>::type id = get(vertex_index, g);

		minimum_degree_ordering(g,
			make_iterator_property_map(&degree[0], id, degree[0]),
			&inverse_perm[0],
			&perm[0],
			make_iterator_property_map(&supernode_sizes[0], id, supernode_sizes[0]),
			delta, id);

		SEXP ansList, invpermList, permList;
		PROTECT(ansList = Rf_allocVector(VECSXP,2));
		PROTECT(invpermList = Rf_allocVector(INTSXP,NV));
		PROTECT(permList = Rf_allocVector(INTSXP,NV));

		std::vector<int>::const_iterator i;
		int j = 0;

		for ( i = inverse_perm.begin(); i != inverse_perm.end(); i++ )
			INTEGER(invpermList)[j++] = inverse_perm[*i];

		j = 0;
		for ( i = perm.begin(); i != perm.end(); i++ )
			INTEGER(permList)[j++] = perm[*i];

		SET_VECTOR_ELT(ansList,0,invpermList);
		SET_VECTOR_ELT(ansList,1,permList);
		UNPROTECT(3);
		return(ansList);
	}

	SEXP BGL_king_ordering(SEXP num_verts_in, SEXP num_edges_in,
                         SEXP R_edges_in,   SEXP R_delta)
	{
		using namespace boost;

		//int delta = Rf_asInteger(R_delta);
		const int NV = Rf_asInteger(num_verts_in);
		typedef graph_traits<Graph_dd>::vertex_descriptor Vertex;
		Graph_dd g(num_verts_in, num_edges_in, R_edges_in);
		std::vector<int> inverse_perm(NV, 0);
		std::vector<int> perm(NV, 0);
		std::vector<int> degree(NV, 0);
		std::vector<int> supernode_sizes(NV, 1);
		property_map<Graph_dd, vertex_index_t>::type id = get(vertex_index, g);

                // TODO: fill in
/*
		king_ordering(g,
			make_iterator_property_map(&degree[0], id, degree[0]),
			&inverse_perm[0],
			&perm[0],
			make_iterator_property_map(&supernode_sizes[0], id, supernode_sizes[0]),
			delta, id);
*/

		SEXP ansList, invpermList, permList;
		PROTECT(ansList = Rf_allocVector(VECSXP,2));
		PROTECT(invpermList = Rf_allocVector(INTSXP,NV));
		PROTECT(permList = Rf_allocVector(INTSXP,NV));

		std::vector<int>::const_iterator i;
		int j = 0;

		for ( i = inverse_perm.begin(); i != inverse_perm.end(); i++ )
			INTEGER(invpermList)[j++] = inverse_perm[*i];

		j = 0;
		for ( i = perm.begin(); i != perm.end(); i++ )
			INTEGER(permList)[j++] = perm[*i];

		SET_VECTOR_ELT(ansList,0,invpermList);
		SET_VECTOR_ELT(ansList,1,permList);
		UNPROTECT(3);
		return(ansList);
	}

	SEXP BGL_sloan_ordering(SEXP num_verts_in, SEXP num_edges_in,
                     SEXP R_edges_in, SEXP R_W1, SEXP R_W2 )
	{
		using namespace boost;

		const int NV = Rf_asInteger(num_verts_in);
		const int NE = Rf_asInteger(num_edges_in);
		const int W1 = Rf_asInteger(R_W1);
		const int W2 = Rf_asInteger(R_W2);

		typedef adjacency_list< setS, vecS, undirectedS,
		    property< vertex_color_t, default_color_type,
		    property< vertex_degree_t, int,
		    property< vertex_priority_t, double > > > > IGraph;

		typedef graph_traits<IGraph>::vertex_descriptor Vertex;
		typedef graph_traits<IGraph>::vertices_size_type size_type;
		typedef std::pair<std::size_t, std::size_t> Pair;

		IGraph g(NV);

		int* edges_in = INTEGER(R_edges_in);
		for ( int i = 0; i < NE; i++, edges_in += 2 )
			add_edge(*edges_in, *(edges_in+1), g);

		property_map<IGraph, vertex_index_t>::type index_map = get(vertex_index, g);

		std::vector<Vertex>    sloan_order(NV);
		std::vector<size_type> perm(NV);

		sloan_ordering(g, sloan_order.begin(), get(vertex_color, g),
			make_degree_map(g), get(vertex_priority, g), W1, W2);

		SEXP ansList, sList, rbw, rpf, rmw, raw, rrw;
		PROTECT(ansList = Rf_allocVector(VECSXP,6));
		PROTECT(sList = Rf_allocVector(INTSXP,NV));
		PROTECT(rbw = Rf_allocVector(INTSXP, 1));
		PROTECT(rpf = Rf_allocVector(INTSXP, 1));
		PROTECT(rmw = Rf_allocVector(INTSXP, 1));
		PROTECT(raw = Rf_allocVector(REALSXP, 1));
		PROTECT(rrw = Rf_allocVector(REALSXP, 1));

		std::vector<Vertex>::const_iterator i;
		int j = 0;

		for ( i = sloan_order.begin(); i != sloan_order.end(); i++ )
			INTEGER(sList)[j++] = index_map[*i];

		for ( size_type c = 0; c != sloan_order.size(); ++c )
			perm[index_map[sloan_order[c]]] = c;

		INTEGER(rbw)[0] = bandwidth(g, make_iterator_property_map(&perm[0], index_map, perm[0]));
		INTEGER(rpf)[0] = profile(g, make_iterator_property_map(&perm[0], index_map, perm[0]));
		INTEGER(rmw)[0] = max_wavefront(g, make_iterator_property_map(&perm[0], index_map, perm[0]));
		REAL(raw)[0] = aver_wavefront(g, make_iterator_property_map(&perm[0], index_map, perm[0]));
		REAL(rrw)[0] = rms_wavefront(g, make_iterator_property_map(&perm[0], index_map, perm[0]));

		SET_VECTOR_ELT(ansList,0,sList);
		SET_VECTOR_ELT(ansList,1,rbw);
		SET_VECTOR_ELT(ansList,2,rpf);
		SET_VECTOR_ELT(ansList,3,rmw);
		SET_VECTOR_ELT(ansList,4,raw);
		SET_VECTOR_ELT(ansList,5,rrw);

		UNPROTECT(7);
		return(ansList);
	}

}


#include <ctime>

#include <boost/format.hpp>

#include "output.hpp"
#include "logger.hpp"
#include "defines.hpp"
#include "particle.hpp"
#include "simulation.hpp"

namespace sph
{

inline void output(const SPHParticle & p, std::ofstream &out, const char sep = ' ')
{
#define OUTPUT_SCALAR(name) do { out << p.name << sep; } while(0)
#define OUTPUT_VECTOR(name) do { for(int i = 0; i < DIM; ++i) out << p.name[i] << sep; } while(0)

    OUTPUT_VECTOR(pos);
    OUTPUT_VECTOR(vel);
    OUTPUT_VECTOR(acc);
    OUTPUT_SCALAR(mass);
    OUTPUT_SCALAR(dens);
    OUTPUT_SCALAR(pres);
    OUTPUT_SCALAR(ene);
    OUTPUT_SCALAR(sml);
    OUTPUT_SCALAR(id);
    OUTPUT_SCALAR(neighbor);
    OUTPUT_SCALAR(alpha);
    OUTPUT_SCALAR(gradh);
    out << std::endl;
}

Output::Output(int count)
{
    m_count = count;
    const std::string dir_name = Logger::get_dir_name();
    const std::string file_name = dir_name + "/energy.dat";
    m_out_energy.open(file_name);
    m_out_energy << "# time kinetic thermal potential total\n";
}

Output::~Output()
{
    m_out_energy.close();
}

void Output::output_particle(std::shared_ptr<Simulation> sim)
{
    const auto & particles = sim->get_particles();
    const int num = sim->get_particle_num();
    const real time = sim->get_time();

    const std::string dir_name = Logger::get_dir_name();
    const std::string file_name = dir_name + (boost::format("/%05d.dat") % m_count).str();
    std::ofstream out(file_name);
    out << "# " << time << std::endl;

    for(int i = 0; i < num; ++i) {
        output(particles[i], out);
    }
    WRITE_LOG << "write " << file_name;
    ++m_count;
}

void Output::output_energy(std::shared_ptr<Simulation> sim)
{
    const auto & particles = sim->get_particles();
    const int num = sim->get_particle_num();
    const real time = sim->get_time();

    real kinetic = 0.0;
    real thermal = 0.0;
    real potential = 0.0;

#pragma omp parallel for reduction(+: kinetic, thermal, potential)
    for(int i = 0; i < num; ++i) {
        const auto & p_i = particles[i];
        kinetic += 0.5 * p_i.mass * abs2(p_i.vel);
        thermal += p_i.mass * p_i.ene;
        potential += 0.5 * p_i.mass * p_i.phi;
    }

    const real e_k = kinetic;
    const real e_t = thermal;
    const real e_p = potential;
    const real total = e_k + e_t + e_p;

    m_out_energy << time << " " << e_k << " " << e_t << " " << e_p << " " << total << std::endl;
}

}


//   -----------------------------------------------------------------------------------------------
//    Copyright 2015 André Bergner. Distributed under the Boost Software License, Version 1.0.
//     (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//      --------------------------------------------------------------------------------------------

#include <iostream>
#include <algorithm>
#include <array>
#include <vector>
#include <boost/mpl/int.hpp>
#include <boost/mpl/min_max.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/next_prior.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/typeof/std/ostream.hpp>
#include <boost/typeof/std/iostream.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
#include <boost/proto/transform.hpp>

#include "demangle.h"
#include "tuple_tools.hpp"

//  TODO
//  √ use callable transform instead of context
//  • implement currying
//  √ operator() -> enable if # of args

namespace mpl = boost::mpl;
namespace proto = boost::proto;

// Forward declaration of the lambda expression wrapper

template<typename T>
struct lambda;

struct lambda_domain
  : proto::domain<proto::pod_generator<lambda> >
{};

namespace boost { namespace proto
{
    // boost.proto (similar to functional from the STL) requires a result
    // type or trait to be declared within function objects. This helper
    // works around this design limitation of boost.proto by automatically
    // deriving the result type by using decltype on the object itself.

    struct callable_decltype : callable
    {
        template< typename Signature >
        struct result;

        template< typename This , typename... Args >
        struct result< This( Args... ) >
        {
            using type = decltype( std::declval<This>()( std::declval<Args>()... ));
        };
    };
}}

//  ------------------------------------------------------------------------------------------------
// main part
//  ------------------------------------------------------------------------------------------------

template< typename I >  struct placeholder       { using arity = I; };
template< typename T >  struct placeholder_arity { using type = typename T::arity; };

BOOST_PROTO_DEFINE_ENV_VAR( current_input_t, current_input );
BOOST_PROTO_DEFINE_ENV_VAR( delayed_input_t, delayed_input );

namespace transforms
{
    using namespace proto;

    template< typename idx = _ >
    using delayed_placeholder = subscript< terminal<placeholder<idx>> , terminal<placeholder<_>> >;

    struct lambda_arity : or_
    <   when
        <   delayed_placeholder<>
        ,   placeholder_arity<_value(_left)>()
        >
    ,   when
        <   terminal< placeholder<_> >
        ,   placeholder_arity<_value>()
        >
    ,   when
        <   terminal<_>
        ,   mpl::int_<0>()
        >
    ,   when
        <   nary_expr<_, vararg<_>>
        ,   fold<_, mpl::int_<0>(), mpl::max<lambda_arity, _state>()>
        >
    >
    {};

    template< typename idx >
    struct static_max_delay_impl   // excapsulates template parameter, proto gets confused elsewise
    {
        struct apply : or_
        <   when
            <   delayed_placeholder<idx>
            ,   placeholder_arity< _value(_right) >()
            >
        ,   when
            <   terminal<_>
            ,   mpl::int_<0>()
            >
        ,   when
            <   nary_expr<_, vararg<_>>
            ,   fold<_, mpl::int_<0>(), mpl::max<apply, _state>()>
            >
        >
        {};
    };

    template< typename idx = _ >
    using static_max_delay = typename static_max_delay_impl<idx>::apply;

    struct place_the_holder : callable_decltype
    {
        template< typename I , typename Tuple >
        auto operator()( placeholder<I> const & , Tuple const & args ) const
        {
            return std::get<I::value-1>( args );
        }
    };

    struct place_delay : callable_decltype
    {
        template< typename I , typename J , typename Delayed_input >
        auto operator()( placeholder<I> const & , placeholder<J> const & , Delayed_input const & del_in ) const
        {
            auto const & s = std::get<I::value-1>(del_in);
            return s[s.size()-J::value];
        }
    };

    struct eval_it : or_
    <   when
        <   delayed_placeholder<>
        ,   place_delay( _value(_left) , _value(_right) , _env_var<delayed_input_t> )
        >
    ,   when
        <   terminal< placeholder<_> >
        ,   place_the_holder( _value , _env_var<current_input_t> )
        >
    ,   when
        <   _
        ,   _default< eval_it >
        >
    >
    {};

}

using  transforms :: lambda_arity;
using  transforms :: static_max_delay;
using  transforms :: eval_it;

template< typename idx , typename Expr >
using static_max_delay_t = typename boost::result_of<static_max_delay<idx>(Expr)>::type;

template< typename Expr >
using lambda_arity_t = typename boost::result_of<lambda_arity(Expr)>::type;

template< size_t arity, typename Expr >
struct delay_for_placeholder
{
    using type = decltype( std::tuple_cat( std::declval<typename delay_for_placeholder<arity-1,Expr>::type>()
                                         , std::declval<std::tuple< static_max_delay_t<mpl::int_<arity>,Expr>>>() ));
};

template< typename Expr >
struct delay_for_placeholder<0,Expr>
{
    using type = std::tuple<>;
};

//  ------------------------------------------------------------------------------------------------
// supporting tools
//  ------------------------------------------------------------------------------------------------

//  ------------------------------------------------------------------------------------------------
// lift_into_tuple  --  lifts a meta-function into a tuple,
//                      i.e. applies it on each type in tuple, returns tuple of new types

template< typename F , typename Tuple >
struct lift_into_tuple;

template< typename F , typename... Ts >
struct lift_into_tuple< F , std::tuple<Ts...> >
{
    using type = std::tuple< typename F::template apply_t<Ts>... >;
};

template< typename F , typename Tuple >
using lift_into_tuple_t = typename lift_into_tuple<F,Tuple>::type;

//  ------------------------------------------------------------------------------------------------
// to_array  --  meta-function that maps mpl::int_<N> to array<T,N>

template< typename T >
struct to_array
{
    template< typename Int >
    struct apply
    {
        using type = std::array< T , Int::value >;
    };

    template< typename Int >
    using apply_t = typename apply<Int>::type;
};

//  ------------------------------------------------------------------------------------------------
// very simple delay_line operation on std::array --> TODO should be own type static_delay_line !

struct {
    template< typename T , size_t N , typename Y >
    void operator()( std::array<T,N>& xs , Y y )
    {
        for ( size_t n = 1 ; n < xs.size() ; ++n )  xs[n-1] = xs[n];
        xs.back() = y;
    }
} rotate_push_back;

//  ------------------------------------------------------------------------------------------------
// compile  --  main function of framework
//              • takes a zignal/dsp expressions
//              • generta
//              • returns clojure
//  ------------------------------------------------------------------------------------------------

auto compile = []( auto expr )        // TODO need to define value_type for state
{
    using expr_t = decltype(expr);
    using tuple_t = typename delay_for_placeholder< lambda_arity_t<expr_t>::value, expr_t >::type;
    using state_t = lift_into_tuple_t< to_array<float> , tuple_t >;

    return [ expr , state = state_t{} ]( auto const&... xs ) mutable
    {
        auto result = eval_it{}( expr, proto::_state{}, ( current_input = std::make_tuple(xs...) , delayed_input = state ) );
        tuple_for_each( rotate_push_back, state, std::make_tuple(xs...) );
        return result;
    };
};

//  ------------------------------------------------------------------------------------------------
// compile2 -- same as compile, but return type is curryable
//  ------------------------------------------------------------------------------------------------

template
<   typename Expression
,   typename State
,   size_t   arity
>
struct stateful_lambda
{
private:

    Expression  expr_;
    State       state_;

    template< typename... Args >
    auto call_impl( mpl::int_<0> , Args const &... args ) -> decltype(auto)
    {
        auto result = eval_it{}( expr_, proto::_state{}, ( current_input = std::make_tuple(args...) , delayed_input = state_ ) );
        tuple_for_each( rotate_push_back, state_, std::make_tuple(args...) );
        return result;
    }

    template< int arg_diff , typename... Args >
    auto call_impl( mpl::int_<arg_diff> , Args const &... args ) const
    {
        return [ &args...                 // TODO should copy args into lambda
               , expr = *this ]           // TODO should not be a lambda, but a type that is
        ( auto const &... missing_args )  //      aware of the # of args (enable_if them)
        {
            return expr( args..., missing_args... );
        };
    }

public:

    stateful_lambda( Expression expr ) : expr_( std::move(expr)) {}

    template< typename... Args , typename = std::enable_if_t< sizeof...(Args) <= arity > >
    auto operator()( Args const &... args ) -> decltype(auto)
    {
        return call_impl( mpl::int_< arity - sizeof...(Args) >{} , args... );
    }
};

auto compile2 = []( auto expr )        // TODO need to define value_type for state
{
    using expr_t = decltype(expr);
    using arity_t = lambda_arity_t<expr_t>;
    using tuple_t = typename delay_for_placeholder< arity_t::value, expr_t >::type;
    using state_t = lift_into_tuple_t< to_array<float> , tuple_t >;

    return stateful_lambda< expr_t, state_t, arity_t::value>{ expr };
};

const proto::terminal< placeholder< mpl::int_<1> >>::type   _1  = {{}};
const proto::terminal< placeholder< mpl::int_<2> >>::type   _2  = {{}};
const proto::terminal< placeholder< mpl::int_<3> >>::type   _3  = {{}};
const proto::terminal< placeholder< mpl::int_<4> >>::type   _4  = {{}};
const proto::terminal< placeholder< mpl::int_<5> >>::type   _5  = {{}};

int main()
{
    std::cout << "arities -----------------" << std::endl;
    std::cout << lambda_arity{}( _1 + _2 ) << std::endl;
    std::cout << lambda_arity{}( _1[_4] + _3 ) << std::endl;
    std::cout << lambda_arity{}( _1 + _2[_1] ) << std::endl;
    std::cout << "delays -----------------" << std::endl;
    std::cout << static_max_delay<mpl::int_<1>>{}( _1 + _2 ) << std::endl;
    std::cout << static_max_delay<mpl::int_<1>>{}( _1[_4] + _3 ) << std::endl;
    std::cout << static_max_delay<mpl::int_<1>>{}( _1 + _2[_1] ) << std::endl;
    std::cout << "-------------------------" << std::endl;

    auto print_expr_state = []( auto expr )
    {
        using expr_t = decltype(expr);
        using state_t = typename delay_for_placeholder< lambda_arity_t<expr_t>::value, expr_t >::type;
        std::cout << lambda_arity_t<expr_t>::value << "  " << type_name<state_t>() << std::endl;
    };

    print_expr_state( _1 + _2 );
    print_expr_state( _1[_4] + _3 );
    print_expr_state( _1 + _2[_1] + _4[_3] );

    std::cout << "-------------------------" << std::endl;

    auto unit_delay = compile( _1[_1] );
    auto differentiator = compile2( _1 - _1[_1] );

    for ( auto x : {1,1,1,1,1} )
        std::cout << differentiator(x) << std::endl;

/*
    {  // test to check assembly output
        auto test_expr = compile( _1[_1] - _1[_2]*2.f );

        std::vector<float> xs(1000);
        {
            float u = 0.87;
            for ( auto& x : xs ) x = u *= 4.0*(1.-u);
        }

        auto acc = 0.f;
        __asm__ __volatile__("nop":::"memory");
        for ( auto x : xs )
            acc += test_expr(x);
        __asm__ __volatile__("nop":::"memory");
        std::cout << acc << std::endl;

        for ( auto x : xs )
            std::cout << test_expr(x) << std::endl;
    }
*/
}