/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Implements the "Walking Control Based on Step Timing Adaptation" foot
 * trajectory QP. The pdf can be found in https://arxiv.org/abs/1704.01271, and
 * in the `doc/` folder in this repository.
 */

#pragma once

#include <eigen-quadprog/QuadProg.h>
#include <Eigen/Eigen>
#include <chrono>
#include <iostream>
#include <limits>
#define RESET "\033[0m"
#define RED "\033[31m"  /* Red */
#define BLUE "\033[34m" /* Blue */
#define MAX_VAR 500
#define EPSILON 1e-9

namespace reactive_planners
{
/**
 * @brief
 */
class DynamicallyConsistentEndEffectorTrajectory
{
    /*
     * Private methods
     */
public:
    /** @brief Constructor. */
    DynamicallyConsistentEndEffectorTrajectory();

    /** @brief Destructor. */
    ~DynamicallyConsistentEndEffectorTrajectory();

    bool compute(const Eigen::Ref<const Eigen::Vector3d>& start_pose,
                 const Eigen::Ref<const Eigen::Vector3d>& current_pose,
                 const Eigen::Ref<const Eigen::Vector3d>& current_velocity,
                 const Eigen::Ref<const Eigen::Vector3d>& target_pose,
                 const double& start_time,
                 const double& current_time,
                 const double& end_time,
                 const bool& is_left_leg_in_contact);

    void update_robot_status(Eigen::Ref<Eigen::Vector3d> next_pose,
                             Eigen::Ref<Eigen::Vector3d> next_velocity,
                             Eigen::Ref<Eigen::Vector3d> next_acceleration);

    /** @brief Get all the forces until landing foot. Returns the number of
     * forces.*/
    int get_forces(Eigen::Ref<Eigen::VectorXd> forces,
                   Eigen::Ref<Eigen::Vector3d> next_pose,
                   Eigen::Ref<Eigen::Vector3d> next_velocity,
                   Eigen::Ref<Eigen::Vector3d> next_acceleration);

    /** @brief Display the matrices of the Problem. */
    void print_solver() const;

    /** @brief Convert the inner data to a string format. */
    std::string to_string() const;

    /*
     * Getters
     */

    /** @brief Get the height of the flying foot. */
    double get_mid_air_height() const
    {
        return mid_air_height_;
    }

    /*
     * Setters
     */
    /** @brief Set the height of the flying foot.
     *
     * @param mid_air_height
     */
    void set_planner_loop(double planner_loop)
    {
        planner_loop_ = planner_loop;
        init_acceleration_velocity_terms();
    }

    /** @brief Set the height of the flying foot.
     *
     * @param mid_air_height
     */
    void set_mid_air_height(double mid_air_height)
    {
        mid_air_height_ = mid_air_height;
    }

    /**
     * @brief return cost.
     *
     * @return double
     */
    double cost()
    {
        x_opt_.resize(nb_var_);
        x_opt_ = qp_solver_.result();
        return (0.5 * x_opt_.transpose() * Q_ * x_opt_ +
                q_.transpose() * x_opt_)(0, 0);
    }

    /**
     * @brief Return the slack values from the last solution.
     **/
    const Eigen::Vector3d& get_slack_variables() const
    {
        return slack_variables_;
    }

    double calculate_t_min(
        const Eigen::Ref<const Eigen::Vector3d>& current_pose,
        const Eigen::Ref<const Eigen::Vector3d>& current_velocity,
        const bool& is_left_leg_in_contact);
    /*
     * Private methods
     */
private:
    /** @brief resize QP variable at each step.
     */
    void resize_matrices()
    {
        Q_.resize(nb_var_, nb_var_);
        Q_.setZero();
        q_.resize(nb_var_);
        q_.setZero();

        A_eq_.resize(nb_eq_, nb_var_);
        A_eq_.setZero();
        B_eq_.resize(nb_eq_);
        B_eq_.setZero();

        A_ineq_.resize(nb_ineq_, nb_var_);
        A_ineq_.setZero();
        B_ineq_.resize(nb_ineq_);
        B_ineq_.setZero();
        qp_solver_.problem(nb_var_, nb_eq_, nb_ineq_);
    }
    /** @brief resize QP variable at each step.
     */
    void resize_matrices_t_min(int index)
    {
        Q_t_min_.resize(3 * index, 3 * index);
        Q_t_min_ = Eigen::MatrixXd::Identity(3 * index, 3 * index);
        q_t_min_.resize(3 * index);
        q_t_min_.setZero();

        A_eq_t_min_.resize(2, 3 * index);
        A_eq_t_min_.setZero();
        B_eq_t_min_.resize(2);
        B_eq_t_min_.setZero();

        A_ineq_t_min_.resize(2 * 3 * index, 3 * index);
        A_ineq_t_min_.setZero();
        B_ineq_t_min_.resize(2 * 3 * index);
        B_ineq_t_min_.setZero();
        qp_solver_t_min_.problem(3 * index, 2, 2 * 3 * index);
    }

    /** @brief initialie acceleration_terms and velocity_terms.
     */
    void init_acceleration_velocity_terms();

    /*
     * Constant problem parameters.
     */

    /** @brief Flying foot apex to be reach mid-air. */
    double mid_air_height_;

    /** @brief Number of force variables in the optimization problem on the
     * one of the axes. */
    int nb_local_sampling_time_;

    /** @brief non_linear_terms.
     */
    Eigen::Vector3d h_c;

    /*
     * Variable problem parameters.
     */

    /** @brief affect of forces_x to the positions.
     * There are two separate matrices for each leg.
     */
    Eigen::MatrixXd* position_terms_F_x_;

    /** @brief affect of forces_y to the positions.
     * There are two separate matrices for each leg.
     */
    Eigen::MatrixXd* position_terms_F_y_;

    /** @brief affect of forces_z to the positions.
     * There are two separate matrices for each leg.
     */
    Eigen::MatrixXd* position_terms_F_z_;

    /** @brief Desired velocity. */
    Eigen::Vector3d v_des_;

    /** @brief Slack variable values corresponding to last solution. */
    Eigen::Vector3d slack_variables_;

    /** @brief affect of non_linear_terms in position. */
    Eigen::MatrixXd* non_linear_terms;

    /** @brief Initial position before the motion. */
    Eigen::Vector3d start_pose_;

    /** @brief Current position. */
    Eigen::Vector3d current_pose_;

    /** @brief Previous computed position from the QP. */
    Eigen::Vector3d previous_solution_pose_;

    /** @brief Current velocity. */
    Eigen::Vector3d current_velocity_;

    /** @brief Current acceleration. */
    Eigen::Vector3d current_acceleration_;

    /** @brief Target pose after the motion. */
    Eigen::Vector3d target_pose_;

    /** @brief Initial time. */
    double start_time_;

    /** @brief Current time. */
    double current_time_;

    /** @brief Last end time register when we computed the QP. */
    double last_end_time_seen_;

    /** @brief Control loop. */
    double control_loop_;

    /** @brief planner loop. */
    double planner_loop_;

    /*
     * QP variables
     */

    /** @brief Number of variables in the optimization problem. */
    int nb_var_;

    /** @brief Number of equality constraints in the optimization problem. */
    int nb_eq_;

    /** @brief Number of inequality in the optimization problem. */
    int nb_ineq_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_t_min_;

    /** @brief Solution of the optimization problem.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd x_opt_;

    /** @brief Quadratic term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd Q_;
    /** @brief Quadratic term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd Q_t_min_;

    /** @brief inverse estimation of mass matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd* M_inv_;

    /** @brief Is the left foot in contact? otherwise the right foot is. */
    bool is_left_leg_in_contact_;

    /** @brief Quadratic term added to the quadratic cost in order regularize
     * the system.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd Q_regul_;

    /** @brief Cost weights for forces. */
    double cost_;

    /** @brief Cost weights for the epsilon_z_mid. */
    double cost_epsilon_z_mid_;

    /** @brief Cost weights for the epsilon_x. */
    double cost_epsilon_x_;

    /** @brief Cost weights for the epsilon_y. */
    double cost_epsilon_y_;

    /** @brief Cost weights for the epsilon_z. */
    double cost_epsilon_z_;

    /** @brief Cost weights for the epsilon_z. */
    double cost_epsilon_vel_;

    /** @brief Linear term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd q_;

    /** @brief Linear equality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_eq_;

    /** @brief Linear equality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_eq_;

    /** @brief Linear inequality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_ineq_;

    /** @brief Linear inequality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_ineq_;

    /** @brief Linear term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd q_t_min_;

    /** @brief Linear equality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_eq_t_min_;

    /** @brief Linear equality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_eq_t_min_;

    /** @brief Linear inequality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_ineq_t_min_;

    /** @brief Linear inequality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_ineq_t_min_;
};

}  // namespace reactive_planners


#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <opengm/inference/inference.hxx>
#include <opengm/graphicalmodel/graphicalmodel.hxx>
#include <opengm/inference/auxiliary/fusion_move/fusion_mover.hxx>

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

// Fusion Move Solver
#include "opengm/inference/astar.hxx"
#include "opengm/inference/lazyflipper.hxx"
#include "opengm/inference/infandflip.hxx"
#include <opengm/inference/messagepassing/messagepassing.hxx>

#ifdef WITH_AD3
#include "opengm/inference/external/ad3.hxx"
#endif
#ifdef WITH_CPLEX
#include "opengm/inference/lpcplex.hxx"
#endif
#ifdef WITH_QPBO
#include "QPBO.h"
#endif

template<class GM,class ACC>
class PythonFusionMover{
	typedef GM GraphicalModelType;
    typedef ACC AccumulationType;
    OPENGM_GM_TYPE_TYPEDEFS;
    typedef opengm::FusionMover<GM,ACC> CppFusionMover;

	typedef typename CppFusionMover::SubGmType 				SubGmType;
	// sub-inf-astar
	typedef opengm::AStar<SubGmType,AccumulationType> 			AStarSubInf;
	// sub-inf-lf
	typedef opengm::LazyFlipper<SubGmType,AccumulationType> 	LazyFlipperSubInf;
	// sub-inf-bp
	typedef opengm::BeliefPropagationUpdateRules<SubGmType,AccumulationType> 						UpdateRulesType;
    typedef opengm::MessagePassing<SubGmType,AccumulationType,UpdateRulesType, opengm::MaxDistance> BpSubInf;
    // sub-inf-bp-lf
    typedef opengm::InfAndFlip<SubGmType,AccumulationType,BpSubInf>        BpLfSubInf;

	//#ifdef WITH_AD3
	//typedef opengm::external::AD3Inf<SubGmType,AccumulationType> 	Ad3SubInf;
	//#endif
	#ifdef WITH_QPBO
	typedef kolmogorov::qpbo::QPBO<double> 			  				QpboSubInf;
	#endif
	#ifdef WITH_CPLEX
	typedef opengm::LPCplex<SubGmType,AccumulationType> 			CplexSubInf;
	#endif

public:
	PythonFusionMover(const GM & gm)
	:	gm_(gm),
		fusionMover_(gm),
		argA_(gm.numberOfVariables()),
		argB_(gm.numberOfVariables()),
		argR_(gm.numberOfVariables()),
		factorOrder_(gm.factorOrder())
	{

	}

	boost::python::tuple fuse(
		opengm::python::NumpyView<LabelType,1> labelsA,
		opengm::python::NumpyView<LabelType,1> labelsB,
		const std::string & fusionSolver
	)
	{
		// copy input
		//#std::cout<<"copy input\n";
		std::copy(labelsA.begin(),labelsA.end(),argA_.begin());
		std::copy(labelsB.begin(),labelsB.end(),argB_.begin());

		// do setup
		//#std::cout<<"setup\n";
		fusionMover_.setup(
			argA_,argB_,argR_,
			gm_.evaluate(argA_.begin()),gm_.evaluate(argB_.begin())
		);
		//std::cout<<"do fusion\n";
		// do the fusion
		const ValueType resultValue = this->doFusion(fusionSolver);

		//std::cout<<"make result\n";
		// return result
	  	return boost::python::make_tuple(
	  		opengm::python::iteratorToNumpy(argR_.begin(), argR_.size()),
	  		fusionMover_.valueResult(),
	  		fusionMover_.valueA(),
	  		fusionMover_.valueB()
	  	);
	}

private:

	ValueType doFusion(const std::string & fusionSolver){
		if(fusionSolver==std::string("qpbo")){
			#ifdef WITH_QPBO
			if(factorOrder_<=2){
				return fusionMover_. template fuseQpbo<QpboSubInf> ();
			}
			else{
				return fusionMover_. template fuseFixQpbo<QpboSubInf> ();
			}
			#else
				OPENGM_CHECK(false,"qpbo fusion solver need WITH_QPBO");
			#endif
		}
		else if(fusionSolver==std::string("lf2")){
			return  fusionMover_. template fuse<LazyFlipperSubInf> (typename LazyFlipperSubInf::Parameter(2),true);
		}
		else if(fusionSolver==std::string("lf3")){
			return  fusionMover_. template fuse<LazyFlipperSubInf> (typename LazyFlipperSubInf::Parameter(3),true);
		}

	}

	const GM & gm_;
	CppFusionMover fusionMover_;
	std::vector<LabelType> argA_;
	std::vector<LabelType> argB_;
	std::vector<LabelType> argR_;
	size_t factorOrder_;
};

template<class GM,class ACC>
void export_fusion_moves(){
   using namespace boost::python;
   boost::python::numeric::array::set_module_and_type("numpy", "ndarray");
   boost::python::docstring_options docstringOptions(true,true,false);

   import_array();

   typedef PythonFusionMover<GM,ACC> PyFusionMover;

	boost::python::class_<PyFusionMover > ("FusionMover",
	boost::python::init<const GM &>("Construct a FusionMover from a graphical model ")
	[with_custodian_and_ward<1 /*custodian == self*/, 2 /*ward == const PyGM& */>()]
	)
	.def("fuse",&PyFusionMover::fuse)
	;
}

template void export_fusion_moves<opengm::python::GmAdder ,opengm::Minimizer>();


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_SSE_SSE2_ALL_HPP_INCLUDED
#define NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_SSE_SSE2_ALL_HPP_INCLUDED

#include <nt2/sdk/meta/strip.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/hmsb.hpp>
#include <nt2/include/functions/split.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <boost/fusion/tuple.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::all_, tag::cpu_,
                      (A0),
                      ((simd_<arithmetic_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::all_(tag::simd_<tag::arithmetic_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>    { typedef bool type; };

    NT2_FUNCTOR_CALL(1)
    {
      return hmsb(is_eqz(a0)) == 0;
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is uint8_t
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::all_, tag::cpu_,
                      (A0),
                      ((simd_<uint8_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::all_(tag::simd_<tag::uint8_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>    { typedef bool type; };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type                            stype;
      typedef typename meta::upgrade<stype>::type                           utype;
      typedef simd::native<utype,tag::sse_>                                 ttype;
      ttype a0h, a0l;
      boost::fusion::tie(a0h, a0l) = split(a0);
      return (hmsb(is_eqz(a0h)) || hmsb(is_eqz(a0l))) == 0;
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int8_t
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::all_, tag::cpu_,
                      (A0),
                      ((simd_<int8_<A0>,tag::sse_>))
                     );

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::all_(tag::simd_<tag::int8_, tag::sse_> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>    { typedef bool type; };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type                            stype;
      typedef typename meta::upgrade<stype>::type                           utype;
      typedef simd::native<utype,tag::sse_>                                 ttype;
      ttype a0h, a0l;
      boost::fusion::tie(a0h, a0l) = split(a0);
      return (hmsb(is_eqz(a0h)) || hmsb(is_eqz(a0l)))== 0;
    }
  };
} }

#endif
// modified by jt the 05/01/2011


#define BOOST_TEST_MODULE pcraster dal step_coordinate_mapper
#include <boost/test/unit_test.hpp>
#include "dal_DataSpace.h"
#include "dal_StepCoordinateMapper.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace dal;

  {
    // Catsop:
    size_t nrCols = 80;
    double cellSize = 10.0;
    double west = 182140.0;
    StepCoordinateMapper mapper(1, nrCols,
         west + 0.5 * cellSize, west + (double(nrCols) - 0.5) * cellSize,
         SetToMissingValue);

    std::vector<size_t> cols;
    cols.push_back(1);
    cols.push_back(100);
    cols.push_back(1);

    DataSpace space;
    space.addDimension(Dimension(Space, cols));
    DataSpaceAddress address = space.address();

    address.setCoordinate<size_t>(0, 1);
    BOOST_CHECK_EQUAL(mapper.toString(space, address, 0),  "182145");

    address.setCoordinate<size_t>(0, nrCols);
    BOOST_CHECK_EQUAL(mapper.toString(space, address, 0),  "182935");
  }

  //  1 "10-2-2005, 18:15: volcano_1"
  //  2 "11-2-2005, 24:15: volcano_1"
  //  3 "11-2-2005, 06:15: volcano_1"
  //  4 "11-2-2005, 12:15: volcano_1"
  //  5 "11-2-2005, 18:15: volcano_2, lava_1"
  //  6 "12-2-2005, 24:15: volcano_2, lava_2"
  //  7 "12-2-2005, 06:15: volcano_2, lava_3"
  //  8 "12-2-2005, 12:15: volcano_2, lava_4"
  //  9 "12-2-2005, 18:15: volcano_3, lava_5"
  // 10 "13-2-2005, 24:15: volcano_3, lava_6"
  // 11 "13-2-2005, 06:15: volcano_3, lava_7"
  // 12 "13-2-2005, 12:15: volcano_3, lava_8"
  // 13 "13-2-2005, 18:15: volcano_3, lava_9"
  {
    // 1 - 13 -> 1 - 4
    StepCoordinateMapper mapper(1, 9, 1, 3, UsePrevious);

    std::vector<size_t> steps;
    steps.push_back(1);
    steps.push_back(13);
    steps.push_back(1);

    DataSpace space;
    space.addDimension(Dimension(Time, steps));
    DataSpaceAddress address = space.address();

    address.setCoordinate<size_t>(0, 1);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(1));

    address.setCoordinate<size_t>(0, 2);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(1));

    address.setCoordinate<size_t>(0, 3);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(1));

    address.setCoordinate<size_t>(0, 4);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(1));

    address.setCoordinate<size_t>(0, 5);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(2));

    address.setCoordinate<size_t>(0, 8);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(2));

    address.setCoordinate<size_t>(0, 9);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(3));
  }

  {
    // 5 - 13 -> 1 - 9
    StepCoordinateMapper mapper(5, 13, 1, 9, UsePrevious);

    std::vector<size_t> steps;
    steps.push_back(5);
    steps.push_back(13);
    steps.push_back(1);

    DataSpace space;
    space.addDimension(Dimension(Time, steps));
    DataSpaceAddress address = space.address();

    address.setCoordinate<size_t>(0, 4);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK(!address.isValid(0));

    address.setCoordinate<size_t>(0, 5);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(1));

    address.setCoordinate<size_t>(0, 6);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(2));

    address.setCoordinate<size_t>(0, 7);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(3));

    address.setCoordinate<size_t>(0, 13);
    mapper.mapToDestination(space, address, 0);
    BOOST_CHECK_EQUAL(address.coordinate<size_t>(0),  size_t(9));
  }
}


#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_EIGEN_EIGEN_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_EIGEN_EIGEN_ALGEBRA_HPP_INCLUDED

#include <Eigen/Dense>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/version.hpp>

// Necessary routines for Eigen matrices to work with vector_space_algebra
// from odeint
// (that is, it lets odeint treat the Eigen matrices correctly, knowing
// how to add, multiply, compute the norm, etc)

#if (EIGEN_VERSION_AT_LEAST(3, 3, 0) && BOOST_VERSION < 107100)
namespace Eigen
{
namespace internal
{

template <typename Scalar>
struct scalar_add_op
{
    // FIXME default copy constructors seems bugged with std::complex<>
    EIGEN_DEVICE_FUNC inline scalar_add_op(const scalar_add_op &other) : m_other(other.m_other) {}
    EIGEN_DEVICE_FUNC inline scalar_add_op(const Scalar &other) : m_other(other) {}
    EIGEN_DEVICE_FUNC inline Scalar operator()(const Scalar &a) const { return a + m_other; }
    template <typename Packet>
    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet &a) const
    {
        return internal::padd(a, pset1<Packet>(m_other));
    }
    const Scalar m_other;
};

template <typename Scalar>
struct functor_traits<scalar_add_op<Scalar>>
{
    enum
    {
        Cost = NumTraits<Scalar>::AddCost,
        PacketAccess = packet_traits<Scalar>::HasAdd
    };
};

} // namespace internal

template <typename D>
inline const typename Eigen::CwiseUnaryOp<
    typename Eigen::internal::scalar_add_op<
        typename Eigen::internal::traits<D>::Scalar>,
    const D>
operator+(const typename Eigen::MatrixBase<D> &m,
          const typename Eigen::internal::traits<D>::Scalar &s)
{
    return Eigen::CwiseUnaryOp<
        typename Eigen::internal::scalar_add_op<
            typename Eigen::internal::traits<D>::Scalar>,
        const D>(m.derived(), Eigen::internal::scalar_add_op<
                                  typename Eigen::internal::traits<D>::Scalar>(s));
}

template <typename D>
inline const typename Eigen::CwiseUnaryOp<
    typename Eigen::internal::scalar_add_op<
        typename Eigen::internal::traits<D>::Scalar>,
    const D>
operator+(const typename Eigen::internal::traits<D>::Scalar &s,
          const typename Eigen::MatrixBase<D> &m)
{
    return Eigen::CwiseUnaryOp<
        typename Eigen::internal::scalar_add_op<
            typename Eigen::internal::traits<D>::Scalar>,
        const D>(m.derived(), Eigen::internal::scalar_add_op<
                                  typename Eigen::internal::traits<D>::Scalar>(s));
}

template <typename D1, typename D2>
inline const typename Eigen::CwiseBinaryOp<
    typename Eigen::internal::scalar_quotient_op<
        typename Eigen::internal::traits<D1>::Scalar>,
    const D1, const D2>
operator/(const Eigen::MatrixBase<D1> &x1, const Eigen::MatrixBase<D2> &x2)
{
    return x1.cwiseQuotient(x2);
}

template <typename D>
inline const typename Eigen::CwiseUnaryOp<
    typename Eigen::internal::scalar_abs_op<
        typename Eigen::internal::traits<D>::Scalar>,
    const D>
abs(const Eigen::MatrixBase<D> &m)
{
    return m.cwiseAbs();
}

} // namespace Eigen

namespace boost
{
namespace numeric
{
namespace odeint
{

template <int S1, int S2, int O, int M1, int M2>
struct vector_space_norm_inf<Eigen::Matrix<double, S1, S2, O, M1, M2>>
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef double result_type;
    result_type operator()(const Eigen::Matrix<double, S1, S2, O, M1, M2> &m) const
    {
        return m.template lpNorm<Eigen::Infinity>();
    }
};

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif
#endif


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef float																                             real_type;
typedef OpenTissue::math::default_math_types		  			                 math_types;
typedef math_types::vector3_type                                         vector3_type;
typedef math_types::value_traits                                         value_traits;

typedef OpenTissue::skeleton::DefaultBoneTraits<math_types>                 default_bone_traits;
typedef OpenTissue::kinematics::inverse::BoneTraits< default_bone_traits >  bone_traits;

typedef OpenTissue::skeleton::Types<math_types, bone_traits>             skeleton_types;
typedef skeleton_types::skeleton_type                                    skeleton_type;
typedef OpenTissue::kinematics::inverse::NonlinearSolver<skeleton_type>  solver_type;

template< typename bone_type>
void test_bones(bone_type const * A, bone_type const * B)
{
  using std::fabs;

  BOOST_CHECK(A->get_name() == B->get_name() );

  real_type const tol = 10e-7;

  BOOST_CHECK( fabs( A->bind_pose().T()(0) - B->bind_pose().T()(0) ) < tol );
  BOOST_CHECK( fabs( A->bind_pose().T()(1) - B->bind_pose().T()(1) ) < tol );
  BOOST_CHECK( fabs( A->bind_pose().T()(2) - B->bind_pose().T()(2) ) < tol );

  BOOST_CHECK( fabs( A->bone_space().T()(0) - B->bone_space().T()(0) ) < tol );
  BOOST_CHECK( fabs( A->bone_space().T()(1) - B->bone_space().T()(1) ) < tol );
  BOOST_CHECK( fabs( A->bone_space().T()(2) - B->bone_space().T()(2) ) < tol );

  BOOST_CHECK( fabs( A->bone_space().Q().s() - B->bone_space().Q().s() ) < tol );
  BOOST_CHECK( fabs( A->bone_space().Q().v()(0) - B->bone_space().Q().v()(0) ) < tol );
  BOOST_CHECK( fabs( A->bone_space().Q().v()(1) - B->bone_space().Q().v()(1) ) < tol );
  BOOST_CHECK( fabs( A->bone_space().Q().v()(2) - B->bone_space().Q().v()(2) ) < tol );

  BOOST_CHECK( fabs( A->bind_pose().Q().s() - B->bind_pose().Q().s() ) < tol );
  BOOST_CHECK( fabs( A->bind_pose().Q().v()(0) - B->bind_pose().Q().v()(0) ) < tol );
  BOOST_CHECK( fabs( A->bind_pose().Q().v()(1) - B->bind_pose().Q().v()(1) ) < tol );
  BOOST_CHECK( fabs( A->bind_pose().Q().v()(2) - B->bind_pose().Q().v()(2) ) < tol );

  BOOST_CHECK( A->type() == B-> type() );
  if(A->active_dofs() == 1u)
  {
    BOOST_CHECK( fabs( A->box_limits().min_limit(0) - B->box_limits().min_limit(0) ) < tol );
    BOOST_CHECK( fabs( A->box_limits().max_limit(0) - B->box_limits().max_limit(0) ) < tol );
    BOOST_CHECK( fabs( A->u()(0) - B->u()(0)  ) < tol );
    BOOST_CHECK( fabs( A->u()(1) - B->u()(1)  ) < tol );
    BOOST_CHECK( fabs( A->u()(2) - B->u()(2)  ) < tol );
  }
  else
  {
    BOOST_CHECK( fabs( A->box_limits().min_limit(0) - B->box_limits().min_limit(0) ) < tol );
    BOOST_CHECK( fabs( A->box_limits().min_limit(1) - B->box_limits().min_limit(1) ) < tol );
    BOOST_CHECK( fabs( A->box_limits().min_limit(2) - B->box_limits().min_limit(2) ) < tol );
    BOOST_CHECK( fabs( A->box_limits().max_limit(0) - B->box_limits().max_limit(0) ) < tol );
    BOOST_CHECK( fabs( A->box_limits().max_limit(1) - B->box_limits().max_limit(1) ) < tol );
    BOOST_CHECK( fabs( A->box_limits().max_limit(2) - B->box_limits().max_limit(2) ) < tol );
  }
  if(A->parent())
  {
    BOOST_CHECK(B->parent());
    BOOST_CHECK(A->parent()->get_name() == B->parent()->get_name() );
  }
}

template<typename chain_type>
void test_chains( chain_type const * A, chain_type const * B)
{
  BOOST_CHECK( A->get_root()->get_name() == B->get_root()->get_name() );
  BOOST_CHECK( A->get_end_effector()->get_name() == B->get_end_effector()->get_name() );
  BOOST_CHECK( A->weight_p() == B->weight_p() );
  BOOST_CHECK( A->weight_x() == B->weight_x() );
  BOOST_CHECK( A->weight_y() == B->weight_y() );
  BOOST_CHECK( A->p_global() == B->p_global() );
  BOOST_CHECK( A->x_global() == B->x_global() );
  BOOST_CHECK( A->y_global() == B->y_global() );
  BOOST_CHECK( A->p_local() == B->p_local() );
  BOOST_CHECK( A->x_local() == B->x_local() );
  BOOST_CHECK( A->y_local() == B->y_local() );
}

template<typename solver_type>
void test_solvers( solver_type const & A, solver_type const & B)
{
  BOOST_CHECK( A.size() == B.size() );
  typename solver_type::skeleton_type::const_bone_iterator bone1 =  A.skeleton()->begin();
  typename solver_type::skeleton_type::const_bone_iterator bone2 =  B.skeleton()->begin();
  typename solver_type::skeleton_type::const_bone_iterator bend1 =  A.skeleton()->end();
  typename solver_type::skeleton_type::const_bone_iterator bend2 =  B.skeleton()->end();
  for(;bone1!=bend1;++bone1, ++bone2)
    test_bones( &(*bone1), &(*bone2) );
  typename solver_type::const_chain_iterator chain1 =  A.chain_begin();
  typename solver_type::const_chain_iterator chain2 =  B.chain_begin();
  typename solver_type::const_chain_iterator cend1  =  A.chain_end();
  typename solver_type::const_chain_iterator cend2  =  B.chain_end();
  for(;chain1!=cend1;++chain1, ++chain2)
    test_chains( &(*chain1), &(*chain2) );
}

BOOST_AUTO_TEST_SUITE(opentissue_kinematics_inverse_ik_io);

BOOST_AUTO_TEST_CASE(test_cases)
{
  // Test that we get an exception if skeleton is missing!
  {
    solver_type solver;
    BOOST_CHECK_THROW( OpenTissue::kinematics::inverse::xml_write( "test.xml", solver ), std::invalid_argument );
    BOOST_CHECK_THROW( OpenTissue::kinematics::inverse::xml_read( "test.xml", solver ), std::invalid_argument );
  }

  // Test cases where we have a simple skeleton
  {
    // First we create a skeleton and fill it with some dummy data
    skeleton_type skeleton1;
    skeleton_type::bone_type * b0 = skeleton1.create_bone();
    skeleton_type::bone_type * b1 = skeleton1.create_bone(b0);
    skeleton_type::bone_type * b2 = skeleton1.create_bone(b0);
    skeleton_type::bone_type * b3 = skeleton1.create_bone(b2);

    b0->relative().T() = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() );
    b1->relative().T() = vector3_type( value_traits::one(), value_traits::one(),  value_traits::zero() );
    b2->relative().T() = vector3_type( value_traits::one(), value_traits::zero(), value_traits::one() );
    b3->relative().T() = vector3_type( value_traits::one(), value_traits::two(),  value_traits::zero() );
    b0->relative().Q().Rx(value_traits::pi_quarter() );
    b1->relative().Q().Ry(value_traits::pi_half() );
    b2->relative().Q().Rz(value_traits::pi() );
    b3->relative().Q().Rx(value_traits::pi_half() );

    b0->bind_pose().T() = vector3_type( value_traits::two(), value_traits::zero(), value_traits::zero() );
    b1->bind_pose().T() = vector3_type( value_traits::two(), value_traits::two(),  value_traits::zero() );
    b2->bind_pose().T() = vector3_type( value_traits::two(), value_traits::zero(), value_traits::two() );
    b3->bind_pose().T() = vector3_type( value_traits::two(), value_traits::one(),  value_traits::zero() );
    b0->bind_pose().Q().Rz(value_traits::pi_quarter() );
    b1->bind_pose().Q().Ry(value_traits::pi_half() );
    b2->bind_pose().Q().Rx(value_traits::pi() );
    b3->bind_pose().Q().Rz(value_traits::pi_half() );

    b0->bone_space().T() = vector3_type( value_traits::four(), value_traits::zero(), value_traits::zero() );
    b1->bone_space().T() = vector3_type( value_traits::four(), value_traits::four(),  value_traits::zero() );
    b2->bone_space().T() = vector3_type( value_traits::four(), value_traits::zero(), value_traits::four() );
    b3->bone_space().T() = vector3_type( value_traits::four(), value_traits::two(),  value_traits::zero() );
    b0->bone_space().Q().Ry(value_traits::pi_quarter() );
    b1->bone_space().Q().Rx(value_traits::pi_half() );
    b2->bone_space().Q().Rz(value_traits::pi() );
    b3->bone_space().Q().Ry(value_traits::pi_half() );

    BOOST_CHECK( skeleton1.rename_bone( b0, "my_first_bone" )  );
    BOOST_CHECK( skeleton1.rename_bone( b1, "my_second_bone" ) );
    BOOST_CHECK( skeleton1.rename_bone( b2, "my_third_bone" )  );
    BOOST_CHECK( skeleton1.rename_bone( b3, "my_fourth_bone" ) );

    b0->type() = bone_traits::hinge_type;
    b1->type() = bone_traits::slider_type;
    b2->type() = bone_traits::ball_type;
    b3->type() = bone_traits::slider_type;

    b0->u() = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() );
    b1->u() = vector3_type( value_traits::zero(), value_traits::one(), value_traits::zero() );
    b2->u() = vector3_type( value_traits::zero(), value_traits::zero(), value_traits::one() );
    b3->u() = vector3_type( value_traits::two(), value_traits::zero(), value_traits::zero() );

    b0->box_limits().min_limit( 0 ) = value_traits::one();
    b0->box_limits().max_limit( 0 ) = value_traits::two();
    b1->box_limits().min_limit( 0 ) = value_traits::one();
    b1->box_limits().max_limit( 0 ) = value_traits::two();
    b2->box_limits().min_limit( 0 ) = -value_traits::pi_quarter();
    b2->box_limits().min_limit( 1 ) = -value_traits::pi_half();
    b2->box_limits().min_limit( 2 ) = -value_traits::pi();
    b2->box_limits().max_limit( 0 ) =  value_traits::pi_quarter();
    b2->box_limits().max_limit( 1 ) =  value_traits::pi_half();
    b2->box_limits().max_limit( 2 ) =  value_traits::pi();
    b3->box_limits().min_limit( 0 ) = value_traits::four();
    b3->box_limits().max_limit( 0 ) = value_traits::four();

    // Now we create a solver based on that skeleton.
    solver_type solver1 = OpenTissue::kinematics::inverse::make_solver( skeleton1 );

    // See if we can write the solver and skeleton data to an xml file
    BOOST_CHECK_NO_THROW( OpenTissue::kinematics::inverse::xml_write( "test.xml", solver1, true ) );

    // Now we will try to create a copy of the first solver by reading from a test file
    skeleton_type skeleton2;
    solver_type solver2;
    solver2.init( skeleton2 );
    BOOST_CHECK_NO_THROW( OpenTissue::kinematics::inverse::xml_read( "test.xml", solver2, true ) );

    // Next we compare the original solver with the one that we read back from the xml file.
    test_solvers( solver1, solver2);

    // Next we will re-create solver 1, ( the idea is to make sure no cached data survives a read operation)
    BOOST_CHECK_NO_THROW( OpenTissue::kinematics::inverse::xml_read( "test.xml", solver1, true ) );

    // now solver 1 should stil match solver 2
    test_solvers( solver2, solver1);

    // Next we will re-create solver 2, but without reading skeleton data (the idea is to make sure that existing skeleton data should be un-affected)
    BOOST_CHECK_NO_THROW( OpenTissue::kinematics::inverse::xml_read( "test.xml", solver2, false ) );

    // The two solvers should stil match
    test_solvers( solver1, solver2);

  }

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef GIFS_BOMD_ELECTRONIC_HPP
#define GIFS_BOMD_ELECTRONIC_HPP

#include <armadillo>
#include "bomd.hpp"
#include "electronic.hpp"
#include "configreader.hpp"

class ElectronicBomd: public BOMD {
public:
  explicit ElectronicBomd(arma::mat& qm_grd, arma::mat& mm_grd): BOMD{qm_grd, mm_grd} {}
  ~ElectronicBomd() { }
  double update_gradient(void) override;

protected:
  // Called within BOMD initialization
  void get_reader_data(ConfigBlockReader& reader) override;
  ConfigBlockReader setup_reader() override;

  arma::mat U, T, V;

  double dtc;
  double dtq;

  size_t min_state;
  int nstates;

  std::string amplitude_file;

  // size_t active_state; inherited from BOMD
  Electronic c {};
};

#endif


//-----------------------------------------------------------------------------
// Created on: 16 February 2019
//-----------------------------------------------------------------------------
// Copyright (c) 2019-present, Sergey Slyadnev
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder(s) nor the
//      names of all contributors may be used to endorse or promote products
//      derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//-----------------------------------------------------------------------------

// Own include
#include <asiAlgo_MeshOBB.h>

// asiAlgo includes
#include <asiAlgo_Utils.h>

// OCCT includes
#include <BRepBuilderAPI_Transform.hxx>
#include <BRepPrimAPI_MakeBox.hxx>

// Eigen includes
#pragma warning(push, 0)
#include <Eigen/Dense>
#pragma warning(pop)

// STL includes
#include <vector>

#undef COUT_DEBUG
#if defined COUT_DEBUG
  #pragma message("===== warning: COUT_DEBUG is enabled")
#endif

#undef DRAW_DEBUG
#if defined DRAW_DEBUG
  #pragma message("===== warning: DRAW_DEBUG is enabled")
#endif

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

static bool compare(const std::pair<double, int>& p1,
                    const std::pair<double, int>& p2)
{
  return p1.first > p2.first;
}

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

asiAlgo_MeshOBB::asiAlgo_MeshOBB(const Handle(Poly_Triangulation)& mesh,
                                 ActAPI_ProgressEntry              progress,
                                 ActAPI_PlotterEntry               plotter)
//
: ActAPI_IAlgorithm (progress, plotter),
  m_input           (mesh)
{}

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

bool asiAlgo_MeshOBB::Perform()
{
  // Check if triangulation exists.
  if ( m_input.IsNull() || !m_input->NbNodes() )
  {
    this->GetProgress().SendLogMessage( LogErr(Normal) << "Cannot build OBB for empty triangulation." );
    return false;
  }

  // Calculate principal axes.
  gp_Ax1 ax_X, ax_Y, ax_Z;
  gp_XYZ mu;
  //
  this->calculateByCovariance(ax_X, ax_Y, ax_Z, mu);

  { // Imperative drawing
    // NOTE: that looks a bit funny, but in order to visualize the just calculated
    //       principal axes of OBB we use AABB as it gives us hint for sizing.

    double xMin, yMin, zMin, xMax, yMax, zMax;
    asiAlgo_Utils::Bounds(m_input, xMin, yMin, zMin, xMax, yMax, zMax);
    //
    gp_Pnt bndMin(xMin, yMin, zMin), bndMax(xMax, yMax, zMax);
    const double magnitude = bndMax.Distance(bndMin)*0.1;

    this->GetPlotter().DRAW_VECTOR_AT(mu, ax_X.Direction().XYZ()*magnitude, Color_Red);
    this->GetPlotter().DRAW_VECTOR_AT(mu, ax_Y.Direction().XYZ()*magnitude, Color_Green);
    this->GetPlotter().DRAW_VECTOR_AT(mu, ax_Z.Direction().XYZ()*magnitude, Color_Blue);
  }

  /* =============================================
   *  Calculate extremities on the principal axes
   * ============================================= */

  double ax_X_param_max = -RealLast(),
         ax_Y_param_max = -RealLast(),
         ax_Z_param_max = -RealLast();
  double ax_X_param_min =  RealLast(),
         ax_Y_param_min =  RealLast(),
         ax_Z_param_min =  RealLast();
  //
  const TColgp_Array1OfPnt& nodes = m_input->Nodes();
  //
  for ( int i = nodes.Lower(); i <= nodes.Upper(); ++i )
  {
    const gp_Pnt& P = nodes(i);
    //
    const double node_X = P.X();
    const double node_Y = P.Y();
    const double node_Z = P.Z();
    //
    gp_XYZ p(node_X, node_Y, node_Z);
    gp_Vec p_local = p - mu;

    const double ax_X_param = p_local.Dot( ax_X.Direction() );
    const double ax_Y_param = p_local.Dot( ax_Y.Direction() );
    const double ax_Z_param = p_local.Dot( ax_Z.Direction() );

    if ( ax_X_param > ax_X_param_max )
      ax_X_param_max = ax_X_param;
    //
    if ( ax_Y_param > ax_Y_param_max )
      ax_Y_param_max = ax_Y_param;
    //
    if ( ax_Z_param > ax_Z_param_max )
      ax_Z_param_max = ax_Z_param;
    //
    if ( ax_X_param < ax_X_param_min )
      ax_X_param_min = ax_X_param;
    //
    if ( ax_Y_param < ax_Y_param_min )
      ax_Y_param_min = ax_Y_param;
    //
    if ( ax_Z_param < ax_Z_param_min )
      ax_Z_param_min = ax_Z_param;
  }

#if defined DRAW_DEBUG
  { // Imperative drawing.
    const gp_XYZ ax_X_max = mu + ax_X.Direction().XYZ()*ax_X_param_max;
    const gp_XYZ ax_Y_max = mu + ax_Y.Direction().XYZ()*ax_Y_param_max;
    const gp_XYZ ax_Z_max = mu + ax_Z.Direction().XYZ()*ax_Z_param_max;
    //
    const gp_XYZ ax_X_min = mu + ax_X.Direction().XYZ()*ax_X_param_min;
    const gp_XYZ ax_Y_min = mu + ax_Y.Direction().XYZ()*ax_Y_param_min;
    const gp_XYZ ax_Z_min = mu + ax_Z.Direction().XYZ()*ax_Z_param_min;

    this->Plotter().DRAW_POINT(ax_X_max, Color_Red);
    this->Plotter().DRAW_POINT(ax_Y_max, Color_Green);
    this->Plotter().DRAW_POINT(ax_Z_max, Color_Blue);
    //
    this->Plotter().DRAW_POINT(ax_X_min, Color_Magenta);
    this->Plotter().DRAW_POINT(ax_Y_min, Color_Yellow);
    this->Plotter().DRAW_POINT(ax_Z_min, Color_Violet);
  }
#endif

  /* =====================
   *  STAGE 4: Set result
   * ===================== */

  // Set placement and corner positions to the result.
  gp_Ax3 ax3_placement( mu, ax_Z.Direction(), ax_X.Direction() );
  gp_Pnt corner_min(ax_X_param_min, ax_Y_param_min, ax_Z_param_min);
  gp_Pnt corner_max(ax_X_param_max, ax_Y_param_max, ax_Z_param_max);
  //
  m_obb.Placement      = ax3_placement;
  m_obb.LocalCornerMin = corner_min;
  m_obb.LocalCornerMax = corner_max;

  return true;
}

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

const asiAlgo_OBB& asiAlgo_MeshOBB::GetResult() const
{
  return m_obb;
}

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

gp_Trsf asiAlgo_MeshOBB::GetResultTrsf() const
{
  gp_Trsf T;
  T.SetTransformation(m_obb.Placement);
  T.Invert();
  return T;
}

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

TopoDS_Solid asiAlgo_MeshOBB::GetResultBox() const
{
  gp_Trsf T = this->GetResultTrsf();

  // Build a properly located box solid representing OBB
  TopoDS_Shape solid;
  try
  {
    BRepPrimAPI_MakeBox mkOBB(m_obb.LocalCornerMin, m_obb.LocalCornerMax);
    solid = BRepBuilderAPI_Transform(mkOBB.Solid(), T, true);
  }
  catch ( ... ) {}

  return TopoDS::Solid(solid);
}

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

void asiAlgo_MeshOBB::calculateByCovariance(gp_Ax1& xAxis,
                                            gp_Ax1& yAxis,
                                            gp_Ax1& zAxis,
                                            gp_XYZ& meanVertex) const
{
  // Get number of nodes.
  const int nNodes = m_input->NbNodes();

  /* =======================
   *  Calculate mean vertex
   * ======================= */

  gp_XYZ mu;
  const TColgp_Array1OfPnt& nodes = m_input->Nodes();
  //
  for ( int i = nodes.Lower(); i <= nodes.Upper(); ++i )
  {
    const gp_Pnt& P = nodes(i);

    const double node_X = P.X();
    const double node_Y = P.Y();
    const double node_Z = P.Z();

    mu += gp_XYZ(node_X, node_Y, node_Z);
  }
  mu /= nNodes;

  this->GetPlotter().DRAW_TRIANGULATION(m_input, Color_Magenta, 0.5);
  this->GetPlotter().DRAW_POINT(mu, Color_Red);

  /* =====================
   *  Calculate main axes
   * ===================== */

  Eigen::Matrix3d C;
  for ( int j = 1; j <= 3; ++j )
  {
    for ( int k = 1; k <= 3; ++k )
    {
      C(j-1, k-1) = 0.0; // TODO: is that necessary?
    }
  }

  for ( int i = nodes.Lower(); i <= nodes.Upper(); ++i )
  {
    const gp_Pnt& P = nodes(i);
    //
    const double node_X = P.X();
    const double node_Y = P.Y();
    const double node_Z = P.Z();
    //
    gp_XYZ p(node_X, node_Y, node_Z);
    gp_XYZ p_dash = p - mu;

    for ( int j = 1; j <= 3; ++j )
    {
      for ( int k = 1; k <= 3; ++k )
      {
        C(j-1, k-1) += ( p_dash.Coord(j)*p_dash.Coord(k) );
      }
    }
  }

  for ( int j = 1; j <= 3; ++j )
  {
    for ( int k = 1; k <= 3; ++k )
    {
      C(j-1, k-1) /= (nNodes);
    }
  }

  Eigen::EigenSolver<Eigen::Matrix3d> EigenSolver(C);

#if defined COUT_DEBUG
  std::cout << "\tThe eigen values of A are:" << std::endl << EigenSolver.eigenvalues() << std::endl;
  std::cout << "\tThe matrix of eigenvectors, V, is:" << std::endl << EigenSolver.eigenvectors() << std::endl << std::endl;
#endif

  Eigen::Vector3cd v1 = EigenSolver.eigenvectors().col(0);
  Eigen::Vector3cd v2 = EigenSolver.eigenvectors().col(1);
  Eigen::Vector3cd v3 = EigenSolver.eigenvectors().col(2);

  gp_Vec V[3] = { gp_Vec( v1.x().real(), v1.y().real(), v1.z().real() ),
                  gp_Vec( v2.x().real(), v2.y().real(), v2.z().real() ),
                  gp_Vec( v3.x().real(), v3.y().real(), v3.z().real() ) };
  //
  std::vector< std::pair<double, int> >
    lambda { std::pair<double, int>( EigenSolver.eigenvalues()(0).real(), 0 ),
             std::pair<double, int>( EigenSolver.eigenvalues()(1).real(), 1 ),
             std::pair<double, int>( EigenSolver.eigenvalues()(2).real(), 2 ) };
  //
  std::sort(lambda.begin(), lambda.end(), compare);
  //
  gp_Ax1 ax_X(mu, V[lambda[0].second]);
  gp_Ax1 ax_Y(mu, V[lambda[1].second]);
  gp_Ax1 ax_Z(mu, V[lambda[2].second]);

  // Check if the system is right-handed.
  const double ang = ax_X.Direction().AngleWithRef( ax_Y.Direction(), ax_Z.Direction() );
  if ( ang < 0 )
    ax_X.Reverse();

  // Store results.
  meanVertex = mu;
  xAxis      = ax_X;
  yAxis      = ax_Y;
  zAxis      = ax_Z;
}


//  boost quaternion.hpp header file

//  (C) Copyright Hubert Holin 2001.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_QUATERNION_HPP
#define BOOST_QUATERNION_HPP

#include <boost/config.hpp> // for BOOST_NO_STD_LOCALE
#include <boost/math_fwd.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/utility/enable_if.hpp>
#ifndef    BOOST_NO_STD_LOCALE
#  include <locale>                                    // for the "<<" operator
#endif /* BOOST_NO_STD_LOCALE */

#include <complex>
#include <iosfwd>                                    // for the "<<" and ">>" operators
#include <sstream>                                    // for the "<<" operator

#include <boost/math/special_functions/sinc.hpp>    // for the Sinus cardinal
#include <boost/math/special_functions/sinhc.hpp>    // for the Hyperbolic Sinus cardinal
#include <boost/math/tools/cxx03_warn.hpp>

#if defined(BOOST_NO_CXX11_NOEXCEPT) || defined(BOOST_NO_CXX11_RVALUE_REFERENCES) || defined(BOOST_NO_SFINAE_EXPR)
#include <boost/type_traits/is_pod.hpp>
#endif

namespace boost
{
   namespace math
   {

      namespace detail {

#if !defined(BOOST_NO_CXX11_NOEXCEPT) && !defined(BOOST_NO_CXX11_RVALUE_REFERENCES) && !defined(BOOST_NO_SFINAE_EXPR)

         template <class T>
         struct is_trivial_arithmetic_type_imp
         {
            typedef boost::integral_constant<bool,
               noexcept(std::declval<T&>() += std::declval<T>())
               && noexcept(std::declval<T&>() -= std::declval<T>())
               && noexcept(std::declval<T&>() *= std::declval<T>())
               && noexcept(std::declval<T&>() /= std::declval<T>())
            > type;
         };

         template <class T>
         struct is_trivial_arithmetic_type : public is_trivial_arithmetic_type_imp<T>::type {};
#else

         template <class T>
         struct is_trivial_arithmetic_type : public boost::is_pod<T> {};

#endif

      }

#ifndef BOOST_NO_CXX14_CONSTEXPR
      namespace constexpr_detail
      {
         template <class T>
         constexpr void swap(T& a, T& b)
         {
            T t(a);
            a = b;
            b = t;
         }
       }
#endif

       template<typename T>
        class quaternion
        {
        public:

            typedef T value_type;

            // constructor for H seen as R^4
            // (also default constructor)

            BOOST_CONSTEXPR explicit            quaternion( T const & requested_a = T(),
                                            T const & requested_b = T(),
                                            T const & requested_c = T(),
                                            T const & requested_d = T())
            :   a(requested_a),
                b(requested_b),
                c(requested_c),
                d(requested_d)
            {
                // nothing to do!
            }

            // constructor for H seen as C^2

            BOOST_CONSTEXPR explicit            quaternion( ::std::complex<T> const & z0,
                                            ::std::complex<T> const & z1 = ::std::complex<T>())
            :   a(z0.real()),
                b(z0.imag()),
                c(z1.real()),
                d(z1.imag())
            {
                // nothing to do!
            }

            // UNtemplated copy constructor
            BOOST_CONSTEXPR quaternion(quaternion const & a_recopier)
               : a(a_recopier.R_component_1()),
               b(a_recopier.R_component_2()),
               c(a_recopier.R_component_3()),
               d(a_recopier.R_component_4()) {}
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
            BOOST_CONSTEXPR quaternion(quaternion && a_recopier)
               : a(std::move(a_recopier.R_component_1())),
               b(std::move(a_recopier.R_component_2())),
               c(std::move(a_recopier.R_component_3())),
               d(std::move(a_recopier.R_component_4())) {}
#endif

            // templated copy constructor

            template<typename X>
            BOOST_CONSTEXPR explicit            quaternion(quaternion<X> const & a_recopier)
            :   a(static_cast<T>(a_recopier.R_component_1())),
                b(static_cast<T>(a_recopier.R_component_2())),
                c(static_cast<T>(a_recopier.R_component_3())),
                d(static_cast<T>(a_recopier.R_component_4()))
            {
                // nothing to do!
            }

            // destructor
            // (this is taken care of by the compiler itself)

            // accessors
            //
            // Note:    Like complex number, quaternions do have a meaningful notion of "real part",
            //            but unlike them there is no meaningful notion of "imaginary part".
            //            Instead there is an "unreal part" which itself is a quaternion, and usually
            //            nothing simpler (as opposed to the complex number case).
            //            However, for practicality, there are accessors for the other components
            //            (these are necessary for the templated copy constructor, for instance).

            BOOST_CONSTEXPR T real() const
            {
               return(a);
            }

            BOOST_CONSTEXPR quaternion<T> unreal() const
            {
               return(quaternion<T>(static_cast<T>(0), b, c, d));
            }

            BOOST_CONSTEXPR T R_component_1() const
            {
               return(a);
            }

            BOOST_CONSTEXPR T R_component_2() const
            {
               return(b);
            }

            BOOST_CONSTEXPR T R_component_3() const
            {
               return(c);
            }

            BOOST_CONSTEXPR T R_component_4() const
            {
               return(d);
            }

            BOOST_CONSTEXPR ::std::complex<T> C_component_1() const
            {
               return(::std::complex<T>(a, b));
            }

            BOOST_CONSTEXPR ::std::complex<T> C_component_2() const
            {
               return(::std::complex<T>(c, d));
            }

            BOOST_CXX14_CONSTEXPR void swap(quaternion& o)
            {
#ifndef BOOST_NO_CXX14_CONSTEXPR
               using constexpr_detail::swap;
#else
               using std::swap;
#endif
               swap(a, o.a);
               swap(b, o.b);
               swap(c, o.c);
               swap(d, o.d);
            }

            // assignment operators

            template<typename X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (quaternion<X> const  & a_affecter)
            {
               a = static_cast<T>(a_affecter.R_component_1());
               b = static_cast<T>(a_affecter.R_component_2());
               c = static_cast<T>(a_affecter.R_component_3());
               d = static_cast<T>(a_affecter.R_component_4());

               return(*this);
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (quaternion<T> const & a_affecter)
            {
               a = a_affecter.a;
               b = a_affecter.b;
               c = a_affecter.c;
               d = a_affecter.d;

               return(*this);
            }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (quaternion<T> && a_affecter)
            {
               a = std::move(a_affecter.a);
               b = std::move(a_affecter.b);
               c = std::move(a_affecter.c);
               d = std::move(a_affecter.d);

               return(*this);
            }
#endif
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (T const & a_affecter)
            {
               a = a_affecter;

               b = c = d = static_cast<T>(0);

               return(*this);
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (::std::complex<T> const & a_affecter)
            {
               a = a_affecter.real();
               b = a_affecter.imag();

               c = d = static_cast<T>(0);

               return(*this);
            }

            // other assignment-related operators
            //
            // NOTE:    Quaternion multiplication is *NOT* commutative;
            //            symbolically, "q *= rhs;" means "q = q * rhs;"
            //            and "q /= rhs;" means "q = q * inverse_of(rhs);"
            //
            // Note2:   Each operator comes in 2 forms - one for the simple case where
            //          type T throws no exceptions, and one exception-safe version
            //          for the case where it might.
         private:
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(T const & rhs, const boost::true_type&)
            {
               a += rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a + rhs, b, c, d); // exception guard
               swap(result);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(std::complex<T> const & rhs, const boost::true_type&)
            {
               a += std::real(rhs);
               b += std::imag(rhs);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(std::complex<T> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a + std::real(rhs), b + std::imag(rhs), c, d); // exception guard
               swap(result);
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(quaternion<X> const & rhs, const boost::true_type&)
            {
               a += rhs.R_component_1();
               b += rhs.R_component_2();
               c += rhs.R_component_3();
               d += rhs.R_component_4();
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(quaternion<X> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a + rhs.R_component_1(), b + rhs.R_component_2(), c + rhs.R_component_3(), d + rhs.R_component_4()); // exception guard
               swap(result);
               return *this;
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(T const & rhs, const boost::true_type&)
            {
               a -= rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a - rhs, b, c, d); // exception guard
               swap(result);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(std::complex<T> const & rhs, const boost::true_type&)
            {
               a -= std::real(rhs);
               b -= std::imag(rhs);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(std::complex<T> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a - std::real(rhs), b - std::imag(rhs), c, d); // exception guard
               swap(result);
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(quaternion<X> const & rhs, const boost::true_type&)
            {
               a -= rhs.R_component_1();
               b -= rhs.R_component_2();
               c -= rhs.R_component_3();
               d -= rhs.R_component_4();
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(quaternion<X> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a - rhs.R_component_1(), b - rhs.R_component_2(), c - rhs.R_component_3(), d - rhs.R_component_4()); // exception guard
               swap(result);
               return *this;
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_multiply(T const & rhs, const boost::true_type&)
            {
               a *= rhs;
               b *= rhs;
               c *= rhs;
               d *= rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_multiply(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a * rhs, b * rhs, c * rhs, d * rhs); // exception guard
               swap(result);
               return *this;
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_divide(T const & rhs, const boost::true_type&)
            {
               a /= rhs;
               b /= rhs;
               c /= rhs;
               d /= rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_divide(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a / rhs, b / rhs, c / rhs, d / rhs); // exception guard
               swap(result);
               return *this;
            }
         public:

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator += (T const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator += (::std::complex<T> const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
            template<typename X> BOOST_CXX14_CONSTEXPR quaternion<T> & operator += (quaternion<X> const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator -= (T const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator -= (::std::complex<T> const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
            template<typename X> BOOST_CXX14_CONSTEXPR quaternion<T> & operator -= (quaternion<X> const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator *= (T const & rhs) { return do_multiply(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator *= (::std::complex<T> const & rhs)
            {
                T    ar = rhs.real();
                T    br = rhs.imag();
                quaternion<T> result(a*ar - b*br, a*br + b*ar, c*ar + d*br, -c*br+d*ar);
                swap(result);
                return(*this);
            }

            template<typename X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator *= (quaternion<X> const & rhs)
            {
                T    ar = static_cast<T>(rhs.R_component_1());
                T    br = static_cast<T>(rhs.R_component_2());
                T    cr = static_cast<T>(rhs.R_component_3());
                T    dr = static_cast<T>(rhs.R_component_4());

                quaternion<T> result(a*ar - b*br - c*cr - d*dr, a*br + b*ar + c*dr - d*cr, a*cr - b*dr + c*ar + d*br, a*dr + b*cr - c*br + d*ar);
                swap(result);
                return(*this);
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator /= (T const & rhs) { return do_divide(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator /= (::std::complex<T> const & rhs)
            {
                T    ar = rhs.real();
                T    br = rhs.imag();
                T    denominator = ar*ar+br*br;
                quaternion<T> result((+a*ar + b*br) / denominator, (-a*br + b*ar) / denominator, (+c*ar - d*br) / denominator, (+c*br + d*ar) / denominator);
                swap(result);
                return(*this);
            }

            template<typename X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator /= (quaternion<X> const & rhs)
            {
                T    ar = static_cast<T>(rhs.R_component_1());
                T    br = static_cast<T>(rhs.R_component_2());
                T    cr = static_cast<T>(rhs.R_component_3());
                T    dr = static_cast<T>(rhs.R_component_4());

                T    denominator = ar*ar+br*br+cr*cr+dr*dr;
                quaternion<T> result((+a*ar+b*br+c*cr+d*dr)/denominator, (-a*br+b*ar-c*dr+d*cr)/denominator, (-a*cr+b*dr+c*ar-d*br)/denominator, (-a*dr-b*cr+c*br+d*ar)/denominator);
                swap(result);
                return(*this);
            }
        private:
           T a, b, c, d;

        };

// swap:
template <class T>
BOOST_CXX14_CONSTEXPR void swap(quaternion<T>& a, quaternion<T>& b) { a.swap(b); }

// operator+
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator + (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(static_cast<T1>(a.R_component_1() + b), a.R_component_2(), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator + (const T1& a, const quaternion<T2>& b)
{
   return quaternion<T2>(static_cast<T2>(b.R_component_1() + a), b.R_component_2(), b.R_component_3(), b.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator + (const quaternion<T1>& a, const std::complex<T2>& b)
{
   return quaternion<T1>(a.R_component_1() + std::real(b), a.R_component_2() + std::imag(b), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator + (const std::complex<T1>& a, const quaternion<T2>& b)
{
   return quaternion<T1>(b.R_component_1() + real(a), b.R_component_2() + imag(a), b.R_component_3(), b.R_component_4());
}
template <class T>
inline BOOST_CONSTEXPR quaternion<T> operator + (const quaternion<T>& a, const quaternion<T>& b)
{
   return quaternion<T>(a.R_component_1() + b.R_component_1(), a.R_component_2() + b.R_component_2(), a.R_component_3() + b.R_component_3(), a.R_component_4() + b.R_component_4());
}
// operator-
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator - (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(static_cast<T1>(a.R_component_1() - b), a.R_component_2(), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator - (const T1& a, const quaternion<T2>& b)
{
   return quaternion<T2>(static_cast<T2>(a - b.R_component_1()), -b.R_component_2(), -b.R_component_3(), -b.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator - (const quaternion<T1>& a, const std::complex<T2>& b)
{
   return quaternion<T1>(a.R_component_1() - std::real(b), a.R_component_2() - std::imag(b), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator - (const std::complex<T1>& a, const quaternion<T2>& b)
{
   return quaternion<T1>(real(a) - b.R_component_1(), imag(a) - b.R_component_2(), -b.R_component_3(), -b.R_component_4());
}
template <class T>
inline BOOST_CONSTEXPR quaternion<T> operator - (const quaternion<T>& a, const quaternion<T>& b)
{
   return quaternion<T>(a.R_component_1() - b.R_component_1(), a.R_component_2() - b.R_component_2(), a.R_component_3() - b.R_component_3(), a.R_component_4() - b.R_component_4());
}

// operator*
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator * (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(static_cast<T1>(a.R_component_1() * b), a.R_component_2() * b, a.R_component_3() * b, a.R_component_4() * b);
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator * (const T1& a, const quaternion<T2>& b)
{
   return quaternion<T2>(static_cast<T2>(a * b.R_component_1()), a * b.R_component_2(), a * b.R_component_3(), a * b.R_component_4());
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator * (const quaternion<T1>& a, const std::complex<T2>& b)
{
   quaternion<T1> result(a);
   result *= b;
   return result;
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator * (const std::complex<T1>& a, const quaternion<T2>& b)
{
   quaternion<T1> result(a);
   result *= b;
   return result;
}
template <class T>
inline BOOST_CXX14_CONSTEXPR quaternion<T> operator * (const quaternion<T>& a, const quaternion<T>& b)
{
   quaternion<T> result(a);
   result *= b;
   return result;
}

// operator/
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator / (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(a.R_component_1() / b, a.R_component_2() / b, a.R_component_3() / b, a.R_component_4() / b);
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator / (const T1& a, const quaternion<T2>& b)
{
   quaternion<T2> result(a);
   result /= b;
   return result;
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator / (const quaternion<T1>& a, const std::complex<T2>& b)
{
   quaternion<T1> result(a);
   result /= b;
   return result;
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator / (const std::complex<T1>& a, const quaternion<T2>& b)
{
   quaternion<T2> result(a);
   result /= b;
   return result;
}
template <class T>
inline BOOST_CXX14_CONSTEXPR quaternion<T> operator / (const quaternion<T>& a, const quaternion<T>& b)
{
   quaternion<T> result(a);
   result /= b;
   return result;
}

        template<typename T>
        inline BOOST_CONSTEXPR const quaternion<T>&             operator + (quaternion<T> const & q)
        {
            return q;
        }

        template<typename T>
        inline BOOST_CONSTEXPR quaternion<T>                    operator - (quaternion<T> const & q)
        {
            return(quaternion<T>(-q.R_component_1(),-q.R_component_2(),-q.R_component_3(),-q.R_component_4()));
        }

        template<typename R, typename T>
        inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<R, T>::value, bool>::type operator == (R const & lhs, quaternion<T> const & rhs)
        {
            return    (
                        (rhs.R_component_1() == lhs)&&
                        (rhs.R_component_2() == static_cast<T>(0))&&
                        (rhs.R_component_3() == static_cast<T>(0))&&
                        (rhs.R_component_4() == static_cast<T>(0))
                    );
        }

        template<typename T, typename R>
        inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<R, T>::value, bool>::type operator == (quaternion<T> const & lhs, R const & rhs)
        {
           return rhs == lhs;
        }

        template<typename T>
        inline BOOST_CONSTEXPR bool                                operator == (::std::complex<T> const & lhs, quaternion<T> const & rhs)
        {
            return    (
                        (rhs.R_component_1() == lhs.real())&&
                        (rhs.R_component_2() == lhs.imag())&&
                        (rhs.R_component_3() == static_cast<T>(0))&&
                        (rhs.R_component_4() == static_cast<T>(0))
                    );
        }

        template<typename T>
        inline BOOST_CONSTEXPR bool                                operator == (quaternion<T> const & lhs, ::std::complex<T> const & rhs)
        {
           return rhs == lhs;
        }

        template<typename T>
        inline BOOST_CONSTEXPR bool                                operator == (quaternion<T> const & lhs, quaternion<T> const & rhs)
        {
            return    (
                        (rhs.R_component_1() == lhs.R_component_1())&&
                        (rhs.R_component_2() == lhs.R_component_2())&&
                        (rhs.R_component_3() == lhs.R_component_3())&&
                        (rhs.R_component_4() == lhs.R_component_4())
                    );
        }

        template<typename R, typename T> inline BOOST_CONSTEXPR bool operator != (R const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
        template<typename T, typename R> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, R const & rhs) { return !(lhs == rhs); }
        template<typename T> inline BOOST_CONSTEXPR bool operator != (::std::complex<T> const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
        template<typename T> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, ::std::complex<T> const & rhs) { return !(lhs == rhs); }
        template<typename T> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }

        // Note:    we allow the following formats, with a, b, c, and d reals
        //            a
        //            (a), (a,b), (a,b,c), (a,b,c,d)
        //            (a,(c)), (a,(c,d)), ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b),(c,d))
        template<typename T, typename charT, class traits>
        ::std::basic_istream<charT,traits> &    operator >> (    ::std::basic_istream<charT,traits> & is,
                                                                quaternion<T> & q)
        {

#ifdef    BOOST_NO_STD_LOCALE
#else
            const ::std::ctype<charT> & ct = ::std::use_facet< ::std::ctype<charT> >(is.getloc());
#endif /* BOOST_NO_STD_LOCALE */

            T    a = T();
            T    b = T();
            T    c = T();
            T    d = T();

            ::std::complex<T>    u = ::std::complex<T>();
            ::std::complex<T>    v = ::std::complex<T>();

            charT    ch = charT();
            char    cc;

            is >> ch;                                        // get the first lexeme

            if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
            cc = ch;
#else
            cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

            if    (cc == '(')                            // read "(", possible: (a), (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d)), ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
            {
                is >> ch;                                    // get the second lexeme

                if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                cc = ch;
#else
                cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                if    (cc == '(')                        // read "((", possible: ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
                {
                    is.putback(ch);

                    is >> u;                                // we extract the first and second components
                    a = u.real();
                    b = u.imag();

                    if    (!is.good())    goto finish;

                    is >> ch;                                // get the next lexeme

                    if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                    cc = ch;
#else
                    cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                    if        (cc == ')')                    // format: ((a)) or ((a,b))
                    {
                        q = quaternion<T>(a,b);
                    }
                    else if    (cc == ',')                // read "((a)," or "((a,b),", possible: ((a),c), ((a),(c)), ((a),(c,d)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
                    {
                        is >> v;                            // we extract the third and fourth components
                        c = v.real();
                        d = v.imag();

                        if    (!is.good())    goto finish;

                        is >> ch;                                // get the last lexeme

                        if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                        cc = ch;
#else
                        cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                        if    (cc == ')')                    // format: ((a),c), ((a),(c)), ((a),(c,d)), ((a,b),c), ((a,b),(c)) or ((a,b,),(c,d,))
                        {
                            q = quaternion<T>(a,b,c,d);
                        }
                        else                            // error
                        {
                            is.setstate(::std::ios_base::failbit);
                        }
                    }
                    else                                // error
                    {
                        is.setstate(::std::ios_base::failbit);
                    }
                }
                else                                // read "(a", possible: (a), (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d))
                {
                    is.putback(ch);

                    is >> a;                                // we extract the first component

                    if    (!is.good())    goto finish;

                    is >> ch;                                // get the third lexeme

                    if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                    cc = ch;
#else
                    cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                    if        (cc == ')')                    // format: (a)
                    {
                        q = quaternion<T>(a);
                    }
                    else if    (cc == ',')                // read "(a,", possible: (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d))
                    {
                        is >> ch;                            // get the fourth lexeme

                        if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                        cc = ch;
#else
                        cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                        if    (cc == '(')                // read "(a,(", possible: (a,(c)), (a,(c,d))
                        {
                            is.putback(ch);

                            is >> v;                        // we extract the third and fourth component

                            c = v.real();
                            d = v.imag();

                            if    (!is.good())    goto finish;

                            is >> ch;                        // get the ninth lexeme

                            if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                            cc = ch;
#else
                            cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                            if    (cc == ')')                // format: (a,(c)) or (a,(c,d))
                            {
                                q = quaternion<T>(a,b,c,d);
                            }
                            else                        // error
                            {
                                is.setstate(::std::ios_base::failbit);
                            }
                        }
                        else                        // read "(a,b", possible: (a,b), (a,b,c), (a,b,c,d)
                        {
                            is.putback(ch);

                            is >> b;                        // we extract the second component

                            if    (!is.good())    goto finish;

                            is >> ch;                        // get the fifth lexeme

                            if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                            cc = ch;
#else
                            cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                            if    (cc == ')')                // format: (a,b)
                            {
                                q = quaternion<T>(a,b);
                            }
                            else if    (cc == ',')        // read "(a,b,", possible: (a,b,c), (a,b,c,d)
                            {
                                is >> c;                    // we extract the third component

                                if    (!is.good())    goto finish;

                                is >> ch;                    // get the seventh lexeme

                                if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                                cc = ch;
#else
                                cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                                if        (cc == ')')        // format: (a,b,c)
                                {
                                    q = quaternion<T>(a,b,c);
                                }
                                else if    (cc == ',')    // read "(a,b,c,", possible: (a,b,c,d)
                                {
                                    is >> d;                // we extract the fourth component

                                    if    (!is.good())    goto finish;

                                    is >> ch;                // get the ninth lexeme

                                    if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                                    cc = ch;
#else
                                    cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                                    if    (cc == ')')        // format: (a,b,c,d)
                                    {
                                        q = quaternion<T>(a,b,c,d);
                                    }
                                    else                // error
                                    {
                                        is.setstate(::std::ios_base::failbit);
                                    }
                                }
                                else                    // error
                                {
                                    is.setstate(::std::ios_base::failbit);
                                }
                            }
                            else                        // error
                            {
                                is.setstate(::std::ios_base::failbit);
                            }
                        }
                    }
                    else                                // error
                    {
                        is.setstate(::std::ios_base::failbit);
                    }
                }
            }
            else                                        // format:    a
            {
                is.putback(ch);

                is >> a;                                    // we extract the first component

                if    (!is.good())    goto finish;

                q = quaternion<T>(a);
            }

            finish:
            return(is);
        }

        template<typename T, typename charT, class traits>
        ::std::basic_ostream<charT,traits> &    operator << (    ::std::basic_ostream<charT,traits> & os,
                                                                quaternion<T> const & q)
        {
            ::std::basic_ostringstream<charT,traits>    s;

            s.flags(os.flags());
#ifdef    BOOST_NO_STD_LOCALE
#else
            s.imbue(os.getloc());
#endif /* BOOST_NO_STD_LOCALE */
            s.precision(os.precision());

            s << '('    << q.R_component_1() << ','
                        << q.R_component_2() << ','
                        << q.R_component_3() << ','
                        << q.R_component_4() << ')';

            return os << s.str();
        }

        // values

        template<typename T>
        inline BOOST_CONSTEXPR T real(quaternion<T> const & q)
        {
            return(q.real());
        }

        template<typename T>
        inline BOOST_CONSTEXPR quaternion<T> unreal(quaternion<T> const & q)
        {
            return(q.unreal());
        }

        template<typename T>
        inline T sup(quaternion<T> const & q)
        {
            using    ::std::abs;
            return (std::max)((std::max)(abs(q.R_component_1()), abs(q.R_component_2())), (std::max)(abs(q.R_component_3()), abs(q.R_component_4())));
        }

        template<typename T>
        inline T l1(quaternion<T> const & q)
        {
           using    ::std::abs;
           return abs(q.R_component_1()) + abs(q.R_component_2()) + abs(q.R_component_3()) + abs(q.R_component_4());
        }

        template<typename T>
        inline T abs(quaternion<T> const & q)
        {
            using    ::std::abs;
            using    ::std::sqrt;

            T            maxim = sup(q);    // overflow protection

            if    (maxim == static_cast<T>(0))
            {
                return(maxim);
            }
            else
            {
                T    mixam = static_cast<T>(1)/maxim;    // prefer multiplications over divisions

                T a = q.R_component_1() * mixam;
                T b = q.R_component_2() * mixam;
                T c = q.R_component_3() * mixam;
                T d = q.R_component_4() * mixam;

                a *= a;
                b *= b;
                c *= c;
                d *= d;

                return(maxim * sqrt(a + b + c + d));
            }

            //return(sqrt(norm(q)));
        }

        // Note:    This is the Cayley norm, not the Euclidean norm...

        template<typename T>
        inline BOOST_CXX14_CONSTEXPR T norm(quaternion<T>const  & q)
        {
            return(real(q*conj(q)));
        }

        template<typename T>
        inline BOOST_CONSTEXPR quaternion<T> conj(quaternion<T> const & q)
        {
            return(quaternion<T>(   +q.R_component_1(),
                                    -q.R_component_2(),
                                    -q.R_component_3(),
                                    -q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    spherical(  T const & rho,
                                                            T const & theta,
                                                            T const & phi1,
                                                            T const & phi2)
        {
            using ::std::cos;
            using ::std::sin;

            //T    a = cos(theta)*cos(phi1)*cos(phi2);
            //T    b = sin(theta)*cos(phi1)*cos(phi2);
            //T    c = sin(phi1)*cos(phi2);
            //T    d = sin(phi2);

            T    courrant = static_cast<T>(1);

            T    d = sin(phi2);

            courrant *= cos(phi2);

            T    c = sin(phi1)*courrant;

            courrant *= cos(phi1);

            T    b = sin(theta)*courrant;
            T    a = cos(theta)*courrant;

            return(rho*quaternion<T>(a,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    semipolar(  T const & rho,
                                                            T const & alpha,
                                                            T const & theta1,
                                                            T const & theta2)
        {
            using ::std::cos;
            using ::std::sin;

            T    a = cos(alpha)*cos(theta1);
            T    b = cos(alpha)*sin(theta1);
            T    c = sin(alpha)*cos(theta2);
            T    d = sin(alpha)*sin(theta2);

            return(rho*quaternion<T>(a,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    multipolar( T const & rho1,
                                                            T const & theta1,
                                                            T const & rho2,
                                                            T const & theta2)
        {
            using ::std::cos;
            using ::std::sin;

            T    a = rho1*cos(theta1);
            T    b = rho1*sin(theta1);
            T    c = rho2*cos(theta2);
            T    d = rho2*sin(theta2);

            return(quaternion<T>(a,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    cylindrospherical(  T const & t,
                                                                    T const & radius,
                                                                    T const & longitude,
                                                                    T const & latitude)
        {
            using ::std::cos;
            using ::std::sin;

            T    b = radius*cos(longitude)*cos(latitude);
            T    c = radius*sin(longitude)*cos(latitude);
            T    d = radius*sin(latitude);

            return(quaternion<T>(t,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    cylindrical(T const & r,
                                                            T const & angle,
                                                            T const & h1,
                                                            T const & h2)
        {
            using ::std::cos;
            using ::std::sin;

            T    a = r*cos(angle);
            T    b = r*sin(angle);

            return(quaternion<T>(a,b,h1,h2));
        }

        // transcendentals
        // (please see the documentation)

        template<typename T>
        inline quaternion<T>                    exp(quaternion<T> const & q)
        {
            using    ::std::exp;
            using    ::std::cos;

            using    ::boost::math::sinc_pi;

            T    u = exp(real(q));

            T    z = abs(unreal(q));

            T    w = sinc_pi(z);

            return(u*quaternion<T>(cos(z),
                w*q.R_component_2(), w*q.R_component_3(),
                w*q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    cos(quaternion<T> const & q)
        {
            using    ::std::sin;
            using    ::std::cos;
            using    ::std::cosh;

            using    ::boost::math::sinhc_pi;

            T    z = abs(unreal(q));

            T    w = -sin(q.real())*sinhc_pi(z);

            return(quaternion<T>(cos(q.real())*cosh(z),
                w*q.R_component_2(), w*q.R_component_3(),
                w*q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    sin(quaternion<T> const & q)
        {
            using    ::std::sin;
            using    ::std::cos;
            using    ::std::cosh;

            using    ::boost::math::sinhc_pi;

            T    z = abs(unreal(q));

            T    w = +cos(q.real())*sinhc_pi(z);

            return(quaternion<T>(sin(q.real())*cosh(z),
                w*q.R_component_2(), w*q.R_component_3(),
                w*q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    tan(quaternion<T> const & q)
        {
            return(sin(q)/cos(q));
        }

        template<typename T>
        inline quaternion<T>                    cosh(quaternion<T> const & q)
        {
            return((exp(+q)+exp(-q))/static_cast<T>(2));
        }

        template<typename T>
        inline quaternion<T>                    sinh(quaternion<T> const & q)
        {
            return((exp(+q)-exp(-q))/static_cast<T>(2));
        }

        template<typename T>
        inline quaternion<T>                    tanh(quaternion<T> const & q)
        {
            return(sinh(q)/cosh(q));
        }

        template<typename T>
        quaternion<T>                            pow(quaternion<T> const & q,
                                                    int n)
        {
            if        (n > 1)
            {
                int    m = n>>1;

                quaternion<T>    result = pow(q, m);

                result *= result;

                if    (n != (m<<1))
                {
                    result *= q; // n odd
                }

                return(result);
            }
            else if    (n == 1)
            {
                return(q);
            }
            else if    (n == 0)
            {
                return(quaternion<T>(static_cast<T>(1)));
            }
            else    /* n < 0 */
            {
                return(pow(quaternion<T>(static_cast<T>(1))/q,-n));
            }
        }
    }
}

#endif /* BOOST_QUATERNION_HPP */


#define BOOST_TEST_MODULE "test_read_tabulated_lennard_jones_attractive_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/input/read_global_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_tabulated_lennard_jones_attractive_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_tabulated_lennard_jones_attractive.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using potential_type = mjolnir::LennardJonesAttractivePotential<real_type>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            interaction = "Pair"
            potential   = "LennardJonesAttractive"
            spatial_partition.type  = "Naive"
            ignore.molecule         = "Nothing"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            table.A.A = {sigma = 1.0, epsilon = 0.5}
            table.A.B = {sigma = 2.0, epsilon = 1.5}
            table.B.B = {sigma = 3.0, epsilon = 2.5}
            parameters = [
                {index =   0, name = "A"},
                {index =   1, name = "B"},
                {index =   2, name = "A"},
                {index =   3, name = "B"},
                {index =   5, name = "A"},
                {index =   7, name = "B"},
                {index = 100, name = "A"},
            ]
        )"_toml;

        const auto pot_para = mjolnir::read_lennard_jones_attractive_potential<traits_type>(v);
        const auto& para = dynamic_cast<
            mjolnir::CombinationTable<traits_type, potential_type> const&
            >(pot_para.second.cref());

        const auto ignore_within = para.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(!para.exclusion_list().is_ignored_molecule(0, 0));
        BOOST_TEST(!para.exclusion_list().is_ignored_molecule(0, 1));
        BOOST_TEST(!para.exclusion_list().is_ignored_molecule(1, 1));

        BOOST_TEST(para.participants().size() ==   7u);
        BOOST_TEST(para.participants().at(0)  ==   0u);
        BOOST_TEST(para.participants().at(1)  ==   1u);
        BOOST_TEST(para.participants().at(2)  ==   2u);
        BOOST_TEST(para.participants().at(3)  ==   3u);
        BOOST_TEST(para.participants().at(4)  ==   5u);
        BOOST_TEST(para.participants().at(5)  ==   7u);
        BOOST_TEST(para.participants().at(6)  == 100u);

        BOOST_TEST(para.parameters().at(  0)  == "A", tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  1)  == "B", tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  2)  == "A", tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  3)  == "B", tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  5)  == "A", tolerance<real_type>());
        BOOST_TEST(para.parameters().at(  7)  == "B", tolerance<real_type>());
        BOOST_TEST(para.parameters().at(100)  == "A", tolerance<real_type>());

        const auto para_AA = para.prepare_params(0, 2);
        const auto para_AB = para.prepare_params(0, 1);
        const auto para_BA = para.prepare_params(1, 2);
        const auto para_BB = para.prepare_params(1, 3);

        BOOST_TEST(para_AA.sigma   == 1.0, tolerance<real_type>());
        BOOST_TEST(para_AA.epsilon == 0.5, tolerance<real_type>());
        BOOST_TEST(para_AB.sigma   == 2.0, tolerance<real_type>());
        BOOST_TEST(para_AB.epsilon == 1.5, tolerance<real_type>());
        BOOST_TEST(para_BA.sigma   == 2.0, tolerance<real_type>());
        BOOST_TEST(para_BA.epsilon == 1.5, tolerance<real_type>());
        BOOST_TEST(para_BB.sigma   == 3.0, tolerance<real_type>());
        BOOST_TEST(para_BB.epsilon == 2.5, tolerance<real_type>());
    }
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 2, Eigen::Dynamic> test_double_2_D(const Eigen::Matrix<double, 2, Eigen::Dynamic> & M)
{
	return M;
}
void export_double_2_D()
{
	boost::python::def("test_double_2_D",test_double_2_D);
}