#ifndef __POTENTIALS_HPP__
#define __POTENTIALS_HPP__

#include <initializer_list>
#include <stdexcept>
#include <vector>
#include <unordered_map>
#include <map>
#include <functional>
#include <armadillo>
#include <stdexcept>
#include <boost/range/combine.hpp>
#include "pauth_types.hpp"
#include "molecular.hpp"

namespace pauth {

/*! \brief Abstract base class for potentials
 *
 * Defines a public interface for all potentials (e.g. spring, LJ, bond-angle,
 * etc.)
 */
class abstract_potential {
public:
  virtual ~abstract_potential() = 0;

  /*! \brief Calculates total potential energy of the current configuration
   *
   * Calculates the total potential energy of the current configuration given
   * each molecule position. Complexity is N^2.
   *
   * \param    sim             Metropolis simulation object
   * \return                   Potential energy of the current configuration
   */
  inline double U(const metropolis &sim) const {
    return _U(sim);
  }

  /*! \brief Calculates the change in potential energy from a molecule moving
   *
   * Calculates the change in potential energy due to the motion of a single
   * molecule given by the index i. Note the complexity of this is N, which
   * is much more efficient than calculating the difference using `U`.
   * Requires: 0 <= i < N
   *
   * \param    sim             Metropolis simulation object
   * \param    j               Index of the molecule that moved
   * \param    rn_j            New position
   * \return                   Change in potential energy
   */
  inline double delta_U(const metropolis &sim, const size_t j,
                        arma::vec& rn_j) const {
    return _delta_U(sim, j, rn_j);
  }

  /*! \brief Calculates the force between molecules i and j
   *
   * \param    sim             Metropolis simulation object
   * \param    i               Index of the first molecule
   * \param    j               Index of the second molecule
   * \return                   Force of molecule j on molecule i
   */
  inline arma::vec forceij(const metropolis &sim, const size_t i,
                           const size_t j) const {
    return _forceij(sim, i, j);
  }

private:
  virtual double _U(const metropolis &sim) const = 0;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &rn_j) const = 0;
  virtual arma::vec _forceij(const metropolis &sim, const size_t i,
                             const size_t j) const = 0;
};

/*! Public interface for a 6-12 Lennard-Jones pairwise potential
 */
class abstract_LJ_potential : public abstract_potential {
public:
  virtual ~abstract_LJ_potential() = 0;

  /*! \brief Get the well depth of the potential (epsilon)
   *
   * \param    id1   Molecular id of the first molecule
   * \param    id2   Molecular id of the second molecule
   * \return         Depth of the potential well
   */
  inline double get_well_depth(const molecular_id id1,
                               const molecular_id id2) const {
    return _get_well_depth(id1, id2);
  }

  /*! \brief Get the distance at which the potential is zero (sigma)
   *
   * \param    id1   Molecular id of the first molecule
   * \param    id2   Molecular id of the second molecule
   * \return         Zero of the potential
   */
  inline double get_rzero(const molecular_id id1,
                          const molecular_id id2) const {
    return _get_rzero(id1, id2);
  }

private:
  virtual double _U(const metropolis &sim) const = 0;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &dx) const = 0;
  virtual arma::vec _forceij(const metropolis &sim, const size_t i,
                             const size_t j) const = 0;
  virtual double _get_well_depth(const molecular_id,
                                 const molecular_id) const = 0;
  virtual double _get_rzero(const molecular_id, const molecular_id) const = 0;
};

/*! Public interface for a 6-12 Lennard-Jones pairwise potential without cutoff
 */
class abstract_LJ_full_potential : virtual public abstract_LJ_potential {
public:
  virtual ~abstract_LJ_full_potential() = 0;

private:
  virtual double _U(const metropolis &sim) const;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &dx) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t i,
                             const size_t j) const;
};

/*! \brief 6-12 Lennard-Jones pairwise potential
 *
 * Public interface for a Lennard-Jones potential to be used in a simulation
 * where the potential parameters between every pair of molecules is loaded
 * from a YAML file and looked up at runtime.
 */
class abstract_LJ_lookup_potential : virtual public abstract_LJ_potential {
public:
  virtual ~abstract_LJ_lookup_potential() = 0;

  /*! \brief Constructor for LJ potential where well parameters are looked up
   *
   * \param       fname     Filename to well parameter data
   * \return                LJ potential with well parameter lookup
   */
  abstract_LJ_lookup_potential(const char *fname);

private:
  virtual double _get_well_depth(const molecular_id, const molecular_id) const;
  virtual double _get_rzero(const molecular_id, const molecular_id) const;

  molecular_pair_interaction_map _well_depth_map;
  molecular_pair_interaction_map _rzero_map;
};

/*! \brief 6-12 Lennard-Jones pairwise potential
 *
 * Lennard-Jones potential to be used in a simulation where the potential
 * parameters between every pair of molecules is loaded from a YAML file
 * and looked up at runtime.
 */
class LJ_potential : public abstract_LJ_full_potential,
                     public abstract_LJ_lookup_potential {
public:
  /*! \brief Constructor for LJ potential where well parameters are looked up
   *
   * \param       fname     Filename to well parameter data
   * \return                LJ potential with well parameter lookup
   */
  LJ_potential(const char *fname)
    : abstract_LJ_lookup_potential(fname) {}
};

/*! \brief 6-12 Lennard-Jones pairwise potential
 *
 * Lennard-Jones potential to be used in a simulation where the potential
 * function between every pair of molecules is the same (i.e. to be used in
 * a simulation where all of the molecules are of the same type).
 */
class const_well_params_LJ_potential : public abstract_LJ_full_potential {
public:
  /*! \brief Constructor for a LJ potential with constant well parameters
   *
   * \param    well_depth    Depth of the potential well
   * \param    rzero         Finite distance at which potential is zero
   * \return                 LJ potential
   */
  const_well_params_LJ_potential(const double well_depth, const double rzero)
      : _well_depth(well_depth), _rzero(rzero) {}

  ~const_well_params_LJ_potential() {}

private:
  virtual double _get_well_depth(molecular_id, molecular_id) const {
    return _well_depth;
  }

  virtual double _get_rzero(molecular_id, molecular_id) const { return _rzero; }

  double _well_depth;
  double _rzero;
};

/*! \brief Public interface for a 6-12 Lennard-Jones potential with a cutoff
 *
 * Public interface for a 6-12 Lennard-Jones potential with a cutoff radius
 * and periodic boundary conditions.
 */
class abstract_LJ_cutoff_potential : virtual public abstract_LJ_potential {
public:
  /*! \brief Constructor for a LJ potential with a cutoff radius
   *
   * \param   cutoff         Cutoff radius, default value is 2.5
   * \return                 LJ potential
   */
  abstract_LJ_cutoff_potential(const double cutoff = 2.5)
      : _cutoff(cutoff), _rc2(_cutoff * _cutoff),
        _rc6(_rc2 * _rc2 * _rc2), _rc7(_rc6 * _cutoff), _rc12(_rc6 * _rc6),
        _rc13(_rc6 * _rc7) {}

  virtual ~abstract_LJ_cutoff_potential() = 0;

  /*! \brief Get cutoff radius
   *
   * \return            Cutoff radius
   */
  inline double cutoff() const { return _cutoff; }

private:
  virtual double _U(const metropolis &sim) const;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &rn_j) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t i,
                             const size_t j) const;

  double _cutoff;

  double _rc2;
  double _rc6;
  double _rc7;
  double _rc12;
  double _rc13;
};

/*! \brief 6-12 Lennard-Jones pairwise potential
 *
 * Lennard-Jones potential with cutoff to be used in a simulation where the
 * parameters between every pair of molecules is loaded from a YAML file
 * and looked up at runtime.
 */
class LJ_cutoff_potential : public abstract_LJ_lookup_potential,
                            public abstract_LJ_cutoff_potential {
public:
  /*! \brief Constructor for LJ potential where well parameters are looked up
   *
   * \param       fname     Filename to well parameter data
   * \param       cutoff    Cutoff radius
   * \return                LJ potential with well parameter lookup
   */
  LJ_cutoff_potential(const char *fname, const double cutoff = 2.5)
    : abstract_LJ_lookup_potential(fname), abstract_LJ_cutoff_potential(cutoff)
      {}
};

/*! \brief 6-12 Lennard-Jones pairwise potential with a cutoff radius
 *
 * Lennard-Jones potential to be used in a simulation where the potential
 * function between every pair of molecules is the same (i.e. to be used in
 * a simulation where all of the molecules are of the same type).
 */
class const_well_params_LJ_cutoff_potential
    : public abstract_LJ_cutoff_potential {
public:
  /*! \brief Constructor for a LJ potential with constant well parameters
   *
   * \param    well_depth    Depth of the potential well
   * \param    rzero         Finite distance at which potential is zero
   * \return                 LJ potential
   */
  const_well_params_LJ_cutoff_potential(const double well_depth,
                                        const double rzero,
                                        const double cutoff = 2.5)
      : abstract_LJ_cutoff_potential(cutoff),
        _well_depth(well_depth), _rzero(rzero) {}

  ~const_well_params_LJ_cutoff_potential() {}

private:
  virtual double _get_well_depth(molecular_id, molecular_id) const {
    return _well_depth;
  }

  virtual double _get_rzero(molecular_id, molecular_id) const { return _rzero; }

  double _well_depth;
  double _rzero;
};

/*! \brief Public interface for a spring potential
 */
class abstract_spring_potential : public abstract_potential {
public:
  virtual ~abstract_spring_potential() = 0;

  /*! \brief Get the spring constant
   *
   * \param    id    Molecular id of the molecule
   * \return         Spring constant
   */
  inline double get_k(molecular_id id) const { return _get_k(id); }

private:
  virtual double _U(const metropolis &sim) const;
  virtual double  _delta_U(const metropolis &sim, const size_t j,
                           arma::vec &rn_j) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t,
                             const size_t) const;

  virtual double _get_k(molecular_id) const = 0;
};

/*! \brief Potential due to a spring with constant k
 *
 * Potential due to a spring in which the spring constant, k, is the same
 * for all molecules, regardless of molecular id.
 * k should always be greater than 0.
 */
class const_k_spring_potential : public abstract_spring_potential {
public:
  /*! \brief Constructor for potential with same spring constant for all
   * molecules
   *
   * Constructor for potential with same spring constant for all molecules.
   * Requires: k > 0
   *
   * \param    k     Spring constant
   * \return         Spring potential
   */
  const_k_spring_potential(const double k) : _k(k) {}

  virtual ~const_k_spring_potential() {}

private:
  virtual double _get_k(molecular_id) const { return _k; }

  double _k;
};

/*! \brief Potential due to a quadratic spring with constant parameters
 *
 * Potential due to a spring in which the spring parameters are the same
 * for all molecules, regardless of molecular id.
 */
class const_quad_spring_potential : public abstract_potential {
public:
  /*! \brief Constructor for potential with same spring parameters for all
   * molecules
   *
   * Constructor for potential with same spring constant for all molecules.
   *
   * \param    k     Spring constant
   * \return         Spring potential
   */
  const_quad_spring_potential(const double a, const double b, const double c)
      : a(a), b(b), c(c) {}

  virtual ~const_quad_spring_potential() {}

private:
  virtual double _U(const metropolis &sim) const;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &rn_j) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t,
                             const size_t) const;

  double a;
  double b;
  double c;
};

/*! \brief Potential due to a spring with a polynomial potential
 *
 * Potential due to a spring in which the potential, a x^n + b x^-1 + ...,
 * is the same for all molecules, regardless of molecular id.
 */
class const_poly_spring_potential : public abstract_potential {
public:
  /*! \brief Constructor for potential with same spring potential for all
   * molecules
   *
   * Constructor for potential with same spring constant for all molecules.
   *
   * \param    coeffs   Polynomial coefficients
   * \return            Spring potential
   */
  const_poly_spring_potential(const std::initializer_list<double> &coeffs);

  virtual ~const_poly_spring_potential() {}

private:
  virtual double _U(const metropolis &sim) const;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &rn_j) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t,
                             const size_t) const;

  std::vector<double> pcoeffs;
  std::vector<double> fcoeffs;
};

/*! \brief Two state potential with interactions between molecules
 */
class twostate_int_potential : public abstract_potential {
public:
  /*! Constructor for two state potential with interactions
   *
   * \param     gamma     Potential energy of state 1
   * \param     mu        Potential energy of state 2
   * \return              Two state potential with interactions
   */
  twostate_int_potential(const double gamma, const double mu) {
    _Us[0] = gamma;
    _Us[1] = mu;
  }

private:
  std::array<double, 2> _Us;

  double _U(const metropolis &sim) const;
  double _delta_U(const metropolis &sim, const size_t j,
                  arma::vec &rn_j) const;
  inline double _Ui(const arma::vec &x) const {
    return _Us[static_cast<unsigned>(x(0))];
  }
  inline bool _check_x(const arma::vec &x) const {
    return (static_cast<unsigned>(x(0)) == 0 ||
            static_cast<unsigned>(x(0)) == 1);
  }
  arma::vec _forceij(const metropolis &sim, const size_t, const size_t) const;
};

/*! \brief Potential of a dipole in an electric field
 */
class dipole_electric_potential : public abstract_potential {
public:
  /*! \brief Potential of a dipole in a (constant) electric field
   *
   * \param   E0        Magnitude of electric field
   * \param   dof_idx   Index of the degree of freedom of the E field
   * \return            Potential of a dipole in a (constant) electric field
   */
  dipole_electric_potential(const double E0, const unsigned dof_idx = 2) {
    _efield[dof_idx] = [E0](const arma::vec&) { return E0; };
  }

  /*! \brief Potential of a dipole in a (constant) electric field
   *
   * \param   E0        Magnitude of electric field
   * \param   dof_idx   Index of the degree of freedom of the E field
   * \return            Potential of a dipole in a (constant) electric field
   */
  dipole_electric_potential(const std::initializer_list<double> Es,
                            const std::initializer_list<unsigned> dof_idxs) {
    if (Es.size() != dof_idxs.size())
      throw std::invalid_argument("E field function list and dof list should "
                                  "be of the same length");

    for(auto tup : boost::combine(dof_idxs, Es)) {
      unsigned idx;
      double E;
      boost::tie(idx, E) = tup;
      _efield[idx] = [E](const arma::vec&) { return E; };
    }
  }

  /*! \brief Potential of a dipole in an electric field
   *
   * \param   Es        Components of electric field
   * \param   dox_idxs  Indexes of the degree of freedom of the E field
   * \return            Potential of a dipole in an electric field
   */
  dipole_electric_potential(std::initializer_list<
      std::function<double(const arma::vec&)> > Es,
      std::initializer_list<unsigned> dof_idxs) {

    if (Es.size() != dof_idxs.size())
      throw std::invalid_argument("E field function list and dof list should "
                                  "be of the same length");

    for(auto tup : boost::combine(dof_idxs, Es))
      _efield[boost::get<0>(tup)] = boost::get<1>(tup);

  }

private:
  virtual double _U(const metropolis &sim) const;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &dx) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t i,
                             const size_t j) const;

  std::map<unsigned, std::function<double(const arma::vec&)>> _efield;
};

/*! \brief Potential of a dipole in an electric field
 */
class abstract_dipole_strain_potential : public abstract_potential {
public:
  virtual ~abstract_dipole_strain_potential() = 0;

  /*! \brief Inverse of the susceptibility tensor
   *
   * \param   xs    Degrees of freedom of dipole
   * \param   id    Molecular id of the particle
   * \return        Inverse susceptibility
   */
  inline arma::mat inv_chi(const arma::vec& xs, const molecular_id id) const {
    return _inv_chi(xs, id);
  }

  /*! \brief Polarization vector
   *
   * \param   xs    Degrees of freedom of dipole
   * \return        Polarization vector
   */
  inline arma::vec p(const arma::vec& xs) const {
    return _p(xs);
  }

private:
  virtual double _U(const metropolis &sim) const;
  virtual double _delta_U(const metropolis &sim, const size_t j,
                          arma::vec &dx) const;
  virtual arma::vec _forceij(const metropolis &sim, const size_t,
                             const size_t) const;
  virtual arma::mat _inv_chi(const arma::vec& xs,
                             const molecular_id id) const = 0;
  virtual arma::vec _p(const arma::vec& xs) const = 0;
};

/*! \brief Dipole in 2D space
 */
class abstract_dipole_strain_2d_potential :
  virtual public abstract_dipole_strain_potential {
public:
  virtual ~abstract_dipole_strain_2d_potential()=0;
private:
  arma::vec _p(const arma::vec& xs) const {
    return arma::vec(xs.memptr(), 2);
  }
};

/*! \brief Dipole in 3D space
 */
class abstract_dipole_strain_3d_potential :
  virtual public abstract_dipole_strain_potential {
public:
  virtual ~abstract_dipole_strain_3d_potential()=0;
private:
  arma::vec _p(const arma::vec& xs) const {
    return arma::vec(xs.memptr(), 3);
  }
};

/*! \brief Dipole whose susceptibility does not depend on space, direction, etc.
 */
class abstract_dipole_strain_linear_potential :
  virtual public abstract_dipole_strain_potential {
public:
  virtual ~abstract_dipole_strain_linear_potential()=0;

  abstract_dipole_strain_linear_potential(const arma::mat& inv_chi)
    : _const_inv_chi(inv_chi) {}

private:
  arma::mat _inv_chi(const arma::vec&, const molecular_id) const {
    return _const_inv_chi;
  }

  arma::mat _const_inv_chi;
};

/*! \brief Dipole, in 2D space, whose susceptibility tensor is linear
 */
class dipole_strain_linear_2d_potential :
  public abstract_dipole_strain_2d_potential,
  public abstract_dipole_strain_linear_potential {
public:
  virtual ~dipole_strain_linear_2d_potential() {}

  dipole_strain_linear_2d_potential(const arma::mat& inv_chi)
    : abstract_dipole_strain_linear_potential(inv_chi) {}
};

/*! \brief Dipole, in 3D space, whose susceptibility tensor is linear
 */
class dipole_strain_linear_3d_potential :
  public abstract_dipole_strain_3d_potential,
  public abstract_dipole_strain_linear_potential {
public:
  virtual ~dipole_strain_linear_3d_potential() {}

  dipole_strain_linear_3d_potential(const arma::mat& inv_chi)
    : abstract_dipole_strain_linear_potential(inv_chi) {}
};

/*! \brief Dipole, in 2D space, whose susceptibility tensor is nonlinear
 */
class dipole_strain_nonlinear_2d_potential :
  public abstract_dipole_strain_2d_potential {
public:
  virtual ~dipole_strain_nonlinear_2d_potential() {}

  dipole_strain_nonlinear_2d_potential(std::function<arma::mat(const arma::vec&,
    const molecular_id)> inv_chi_f) : _inv_chi_f(inv_chi_f) {}
private:
  arma::mat _inv_chi(const arma::vec& xs, const molecular_id id) const {
    const double theta = xs(2);
    arma::vec n({std::cos(theta), std::sin(theta)});
    return _inv_chi_f(n, id);
  }

  std::function<arma::mat(const arma::vec&, const molecular_id)> _inv_chi_f;
};

/*! \brief Dipole, in 3D space, whose susceptibility tensor is nonlinear
 */
class dipole_strain_nonlinear_3d_potential :
  public abstract_dipole_strain_3d_potential {
public:
  virtual ~dipole_strain_nonlinear_3d_potential() {}

  dipole_strain_nonlinear_3d_potential(std::function<arma::mat(const arma::vec&,
    const molecular_id)> inv_chi_f) : _inv_chi_f(inv_chi_f) {}
private:
  arma::mat _inv_chi(const arma::vec& xs, const molecular_id id) const {
    const double phi = xs(3);
    const double theta = xs(4);
    const double cp = std::cos(phi);
    const double sp = std::sin(phi);
    const double ct = std::cos(theta);
    const double st = std::sin(theta);
    arma::vec n({cp * st, sp * st, ct});
    return _inv_chi_f(n, id);
  }

  std::function<arma::mat(const arma::vec&, const molecular_id)> _inv_chi_f;
};

} // namespace pauth

#endif


#pragma once
#include "spn_math.hpp"
#include <type_traits>
#include <boost/serialization/access.hpp>
#include <boost/serialization/nvp.hpp>
#include "serialization/traits.hpp"
#include "../random/rotation.hpp"

namespace spn {
	// Degree角度を示すタグ構造体
	struct Degree_t {};
	// Radian角度を示すタグ構造体
	struct Radian_t {};

	//! 角度変換ヘルパークラス
	template <class From, class To>
	struct ConvertAngle;
	// 同じ単位なので変換の必要なし
	template <class TAG>
	struct ConvertAngle<TAG, TAG> {
		template <class T>
		T operator()(T ang) const {
			return ang;
		}
	};
	template <>
	struct ConvertAngle<Degree_t, Radian_t> {
		template <class T>
		T operator()(T deg) const {
			return deg / 180 * Pi<T>;
		}
	};
	template <>
	struct ConvertAngle<Radian_t, Degree_t> {
		template <class T>
		T operator()(T rad) const {
			return rad / Pi<T> * 180;
		}
	};

	template <class TAG>
	struct AngleInfo;
	template <>
	struct AngleInfo<Degree_t> {
		template <class T>
		constexpr static T onerotation = T(360);
		const static char *name,
							*name_short;
	};
	template <>
	struct AngleInfo<Radian_t> {
		template <class T>
		constexpr static T onerotation = T(2*Pi<T>);
		const static char *name,
							*name_short;
	};

	//! 角度クラス
	/*! 混同を避けるために数値への変換は明示的
		他形式との変換は暗黙的 */
	template <class TAG, class V>
	class Angle {
		private:
			using tag_type = TAG;
			using value_type = V;
			value_type	_angle;

			friend class boost::serialization::access;
			template <class Ar>
			void serialize(Ar& ar, const unsigned int /*ver*/) {
				ar & BOOST_SERIALIZATION_NVP(_angle);
			}
		public:
			constexpr static value_type OneRotationAng = AngleInfo<tag_type>::template onerotation<value_type>;
			Angle() = default;
			//! Degreeで角度指定
			template <class TAG2, class V2>
			Angle(const Angle<TAG2,V2>& ang) {
				_angle = ConvertAngle<TAG2, TAG>()(ang.get());
			}
			//! floatで直接角度指定
			explicit constexpr Angle(value_type ang):
				_angle(ang)
			{}
			constexpr static Angle Rotation(value_type r=value_type(1)) {
				return Angle(OneRotationAng * r);
			}
			Angle& operator = (const Angle& ang) = default;
			template <class TAG2, class V2>
			Angle& operator = (const Angle<TAG2,V2>& ang) {
				_angle = Angle(ang).get();
				return *this;
			}

			// 浮動少数点数型なら明示的に角度を出力
			template <class T2, class=std::enable_if_t<std::is_floating_point<T2>::value>>
			explicit operator T2() const {
				return _angle;
			}
			template <class TAG2, class V2=value_type>
			Angle<TAG2,V2> convert() const {
				return Angle<TAG2,V2>(*this);
			}
			void set(value_type ang) {
				_angle = ang;
			}
			value_type get() const {
				return _angle;
			}
			//! 角度をループ(0から2Piの間に収める)
			void single() {
				auto ang = std::fmod(_angle, OneRotationAng);
				if(ang < 0)
					ang += OneRotationAng;
				_angle = ang;
			}
			void rangeValue(V low, V high) {
				if(_angle < low)
					_angle = low;
				else if(_angle > high)
					_angle = high;
			}
			//! 角度に制限をつける
			void range(const Angle& low, const Angle& high) {
				if(_angle < low.get())
					_angle = low.get();
				else if(_angle > high.get())
					_angle = high.get();
			}

			template <class TAG2, class V2>
			Angle operator + (const Angle<TAG2,V2>& ang) const {
				return Angle(_angle + Angle(ang).get());
			}
			template <class TAG2, class V2>
			Angle& operator += (const Angle<TAG2,V2>& ang) {
				_angle += Angle(ang).get();
				return *this;
			}
			template <class TAG2, class V2>
			Angle operator - (const Angle<TAG2,V2>& ang) const {
				return Angle(_angle - Angle(ang).get());
			}
			template <class TAG2, class V2>
			Angle& operator -= (const Angle<TAG2,V2>& ang) {
				_angle -= Angle(ang).get();
				return *this;
			}
			Angle operator * (value_type r) const {
				return Angle(_angle * r);
			}
			Angle& operator *= (value_type r) {
				_angle *= r;
				return *this;
			}
			Angle operator / (value_type r) const {
				return Angle(_angle / r);
			}
			Angle& operator /= (value_type r) {
				_angle /= r;
				return *this;
			}
			Angle operator -() const {
				return Angle(-_angle);
			}
			template <class... Ts>
			static Angle Random(Ts&&... ts) {
				return random::GenRAngle<Angle>(std::forward<Ts>(ts)...);
			}

			// ---- 比較演算子の定義 ----
			#define DEF_ANGLE_OPERATOR(op) \
				template <class TAG2, class V2> \
				bool operator op (const Angle<TAG2,V2>& ang) const { \
					return _angle op Angle(ang).get(); \
				}
			DEF_ANGLE_OPERATOR(==)
			DEF_ANGLE_OPERATOR(!=)
			DEF_ANGLE_OPERATOR(<)
			DEF_ANGLE_OPERATOR(<=)
			DEF_ANGLE_OPERATOR(>)
			DEF_ANGLE_OPERATOR(>=)
			#undef DEF_ANGLE_OPERATOR

			// -------- Luaへのエクスポート用 --------
			using Deg_t = Angle<Degree_t, value_type>;
			using Rad_t = Angle<Radian_t, value_type>;
			Angle luaAddD(const Deg_t& a) const {
				return *this + a;
			}
			Angle luaAddR(const Rad_t& a) const {
				return *this + a;
			}
			Angle luaSubD(const Deg_t& a) const {
				return *this - a;
			}
			Angle luaSubR(const Rad_t& a) const {
				return *this - a;
			}
			Deg_t luaToDegree() const {
				return *this;
			}
			Rad_t luaToRadian() const {
				return *this;
			}
			Angle luaMulF(float s) const {
				return *this * s;
			}
			Angle luaDivF(float s) const {
				return *this / s;
			}
			Angle luaInvert() const {
				return -*this;
			}
			bool luaLessthan(const Angle& a) const {
				return *this < a;
			}
			bool luaLessequal(const Angle& a) const {
				return *this <= a;
			}
			bool luaEqual(const Angle& a) const {
				return *this == a;
			}
			std::string luaToString() const {
				return ToString(*this);
			}
	};

	template <class T>
	using Degree = Angle<Degree_t, T>;
	using DegF = Degree<float>;
	using DegD = Degree<double>;
	template <class T>
	using Radian = Angle<Radian_t, T>;
	using RadF = Radian<float>;
	using RadD = Radian<double>;

	template <class TAG, class V>
	std::ostream& operator << (std::ostream& os, const Angle<TAG,V>& ang) {
		return os << ang.get() << "(" << AngleInfo<TAG>::name_short << ")";
	}
}
BOOST_CLASS_IMPLEMENTATION_TEMPLATE((class)(class), spn::Angle, object_serializable)
BOOST_CLASS_TRACKING_TEMPLATE((class)(class), spn::Angle, track_never)

// ------------- Angle系関数 -------------
namespace spn {
	template <int N, bool A>
	struct VecT;
	using Vec2 = VecT<2,false>;

	//! dirAを基準に反時計回りに増加する値を返す
	/*! \param[in] dir 値を算出したい単位ベクトル
		\param[in] dirA 基準の単位ベクトル
		\return 角度に応じた0〜4の値(一様ではない) */
	float AngleValueNL(const Vec2& dir, const Vec2& dirA);
	//! 上方向を基準としたdirの角度を返す(半時計周り)
	/*! \return 0〜2*PI未満の角度値 */
	RadF AngleValue(const Vec2& dir);
	//! ang0,ang1における-oneloop以上oneloop未満の差分角度値を計算
	float AngleLerpValueDiff(float ang0, float ang1, const float oneloop);
	template <class Proc>
	float AngleLerpValue(float ang0, float ang1, const Proc& proc, const float oneloop) {
		auto diff = AngleLerpValueDiff(ang0, ang1, oneloop);
		return ang0 + proc(diff);
	}
	//! 上方向を0度とし、反時計回り方向に指定角度回転したベクトルを返す
	Vec2 VectorFromAngle(const RadF& ang);
	//! 2つの角度値をang0 -> ang1の線形補間
	template <class T>
	T AngleLerp(const T& ang0, const T& ang1, float r) {
		auto fn = [r](auto&& v){ return v*r; };
		return T(AngleLerpValue(ang0.get(), ang1.get(), fn, T::OneRotationAng));
	}
	//! ang0からang1へ向けてmaxDiff以下の分だけ近づける
	template <class T>
	T AngleMove(const T& ang0, const T& ang1, const T& maxDiff) {
		auto fn = [mdiff = maxDiff.get()](auto&& v) {
			if(std::abs(v) > mdiff)
				return (v > 0) ? mdiff : -mdiff;
			return decltype(mdiff)(v);
		};
		return T(AngleLerpValue(ang0.get(), ang1.get(), fn, T::OneRotationAng));
	}
}


// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_LINEARELLIPTIC_PROBLEMS_THERMALBLOCK_HH
#define DUNE_HDD_LINEARELLIPTIC_PROBLEMS_THERMALBLOCK_HH

#include <memory>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/static_assert.hh>
#include <dune/common/timer.hh>
#include <dune/common/typetraits.hh>

#if HAVE_DUNE_GRID_MULTISCALE
# include <dune/grid/multiscale/provider/cube.hh>
#endif

#include <dune/stuff/common/memory.hh>
#include <dune/stuff/common/string.hh>
#include <dune/stuff/functions/constant.hh>
#include <dune/stuff/functions/expression.hh>
#include <dune/stuff/playground/functions/indicator.hh>
#include <dune/stuff/grid/provider/cube.hh>
#include <dune/stuff/grid/boundaryinfo.hh>

#include <dune/pymor/functions/default.hh>
#include <dune/pymor/functions/checkerboard.hh>

#include "default.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {
namespace Problems {

template< class E, class D, int d, class R, int r = 1 >
class Thermalblock
  : public ProblemInterface< E, D, d, R, r >
{
  Thermalblock() { static_assert(AlwaysFalse< E >::value, "Not available for these dimensions!"); }
};

template< class EntityImp, class DomainFieldImp, int domainDim, class RangeFieldImp >
class Thermalblock< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
  : public Default< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
{
  typedef Default< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 > BaseType;
  typedef Thermalblock< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 > ThisType;

public:
  typedef Pymor::Functions::Checkerboard< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
      CheckerboardFunctionType;
  using typename BaseType::DiffusionTensorType;
  using typename BaseType::FunctionType;

  static const bool available = true;

  static std::string static_id()
  {
    return BaseType::BaseType::static_id() + ".thermalblock";
  }

  static Stuff::Common::Configuration default_config(const std::string sub_name = "")
  {
    typedef Stuff::Functions::Constant< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
        ConstantFunctionType;
    typedef Stuff::Functions::Constant< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, domainDim, domainDim >
        ConstantMatrixFunctionType;
    Stuff::Common::Configuration config;
    config["type"] = static_id();
    Stuff::Common::Configuration checkerboard_config = CheckerboardFunctionType::default_config();
    checkerboard_config["name"] = "diffusion_factor";
    checkerboard_config["type"] = CheckerboardFunctionType::static_id();
    checkerboard_config["num_elements"] = "[4 4 4]";
    checkerboard_config["parameter_name"] = "diffusion";
    config.add(checkerboard_config, "diffusion_factor");
    Stuff::Common::Configuration diffusion_tensor_config = ConstantMatrixFunctionType::default_config();
    diffusion_tensor_config["name"] = "diffusion_tensor";
    diffusion_tensor_config["type"] = ConstantMatrixFunctionType::static_id();
    config.add(diffusion_tensor_config, "diffusion_tensor");
    Stuff::Common::Configuration constant_config = ConstantFunctionType::default_config();
    constant_config["type"] = ConstantFunctionType::static_id();
    constant_config["name"] = "force";
    constant_config["value"] = "1";
    config.add(constant_config, "force");
    constant_config["name"] = "dirichlet";
    constant_config["value"] = "0";
    config.add(constant_config, "dirichlet");
    constant_config["name"] = "neumann";
    config.add(constant_config, "neumann");
    if (sub_name.empty())
      return config;
    else {
      Stuff::Common::Configuration tmp;
      tmp.add(config, sub_name);
      return tmp;
    }
  } // ... default_config(...)

  static std::unique_ptr< ThisType > create(const Stuff::Common::Configuration config = default_config(),
                                            const std::string sub_name = static_id())
  {
    const Stuff::Common::Configuration cfg = config.has_sub(sub_name) ? config.sub(sub_name) : config;
    std::shared_ptr< CheckerboardFunctionType >
        checkerboard_function(CheckerboardFunctionType::create(cfg.sub("diffusion_factor")));
    return Stuff::Common::make_unique< ThisType >(checkerboard_function,
                                                  BaseType::create_matrix_function("diffusion_tensor", cfg),
                                                  BaseType::create_vector_function("force", cfg),
                                                  BaseType::create_vector_function("dirichlet", cfg),
                                                  BaseType::create_vector_function("neumann", cfg));
  } // ... create(...)

  Thermalblock(const std::shared_ptr< const CheckerboardFunctionType >& checkerboard_function,
               const std::shared_ptr< const DiffusionTensorType >& diffusion_tensor,
               const std::shared_ptr< const FunctionType >& force,
               const std::shared_ptr< const FunctionType >& dirichlet,
               const std::shared_ptr< const FunctionType >& neumann)
    : BaseType(checkerboard_function, diffusion_tensor, force, dirichlet, neumann)
  {}

  virtual std::string type() const override
  {
    return BaseType::BaseType::static_id() + ".thermalblock";
  }
}; // class Thermalblock< ..., 1 >

template< class EntityImp, class DomainFieldImp, int domainDim, class RangeFieldImp, int rangeDim = 1 >
class LocalThermalblock
{
  static_assert(AlwaysFalse< EntityImp >::value, "Not available for dimRange > 1!");
};

template< class EntityImp, class DomainFieldImp, int domainDim, class RangeFieldImp >
class LocalThermalblock< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
  : public Default< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
{
  typedef Default< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 > BaseType;
  typedef LocalThermalblock< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 > ThisType;

  typedef Pymor::Functions::AffinelyDecomposableDefault< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
      AffinelyDecomposableDefaultFunctionType;
  typedef Stuff::Functions::Constant< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, domainDim, domainDim >
      ConstantMatrixFunctionType;
public:
  using typename BaseType::FunctionType;

  static std::string static_id()
  {
    return BaseType::BaseType::static_id() + ".localthermalblock";
  }

  static Stuff::Common::Configuration default_config(const std::string sub_name = "")
  {
    typedef Stuff::Functions::Constant< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 >
        ConstantFunctionType;
    Stuff::Common::Configuration config;
    Stuff::Common::Configuration constant_config = ConstantFunctionType::default_config();
    constant_config["type"] = ConstantFunctionType::static_id();
    constant_config["name"] = "force";
    constant_config["value"] = "1";
    config.add(constant_config, "force");
    constant_config["name"] = "dirichlet";
    constant_config["value"] = "0";
    config.add(constant_config, "dirichlet");
    constant_config["name"] = "neumann";
    config.add(constant_config, "neumann");
    if (sub_name.empty())
      return config;
    else {
      Stuff::Common::Configuration tmp;
      tmp.add(config, sub_name);
      return tmp;
    }
  } // ... default_config(...)

  static std::unique_ptr< ThisType > create(const Stuff::Common::Configuration config = default_config(),
                                            const std::string sub_name = static_id())
  {
    const Stuff::Common::Configuration cfg = config.has_sub(sub_name) ? config.sub(sub_name) : config;
    return Stuff::Common::make_unique< ThisType >(BaseType::create_vector_function("force", cfg),
                                                  BaseType::create_vector_function("dirichlet", cfg),
                                                  BaseType::create_vector_function("neumann", cfg));
  } // ... create(...)

  LocalThermalblock(const std::shared_ptr< const FunctionType >& force,
                    const std::shared_ptr< const FunctionType >& dirichlet,
                    const std::shared_ptr< const FunctionType >& neumann)
    : BaseType(create_diffusion_factor(),
               create_diffusion_tensor(),
               force,
               dirichlet,
               neumann)
  {}

  virtual std::string type() const override
  {
    return BaseType::BaseType::static_id() + ".localthermalblock";
  }

private:
  static std::shared_ptr< AffinelyDecomposableDefaultFunctionType > create_diffusion_factor()
  {
    typedef Stuff::Functions::DomainIndicator< EntityImp, DomainFieldImp, domainDim, RangeFieldImp, 1 > IndicatorFunctionType;
    const Pymor::ParameterType mu("diffusion", 3);

    auto ret = std::make_shared< AffinelyDecomposableDefaultFunctionType >("diffusion_factor");
    ret->register_component(new IndicatorFunctionType({{{{0.0, 0.0}, {0.5, 0.16}}, 1.0},
                                                       {{{0.0, 0.16}, {0.16, 0.33}}, 1.0},
                                                       {{{0.33, 0.16}, {0.5, 0.33}}, 1.0},
                                                       {{{0.0, 0.33}, {0.5, 1.0}}, 1.0}},
                                                      "left_block"),
                            new Pymor::ParameterFunctional(mu, "diffusion_factor[0]"));
    ret->register_component(new IndicatorFunctionType({{{{0.5, 0.0}, {1.0, 1.0}}, 1.0}},
                                                      "right_block"),
                            new Pymor::ParameterFunctional(mu, "diffusion_factor[1]"));
    ret->register_component(new IndicatorFunctionType({{{{0.16, 0.16}, {0.33, 0.33}}, 1.0}},
                                                      "small_block"),
                            new Pymor::ParameterFunctional(mu, "diffusion_factor[2]"));
    return ret;
  } // ... create_diffusion_factor()

  typedef Pymor::Functions::NonparametricDefault
      < EntityImp, DomainFieldImp, domainDim, RangeFieldImp, domainDim, domainDim > NonparametricFunctionType;

  static std::shared_ptr< NonparametricFunctionType > create_diffusion_tensor()
  {
    Stuff::Common::Configuration diffusion_tensor_config = ConstantMatrixFunctionType::default_config();
    diffusion_tensor_config["name"] = "diffusion_tensor";
    return std::make_shared< NonparametricFunctionType >(ConstantMatrixFunctionType::create(diffusion_tensor_config));
  } // ... create_diffusion_tensor()
}; // class LocalThermalblock< ..., 1 >

} // namespace Problems
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // DUNE_HDD_LINEARELLIPTIC_PROBLEMS_THERMALBLOCK_HH


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_REDUCTION_INCLUDE
#define MTL_REDUCTION_INCLUDE

#include <boost/mpl/bool.hpp>
#include <boost/numeric/meta_math/loop1.hpp>
#include <boost/numeric/mtl/utility/omp_size_type.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>

namespace mtl { namespace vec {

    namespace impl {

	template <unsigned long Index0, unsigned long Max0, typename Functor>
	struct reduction
	{
	    typedef reduction<Index0+1, Max0, Functor>     next;

	    template <typename Value>
	    static inline void init(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
				    Value& tmp05, Value& tmp06, Value& tmp07)
	    {
		Functor::init(tmp00);
		next::init(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp00);
	    }

	    template <typename Value, typename Vector, typename Size>
	    static inline void update(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
				      Value& tmp05, Value& tmp06, Value& tmp07, const Vector& v, Size i)
	    {
		Functor::update(tmp00, v[ i + Index0-1 ]);
		next::update(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp00, v, i);
	    }

	    template <typename Value>
	    static inline void finish(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
				    Value& tmp05, Value& tmp06, Value& tmp07)
	    {
		next::finish(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp00);
		Functor::finish(tmp00, tmp01);
	    }
	};

	template <unsigned long Max0, typename Functor>
	struct reduction<Max0, Max0, Functor>
	{
	    template <typename Value>
	    static inline void init(Value& tmp00, Value&, Value&, Value&, Value&, Value&, Value&, Value&)
	    {
		Functor::init(tmp00);
	    }

	    template <typename Value, typename Vector, typename Size>
	    static inline void update(Value& tmp00, Value&, Value&, Value&, Value&, Value&, Value&, Value&,
				      const Vector& v, Size i)
	    {
		Functor::update(tmp00, v[ i + Max0-1 ]);
	    }

	    template <typename Value>
	    static inline void finish(Value&, Value&, Value&, Value&, Value&, Value&, Value&, Value&) {}
	};

    } // namespace impl

// Will need distinction between dense and sparse in the future
template <unsigned long Unroll, typename Functor, typename Result>
struct reduction
{
    template <typename Vector>
    Result static inline apply(const Vector& v)
    {
	vampir_trace<2009> tracer;
	return apply(v, typename mtl::traits::is_sparse<Vector>());
    }

  private:
    template <typename Vector>
    Result static inline apply(const Vector& v, boost::mpl::true_)
    {
	Result tmp00;
	Functor::init(tmp00);

	for (std::size_t i= 0, n= v.nnz(); i < n; i++)
	    Functor::update(tmp00, v.value(i));
	// std::cout << "i == " << i << "

#if 0
	typename mtl::traits::const_value<Vector>::type                        value(v);
	typedef typename mtl::traits::range_generator<tag::nz, Vector>::type   cursor_type;

	for (cursor_type cursor = begin<tag::nz>(v), cend = end<tag::nz>(v); cursor != cend; ++cursor)
	    Functor::update(tmp00, value(*cursor));
#endif
	return tmp00;
    }

# ifdef MTL_WITH_OPENMP

    template <typename Vector>
    Result static inline apply(const Vector& v, boost::mpl::false_)
    {
	MTL_STATIC_ASSERT((Unroll >= 1), "Unroll size must be at least 1.");
	MTL_STATIC_ASSERT((Unroll <= 8), "Maximal unrolling is 8."); // Might be relaxed in future versions

	Result result;
	Functor::init(result);

	typedef typename mtl::traits::omp_size_type<typename Collection<Vector>::size_type>::type size_type;
	const size_type  i_max= mtl::size(v), i_block= Unroll * (i_max / Unroll);

	#pragma omp parallel
	{
	    vampir_trace<8002> tracer;
	    Result tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07;
	    impl::reduction<1, Unroll, Functor>::init(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07);

	    #pragma omp for
	    for (size_type i= 0; i < i_block; i+= Unroll)
		impl::reduction<1, Unroll, Functor>::update(tmp00, tmp01, tmp02, tmp03,
							    tmp04, tmp05, tmp06, tmp07, v, i);

	    impl::reduction<1, Unroll, Functor>::finish(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07);

	    #pragma omp critical
	    Functor::finish(result, tmp00);
	}

	for (size_type i= i_block; i < i_max; i++)
	    Functor::update(result, v[i]);

	return result;
    }

# else

    template <typename Vector>
    Result static inline apply(const Vector& v, boost::mpl::false_)
    {
	MTL_STATIC_ASSERT((Unroll >= 1), "Unroll size must be at least 1.");
	MTL_STATIC_ASSERT((Unroll <= 8), "Maximal unrolling is 8."); // Might be relaxed in future versions

	Result tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07;
	impl::reduction<1, Unroll, Functor>::init(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07);

	typedef typename Collection<Vector>::size_type              size_type;
	const size_type  i_max= mtl::vec::size(v), i_block= Unroll * (i_max / Unroll);
	for (size_type i = 0; i < i_block; i += Unroll)
		impl::reduction<1, Unroll, Functor>::update(tmp00, tmp01, tmp02, tmp03,
			tmp04, tmp05, tmp06, tmp07, v, i);
	for (size_type i= i_block; i < i_max; i++)
	    Functor::update(tmp00, v[i]);

	impl::reduction<1, Unroll, Functor>::finish(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07);
	return tmp00;
    }

# endif

};

}} // namespace mtl

#endif // MTL_REDUCTION_INCLUDE


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * 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.
*/

#include "SiconosConfig.h"
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "SiconosAlgebraProd.hpp" // for axpy_prod and prod
// Note Franck : sounds useless. It seems it's defined in bindings
// (to be checked, especially on windows)

// #define BIND_FORTRAN_LOWERCASE_UNDERSCORE

// needed for blas3
#include <assert.h>

namespace siconosBindings = boost::numeric::bindings;

// for ublas::axpy_prod, ...
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

// require for matrix stuff like value_type
//#include <boost/numeric/bindings/traits/ublas_matrix.hpp>

#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"
#include "SiconosAlgebraProd.hpp" // for prod

using namespace Siconos;

//======================
// Product of matrices
//======================
// Note FP: this function is never used. We keep it for the record. Remove it later ?

// const SimpleMatrix prod(const SiconosMatrix &A, const SiconosMatrix& B)
// {
//   // To compute C = A * B
//   assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
//   assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

//   if((A.size(1) != B.size(0)))
//     THROW_EXCEPTION("Matrix function C=prod(A,B): inconsistent sizes");

//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numB = B.num();

//   // == TODO: implement block product ==
//   if(numA == 0 || numB == 0)
//     THROW_EXCEPTION("Matrix product ( C=prod(A,B) ): not yet implemented for BlockMatrix objects.");

//   if(numA == Siconos::IDENTITY || numB == Siconos::ZERO)  // A = identity or B = 0
//     return SimpleMatrix(B);

//   else if(numB == Siconos::IDENTITY || numA == Siconos::ZERO)  // B = identity or A = 0
//     return SimpleMatrix(A);

//   else // neither A or B is equal to identity or zero.
//   {
//     if(numB == Siconos::DENSE)
//     {
//       if(numA == Siconos::DENSE)
//       {
//         DenseMat p(A.size(0), B.size(1));
//         siconosBindings::blas::gemm(1.0, *A.dense(), *B.dense(), 1.0, p);
//         //      return (DenseMat)(prod(*A.dense(),*B.dense()));
//         return p;
//       }
//       else if(numA == Siconos::TRIANGULAR)
//         return (DenseMat)(prod(*A.triang(), *B.dense()));
//       else if(numA == Siconos::SYMMETRIC)
//         return (DenseMat)(prod(*A.sym(), *B.dense()));
//       else if(numA == Siconos::SPARSE)
//         return (DenseMat)(prod(*A.sparse(), *B.dense()));
//       else if(numA == Siconos::SPARSE_COORDINATE)
//         return (DenseMat)(prod(*A.sparseCoordinate(), *B.dense()));
//       else// if(numA==Siconos::BANDED)
//         return (DenseMat)(prod(*A.banded(), *B.dense()));
//     }
//     else if(numB == Siconos::TRIANGULAR)
//     {
//       if(numA == Siconos::DENSE)
//         return (DenseMat)(prod(*A.dense(), *B.triang()));
//       else if(numA == Siconos::TRIANGULAR)
//         return (TriangMat)(prod(*A.triang(), *B.triang()));
//       else if(numA == Siconos::SYMMETRIC)
//         return (DenseMat)(prod(*A.sym(), *B.triang()));
//       else if(numA == Siconos::SPARSE)
//         return (DenseMat)(prod(*A.sparse(), *B.triang()));
//       else if(numA == Siconos::SPARSE_COORDINATE)
//         return (DenseMat)(prod(*A.sparseCoordinate(), *B.triang()));
//       else //if(numA==Siconos::BANDED)
//         return (DenseMat)(prod(*A.banded(), *B.triang()));
//     }
//     else if(numB == Siconos::SYMMETRIC)
//     {
//       if(numA == Siconos::DENSE)
//         return (DenseMat)(prod(*A.dense(), *B.sym()));
//       else if(numA == Siconos::TRIANGULAR)
//         return (DenseMat)(prod(*A.triang(), *B.sym()));
//       else if(numA == Siconos::SYMMETRIC)
//         return (SymMat)(prod(*A.sym(), *B.sym()));
//       else if(numA == Siconos::SPARSE)
//         return (DenseMat)(prod(*A.sparse(), *B.sym()));
//       else if(numA == Siconos::SPARSE_COORDINATE)
//         return (DenseMat)(prod(*A.sparseCoordinate(), *B.sym()));
//       else // if (numA == Siconos::BANDED)
//         return (DenseMat)(prod(*A.banded(), *B.sym()));
//     }
//     else if(numB == Siconos::SPARSE)
//     {
//       if(numA == Siconos::DENSE)
//         return (DenseMat)(prod(*A.dense(), *B.sparse()));
//       else if(numA == Siconos::TRIANGULAR)
//         return (DenseMat)(prod(*A.triang(), *B.sparse()));
//       else if(numA == Siconos::SYMMETRIC)
//         return (DenseMat)(prod(*A.sym(), *B.sparse()));
//       else if(numA == Siconos::SPARSE)
//         return (SparseMat)(prod(*A.sparse(), *B.sparse()));
//       else if(numA == Siconos::SPARSE_COORDINATE)
//         return (SparseMat)(prod(*A.sparseCoordinate(), *B.sparse()));
//       else //if(numA==Siconos::BANDED){
//         return (DenseMat)(prod(*A.banded(), *B.sparse()));
//     }
//     else if(numB == Siconos::SPARSE_COORDINATE)
//     {
//       if(numA == Siconos::DENSE)
//         return (DenseMat)(prod(*A.dense(), *B.sparseCoordinate()));
//       else if(numA == Siconos::TRIANGULAR)
//         return (DenseMat)(prod(*A.triang(), *B.sparseCoordinate()));
//       else if(numA == Siconos::SYMMETRIC)
//         return (DenseMat)(prod(*A.sym(), *B.sparseCoordinate()));
//       else if(numA == Siconos::SPARSE)
//         return (SparseMat)(prod(*A.sparse(), *B.sparseCoordinate()));
//       else if(numA == Siconos::SPARSE_COORDINATE)
//         return (SparseMat)(prod(*A.sparseCoordinate(), *B.sparseCoordinate()));
//       else //if(numA==Siconos::BANDED){
//         return (DenseMat)(prod(*A.banded(), *B.sparseCoordinate()));
//     }
//     else //if(numB==Siconos::BANDED)
//     {
//       if(numA == Siconos::DENSE)
//         return (DenseMat)(prod(*A.dense(), *B.banded()));
//       else if(numA == Siconos::TRIANGULAR)
//         return (DenseMat)(prod(*A.triang(), *B.banded()));
//       else if(numA == Siconos::SYMMETRIC)
//         return (DenseMat)(prod(*A.sym(), *B.banded()));
//       else if(numA == Siconos::SPARSE)
//         return (DenseMat)(prod(*A.sparse(), *B.banded()));
//       else if(numA == Siconos::SPARSE_COORDINATE)
//         return (DenseMat)(prod(*A.sparseCoordinate(), *B.banded()));
//       else //if(numA==Siconos::BANDED)
//         return (DenseMat)(prod(*A.banded(), *B.banded()));
//     }
//   }
// }
/**

indexStart : indexStart[0] is the first raw, indexStart[1] is the first col
dim : dim[0] number of raw, dim[1] number of col
*/
// void zeroBlock(const SiconosMatrix& A, index indexStart, index dim){
//   ;
// }
// void prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, int indexACol, bool init){
//   // To compute C[indexAcol::] = A * B

//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numB = B.num();
//   Siconos::UBLAS_TYPE numC = C.num();
//   if (numA == 0 || numB == 0 || numC == 0)
//     THROW_EXCEPTION("Matrix function prod(A,B,C,index): inconsistent sizes");
//   // === if C is zero or identity => read-only ===
//   if (numC == Siconos::Zero || numC == Siconos::IDENTITY)
//     THROW_EXCEPTION("Matrix product ( prod(A,B,C,index) ): wrong type for resulting matrix C (read-only: zero or identity).");

//   if (numA == Siconos::IDENTITY || numC == Siconos::Zero) // A = identity or 0
//     THROW_EXCEPTION("Matrix function prod(A,B,C,index): numA == Siconos::IDENTITY || numC == Siconos::Zero not yet implemented");

//   int rawB = B.size(0);
//   int colB = B.size(1);

// }

void axpy_prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, bool init)
{

  // To compute C = A * B (init = true) or C += A * B (init = false) using ublas axpy_prod.
  // High speedup for sparse matrices.
  // Warning FP: ublas::axpy_prod(A, B, C, init) with init = True is equivalent
  // to C = A*B with C.clear BEFORE product. So C==A or B must be forbidden.
  // See http://www.boost.org/doc/libs/1_63_0/libs/numeric/ublas/doc/products.html
  //

  if((A.size(1) != B.size(0)))
    THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): inconsistent sizes");

  if(A.size(0) != C.size(0) || B.size(1) != C.size(1))
    THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): inconsistent sizes");

  if(&A == &C || &B == &C)
    THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): C must be different from A and B.");

  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorized in place in prod !!");
  assert(!(B.isPLUFactorizedInPlace()) && "B is PLUFactorized in place in prod !!");
  if(!C.isBlock())
    C.resetFactorizationFlags();
  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numB = B.num();
  Siconos::UBLAS_TYPE numC = C.num();
  // == TODO: implement block product ==
  if(numA == Siconos::BLOCK || numB == Siconos::BLOCK)
    THROW_EXCEPTION("Matrix product ( prod(A,B,C) ): not yet implemented for BlockMatrix objects.");

  // === if C is zero or identity => read-only ===
  if(numC == Siconos::ZERO || numC == Siconos::IDENTITY)
    THROW_EXCEPTION("Matrix product ( prod(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  if(numA == Siconos::IDENTITY)  // A = identity ...
  {
    if(!init)
      C += B;
    else
      C = B; // if C and B are two different objects.
  }

  else if(numB == Siconos::IDENTITY)  // B = identity
  {
    if(!init)
      C += A;
    else
      C = A; // if C and A are two different objects.
  }

  else if(numA == Siconos::ZERO || numB == Siconos::ZERO)  // if A or B = 0
  {
    if(init) C.zero();  // else nothing
  }
  else if(numC == Siconos::BLOCK)  // if C is Block - Temp. solution
  {
    SimpleMatrix tmp(C);
    axpy_prod(A, B, tmp, init);
    C = tmp;
  }
  else // neither A or B is equal to identity or zero.
  {
    switch(numC)
    {
    case Siconos::DENSE:
      if(numB == Siconos::DENSE)
      {
        if(numA == Siconos::DENSE)
          ublas::axpy_prod(*A.dense(), *B.dense(), *C.dense(), init);
        else if(numA == Siconos::TRIANGULAR)
          ublas::axpy_prod(*A.triang(), *B.dense(), *C.dense(), init);
        else if(numA == Siconos::SYMMETRIC)
          ublas::axpy_prod(*A.sym(), *B.dense(), *C.dense(), init);
        else if(numA == Siconos::SPARSE)
          ublas::axpy_prod(*A.sparse(), *B.dense(), *C.dense(), init);
        else// if(numA==Siconos::BANDED)
          ublas::axpy_prod(*A.banded(), *B.dense(), *C.dense(), init);
      }
      else if(numB == Siconos::TRIANGULAR)
      {
        if(numA == Siconos::DENSE)
          ublas::axpy_prod(*A.dense(), *B.triang(), *C.dense(), init);
        else if(numA == Siconos::TRIANGULAR)
          ublas::axpy_prod(*A.triang(), *B.triang(), *C.dense(), init);
        else if(numA == Siconos::SYMMETRIC)
          ublas::axpy_prod(*A.sym(), *B.triang(), *C.dense(), init);
        else if(numA == Siconos::SPARSE)
          ublas::axpy_prod(*A.sparse(), *B.triang(), *C.dense(), init);
        else //if(numA==Siconos::BANDED)
          ublas::axpy_prod(*A.banded(), *B.triang(), *C.dense(), init);
      }
      else if(numB == Siconos::SYMMETRIC)
      {
        if(numA == Siconos::DENSE)
          ublas::axpy_prod(*A.dense(), *B.sym(), *C.dense(), init);
        else if(numA == Siconos::TRIANGULAR)
          ublas::axpy_prod(*A.triang(), *B.sym(), *C.dense(), init);
        else if(numA == Siconos::SYMMETRIC)
          ublas::axpy_prod(*A.sym(), *B.sym(), *C.dense(), init);
        else if(numA == Siconos::SPARSE)
          ublas::axpy_prod(*A.sparse(), *B.sym(), *C.dense(), init);
        else // if (numA == Siconos::BANDED)
          ublas::axpy_prod(*A.banded(), *B.sym(), *C.dense(), init);
      }
      else if(numB == Siconos::SPARSE)
      {
        if(numA == Siconos::DENSE)
          ublas::axpy_prod(*A.dense(), *B.sparse(), *C.dense(), init);
        else if(numA == Siconos::TRIANGULAR)
          ublas::axpy_prod(*A.triang(), *B.sparse(), *C.dense(), init);
        else if(numA == Siconos::SYMMETRIC)
          ublas::axpy_prod(*A.sym(), *B.sparse(), *C.dense(), init);
        else if(numA == Siconos::SPARSE)
          ublas::axpy_prod(*A.sparse(), *B.sparse(), *C.dense(), init);
        else //if(numA==Siconos::BANDED){
          ublas::axpy_prod(*A.banded(), *B.sparse(), *C.dense(), init);
      }
      else //if(numB==Siconos::BANDED)
      {
        if(numA == Siconos::DENSE)
          ublas::axpy_prod(*A.dense(), *B.banded(), *C.dense(), init);
        else if(numA == Siconos::TRIANGULAR)
          ublas::axpy_prod(*A.triang(), *B.banded(), *C.dense(), init);
        else if(numA == Siconos::SYMMETRIC)
          ublas::axpy_prod(*A.sym(), *B.banded(), *C.dense(), init);
        else if(numA == Siconos::SPARSE)
          ublas::axpy_prod(*A.sparse(), *B.banded(), *C.dense(), init);
        else //if(numA==Siconos::BANDED)
          ublas::axpy_prod(*A.banded(), *B.banded(), *C.dense(), init);
      }
      break;
    case Siconos::TRIANGULAR:
      // if(numA!= Siconos::TRIANGULAR || numB != Siconos::TRIANGULAR)
      THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      //ublas::axpy_prod(*A.triang(), *B.triang(),*C.triang(), init);
      break;
    case Siconos::SYMMETRIC:
      //        if(numA!= Siconos::SYMMETRIC || numB != Siconos::SYMMETRIC)
      THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      //ublas::axpy_prod(*A.sym(), *B.sym(),*C.sym(),init);
      break;
    case Siconos::SPARSE:
      if(numA != Siconos::SPARSE || numB != Siconos::SPARSE)
        THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      ublas::sparse_prod(*A.sparse(), *B.sparse(), *C.sparse(), init);
      break;
    default:
      THROW_EXCEPTION("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
    }
    if(!C.isBlock())
      C.resetFactorizationFlags();
  }
}

// Note FP: this function is never used. We keep it for the record. Remove it later ?
// void gemmtranspose(double a, const SiconosMatrix& A, const SiconosMatrix& B, double b, SiconosMatrix& C)
// {
//   if(A.isBlock() || B.isBlock() || C.isBlock())
//     THROW_EXCEPTION("gemm(...) not yet implemented for block matrices.");
//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numB = B.num();
//   Siconos::UBLAS_TYPE numC = C.num();
//   if(numA != Siconos::DENSE || numB != Siconos::DENSE || numC != Siconos::DENSE)
//     THROW_EXCEPTION("gemm(...) failed: reserved to dense matrices.");

//   assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
//   assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

//   siconosBindings::blas::gemm(a, siconosBindings::trans(*A.dense()), siconosBindings::trans(*B.dense()), b, *C.dense());

//   C.resetFactorizationFlags();
// }

// Note FP: this function is never used. We keep it for the record. Remove it later ?
// void gemm(double a, const SiconosMatrix& A, const SiconosMatrix& B, double b, SiconosMatrix& C)
// {
//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numB = B.num();
//   Siconos::UBLAS_TYPE numC = C.num();
//   assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
//   assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");
//   C.resetFactorizationFlags();

//   // At the time, only dense output allowed
//   DenseMat * tmpC = nullptr;
//   if(numA == 0 || numB == 0 || numC == 0)
//     THROW_EXCEPTION("gemm(...) not yet implemented for block matrices.");

//   if(numA == Siconos::DENSE && numB == Siconos::DENSE && numC == Siconos::DENSE)
//     siconosBindings::blas::gemm(a, *A.dense(), *B.dense(), b, *C.dense());
//   else if(numA == Siconos::DENSE && numB == Siconos::DENSE && numC != Siconos::DENSE)
//   {
//     // Copy C into tmpC ...
//     tmpC = new DenseMat(*C.dense());
//     siconosBindings::blas::gemm(a, *A.dense(), *B.dense(), b, *tmpC);
//     std::cout << *tmpC << std::endl;
//     noalias(*C.dense()) = *tmpC;
//     delete tmpC;
//   }
//   else
//     THROW_EXCEPTION("gemm(...) not yet implemented for these kinds of matrices.");
//   C.resetFactorizationFlags();
// }


#ifdef RICH_MPI
#include <mpi.h>
#endif
#include <boost/foreach.hpp>
#include "source/newtonian/two_dimensional/diagnostics.hpp"
#include "source/newtonian/test_2d/main_loop_2d.hpp"
#include "source/tessellation/shape_2d.hpp"
#include "source/newtonian/test_2d/piecewise.hpp"
#include "source/newtonian/two_dimensional/physical_geometry.hpp"
#include "source/newtonian/two_dimensional/geometric_outer_boundaries/SquareBox.hpp"
#include "source/tessellation/VoronoiMesh.hpp"
#include "source/newtonian/common/ideal_gas.hpp"
#include "source/newtonian/two_dimensional/point_motions/eulerian.hpp"
#include "source/newtonian/common/hllc.hpp"
#include "source/newtonian/two_dimensional/source_terms/zero_force.hpp"
#include "source/misc/mesh_generator.hpp"
#include "source/newtonian/two_dimensional/spatial_distributions/uniform2d.hpp"
#include "source/newtonian/two_dimensional/source_terms/cylindrical_complementary.hpp"
#include "source/newtonian/two_dimensional/simple_flux_calculator.hpp"
#include "source/newtonian/two_dimensional/simple_cell_updater.hpp"
#include "source/newtonian/two_dimensional/hdf5_diagnostics.hpp"
#include "source/newtonian/two_dimensional/simple_extensive_updater.hpp"
#include "source/newtonian/two_dimensional/stationary_box.hpp"

using namespace std;
using namespace simulation2d;

namespace {

  bool same_side(const Vector2D& p1,
		 const Vector2D& p2,
		 const Vector2D& a,
		 const Vector2D& b)
  {
    const double cp1 = CrossProduct(b-a,p1-a);
    const double cp2 = CrossProduct(b-a,p2-a);
    return cp1*cp2>=0;
  }

  class Triangle: public Shape2D
  {
  public:

    const Vector2D p1;
    const Vector2D p2;
    const Vector2D p3;

    Triangle(const Vector2D& p1_i,
	     const Vector2D& p2_i,
	     const Vector2D& p3_i):
      p1(p1_i), p2(p2_i), p3(p3_i) {}

    bool operator()(const Vector2D& p) const
    {
      return same_side(p,p1,p2,p3)&&
	same_side(p,p2,p1,p3)&&
	same_side(p,p3,p1,p2);
    }
  };

  vector<ComputationalCell> calc_init_cond
  (const Tessellation& tess)
  {
    vector<ComputationalCell> res(static_cast<size_t>(tess.GetPointNo()));
    for(size_t i=0;i<res.size();++i){
      const Triangle triangle(Vector2D(0.5,0.6),
			      Vector2D(0.7,0.5),
			      Vector2D(0.4,0.4));
      const Vector2D r = tess.GetMeshPoint(static_cast<int>(i));
      res[i].density = 1;
      res[i].pressure = triangle(r) ? 2 : 1;
      res[i].velocity = triangle(r) ? Vector2D(1,-1) : Vector2D(0,0);
      //      res[i].tracers.push_back(triangle(r) ? 1 : 0);
      res[i].tracers[0] = triangle(r) ? 1 : 0;
    }
    return res;
  }

  class SimData
  {
  public:

    SimData(void):
      pg_(),
      outer_(Vector2D(0,0), Vector2D(1,1)),
#ifdef RICH_MPI
      proc_tess_(process_positions(outer_),outer_),
      init_points_
      (distribute_grid(proc_tess_,CartesianGridGenerator
		       (30,30, outer_.getBoundary().first,
			outer_.getBoundary().second))),
#else
      init_points_(cartesian_mesh(30,30,
				  outer_.getBoundary().first,
				  outer_.getBoundary().second)),
#endif
      tess_(init_points_,outer_),
      eos_(5./3.),
      pm_(),
      evc_(),
      rs_(),
      force_(),
      tsf_(0.3),
      fc_(rs_),
      eu_(),
      cu_(),
      sim_(tess_,
	   outer_,
	   pg_,
	   calc_init_cond(tess_),
	   eos_,
	   pm_,
	   evc_,
	   force_,
	   tsf_,
	   fc_,
	   eu_,
	   cu_,
	   TracerStickerNames(vector<string>(1,"tracer"),vector<string>())) {}

    hdsim& getSim(void)
    {
      return sim_;
    }

  private:
    SlabSymmetry pg_;
    SquareBox outer_;
#ifdef RICH_MPI
    VoronoiMesh proc_tess_;
#endif
    const vector<Vector2D> init_points_;
    VoronoiMesh tess_;
    IdealGas eos_;
    Eulerian pm_;
    const StationaryBox evc_;
    Hllc rs_;
    ZeroForce force_;
    SimpleCFL tsf_;
    SimpleFluxCalculator fc_;
    const SimpleExtensiveUpdater eu_;
    SimpleCellUpdater cu_;
    hdsim sim_;
  };

  class WriteConserved: public DiagnosticFunction
  {
  public:

    WriteConserved(string const& fname):
      cons_(), fname_(fname) {}

    void operator()(hdsim const& sim)
    {
      cons_.push_back(total_conserved(sim));
    }

    ~WriteConserved(void)
    {
#ifdef RICH_MPI
      if(get_mpi_rank()==0){
#endif
	ofstream f(fname_.c_str());
	for(size_t i=0;i<cons_.size();++i)
	  f << cons_[i].mass << " "
	    << cons_[i].momentum.x << " "
	    << cons_[i].momentum.y << " "
	    << cons_[i].energy << " "
	    << cons_[i].tracers[0] << "\n";
	f.close();
#ifdef RICH_MPI
      }
#endif
    }

  private:
    mutable vector<Extensive> cons_;
    const string fname_;
  };
}

namespace {
  void my_main_loop(hdsim& sim)
  {
    SafeTimeTermination term_cond(0.05,1e6);
    WriteConserved diag("res.txt");
    write_snapshot_to_hdf5(sim,"initial.h5");
    main_loop(sim,
	      term_cond,
	      &hdsim::TimeAdvance,
	      &diag);
    write_snapshot_to_hdf5(sim,"final.h5");
  }
}

int main(void)
{
#ifdef RICH_MPI
  MPI_Init(NULL, NULL);
#endif
  SimData sim_data;
  hdsim& sim = sim_data.getSim();

  my_main_loop(sim);

#ifdef RICH_MPI
  MPI_Finalize();
#endif

  return 0;
}


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2012-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <algorithm>  // all_of, copy, equal
#include <iosfwd>
#include <stdexcept>  // invalid_argument

#include <boost/container/flat_set.hpp>

#include "sdd/internal_manager_fwd.hh"
#include "sdd/dd/definition.hh"
#include "sdd/hom/consolidate.hh"
#include "sdd/hom/context_fwd.hh"
#include "sdd/hom/definition_fwd.hh"
#include "sdd/hom/identity.hh"
#include "sdd/hom/local.hh"
#include "sdd/order/order.hh"
#include "sdd/util/packed.hh"

namespace sdd { namespace hom {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Saturation Fixpoint homomorphism.
template <typename C>
struct LIBSDD_ATTRIBUTE_PACKED _saturation_fixpoint
{
  /// @brief The type of a const iterator on this saturation_fixpoint's G operands.
  using const_iterator = const homomorphism<C>*;

  /// @brief The variable type.
  using variable_type = typename C::variable_type;

  /// @brief The type deduced from configuration of the number of operands.
  using operands_size_type = typename C::operands_size_type;

  /// @brief The variable on which this sum works.
  const variable_type variable;

  /// @brief The homomorphism's F part.
  const homomorphism<C> F;

  /// @brief The homomorphism's G part size.
  const operands_size_type G_size;

  /// @brief The homomorphism's L part.
  const homomorphism<C> L;

public:

  /// @brief Constructor.
  _saturation_fixpoint( variable_type var, homomorphism<C> f
                      , boost::container::flat_set<homomorphism<C>>& g
                      , homomorphism<C> l)
    : variable{var}
    , F{std::move(f)}
    , G_size{static_cast<operands_size_type>(g.size())}
    , L{std::move(l)}
  {
    // Put all homomorphisms operands right after this sum instance.
    hom::consolidate(G_operands_addr(), g.begin(), g.end());
  }

  /// @brief Destructor.
  ~_saturation_fixpoint()
  {
    for (auto& elem : *this)
    {
      elem.~homomorphism<C>();
    }
  }

  /// @brief Evaluation.
  SDD<C>
  operator()(context<C>& cxt, const order<C>& o, const SDD<C>& s)
  const
  {
    auto& sdd_context = cxt.sdd_context();

    SDD<C> s1 = s;
    SDD<C> s2 = s;

    do
    {
      s1 = s2;

      s2 = F(cxt, o, s2); // apply (F + Id)*
      s2 = L(cxt, o, s2); // apply (L + Id)*

      for (const auto& g : *this)
      {
        // chain applications of G
        s2 = dd::sum(sdd_context, dd::sum_builder<C, SDD<C>>(sdd_context, {s2, g(cxt, o, s2)}));
      }
    } while (s1 != s2);

    return s1;
  }

  /// @brief Skip predicate.
  bool
  skip(const order<C>& o)
  const noexcept
  {
    return variable != o.variable();
  }

  /// @brief Selector predicate.
  bool
  selector()
  const noexcept
  {
    return F.selector() and L.selector()
       and std::all_of(begin(), end(), [](const auto& h){return h.selector();});
  }

  /// @brief Get an iterator to the first operand of G.
  ///
  /// O(1).
  const_iterator
  begin()
  const noexcept
  {
    return reinterpret_cast<const homomorphism<C>*>(G_operands_addr());
  }

  /// @brief Get an iterator to the end of operands of G.
  ///
  /// O(1).
  const_iterator
  end()
  const noexcept
  {
    return reinterpret_cast<const homomorphism<C>*>(G_operands_addr()) + G_size;
  }

  friend
  bool
  operator==(const _saturation_fixpoint& lhs, const _saturation_fixpoint& rhs)
  noexcept
  {
    return lhs.variable == rhs.variable and lhs.F == rhs.F
       and lhs.L == rhs.L and lhs.G_size == rhs.G_size
       and std::equal(lhs.begin(), lhs.end(), rhs.begin());
  }

  friend
  std::ostream&
  operator<<(std::ostream& os, const _saturation_fixpoint& s)
  {
    os << "Sat(@" << s.variable << ",  " << s.F << " + " << s.L;
    if (s.G_size != 0)
    {
      os << " + ";
      std::copy( s.begin(), std::prev(s.end())
               , std::ostream_iterator<homomorphism<C>>(os, " + "));
      os << *std::prev(s.end()) << ")*";
    }
    return os;
  }

private:

  /// @brief Return the address of the beginning of the operands of G.
  char*
  G_operands_addr()
  const noexcept
  {
    return reinterpret_cast<char*>(const_cast<_saturation_fixpoint*>(this))
         + sizeof(_saturation_fixpoint);
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Create the Saturation Fixpoint homomorphism.
/// @related sdd::homomorphism
///
/// We suppose that a saturation fixpoint is created in the rewriting process. Thus, we assume
/// that operands of the G part are already optimized (e.g. local merged and sums flatten).
template <typename C, typename InputIterator>
homomorphism<C>
saturation_fixpoint( typename C::variable_type var
                  , const homomorphism<C>& f
                  , InputIterator gbegin, InputIterator gend
                  , const homomorphism<C>& l)
{
  const std::size_t gsize = std::distance(gbegin, gend);

  if (gsize == 0)
  {
    if (f != id<C>() and l == id<C>()) return f;
    if (f == id<C>() and l != id<C>()) return l;
  }

  // A global flat_set to avoid reallocating a new set of operands each time.
  auto& g = global<C>().saturation_fixpoint_data;
  g.clear();
  g.insert(gbegin, gend);
  const std::size_t extra_bytes = g.size() * sizeof(homomorphism<C>);
  return hom::make_variable_size<C, _saturation_fixpoint<C>>(extra_bytes, var, f, g, l);
}

/*------------------------------------------------------------------------------------------------*/

}} // namespace sdd::hom

namespace std {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Hash specialization for sdd::hom::_saturation_fixpoint.
template <typename C>
struct hash<sdd::hom::_saturation_fixpoint<C>>
{
  std::size_t
  operator()(const sdd::hom::_saturation_fixpoint<C>& s)
  const
  {
    using namespace sdd::hash;
    return seed(s.variable) (val(s.F)) (val(s.L)) (range(s));
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace std


// This file is part of the dune-mlmc project:
//   http://users.dune-project.org/projects/dune-mlmc
// Copyright Holders: Jan Mohring
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef MLMC_H
#define MLMC_H

#include <mpi.h>
#include <vector>
#include <math.h>
#include <iostream>
#include <fstream>
#include <sstream>

#include <boost/noncopyable.hpp>
#include <dune/stuff/common/exceptions.hh>
#include <dune/xt/common/configuration.hh>

/// \file Environment for computing expected values by the Multi Level
/// Monte Carlo approach. For each level you have to provide extensions
/// of the base class Difference which implement the two methods
/// init and eval.
/// \author jan.mohring@itwm.fraunhofer.de
/// \date 2015

namespace MultiLevelMonteCarlo {

/// Terminates program if condition is not satisfied.
/// \param condition   condition to check
/// \param message     message to show when condition is false
/// TODO embed into exception handling
static void check(bool condition, const char* message) {
  if (!condition) {
    std::cerr << message << "\n";
    DUNE_THROW(Dune::InvalidStateException, message);
  }
}

/// Checks for power of 2
/// \param x  number to check
static bool isPowerOf2(int x) { return (x != 0) && ((x & (x - 1)) == 0); }

/// Base class of difference of solutions on subsequent levels.
/// On the coarsest level the solution itself has to be returned.
class Difference : boost::noncopyable {
public:
  /// Initialize level.
  /// \param global  global communicator
  /// \param local   communicator of processors involved in this solution
  virtual void init(MPI_Comm global, MPI_Comm local) = 0;

  /// Evaluate difference of solutions on subsequent levels.
  virtual double eval() = 0;

  virtual ~Difference() {}
};

/// Class for doing statistics on a given level.
class Level {
public:
  /// Constructs empty level.
  Level()
    : _n(0)
    , _N(0)
    , _p(0)
    , _g(0)
    , _sumX(0)
    , _sumX2(0)
    , _T(0)
    , _diff(nullptr)
    , _masters(MPI_COMM_NULL) {}

  /// Constructs level from Difference object.
  /// \param diff     Difference object
  /// \param minProc  minimal number of processors needed to compute solution
  Level(const std::shared_ptr<Difference> diff, int minProc = 1)
    : _n(0)
    , _N(0)
    , _p(minProc)
    , _g(0)
    , _sumX(0)
    , _sumX2(0)
    , _T(0)
    , _diff(diff)
    , _masters(MPI_COMM_NULL) {
    check(minProc > 0, "minProc must be positive.");
  }

  /// Assigns communicator and group
  /// \param world   global communicator
  void assignProcessors(MPI_Comm world = MPI_COMM_WORLD) {
    check(_diff != NULL, "No Difference object set.");
    int size, rank;
    int range[1][3];
    MPI_Group all, group, masters;
    MPI_Comm_size(world, &size);
    MPI_Comm_rank(world, &rank);
    MPI_Comm_group(world, &all);
    range[0][0] = rank - rank % _p;
    range[0][1] = range[0][0] + _p - 1;
    range[0][2] = 1;
    MPI_Group_range_incl(all, 1, range, &group);
    MPI_Comm_create(world, group, &_group);
    _g = size / _p;
    _diff->init(world, _group);
    range[0][0] = 0;
    range[0][1] = size - 1;
    range[0][2] = _p;
    MPI_Group_range_incl(all, 1, range, &masters);
    MPI_Comm_create(world, masters, &_masters);
    int grank;
    MPI_Comm_rank(_group, &grank);
    _isMaster = grank == 0;
  }

  /// Sets number of repetitions.
  /// \param N   number of repetitions
  void setRepetitions(int N) { _N = N; }

  /// Gets number of repetitions.
  int getRepetitions() const { return _N; }

  /// Returns number of repetitions till next break.
  /// \param iBreak  index of next break
  /// \param nBreak  total number of breaks
  int nextRepetitions(int iBreak, int nBreak) const { return _N * iBreak / nBreak - _n; }

  /// Returns communicator of masters
  MPI_Comm getMasters() const { return _masters; }

  /// Indicates master
  bool isMaster() const { return _isMaster; }

  /// Returns communicator of masters
  MPI_Comm getGroup() const { return _group; }

  /// Clears statistics.
  void clear() {
    _n = 0;
    _sumX = 0;
    _sumX2 = 0;
    _T = 0;
  }

  /// Updates statistics.
  void update(int n, double sumX, double sumX2, double T) {
    _n += n;
    _sumX += sumX;
    _sumX2 += sumX2;
    _T += T;
  }

  /// Returns mean value of realizations on present level.
  double mean() const { return _sumX / (_n * _g); }

  /// Returns empirical variance of realizations on present level.
  double var() const { return _sumX2 / (_n * _g) - mean() * mean(); }

  /// Returns average time of repetition on present level.
  double time() const { return _T / (_n *  _g); }

  /// Returns number of groups on present level.
  int groups() const { return _g; }

  /// Return number of processors per group.
  int procs() const { return _p; }

  int totalRepetitions() const { return _N * _g; }
  int doneRepetitions() const { return _n * _g; }

  /// Evaluate difference.
  double eval() { return _diff->eval(); }

private:
  int _n;                  ///< number of performed repetitions
  int _N;                  ///< number of required repetitions
  int _p;                  ///< number of processors per realization
  int _g;                  ///< number of groups acting in parallel per repetition
  double _sumX;            ///< sum of results so far
  double _sumX2;           ///< sum of squared results so far
  double _T;               ///< total time of repetitions so far
  const std::shared_ptr<Difference> _diff; ///< pointer to Difference object
  MPI_Comm _group;         ///< communicator of group
  MPI_Comm _masters;       ///< communicator of masters
  bool _isMaster;          ///< flag indicating master
};

/// Environment for computing expected values by the
/// Multi Level Monte Carlo Method.
class MLMC {

public:
  /// Adds difference object.
  /// \param diff     Difference object
  /// \param minProc  minimal number of processors needed to compute solution
  void addDifference(const std::shared_ptr<Difference> diff, int minProc) {
    check(isPowerOf2(minProc), "minProc must be power of 2.");
    _level.emplace_back(diff, minProc);
  }

  /// Computes optimal number of repetitions per level .
  /// \param tol  absolute tolerance of expected value
  void setRepetitions(double tol) {
    double alpha = 0;
    int n = _level.size();
    for (int i = 0; i < n; ++i)
      alpha += sqrt(_level[i].time() * _level[i].var());
    alpha /= tol * tol;
    for (int i = 0; i < n; ++i)
      _level[i].setRepetitions(ceil(alpha * sqrt(_level[i].var() / _level[i].time()) / _level[i].groups()));
  }

  /// Computes the expected value of a scalar random variable up to a given
  /// tolerance by the Multi Level Monte Carlo approach.
  /// \param tol    absolute tolerance of expected value
  /// \param nBreak number of breaks for recomputing required realizations
  double expectation(double tol, int nBreak, MPI_Comm world = MPI_COMM_WORLD) {
    int nLevel = _level.size();
    int rank, size;
    MPI_Comm_rank(world, &rank);
    MPI_Comm_size(world, &size);
    check(isPowerOf2(size), "Number of processors must be power of 2.");

    // XXX
    double duration;
    std::stringstream ss;
    std::fstream fs;
    if (rank == 0) {
      duration = MPI_Wtime();
      ss << DXTC_CONFIG_GET("global.datadir", "data/") << "/mlmc" << size << ".txt";
      fs.open(ss.str(), std::fstream::out);
    }
    // XXX

    // Check input.
    check(nLevel > 0, "No levels set.");
    check(tol > 0, "tol must be positive.");
    check(nBreak > 1, "nBreak must be greater than 1.");

    // Create groups and communicators for different levels.
    int startRepititions = DXTC_CONFIG_GET("mlmc.start_repititions", 16);
    for (int i = 0; i < nLevel; ++i) {
      _level[i].assignProcessors(world);
      _level[i].setRepetitions(std::max(nBreak, nBreak * startRepititions / _level[i].groups()));
    }

    // Loop over breaks and levels
    for (int iBreak = 1; iBreak <= nBreak; ++iBreak) {
      for (int iLevel = 0; iLevel < nLevel; ++iLevel) {
        // Moments of groups.
        Level& l = _level[iLevel];
        MPI_Comm masters = l.getMasters();
        MPI_Comm group = l.getGroup();

        int n = l.nextRepetitions(iBreak, nBreak);
        if (n <= 0)
          continue;

        double gdata[3] = {0};
        gdata[2] = MPI_Wtime();
        for (int i = 0; i < n; ++i) {
          double x = l.eval();
          gdata[0] += x;
          gdata[1] += x * x;
        }
        gdata[2] = MPI_Wtime() - gdata[2];

        double data[3];
        // Accumulate over group masters
        if (l.isMaster()) {
          MPI_Allreduce(gdata, data, 3, MPI_DOUBLE, MPI_SUM, masters);
        }
        // Distribute master results over groups
        MPI_Bcast(data, 3, MPI_DOUBLE, 0, group);
        // Update moments.
        l.update(n, data[0], data[1], data[2]);
      }

      // Compute optimal number of repetitions per algorithm.
      setRepetitions(tol);

      // XXX fs -> std::cout
      if (rank == 0) {
        fs << "Break: " << iBreak << "\n";
        for (int i = 0; i < nLevel; ++i)
          fs << "\tL: " << i
             << "\tn: " << _level[i].doneRepetitions()
             << "\tN: " << _level[i].totalRepetitions()
             << "\tg: " << _level[i].groups()
             << "\tQ: " << _level[i].mean()
             << "\tV: " << _level[i].var()
             << "\tt: " << _level[i].time()
             << "\n";
        fs << "\n";
        fs.flush();
        // XXX
      }
    }

    // Compute expected value from mean values of differences.
    double e = 0;
    for (int i = 0; i < nLevel; ++i)
      e += _level[i].mean();

    // XXX
    if (rank == 0) {
      duration = MPI_Wtime() - duration;
      fs << "Expected value: " << e << " Duration: " << duration << " s\n";
      fs.close();
    }
    // XXX

    return e;
  }

private:
  std::vector<Level> _level; ///< vector of levels
};
} // namespace MultiLevelMonteCarlo {

#endif


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * 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.
*/

#include "SiconosConfig.h"

#include "BlockCSRMatrix.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include "NonSmoothLaw.hpp"
#include "Interaction.hpp"

#include "NewtonEulerDS.hpp"
#include "NewtonEulerR.hpp"
#include "SimulationGraphs.hpp"
#include "SparseBlockMatrix.h" // From numerics, for SparseBlockStructuredMatrix
#include "Tools.hpp"

//#define DEBUG_STDOUT
//#define DEBUG_MESSAGES 1
#include "siconos_debug.h"

// Default constructor: empty matrix
BlockCSRMatrix::BlockCSRMatrix():
  _nr(0),
  _blockCSR(new CompressedRowMat()),
  _sparseBlockStructuredMatrix(new SparseBlockStructuredMatrix()),
  _diagsize0(new IndexInt()),
  _diagsize1(new IndexInt()),
  rowPos(new IndexInt()),
  colPos(new IndexInt())
{}

// Constructor with dimensions
BlockCSRMatrix::BlockCSRMatrix(unsigned int nRow):
  _nr(nRow),
  // Only square-blocks matrices for the moment (ie nRow = nr = nrol)

  // Allocate memory and fill in the matrix rowPos, rowCol ... are
  // initialized with nr to reserve at first step the maximum possible
  // (according to given nr) space in memory.  Thus a future resize
  // will not require memory allocation or copy.
  _blockCSR(new CompressedRowMat(_nr, _nr)),
  _sparseBlockStructuredMatrix(new SparseBlockStructuredMatrix()),
  _diagsize0(new IndexInt(_nr)),
  _diagsize1(new IndexInt(_nr)),
  rowPos(new IndexInt(_nr)),
  colPos(new IndexInt(_nr))
{}

// Basic constructor
BlockCSRMatrix::BlockCSRMatrix(InteractionsGraph& indexSet):
  _nr(indexSet.size()),
  _blockCSR(new CompressedRowMat(_nr, _nr)),
  _sparseBlockStructuredMatrix(new SparseBlockStructuredMatrix()),
  _diagsize0(new IndexInt(_nr)),
  _diagsize1(new IndexInt(_nr)),
  rowPos(new IndexInt(_nr)),
  colPos(new IndexInt(_nr))
{
  DEBUG_BEGIN("BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet)\n");
  fill(indexSet);
  DEBUG_END("BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet)\n");
}

BlockCSRMatrix::~BlockCSRMatrix()
{}

// Fill the SparseMat
void BlockCSRMatrix::fill(InteractionsGraph& indexSet)
{
  // ======> Aim: find inter1 and inter2 both in indexSets[level] and which
  // have common DynamicalSystems.  Then get the corresponding matrix
  // from map blocks.

  // Number of blocks in a row = number of active constraints.
  _nr = indexSet.size();

  // (re)allocate memory for ublas matrix
  _blockCSR->resize(_nr, _nr, false);

  _diagsize0->resize(_nr);
  _diagsize1->resize(_nr);

  // === Loop through "active" Interactions (ie present in
  // indexSets[level]) ===

  int sizeV = 0;

  InteractionsGraph::VIterator vi, viend;
  for(std::tie(vi, viend) = indexSet.vertices();
      vi != viend; ++vi)
  {
    SP::Interaction inter = indexSet.bundle(*vi);

    assert(inter->nonSmoothLaw()->size() > 0);

    sizeV  += inter->nonSmoothLaw()->size();
    (*_diagsize0)[indexSet.index(*vi)] = sizeV;
    (*_diagsize1)[indexSet.index(*vi)] = sizeV;
    assert((*_diagsize0)[indexSet.index(*vi)] > 0);
    assert((*_diagsize1)[indexSet.index(*vi)] > 0);

    (*_blockCSR)(indexSet.index(*vi), indexSet.index(*vi)) =
      indexSet.properties(*vi).block->getArray();
  }

  InteractionsGraph::EIterator ei, eiend;
  for(std::tie(ei, eiend) = indexSet.edges();
      ei != eiend; ++ei)
  {
    InteractionsGraph::VDescriptor vd1 = indexSet.source(*ei);
    InteractionsGraph::VDescriptor vd2 = indexSet.target(*ei);
    SP::Interaction inter1 = indexSet.bundle(vd1);
    SP::Interaction inter2 = indexSet.bundle(vd2);

    assert(indexSet.index(vd1) < _nr);
    assert(indexSet.index(vd2) < _nr);

    assert(indexSet.is_vertex(inter2));

    assert(vd2 == indexSet.descriptor(inter2));
    assert(indexSet.index(vd2) == indexSet.index(indexSet.descriptor(inter2)));

    unsigned int pos = indexSet.index(vd1);
    unsigned int col = indexSet.index(vd2);

    assert(pos != col);

    (*_blockCSR)(std::min(pos, col), std::max(pos, col)) =
      indexSet.properties(*ei).upper_block->getArray();

    (*_blockCSR)(std::max(pos, col), std::min(pos, col)) =
      indexSet.properties(*ei).lower_block->getArray();
  }
  DEBUG_EXPR(display(););
}

void BlockCSRMatrix::fillW(InteractionsGraph& indexSet)
{
  /* on adjoint graph a dynamical system may be on several edges */
  std::map<SP::DynamicalSystem, bool> involvedDS;
  InteractionsGraph::EIterator ei, eiend;
  for(std::tie(ei, eiend) = indexSet.edges();
      ei != eiend; ++ei)
  {
    if(Type::value(*indexSet.bundle(*ei)) != Type::NewtonEulerDS)
    {
      THROW_EXCEPTION("BlockCSRMatrix::fillW only for Newton EulerDS");
    }

    _nr = 0;

    if(involvedDS.find(indexSet.bundle(*ei)) == involvedDS.end())
    {
      _nr++;
      involvedDS[indexSet.bundle(*ei)] = true;
      _blockCSR->resize(_nr, _nr, false);

      (*_blockCSR)(_nr-1, _nr-1) = std::static_pointer_cast<NewtonEulerDS>
                                   (indexSet.bundle(*ei))->mass()->getArray();
    }
  }

  _diagsize0->resize(involvedDS.size());
  _diagsize1->resize(involvedDS.size());

  /* here we suppose NewtonEuler with 6 dofs */
  /* it cannot be another case at this point */
  unsigned int index, ac;
  for(index = 0, ac = 6;
      index < involvedDS.size();
      ++index, ac+=6)
  {
    (*_diagsize0)[index] = ac;
    (*_diagsize1)[index] = ac;
  }

}

void BlockCSRMatrix::fillH(InteractionsGraph& indexSet)
{
  /* on adjoint graph a dynamical system may be on several edges */
  std::map<SP::DynamicalSystem, unsigned int> involvedDS;
  InteractionsGraph::EIterator ei, eiend;
  {
    unsigned int index;
    for(std::tie(ei, eiend) = indexSet.edges(), index=0;
        ei != eiend; ++ei, ++index)
    {
      if(involvedDS.find(indexSet.bundle(*ei)) == involvedDS.end())
      {
        if(Type::value(*indexSet.bundle(*ei)) != Type::NewtonEulerDS)
        {
          THROW_EXCEPTION("BlockCSRMatrix::fillH only for Newton EulerDS");
        }
        involvedDS[indexSet.bundle(*ei)] = index;
      }
    }
  }

  _nr = involvedDS.size();

  _blockCSR->resize(_nr, _nr, false);

  InteractionsGraph::VIterator vi, viend;
  for(std::tie(vi, viend) = indexSet.vertices();
      vi != viend; ++vi)
  {

    SP::DynamicalSystem first = SP::DynamicalSystem();
    unsigned int pos=0, col=0;
    InteractionsGraph::EDescriptor ed1, ed2;
    InteractionsGraph::OEIterator oei, oeiend;
    for(std::tie(oei, oeiend) = indexSet.out_edges(*vi);
        oei != oeiend; ++oei)
    {
      if(!first)
      {
        first = indexSet.bundle(*oei);
        col = involvedDS[first];
        pos = involvedDS[first];
      }
      else
      {
        if(indexSet.bundle(*oei) != first)
        {
          pos = involvedDS[indexSet.bundle(*oei)];
        }
      }
    }

    (*_blockCSR)(std::min(pos, col), std::max(pos, col)) =
      std::static_pointer_cast<NewtonEulerR>(indexSet.bundle(*vi)->relation())->jachqT()->getArray();

    (*_blockCSR)(std::max(pos, col), std::min(pos, col)) =
      std::static_pointer_cast<NewtonEulerR>(indexSet.bundle(*vi)->relation())->jachqT()->getArray();

  }

  _diagsize0->resize(involvedDS.size());
  _diagsize1->resize(involvedDS.size());

  /* only NewtonEuler3DR */
  unsigned int index, ac0, ac1;
  for(index= 0, ac0 = 6, ac1 = 3;
      index < involvedDS.size();
      ++index, ac0 +=6, ac1 +=3)
  {
    (*_diagsize0)[index] = ac0;
    (*_diagsize1)[index] = ac1;
  }

}

// convert _blockCSR to numerics structure
void BlockCSRMatrix::convert()
{
  DEBUG_BEGIN("void BlockCSRMatrix::convert()\n");
  _sparseBlockStructuredMatrix->blocknumber0 = _nr;
  _sparseBlockStructuredMatrix->blocknumber1 = _nr;  // nc not always set
  _sparseBlockStructuredMatrix->nbblocks = (*_blockCSR).nnz();
  // Next copies: pointer links!!
  _sparseBlockStructuredMatrix->blocksize0 =  _diagsize0->data();
  _sparseBlockStructuredMatrix->blocksize1 =  _diagsize1->data(); // nr = nc

  // boost
  _sparseBlockStructuredMatrix->filled1 = (*_blockCSR).filled1();
  _sparseBlockStructuredMatrix->filled2 = (*_blockCSR).filled2();
  _sparseBlockStructuredMatrix->index1_data = _blockCSR->index1_data().begin();
  if(_nr > 0)
  {
    _sparseBlockStructuredMatrix->index2_data = _blockCSR->index2_data().begin();
    _sparseBlockStructuredMatrix->block =  _blockCSR->value_data().begin();
  };
  if(_sparseBlockStructuredMatrix->diagonal_blocks)
  {
    free(_sparseBlockStructuredMatrix->diagonal_blocks);
    _sparseBlockStructuredMatrix->diagonal_blocks = nullptr;
  }
  //   // Loop through the non-null blocks
  //   for (SpMatIt1 i1 = _blockCSR->begin1(); i1 != _blockCSR->end1(); ++i1)
  //     {
  //       for (SpMatIt2 i2 = i1.begin(); i2 != i1.end(); ++i2)
  //  {
  //    block[i] = *i2;
  //  }
  //     }
  DEBUG_END("void BlockCSRMatrix::convert()\n");
}

// Display data
void BlockCSRMatrix::display() const
{
  std::cout << "----- Sparse Block Matrix with "
            << _nr << " blocks in a row/col and "
            << _blockCSR->nnz()
            << " non-null blocks" <<std::endl;
  std::cout << "filled1 (index of the last non empty line + 1):" << _blockCSR->filled1() <<std::endl;
  std::cout << "filled2 (number of non null blocks):" << _blockCSR->filled2() <<std::endl;
  std::cout << "_blockCSR->index1_data().size()" << _blockCSR->index1_data().size() << std::endl;
  print(_blockCSR->index1_data().begin(), _blockCSR->index1_data().end(),"index1_data", "\t");

  assert(_blockCSR->index2_data().size() >= _blockCSR->filled2());

  std::cout << "_blockCSR->index2_data().size()" << _blockCSR->index2_data().size() << std::endl;
  print(_blockCSR->index2_data().begin(), _blockCSR->index2_data().end(), "index2_data (column number for each block)", "\t");

  std::cout << "last column number  "<<   _blockCSR->index2_data()[_blockCSR->filled2()-1] <<  " for block   " << _blockCSR->filled2() << std::endl;
  print(_diagsize0->begin(), _diagsize0->end(),"_diagsize0 , sum of row sizes of the diagonal blocks", "\t");
  print(_diagsize1->begin(), _diagsize1->end(),"_diagsize1 , sum of col sizes of the diagonal blocks", "\t");
}

unsigned int BlockCSRMatrix::getNbNonNullBlocks() const
{
  return _blockCSR->nnz();
};


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

/**
 * @file rkf_integrator.hpp
 * @brief Integration of ODEs using the Runge-Kutta-Fehlberg method
 */

#ifndef RKF_INTEGRATOR_H
#define RKF_INTEGRATOR_H

#include "error.hpp"
#include <string>

#include <Eigen/Core>

namespace flexiblesusy {

namespace runge_kutta {

/**
 * @class RKF_integrator
 * @brief Class for integrating ODEs using a Runge-Kutta-Fehlberg method
 *
 * This class makes use of the 8th order Runge-Kutta-Fehlberg algorithm
 * provided by the odeint library in Boost.
 */
class RKF_integrator {
public:
   using Derivs = std::function<Eigen::ArrayXd(double, const Eigen::ArrayXd&)>;

   /// Integrates the system over an interval
   void operator()(double start, double end, Eigen::ArrayXd& pars,
                   const Derivs& derivs, double tol) const;
private:
   class DisabledOdeintError : Error {
   public:
      explicit DisabledOdeintError(const std::string& msg_) : msg(msg_) {}
      virtual ~DisabledOdeintError() = default;
      virtual std::string what() const override { return msg; }
   private:
      std::string msg;
   };

   struct RKF_observer {
      void operator()(const Eigen::ArrayXd&, double) const;
   };
};

} // namespace runge_kutta

} // namespace flexiblesusy

#endif