//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_FACTORIZATIONS_SYMEIG_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_FACTORIZATIONS_SYMEIG_HPP_INCLUDED

#include <nt2/include/functions/symeig.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/hsev_w.hpp>
#include <nt2/include/functions/hsev_wu.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/core/utility/assign_swap.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/container/table/table.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  //SYMEIG Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( symeig_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return real(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( symeig_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      return real(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( symeig_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                              (unspecified_<A2>)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&, const A2&) const
    {
      return real(a0);
    }
  };

  //============================================================================
  //SYMEIG
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( symeig_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::symeig_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
    typedef typename child0::value_type                                  type_t;
    typedef typename nt2::meta::as_real<type_t>::type                   rtype_t;
    typedef nt2::memory::container<tag::table_,  type_t, nt2::_2D>   o_semantic;
    typedef nt2::memory::container<tag::table_, rtype_t, nt2::_2D>   r_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }
  private:
    //==========================================================================
    /// INTERNAL ONLY - W = SYMEIG(A)
    // returns eigenvalues as a vector
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>()
             , nt2::policy<ext::upper_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - W = SYMEIG(A, matrix_/vector_/upper_/lower_/raw_)
    // returns eigenvalues as a vector
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
                , boost::mpl::long_<2> const&
                , boost::mpl::long_<1> const&
                ) const
    {
      eval1_2(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                    , nt2::policy<ext::vector_>
                   ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>()
             , nt2::policy<ext::upper_>());
    }

    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                    , nt2::policy<ext::raw_>
                   ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>()
             , nt2::policy<ext::upper_>());
    }

    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                    , nt2::policy<ext::matrix_>
                   ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::upper_>());
    }

    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                    , nt2::policy<ext::upper_>
                   ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>()
             , nt2::policy<ext::upper_>());
    }

    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                    , nt2::policy<ext::lower_>
                   ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>()
             , nt2::policy<ext::lower_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - W = SYMEIG(A, matrix_/vector_, upper_/lower_)
    // returns eigenvalues as a vector
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0))
             );
    }

    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 , nt2::policy<ext::lower_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (r_semantic, w
                           , boost::proto::child_c<0>(a1));
      w.resize(nt2::of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_w( boost::proto::value(a)
                                   , boost::proto::value(w)
                                   , 'L'));
      assign_swap(boost::proto::child_c<0>(a1), w);
    }

    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 , nt2::policy<ext::upper_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (r_semantic, w
                           , boost::proto::child_c<0>(a1));
      w.resize(nt2::of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_w( boost::proto::value(a)
                                   , boost::proto::value(w)
                                   , 'U'));
      assign_swap(boost::proto::child_c<0>(a1), w);
    }

    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 , nt2::policy<ext::lower_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), work);
      nt2::container::table<rtype_t> w(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_w( boost::proto::value(a)
                                   , boost::proto::value(w)
                                   , 'L'));
      boost::proto::child_c<0>(a1) = from_diag(w);
    }

    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 , nt2::policy<ext::upper_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), work);
      nt2::container::table<rtype_t>  w(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_w( boost::proto::value(a)
                                   , boost::proto::value(w)
                                   , 'U'));
      boost::proto::child_c<0>(a1) = from_diag(w);
    }

    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::raw_>
                 , nt2::policy<ext::upper_>
                 ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>(), nt2::policy<ext::upper_>());
    }

    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::raw_>
                 , nt2::policy<ext::lower_>
                 ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>(), nt2::policy<ext::lower_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [W, V]= SYMEIG(A, matrix_/vector_/upper_/lower_/raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0)));

    }

    //==========================================================================
    /// INTERNAL ONLY - [W, U]= SYMEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_3(a0, a1, nt2::matrix_, nt2::upper_);
    }

    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, u
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      nt2::container::table<rtype_t> w(of_size(height(u), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_wu( boost::proto::value(u)
                                    , boost::proto::value(w)
                                    , 'U'));
      boost::proto::child_c<0>(a1) = from_diag(w);
      assign_swap(boost::proto::child_c<1>(a1), u);
    }

    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, u
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (r_semantic, w, boost::proto::child_c<0>(a1));
      w.resize(nt2::of_size(height(u), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_wu( boost::proto::value(u)
                                    , boost::proto::value(w)
                                    , 'U'));
      assign_swap(boost::proto::child_c<0>(a1), w);
      assign_swap(boost::proto::child_c<1>(a1), u);
    }

    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::raw_>
                 ) const
    {
      eval2_2(a0, a1, nt2::policy<ext::vector_>());
    }

    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::lower_>
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::lower_>());
    }

    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::upper_>
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::upper_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [W, V]= SYMEIG(A, matrix_/vector_, lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_3( a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0))
             );

    }

    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_>
                 , nt2::policy<ext::lower_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, u
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      nt2::container::table<rtype_t> w(of_size(height(u), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_wu( boost::proto::value(u)
                                    , boost::proto::value(w)
                                    , 'L'));
      boost::proto::child_c<0>(a1) = from_diag(w);
      assign_swap(boost::proto::child_c<1>(a1), u);
    }

    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_>
                 , nt2::policy<ext::upper_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, u
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      nt2::container::table<rtype_t> w(of_size(height(u), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_wu( boost::proto::value(u)
                                    , boost::proto::value(w)
                                    , 'U'));
      boost::proto::child_c<0>(a1) = from_diag(w);
      assign_swap(boost::proto::child_c<1>(a1), u);
    }

    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_>
                 , nt2::policy<ext::upper_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, u
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (r_semantic, w
                           , boost::proto::child_c<0>(a1));
      w.resize(of_size(height(u), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_wu( boost::proto::value(u)
                                    , boost::proto::value(w)
                                    , 'U'));
      assign_swap(boost::proto::child_c<0>(a1), w);
      assign_swap(boost::proto::child_c<1>(a1), u);
    }

    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::raw_>
                 , nt2::policy<ext::upper_>
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::vector_>(), nt2::policy<ext::upper_>());
    }

    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::raw_>
                 , nt2::policy<ext::lower_>
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::vector_>(), nt2::policy<ext::lower_>());
    }

    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_>
                 , nt2::policy<ext::lower_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, u
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (r_semantic, w
                           , boost::proto::child_c<0>(a1));
      w.resize(of_size(height(u), 1));
      NT2_LAPACK_VERIFY(nt2::hsev_wu( boost::proto::value(u)
                                    , boost::proto::value(w)
                                    , 'L'));
      assign_swap(boost::proto::child_c<0>(a1), w);
      assign_swap(boost::proto::child_c<1>(a1), u);
    }

#define MORE_OPTIONS(PARAM, OPT1, OPT2)        \
    BOOST_FORCEINLINE                          \
      void eval##PARAM( A0& a0, A1& a1         \
                      , nt2::policy<ext::OPT1> \
                      , nt2::policy<ext::OPT2> \
                      ) const                  \
    {                                          \
      eval##PARAM(a0, a1                       \
                 , nt2::policy<ext::OPT2>()    \
                 , nt2::policy<ext::OPT1>()    \
                 );                            \
    }                                          \
    /**/

    MORE_OPTIONS(2_3, lower_, raw_)
    MORE_OPTIONS(2_3, upper_, raw_)
    MORE_OPTIONS(2_3, lower_, vector_)
    MORE_OPTIONS(2_3, upper_, vector_)
    MORE_OPTIONS(2_3, lower_, matrix_)
    MORE_OPTIONS(2_3, upper_, matrix_)
    MORE_OPTIONS(1_3, lower_, vector_)
    MORE_OPTIONS(1_3, upper_, vector_)
    MORE_OPTIONS(1_3, lower_, raw_)
    MORE_OPTIONS(1_3, upper_, raw_)
    MORE_OPTIONS(1_3, lower_, matrix_)
    MORE_OPTIONS(1_3, upper_, matrix_)

#undef MORE_OPTIONS
  };
} }

#endif


/****
   Copyright 2005-2007, Moshe Looks

   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 "knobs.h"
#include <boost/iterator/counting_iterator.hpp>

namespace local {
  int knob_count(kpre_it ks) {
    return ks->size()+accumulate
      (make_transform_iterator(make_counting_iterator(ks.begin()),knob_count),
       make_transform_iterator(make_counting_iterator(ks.end()),knob_count),0);
  }
} //~namespace local


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

#ifndef CAMERA_HPP
#define CAMERA_HPP

#include <Eigen/Dense>
#include <memory>

class Trackball;

namespace GLviz
{

class Frustum
{

public:
    float& left() { return m_left; }
    float& right() { return m_right; }

    float& bottom() { return m_bottom; }
    float& top() { return m_top; }

    float& near_() { return m_near; }
    float& far_() { return m_far; }

private:
    float m_left, m_right;
    float m_bottom, m_top;
    float m_near, m_far;
};

class Camera
{

public:
    Camera();
    virtual ~Camera();

    Eigen::Matrix4f const& get_modelview_matrix() const;
    Eigen::Matrix4f const& get_projection_matrix() const;
    Frustum const& get_frustum() const;

    void set_frustum(Frustum const& frustum);
    void set_aspect(float aspect);
    void set_perspective(float fovy, float aspect, float near_,
        float far_);

    void set_position(Eigen::Vector3f const& position);

    void set_orientation(Eigen::Matrix3f const& orientation);
    void set_orientation(Eigen::Quaternionf const& orientation);

    void rotate(Eigen::Quaternionf const& rotation);
    void rotate(Eigen::Matrix3f const& rotation);

    void translate(Eigen::Vector3f const& translation);

    void trackball_begin_motion(float begin_x, float begin_y);

    void trackball_end_motion_rotate(float end_x, float end_y);
    void trackball_end_motion_zoom(float end_x, float end_y);
    void trackball_end_motion_translate(float end_x, float end_y);

private:
    void set_projection_matrix_from_frustum();
    void set_modelview_matrix_from_orientation();

private:
    Eigen::Vector3f m_position;
    Eigen::Quaternionf m_orientation;

    Eigen::Matrix4f m_modelview_matrix;
    Eigen::Matrix4f m_projection_matrix;

    Frustum m_frustum;
    float m_fovy_rad;
    float m_aspect;

    float m_begin_x, m_begin_y;

    std::unique_ptr<Trackball> m_trackball;
};

}

#endif // CAMERA_HPP


/**
 * @file restoration_plan.hpp
 * @author François Hamonic (francois.hamonic@gmail.com)
 * @brief Restorationplan class declaration
 * @version 0.3
 * @date 2021-07-27
 *
 * @copyright Copyright (c) 2021
 */
#ifndef RESTORATION_PLAN_HPP
#define RESTORATION_PLAN_HPP

#include <cassert>
#include <list>
#include <map>
#include <memory>
#include <numeric>

#include "landscape/concept/abstract_landscape.hpp"

#include <boost/container/small_vector.hpp>
#include <boost/range/algorithm/find_if.hpp>

#include <range/v3/view/iota.hpp>
#include <range/v3/view/transform.hpp>

/**
 * @brief A generic class for respresenting a landscape restoration plan.
 *
 * This class represents a restoration plan wich is theorically a set of
 * restoration options. Each restoration option have a cost and a set of nodes
 * and arcs that could be enhanced alongside with informations about their
 * weights enhancements. In this class an Option is represented by an int, a
 * vector maps Otions to their costs and each Node/Arc is associated with the
 * list of pairs (option, weight_enhancement) concerning it.
 */
template <typename LS>  // requires concepts::IsLandscape<LS> //c++20
class RestorationPlan {
public:
    using Graph = typename LS::Graph;
    using Node = typename LS::Node;
    using Arc = typename LS::Arc;

    using Option = int;

    struct NodeRestorationElement {
        int id;
        Option option;
        double quality_gain;
        NodeRestorationElement(int i, Option o, double q)
            : id(i), option(o), quality_gain(q) {}
    };
    struct ArcRestorationElement {
        int id;
        Option option;
        double restored_probability;
        ArcRestorationElement(int i, Option o, double p)
            : id(i), option(o), restored_probability(p) {}
    };

    using NodeRestorationsList =
        boost::container::small_vector<NodeRestorationElement, 1>;
    using ArcRestorationsList =
        boost::container::small_vector<ArcRestorationElement, 1>;

    using NodeEnhancements = std::vector<std::pair<Node, double>>;
    using ArcEnhancements = std::vector<std::pair<Arc, double>>;

    using NodeOptionsMap = std::vector<NodeEnhancements>;
    using ArcOptionsMap = std::vector<ArcEnhancements>;

private:
    const LS & _landscape;

    typename Graph::template NodeMap<NodeRestorationsList>
        _qualityRestorationOptions;
    typename Graph::template ArcMap<ArcRestorationsList>
        _probabilityRestorationOptions;

    std::vector<double> _costs;

public:
    RestorationPlan(const LS & l)
        : _landscape(l)
        , _qualityRestorationOptions(l.getNetwork())
        , _probabilityRestorationOptions(l.getNetwork()) {}
    RestorationPlan(const RestorationPlan<LS> & rp)
        : _landscape(rp.getLandscape())
        , _qualityRestorationOptions(_landscape.getNetwork())
        , _probabilityRestorationOptions(_landscape.getNetwork()) {
        throw std::runtime_error(
            "RestorationPlan : non implement copy constructors");
    }
    RestorationPlan(RestorationPlan<LS> && rp)
        : _landscape(rp.getLandscape())
        , _qualityRestorationOptions(_landscape.getNetwork())
        , _probabilityRestorationOptions(_landscape.getNetwork()) {
        throw std::runtime_error(
            "RestorationPlan : non implement copy constructors");
    }
    ~RestorationPlan() {}
    RestorationPlan<LS> & operator=(const RestorationPlan<LS> &) {
        throw std::runtime_error(
            "RestorationPlan : non implement copy constructors");
        return *this;
    };
    RestorationPlan<LS> & operator=(RestorationPlan<LS> &&) {
        throw std::runtime_error(
            "RestorationPlan : non implement copy constructors");
        return *this;
    };

    const LS & getLandscape() const noexcept { return _landscape; }

    /**
     * @brief add an option of cost **c** and returns its id
     * @param c cost
     * @return Option
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    Option addOption(double c) noexcept {
        _costs.push_back(c);
        return _costs.size() - 1;
    }

    /**
     * @brief Get the number of options
     * @return int
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    int getNbOptions() const noexcept { return _costs.size(); }

    /**
     * @brief Return true if the restoration plan contains an
     *      option of id **i**
     * @param i - Option
     * @return bool
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    bool contains(Option i) const noexcept {
        return i >= 0 && i < getNbOptions();
    }

    /**
     * @brief Set the cost of option **i**
     * @param i Option
     * @param c cost
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    void setCost(Option i, double cost) noexcept { _costs[i] = cost; }

    /**
     * @brief Get the cost of option **i**
     * @param i Option
     * @return double
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    double getCost(Option i) const noexcept { return _costs[i]; }

    /**
     * Add a restoration enhancement for the node **u** within option **i**.
     * If there is already an enhancement for the node **u** and option **i**,
     * **quality_gain** is added to the enhancement quality gain.
     *
     * @brief Add the node **v** to the option **i**
     * @param i Option
     * @param v Node
     * @param quality_gain - quality gain on **v**
     * @time \f$O(\#options(v))\f$
     * @space \f$O(1)\f$
     */
    void addNode(Option i, Node v, double quality_gain) noexcept {
        assert(contains(i));
        auto & restoration_options = _qualityRestorationOptions[v];
        auto it = boost::find_if(restoration_options,
                                 [i](const auto & p) { return p.option == i; });
        if(it != restoration_options.end()) {
            it->quality_gain += quality_gain;
            return;
        }
        restoration_options.emplace_back(-1, i, quality_gain);
    }

    /**
     * @brief Return true if the option **i** concerns node **v**
     * @param i Option
     * @param v Node
     * @return bool true if the option **i** concerns node **v**
     *      false otherwise
     * @time \f$O(\#options(v))\f$
     * @space \f$O(1)\f$
     */
    bool contains(Option i, Node v) const noexcept {
        assert(contains(i));
        auto & restoration_options = _qualityRestorationOptions[v];
        return std::any_of(restoration_options.begin(),
                           restoration_options.end(),
                           [i](const auto & p) { return p.option == i; });
    }

    /**
     * @brief Remove the node **v** from the option **i**
     * @param i Option
     * @param v Node
     * @time \f$O(\#options(v))\f$
     * @space \f$O(1)\f$
     */
    void removeNode(Option i, Node v) noexcept {
        assert(contains(i));
        auto & restoration_options = _qualityRestorationOptions[v];
        auto it = boost::find_if(restoration_options,
                                 [i](const auto & p) { return p.option == i; });
        if(it == restoration_options.end()) return;
        restoration_options.erase(it);
    }

    /**
     * @brief Test if there is an option concerning the node **v**
     * @param v Node
     * @return bool true if the option **i** concerns node **v**
     *      false otherwise
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    bool contains(Node v) const noexcept {
        return _qualityRestorationOptions[v].size() > 0;
    }

    /**
     * @brief Remove the node **v** from every option
     * @param v Node
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    void removeNode(Node v) noexcept { _qualityRestorationOptions[v].clear(); }

    /**
     * Add a restoration enhancement for the arc **a** within option **i**.
     * If there is already an enhancement for the arc **a** and option **i**,
     * the restored probability is updated to keep the maximum value.
     *
     * @brief Add the arc **a** to the option **i**
     * @param i Option
     * @param a Arc
     * @param restored_probability - restored probability of **a**
     * @time \f$O(\#options(a))\f$
     * @space \f$O(1)\f$
     */
    void addArc(Option i, Arc a, double restored_probability) noexcept {
        assert(contains(i));
        auto & restoration_options = _probabilityRestorationOptions[a];
        auto it = boost::find_if(restoration_options,
                                 [i](const auto & p) { return p.option == i; });
        if(it != restoration_options.end()) {
            it->restored_probability =
                std::max(restored_probability, it->restored_probability);
            return;
        }
        restoration_options.emplace_back(-1, i, restored_probability);
    }

    /**
     * @brief Return true if the option **i** concerns arc **a**
     * @param i Option
     * @param a Arc
     * @return bool true if the option **i** concerns arc **a** false otherwise
     * @time \f$O(\#options(a))\f$
     * @space \f$O(1)\f$
     */
    bool contains(Option i, Arc a) const noexcept {
        assert(contains(i));
        auto & restoration_options = _probabilityRestorationOptions[a];
        return std::any_of(restoration_options.begin(),
                           restoration_options.end(),
                           [i](const auto & p) { return p.option == i; });
    }

    /**
     * @brief Remove the arc **a** from the option **i**
     * @param i Option
     * @param a Arc
     * @time \f$O(\#options(a))\f$
     * @space \f$O(1)\f$
     */
    void removeArc(Option i, Arc a) noexcept {
        assert(contains(i));
        auto & restoration_options = _probabilityRestorationOptions[a];
        auto it = boost::find_if(restoration_options,
                                 [i](const auto & p) { return p.option == i; });
        if(it == restoration_options.end()) return;
        restoration_options.erase(it);
    }

    /**
     * @brief Test if there is an option concerning the arc **a**
     * @param a Arc
     * @return bool true if the option **i** concerns arc **a** false otherwise
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    bool contains(Arc a) const noexcept {
        return _probabilityRestorationOptions[a].size() > 0;
    }

    /**
     * @brief Remove the arc **a** from every option
     * @param a Arc
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    void removeArc(Arc a) noexcept {
        _probabilityRestorationOptions[a].clear();
    }

    /**
     * @brief Get the number of possible actions concerning nodes
     * @return int
     * @time \f$O(|V|)\f$
     * @space \f$O(1)\f$
     */
    int getNbNodeRestorationElements() const noexcept {
        const Graph & graph = _landscape.getNetwork();
        int count = 0;
        for(typename Graph::NodeIt u(graph); u != lemon::INVALID; ++u)
            count += _qualityRestorationOptions[u].size();
        return count;
    }

    /**
     * @brief Get the number of possible actions concerning nodes
     * @return int
     * @time \f$O(|A|)\f$
     * @space \f$O(1)\f$
     */
    int getNbArcRestorationElements() const noexcept {
        const Graph & graph = _landscape.getNetwork();
        int count = 0;
        for(typename Graph::ArcIt a(graph); a != lemon::INVALID; ++a)
            count += _probabilityRestorationOptions[a].size();
        return count;
    }

    /**
     * @brief Get the number of nodes and arcs concerned by at least one option
     * @return int
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    int getNbElements() const noexcept {
        return getNbNodeRestorationElements() + getNbArcRestorationElements();
    }

    /**
     * @brief Initializes nodes restoration elements ids.
     * @time \f$O(|V| * \#options)\f$
     * @space \f$O(1)\f$
     */
    void initNodeElementIDs() {
        const Graph & graph = _landscape.getNetwork();
        int count = 0;
        for(typename Graph::NodeIt u(graph); u != lemon::INVALID; ++u)
            for(auto & restoration_element : _qualityRestorationOptions[u])
                restoration_element.id = count++;
    }

    /**
     * @brief Initializes arcs restoration elements ids.
     * @time \f$O(|A| * \#options)\f$
     * @space \f$O(1)\f$
     */
    void initArcElementIDs() {
        const Graph & graph = _landscape.getNetwork();
        int count = 0;
        for(typename Graph::ArcIt a(graph); a != lemon::INVALID; ++a)
            for(auto & restoration_element : _probabilityRestorationOptions[a])
                restoration_element.id = count++;
    }

    /**
     * @brief Initializes restoration elements ids.
     * @time \f$O((|V| + |A|) * \#options)\f$
     * @space \f$O(1)\f$
     */
    void initElementIDs() {
        initNodeElementIDs();
        initArcElementIDs();
    }

    /**
     * @brief
     * @return NodeOptionsMap
     * @time \f$O(|V| * \#options)\f$
     * @space \f$O(|V| * \#options)\f$
     */
    NodeOptionsMap computeNodeOptionsMap() const {
        NodeOptionsMap nodeOptionsMap(getNbOptions());
        for(typename Graph::NodeIt v(_landscape.getNetwork());
            v != lemon::INVALID; ++v)
            for(auto const & e : _qualityRestorationOptions[v])
                nodeOptionsMap[e.option].emplace_back(v, e.quality_gain);
        return nodeOptionsMap;
    }

    /**
     * @brief
     * @return ArcOptionsMap
     * @time \f$O(|A| * \#options)\f$
     * @space \f$O(|A| * \#options)\f$
     */
    ArcOptionsMap computeArcOptionsMap() const {
        ArcOptionsMap arcOptionsMap(getNbOptions());
        for(typename Graph::ArcIt a(_landscape.getNetwork());
            a != lemon::INVALID; ++a)
            for(auto const & e : _probabilityRestorationOptions[a])
                arcOptionsMap[e.option].emplace_back(a, e.restored_probability);
        return arcOptionsMap;
    }

    /**
     * @brief Returns the restoration options concerning the node **u**
     * @param u Node
     * @return NodeRestorationsList
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    const NodeRestorationsList & operator[](Node u) const noexcept {
        return _qualityRestorationOptions[u];
    }

    /**
     * @brief Returns the restoration options concerning the arc **a**
     * @param a Arc
     * @return ArcRestorationsList
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    const ArcRestorationsList & operator[](Arc a) const noexcept {
        return _probabilityRestorationOptions[a];
    }

    /**
     * @brief Returns the restoration options concerning the node **u**
     * @param u Node
     * @return NodeRestorationsList
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    NodeRestorationsList & operator[](Node u) noexcept {
        return _qualityRestorationOptions[u];
    }

    /**
     * @brief Returns the restoration options concerning the arc **a**
     * @param a Arc
     * @return ArcRestorationsList
     * @time \f$O(1)\f$
     * @space \f$O(1)\f$
     */
    ArcRestorationsList & operator[](Arc a) noexcept {
        return _probabilityRestorationOptions[a];
    }

    /**
     * @brief Computes the total cost of the restoration plan, i.e. sum of
     * options costs
     * @return double
     */
    double totalCost() const noexcept {
        return std::accumulate(_costs.begin(), _costs.end(), 0.0);
    }

    /**
     * @brief Return a view for iterating over the option objects
     * @return auto
     */
    auto options() const {
        // auto view = ranges::views::transform(
        //     ranges::iota_view<int, int>(0, static_cast<int>(getNbOptions())),
        //     [](int constr_id) { return Option(constr_id); });
        auto view = ranges::iota_view<int, int>(0, static_cast<int>(getNbOptions()));
        return view;
    }
};

template <typename LS>  // requires concepts::IsLandscape<LS> //c++20
std::ostream & operator<<(std::ostream & in, const RestorationPlan<LS> & plan);

#endif  // RESTORATION_PLAN_2_HPP


/*
 * Copyright (c) 2013 Juniper Networks, Inc. All rights reserved.
 */
#include "net/address_util.h"

#include <boost/algorithm/string.hpp>
#include <boost/asio/io_service.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/foreach.hpp>

/* Returns true if the given IPv4 address is member of the IPv4 subnet
 * indicated by IPv4 address and prefix length. Otherwise returns false.
 * We convert both the input IPv4 addresses to their subnet addresses and
 * then compare with each other to conclude whether the IPv4 address is
 * member of the subnet or not.
 */
bool IsIp4SubnetMember(
    const Ip4Address &ip, const Ip4Address &prefix_ip, uint16_t plen) {
    Ip4Address prefix = Address::GetIp4SubnetAddress(prefix_ip, plen);
    return ((prefix.to_ulong() | ~(0xFFFFFFFF << (32 - plen))) ==
            (ip.to_ulong() | ~(0xFFFFFFFF << (32 - plen))));
}

/* Returns true if the given IPv6 address is member of the IPv6 subnet
 * indicated by IPv6 address and prefix length. Otherwise returns false.
 * We do byte by byte comparison of IPv6 subnet address with bitwise AND of
 * IPv6 address and subnet address to decide whether an IPv6 address belongs to
 * its subnet.
 */
bool IsIp6SubnetMember(
    const Ip6Address &ip, const Ip6Address &subnet, uint8_t plen) {
    Ip6Address mask = Address::GetIp6SubnetAddress(subnet, plen);
    for (int i = 0; i < 16; ++i) {
        if ((ip.to_bytes()[i] & mask.to_bytes()[i]) != mask.to_bytes()[i])
            return false;
    }
    return true;
}

Ip4Address GetIp4SubnetBroadcastAddress(
              const Ip4Address &ip_prefix, uint16_t plen) {
    Ip4Address subnet(ip_prefix.to_ulong() | ~(0xFFFFFFFF << (32 - plen)));
    return subnet;
}

// Validate IPv4/IPv6 address string.
bool ValidateIPAddressString(std::string ip_address_str,
                             std::string *error_msg) {
    boost::system::error_code error;
    boost::asio::ip::address::from_string(ip_address_str, error);
    if (error) {
        std::ostringstream out;
        out << "Invalid IP address: " << ip_address_str << std::endl;
        *error_msg = out.str();
        return false;
    }

    return true;
}

IpAddress PrefixToIpNetmask(uint32_t prefix_len) {
    uint32_t mask;

    if (prefix_len == 0) {
        mask = 0;
    } else {
        mask = (~((1 << (32 - prefix_len)) - 1));
    }
    return IpAddress(Ip4Address(mask));
}

uint32_t NetmaskToPrefix(uint32_t netmask) {
    uint32_t count = 0;

    while (netmask) {
        count++;
        netmask = (netmask - 1) & netmask;
    }
    return count;
}

// Validate a list of <ip-address>:<port> endpoints.
bool ValidateServerEndpoints(std::vector<std::string> list,
                             std::string *error_msg) {
    std::ostringstream out;

    BOOST_FOREACH(std::string endpoint, list) {
        std::vector<std::string> tokens;
        boost::split(tokens, endpoint, boost::is_any_of(":"));
        if (tokens.size() != 2) {
            out << "Invalid endpoint " << endpoint << std::endl;
            *error_msg = out.str();
            return false;
        }

        boost::system::error_code error;
        boost::asio::ip::address::from_string(tokens[0], error);

        if (error) {
            out << "Invalid IP address: " << tokens[0] << std::endl;
            *error_msg = out.str();
            return false;
        }

        unsigned long port = strtoul(tokens[1].c_str(), NULL, 0);
        if (errno || port > 0xffFF) {
            out << "Invalid port : " << tokens[1];
            if (errno) {
                out << " " << strerror(errno) << std::endl;
            }
            *error_msg = out.str();
            return false;
        }
    }

    return true;
}

// Return IP address string for a host if it is resolvable, empty string
// otherwise.
std::string GetHostIp(boost::asio::io_service *io_service,
                      const std::string &hostname) {
    boost::asio::ip::tcp::resolver::iterator iter, end;
    boost::system::error_code error;
    boost::asio::ip::tcp::resolver resolver(*io_service);
    boost::asio::ip::tcp::resolver::query query(hostname, "");

    iter = resolver.resolve(query, error);
    return iter != end ? iter->endpoint().address().to_string() : "";
}

//
// Get VN name from routing instance
//
std::string GetVNFromRoutingInstance(const std::string &vn) {
    std::vector<std::string> tokens;
    boost::split(tokens, vn, boost::is_any_of(":"), boost::token_compress_on);
    if (tokens.size() < 3) return "";
    return tokens[0] + ":" + tokens[1] + ":" + tokens[2];
}


//=============================================================================
//
//  CLASS NProblemInterface
//
//=============================================================================

#ifndef COMISO_NPROBLEMINTERFACE_HH
#define COMISO_NPROBLEMINTERFACE_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================

#include <Base/Code/Quality.hh>
#include <iostream>
#include <cfloat>

LOW_CODE_QUALITY_SECTION_BEGIN
#include <Eigen/Eigen>
#if !(EIGEN_VERSION_AT_LEAST(3,1,0))
  #define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#endif
#include <Eigen/Sparse>
LOW_CODE_QUALITY_SECTION_END

#include <CoMISo/Config/CoMISoDefines.hh>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemInterface NProblemGmmInterface.hh <COMISO/NSolver/NPRoblemInterface.hh>

    Brief Description.

    A more elaborate description follows.
*/
class COMISODLLEXPORT NProblemInterface
{
public:

  // Sparse Matrix Type
 #if EIGEN_VERSION_AT_LEAST(3,1,0)
  typedef Eigen::SparseMatrix<double,Eigen::ColMajor> SMatrixNP;
 #else
  typedef Eigen::DynamicSparseMatrix<double,Eigen::ColMajor> SMatrixNP;
 #endif

  /// Default constructor
  NProblemInterface();

  /// Destructor
  virtual ~NProblemInterface();

  // problem definition
  virtual int    n_unknowns   (                                ) = 0;
  virtual void   initial_x    (       double* _x               ) = 0;
  virtual double eval_f       ( const double* _x               ) = 0;
  virtual void   eval_gradient( const double* _x, double*    _g) = 0;
  virtual void   eval_hessian ( const double* _x, SMatrixNP& _H) = 0;
  virtual void   store_result ( const double* _x               ) = 0;

  // advanced properties
  virtual bool   constant_gradient ()                                    const { return false; }
  virtual bool   constant_hessian  ()                                    const { return false; }
  virtual double max_feasible_step ( const double* _x, const double* _v)       { return DBL_MAX; }
};

//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================
#endif // COMISO_NPROBLEMGMMINTERFACE_HH defined
//=============================================================================


/**
 * \file
 * \brief Base driver for a generic n-joint BLMC robot.
 * \copyright Copyright (c) 2019, New York University and Max Planck
 *            Gesellschaft.
 */

#pragma once

#include <array>
#include <cmath>
#include <iterator>
#include <string>

#include <yaml-cpp/yaml.h>
#include <Eigen/Eigen>

#include <robot_interfaces/monitored_robot_driver.hpp>
#include <robot_interfaces/n_joint_robot_types.hpp>
#include <robot_interfaces/robot_driver.hpp>
#include <yaml_utils/yaml_eigen.hpp>

#include <blmc_drivers/blmc_joint_module.hpp>
#include <robot_fingers/clamp.hpp>

namespace robot_fingers
{
/**
 * @brief Parameters related to the motor.
 */
struct MotorParameters
{
    //! @brief Torque constant K_t of the motor [Nm/A].
    double torque_constant_NmpA;

    /**
     * @brief Gear ratio between motor and joint.
     *
     * For a `n:1` ratio (i.e. one joint revolution requires n motor
     * revolutions) set this value to `n`.
     */
    double gear_ratio;
};

/**
 * @brief Base class for n-joint BLMC robots.
 *
 * This is a generic base class to easily implement drivers for simple BLMC
 * robots that consist of N_JOINTS joints.
 *
 * @tparam N_JOINTS Number of joints.
 * @tparam N_MOTOR_BOARDS Number of motor control boards that are used.
 */
template <typename Observation, size_t N_JOINTS, size_t N_MOTOR_BOARDS>
class NJointBlmcRobotDriver
    : public robot_interfaces::RobotDriver<
          typename robot_interfaces::NJointAction<N_JOINTS>,
          Observation>
{
public:
    static constexpr size_t num_joints = N_JOINTS;
    static constexpr size_t num_motor_boards = N_MOTOR_BOARDS;

    typedef typename robot_interfaces::NJointAction<N_JOINTS> Action;
    typedef typename robot_interfaces::RobotInterfaceTypes<Action, Observation>
        Types;

    typedef typename Action::Vector Vector;
    typedef std::array<std::shared_ptr<blmc_drivers::MotorInterface>, N_JOINTS>
        Motors;
    typedef std::array<std::shared_ptr<blmc_drivers::CanBusMotorBoard>,
                       N_MOTOR_BOARDS>
        MotorBoards;

    struct Config;  // actual declaration see below

    /**
     * @brief True if the joints have mechanical end stops, false if not.
     *
     * If set to true, it is assumed that all joints of the robot have
     * end stops that mechanically prevent them from moving out of the valid
     * range.
     *
     * If present, the end stops are used for a fully automated homing procedure
     * in which the joints first move until they hit the end stop before
     * starting the encoder index search.  This way it is ensured that the
     * correct index is used for homing without any need for manual set up.
     */
    const bool has_endstop_;

    NJointBlmcRobotDriver(const MotorBoards &motor_boards,
                          const Motors &motors,
                          const MotorParameters &motor_parameters,
                          const Config &config)
        : robot_interfaces::RobotDriver<Action, Observation>(),
          has_endstop_(config.has_endstop),
          joint_modules_(motors,
                         motor_parameters.torque_constant_NmpA * Vector::Ones(),
                         motor_parameters.gear_ratio * Vector::Ones(),
                         Vector::Zero(),
                         config.max_current_A * Vector::Ones()),
          motor_boards_(motor_boards),
          motor_parameters_(motor_parameters),
          max_torque_Nm_(config.max_current_A *
                         motor_parameters.torque_constant_NmpA *
                         motor_parameters.gear_ratio),
          config_(config)
    {
        pause_motors();
    }

    static MotorBoards create_motor_boards(
        const std::array<std::string, N_MOTOR_BOARDS> &can_ports);

    Vector get_max_torques() const
    {
        return max_torque_Nm_ * Vector::Ones();
    }

    void pause_motors();

    Vector get_measured_index_angles() const;

    /**
     * @brief Find home position of all joints and move to start position.
     *
     * Homes all joints using home_on_index_after_negative_end_stop.  When
     * finished, move the joint to the starting position (defined in
     * `config_.initial_position_rad`).
     *
     */
    void initialize() override;

    virtual Observation get_latest_observation() override = 0;
    Action apply_action(const Action &desired_action) override;
    std::string get_error() override;
    void shutdown() override;

    /**
     * @brief Process the desired action provided by the user.
     *
     * Takes the desired action from the user and does the following processing:
     *
     * ## 1. Check position limits
     *
     *   If the observed position of a joint exceeds the limits, actions for
     *   that joint that do not point back towards the allowed range are
     *   replaced with a position command to the limit value.  Further custom
     *   PD-gains are ignored in this case.
     *
     * ## 2. Run the position controller in case a target position is set.
     *
     *   If the target position is set to a value unequal to NaN for any joint,
     *   a PD position controller is executed for this joint and the resulting
     *   torque command is added to the torque command in the action.
     *
     *   If the P- and/or D-gains are set to a non-NaN value in the action, they
     *   are used for the control.  NaN-values are replaced with the default
     *   gains.
     *
     * ## 3. Apply safety checks.
     *
     *   - Limit the torque to the allowed maximum value.
     *   - Dampen velocity using the given safety_kd gains.  Damping us done
     *     joint-wise using this equation:
     *
     *         torque_damped = torque_desired - safety_kd * current_velocity
     *
     * The resulting action with modifications of all steps is returned.
     *
     * @param desired_action  Desired action given by the user.
     * @param latest_observation  Latest observation from the robot.
     * @param max_torque_Nm  Maximum allowed absolute torque.
     * @param safety_kd  D-gain for velocity damping.
     * @param default_position_control_kp  Default P-gain for position control.
     * @param default_position_control_kd  Default D-gain for position control.
     * @param lower_position_limits  Lower limits for joint positions.
     * @param upper_position_limits  Upper limits for joint positions.
     *
     * @return Resulting action after applying all the processing.
     */
    static Action process_desired_action(
        const Action &desired_action,
        const Observation &latest_observation,
        const double max_torque_Nm,
        const Vector &safety_kd,
        const Vector &default_position_control_kp,
        const Vector &default_position_control_kd,
        const Vector &lower_position_limits =
            Vector::Constant(-std::numeric_limits<double>::infinity()),
        const Vector &upper_position_limits =
            Vector::Constant(std::numeric_limits<double>::infinity()));

    /**
     * @brief Check if the joint position is within the hard limits.
     *
     * @see Config::is_within_hard_position_limits
     */
    bool is_within_hard_position_limits(const Observation &observation) const;

protected:
    blmc_drivers::BlmcJointModules<N_JOINTS> joint_modules_;
    MotorBoards motor_boards_;

    //! \brief Fixed motor parameters (assuming all joints use same setup).
    MotorParameters motor_parameters_;

    //! \brief Maximum torque allowed on each joint.
    double max_torque_Nm_;

    /**
     * \brief User-defined configuration of the driver
     *
     * Contains all configuration values that can be modified by the user (via
     * the configuration file).
     */
    Config config_;

    bool is_initialized_ = false;

    //! \brief Counter for the number of actions sent to the robot.
    uint32_t action_counter_ = 0;

    Action apply_action_uninitialized(const Action &desired_action);

    //! \brief Actual initialization that is called in a real-time thread in
    //!        initialize().
    void _initialize();

    /**
     * @brief Move with constant torque until all joints are blocking.
     *
     * Applies a constant torque until all joints are reporting a velocity close
     * to zero.  This can be used to move against the end-stops.
     *
     * @param torques_Nm Torques that are applied to the joints.
     */
    void move_until_blocking(Vector torques_Nm);

    /**
     * @brief Homing of all joints, based on the robot configuration.
     *
     * Procedure for finding an absolute zero position when using relative
     * encoders.
     *
     * The method for finding the home position is depending on the
     * "homing_method" setting in the configuration.  See @ref
     * Config::HomingMethod for the different options.
     *
     * By default, the zero position after homing is the same as the home
     * position.  The optional configuration parameter @ref
     * Config::home_offset_rad provides a means to move the zero position
     * relative to the home position.  The zero position is
     * computed as
     *
     *     zero position = home position + home offset
     *
     * @returns True if the homing was successful, false if not.  In case of a
     *     failure, an error message with more information is printed to stdout.
     */
    bool homing();

    /**
     * @brief Move to given goal position with a minimum jerk trajectory.
     *
     * Use a series of position actions to move to the given goal position on a
     * minimum jerk trajectory.
     *
     * @param goal_pos Angular goal position for each joint.
     * @param tolerance Allowed position error for reaching the goal.  This is
     *     checked per joint, that is the maximal possible error is +/-tolerance
     *     on each joint.
     * @param time_steps Number of control loop cycles for reaching the goal.
     *     The lower the number of steps, the faster the robot will move.
     * @return True if goal position is reached, false if timeout is exceeded.
     */
    bool move_to_position(const Vector &goal_pos,
                          const double tolerance,
                          const uint32_t time_steps);
};

/**
 * @brief Configuration of the robot that can be changed by the user.
 */
template <typename Observation, size_t N_JOINTS, size_t N_MOTOR_BOARDS>
struct NJointBlmcRobotDriver<Observation, N_JOINTS, N_MOTOR_BOARDS>::Config
{
    typedef std::array<std::string, N_MOTOR_BOARDS> CanPortArray;

    //! @brief A sub-goal of a trajectory.
    struct TrajectoryStep
    {
        //! @brief Target position to which the joints should move.
        Vector target_position_rad = Vector::Zero();

        //! @brief Number of time steps for reaching the target position.
        uint32_t move_steps = 0;
    };

    //! @brief Different homing methods that can be selected.
    enum class HomingMethod
    {
        //! Do not perform any homing (i.e. use position as reported by the
        //! motor board).
        NONE,

        //! Home at the current position.
        CURRENT_POSITION,

        /**
         * @brief Search for next encoder index and home there.
         *
         * To determine the search direction, the signs of the values from
         * @ref endstop_search_torques_Nm are used and negated.  This means even
         * if not performing any end-stop search, you need to set some values
         * there that point in the opposite direction as the desired index
         * search.
         */
        NEXT_INDEX,

        /**
         * @brief Move to end stop and home there.
         *
         * Move using the torques defined in @ref endstop_search_torques_Nm
         * until all joints are blocking (assuming this is due to the end stop)
         * and home at this position.
         */
        ENDSTOP,

        /**
         * @brief First move to the end stop, then search for encoder index.
         *
         * First move to the end stops using the torques defined in @ref
         * endstop_search_torques_Nm.  From there search for the next encoder
         * index in opposite direction and use the index position for homing.
         */
        ENDSTOP_INDEX,

        /**
         * @brief First move to the end stop, then release motors and home.
         *
         * First move to the end stops using the torques defined in @ref
         * endstop_search_torques_Nm.  Then release the motors (i.e. send zero
         * torque commands) for a moment, so that the joints are not actively
         * pushing against the end stop anymore.  Then home at the reached
         * position.
         *
         * This requires the robot to have "stable" end-stops, i.e. if a joint
         * is at the end-stop and the torque is set to zero, it should not move
         * away on its own.
         */
        ENDSTOP_RELEASE,
    };

    // All parameters should have default values that should not result in
    // dangerous behaviour in case someone forgets to specify them.

    /**
     * @brief List of CAN port names used by the robot.
     *
     * For each motor control board used by the robot, this specifies the
     * CAN port through which it is connected.
     *
     * Example: `{"can0", "can1"}`
     */
    CanPortArray can_ports;

    //! @brief Maximum current that can be sent to the motor [A].
    double max_current_A = 0.0;

    /**
     * @brief Whether the joints have physical end stops or not.
     *
     * This is for example relevant for homing, where (in case this value
     * is set to true) all joints move until they hit the end stop to
     * determine their absolute position.
     *
     * Note that not having end stops does not mean that the joint can
     * rotate freely in general.
     */
    bool has_endstop = false;

    //! @brief Which method to use for homing.
    HomingMethod homing_method = HomingMethod::NONE;

    //! @brief Parameters related to calibration.
    struct CalibrationParameters
    {
        //! @brief Torque that is used to find the end stop.
        Vector endstop_search_torques_Nm = Vector::Zero();
        //! @brief Number of time steps for reaching the initial position.
        uint32_t move_steps = 0;
    } calibration;

    //! @brief Tolerance for reaching the target with
    //!        @ref NJointBlmcRobotDriver::move_to_position.
    double move_to_position_tolerance_rad = 0.0;

    //! @brief D-gain to dampen velocity.  Set to zero to disable damping.
    // set some rather high damping by default
    Vector safety_kd = Vector::Constant(0.1);

    //! @brief Default control gains for the position PD controller.
    struct PositionControlGains
    {
        Vector kp = Vector::Zero();
        Vector kd = Vector::Zero();
    } position_control_gains;

    /**
     * @brief Hard lower limits for joint position.
     *
     * Exceeding this limit results in an error and robot shutdown.
     */
    Vector hard_position_limits_lower = Vector::Zero();
    /**
     * @brief Hard upper limits for joint position.
     *
     * Exceeding this limit results in an error and robot shutdown.
     */
    Vector hard_position_limits_upper = Vector::Zero();

    /**
     * @brief Soft lower limits for joint position.
     *
     * Exceeding this limit results in the action being adjusted to move the
     * joint back inside the limits.
     */
    Vector soft_position_limits_lower =
        Vector::Constant(-std::numeric_limits<double>::infinity());
    /**
     * @brief Soft upper limits for joint position.
     *
     * Exceeding this limit results in the action being adjusted to move the
     * joint back inside the limits.
     */
    Vector soft_position_limits_upper =
        Vector::Constant(std::numeric_limits<double>::infinity());

    //! @brief Offset between home position and zero.
    Vector home_offset_rad = Vector::Zero();

    /**
     * @brief Initial position to which the robot moves after
     *        initialization.
     */
    Vector initial_position_rad = Vector::Zero();

    /**
     * @brief Trajectory which is executed in the shutdown method.
     *
     * Use this to move the robot to a "rest position" during shutdown of the
     * robot driver.  It can consist of arbitrarily many steps.  Leave it empty
     * to not move during shutdown.
     */
    std::vector<TrajectoryStep> shutdown_trajectory;

    /**
     * @brief List of file to which run duration logs are written.
     *
     * You can specify multiple files here if you want to log the runtime of
     * different independent components separately.  For example on a robot with
     * multiple manipulators, you can have a separate log for each manipulator,
     * so if one of them is replaced, only the log file of this manipulator
     * needs to be changed.
     */
    std::vector<std::string> run_duration_logfiles;

    /**
     * @brief Check if the given position is within the hard limits.
     *
     * @param position Joint positions.
     *
     * @return True if `hard_position_limits_lower <= position <=
     *     hard_position_limits_upper`.
     */
    bool is_within_hard_position_limits(const Vector &position) const;

    /**
     * @brief Print the given configuration in a human-readable way.
     */
    void print() const;

    /**
     * @brief Load driver configuration from file.
     *
     * Load the configuration from the specified YAML file.  The file is
     * expected to have the same structure/key naming as the Config struct.
     * If a value can not be read from the file, the application exists
     * with an error message.
     *
     * @param config_file_name  Path/name of the configuration YAML file.
     *
     * @return Configuration
     */
    static Config load_config(const std::string &config_file_name);

    /**
     * @brief Parse a homing method name.
     *
     * @param method_name  Homing method name.
     * @throws std::invalid_argument if the given string does not represent a
     *     valid homing method.
     *
     * @return The corresponding homing method.
     */
    static HomingMethod parse_homing_method_name(const std::string method_name)
    {
        if (method_name == "none")
        {
            return HomingMethod::NONE;
        }
        else if (method_name == "current_position")
        {
            return HomingMethod::CURRENT_POSITION;
        }
        else if (method_name == "next_index")
        {
            return HomingMethod::NEXT_INDEX;
        }
        else if (method_name == "endstop")
        {
            return HomingMethod::ENDSTOP;
        }
        else if (method_name == "endstop_index")
        {
            return HomingMethod::ENDSTOP_INDEX;
        }
        else if (method_name == "endstop_release")
        {
            return HomingMethod::ENDSTOP_RELEASE;
        }
        else
        {
            throw std::invalid_argument("Invalid homing method " + method_name);
        }
    }

    //! @brief Get the name of the specified homing method.
    static std::string get_homing_method_name(HomingMethod method)
    {
        switch (method)
        {
            case HomingMethod::NONE:
                return "none";
            case HomingMethod::CURRENT_POSITION:
                return "current_position";
            case HomingMethod::NEXT_INDEX:
                return "next_index";
            case HomingMethod::ENDSTOP:
                return "endstop";
            case HomingMethod::ENDSTOP_INDEX:
                return "endstop_index";
            case HomingMethod::ENDSTOP_RELEASE:
                return "endstop_release";
            default:
                throw std::runtime_error(
                    "No name for the given homing method.  This is a bug, "
                    "please report to the maintainers of this package.");
        }
    }

private:
    /**
     * \brief Set value from user configuration to var if specified.
     *
     * Checks if a field `name` exists in `user_config`.  If yes, its value
     * is written to `var`, otherwise `var` is unchanged.
     *
     * \param[in] user_config  YAML node containing the user configuration.
     * \param[in] name  Name of the configuration entry.
     * \param[out] var  Variable to which configuration is written.  Value
     * is unchanged if the specified field name does not exist in
     * user_config, i.e.  it can be initialized with a default value.
     */
    template <typename T>
    static void set_config_value(const YAML::Node &user_config,
                                 const std::string &name,
                                 T *var);
};

/**
 * @brief Simple n-joint robot driver that uses NJointObservation.
 *
 * @tparam N_JOINTS  Number of joints
 * @tparam N_MOTOR_BOARDS  Number of motor boards.
 */
template <size_t N_JOINTS, size_t N_MOTOR_BOARDS = (N_JOINTS + 1) / 2>
class SimpleNJointBlmcRobotDriver
    : public NJointBlmcRobotDriver<
          robot_interfaces::NJointObservation<N_JOINTS>,
          N_JOINTS,
          N_MOTOR_BOARDS>
{
public:
    typedef robot_interfaces::NJointObservation<N_JOINTS> Observation;

    using NJointBlmcRobotDriver<robot_interfaces::NJointObservation<N_JOINTS>,
                                N_JOINTS,
                                N_MOTOR_BOARDS>::NJointBlmcRobotDriver;

    Observation get_latest_observation() override;
};

/**
 * @brief Create backend using the specified driver.
 *
 * @tparam Driver  Type of the driver.  Expected to inherit from
 *     NJointBlmcRobotDriver.
 *
 * @param robot_data  Instance of RobotData used for communication.
 * @param config  Driver configuration.
 * @param first_action_timeout  Duration for which the backend waits for the
 *     first action to arrive.  If exceeded, the backend shuts down.
 * @param max_number_of_actions  Number of actions after which the backend
 *     automatically shuts down.
 *
 * @return A RobotBackend instances with a driver of the specified type.
 */
template <typename Driver>
typename Driver::Types::BackendPtr create_backend(
    typename Driver::Types::BaseDataPtr robot_data,
    const typename Driver::Config &config,
    const double first_action_timeout = std::numeric_limits<double>::infinity(),
    const uint32_t max_number_of_actions = 0)
{
    constexpr double MAX_ACTION_DURATION_S = 0.003;
    constexpr double MAX_INTER_ACTION_DURATION_S = 0.005;

    config.print();

    // wrap the actual robot driver directly in a MonitoredRobotDriver
    auto monitored_driver =
        std::make_shared<robot_interfaces::MonitoredRobotDriver<Driver>>(
            std::make_shared<Driver>(config),
            MAX_ACTION_DURATION_S,
            MAX_INTER_ACTION_DURATION_S);

    constexpr bool real_time_mode = true;
    auto backend = std::make_shared<typename Driver::Types::Backend>(
        monitored_driver,
        robot_data,
        real_time_mode,
        first_action_timeout,
        max_number_of_actions);
    backend->set_max_action_repetitions(std::numeric_limits<uint32_t>::max());

    return backend;
}

/**
 * @brief Create backend using the specified driver.
 *
 * Overloaded version that takes a path to a configuration YAML file instead of
 * a Config instance.
 */
template <typename Driver>
typename Driver::Types::BackendPtr create_backend(
    typename Driver::Types::BaseDataPtr robot_data,
    const std::string &config_file_path,
    const double first_action_timeout = std::numeric_limits<double>::infinity(),
    const uint32_t max_number_of_actions = 0)
{
    std::cout << "Load robot driver configuration from file '"
              << config_file_path << "'" << std::endl;
    auto config = Driver::Config::load_config(config_file_path);

    return create_backend<Driver>(
        robot_data, config, first_action_timeout, max_number_of_actions);
}

}  // namespace robot_fingers

#include "n_joint_blmc_robot_driver.hxx"