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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014-2021.
// Modifications copyright (c) 2014-2021 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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_CONVEX_HULL_INTERFACE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_INTERFACE_HPP

#include <array>

#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>
#include <boost/geometry/algorithms/detail/convex_hull/graham_andrew.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/for_each_range.hpp>
#include <boost/geometry/algorithms/detail/select_geometry_type.hpp>
#include <boost/geometry/algorithms/detail/visit.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/geometry_types.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/visit.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp> // For backward compatibility
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/geometries/ring.hpp>

#include <boost/geometry/strategies/convex_hull/cartesian.hpp>
#include <boost/geometry/strategies/convex_hull/geographic.hpp>
#include <boost/geometry/strategies/convex_hull/spherical.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/range.hpp>
#include <boost/geometry/util/sequence.hpp>
#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

// TODO: This file is named interface.hpp but the code below is not the interface.
//       It's the implementation of the algorithm.

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace convex_hull
{

// Abstraction representing ranges/rings of a geometry
template <typename Geometry>
struct input_geometry_proxy
{
    input_geometry_proxy(Geometry const& geometry)
        : m_geometry(geometry)
    {}

    template <typename UnaryFunction>
    inline void for_each_range(UnaryFunction fun) const
    {
        geometry::detail::for_each_range(m_geometry, fun);
    }

    Geometry const& m_geometry;
};

// Abstraction representing ranges/rings of subgeometries of geometry collection
// with boxes converted to rings
template <typename Geometry, typename BoxRings>
struct input_geometry_collection_proxy
{
    input_geometry_collection_proxy(Geometry const& geometry, BoxRings const& box_rings)
        : m_geometry(geometry)
        , m_box_rings(box_rings)
    {}

    template <typename UnaryFunction>
    inline void for_each_range(UnaryFunction fun) const
    {
        detail::visit_breadth_first([&](auto const& g)
        {
            input_geometry_collection_proxy::call_for_non_boxes(g, fun);
            return true;
        }, m_geometry);

        for (auto const& r : m_box_rings)
        {
            geometry::detail::for_each_range(r, fun);
        }
    }

private:
    template <typename G, typename F, std::enable_if_t<! util::is_box<G>::value, int> = 0>
    static inline void call_for_non_boxes(G const& g, F & f)
    {
        geometry::detail::for_each_range(g, f);
    }
    template <typename G, typename F, std::enable_if_t<util::is_box<G>::value, int> = 0>
    static inline void call_for_non_boxes(G const&, F &)
    {}

    Geometry const& m_geometry;
    BoxRings const& m_box_rings;
};

// TODO: Or just implement point_type<> for GeometryCollection
//   and enforce the same point_type used in the whole sequence in check().
template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct default_strategy
{
    using type = typename strategies::convex_hull::services::default_strategy
        <
            Geometry
        >::type;
};

template <typename Geometry>
struct default_strategy<Geometry, geometry_collection_tag>
    : default_strategy<typename detail::first_geometry_type<Geometry>::type>
{};

// Utilities for output GC and DG
template <typename G1, typename G2>
struct output_polygonal_less
{
    template <typename G>
    using priority = std::integral_constant
        <
            int,
            (util::is_ring<G>::value ? 0 :
             util::is_polygon<G>::value ? 1 :
             util::is_multi_polygon<G>::value ? 2 : 3)
        >;

    static const bool value = priority<G1>::value < priority<G2>::value;
};

template <typename G1, typename G2>
struct output_linear_less
{
    template <typename G>
    using priority = std::integral_constant
        <
            int,
            (util::is_segment<G>::value ? 0 :
             util::is_linestring<G>::value ? 1 :
             util::is_multi_linestring<G>::value ? 2 : 3)
        >;

    static const bool value = priority<G1>::value < priority<G2>::value;
};

template <typename G1, typename G2>
struct output_pointlike_less
{
    template <typename G>
    using priority = std::integral_constant
        <
            int,
            (util::is_point<G>::value ? 0 :
             util::is_multi_point<G>::value ? 1 : 2)
        >;

    static const bool value = priority<G1>::value < priority<G2>::value;
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct convex_hull
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        detail::convex_hull::input_geometry_proxy<Geometry> in_proxy(geometry);
        detail::convex_hull::graham_andrew
            <
                typename point_type<Geometry>::type
            >::apply(in_proxy, out, strategy);
    }
};

// A hull for boxes is trivial. Any strategy is (currently) skipped.
// TODO: This is not correct in spherical and geographic CS.
template <typename Box>
struct convex_hull<Box, box_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Box const& box,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        static bool const Close
            = geometry::closure<OutputGeometry>::value == closed;
        static bool const Reverse
            = geometry::point_order<OutputGeometry>::value == counterclockwise;

        std::array<typename point_type<OutputGeometry>::type, 4> arr;
        // TODO: This assigns only 2d cooridnates!
        //       And it is also used in box_view<>!
        geometry::detail::assign_box_corners_oriented<Reverse>(box, arr);

        std::move(arr.begin(), arr.end(), range::back_inserter(out));
        if (BOOST_GEOMETRY_CONDITION(Close))
        {
            range::push_back(out, range::front(out));
        }
    }
};

template <typename GeometryCollection>
struct convex_hull<GeometryCollection, geometry_collection_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(GeometryCollection const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        // Assuming that single point_type is used by the GeometryCollection
        using subgeometry_type = typename detail::first_geometry_type<GeometryCollection>::type;
        using point_type = typename geometry::point_type<subgeometry_type>::type;
        using ring_type = model::ring<point_type, true, false>;

        // Calculate box rings once
        std::vector<ring_type> box_rings;
        detail::visit_breadth_first([&](auto const& g)
        {
            convex_hull::add_ring_for_box(box_rings, g, strategy);
            return true;
        }, geometry);

        detail::convex_hull::input_geometry_collection_proxy
            <
                GeometryCollection, std::vector<ring_type>
            > in_proxy(geometry, box_rings);

        detail::convex_hull::graham_andrew
            <
                point_type
            >::apply(in_proxy, out, strategy);
    }

private:
    template
    <
        typename Ring, typename SubGeometry, typename Strategy,
        std::enable_if_t<util::is_box<SubGeometry>::value, int> = 0
    >
    static inline void add_ring_for_box(std::vector<Ring> & rings, SubGeometry const& box,
                                        Strategy const& strategy)
    {
        Ring ring;
        convex_hull<SubGeometry>::apply(box, ring, strategy);
        rings.push_back(std::move(ring));
    }
    template
    <
        typename Ring, typename SubGeometry, typename Strategy,
        std::enable_if_t<! util::is_box<SubGeometry>::value, int> = 0
    >
    static inline void add_ring_for_box(std::vector<Ring> & , SubGeometry const& ,
                                        Strategy const& )
    {}
};

template <typename OutputGeometry, typename Tag = typename tag<OutputGeometry>::type>
struct convex_hull_out
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE("This OutputGeometry is not supported.", OutputGeometry, Tag);
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, ring_tag>
{
    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        dispatch::convex_hull<Geometry>::apply(geometry, out, strategies);
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, polygon_tag>
{
    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        auto&& ring = exterior_ring(out);
        dispatch::convex_hull<Geometry>::apply(geometry, ring, strategies);
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, multi_polygon_tag>
{
    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        typename boost::range_value<OutputGeometry>::type polygon;
        auto&& ring = exterior_ring(polygon);
        dispatch::convex_hull<Geometry>::apply(geometry, ring, strategies);
        // Empty input is checked so the output shouldn't be empty
        range::push_back(out, std::move(polygon));
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, geometry_collection_tag>
{
    using polygonal_t = typename util::sequence_min_element
        <
            typename traits::geometry_types<OutputGeometry>::type,
            detail::convex_hull::output_polygonal_less
        >::type;
    using linear_t = typename util::sequence_min_element
        <
            typename traits::geometry_types<OutputGeometry>::type,
            detail::convex_hull::output_linear_less
        >::type;
    using pointlike_t = typename util::sequence_min_element
        <
            typename traits::geometry_types<OutputGeometry>::type,
            detail::convex_hull::output_pointlike_less
        >::type;

    // select_element may define different kind of geometry than the one that is desired
    BOOST_GEOMETRY_STATIC_ASSERT(util::is_polygonal<polygonal_t>::value,
        "It must be possible to store polygonal geometry in OutputGeometry.", polygonal_t);
    BOOST_GEOMETRY_STATIC_ASSERT(util::is_linear<linear_t>::value,
        "It must be possible to store linear geometry in OutputGeometry.", linear_t);
    BOOST_GEOMETRY_STATIC_ASSERT(util::is_pointlike<pointlike_t>::value,
        "It must be possible to store pointlike geometry in OutputGeometry.", pointlike_t);

    template <typename Geometry, typename Strategies>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        polygonal_t polygonal;
        convex_hull_out<polygonal_t>::apply(geometry, polygonal, strategies);
        // Empty input is checked so the output shouldn't be empty
        auto&& out_ring = ring(polygonal);

        if (boost::size(out_ring) == detail::minimum_ring_size<polygonal_t>::value)
        {
            using detail::equals::equals_point_point;
            if (equals_point_point(range::front(out_ring), range::at(out_ring, 1), strategies))
            {
                pointlike_t pointlike;
                move_to_pointlike(out_ring, pointlike);
                move_to_out(pointlike, out);
                return;
            }
            if (equals_point_point(range::front(out_ring), range::at(out_ring, 2), strategies))
            {
                linear_t linear;
                move_to_linear(out_ring, linear);
                move_to_out(linear, out);
                return;
            }
        }

        move_to_out(polygonal, out);
    }

private:
    template <typename Polygonal, util::enable_if_ring_t<Polygonal, int> = 0>
    static decltype(auto) ring(Polygonal const& polygonal)
    {
        return polygonal;
    }
    template <typename Polygonal, util::enable_if_polygon_t<Polygonal, int> = 0>
    static decltype(auto) ring(Polygonal const& polygonal)
    {
        return exterior_ring(polygonal);
    }
    template <typename Polygonal, util::enable_if_multi_polygon_t<Polygonal, int> = 0>
    static decltype(auto) ring(Polygonal const& polygonal)
    {
        return exterior_ring(range::front(polygonal));
    }

    template <typename Range, typename Linear, util::enable_if_segment_t<Linear, int> = 0>
    static void move_to_linear(Range & out_range, Linear & seg)
    {
        detail::assign_point_to_index<0>(range::front(out_range), seg);
        detail::assign_point_to_index<1>(range::at(out_range, 1), seg);
    }
    template <typename Range, typename Linear, util::enable_if_linestring_t<Linear, int> = 0>
    static void move_to_linear(Range & out_range, Linear & ls)
    {
        std::move(boost::begin(out_range), boost::begin(out_range) + 2, range::back_inserter(ls));
    }
    template <typename Range, typename Linear, util::enable_if_multi_linestring_t<Linear, int> = 0>
    static void move_to_linear(Range & out_range, Linear & mls)
    {
        typename boost::range_value<Linear>::type ls;
        std::move(boost::begin(out_range), boost::begin(out_range) + 2, range::back_inserter(ls));
        range::push_back(mls, std::move(ls));
    }

    template <typename Range, typename PointLike, util::enable_if_point_t<PointLike, int> = 0>
    static void move_to_pointlike(Range & out_range, PointLike & pt)
    {
        pt = range::front(out_range);
    }
    template <typename Range, typename PointLike, util::enable_if_multi_point_t<PointLike, int> = 0>
    static void move_to_pointlike(Range & out_range, PointLike & mpt)
    {
        range::push_back(mpt, std::move(range::front(out_range)));
    }

    template
    <
        typename Geometry, typename OutputGeometry_,
        util::enable_if_geometry_collection_t<OutputGeometry_, int> = 0
    >
    static void move_to_out(Geometry & g, OutputGeometry_ & out)
    {
        range::emplace_back(out, std::move(g));
    }
    template
    <
        typename Geometry, typename OutputGeometry_,
        util::enable_if_dynamic_geometry_t<OutputGeometry_, int> = 0
    >
    static void move_to_out(Geometry & g, OutputGeometry_ & out)
    {
        out = std::move(g);
    }
};

template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, dynamic_geometry_tag>
    : convex_hull_out<OutputGeometry, geometry_collection_tag>
{};

// For backward compatibility
template <typename OutputGeometry>
struct convex_hull_out<OutputGeometry, linestring_tag>
    : convex_hull_out<OutputGeometry, ring_tag>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

template <typename Strategies>
struct convex_hull
{
    template <typename Geometry, typename OutputGeometry>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategies const& strategies)
    {
        dispatch::convex_hull_out<OutputGeometry>::apply(geometry, out, strategies);
    }
};

template <>
struct convex_hull<default_strategy>
{
    template <typename Geometry, typename OutputGeometry>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             default_strategy const&)
    {
        using strategy_type = typename detail::convex_hull::default_strategy
            <
                Geometry
            >::type;

        dispatch::convex_hull_out<OutputGeometry>::apply(geometry, out, strategy_type());
    }
};

} // namespace resolve_strategy

namespace resolve_dynamic {

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct convex_hull
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        concepts::check_concepts_and_equal_dimensions<
            const Geometry,
            OutputGeometry
        >();

        resolve_strategy::convex_hull<Strategy>::apply(geometry, out, strategy);
    }
};

template <typename Geometry>
struct convex_hull<Geometry, dynamic_geometry_tag>
{
    template <typename OutputGeometry, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             OutputGeometry& out,
                             Strategy const& strategy)
    {
        traits::visit<Geometry>::apply([&](auto const& g)
        {
            convex_hull<util::remove_cref_t<decltype(g)>>::apply(g, out, strategy);
        }, geometry);
    }
};

} // namespace resolve_dynamic

/*!
\brief \brief_calc{convex hull} \brief_strategy
\ingroup convex_hull
\details \details_calc{convex_hull,convex hull} \brief_strategy.
\tparam Geometry the input geometry type
\tparam OutputGeometry the output geometry type
\tparam Strategy the strategy type
\param geometry \param_geometry,  input geometry
\param out \param_geometry \param_set{convex hull}
\param strategy \param_strategy{area}

\qbk{distinguish,with strategy}

\qbk{[include reference/algorithms/convex_hull.qbk]}
 */
template<typename Geometry, typename OutputGeometry, typename Strategy>
inline void convex_hull(Geometry const& geometry, OutputGeometry& out, Strategy const& strategy)
{
    if (geometry::is_empty(geometry))
    {
        // Leave output empty
        return;
    }

    resolve_dynamic::convex_hull<Geometry>::apply(geometry, out, strategy);
}

/*!
\brief \brief_calc{convex hull}
\ingroup convex_hull
\details \details_calc{convex_hull,convex hull}.
\tparam Geometry the input geometry type
\tparam OutputGeometry the output geometry type
\param geometry \param_geometry,  input geometry
\param hull \param_geometry \param_set{convex hull}

\qbk{[include reference/algorithms/convex_hull.qbk]}
 */
template<typename Geometry, typename OutputGeometry>
inline void convex_hull(Geometry const& geometry, OutputGeometry& hull)
{
    geometry::convex_hull(geometry, hull, default_strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_CONVEX_HULL_INTERFACE_HPP


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Daichi Yoshikawa
 *  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 Daichi Yoshikawa 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.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#ifndef __AHL_ROBOT_SAMPLES_RED_ARM_PARAM_HPP
#define __AHL_ROBOT_SAMPLES_RED_ARM_PARAM_HPP

#include <memory>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include "ahl_robot_samples/RedArmParamConfig.h"

namespace ahl_sample
{

  class RedArmParam
  {
  public:
    explicit RedArmParam()
    {
      x = Eigen::Vector3d::Zero();
      R = Eigen::Matrix3d::Identity();
      q = Eigen::VectorXd::Zero(7);

      ros::NodeHandle local_nh("~/red_arm/");
      f_ = boost::bind(&RedArmParam::update, this, _1, _2);
      server_ = std::make_shared<RedArmParamConfigServer>(local_nh);
      server_->setCallback(f_);
    }

    bool show_target = true;
    bool sin_x = false;
    bool sin_y = false;
    bool sin_z = false;

    Eigen::Vector3d x;
    Eigen::Matrix3d R;
    Eigen::VectorXd q;

  private:
    void update(ahl_robot_samples::RedArmParamConfig& config, uint32_t level)
    {
      show_target = config.show_target;

      sin_x = config.sin_x;
      sin_y = config.sin_y;
      sin_z = config.sin_z;

      x << config.x_arm, config.y_arm, config.z_arm;

      R = Eigen::AngleAxisd(config.roll_arm,  Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(config.pitch_arm, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(config.yaw_arm,   Eigen::Vector3d::UnitZ());

      q << config.q1, config.q2, config.q3, config.q4, config.q5, config.q6, config.q7;
    }

    using RedArmParamConfigServer = dynamic_reconfigure::Server<ahl_robot_samples::RedArmParamConfig>;
    using RedArmParamConfigServerPtr = std::shared_ptr<RedArmParamConfigServer>;

    RedArmParamConfigServerPtr server_;
    dynamic_reconfigure::Server<ahl_robot_samples::RedArmParamConfig>::CallbackType f_;
  };

  using RedArmParamPtr = std::shared_ptr<RedArmParam>;

} // namespace ahl_sample

#endif // __AHL_ROBOT_SAMPLES_RED_ARM_PARAM_HPP


#include <iostream>
#include <iomanip>
#include <fstream>
#include <iterator>
#include <cmath>
#include <regex>
#include <numeric>

#include <boost/program_options.hpp>
#include <boost/log/sources/logger.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/map.hpp>
#include <boost/mpi.hpp>
#include <boost/core/demangle.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/combine.hpp>

#include <benchmark.hpp>
#include <logging.hpp>

#include <blas.hpp>
#include <matrix_serialization.hpp>

namespace po = boost::program_options;

auto init_options() -> po::options_description
{
	po::options_description description;
	description.add_options()
		( "help", "Produce the help message." )
		( "type"
		, po::value<std::string>()->required()
		, "Type of an element of the epsilon matrix. It may be "
		  "either cfloat or cdouble." )
		( "epsilon"
		, po::value<std::string>()->required()
		, "File to where epsilon matrix was saved to." )
		( "positions"
		, po::value<std::string>()->required()
		, "File to where atomic site positions were saved to." )
		( "q"
		, po::value<std::string>()->required()
		, "List of |q|s separated by commas. MIND YOU: "
		  "no spaces!" )
		( "direction"
		, po::value<std::string>()->required()
		, "Direction of q as (x,y,z). It is automatically"
		  "normalized." );
	return description;
}

auto element_type(std::string input) -> std::type_index
{
	using namespace std::string_literals;
	static std::unordered_map<std::string, std::type_index> const types =
		{ { "cfloat"s,  std::type_index(typeid(std::complex<float>))  }
		, { "cdouble"s, std::type_index(typeid(std::complex<double>)) }
		};

	boost::to_lower(input);
	try {
		return types.at(input);
	} catch(std::out_of_range & e) {
		std::cerr << "Invalid element type `" + input + "`!\n";
		throw;
	}
}

template <class _R>
auto parse_direction(std::string str) -> std::array<_R, 3>
{
	using namespace boost;
	using namespace boost::algorithm;

	auto const normalize = [](_R const x, _R const y, _R const z)
		-> std::array<_R, 3> {
		auto const _abs = _R{1.0} / std::sqrt(std::norm(x) + std::norm(y) + std::norm(z));
		return {_abs * x, _abs * y, _abs * z};
	};

	std::regex vector_re{"\\((.*),(.*),(.*)\\)"};
	std::smatch results;

	trim(str);
	if (std::regex_match(str, results, vector_re)) {
		assert(results.size() == 4);
		return normalize( lexical_cast<_R>(trim_copy(results[1].str()))
		                , lexical_cast<_R>(trim_copy(results[2].str()))
			            , lexical_cast<_R>(trim_copy(results[3].str())) );
	}
	throw std::invalid_argument{ "Could not convert '" + str
	                           + "' to a 3D vector."};
}

template <class _R>
auto parse_qs(std::string const& str) -> std::vector<_R>
{
	using namespace boost;
	using namespace boost::adaptors;
	using namespace boost::algorithm;

	auto const parse_number = [](auto const& s) {
		return lexical_cast<_R>(trim_copy(s));
	};

	std::vector<std::string> tokens;
	split(tokens, str, [](auto const ch) { return ch == ','; });

	std::vector<_R> qs;
	qs.reserve(tokens.size());
	copy( tokens | transformed(std::cref(parse_number))
	    , std::back_inserter(qs) );
	return qs;
}

template<class _Help, class _Run>
auto process_command_line( int argc, char** argv
                         , _Help&& help
						 , _Run&& run ) -> void
{
	auto const description = init_options();
	po::variables_map vm;

	po::store( po::command_line_parser(argc, argv)
	              .options(description)
	              .run()
	         , vm );

	if (vm.count("help")) {
		help(description);
		return;
	}

	po::notify(vm);
	run(vm);
}

template<class _T>
auto load_matrix(std::string const& file_name) -> tcm::Matrix<_T>
{
	std::ifstream in_stream{file_name};
	if (not in_stream)
		throw std::runtime_error{"Failed to open `" + file_name + "`."};
	boost::archive::binary_iarchive in_archive{in_stream};

	tcm::Matrix<_T> A;
	in_archive >> A;
	return A;
}

template<class _T>
auto read_positions(std::string const& file_name)
	-> std::vector<std::array<_T, 3>>
{
	std::ifstream in_stream{file_name};
	if (not in_stream)
		throw std::runtime_error{"Failed to open `" + file_name + "`."};

	std::vector<std::array<_T, 3>> positions;
	auto i = std::istream_iterator<_T>{in_stream};
	while(i != std::istream_iterator<_T>{}) {
		const auto x = *i++;
		const auto y = *i++;
		const auto z = *i++;
		positions.push_back({x, y, z});
	}

	return positions;
}

template < std::size_t _Dim = 3
         , class _R1 = double
         , class _R2 = double
         , class _R3 = std::common_type_t<_R1, _R2>
         >
auto make_momentum_eigenvector( std::array<_R1, 3> const wavevector
                              , std::vector<std::array<_R2, 3>> const& positions
                              , _R3 const pi = _R3{M_PI} )
{
	using _R = std::common_type_t<_R1, _R2, _R3>;
	using _C = std::complex<_R>;

	auto constexpr I = _C{0, 1};
	auto const _norm =
		std::pow(_R{2} * pi, -boost::numeric_cast<_R>(_Dim) / _R{2});
	auto const _dot = [] (auto const& x, auto const& y) noexcept {
		return x[0]*y[0] + x[1]*y[1] + x[2]*y[2];
	};

	tcm::Matrix<_C> q{positions.size(), 1};
	std::transform( std::begin(positions), std::end(positions)
	              , q.data()
				  , [ _norm
	                , &wavevector
	                , _dot = std::cref(_dot)
	                ] (auto const& r) {
	                    return _norm * std::exp<_R>(I * _dot(wavevector, r));
	                } );
	return q;
}

template <class _R, class _Matrix>
auto loss_function( std::array<_R, 3> const& direction
                  , std::vector<_R> const& qs
                  , _Matrix const& epsilon
                  , std::vector<std::array<_R, 3>> const& positions )
{
	using namespace boost;
	using namespace boost::adaptors;

	assert(tcm::is_square(epsilon));
	assert(epsilon.height() == positions.size());
	using _C = typename _Matrix::value_type;
	static_assert( std::is_same<typename _C::value_type, _R>::value
	             , "Types mismatch." );

	auto const make_wavevector = [&direction](auto const q) noexcept
		-> std::array<_R, 3> {
		return {q * direction[0], q * direction[1], q * direction[2]};
	};

	auto const make_epsilon_q = [&epsilon,&positions](auto const& wavevector) {
		auto const q_state = make_momentum_eigenvector(wavevector, positions);
		tcm::Matrix<_C> _temp{epsilon.height(), 1};
		tcm::blas::gemv( tcm::blas::Operator::H
		               , _C{1}, epsilon, q_state
		               , _C{0}, _temp );
		return tcm::blas::dot(_temp, q_state);
	};

	std::vector<_C> epsilon_q;
	epsilon_q.reserve(qs.size());
	copy( qs | transformed(std::cref(make_wavevector))
	         | transformed(std::cref(make_epsilon_q))
	    , std::back_inserter(epsilon_q) );
	return epsilon_q;
}

template <class _C>
auto run(boost::program_options::variables_map const& vm) -> void
{
	using namespace boost;
	using _R = typename _C::value_type;

	auto const direction =
		parse_direction<_R>(vm["direction"].as<std::string>());
	auto const qs =
		parse_qs<_R>(vm["q"].as<std::string>());
	auto const positions =
		read_positions<_R>(vm["positions"].as<std::string>());
	auto const epsilon =
		load_matrix<_C>(vm["epsilon"].as<std::string>());

	auto const epsilon_q = loss_function( direction, qs
	                                    , epsilon
	                                    , positions );

	auto const print = [](auto const& t) {
		std::cout << tuples::get<0>(t) << '\t'
		          << std::real(tuples::get<1>(t)) << '\t'
		          << std::imag(tuples::get<1>(t)) << '\n';
	};

	std::cout << std::scientific << std::setprecision(15);
	for_each(combine(qs, epsilon_q), std::cref(print));
}

auto dispatch(boost::program_options::variables_map const& vm) -> void
{
	auto const type = element_type(vm["type"].as<std::string>());
	if (type == typeid(std::complex<float>)) {
		run<std::complex<float>>(vm);
	}
	else if (type == typeid(std::complex<double>)) {
		run<std::complex<double>>(vm);
	}
	else throw std::invalid_argument{"Wrong type."};
}

int main(int argc, char** argv)
{
	process_command_line
		( argc, argv
		, [](auto desc) { std::cout << desc << '\n'; }
		, &dispatch
		);
	return EXIT_SUCCESS;
}


#include <cslibs_kdl/dynamic_model.h>
#include <cslibs_kdl/kdl_conversion.h>
#include <random>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainidsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <tf_conversions/tf_kdl.h>
#include <ros/ros.h>
#include <Eigen/SVD>

using namespace cslibs_kdl;

DynamicModel::DynamicModel():
    gravity_(0,0,9.81)//, solverID_(chain_,gravity_)
{
}

DynamicModel::DynamicModel(const std::string &robot_model, const std::string& chain_root, const std::string& chain_tip):
    KinematicModel(robot_model, chain_root, chain_tip),
    gravity_(0,0,9.81)
{
    initialize();
}

bool DynamicModel::setTreeParam(const std::string &robot_model)
{

    robot_model_.initString(robot_model);
    urdf_param_ = robot_model;
    return initialize();
}

//bool DynamicModel::setTreeFile(const std::string &robot_model)
//{
//    robot_model_.initFile(robot_model);
//    urdf_param_ = robot_model;
//    return initialize();
//}

bool DynamicModel::initialize()
{

    KinematicModel::initialize();

    if (!kdl_parser::treeFromUrdfModel(robot_model_, tree_)){
        ROS_ERROR("Failed to construct kdl tree");
        return false;
    }
    if(tree_.getChain(root_,tip_,chain_)){
        chainFile_ = chain_;
        // inverse dynamics solver
        solverID_.reset(new KDL::ChainIdSolver_RNE(chain_,gravity_));
    }
    else{
        ROS_ERROR_STREAM("Chain extraction is not possible. Solver is not probably initalized. robot_model: " << robot_model_.name_
                         << "; Root link: "<< root_ << "; tip: " << tip_);
        return false;
    }
    return true;
}

int DynamicModel::getTorques(const std::vector<double> &q, const std::vector<double> &q_Dot, const std::vector<double> &q_DotDot,
                                  std::vector<double> &torques, const std::vector<cslibs_kdl_data::Wrench> &wrenches_ext)
{
    std::size_t njoints = chain_.getNrOfJoints();
    if(q.size() != q_Dot.size() || q.size() != q_DotDot.size() || q.size() < njoints){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is " << q.size() << ". While q_Dot has dimension " << q_Dot.size() << " and q_DotDot "
                         << q_DotDot.size()<<". The KDL chain contains " <<  njoints << "  joints." << std::endl);
        return KDL::SolverI::E_UNDEFINED;
    }

    KDL::JntArray qkdl(njoints);
    KDL::JntArray qkdl_Dot(njoints);
    KDL::JntArray qkdl_DotDot(njoints);
    KDL::JntArray torques_kdl(chain_.getNrOfJoints());
    KDL::Wrenches wrenches;
    wrenches.resize(chain_.getNrOfSegments());
    for(std::size_t i = 0; i < njoints; ++i) {
        qkdl(i) = q[i];
        qkdl_Dot(i) = q_Dot[i];
        qkdl_DotDot(i) = q_DotDot[i];
    }
    if(wrenches_ext.size() == 0){
        for(std::size_t i = 0; i < chain_.getNrOfSegments(); ++i)
        {
            wrenches[i] = KDL::Wrench::Zero();
        }
    }
    else if(wrenches_ext.size() != chain_.getNrOfSegments()){
        ROS_ERROR_STREAM("Dimension mismatch: " << wrenches_ext.size() <<" wrenches are given as input, but kdl expects " << chain_.getNrOfSegments() << "wenches.");
        return KDL::SolverI::E_UNDEFINED;
    }
    else{
        for(std::size_t i = 0; i < chain_.getNrOfSegments(); ++i){
            wrenches[i] = convert(wrenches_ext[i]);
        }
    }
    int e_code = -1;
    e_code = solverID_->CartToJnt(qkdl,qkdl_Dot,qkdl_DotDot,wrenches,torques_kdl);

    cslibs_kdl::convert(torques_kdl,torques);

    return e_code;
}

int DynamicModel::getTorques(const std::vector<double> &q,
                                  const std::vector<double> &q_Dot,
                                  const std::vector<double> &q_DotDot,
                                  std::vector<double> &torques,
                                  const std::vector<KDL::Wrench> &wrenches_ext)
{
    std::size_t njoints = chain_.getNrOfJoints();
    if(q.size() != q_Dot.size() || q.size() != q_DotDot.size() || q.size() < njoints){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is " << q.size() << ". While q_Dot has dimension " << q_Dot.size() << " and q_DotDot "
                         << q_DotDot.size()<<". The KDL chain contains " <<  chain_.getNrOfJoints() << "  joints." << std::endl);
        return KDL::SolverI::E_UNDEFINED;
    }

    KDL::JntArray qkdl(njoints);
    KDL::JntArray qkdl_Dot(njoints);
    KDL::JntArray qkdl_DotDot(njoints);
    KDL::JntArray torques_kdl(njoints);
    KDL::Wrenches wrenches;
    wrenches.resize(chain_.getNrOfSegments());
    for(std::size_t i = 0; i < njoints; ++i) {
        qkdl(i) = q[i];
        qkdl_Dot(i) = q_Dot[i];
        qkdl_DotDot(i) = q_DotDot[i];
    }

    int e_code = -1;

    if(wrenches_ext.size() == 0){
        for(std::size_t i = 0; i < chain_.getNrOfSegments(); ++i)
        {
            wrenches[i] = KDL::Wrench::Zero();
        }
        e_code = solverID_->CartToJnt(qkdl,qkdl_Dot,qkdl_DotDot,wrenches,torques_kdl);
    }
    else if(wrenches_ext.size() != chain_.getNrOfSegments()){
        ROS_ERROR_STREAM("Dimension mismatch: " << wrenches_ext.size() <<" wrenches are given as input, but kdl expects " << chain_.getNrOfSegments() << "wenches.");
        return KDL::SolverI::E_UNDEFINED;
    }
    else{
        e_code = solverID_->CartToJnt(qkdl,qkdl_Dot,qkdl_DotDot,wrenches_ext,torques_kdl);
    }

    cslibs_kdl::convert(torques_kdl,torques);

    return e_code;
}

int DynamicModel::getJointAcceleration(const std::vector<double> &q,
                                            const std::vector<double> &q_Dot,
                                            const std::vector<double> &torques,
                                            std::vector<double> &q_Dot_Dot)
{
    Eigen::MatrixXd H;
    Eigen::VectorXd C,g;
    int e_code = getChainDynParam(q, q_Dot, H, C, g);
    Eigen::VectorXd tau;
    convert(torques, tau);
    Eigen::VectorXd residual = tau - C - g;
    //    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV | Eigen::FullPivHouseholderQRPreconditioner);
    //    Eigen::VectorXd qDotDot = svd.solve(residual);
    Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(H);
    Eigen::VectorXd  qDotDot = qr.solve(residual);
    convert(qDotDot,q_Dot_Dot);
    return e_code;
}

void DynamicModel::setGravity(double x, double y, double z)
{
    gravity_ = KDL::Vector(x, y, z);
    // inverse dynamics solver
    solverID_.reset(new KDL::ChainIdSolver_RNE(chain_,gravity_));
}

void DynamicModel::getGravity(double &gx, double &gy, double &gz)
{
    gx = gravity_(0);
    gy = gravity_(1);
    gz = gravity_(2);
}

int DynamicModel::getAcceleration(const std::vector<double>& q,
                                       const std::vector<double>& q_Dot,
                                       const std::vector<double>& q_DotDot,
                                       std::vector<std::string>& links,
                                       std::vector<KDL::Twist > &spatial_acc )
{
    std::size_t njoints = chain_.getNrOfJoints();

    if(q.size() != q_Dot.size() || q.size() != q_DotDot.size() || q.size() < njoints){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is " << q.size() << ". While q_Dot has dimension " << q_Dot.size() << " and q_DotDot "
                         << q_DotDot.size()<<". The KDL chain contains " <<  chain_.getNrOfJoints() << "  joints." << std::endl);
        return KDL::SolverI::E_UNDEFINED;
    }

    std::vector<KDL::Frame> X(njoints);
    std::vector<KDL::Frame> Xij(njoints);
    std::vector<KDL::Twist> S(njoints);
    std::vector<KDL::Twist> v(njoints);
    std::vector<KDL::Twist> a(njoints);

    KDL::Twist ag = -KDL::Twist(gravity_, KDL::Vector::Zero());
    int j = 0;
    double ns = chain_.getNrOfSegments() ;
    //    std::cout << "ns " << ns << " nj " << chain_.getNrOfJoints() << std::endl;
    for(int i = 0; i < chain_.getNrOfJoints(); ++i) {
        double q_,qdot_,qdotdot_;
        if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
            q_=q[j];
            qdot_=q_Dot[j];
            qdotdot_=q_DotDot[j];
            ++j;
        }else
            q_=qdot_=qdotdot_=0.0;

        // Calculate segment properties: X,S,vj,cj
        X[i]=chain_.getSegment(i).pose(q_);//Remark this is the inverse of the
        // frame for transformations from
        // the parent to the current coord frame

        // Transform velocity and unit velocity to segment frame
        KDL::Twist vj = X[i].M.Inverse(chain_.getSegment(i).twist(q_,qdot_));

        KDL::Twist vparent;
        KDL::Twist aparent;
        if(i==0){
            vparent = KDL::Twist(KDL::Vector::Zero(), KDL::Vector::Zero());
            aparent = X[i].Inverse(ag);
            Xij[i] = X[i];
        }else{
            vparent = X[i].Inverse(v[i-1]);
            aparent = X[i].Inverse(a[i-1]);
            Xij[i] = Xij[i-1] * X[i];
        }
        v[i] = vparent + vj;
        a[i] = aparent + S[i]*qdotdot_ + v[i]*vj;

    }
    spatial_acc.resize(chain_.getNrOfSegments());
    links.resize(chain_.getNrOfSegments());
    for(std::size_t i = 0; i < links.size(); ++i) {
        links[i] = chain_.getSegment(i).getName();
        spatial_acc[i] = a[i];
    }
    return KDL::SolverI::E_NOERROR;
}

int DynamicModel::getAcceleration(const std::vector<double> &q, const std::vector<double> &q_Dot,
                                       const std::vector<double> &q_DotDot, std::vector<std::string> &links,
                                       std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > > &spatial_acc)
{

    std::vector<KDL::Twist> acc;
    int ec = getAcceleration(q,q_Dot, q_DotDot, links, acc);

    spatial_acc.resize(links.size());
    for(std::size_t i = 0; i < links.size(); ++i) {
        spatial_acc[i](0) = acc[i].rot(0);
        spatial_acc[i](1) = acc[i].rot(1);
        spatial_acc[i](2) = acc[i].rot(2);
        spatial_acc[i](3) = acc[i].vel(0);
        spatial_acc[i](4) = acc[i].vel(1);
        spatial_acc[i](5) = acc[i].vel(2);
    }
    return ec;

}
int DynamicModel::getChainDynParam(const std::vector<double> &q,
                                        const std::vector<double> &q_Dot,
                                        Eigen::MatrixXd &H, Eigen::VectorXd &C, Eigen::VectorXd &G)
{
    std::size_t njoints = chain_.getNrOfJoints();
    if(q.size() != q_Dot.size() || q.size() < njoints){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is " << q.size() << ". While q_Dot has dimension " << q_Dot.size()
                         <<". The KDL chain contains " <<  chain_.getNrOfJoints() << "  joints." << std::endl);
        return KDL::SolverI::E_UNDEFINED;
    }

    KDL::ChainDynParam dynparam(chain_, gravity_);

    KDL::JntArray theta;
    KDL::JntArray omega;
    KDL::JntArray coriolis(njoints);
    KDL::JntArray gravity(njoints);
    KDL::JntSpaceInertiaMatrix inertia(njoints);
    cslibs_kdl::convert(q,theta);
    cslibs_kdl::convert(q_Dot, omega);

    int res = dynparam.JntToCoriolis(theta, omega, coriolis);
    if(res == KDL::SolverI::E_NOERROR){
        res = dynparam.JntToGravity(theta, gravity);
        if(res == KDL::SolverI::E_NOERROR){
            res = dynparam.JntToMass(theta, inertia);
        }
        else{
            return res;
        }
    }
    else{
        return res;
    }

    kdlJntArray2Eigen(coriolis, C);
    kdlJntArray2Eigen(gravity, G);

    convert2Eigen(inertia, H);

    return res;

}

int DynamicModel::getChainDynInertiaAndGravity(const std::vector<double>& q,
                                                    Eigen::MatrixXd& H,
                                                    Eigen::VectorXd &G)
{
    if(q.size() != chain_.getNrOfJoints()){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is " << q.size()
                         <<". The KDL chain contains " <<  chain_.getNrOfJoints() << "  joints." << std::endl);
        return KDL::SolverI::E_UNDEFINED;
    }

    KDL::ChainDynParam dynparam(chain_, gravity_);

    KDL::JntArray theta;
    KDL::JntArray gravity(q.size());

    KDL::JntSpaceInertiaMatrix inertia(q.size());
    cslibs_kdl::convert(q,theta);

    int res = dynparam.JntToGravity(theta, gravity);
    if(res == KDL::SolverI::E_NOERROR){
        res = dynparam.JntToMass(theta, inertia);
    }
    else{
        return res;
    }

    kdlJntArray2Eigen(gravity, G);

    convert2Eigen(inertia, H);

    return res;

}

void DynamicModel::changeDynamicParams(const std::string &link, const double mass, const Eigen::Vector3d &com, const Eigen::Matrix3d& inertia)
{
    int segId = getKDLSegmentIndex(link);

    if(segId > -1)
    {
        KDL::Vector cog(com(0), com(1), com(2));
        KDL::RotationalInertia inertiaKDL(inertia(0,0), inertia(1,1), inertia(2,2),
                                          inertia(0,1), inertia(0,2), inertia(1,2));

        KDL::Chain newChain;
        for(int i = 0; i < segId; ++i)
        {
            newChain.addSegment(chain_.getSegment(i));
        }
        KDL::Segment oldSeg = chain_.getSegment(segId);

        KDL::RigidBodyInertia newInertia(mass, cog, inertiaKDL);
        KDL::Segment newSeg(oldSeg.getName(),oldSeg.getJoint(), oldSeg.getFrameToTip(), newInertia);
        newChain.addSegment(newSeg);
        for(int i = segId +1; i < chain_.getNrOfSegments(); ++i)
        {
            newChain.addSegment(chain_.getSegment(i));
        }
        chain_ = newChain;

        // inverse dynamics solver
        solverID_.reset(new KDL::ChainIdSolver_RNE(chain_,gravity_));

    }
}

void DynamicModel::changeDynamicParams(const std::string &link, const double mass, const Eigen::Vector3d &com, const Eigen::Matrix3d& inertia, double gx, double gy, double gz)
{
    gravity_ =KDL::Vector(gx, gy, gz);
    int segId = getKDLSegmentIndex(link);
    if(segId > -1)
    {
        KDL::Vector cog(com(0), com(1), com(2));
        KDL::RotationalInertia inertiaKDL(inertia(0,0), inertia(1,1), inertia(2,2),
                                          inertia(0,1), inertia(0,2), inertia(1,2));

        KDL::Chain newChain;
        for(int i = 0; i < segId; ++i)
        {
            newChain.addSegment(chain_.getSegment(i));
        }
        KDL::Segment oldSeg = chain_.getSegment(segId);

        KDL::RigidBodyInertia newInertia(mass, cog, inertiaKDL);
        KDL::Segment newSeg(oldSeg.getName(),oldSeg.getJoint(), oldSeg.getFrameToTip(), newInertia);
        newChain.addSegment(newSeg);
        for(int i = segId +1; i < chain_.getNrOfSegments(); ++i)
        {
            newChain.addSegment(chain_.getSegment(i));
        }
        chain_ = newChain;

        // inverse dynamics solver
        solverID_.reset(new KDL::ChainIdSolver_RNE(chain_,gravity_));
    }
}

void DynamicModel::changeKineticParams(const std::string &link, const Eigen::Vector3d &trans, const Eigen::Matrix3d& rotation)
{
    {
        int segId = getKDLSegmentIndex(link);
        if(segId > -1)
        {
            KDL::Vector p(trans(0), trans(1), trans(2));
            //            KDL::Rotation rot(xx,yx,zx,xy,yy,zy,xz,yz,zz);
            KDL::Rotation rot(rotation(0,0), rotation(1,0), rotation(2,0),
                              rotation(0,1), rotation(1,1), rotation(2,1),
                              rotation(0,2), rotation(1,2), rotation(2,2));

            KDL::Chain newChain;
            for(int i = 0; i < segId; ++i)
            {
                newChain.addSegment(chain_.getSegment(i));
            }
            KDL::Segment oldSeg = chain_.getSegment(segId);
            KDL::Frame matT(rot,p);
            KDL::Segment newSeg(oldSeg.getName(),oldSeg.getJoint(), matT, oldSeg.getInertia());
            newChain.addSegment(newSeg);
            for(int i = segId +1; i < chain_.getNrOfSegments(); ++i)
            {
                newChain.addSegment(chain_.getSegment(i));
            }
            chain_ = newChain;
            // inverse dynamics solver
            solverID_.reset(new KDL::ChainIdSolver_RNE(chain_,gravity_));

            // forward kinematic
            solver_fk_.reset(new KDL::ChainFkSolverPos_recursive(chain_));

            //initialize TRAC_IK solver: inverse kinematics
            solver_ik_.reset(new TRAC_IK::TRAC_IK(chain_, lower_limits_, upper_limits_));
        }
    }
}

void DynamicModel::useUrdfDynamicParams()
{
    chain_ = chainFile_;
    // inverse dynamics solver
    solverID_.reset(new KDL::ChainIdSolver_RNE(chain_,gravity_));

    // forward kinematic
    solver_fk_.reset(new KDL::ChainFkSolverPos_recursive(chain_));

    //initialize TRAC_IK solver: inverse kinematics
    solver_ik_.reset(new TRAC_IK::TRAC_IK(chain_, lower_limits_, upper_limits_));
}

double DynamicModel::getLinkMass(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        return chain_.getSegment(segmentID).getInertia().getMass();
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return 0;
    }
}
Eigen::Vector3d DynamicModel::getLinkCoM(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        KDL::Vector vec = chain_.getSegment(segmentID).getInertia().getCOG();
        Eigen::Vector3d result(vec(0), vec(1), vec(2));
        return result;
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return Eigen::Vector3d ();
    }
}

Eigen::Matrix3d DynamicModel::getLinkInertia(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        KDL::RotationalInertia mat = chain_.getSegment(segmentID).getInertia().getRotationalInertia();
        Eigen::Matrix3d result;
        result << mat.data[0], mat.data[1], mat.data[2],
                mat.data[3], mat.data[4], mat.data[5],
                mat.data[6], mat.data[7], mat.data[8];
        return result;
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return Eigen::Matrix3d();
    }
}

Eigen::Matrix3d DynamicModel::getLinkInertiaCoM(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        KDL::RotationalInertia mat = chain_.getSegment(segmentID).getInertia().getRotationalInertia();
        double mass = chain_.getSegment(segmentID).getInertia().getMass();
        KDL::Vector com = chain_.getSegment(segmentID).getInertia().getCOG();
        Eigen::Matrix3d result;

        double xx = mat.data[0] - mass *(com(1)*com(1) + com(2)*com(2));
        double yy = mat.data[4] - mass *(com(0)*com(0) + com(2)*com(2));
        double zz = mat.data[8] - mass *(com(0)*com(0) + com(1)*com(1));
        double xy = mat.data[1] + mass * com(0)*com(1);
        double xz = mat.data[2] + mass * com(0)*com(2);
        double yz = mat.data[5] + mass * com(1)*com(2);

        result << xx, xy, xz,
                xy, yy, yz,
                xz, yz, zz;
        return result;
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return Eigen::Matrix3d();
    }
}

KDL::RotationalInertia DynamicModel::getLinkInertiaCoM(const int id) const
{
    KDL::RotationalInertia mat = chain_.getSegment(id).getInertia().getRotationalInertia();
    double mass = chain_.getSegment(id).getInertia().getMass();
    KDL::Vector com = chain_.getSegment(id).getInertia().getCOG();

    double xx = mat.data[0] - mass *(com(1)*com(1) + com(2)*com(2));
    double yy = mat.data[4] - mass *(com(0)*com(0) + com(2)*com(2));
    double zz = mat.data[8] - mass *(com(0)*com(0) + com(1)*com(1));
    double xy = mat.data[1] + mass * com(0)*com(1);
    double xz = mat.data[2] + mass * com(0)*com(2);
    double yz = mat.data[5] + mass * com(1)*com(2);

    return KDL::RotationalInertia(xx, yy, zz, xy, xz, yz);
}

double DynamicModel::getURDFLinkMass(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        return chainFile_.getSegment(segmentID).getInertia().getMass();
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return 0;
    }
}
Eigen::Vector3d DynamicModel::getURDFLinkCoM(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        KDL::Vector vec = chainFile_.getSegment(segmentID).getInertia().getCOG();
        Eigen::Vector3d result(vec(0), vec(1), vec(2));
        return result;
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return Eigen::Vector3d ();
    }
}

Eigen::Matrix3d DynamicModel::getURDFLinkInertiaCoM(const std::string &link) const
{
    int segmentID = getKDLSegmentIndex(link);
    if(segmentID > -1){
        KDL::RotationalInertia mat = chainFile_.getSegment(segmentID).getInertia().getRotationalInertia();
        double mass = chainFile_.getSegment(segmentID).getInertia().getMass();
        KDL::Vector com = chainFile_.getSegment(segmentID).getInertia().getCOG();
        Eigen::Matrix3d result;

        double xx = mat.data[0] - mass *(com(1)*com(1) + com(2)*com(2));
        double yy = mat.data[4] - mass *(com(0)*com(0) + com(2)*com(2));
        double zz = mat.data[8] - mass *(com(0)*com(0) + com(1)*com(1));
        double xy = mat.data[1] + mass * com(0)*com(1);
        double xz = mat.data[2] + mass * com(0)*com(2);
        double yz = mat.data[5] + mass * com(1)*com(2);

        result << xx, xy, xz,
                xy, yy, yz,
                xz, yz, zz;
        return result;
    }
    else{
        ROS_ERROR_STREAM("Link " << link << " not found! Wrong name?");
        return Eigen::Matrix3d();
    }
}

Eigen::MatrixXd DynamicModel::getRigidBodyRegressionMatrix(const std::string &root,
                                                                const std::string &tip,
                                                                const std::vector<double> &q,
                                                                const std::vector<double> &q_Dot,
                                                                const std::vector<double> &q_DotDot,
                                                                const double& gx,
                                                                const double& gy,
                                                                const double& gz,
                                                                const bool project)
{

    int tipId = getKDLSegmentIndex(tip);
    int rootId = getKDLSegmentIndex(root);
    // determine Matrix dimensions
    int nLinks = tipId - rootId + 1;

    if(q.size() != q_Dot.size() && q.size() != q_DotDot.size() && q.size() < chain_.getNrOfJoints()){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is" << q.size() << ". While q_Dot has dimension " << q_Dot.size() << " and q_DotDot "
                         << q_DotDot.size()<<". The KDL chain contains " <<  chain_.getNrOfJoints() << "  joints." << std::endl);
        return Eigen::MatrixXd(0,0);
    }

    Eigen::MatrixXd result;

    if(project) {
        result = Eigen::MatrixXd(nLinks,nLinks*10); // 10 is the total number of rigid body dynamic parameters 1 (mass) + 3 (center of mass) + 6 (inertia matrix)
        // for each link n between root and tip calculate regression matrices A_n see Handbook of Robotics EQ (14.41) page 331.
        // similar to newton - euler algorighm cf. KDL
    }
    else {
        result = Eigen::MatrixXd(nLinks * 6, nLinks * 10); // if we have 3D force sensing in each joint
    }

    // calculate A_n in body coordinates;
    // Sweep from root to leaf
    int j = 0;
    std::vector<KDL::Frame> X(q.size());
    std::vector<KDL::Frame> Xij(q.size());
    std::vector<KDL::Twist> S(q.size());
    std::vector<KDL::Twist> v(q.size());
    std::vector<KDL::Twist> a(q.size());
    std::vector<Eigen::Matrix<double, 6, 10>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 10>> > An(q.size());
    KDL::Twist ag = -KDL::Twist(KDL::Vector(gx,gy,gz),KDL::Vector::Zero());

    for(unsigned int i = 0; i < q.size(); ++i) {
        double q_,qdot_,qdotdot_;
        if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
            q_=q[j];
            qdot_=q_Dot[j];
            qdotdot_=q_DotDot[j];
            ++j;
        }else
            q_=qdot_=qdotdot_=0.0;

        // Calculate segment properties: X,S,vj,cj
        X[i]=chain_.getSegment(i).pose(q_);//Remark this is the inverse of the
        // frame for transformations from
        // the parent to the current coord frame

        // Transform velocity and unit velocity to segment frame
        KDL::Twist vj = X[i].M.Inverse(chain_.getSegment(i).twist(q_,qdot_));
        S[i]=X[i].M.Inverse(chain_.getSegment(i).twist(q_,1.0));
        // We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
        // calculate velocity and acceleration of the segment (in segment coordinates)

        KDL::Twist vparent;
        KDL::Twist aparent;
        if(i==0){
            vparent = KDL::Twist(KDL::Vector::Zero(), KDL::Vector::Zero());
            aparent = X[i].Inverse(ag);
            Xij[i] = X[i];
        }else{
            vparent = X[i].Inverse(v[i-1]);
            aparent = X[i].Inverse(a[i-1]);
            Xij[i] = Xij[i-1] * X[i];
        }
        v[i] = vparent + vj;
        a[i] = aparent + S[i]*qdotdot_ + v[i]*vj;

        // Calculate the regression Matrix An for each Link
        // See Handbook of Robotics page 331

        KDL::Vector d = a[i].vel + v[i].rot * v[i].vel; // - Xij[i].Inverse(ag); // see text page 331
        An[i].block<3,1>(0,0).setZero();
        An[i].block<3,3>(0,1) = -1.0 * skewSymMat(d);
        An[i].block<3,6>(0,4) = inertiaProductMat(a[i].rot) + skewSymMat(v[i].rot) * inertiaProductMat(v[i].rot);
        An[i].block<3,1>(3,0) = Eigen::Vector3d(d(0), d(1), d(2));
        An[i].block<3,3>(3,1) = skewSymMat(a[i].rot) + skewSymMat(v[i].rot) * skewSymMat(v[i].rot);
        An[i].block<3,6>(3,4).setZero();
    }

    // Calculate Matrix K .transform matrices into each link n frame see Handbook of Robotics EQ (14.49) page 333.
    for(int col = 0; col <nLinks; ++col) {
        for(int row = 0; row < nLinks; ++row) {
            if(col <= row) { // == nLinks -1 - row <= nLinks -1 - col
                KDL::Vector rotAxis = X[tipId - row].Inverse().M*chain_.getSegment(tipId - row).getJoint().JointAxis();
                Eigen::Matrix<double, 1, 6> zi;
                double norm =rotAxis.Norm();
                zi << rotAxis(0)/norm, rotAxis(1)/norm, rotAxis(2)/norm, 0, 0, 0;
                KDL::Frame xi = Xij[tipId - row].Inverse() * Xij[tipId - col];
                Eigen::Matrix<double, 6, 6> Xi = convert2EigenTwistTransform(xi.Inverse()).transpose();
                if(project) {
                    Eigen::Matrix<double, 1, 10> Kij = zi * Xi * An[tipId - col];
                    result.block<1,10>( nLinks -1 - row,(nLinks -1 - col) * 10) = Kij;
                }
                else {
                    Eigen::Matrix<double, 6, 10> Aij = Xi * An[tipId - col];
                    result.block<6,10>( (nLinks -1 - row) * 6, (nLinks -1 - col) * 10) = Aij;
                }
            }
            else {
                if(project) {
                    result.block<1,10>(nLinks -1 - row,(nLinks -1 - col) * 10).setZero();
                }
                else {
                    result.block<6,10>((nLinks -1 - row) * 6,(nLinks -1 - col) * 10).setZero();
                }
            }

        }
    }

    return result;
}

Eigen::MatrixXd DynamicModel::getStaticRigidBodyRegressionMatrix(const std::string &root,
                                                                const std::string &tip,
                                                                const std::vector<double> &q,
                                                                const double& gx,
                                                                const double& gy,
                                                                const double& gz,
                                                                const bool project)
{

    int tipId = getKDLSegmentIndex(tip);
    int rootId = getKDLSegmentIndex(root);
    // determine Matrix dimensions
    int nLinks = tipId - rootId + 1;

    if(q.size() < chain_.getNrOfJoints()){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is" << chain_.getNrOfJoints() << "  joints." << std::endl);
        return Eigen::MatrixXd(0,0);
    }

    Eigen::MatrixXd result;

    if(project) {
        result = Eigen::MatrixXd(nLinks,nLinks*4); // 4 is the total number of static rigid body dynamic parameters 1 (mass) + 3 (center of mass)
        // for each link n between root and tip calculate regression matrices A_n see Handbook of Robotics EQ (14.41) page 331.
        // similar to newton - euler algorighm cf. KDL
    }
    else {
        result = Eigen::MatrixXd(nLinks * 6, nLinks * 4); // if we have 3D force sensing in each joint
    }

    // calculate A_n in body coordinates;
    // Sweep from root to leaf
    int j = 0;
    std::vector<KDL::Frame> X(q.size());
    std::vector<KDL::Frame> Xij(q.size());
    std::vector<KDL::Twist> S(q.size());
    std::vector<KDL::Twist> v(q.size());
    std::vector<KDL::Twist> a(q.size());
    std::vector<Eigen::Matrix<double, 6, 4>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 4>> > An(q.size());
    KDL::Twist ag = -KDL::Twist(KDL::Vector(gx,gy,gz),KDL::Vector::Zero());

    double qdot_ = 0;
    double qdotdot_ = 0;
    for(unsigned int i = 0; i < q.size(); ++i) {
        double q_;
        if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
            q_=q[j];
            ++j;
        }else
            q_=qdot_=qdotdot_=0.0;

        // Calculate segment properties: X,S,vj,cj
        X[i]=chain_.getSegment(i).pose(q_);//Remark this is the inverse of the
        // frame for transformations from
        // the parent to the current coord frame

        // Transform velocity and unit velocity to segment frame
        KDL::Twist vj = X[i].M.Inverse(chain_.getSegment(i).twist(q_,qdot_));
        S[i]=X[i].M.Inverse(chain_.getSegment(i).twist(q_,1.0));
        // We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
        // calculate velocity and acceleration of the segment (in segment coordinates)

        KDL::Twist vparent;
        KDL::Twist aparent;
        if(i==0){
            vparent = KDL::Twist(KDL::Vector::Zero(), KDL::Vector::Zero());
            aparent = X[i].Inverse(ag);
            Xij[i] = X[i];
        }else{
            vparent = X[i].Inverse(v[i-1]);
            aparent = X[i].Inverse(a[i-1]);
            Xij[i] = Xij[i-1] * X[i];
        }
        v[i] = vparent + vj;
        a[i] = aparent + S[i]*qdotdot_ + v[i]*vj;

        // Calculate the regression Matrix An for each Link
        // See Handbook of Robotics page 331

        KDL::Vector d = a[i].vel + v[i].rot * v[i].vel; // - Xij[i].Inverse(ag); // see text page 331
        An[i].block<3,1>(0,0).setZero();
        An[i].block<3,3>(0,1) = -1.0 * skewSymMat(d);
        An[i].block<3,1>(3,0) = Eigen::Vector3d(d(0), d(1), d(2));
        An[i].block<3,3>(3,1).setZero();
    }

    // Calculate Matrix K .transform matrices into each link n frame see Handbook of Robotics EQ (14.49) page 333.
    for(int col = 0; col <nLinks; ++col) {
        for(int row = 0; row < nLinks; ++row) {
            if(col <= row) { // == nLinks -1 - row <= nLinks -1 - col
                KDL::Vector rotAxis = X[tipId - row].Inverse().M*chain_.getSegment(tipId - row).getJoint().JointAxis();
                Eigen::Matrix<double, 1, 6> zi;
                double norm =rotAxis.Norm();
                zi << rotAxis(0)/norm, rotAxis(1)/norm, rotAxis(2)/norm, 0, 0, 0;
                KDL::Frame xi = Xij[tipId - row].Inverse() * Xij[tipId - col];
                Eigen::Matrix<double, 6, 6> Xi = convert2EigenTwistTransform(xi.Inverse()).transpose();
                if(project) {
                    Eigen::Matrix<double, 1, 4> Kij = zi * Xi * An[tipId - col];
                    result.block<1,4>( nLinks -1 - row,(nLinks -1 - col) * 4) = Kij;
                }
                else {
                    Eigen::Matrix<double, 6, 4> Aij = Xi * An[tipId - col];
                    result.block<6,4>( (nLinks -1 - row) * 6, (nLinks -1 - col) * 10) = Aij;
                }
            }
            else {
                if(project) {
                    result.block<1,4>(nLinks -1 - row,(nLinks -1 - col) * 4).setZero();
                }
                else {
                    result.block<6,4>((nLinks -1 - row) * 6,(nLinks -1 - col) * 4).setZero();
                }
            }

        }
    }

    return result;
}

void DynamicModel::modifiedRNE(const double gx, const double gy, const double gz,
                                    const std::vector<double> &q1, const std::vector<double> &q2,
                                    const std::vector<double> &q3, const std::vector<double> &q4,
                                    Eigen::MatrixXd& res, std::size_t column)
{
    std::size_t nj = chain_.getNrOfJoints();
    std::size_t ns = chain_.getNrOfSegments();

    if(q1.size() != q2.size()
            || q2.size() != q3.size()
            || q3.size() != q4.size()
            || q4.size() < chain_.getNrOfJoints()
            || res.cols() < 1
            || res.rows() > q1.size()){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q1 is " << q1.size()
                         << ". While q2 has dimension " << q2.size() << ", q3 "
                         << q3.size() << " and q4 " << q4.size() <<". The KDL chain contains "
                         <<  chain_.getNrOfJoints() << "  joints."
                         << " The result must have at least dimension "
                         << chain_.getNrOfJoints() << " x 1. But is "
                         << res.rows() << " x " << res.cols() << "!"
                         << std::endl);
        return;
    }

    unsigned int j = 0;
    //Sweep from root to leaf
    KDL::Vector ag(-gx, -gy, -gz);  // as orocos kdl

    std::vector<KDL::Frame> X(ns);
    std::vector<KDL::Twist> S(ns);
    std::vector<KDL::Vector> omega(ns);
    std::vector<KDL::Vector> omega_a(ns);;
    std::vector<KDL::Vector> omegaDot(ns);
    std::vector<KDL::Vector> a(ns);
    std::vector<KDL::Vector> F(ns);
    std::vector<KDL::Vector> N(ns);
    std::vector<KDL::Vector> n(ns);
    std::vector<KDL::Vector> f(ns);
    //    res.resize(nj);

    for(unsigned int i = 0; i < nj; ++i){
        double q_,qdot_, qdot_a_,qdotdot_;
        if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
            q_=q1[j];
            qdot_=q2[j];
            qdot_a_ =q3[j];
            qdotdot_=q4[j];
            j++;
        }else
            q_=qdot_=qdot_a_=qdotdot_=0.0;

        //Calculate segment properties: X,S
        X[i]=chain_.getSegment(i).pose(q_);//Remark this is the inverse of the
        //frame for transformations from
        //the parent to the current coord frame
        S[i]=X[i].M.Inverse(chain_.getSegment(i).twist(q_,1.0));

        // calculate angular velocity

        if(i == 0){
            omega[i]    = S[i].rot * qdot_ ;
            omega_a[i]  = S[i].rot * qdot_a_;
            omegaDot[i] = S[i].rot * qdotdot_;
            a[i]=X[i].Inverse().M*ag;
        }
        else{
            KDL::Frame Xij = X[i].Inverse();
            KDL::Rotation Eij = Xij.M;
            KDL::Vector rij = X[i].p;
            omega[i]    = Eij * omega[i-1] + S[i].rot * qdot_;                                                    // (AVR)
            omega_a[i]  = Eij * omega_a[i-1] + S[i].rot * qdot_a_;                                                // (AVR*)
            omegaDot[i] = Eij * omegaDot[i-1] + S[i].rot * qdotdot_ + (Eij * omega_a[i-1]) * (S[i].rot *  qdot_); // (AAR*0)
            a[i]        = Eij * (a[i-1] + omegaDot[i-1] * rij + omega[i-1] * (omega_a[i-1] * rij) );              // (LAR'*0)

        }
        // Calculate the force for the joint
        // Collect RigidBodyInertia no external forces
        KDL::RigidBodyInertia I = chain_.getSegment(i).getInertia();
        KDL::Vector cij = I.getCOG();
        F[i] = I.getMass() * (a[i] + omegaDot[i] * cij + omega[i] *(omega_a[i] * cij)); // body forces
        KDL::RotationalInertia Icm = getLinkInertiaCoM(i); // body torques
        N[i] = Icm * omegaDot[i] + omega_a[i] * (Icm * omega[i]);
    }

    //    Sweep from leaf to root
    j = nj -1;
    f[ns -1] = F[ns-1];
    n[ns -1] = N[ns-1] + chain_.getSegment(ns-1).getInertia().getCOG() * F[ns-1];

    for(int i = ns - 1; i >= 0; --i){
        if(i != 0){
            KDL::Rotation Eij = X[i].M;
            KDL::Vector fi = Eij* f[i];
            KDL::RigidBodyInertia I = chain_.getSegment(i-1).getInertia();
            f[i-1]  = F[i-1] + fi;
            n[i-1]  = N[i-1] + I.getCOG() * F[i-1]  + Eij * n[i]  + X[i].p * fi;
        }
        //        }
        if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
            res(j--,column) = dot(S[i].rot, n[i]);
        }
    }
    //    return (error = E_NOERROR);

}

void DynamicModel::getMatrixC(const std::vector<double> &q,
                                   const std::vector<double> &qDot,
                                   Eigen::MatrixXd& res)
{

    std::size_t n_joints = chain_.getNrOfJoints();
    if(q.size() != qDot.size() || q.size() < n_joints){
        ROS_ERROR_STREAM("Dimension mismatch of input: Dimenion of q is" << q.size() << ". While q_Dot has dimension " << qDot.size()
                         <<". The KDL chain contains " <<  chain_.getNrOfJoints() << "  joints." << std::endl);
        return;
    }
    std::vector<double> qDotDot(n_joints, 0);
    res.resize(n_joints, n_joints);

    for(std::size_t i = 0; i < n_joints; ++i){
        std::vector<double> unit_vec(n_joints);
        unit_vec[i] = 1.0;

        modifiedRNE(0, 0, 0, q, qDot, unit_vec, qDotDot, res, i);

    }
}


#pragma once

#include <Eigen/Dense>
#include <zmq.hpp>

#include "messages/draco.pb.h"

class DracoData;

class DracoDataManager {
public:
  static DracoDataManager *GetDracoDataManager();
  virtual ~DracoDataManager();

  void InitializeSockets(const std::string &_addr);

  void Send();

  std::unique_ptr<DracoData> data;

private:
  /* data */
  DracoDataManager();

  std::unique_ptr<zmq::context_t> context_;
  std::unique_ptr<zmq::socket_t> socket_;

  bool b_socket_initialized_;
};

class DracoData {
public:
  DracoData(){};
  ~DracoData(){};

  // should be matching with protobuf msg
  double time = 0.;
  int phase = 0;

  Eigen::VectorXd task_com_pos = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_com_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_com_pos_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_com_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_com_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_com_local_pos_err = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_com_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_cam_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_cam_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_cam_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_cam_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_torso_ori_pos_des = Eigen::VectorXd::Zero(4);
  Eigen::VectorXd task_torso_ori_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_torso_ori_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_torso_ori_pos = Eigen::VectorXd::Zero(4);
  Eigen::VectorXd task_torso_ori_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_torso_ori_local_pos_err = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_torso_ori_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_rfoot_lin_pos = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_lin_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_lin_pos_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_lin_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_lin_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_lin_local_pos_err = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_lin_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_rfoot_ori_pos_des = Eigen::VectorXd::Zero(4);
  Eigen::VectorXd task_rfoot_ori_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_ori_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_ori_pos = Eigen::VectorXd::Zero(4);
  Eigen::VectorXd task_rfoot_ori_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_ori_local_pos_err = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_rfoot_ori_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_lfoot_lin_pos = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_lin_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_lin_pos_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_lin_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_lin_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_lin_local_pos_err = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_lin_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_lfoot_ori_pos_des = Eigen::VectorXd::Zero(4);
  Eigen::VectorXd task_lfoot_ori_vel_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_ori_acc_des = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_ori_pos = Eigen::VectorXd::Zero(4);
  Eigen::VectorXd task_lfoot_ori_vel = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_ori_local_pos_err = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd task_lfoot_ori_local_vel_err = Eigen::VectorXd::Zero(3);

  Eigen::VectorXd task_upper_body_pos_des = Eigen::VectorXd::Zero(13);
  Eigen::VectorXd task_upper_body_vel_des = Eigen::VectorXd::Zero(13);
  Eigen::VectorXd task_upper_body_acc_des = Eigen::VectorXd::Zero(13);
  Eigen::VectorXd task_upper_body_pos = Eigen::VectorXd::Zero(13);
  Eigen::VectorXd task_upper_body_vel = Eigen::VectorXd::Zero(13);

  // wbc cmd
  Eigen::VectorXd cmd_rfoot_rf = Eigen::VectorXd::Zero(6);
  Eigen::VectorXd cmd_lfoot_rf = Eigen::VectorXd::Zero(6);

  Eigen::VectorXd cmd_joint_positions = Eigen::VectorXd::Zero(27);
  Eigen::VectorXd cmd_joint_velocities = Eigen::VectorXd::Zero(27);
  Eigen::VectorXd cmd_joint_torques = Eigen::VectorXd::Zero(27);
  Eigen::VectorXd joint_velocities = Eigen::VectorXd::Zero(27);

  // for meshcat visualization
  Eigen::VectorXd joint_positions = Eigen::VectorXd::Zero(27);
  Eigen::VectorXd base_joint_pos = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd base_joint_quat = Eigen::VectorXd::Zero(4);

  // filtered data
  Eigen::VectorXd com_vel_est = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd com_vel_raw = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd imu_ang_vel_est = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd imu_ang_vel_raw = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd cam_est = Eigen::VectorXd::Zero(3);
  Eigen::VectorXd cam_raw = Eigen::VectorXd::Zero(3);

  // icp
  Eigen::VectorXd icp = Eigen::VectorXd::Zero(2);
  Eigen::VectorXd icp_des = Eigen::VectorXd::Zero(2);
  Eigen::VectorXd icp_dot = Eigen::VectorXd::Zero(2);
  Eigen::VectorXd icp_dot_des = Eigen::VectorXd::Zero(2);
};


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

#include "common.h"
#include <bitcoin/bitcoin.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <ethash/keccak.hpp>
#include "utility/hex.h"

namespace
{
    const std::string kHexPrefix = "0x";

    bool HasHexPrefix(const std::string& value)
    {
        return value.find(kHexPrefix) == 0;
    }

    libbitcoin::wallet::hd_private ProcessHDPrivate(const libbitcoin::wallet::hd_private& privateKey, uint32_t index, bool hard = true)
    {
        static constexpr auto first = libbitcoin::wallet::hd_first_hardened_key;
        const auto position = hard ? first + index : index;
        return privateKey.derive_private(position);
    }
}

namespace beam::ethereum
{
std::string ConvertEthAddressToStr(const libbitcoin::short_hash& addr)
{
    return kHexPrefix + libbitcoin::encode_base16(addr);
}

libbitcoin::short_hash ConvertStrToEthAddress(const std::string& addressStr)
{
    libbitcoin::short_hash address;
    libbitcoin::decode_base16(address, RemoveHexPrefix(addressStr));
    return address;
}

libbitcoin::short_hash GetEthAddressFromPubkeyStr(const std::string& pubkeyStr)
{
    auto tmp = beam::from_hex(std::string(pubkeyStr.begin() + 2, pubkeyStr.end()));
    auto hash = ethash::keccak256(&tmp[0], tmp.size());
    libbitcoin::short_hash address;

    std::copy_n(&hash.bytes[12], 20, address.begin());
    return address;
}

libbitcoin::ec_secret GeneratePrivateKey(const std::vector<std::string> words, uint32_t accountIndex)
{
    auto seed = libbitcoin::wallet::decode_mnemonic(words);
    libbitcoin::data_chunk seed_chunk(libbitcoin::to_chunk(seed));

    const auto prefixes = libbitcoin::wallet::hd_private::to_prefixes(0, 0);
    libbitcoin::wallet::hd_private private_key(seed_chunk, prefixes);

    private_key = ProcessHDPrivate(private_key, 44);
    private_key = ProcessHDPrivate(private_key, 60);
    private_key = ProcessHDPrivate(private_key, 0);
    private_key = ProcessHDPrivate(private_key, 0, false);
    private_key = ProcessHDPrivate(private_key, accountIndex, false);

    return private_key.secret();
}

libbitcoin::short_hash GenerateEthereumAddress(const std::vector<std::string> words, uint32_t accountIndex)
{
    auto privateKey = GeneratePrivateKey(words, accountIndex);
    libbitcoin::ec_compressed point;

    libbitcoin::secret_to_public(point, privateKey);

    auto pk = libbitcoin::wallet::ec_public(point, false);
    auto rawPk = pk.encoded();

    return GetEthAddressFromPubkeyStr(rawPk);
}

ECC::uintBig ConvertStrToUintBig(const std::string& number, bool hex)
{
    libbitcoin::data_chunk dc;

    if (hex)
    {
        dc = beam::from_hex(RemoveHexPrefix(number));
    }
    else
    {
        boost::multiprecision::uint256_t value(number);
        std::stringstream stream;

        stream << std::hex << value;
        dc = beam::from_hex(stream.str());
    }

    ECC::uintBig result = ECC::Zero;
    std::copy(dc.crbegin(), dc.crend(), std::rbegin(result.m_pData));
    return result;
}

std::string AddHexPrefix(const std::string& value)
{
    if (!HasHexPrefix(value))
    {
        return kHexPrefix + value;
    }

    return value;
}

std::string RemoveHexPrefix(const std::string& value)
{
    if (HasHexPrefix(value))
    {
        return std::string(value.begin() + 2, value.end());
    }

    return value;
}

void AddContractABIWordToBuffer(const libbitcoin::data_slice& src, libbitcoin::data_chunk& dst)
{
    assert(src.size() <= kEthContractABIWordSize);
    if (src.size() < kEthContractABIWordSize)
    {
        dst.insert(dst.end(), kEthContractABIWordSize - src.size(), 0x00);
    }
    dst.insert(dst.end(), src.begin(), src.end());
}

uint32_t GetCoinUnitsMultiplier(beam::wallet::AtomicSwapCoin swapCoin)
{
    switch (swapCoin)
    {
    case beam::wallet::AtomicSwapCoin::Ethereum:
    case beam::wallet::AtomicSwapCoin::Dai:
        return 1'000'000'000u;
    case beam::wallet::AtomicSwapCoin::Usdt:
    case beam::wallet::AtomicSwapCoin::WBTC:
        return 1u;
    default:
        assert(false && "Unexpected swapCoin!");
        return 1u;
    }
}

bool IsEthereumBased(wallet::AtomicSwapCoin swapCoin)
{
    switch (swapCoin)
    {
    case beam::wallet::AtomicSwapCoin::Ethereum:
    case beam::wallet::AtomicSwapCoin::Dai:
    case beam::wallet::AtomicSwapCoin::Usdt:
    case beam::wallet::AtomicSwapCoin::WBTC:
        return true;
    default:
        return false;
    }
}

namespace swap_contract
{
    std::string GetRefundMethodHash(bool isHashLockScheme)
    {
        return isHashLockScheme ? "7249fbb6" : "fa89401a";
    }

    std::string GetLockMethodHash(bool isErc20, bool isHashLockScheme)
    {
        if (isErc20)
        {
            return isHashLockScheme ? "15601f4f" : "71c472e6";
        }
        return isHashLockScheme ? "ae052147" : "bc18cc34";
    }

    std::string GetRedeemMethodHash(bool isHashLockScheme)
    {
        return isHashLockScheme ? "b31597ad" : "8772acd6";
    }

    std::string GetDetailsMethodHash(bool isHashLockScheme)
    {
        return isHashLockScheme ? "6bfec360" : "7cf3285f";
    }
} // namespace swap_contract
} // namespace beam::ethereum


#ifndef PYTHONIC_INCLUDE_NUMPY_EXPM1_HPP
#define PYTHONIC_INCLUDE_NUMPY_EXPM1_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/expm1.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME expm1
#define NUMPY_NARY_FUNC_SYM boost::simd::expm1
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


// Copyright (c) 2012-2013 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "key.h"

#include "base58.h"
#include "script/script.h"
#include "uint256.h"
#include "util.h"
#include "utilstrencodings.h"

#include <string>
#include <vector>

#include <boost/test/unit_test.hpp>

using namespace std;

static const string strSecret1C    ("cNmhcBv76pk1AePVXwwsJttHFQytnvy5WysDiFqcT5qbxmg83hek");
static const string strSecret2C    ("cTEj9LkP1DMycniUVpNj74AkdKH9Go7urU64gNZ79xTVyj1E8uH9");

//Note: Those are TestNet addresses, so they don't start with a "L"
static const string addr1C = "yHMyqqUaSGifPgGqiciFjVuuako1agdHXq";
static const string addr2C = "xxE211DyUWRUnPFMXcQ2wwj2ZZmhavMP6x";

static const string strAddressBad("Xta1praZQjyELweyMByXyiREw1ZRsjXzVP");

BOOST_AUTO_TEST_SUITE(key_tests)

BOOST_AUTO_TEST_CASE(key_test1)
{
    CBitcoinSecret bsecret1C, bsecret2C, baddress1;
    BOOST_CHECK( bsecret1C.SetString(strSecret1C));
    BOOST_CHECK( bsecret2C.SetString(strSecret2C));
    BOOST_CHECK(!baddress1.SetString(strAddressBad));

    CKey key1C = bsecret1C.GetKey();
    BOOST_CHECK(key1C.IsCompressed() == true);
    CKey key2C = bsecret2C.GetKey();
    BOOST_CHECK(key2C.IsCompressed() == true);

    CPubKey pubkey1C = key1C.GetPubKey();
    CPubKey pubkey2C = key2C.GetPubKey();

    BOOST_CHECK(key1C.VerifyPubKey(pubkey1C));
    BOOST_CHECK(!key1C.VerifyPubKey(pubkey2C));

    BOOST_CHECK(!key2C.VerifyPubKey(pubkey1C));
    BOOST_CHECK(key2C.VerifyPubKey(pubkey2C));

    BOOST_CHECK(DecodeDestination(addr1C) == CTxDestination(pubkey1C.GetID()));
    BOOST_CHECK(DecodeDestination(addr2C) == CTxDestination(pubkey2C.GetID()));

    for (int n=0; n<16; n++)
    {
        string strMsg = strprintf("Very secret message %i: 11", n);
        uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());

        // normal signatures

        vector<unsigned char> sign1C, sign2C;

        BOOST_CHECK(key1C.Sign(hashMsg, sign1C));
        BOOST_CHECK(key2C.Sign(hashMsg, sign2C));

        BOOST_CHECK( pubkey1C.Verify(hashMsg, sign1C));
        BOOST_CHECK(!pubkey1C.Verify(hashMsg, sign2C));

        BOOST_CHECK(!pubkey2C.Verify(hashMsg, sign1C));
        BOOST_CHECK( pubkey2C.Verify(hashMsg, sign2C));

        // compact signatures (with key recovery)

        vector<unsigned char> csign1C, csign2C;

        BOOST_CHECK(key1C.SignCompact(hashMsg, csign1C));
        BOOST_CHECK(key2C.SignCompact(hashMsg, csign2C));

        CPubKey rkey1C, rkey2C;

        BOOST_CHECK(rkey1C.RecoverCompact(hashMsg, csign1C));
        BOOST_CHECK(rkey2C.RecoverCompact(hashMsg, csign2C));

        BOOST_CHECK(rkey1C == pubkey1C);
        BOOST_CHECK(rkey2C == pubkey2C);
    }

    // test deterministic signing

    std::vector<unsigned char> detsigc;
    string strMsg = "Very deterministic message";
    uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());
    BOOST_CHECK(key1C.Sign(hashMsg, detsigc));
    BOOST_CHECK(key2C.Sign(hashMsg, detsigc));
    BOOST_CHECK(key1C.SignCompact(hashMsg, detsigc));
    BOOST_CHECK(detsigc == ParseHex("20270f3681e1bc5f891c07b1401e4b3de0014cf49170d9bfe90142e6b9fe5442dc5ba676b660a2c604946f85f49309c993acd0e779ceef94e9b0785a6acb368c34"));
    BOOST_CHECK(key2C.SignCompact(hashMsg, detsigc));
    BOOST_CHECK(detsigc == ParseHex("20a12c9145860e14ee6dba6c15e66e4cfa5984aa6bc9c094ea7af0f35cc842d4a229b5f1276da285b2f92bdb2c8c5e41f1bdb5519384b1b5bca23caeeeedf5b68e"));
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (C) 2015 Ion Torrent Systems, Inc. All Rights Reserved */

/* Author: Alex Artyomenko <aartyomenko@cs.gsu.edu> */

#ifndef ION_ANALYSIS_VITERBI_HPP
#define ION_ANALYSIS_VITERBI_HPP

#include <stdlib.h>
#include <vector>
#include <algorithm>
#include <numeric>
#include <boost/math/distributions/poisson.hpp>
#include <boost/algorithm/minmax_element.hpp>

template<typename T>
template<typename _ForwardIterator>
markov_chain<T>::markov_chain(_ForwardIterator begin, _ForwardIterator end) {
  pair<_ForwardIterator, _ForwardIterator> m = boost::minmax_element(begin, end);
  initialize(begin, end, *(m.first), *(m.second));
}

template<typename T>
template<typename _ForwardIterator>
markov_chain<T>::markov_chain(_ForwardIterator begin, _ForwardIterator end, T min, T max) {
  initialize(begin, end, min, max);
}

template<typename T>
template<typename _ForwardIterator>
void markov_chain<T>::initialize(_ForwardIterator begin, _ForwardIterator end, T min, T max) {
  // initialize a constructor delegation method
  size_t s = (size_t)distance(begin, end);
  T dmax = (T) max < MAX_STATE_VALUE ? max : MAX_STATE_VALUE,
    dmin = (T) min < MAX_STATE_VALUE ? min : MAX_STATE_VALUE;
  if (dmin == dmax) {
    T avg = (T) round(accumulate(begin, end, 0.0) / s);
    items.push_back(make_pair(depth_info<T>(avg, min, max), s - 1));
    return;
  }
  values.reserve(s);
  reserve(s);
  generate_states(dmin, dmax);
  for (_ForwardIterator it = begin; it != end; it++)
    process_next_value(*it);
  optimal_path();
}

template<typename T>
void markov_chain<T>::generate_states(T min, T max) {
  // prepare state values and state objects for viterbi execution
  double dmax = (double) max,
      dmin = (double) min,
      alpha = 2,
      val;
  val = ceil(pow(dmin, 1 / alpha));

  states.push_back(dmin);
  for (double st = pow(val, alpha); st < dmax; st = pow(val, alpha), val++) {
    states.push_back(st);
  }
  states.push_back(dmax);
}

template<typename T>
void markov_chain<T>::process_next_value(T val) {
  // accept next value and fill out column in table of states
  vector<long> states_trace;
  states_trace.reserve(states.size());
  for (vector<markov_state>::iterator st = states.begin(); st != states.end(); st++) {
    markov_state& current = *st;
    markov_chain_comparator mccmp = markov_chain_comparator(current);
    vector<markov_state>::iterator min_prev = min_element(states.begin(), states.end(), mccmp);
    states_trace.push_back(min_prev - states.begin());
    double new_penalty = min_prev->prev + current.cost(*min_prev) + current.cost(val);
    st->current = new_penalty;
  }
  for (vector<markov_state>::iterator st = states.begin(); st != states.end(); st++) st->step_forward();
  push_back(states_trace);
  values.push_back(val);
}

template<typename T>
void markov_chain<T>::optimal_path() {
  // find optimal state switches and generate items for a chain

  double mean = 0;
  int i = 0;

  items.reserve(size());
  long state_index = min_element(states.begin(), states.end(), markov_chain_comparator()) - states.begin();

  markov_chain::reverse_iterator it = rbegin();
  typename vector<T>::reverse_iterator vit = values.rbegin();
  items.push_back(make_pair(depth_info<T>(*vit), size() - 1));
  for (; it != rend() && vit != values.rend(); it++, vit++) {
    mean += *vit;
    i++;
    if (vit == values.rend() - 1) {
      items.back().first.dp = (T)round(mean / i);
    } else if ((*it)[state_index] != state_index){
      items.back().first.dp = (T)round(mean / i);
      mean = 0;
      i = 0;
      items.push_back(make_pair(depth_info<T>(*(vit+1)), rend() - it - 2));
    } else {
      depth_info<T>& cdi = items.back().first;
      cdi.max_dp = max(*vit, cdi.max_dp);
      cdi.min_dp = min(*vit, cdi.min_dp);
    }
    state_index = (*it)[state_index];
  }
}

#endif //ION_ANALYSIS_VITERBI_HPP


#ifndef VERUS_H_
#define VERUS_H_

#include <signal.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <sys/time.h>
#include <iostream>
#include <vector>
#include <math.h>
#include <fstream>

#ifndef __ANDROID__
#include <atomic>
#include <queue>
#include <map>

// ALGLIB library
#include "./lib/alglib/src/ap.h"
#include "./lib/alglib/src/interpolation.h"
using namespace alglib;

#include <boost/asio.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/assign/std/vector.hpp>

#endif

// VERUS PARAMETERS
#define  MTU 1448
#define  VERUS_M_DECREASE 0.7
#define  CURVE_TIMER 1e6  // timer in microseconds, how often do we update the curve
#define  EPOCH 5e3 // Verus epoch in microseconds
#define  DELTA1 1.0 // delta decrease
#define  DELTA2 2.0 // delta increase
#define  VERUS_R 4.0 // verus ratio of Dmax/Dmin
#define	 SS_EXIT_THRESHOLD 500.0
#define  MAX_W_DELAY_CURVE 40000

typedef struct __attribute__((packed, aligned(2))) m {
  int payloadlength;
  unsigned long long seq;
} verus_header;

typedef struct __attribute__((packed, aligned(2))) n {
  // verus_header header;
  char * data;
} verus_packet;

void* delayProfile_thread (void *arg);
void restartSlowStart(void);
double ewma (double vals, double delay, double alpha);

#endif /* VERUS_H_ */


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_Nuclide.hpp
//! \author Alex Robinson
//! \brief  The nuclide class declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_NUCLIDE_HPP
#define MONTE_CARLO_NUCLIDE_HPP

// Boost Includes
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <boost/scoped_ptr.hpp>

// FRENSIE Includes
#include "MonteCarlo_NuclearReaction.hpp"
#include "Data_XSSNeutronDataExtractor.hpp"

namespace MonteCarlo{

/*! The nuclide class
 * \details This is the base class for all nuclides. No unresolved
 * resonance data is stored in this base class.
 */
class Nuclide
{

private:

  // Typedef for Teuchos ScalarTraits
  typedef Teuchos::ScalarTraits<double> ST;

public:

  //! Typedef for the reaction map
  typedef boost::unordered_map<NuclearReactionType,
			       Teuchos::RCP<NuclearReaction> > ReactionMap;

  //! Typedef for the const reaction map
  typedef boost::unordered_map<NuclearReactionType,
			       Teuchos::RCP<const NuclearReaction> >
  ConstReactionMap;

  //! Set the nuclear reaction types that will be considered as absorption
  static void setAbsorptionReactionTypes(
	const Teuchos::Array<NuclearReactionType>& absorption_reaction_types );

  //! Add nuclear reaction types that will be considered as absorption
  static void addAbsorptionReactionTypes(
	const Teuchos::Array<NuclearReactionType>& absorption_reaction_types );

  //! Create a unique id for the nuclide based on its name
  static unsigned getUniqueIdFromName( const std::string& name );

  //! Constructor
  Nuclide( const std::string& name,
	   const unsigned atomic_number,
	   const unsigned atomic_mass_number,
	   const unsigned isomer_number,
	   const double atomic_weight_ratio,
	   const double temperature,
	   const Teuchos::ArrayRCP<double>& energy_grid,
	   const ReactionMap& standard_scattering_reactions,
	   const ReactionMap& standard_absorption_reactions );

  //! Destructor
  ~Nuclide()
  { /* ... */ }

  //! Return the nuclide name
  const std::string& getName() const;

  //! Return the unique id used to refer to this nuclide
  unsigned getId() const;

  //! Return the atomic number of the nuclide
  unsigned getAtomicNumber() const;

  //! Return the atomic mass number of the nuclide
  unsigned getAtomicMassNumber() const;

  //! Return the nuclear isomer number of the nuclide
  unsigned getIsomerNumber() const;

  //! Return the atomic weight ratio
  double getAtomicWeightRatio() const;

  //! Return the temperature of the nuclide (in MeV)
  double getTemperature() const;

  //! Return the total cross section at the desired energy
  double getTotalCrossSection( const double energy ) const;

  //! Return the total absorption cross section at the desired energy
  double getAbsorptionCrossSection( const double energy ) const;

  //! Return the survival probability at the desired energy
  double getSurvivalProbability( const double energy ) const;

  //! Return the cross section for a specific nuclear reaction
  double getReactionCrossSection( const double energy,
				  const NuclearReactionType reaction ) const;

  //! Collide with a neutron
  void collideAnalogue( NeutronState& neutron, ParticleBank& bank ) const;

  //! Collide with a neutron and survival bias
  void collideSurvivalBias( NeutronState& neutron, ParticleBank& bank ) const;

private:

  // Set the default absorption reaction types
  static boost::unordered_set<NuclearReactionType>
  setDefaultAbsorptionReactionTypes();

  // Calculate the total absorption cross section
  void calculateTotalAbsorptionReaction(
			        const Teuchos::ArrayRCP<double>& energy_grid );

  // Calculate the total cross section
  void calculateTotalReaction( const Teuchos::ArrayRCP<double>& energy_grid );

  // Sample an absorption reaction
  void sampleAbsorptionReaction( const double scaled_random_number,
				 NeutronState& neutron,
				 ParticleBank& bank ) const;

  // Sample a scattering reaction
  void sampleScatteringReaction( const double scaled_random_number,
				 NeutronState& neutron,
				 ParticleBank& bank ) const;

  // Reactions that should be treated as absorption
  static boost::unordered_set<NuclearReactionType> absorption_reaction_types;

  // The nuclide name
  std::string d_name;

  // The unique id used to refer to this nuclide
  unsigned d_id;

  // The atomic number of the nuclide
  unsigned d_atomic_number;

  // The atomic mass number
  unsigned d_atomic_mass_number;

  // The nuclear isomer number
  unsigned d_isomer_number;

  // The weight of the nucleus in neutron masses
  double d_atomic_weight_ratio;

  // The temperature of the nuclide (MeV)
  double d_temperature;

  // The total reaction
  boost::scoped_ptr<const NuclearReaction> d_total_reaction;

  // The total absorption reaction
  boost::scoped_ptr<const NuclearReaction> d_total_absorption_reaction;

  // The scattering reactions
  ConstReactionMap d_scattering_reactions;

  // The absorption reactions
  ConstReactionMap d_absorption_reactions;

  // Miscellaneous reactions
  ConstReactionMap d_miscellaneous_reactions;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_NUCLIDE_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_Nuclide.hpp
//---------------------------------------------------------------------------//