// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

#ifndef BOOST_UNITS_AMOUNT_BASE_DIMENSION_HPP
#define BOOST_UNITS_AMOUNT_BASE_DIMENSION_HPP

#include <boost/units/config.hpp>
#include <boost/units/base_dimension.hpp>

namespace boost {

namespace units {

/// base dimension of amount
struct amount_base_dimension :
    boost::units::base_dimension<amount_base_dimension,-4>
{ };

} // namespace units

} // namespace boost

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TYPE(boost::units::amount_base_dimension)

#endif

namespace boost {

namespace units {

/// dimension of amount of substance (N)
typedef amount_base_dimension::dimension_type     amount_dimension;

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_AMOUNT_BASE_DIMENSION_HPP


#ifndef QUBUS_TENSOR_VAR_HPP
#define QUBUS_TENSOR_VAR_HPP

#include <qubus/runtime.hpp>

#include <qubus/qtl/ast.hpp>

#include <qubus/util/handle.hpp>
#include <qubus/util/integers.hpp>

#include <qubus/IR/expression.hpp>

#include <qubus/get_view.hpp>
#include <qubus/array.hpp>

#include <qubus/associated_qubus_type.hpp>

#include <boost/hana/any.hpp>

#include <memory>
#include <type_traits>
#include <utility>
#include <vector>

namespace qubus
{
namespace qtl
{
namespace ast
{

template <typename T, long int Rank>
class tensor;

template <typename T, long int Rank>
class tensor_expr
{
public:
     template <typename... Indices>
    auto operator()(Indices... indices) const
    {
        using subscription_type = std::conditional_t<
                boost::hana::any(boost::hana::make_tuple(std::is_same<Indices, range>::value...)),
                sliced_tensor<tensor_expr<T, Rank>, Indices...>, subscripted_tensor<tensor_expr<T, Rank>, Indices...>>;

        return subscription_type(*this, indices...);
    }

    const expression& code() const
    {
        return *code_;
    }

private:
    std::shared_ptr<expression> code_;
};

template <typename T, long int Rank>
class tensor
{
public:
    using value_type = T;

    template <typename... SizeTypes>
    explicit tensor(SizeTypes... sizes_)
    : data_(get_runtime().get_object_factory().create_array(associated_qubus_type<T>::get(),
                                                            {util::to_uindex(sizes_)...}))
    {
    }

    ~tensor()
    {
        data_.finalize();
    }

    template <typename... Indices>
    auto operator()(Indices... indices) const
    {
        using subscription_type = std::conditional_t<
                boost::hana::any(boost::hana::make_tuple(std::is_same<Indices, range>::value...)),
                sliced_tensor<tensor<T, Rank>, Indices...>, subscripted_tensor<tensor<T, Rank>, Indices...>>;

        return subscription_type(*this, indices...);
    }

    object get_object() const
    {
        return data_;
    }

private:
    object data_;
};

template <typename T, long int Rank, typename AccessType, typename ArchTag>
auto get_view(const tensor<T, Rank>& value, AccessType access_type, ArchTag arch)
{
    return get_view<array<T, Rank>>(value.get_object(), access_type, arch);
}
}

template <typename T, long int Rank>
using tensor_expr = ast::tensor_expr<T, Rank>;

template <typename T, long int Rank>
using tensor = ast::tensor<T, Rank>;

using ast::get_view;
}

template <typename T, long int Rank>
struct associated_qubus_type<qtl::ast::tensor<T, Rank>>
{
    static type get()
    {
        return types::array(associated_qubus_type<T>::get(), Rank);
    }
};
}

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_MULTI_BUFFER_INSERTER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_MULTI_BUFFER_INSERTER_HPP

#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/geometry/multi/core/point_type.hpp>

#include <boost/geometry/extensions/algorithms/buffer/buffer_inserter.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

template <>
struct check_original<multi_polygon_tag>
{
    template <typename Point, typename Geometry>
    static inline int apply(Point const& point, Geometry const& geometry)
    {
        return geometry::covered_by(point, geometry) ? 1 : -1;
    }
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Multi,
    typename PolygonOutput
>
struct buffer_inserter<multi_polygon_tag, Multi, PolygonOutput>
{
    template
    <
        typename Collection, typename DistanceStrategy, typename JoinStrategy
    >
    static inline void apply(Multi const& multi,
            Collection& collection,
            DistanceStrategy const& distance,
            JoinStrategy const& join_strategy)
    {
        typedef typename ring_type<PolygonOutput>::type output_ring_type;
        typedef buffer_inserter
            <
                typename single_tag_of
                            <
                                typename tag<Multi>::type
                            >::type,
                typename boost::range_value<Multi const>::type,
                output_ring_type
            > policy;

        for (typename boost::range_iterator<Multi const>::type
                it = boost::begin(multi);
            it != boost::end(multi);
            ++it)
        {
            policy::apply(*it, collection, distance, join_strategy);
        }
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_MULTI_BUFFER_INSERTER_HPP


// Copyright (c) 2005 - 2014 Marc de Kamps
// 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 nor the names of its 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 COPYRIGHT OWNER 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.
//
//      If you use this software in work leading to a scientific publication, you should include a reference there to
//      the 'currently valid reference', which can be found at http://miind.sourceforge.net
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/test/execution_monitor.hpp>
#include <GeomLib.hpp>
#include <MPILib/include/NodeType.hpp>
#include <MPILib/include/DelayedConnection.hpp>
#include <MPILib/include/TypeDefinitions.hpp>

using namespace GeomLib;

struct ConvertorFixture {

	ConvertorFixture();
	~ConvertorFixture(){}

	std::vector<InputParameterSet>
		GenerateSet
		(
			const std::vector<MPILib::Rate>&,
			const std::vector<MPILib::DelayedConnection>&,
			const std::vector<MPILib::NodeType>&
		);

	NeuronParameter         		_par_neuron;
	DiffusionParameter				_par_diff;
	CurrentCompensationParameter	_par_curr;
	OdeParameter					_par_ode;
	LifNeuralDynamics				_dyn;

	std::unique_ptr<GeomInputConvertor> _conv;
};

std::vector<InputParameterSet> ConvertorFixture::GenerateSet
(
	const std::vector<MPILib::Rate>&                                  vec_rate,
	const std::vector<MPILib::DelayedConnection>& vec_con,
	const std::vector<MPILib::NodeType>&							  vec_type
)
{
	_conv = std::unique_ptr<GeomInputConvertor>(new GeomInputConvertor(_par_neuron,_par_diff,_par_curr,_dyn.InterpretationArray()));
	_conv->SortConnectionvector(vec_rate,vec_con,vec_type);
	return _conv->SolverParameter();
}

ConvertorFixture::ConvertorFixture():
_par_neuron(20e-3,0.0,0.0,0.,10e-3),
_par_diff(0.03,0.05),
_par_curr(0.0,0.),
_par_ode(5,0.0,_par_neuron,InitialDensityParameter(0.,0.)),
_dyn(_par_ode,0.01)
{
}

BOOST_AUTO_TEST_CASE(SingleInputTest) {

	vector<MPILib::NodeType> 	vec_type;
	vector<MPILib::Rate>     	vec_rate;
	vector<MPILib::DelayedConnection>	vec_con;

	NeuronParameter par_neuron(20e-3,0.0,0.0,0.,10e-3);
	DiffusionParameter par_diff(0.03,0.05);

	CurrentCompensationParameter par_curr(0.,0.);
	InitialDensityParameter par_dense(0.,0.);

	Number n_bins = 5;
	Potential V_min = 0.0;

	OdeParameter par_ode(n_bins,V_min,par_neuron,par_dense);
	LifNeuralDynamics dyn(par_ode,0.01);
	vector<Potential> vec_interp = dyn.InterpretationArray();

	GeomInputConvertor
		convertor(
			par_neuron,
			par_diff,
			par_curr,
			vec_interp
		);

	vec_type.push_back(MPILib::EXCITATORY_DIRECT);
	MPILib::Rate rate = 800.0;
	vec_rate.push_back(rate);

	MPILib::Efficacy eff = 0.03;
	MPILib::DelayedConnection con(1,eff,0.0);
	vec_con.push_back(con);

	convertor.SortConnectionvector(vec_rate, vec_con, vec_type);
	std::vector<InputParameterSet> vec_set = convertor.SolverParameter();

	BOOST_CHECK(vec_set.size() == 2);
	BOOST_CHECK(vec_set[0]._rate_exc  == 0.0);
	BOOST_CHECK(vec_set[0]._rate_inh  == 0.0);
	BOOST_CHECK(vec_set[1]._rate_exc == rate);
	BOOST_CHECK(vec_set[1]._h_exc    == eff);
	BOOST_CHECK(vec_set[1]._rate_inh == 0.0);
	BOOST_CHECK(vec_set[1]._h_inh    == 0.0);

}

BOOST_FIXTURE_TEST_CASE(test_fixture, ConvertorFixture)
{
	// Repeat the last test to examine the fixture

	vector<MPILib::NodeType> 	vec_type;
	vector<MPILib::Rate>     	vec_rate;
	vector<MPILib::DelayedConnection>	vec_con;

	vec_type.push_back(MPILib::EXCITATORY_DIRECT);
	MPILib::Rate rate = 800.0;
	vec_rate.push_back(rate);

	MPILib::Efficacy eff = 0.03;
	MPILib::DelayedConnection con(1,eff,0.0);
	vec_con.push_back(con);

	std::vector<InputParameterSet> vec_set = GenerateSet(vec_rate, vec_con, vec_type);

	BOOST_CHECK(vec_set.size() == 2);
	BOOST_CHECK(vec_set[0]._rate_exc  == 0.0);
	BOOST_CHECK(vec_set[0]._rate_inh  == 0.0);
	BOOST_CHECK(vec_set[1]._rate_exc == rate);
	BOOST_CHECK(vec_set[1]._h_exc    == eff);
	BOOST_CHECK(vec_set[1]._rate_inh == 0.0);
	BOOST_CHECK(vec_set[1]._h_inh    == 0.0);

}

BOOST_FIXTURE_TEST_CASE(test_current_compensation, ConvertorFixture)
{
	// Repeat the last test to examine the fixture

	vector<MPILib::NodeType> 	vec_type;
	vector<MPILib::Rate>     	vec_rate;
	vector<MPILib::DelayedConnection>	vec_con;

	vec_type.push_back(MPILib::EXCITATORY_DIRECT);
	MPILib::Rate rate = 800.0;
	vec_rate.push_back(rate);

	MPILib::Efficacy eff = 0.03;
	MPILib::DelayedConnection con(1,eff,0.0);
	vec_con.push_back(con);

	double mu    = 0.0;
	double sigma = 0.01;
	_par_curr = CurrentCompensationParameter(mu,sigma);

	std::vector<InputParameterSet> vec_set = GenerateSet(vec_rate, vec_con, vec_type);

	BOOST_CHECK(vec_set.size() == 2);
	BOOST_CHECK(vec_set[1]._rate_exc == rate);
	BOOST_CHECK(vec_set[1]._h_exc    == eff);
	BOOST_CHECK(vec_set[1]._rate_inh == 0.0);
	BOOST_CHECK(vec_set[1]._h_inh    == 0.0);

	double h_e  = vec_set[0]._h_exc;
	double h_i  = vec_set[0]._h_inh;
	double nu_e = vec_set[0]._rate_exc;
	double nu_i = vec_set[0]._rate_inh;

	BOOST_CHECK(h_e == -h_i);

	double mu_r = _par_neuron._tau*(nu_e*h_e + nu_i*h_i);
	double sigma_r = sqrt(_par_neuron._tau*(nu_e*h_e*h_e + nu_i*h_i*h_i));

	BOOST_CHECK(mu == mu_r);
	BOOST_CHECK(sigma == sigma_r);

}

BOOST_FIXTURE_TEST_CASE(test_double_diffusion, ConvertorFixture)
{
	// Repeat the last test to examine the fixture

	vector<MPILib::NodeType> 	vec_type;
	vector<MPILib::Rate>     	vec_rate;
	vector<MPILib::DelayedConnection>	vec_con;

	vec_type.push_back(MPILib::EXCITATORY_GAUSSIAN);
	vec_type.push_back(MPILib::INHIBITORY_GAUSSIAN);

	MPILib::Rate rate = 800.0;
	vec_rate.push_back(rate);
	vec_rate.push_back(rate);

	MPILib::Efficacy eff = 0.01;
	MPILib::DelayedConnection con_e(1,eff,0.0);
	vec_con.push_back(con_e);
	MPILib::DelayedConnection con_i(1,-eff,0.0);
	vec_con.push_back(con_i);

	std::vector<InputParameterSet> vec_set = GenerateSet(vec_rate, vec_con, vec_type);

	BOOST_CHECK(vec_set.size() == 3);             // always one more than there are inputs
	BOOST_CHECK(_conv->NumberDirect() == 0);  // but the number of direct inputs should be 0

	double h_e  = vec_set[0]._h_exc;
	double h_i  = vec_set[0]._h_inh;
	double nu_e = vec_set[0]._rate_exc;
	double nu_i = vec_set[0]._rate_inh;

	double mu     = _par_neuron._tau*(h_e*nu_e + h_i*nu_i);
	double sigma  = sqrt(_par_neuron._tau*(h_e*h_e*nu_e + h_i*h_i*nu_i));

	BOOST_CHECK(mu == 0.0);
	double sigma_r = sqrt(_par_neuron._tau*(2*eff*eff*rate));
	BOOST_CHECK(sigma_r  ==  sigma);
}


#define BOOST_TEST_MODULE "test_read_multiple_basin_forcefield"

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

BOOST_AUTO_TEST_CASE(read_empty_forcefield_2basin)
{
    mjolnir::LoggerManager::set_default_logger("test_read_multiple_basin_forcefield.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
        [files]
        output.prefix = "test"
        [simulator]
        forcefields.type = "MultipleBasin"
        forcefields.units = [
            {basins=["open", "close"], dVs=[0.0, 10.0], delta = 60.0}
        ]
        [[forcefields]]
        name = "open"
        # empty
        [[forcefields]]
        name = "close"
        # empty
        )"_toml;

        const auto ffb = mjolnir::read_forcefield<traits_type>(v, v.at("simulator"));
        const auto ffp = dynamic_cast<mjolnir::MultipleBasinForceField<traits_type>*>(ffb.get());
        BOOST_REQUIRE(ffp);
        const auto& ff = *ffp;

        BOOST_TEST(ff.common_local().size()    == 0u);
        BOOST_TEST(ff.common_global().size()   == 0u);
        BOOST_TEST(ff.common_external().size() == 0u);

        const auto& units = ff.units();
        BOOST_REQUIRE(units.size() == 1u);

        const auto  unit1_ptr = dynamic_cast<mjolnir::MultipleBasin2BasinUnit<traits_type>*>(units.at(0).get());
        BOOST_REQUIRE(unit1_ptr);
        const auto& unit1 = *unit1_ptr;

        BOOST_TEST(unit1.delta() == -60.0);
        BOOST_TEST(unit1.dV1()   ==   0.0);
        BOOST_TEST(unit1.dV2()   ==  10.0);

        BOOST_TEST(unit1.name1() == "open");
        BOOST_TEST(unit1.name2() == "close");

        BOOST_TEST(unit1.local1().size()    == 0u);
        BOOST_TEST(unit1.local2().size()    == 0u);
        BOOST_TEST(unit1.global1().size()   == 0u);
        BOOST_TEST(unit1.global2().size()   == 0u);
        BOOST_TEST(unit1.external1().size() == 0u);
        BOOST_TEST(unit1.external2().size() == 0u);
    }
}

BOOST_AUTO_TEST_CASE(read_empty_forcefield_3basin)
{
    mjolnir::LoggerManager::set_default_logger("test_read_multiple_basin_forcefield.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
        [files]
        output.prefix = "test"
        [simulator]
        forcefields.type = "MultipleBasin"
        forcefields.units = [
            {basins=["open", "mid", "close"], dVs=[0.0, -5.0, 10.0], delta.open-mid = 60.0, delta.mid-close=50.0, delta.close-open = 100.0}
        ]
        [[forcefields]]
        name = "open"
        # empty
        [[forcefields]]
        name = "mid"
        # empty
        [[forcefields]]
        name = "close"
        # empty
        [[forcefields]]
        name = "common"
        # empty
        )"_toml;

        const auto ffb = mjolnir::read_forcefield<traits_type>(v, v.at("simulator"));
        const auto ffp = dynamic_cast<mjolnir::MultipleBasinForceField<traits_type>*>(ffb.get());
        BOOST_REQUIRE(ffp);
        const auto& ff = *ffp;

        BOOST_TEST(ff.common_local().size()    == 0u);
        BOOST_TEST(ff.common_global().size()   == 0u);
        BOOST_TEST(ff.common_external().size() == 0u);

        const auto& units = ff.units();
        BOOST_REQUIRE(units.size() == 1u);

        const auto  unit1_ptr = dynamic_cast<mjolnir::MultipleBasin3BasinUnit<traits_type>*>(units.at(0).get());
        BOOST_REQUIRE(unit1_ptr);
        const auto& unit1 = *unit1_ptr;

        BOOST_TEST(unit1.delta12() ==  -60.0);
        BOOST_TEST(unit1.delta23() ==  -50.0);
        BOOST_TEST(unit1.delta31() == -100.0);
        BOOST_TEST(unit1.dV1()     ==   0.0);
        BOOST_TEST(unit1.dV2()     ==  -5.0);
        BOOST_TEST(unit1.dV3()     ==  10.0);

        BOOST_TEST(unit1.name1() == "open");
        BOOST_TEST(unit1.name2() == "mid");
        BOOST_TEST(unit1.name3() == "close");

        BOOST_TEST(unit1.local1().size()    == 0u);
        BOOST_TEST(unit1.local2().size()    == 0u);
        BOOST_TEST(unit1.local3().size()    == 0u);
        BOOST_TEST(unit1.global1().size()   == 0u);
        BOOST_TEST(unit1.global2().size()   == 0u);
        BOOST_TEST(unit1.global3().size()   == 0u);
        BOOST_TEST(unit1.external1().size() == 0u);
        BOOST_TEST(unit1.external2().size() == 0u);
        BOOST_TEST(unit1.external3().size() == 0u);
    }
}

BOOST_AUTO_TEST_CASE(read_several_forcefield_2basin)
{
    mjolnir::LoggerManager::set_default_logger("test_read_multiple_basin_forcefield.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
        [files]
        output.prefix = "test"
        [simulator]
        forcefields.type = "MultipleBasin"
        forcefields.units = [
            {basins=["open", "close"], dVs=[0.0, 10.0], delta = 60.0}
        ]
        [[forcefields]]
            name = "open"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
        [[forcefields]]
            name = "close"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.local]]
            interaction = "BondAngle"
            potential   = "Harmonic"
            topology    = "none"
            parameters  = []
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "LennardJones"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
        [[forcefields]]
            name = "common"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.local]]
            interaction = "BondAngle"
            potential   = "Harmonic"
            topology    = "none"
            parameters  = []
          [[forcefields.local]]
            interaction = "DihedralAngle"
            potential   = "Gaussian"
            topology    = "none"
            parameters  = []

          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "LennardJones"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "DebyeHuckel"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"

          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}

        )"_toml;

        const auto ffb = mjolnir::read_forcefield<traits_type>(v, v.at("simulator"));
        const auto ffp = dynamic_cast<mjolnir::MultipleBasinForceField<traits_type>*>(ffb.get());
        BOOST_REQUIRE(ffp);
        const auto& ff = *ffp;

        BOOST_TEST(ff.common_local().size()    == 3u);
        BOOST_TEST(ff.common_global().size()   == 3u);
        BOOST_TEST(ff.common_external().size() == 2u);

        const auto& units = ff.units();
        BOOST_REQUIRE(units.size() == 1u);

        const auto  unit1_ptr = dynamic_cast<mjolnir::MultipleBasin2BasinUnit<traits_type>*>(units.at(0).get());
        BOOST_REQUIRE(unit1_ptr);
        const auto& unit1 = *unit1_ptr;

        BOOST_TEST(unit1.delta() == -60.0);
        BOOST_TEST(unit1.dV1()   ==   0.0);
        BOOST_TEST(unit1.dV2()   ==  10.0);

        BOOST_TEST(unit1.name1() == "open");
        BOOST_TEST(unit1.name2() == "close");

        BOOST_TEST(unit1.local1().size()    == 1u);
        BOOST_TEST(unit1.global1().size()   == 1u);
        BOOST_TEST(unit1.external1().size() == 1u);

        BOOST_TEST(unit1.local2().size()    == 2u);
        BOOST_TEST(unit1.global2().size()   == 2u);
        BOOST_TEST(unit1.external2().size() == 0u);
    }
}

BOOST_AUTO_TEST_CASE(read_several_forcefield_3basin)
{
    mjolnir::LoggerManager::set_default_logger("test_read_multiple_basin_forcefield.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
        [files]
        output.prefix = "test"
        [simulator]
        [simulator.forcefields]
        type = "MultipleBasin"
        [[simulator.forcefields.units]]
        basins = ["open", "mid", "close"]
        dVs    = [   0.0,  -5.0,    10.0]
        delta.open-mid   = 60.0
        delta.mid-close  = 50.0
        delta.close-open = 100.0

        [[forcefields]]
            name = "open"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}

        [[forcefields]]
            name = "mid"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.local]]
            interaction = "BondAngle"
            potential   = "Harmonic"
            topology    = "none"
            parameters  = []
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "LennardJones"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}

        [[forcefields]]
            name = "close"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.local]]
            interaction = "BondAngle"
            potential   = "Harmonic"
            topology    = "none"
            parameters  = []
          [[forcefields.local]]
            interaction = "BondAngle"
            potential   = "Harmonic"
            topology    = "none"
            parameters  = []

          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "LennardJones"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"

          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}

        [[forcefields]]
            name = "common"
          [[forcefields.local]]
            interaction = "BondLength"
            potential   = "Harmonic"
            topology    = "bond"
            parameters  = []
          [[forcefields.local]]
            interaction = "BondAngle"
            potential   = "Harmonic"
            topology    = "none"
            parameters  = []
          [[forcefields.local]]
            interaction = "DihedralAngle"
            potential   = "Gaussian"
            topology    = "none"
            parameters  = []
          [[forcefields.local]]
            interaction = "DihedralAngle"
            potential   = "Gaussian"
            topology    = "none"
            parameters  = []

          [[forcefields.global]]
            interaction = "Pair"
            potential   = "ExcludedVolume"
            epsilon     = 3.14
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "LennardJones"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "DebyeHuckel"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"
          [[forcefields.global]]
            interaction = "Pair"
            potential   = "DebyeHuckel"
            parameters  = []
            ignore.molecule = "Nothing"
            ignore.particles_within = {bond = 3, contact = 1}
            spatial_partition.type = "Naive"

          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}
          [[forcefields.external]]
            interaction = "Distance"
            potential   = "ExcludedVolumeWall"
            epsilon     = 3.14
            parameters  = []
            shape = {name = "AxisAlignedPlane", axis="+X", position=1.0, margin=0.5}

        )"_toml;

        const auto ffb = mjolnir::read_forcefield<traits_type>(v, v.at("simulator"));
        const auto ffp = dynamic_cast<mjolnir::MultipleBasinForceField<traits_type>*>(ffb.get());
        BOOST_REQUIRE(ffp);
        const auto& ff = *ffp;

        BOOST_TEST(ff.common_local().size()    == 4u);
        BOOST_TEST(ff.common_global().size()   == 4u);
        BOOST_TEST(ff.common_external().size() == 4u);

        const auto& units = ff.units();
        BOOST_REQUIRE(units.size() == 1u);

        const auto  unit1_ptr = dynamic_cast<mjolnir::MultipleBasin3BasinUnit<traits_type>*>(units.at(0).get());
        BOOST_REQUIRE(unit1_ptr);
        const auto& unit1 = *unit1_ptr;

        BOOST_TEST(unit1.delta12() ==  -60.0);
        BOOST_TEST(unit1.delta23() ==  -50.0);
        BOOST_TEST(unit1.delta31() == -100.0);
        BOOST_TEST(unit1.dV1()     ==   0.0);
        BOOST_TEST(unit1.dV2()     ==  -5.0);
        BOOST_TEST(unit1.dV3()     ==  10.0);

        BOOST_TEST(unit1.name1() == "open");
        BOOST_TEST(unit1.name2() == "mid");
        BOOST_TEST(unit1.name3() == "close");

        BOOST_TEST(unit1.local1().size()    == 1u);
        BOOST_TEST(unit1.global1().size()   == 1u);
        BOOST_TEST(unit1.external1().size() == 1u);

        BOOST_TEST(unit1.local2().size()    == 2u);
        BOOST_TEST(unit1.global2().size()   == 2u);
        BOOST_TEST(unit1.external2().size() == 2u);

        BOOST_TEST(unit1.local3().size()    == 3u);
        BOOST_TEST(unit1.global3().size()   == 3u);
        BOOST_TEST(unit1.external3().size() == 3u);
    }
}

BOOST_AUTO_TEST_CASE(read_empty_forcefield_2basins_2units)
{
    mjolnir::LoggerManager::set_default_logger("test_read_multiple_basin_forcefield.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const auto v = u8R"(
        [files]
        output.prefix = "test"
        [simulator]
        forcefields.type = "MultipleBasin"
        forcefields.units = [
            {basins=["domain1-open", "domain1-close"], dVs=[0.0, 10.0], delta = 60.0},
            {basins=["domain2-open", "domain2-close"], dVs=[0.0, 15.0], delta = 50.0}
        ]
        [[forcefields]]
        name = "domain1-open"
        # empty
        [[forcefields]]
        name = "domain1-close"
        # empty
        [[forcefields]]
        name = "domain2-open"
        # empty
        [[forcefields]]
        name = "domain2-close"
        # empty
        )"_toml;

        const auto ffb = mjolnir::read_forcefield<traits_type>(v, v.at("simulator"));
        const auto ffp = dynamic_cast<mjolnir::MultipleBasinForceField<traits_type>*>(ffb.get());
        BOOST_REQUIRE(ffp);
        const auto& ff = *ffp;

        BOOST_TEST(ff.common_local().size()    == 0u);
        BOOST_TEST(ff.common_global().size()   == 0u);
        BOOST_TEST(ff.common_external().size() == 0u);

        const auto& units = ff.units();
        BOOST_REQUIRE(units.size() == 2u);

        const auto  unit1_ptr = dynamic_cast<mjolnir::MultipleBasin2BasinUnit<traits_type>*>(units.at(0).get());
        BOOST_REQUIRE(unit1_ptr);
        const auto& unit1 = *unit1_ptr;

        BOOST_TEST(unit1.delta() == -60.0);
        BOOST_TEST(unit1.dV1()   ==   0.0);
        BOOST_TEST(unit1.dV2()   ==  10.0);

        BOOST_TEST(unit1.name1() == "domain1-open");
        BOOST_TEST(unit1.name2() == "domain1-close");

        BOOST_TEST(unit1.local1().size()    == 0u);
        BOOST_TEST(unit1.local2().size()    == 0u);
        BOOST_TEST(unit1.global1().size()   == 0u);
        BOOST_TEST(unit1.global2().size()   == 0u);
        BOOST_TEST(unit1.external1().size() == 0u);
        BOOST_TEST(unit1.external2().size() == 0u);

        const auto  unit2_ptr = dynamic_cast<mjolnir::MultipleBasin2BasinUnit<traits_type>*>(units.at(1).get());
        BOOST_REQUIRE(unit2_ptr);
        const auto& unit2 = *unit2_ptr;

        BOOST_TEST(unit2.delta() == -50.0);
        BOOST_TEST(unit2.dV1()   ==   0.0);
        BOOST_TEST(unit2.dV2()   ==  15.0);

        BOOST_TEST(unit2.name1() == "domain2-open");
        BOOST_TEST(unit2.name2() == "domain2-close");

        BOOST_TEST(unit2.local1().size()    == 0u);
        BOOST_TEST(unit2.local2().size()    == 0u);
        BOOST_TEST(unit2.global1().size()   == 0u);
        BOOST_TEST(unit2.global2().size()   == 0u);
        BOOST_TEST(unit2.external1().size() == 0u);
        BOOST_TEST(unit2.external2().size() == 0u);

    }
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SHIFT_RIGHT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SHIFT_RIGHT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-operator
    This function object returns the first operand shifted right by the second one

    Infix notation can be used with operator '>>'

    @par Header <boost/simd/function/shift_right.hpp>

    @see  shift_left, shr, rshl, rshr, rol, ror
    @pre the second parameter must be of integer type, scalar or of the same number
    of elements as the first parameter

    @par Note:
      - on many architectures a scalar second parameter results in a much faster call

    @par Example:

      @snippet shift_right.cpp shift_right

    @par Possible output:

      @snippet shift_right.txt shift_right

  **/
  Value shift_right(Value const& x, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/shift_right.hpp>
#include <boost/simd/function/simd/shift_right.hpp>

#endif


/*
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright 2011-2014 Andrea Bedini <andrea@andreabedini.com>
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 */

#ifndef GRAPH_TYPE_HPP
#define GRAPH_TYPE_HPP

#include <boost/graph/adjacency_list.hpp>

typedef boost::adjacency_list
  < boost::listS, boost::listS, boost::undirectedS
  , boost::property<boost::vertex_index_t, unsigned int>
  , boost::property<boost::edge_index_t, unsigned int>
  > graph_type;

#endif


// Boost test for Conan package
// Dmitriy Vetutnev, ODANT, 2018

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <iostream>
#include <cstdlib>

int main(int, char**) {

    auto g_date = boost::gregorian::date_from_iso_string("19901015");
    std::cout << "g_date.year(): " << g_date.year() << std::endl;
    std::cout << "g_date.month(): " << g_date.month() << std::endl;
    std::cout << "g_date.day(): " << g_date.day() << std::endl;

    auto pt = boost::posix_time::second_clock::universal_time();
    std::cout << "pt.date(): " << pt.date() << std::endl;
    std::cout << "pt.time_of_day(): " << pt.time_of_day() << std::endl;

    return EXIT_SUCCESS;
}


#include <test/unit/mcmc/hmc/mock_hmc.hpp>
#include <test/test-models/good/mcmc/hmc/hamiltonians/funnel.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <stan/callbacks/stream_writer.hpp>
#include <boost/random/additive_combine.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>

typedef boost::ecuyer1988 rng_t;

TEST(BaseHamiltonian, update_potential_gradient) {

  std::fstream data_stream(std::string("").c_str(), std::fstream::in);
  stan::io::dump data_var_context(data_stream);
  data_stream.close();

  std::stringstream model_output;
  funnel_model_namespace::funnel_model model(data_var_context, &model_output);

  stan::mcmc::mock_hamiltonian<funnel_model_namespace::funnel_model, rng_t> metric(model);
  stan::mcmc::ps_point z(11);
  z.q.setOnes();

  std::stringstream debug, info, warn, error, fatal;
  stan::callbacks::stream_logger logger(debug, info, warn, error, fatal);

  metric.update_potential_gradient(z, logger);

  EXPECT_FLOAT_EQ(10.73223197, z.V);

  EXPECT_FLOAT_EQ(8.757758279, z.g(0));
  for (int i = 1; i < z.q.size(); ++i)
    EXPECT_FLOAT_EQ(0.1353352832, z.g(i));

  EXPECT_EQ("", model_output.str());
  EXPECT_EQ("", debug.str());
  EXPECT_EQ("", info.str());
  EXPECT_EQ("", warn.str());
  EXPECT_EQ("", error.str());
  EXPECT_EQ("", fatal.str());
}

TEST(BaseHamiltonian, streams) {
  stan::test::capture_std_streams();

  std::fstream data_stream(std::string("").c_str(), std::fstream::in);
  stan::io::dump data_var_context(data_stream);
  data_stream.close();

  EXPECT_NO_THROW(funnel_model_namespace::funnel_model
                  model(data_var_context, 0,
                        static_cast<std::stringstream*>(0)));
  std::stringstream output;
  EXPECT_NO_THROW(funnel_model_namespace::funnel_model model(data_var_context, &output));
  EXPECT_EQ("", output.str());

  stan::test::reset_std_streams();
  EXPECT_EQ("", stan::test::cout_ss.str());
  EXPECT_EQ("", stan::test::cerr_ss.str());
}


#include "assembly_dual_controllers/robot_model/franka_panda_model.h"
#include <Eigen/Geometry>

FrankaPandaModel::FrankaPandaModel()
{
  initModel();
}

Eigen::MatrixXd FrankaPandaModel::getJacobianMatrix(const Eigen::Ref<const Eigen::VectorXd> &q) const
{
  auto ee_default = Eigen::Vector3d(0.0, 0.0, 0.107);
  return getJacobianMatrix(q, ee_default);
}

Eigen::MatrixXd FrankaPandaModel::getJacobianMatrix(const Eigen::Ref<const Eigen::VectorXd> &q, const Eigen::Ref<const Eigen::Vector3d> & tool_tf) const
{
  Eigen::Vector3d tool_position = rot_ee_ * tool_tf;
  Eigen::MatrixXd j_temp;
  j_temp.setZero(6, kDof);
  Eigen::Matrix<double, 6, kDof> j;
  RigidBodyDynamics::CalcPointJacobian6D(*model_, q, body_id_[kDof - 1], tool_position, j_temp, true);

  for (int i = 0; i < 2; i++)
  {
    j.block<3, kDof>(i * 3, 0) = j_temp.block<3, kDof>(3 - i * 3, 0);
  }

  return j;
}

Eigen::Vector3d FrankaPandaModel::getTranslation(const Eigen::Ref<const Eigen::VectorXd> &q) const
{
  return RigidBodyDynamics::CalcBodyToBaseCoordinates(*model_, q, body_id_[kDof - 1], ee_position_, true);
}

Eigen::Vector3d FrankaPandaModel::getTranslation(const Eigen::Ref<const Eigen::VectorXd> &q, const Eigen::Ref<const Eigen::Vector3d> & tool_tf) const
{
  Eigen::Vector3d tool_position = rot_ee_ * tool_tf;
  return RigidBodyDynamics::CalcBodyToBaseCoordinates(*model_, q, body_id_[kDof - 1], ee_position_, true);
}

Eigen::Matrix3d FrankaPandaModel::getRotation(const Eigen::Ref<const Eigen::VectorXd> &q) const
{
  auto M = rot_ee_ * Eigen::AngleAxisd(-M_PI/4., Eigen::Vector3d::UnitZ());
  return RigidBodyDynamics::CalcBodyWorldOrientation(*model_, q, body_id_[kDof - 1], true).transpose() * M;
}

Eigen::Isometry3d FrankaPandaModel::getTransform(const Eigen::Ref<const Eigen::VectorXd> &q) const
{
  Eigen::Isometry3d transform;
  transform.linear() = getRotation(q);
  transform.translation() = getTranslation(q);

  return transform;
}

Eigen::Isometry3d FrankaPandaModel::getTransform(const Eigen::Ref<const Eigen::VectorXd> &q, const Eigen::Ref<const Eigen::Vector3d> & tool_tf) const
{
  Eigen::Isometry3d transform;
  Eigen::Matrix3d R_7a;
  R_7a << 0.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 0.0;

  transform.linear() = getRotation(q)*R_7a;
  transform.translation() = getTranslation(q, tool_tf);

  return transform;
}

Eigen::MatrixXd FrankaPandaModel::getJointLimit() const
{
  Eigen::Matrix<double, kDof, 2> joint_limits;
  joint_limits << -2.8973, 2.8973,
                  -1.7628, 1.7628,
                  -2.8973, 2.8973,
                  -3.0718, -0.0698,
                  -2.8973, 2.8973,
                  -0.0175, 3.7525,
                  -2.8973, 2.8973;
  return joint_limits;
}

Eigen::VectorXd FrankaPandaModel::getInitialConfiguration() const
{
}

int FrankaPandaModel::getDof()
{
  return kDof;
}

void FrankaPandaModel::initModel()
{
  Eigen::Matrix<double, 7, 4> dh;
  dh.setZero();
  initModel(dh);
}

void FrankaPandaModel::initModel(const Eigen::Ref<const Eigen::Matrix<double, 7, 4>> &dh)
{
  model_ = std::make_shared<RigidBodyDynamics::Model>();

  model_->gravity = Eigen::Vector3d(0., 0., -kGravity);

  double mass[kDof];
  mass[0] = 1.0;
  mass[1] = 1.0;
  mass[2] = 1.0;
  mass[3] = 1.0;
  mass[4] = 1.0;
  mass[5] = 1.0;
  mass[6] = 1.0;

  Eigen::Vector3d global_joint_position[kDof], global_joint_position_new[kDof], axis[kDof];
  Eigen::VectorXd dh_al(7), dh_a(7), dh_d(7), dh_q(7);
  Eigen::Isometry3d transform_joint;
  transform_joint.setIdentity();
  const Eigen::Ref<const Eigen::VectorXd> &a_offset = dh.col(0);
  const Eigen::Ref<const Eigen::VectorXd> &d_offset = dh.col(1);
  const Eigen::Ref<const Eigen::VectorXd> &q_offset = dh.col(2);
  const Eigen::Ref<const Eigen::VectorXd> &alpha_offset = dh.col(3);

  dh_al << 0.0, -1.0 * M_PI_2, M_PI_2, M_PI_2, -1.0 * M_PI_2, M_PI_2, M_PI_2;
  dh_a << 0.0, 0.0, 0.0, 0.0825, -0.0825, 0.0, 0.088;
  dh_d << 0.333, 0.0, 0.316, 0.0, 0.384, 0.0, 0.0;

  global_joint_position[0] = Eigen::Vector3d(0.0, 0.0, 0.3330);
  global_joint_position[1] = Eigen::Vector3d(0.0, 0.0, 0.3330);
  global_joint_position[2] = Eigen::Vector3d(0.0, 0.0, 0.6490);
  global_joint_position[3] = Eigen::Vector3d(0.0825, 0.0, 0.6490);
  global_joint_position[4] = Eigen::Vector3d(0.0, 0.0, 1.0330);
  global_joint_position[5] = Eigen::Vector3d(0.0, 0.0, 1.0330);
  global_joint_position[6] = Eigen::Vector3d(0.0880, 0.0, 1.0330);

  com_position_[0] = Eigen::Vector3d(0.000096, -0.0346, 0.2575);
  com_position_[1] = Eigen::Vector3d(0.0002, 0.0344, 0.4094);
  com_position_[2] = Eigen::Vector3d(0.0334, 0.0266, 0.6076);
  com_position_[3] = Eigen::Vector3d(0.0331, -0.0266, 0.6914);
  com_position_[4] = Eigen::Vector3d(0.0013, 0.0423, 0.9243);
  com_position_[5] = Eigen::Vector3d(0.0421, -0.0103, 1.0482);
  com_position_[6] = Eigen::Vector3d(0.1, -0.0120, 0.9536);

  for (int i = 0; i < kDof; i++)
  {
    transform_joint = transform_joint * transformDH(dh_a(i) + a_offset(i), dh_d(i) + d_offset(i), dh_al(i) + alpha_offset(i), q_offset(i));
    axis[i] = transform_joint.matrix().block<3, 1>(0, 2);
    global_joint_position_new[i] = transform_joint.translation();
  }

  rot_ee_ = transform_joint.linear();
  ee_position_ = Eigen::Vector3d(0.0, 0.0, 0.107);
  ee_position_ = rot_ee_ * ee_position_;

  joint_posision_[0] = global_joint_position_new[0];
  for (int i = 1; i < kDof; i++)
    joint_posision_[i] = global_joint_position_new[i] - global_joint_position_new[i - 1];

  for (int i = 0; i < kDof; i++)
    com_position_[i] -= global_joint_position[i];

  RigidBodyDynamics::Math::Vector3d inertia[kDof];
  for (int i = 0; i < kDof; i++)
    inertia[i] = Eigen::Vector3d::Identity() * 0.001;

  for (int i = 0; i < kDof; i++)
  {
    body_[i] = RigidBodyDynamics::Body(mass[i], com_position_[i], inertia[i]);
    joint_[i] = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeRevolute, axis[i]);
    if (i == 0)
      body_id_[i] = model_->AddBody(0, RigidBodyDynamics::Math::Xtrans(joint_posision_[i]), joint_[i], body_[i]);
    else
      body_id_[i] = model_->AddBody(body_id_[i - 1], RigidBodyDynamics::Math::Xtrans(joint_posision_[i]), joint_[i], body_[i]);
  }
}

Eigen::Isometry3d FrankaPandaModel::transformDH(const double a, const double d, const double alpha, const double theta)
{
  Eigen::Isometry3d transform_dh;
  double st = sin(theta), ct = cos(theta);
  double sa = sin(alpha), ca = cos(alpha);
  transform_dh.setIdentity();
  transform_dh.linear() << ct, -1 * st, 0.0,
      st * ca, ct * ca, -1 * sa,
      st * sa, ct * sa, ca;
  transform_dh.translation() << a, -1 * sa * d, ca * d;
  return transform_dh;
}


//==================================================================================================
/*!
  @file

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IDIVNEARBYINT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IDIVNEARBYINT_HPP_INCLUDED

// workaround for "idivnearbyint(a0/a1) in floating scalar mode when a0 is non zero and a1 is zero
// with clang 3.7 and 3.8
// 04/24/2016

#include <boost/predef/compiler.h>

#if BOOST_COMP_CLANG >= BOOST_VERSION_NUMBER(3,7,0)
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using result_t =  bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( bd::functor<bs::tag::inearbyint_> const&
                                          , A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      if (a1) return  inearbyint(a0/a1);
      if (!a0) return  Nan<result_t>();
      return is_negative(bitwise_or(a0, a1)) ? Valmin<result_t>() : Valmax<result_t>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::inearbyint_> const&,
                                      A0  a0, A0  a1) const BOOST_NOEXCEPT
    {
      return div(nearbyint, a0, a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( bd::functor<bs::tag::inearbyint_> const&
                                                      , A0  a0
                                                      , A0  a1) const BOOST_NOEXCEPT
    {
      return inearbyint(a0/a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif
} } }

#endif
#endif


/*******************************************************************************
*
*  Filename    : RooFit_MCTemplate.cc
*  Description : Plotting to file for template methods.cc
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"
#include "TstarAnalysis/LimitCalc/interface/MakeKeysPdf.hpp"
#include "TstarAnalysis/LimitCalc/interface/SampleRooFitMgr.hpp"
#include "TstarAnalysis/LimitCalc/interface/SimFit.hpp"
#include "TstarAnalysis/LimitCalc/interface/Template.hpp"

#include "ManagerUtils/Maths/interface/RooFitExt.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"

#include <boost/format.hpp>
#include <fstream>
#include <string>

#include "RooDataSet.h"
#include "RooNLLVar.h"
#include "RooMinimizer.h"

using namespace std;
using namespace mgr;

/*******************************************************************************
*   Object naming convention
*******************************************************************************/
string
TemplatePdfName( const std::string& datasetname )
{
  return datasetname + "template";
}

extern const string StitchTemplatePdfName = "templatemaster";

/*******************************************************************************
*   Main control flow to be caleed by main function
*******************************************************************************/
void
MakeTemplate(
  SampleRooFitMgr*          data,
  SampleRooFitMgr*          bg,
  vector<SampleRooFitMgr*>& signal_list
  )
{
  vector<RooAbsPdf*> pdflist;
  vector<RooAbsReal*> funclist;

  for( auto& sig : signal_list ){
    pdflist.push_back( MakeFullKeysPdf( sig ) );
    // funclist.push_back( sig->Func(StitchKeyNormName));
  }

  auto fitresultlist = MakeFullTemplate( bg );
  MakeTemplatePlot( data, bg, signal_list.front(), fitresultlist.front(), true );
  MakeTemplatePlot( data, bg, signal_list.front(), fitresultlist.front(), false );
  pdflist.push_back( bg->Pdf( StitchTemplatePdfName ) );

  SaveRooWorkSpace(
    data->DataSet( "" ),
    pdflist,
    {}
    );

  for( auto& signal : signal_list ){
    MakeTemplateCardFile( data, bg, signal );
  }
}

/*******************************************************************************
*   Fitting function implementations
*******************************************************************************/
RooFitResult*
FitBackgroundTemplate( SampleRooFitMgr* bg, const string& datatag )
{
  const string bgpdfname = TemplatePdfName( datatag );

  RooAbsPdf* bgpdf = bg->NewPdf( bgpdfname, limnamer.GetInput<string>( "fitfunc" ) );
  // Manually calling NNL minizer functions
  static RooCmdArg min    = RooFit::Minimizer( "Minuit", "Migrad" );
  static RooCmdArg sumerr = RooFit::SumW2Error( kTRUE );
  static RooCmdArg hesse  = RooFit::Minos( kTRUE );
  static RooCmdArg save   = RooFit::Save();
  //static RooCmdArg ncpu   = RooFit::NumCPU( 6 );
  static RooCmdArg verb   = RooFit::Verbose( kFALSE );
  static RooCmdArg printl = RooFit::PrintLevel( -1 );
  static RooCmdArg printe = RooFit::PrintEvalErrors( -1 );
  static RooCmdArg printw = RooFit::Warnings( kFALSE );

  RooLinkedList fitopt;
  fitopt.Add( &min    );
  fitopt.Add( &sumerr );
  fitopt.Add( &hesse  );
  fitopt.Add( &save   );
  //fitopt.Add( &ncpu   );
  fitopt.Add( &verb   );
  fitopt.Add( &printl );
  fitopt.Add( &printe );
  fitopt.Add( &printw );

  const unsigned maxiter = 30;
  unsigned iter = 0 ;
  RooFitResult* ans = NULL ;
  while( !ans ){
    ans = bgpdf->fitTo( *( bg->DataSet(datatag) ), fitopt );
    if( ans->status() ){ // Not properly converged
      iter++;
      if( iter > maxiter ) { break; }
      delete ans;
      ans = NULL;
    }
  }
  cout << ">>>> BKG FIT iteration: " << iter << endl;
  bg->SetConstant( kTRUE );// Freezing all constants

  return ans;
}

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

vector<RooFitResult*>
MakeFullTemplate( SampleRooFitMgr* bg )
{
  vector<string> bgpdflist;
  vector<RooFitResult*> ans;

  for( const auto& datasetname : bg->SetNameList() ){
    ans.push_back( FitBackgroundTemplate( bg, datasetname ) );
    bgpdflist.push_back( TemplatePdfName( datasetname ) );
  }

  MakeSimpleStitchPdf( bg, StitchTemplatePdfName, bgpdflist );
  return ans;
}

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

void
MakeTemplateCardFile( SampleRooFitMgr* data, SampleRooFitMgr* bg, SampleRooFitMgr* sig )
{
  RooAbsData* dataobs = data->DataSet( "" );
  RooAbsPdf* bgpdf    = bg->Pdf( StitchTemplatePdfName );
  RooAbsPdf* sigpdf   = sig->Pdf( StitchKeyPdfName );

  ofstream cardfile( limnamer.TextFileName( "card", {sig->Name()} ) );

  MakeCardCommon( cardfile, dataobs, bgpdf, sigpdf );

  cardfile << boost::format( "%12s %15lf %15lf" )
    % "rate"
    % sig->DataSet()->sumEntries()
    % bg->DataSet()->sumEntries()
           << endl;

  cardfile << "----------------------------------------" << endl;

  const Parameter null( 0, 0, 0 );
  const Parameter lumi( 1, 0.062, 0.062 );
  const Parameter lepunc( 1, 0.03, 0.03 );
  const Parameter sigstatunc = sig->Sample().SelectionEfficiency();
  const Parameter bkgstatunc( 1, 0.03, 0.03 );// includes uncertaintly from cross section and selection effiency

  const Parameter sigjecunc  = GetMCNormError( sig, "jecUp",    "jecDown"    );
  const Parameter sigjerunc  = GetMCNormError( sig, "jetresUp", "jetresDown" );
  const Parameter siglepunc  = GetMCNormError( sig, "lepUp",    "lepDown"    );
  const Parameter sigbtagunc = GetMCNormError( sig, "btagUp",   "btagDown"   );
  const Parameter sigpuunc   = GetMCNormError( sig, "puUp",     "puDown"     );
  const Parameter sigpdfunc  = GetMCNormError( sig, "pdfUp",    "pdfDown"    );

  const Parameter bkgjecunc  = GetMCNormError( bg, "jecUp",    "jecDown"    );
  const Parameter bkgjerunc  = GetMCNormError( bg, "jetresUp", "jetresDown" );
  const Parameter bkglepunc  = GetMCNormError( bg, "lepUp",    "lepDown"    );
  const Parameter bkgbtagunc = GetMCNormError( bg, "btagUp",   "btagDown"   );
  const Parameter bkgpuunc   = GetMCNormError( bg, "puUp",     "puDown"     );
  const Parameter bkgpdfunc  = GetMCNormError( bg, "pdfUp",    "pdfDown"    );

  PrintNuisanceFloats( cardfile, "Lumi",    "lnN", lumi,       lumi        );
  PrintNuisanceFloats( cardfile, "lepsys",  "lnN", lepunc,     lepunc      );
  PrintNuisanceFloats( cardfile, "sigstat", "lnN", sigstatunc, null        );
  PrintNuisanceFloats( cardfile, "bkgstat", "lnN", null,       bkgstatunc  );
  PrintNuisanceFloats( cardfile, "jec",     "lnN", sigjecunc,  bkgjecunc   );
  PrintNuisanceFloats( cardfile, "jer",     "lnN", sigjerunc,  bkgjerunc   );
  PrintNuisanceFloats( cardfile, "lep",     "lnN", siglepunc,  bkglepunc   );
  PrintNuisanceFloats( cardfile, "btag",    "lnN", sigbtagunc, bkgbtagunc  );
  PrintNuisanceFloats( cardfile, "pileup",  "lnN", sigpuunc,   bkgpuunc    );
  PrintNuisanceFloats( cardfile, "pdf",     "lnN", sigpdfunc,  bkgpdfunc   );

  // Getting fitting parameters
  for( const auto& var : bg->VarContains( "template" ) ){
    const string varname = var->GetName();
    if( varname.find( "coeff" ) == string::npos ){
      PrintFloatParam( cardfile, var );
    }
  }

  // Getting stitching co-efficiencts
  for( const auto& var : bg->VarContains( "coeff" ) ){
    PrintFlatParam( cardfile, var );
  }

  for( const auto& var : sig->VarContains( "coeff" ) ){
    PrintFlatParam( cardfile, var );
  }

  cardfile.close();
}

/*******************************************************************************
*   Plotting fit results
*******************************************************************************/
void
MakeTemplatePlot(
  SampleRooFitMgr* data,
  SampleRooFitMgr* mc,
  SampleRooFitMgr* signal,
  RooFitResult*    fitresult,
  const bool       use_data )
{
  // First plot against MC
  const double TotalLuminosity = mgr::SampleMgr::TotalLuminosity();
  const double xmin            = SampleRooFitMgr::x().getMin();
  const double xmax            = SampleRooFitMgr::x().getMax();

  TCanvas* c     = mgr::NewCanvas();
  TPad* toppad   = mgr::NewTopPad();
  TPad* botpad   = mgr::NewBottomPad();
  RooPlot* frame = SampleRooFitMgr::x().frame();

  // Objects to draw
  RooDataSet* set   = (RooDataSet*)( use_data ? data->DataSet() : mc->DataSet() );
  RooAbsPdf* bkgpdf = mc->Pdf( TemplatePdfName( "" ) );// Taking central value only
  RooAbsPdf* sigpdf = signal->Pdf( StitchKeyPdfName );

  /*******************************************************************************
  *   Objects for drawing top pad
  *******************************************************************************/
  toppad->Draw();
  toppad->cd();

  TGraph* setplot = mgr::PlotOn(
    frame,
    set,
    RooFit::DrawOption( PGS_DATA )
    );

  TGraph* pdfplot = mgr::PlotFitErrorOn(
    frame, bkgpdf, fitresult,
    RooFit::Normalization( set->sumEntries(), RooAbsReal::NumEvent )
    );

  TGraph* sigplot = mgr::PlotOn(
    frame,
    sigpdf,
    RooFit::DrawOption( PGS_SIGNAL ),
    RooFit::Normalization( signal->ExpectedYield(), RooAbsReal::NumEvent )
    );
  tstar::RemoveDataXBar( (TGraphAsymmErrors*)setplot );

  // Typical styling options
  frame->Draw();
  frame->SetMinimum( 0.3 );
  mgr::SetTopPlotAxis( frame );
  frame->SetTitle("");

  c->cd();
  /*******************************************************************************
  *   Objects for drawing bottom pad
  *******************************************************************************/
  botpad->Draw();
  botpad->cd();

  TGraphAsymmErrors* bgrelplot = mgr::DividedGraph(
    (TGraphAsymmErrors*)pdfplot,
    pdfplot
    );
  TGraphAsymmErrors* datarelplot = mgr::DividedGraph(
    (TGraphAsymmErrors*)setplot,
    pdfplot
    );
  tstar::RemoveDataXBar( datarelplot );
  bgrelplot->Draw( "AL3" );
  datarelplot->Draw( PGS_DATA );

  TLine cen( xmin, 1, xmax, 1 );
  TLine lineup( xmin, 1.5, xmax, 1.5 );
  TLine linedown( xmin, 0.5, xmax, 0.5 );
  cen.Draw();
  cen.SetLineColor( KBLUE );
  cen.SetLineWidth( 2 );
  lineup.Draw();
  lineup.SetLineColor( kBlack );
  lineup.SetLineStyle( 3 );
  linedown.Draw();
  linedown.SetLineColor( kBlack );
  linedown.SetLineStyle( 3 );

  // Title setting
  bgrelplot->GetXaxis()->SetTitle( frame->GetXaxis()->GetTitle() );
  bgrelplot->GetXaxis()->SetRangeUser( xmin, xmax );
  bgrelplot->GetYaxis()->SetTitle( "Data/Bkg.fit" );
  bgrelplot->SetMaximum( 1.6 );
  bgrelplot->SetMinimum( 0.4 );
  mgr::SetBottomPlotAxis( bgrelplot );

  c->cd();
  /*******************************************************************************
  *   Common styling and additiona objects
  *******************************************************************************/
  tstar::SetSignalStyle( sigplot );
  tstar::SetFitBGStyle( pdfplot );
  tstar::SetSignalStyle( sigplot );

  tstar::SetFitBGStyle( bgrelplot );
  tstar::SetDataStyle( datarelplot );
  tstar::SetDataStyle( setplot );

  // Legend entries
  const double legend_x_min = 0.65;
  const double legend_y_min = 0.70;
  TLegend* l                = mgr::NewLegend( legend_x_min, legend_y_min );
  boost::format sigfmt( "%s" );
  const string sigentry  = str( sigfmt % signal->RootName() );
  const string dataentry = ( use_data ) ? "Data" : "M.C. Bkg.";
  const string fitentry  = string( "Bkg. fit to MC" ) + ( use_data ? "(Norm.)" : "" );

  l->AddEntry( setplot, dataentry.c_str(), "pe" );
  l->AddEntry( pdfplot, fitentry.c_str(),  "l"  );
  l->AddEntry( sigplot, sigentry.c_str(),  "l"  );
  l->Draw();

  // Additional information plotting
  boost::format goffmt( "K = %.3lf" );
  const double ksprob   = KSTest( *( set ), *( bkgpdf ), SampleRooFitMgr::x() );
  const string gofentry = str( goffmt % ksprob );

  mgr::DrawCMSLabel();
  mgr::DrawLuminosity( TotalLuminosity );

  LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX + TEXT_MARGIN/2, TOP_LEFT )
  .WriteLine( limnamer.GetChannelEXT( "Root Name" ) )
  .SetOrigin( PLOT_X_TEXT_MAX, legend_y_min-TEXT_MARGIN, TOP_RIGHT )
  .WriteLine( gofentry );

  // Range setting and saving
  const double ymax = mgr::GetYmax( pdfplot, setplot, sigplot );
  frame->SetMaximum( ymax * 1.5 );

  const string rootfile = limnamer.PlotRootFile();
  const string lastag   = use_data ? "fitmc-vs-data" : "fitmc-vs-mc";
  mgr::SaveToPDF( c, limnamer.PlotFileName( "", "fitplot", signal->Name(), lastag ) );
  mgr::SaveToROOT( c, rootfile, limnamer.PlotFileName( "fitplot", signal->Name(), lastag ) );
  frame->SetMaximum( ymax * 300 );
  toppad->SetLogy( kTRUE );
  mgr::SaveToPDF( c, limnamer.PlotFileName( "fitplot", signal->Name(), lastag, "log" ) );

  // Cleaning up
  delete frame;
  delete c;
  delete l;
}


// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2021 FZI Forschungszentrum Informatik
// Created on behalf of Universal Robots A/S
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------

//-----------------------------------------------------------------------------
/*!\file    cartesian_trajectory_segment.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2021/01/20
 *
 */
//-----------------------------------------------------------------------------

#include <cartesian_trajectory_interpolation/cartesian_trajectory_segment.h>
#include <algorithm>
#include <cmath>
#include "Eigen/src/Core/GlobalFunctions.h"
#include "Eigen/src/Core/Matrix.h"
#include "Eigen/src/Core/util/Constants.h"
#include "Eigen/src/Geometry/Quaternion.h"
#include <Eigen/Dense>

namespace ros_controllers_cartesian
{
using Time = CartesianTrajectorySegment::Time;
using SplineState = CartesianTrajectorySegment::SplineState;

CartesianTrajectorySegment::CartesianTrajectorySegment(const Time& start_time, const CartesianState& start_state,
                                                       const Time& end_time, const CartesianState& end_state)
  : QuinticSplineSegment(start_time, convert(start_state), end_time, convert(end_state)){};

void CartesianTrajectorySegment::sample(const Time& time, CartesianState& state) const
{
  // Sample from the underlying spline segment.
  SplineState s(7);
  QuinticSplineSegment::sample(time, s);

  if (time < this->startTime() || time > this->endTime())
  {
    state.p = Eigen::Vector3d(s.position[0], s.position[1], s.position[2]);
    state.q = Eigen::Quaterniond(s.position[3], s.position[4], s.position[5], s.position[6]).normalized();

    state.v = Eigen::Vector3d::Zero();
    state.w = Eigen::Vector3d::Zero();
    state.v_dot = Eigen::Vector3d::Zero();
    state.w_dot = Eigen::Vector3d::Zero();
  }
  else
  {
    state = convert(s);
  }
}

SplineState convert(const CartesianState& state)
{
  SplineState spline_state;

  // Note: The pre-multiplication of velocity and acceleration terms with
  // `state.q.inverse()` transforms them into the body-local reference frame.
  // This is required for computing quaternion-based velocities and
  // accelerations below.

  // Convenience method
  auto fill = [](auto& vec, const auto& first, const auto& second) {
    vec.push_back(first.x());
    vec.push_back(first.y());
    vec.push_back(first.z());

    vec.push_back(second.w());
    vec.push_back(second.x());
    vec.push_back(second.y());
    vec.push_back(second.z());
  };

  // Spline positions
  fill(spline_state.position, state.p, state.q);

  // Spline velocities
  if (std::isnan(state.v.x()) || std::isnan(state.v.y()) || std::isnan(state.v.z()) || std::isnan(state.w.x()) ||
      std::isnan(state.w.y()) || std::isnan(state.w.z()))
  {
    return spline_state;  // with uninitialized velocity/acceleration data
  }
  Eigen::Quaterniond q_dot;
  Eigen::Vector3d tmp = state.q.inverse() * state.w;
  Eigen::Quaterniond omega(0, tmp.x(), tmp.y(), tmp.z());
  q_dot.coeffs() = 0.5 * (omega * state.q).coeffs();

  fill(spline_state.velocity, state.q.inverse() * state.v, q_dot);

  // Spline accelerations
  if (std::isnan(state.v_dot.x()) || std::isnan(state.v_dot.y()) || std::isnan(state.v_dot.z()) ||
      std::isnan(state.w_dot.x()) || std::isnan(state.w_dot.y()) || std::isnan(state.w_dot.z()))
  {
    return spline_state;  // with uninitialized acceleration data
  }
  Eigen::Quaterniond q_ddot;
  tmp = state.q.inverse() * state.w_dot;
  Eigen::Quaterniond omega_dot(0, tmp.x(), tmp.y(), tmp.z());
  q_ddot.coeffs() = 0.5 * (omega_dot * state.q).coeffs() + 0.5 * (omega * q_dot).coeffs();

  fill(spline_state.acceleration, state.q.inverse() * state.v_dot, q_ddot);

  return spline_state;
};

CartesianState convert(const SplineState& s)
{
  CartesianState state;

  // Cartesian positions
  if (s.position.empty())
  {
    return state;  // with positions/velocities/accelerations zero initialized
  }
  state.p = Eigen::Vector3d(s.position[0], s.position[1], s.position[2]);
  state.q = Eigen::Quaterniond(s.position[3], s.position[4], s.position[5], s.position[6]).normalized();

  // Cartesian velocities
  if (s.velocity.empty())
  {
    return state;  // with velocities/accelerations zero initialized
  }
  Eigen::Quaterniond q_dot(s.velocity[3], s.velocity[4], s.velocity[5], s.velocity[6]);

  Eigen::Quaterniond omega;
  omega.coeffs() = 2.0 * (q_dot * state.q.inverse()).coeffs();

  state.v = Eigen::Vector3d(s.velocity[0], s.velocity[1], s.velocity[2]);
  state.w = Eigen::Vector3d(omega.x(), omega.y(), omega.z());

  // Cartesian accelerations
  if (s.acceleration.empty())
  {
    return state;  // with accelerations zero initialized
  }
  Eigen::Quaterniond q_ddot(s.acceleration[3], s.acceleration[4], s.acceleration[5], s.acceleration[6]);

  Eigen::Quaterniond omega_dot;
  omega_dot.coeffs() = 2.0 * ((q_ddot * state.q.inverse()).coeffs() -
                              ((q_dot * state.q.inverse()) * (q_dot * state.q.inverse())).coeffs());

  state.v_dot = Eigen::Vector3d(s.acceleration[0], s.acceleration[1], s.acceleration[2]);
  state.w_dot = Eigen::Vector3d(omega_dot.x(), omega_dot.y(), omega_dot.z());

  // Re-transform vel and acc to the correct reference frame.
  state.v = state.q * state.v;
  state.w = state.q * state.w;
  state.v_dot = state.q * state.v_dot;
  state.w_dot = state.q * state.w_dot;

  return state;
}

std::ostream& operator<<(std::ostream& out, const CartesianTrajectorySegment::SplineState& state)
{
  out << "pos:\n";
  for (size_t i = 0; i < state.position.size(); ++i)
  {
    out << state.position[i] << '\n';
  }
  out << "vel:\n";
  for (size_t i = 0; i < state.velocity.size(); ++i)
  {
    out << state.velocity[i] << '\n';
  }
  out << "acc:\n";
  for (size_t i = 0; i < state.acceleration.size(); ++i)
  {
    out << state.acceleration[i] << '\n';
  }

  return out;
}
}  // namespace ros_controllers_cartesian


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_control/CompletionCriteria.h>
#include <mc_rbdyn/RobotLoader.h>
#include <mc_rtc/pragma.h>
#include <mc_tasks/CoMTask.h>

#include <boost/test/unit_test.hpp>

#include "utils.h"

const double dt = 0.005;

mc_rbdyn::Robots & get_robots()
{
  static std::shared_ptr<mc_rbdyn::Robots> robots_ptr = nullptr;
  if(robots_ptr)
  {
    return *robots_ptr;
  }
  configureRobotLoader();
  auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1");
  robots_ptr = mc_rbdyn::loadRobot(*rm);
  return *robots_ptr;
}

struct MockTask : public mc_tasks::CoMTask
{
  MockTask() : mc_tasks::CoMTask(get_robots(), 0) {}

  Eigen::VectorXd eval() const override
  {
    return eval_;
  }
  Eigen::VectorXd speed() const override
  {
    return speed_;
  }

  std::function<bool(const mc_tasks::MetaTask & t, std::string & out)> buildCompletionCriteria(
      double dt,
      const mc_rtc::Configuration & config) const override
  {
    if(config.has("MYCRITERIA"))
    {
      Eigen::Vector3d myCrit = config("MYCRITERIA");
      return [myCrit](const mc_tasks::MetaTask & t, std::string & out) {
        MC_RTC_diagnostic_push
        MC_RTC_diagnostic_ignored(GCC, "-Wunused-value")
        BOOST_REQUIRE_NO_THROW(dynamic_cast<const MockTask &>(t));
        MC_RTC_diagnostic_pop
        const auto & self = static_cast<const MockTask &>(t);
        const auto & eval_ = self.eval_;
        if(eval_.x() < myCrit.x() && eval_.y() < myCrit.y() && eval_.z() < myCrit.z())
        {
          out += "MYCRITERIA";
          return true;
        }
        return false;
      };
    }
    return MetaTask::buildCompletionCriteria(dt, config);
  }

  Eigen::Vector3d eval_;
  Eigen::Vector3d speed_;
};

BOOST_AUTO_TEST_CASE(TestDefault)
{
  MockTask task;
  mc_control::CompletionCriteria criteria;
  BOOST_REQUIRE(criteria.completed(task));
}

BOOST_AUTO_TEST_CASE(TestTimeout)
{
  MockTask task;
  double timeout = 5.0;
  mc_rtc::Configuration config;
  config.add("timeout", timeout);
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  unsigned int ticks = static_cast<unsigned int>(std::floor(timeout / dt));
  for(size_t i = 0; i < ticks; ++i)
  {
    task.incrementIterInSolver();
    BOOST_REQUIRE(!criteria.completed(task));
  }
  task.incrementIterInSolver();
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "timeout");
  // Reset the criteria, timeout should resume from the current iteration count
  criteria.configure(task, dt, config);
  for(size_t i = 0; i < ticks; ++i)
  {
    task.incrementIterInSolver();
    BOOST_REQUIRE(!criteria.completed(task));
  }
  task.incrementIterInSolver();
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "timeout");
}

BOOST_AUTO_TEST_CASE(TestEval)
{
  MockTask task;
  double norm = 1e-3;
  mc_rtc::Configuration config;
  config.add("eval", norm);
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  task.eval_ = Eigen::Vector3d::UnitZ();
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_.z() = norm; // task.eval().norm() == 1e-3 == norm
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "eval");
}

BOOST_AUTO_TEST_CASE(TestSpeed)
{
  MockTask task;
  double norm = 1e-3;
  mc_rtc::Configuration config;
  config.add("speed", norm);
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  task.speed_ = Eigen::Vector3d::UnitZ();
  BOOST_REQUIRE(!criteria.completed(task));
  task.speed_.z() = norm; // task.speed().norm() == 1e-3 == norm
  BOOST_REQUIRE(!criteria.completed(task));
  task.speed_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "speed");
}

BOOST_AUTO_TEST_CASE(TestEvalAndSpeed)
{
  MockTask task;
  double norm = 1e-3;
  mc_rtc::Configuration config;
  auto AND = config.array("AND", 2);
  AND.push([norm]() {
    mc_rtc::Configuration c;
    c.add("eval", norm);
    return c;
  }());
  AND.push([norm]() {
    mc_rtc::Configuration c;
    c.add("speed", norm);
    return c;
  }());
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  task.eval_ = Eigen::Vector3d::UnitZ();
  task.speed_ = Eigen::Vector3d::UnitZ();
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_.z() = norm * 1e-1;
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_ = Eigen::Vector3d::UnitZ();
  task.speed_.z() = norm * 1e-1;
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "eval AND speed");
}

BOOST_AUTO_TEST_CASE(TestEvalOrSpeed)
{
  MockTask task;
  double norm = 1e-3;
  mc_rtc::Configuration config;
  auto OR = config.array("OR", 2);
  OR.push([norm]() {
    mc_rtc::Configuration c;
    c.add("eval", norm);
    return c;
  }());
  OR.push([norm]() {
    mc_rtc::Configuration c;
    c.add("speed", norm);
    return c;
  }());
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  task.eval_ = Eigen::Vector3d::UnitZ();
  task.speed_ = Eigen::Vector3d::UnitZ();
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "eval");
  task.eval_ = Eigen::Vector3d::UnitZ();
  task.speed_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "speed");
  task.eval_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "eval");
}

BOOST_AUTO_TEST_CASE(TestEvalAndSpeedOrTimeout)
{
  MockTask task;
  double norm = 1e-3;
  double timeout = 5.0;
  mc_rtc::Configuration config;
  auto OR = config.array("OR", 2);
  OR.push([norm]() {
    mc_rtc::Configuration c;
    auto AND = c.array("AND", 2);
    AND.push([norm]() {
      mc_rtc::Configuration c;
      c.add("eval", norm);
      return c;
    }());
    AND.push([norm]() {
      mc_rtc::Configuration c;
      c.add("speed", norm);
      return c;
    }());
    return c;
  }());
  OR.push([timeout]() {
    mc_rtc::Configuration c;
    c.add("timeout", timeout);
    return c;
  }());
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  // criteria <=> (eval().norm() < norm && speed().norm() < 1e-3) || timeout)
  task.eval_ = Eigen::Vector3d::UnitZ();
  task.speed_ = Eigen::Vector3d::UnitZ();
  task.incrementIterInSolver();
  BOOST_REQUIRE(!criteria.completed(task)); // every condition false
  task.eval_.z() = norm * 1e-1;
  task.incrementIterInSolver();
  BOOST_REQUIRE(!criteria.completed(task)); // eval true, speed false, timeout false
  task.eval_.z() = 1.0;
  task.speed_.z() = norm * 1e-1;
  task.incrementIterInSolver();
  BOOST_REQUIRE(!criteria.completed(task)); // eval false, speed true, timeout false
  task.eval_.z() = norm * 1e-1;
  BOOST_REQUIRE(criteria.completed(task)); // eval true, speed true, timeout false
  BOOST_REQUIRE(criteria.output() == "eval AND speed");
  task.eval_.z() = 1.0; // from now, eval always false, speed always true
  unsigned int ticks = static_cast<unsigned int>(std::floor(timeout / dt));
  for(size_t i = 3; i < ticks; ++i)
  {
    task.incrementIterInSolver();
    BOOST_REQUIRE(!criteria.completed(task));
  }
  task.incrementIterInSolver();
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "timeout");
}

BOOST_AUTO_TEST_CASE(TestCustomCriteria)
{
  MockTask task;
  Eigen::Vector3d myCrit{1., 2., 3.};
  mc_rtc::Configuration config;
  config.add("MYCRITERIA", myCrit);
  mc_control::CompletionCriteria criteria;
  criteria.configure(task, dt, config);
  task.eval_ = Eigen::Vector3d::UnitZ() * 4.;
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_ = myCrit;
  BOOST_REQUIRE(!criteria.completed(task));
  task.eval_ = myCrit - Eigen::Vector3d::Ones();
  BOOST_REQUIRE(criteria.completed(task));
  BOOST_REQUIRE(criteria.output() == "MYCRITERIA");
}