#include <iostream>
#include <string>

#include <boost/algorithm/string/split.hpp>

#include <opts/opts.h>
#include <dlog/log.h>
#include <dlog/stats.h>

#include <diy/decomposition.hpp>
#include <diy/reduce-operations.hpp>
#include <diy/io/block.hpp>

#include <reeber/format.h>

#include "reader-interfaces.h"

#include "merge-tree-block.h"
#include "persistent-integral-block.h"

typedef diy::RegularDecomposer<diy::DiscreteBounds>                 Decomposer;
typedef MergeTreeBlock::Vertex                                      Vertex;
typedef MergeTreeBlock::Value                                       Value;

bool vv_cmp(const MergeTreeNode::ValueVertex& a, const MergeTreeNode::ValueVertex& b)
{
    return a.second < b.second;
}

class TreeTracer
{
    private:
        typedef std::map<MergeTreeNode::Vertex, MinIntegral>        MinIntegralMap;
        typedef MergeTreeBlock::OffsetGrid                          OffsetGrid;
        typedef std::vector<OffsetGrid*>                            OffsetGridVector;

    public:
                    TreeTracer(const Decomposer& decomposer_, diy::Master& pi_master_,
                               Real m_, Real t_, Real e_,
                               std::vector<std::string> avg_fn_list_,
                               std::string density_fn_,
                               bool density_weighted_):
                         decomposer(decomposer_), pi_master(pi_master_),
                         m(m_), t(t_), e(e_),
                         density_reader(0), density_weighted(density_weighted_)
        {
            diy::mpi::communicator& world = pi_master_.communicator();

            for(const std::string& fn : avg_fn_list_)
                avg_var_readers.push_back(Reader::create(fn, world));

            if (!density_fn_.empty())
                density_reader = Reader::create(density_fn_, world);
        }

                    ~TreeTracer()
        {
            for(Reader* reader : avg_var_readers)
                delete reader;
            delete density_reader;
        }

        void        operator()(void* b, const diy::ReduceProxy& rp) const
        {
            MergeTreeBlock &block = *static_cast<MergeTreeBlock*>(b);

            MinIntegralMap mi_map;

            if (rp.in_link().size() == 0)
            {
                OffsetGridVector add_data;
                for(Reader* reader : avg_var_readers)
                    add_data.push_back(reader->read(block.core));
                MergeTreeBlock::OffsetGrid *density_data = density_reader ? density_reader->read(block.core) : 0;

                reeber::traverse_persistence(block.mt, Integrator(m,t,e,density_weighted, &mi_map, &block, &add_data, density_data));

                for(const MinIntegral& mi : mi_map | reeber::range::map_values)
                {
                    if (block.mt.cmp(m, mi.min_val))
                        continue;

                    if (mi.integral == 0)                                            // this block doesn't contribute anything to this extremum
                        continue;

                    int dest_gid = decomposer.point_to_gid(block.global.position(mi.min_vtx));
                    diy::BlockID dest = rp.out_link().target(dest_gid);              // out_link targets are ordered as gids
                    assert(dest.gid == dest_gid);
                    rp.enqueue(dest, mi);
                }

                for(MergeTreeBlock::OffsetGrid* og : add_data)
                    delete og;
                delete density_data;
            }
            else
            {
                for (int i = 0; i < rp.in_link().size(); ++i)
                {
                    int gid = rp.in_link().target(i).gid;
                    assert(gid == i);

                    while (rp.incoming(gid))
                    {
                        MinIntegral mi;
                        rp.dequeue(gid, mi);
                        if (mi_map.find(mi.min_vtx) == mi_map.end())
                            mi_map[mi.min_vtx] = mi;
                        else
                            mi_map[mi.min_vtx].combine(mi);
                    }
                }

                PersistentIntegralBlock *pi_block = new PersistentIntegralBlock(block);
                for (MinIntegralMap::const_iterator it = mi_map.begin(); it != mi_map.end(); ++it)
                    pi_block->add_integral(it->second);
                pi_master.add(rp.gid(), pi_block, new diy::Link);
            }
        }

        struct Integrator
        {
            Real                m,t,e;
            bool                density_weighted;
            MinIntegralMap*     mi_map_;
            MergeTreeBlock*     b;
            OffsetGridVector*   add_data_;
            OffsetGrid*         density_data;

                        Integrator(Real m_, Real t_, Real e_, bool density_weighted_,
                                   MinIntegralMap* mi_map, MergeTreeBlock* b_, OffsetGridVector* add_data, OffsetGrid* density_data_):
                            m(m_), t(t_), e(e_), density_weighted(density_weighted_),
                            mi_map_(mi_map), b(b_), add_data_(add_data), density_data(density_data_)    {}

            void        operator()(Neighbor from, Neighbor through, Neighbor to) const
            {
                MinIntegralMap& mi_map = *mi_map_;

                // contribution of the edge from -- from->parent to the integral of from
                mi_map[from->vertex].combine(integrate(from));

                // figure out the contribution of the edge through -- through->parent to the integral of to
                bool record_through = through->children.size() == 2;
                if (!record_through && from != to)    // if we have more than 2 children record through when processing the highest one (lexicographically)
                {
                    Neighbor n = 0;
                    for (size_t i = 0; i < through->children.size(); ++i)
                    {
                        Neighbor nc = through->children[i];
                        if (static_cast<Neighbor>(nc->aux) != to && nc > n)
                            n = nc;
                    }

                    if (n == 0)
                        assert(from == to);
                    else if (static_cast<Neighbor>(n->aux) == from)
                        record_through = true;
                }

                if (record_through && from != to)
                    mi_map[to->vertex].combine(integrate(through));

                // if we are not looking at the global pair, add the entire from-integral (now correct) to the integral of to
                if (from != to && !b->mt.cmp(t, through->value))
                    mi_map[to->vertex].combine(mi_map[from->vertex]);

                // from-through pair is not persistent enough, erase
                if (from->value/through->value < e)     // we should make this more generic somehow
                    mi_map.erase(from->vertex);
                else
                {
                    // fix the integral id (initialized to zero up until now)
                    MinIntegral mi(from);
                    mi.combine(mi_map[from->vertex]);
                    mi_map[from->vertex] = mi;
                }
            }

            void        integrate(MinIntegral& mi, Value val, MergeTree::Vertex vrt) const
            {
                OffsetGridVector& add_data = *add_data_;

                if (b->core.contains(vrt) && !b->mt.cmp(t, val))
                {
                    mi.integral += val * b->cell_size[0] * b->cell_size[1] * b->cell_size[2];
                    ++mi.n_cells;
                    for (size_t i = 0; i < add_data.size(); ++i)
                    {
                        Real new_val = (*add_data[i])(vrt);
                        if (density_data)
                            new_val /= (*density_data)(vrt);
                        if (density_weighted)
                            new_val *= val * b->cell_size[0] * b->cell_size[1] * b->cell_size[2];
                        mi.add_sums[i] += new_val;
                    }
                    mi.push_back(MergeTreeNode::ValueVertex(val, vrt));
                }
            }

            MinIntegral integrate(Neighbor n) const
            {
                MinIntegral mi;

                integrate(mi, n->value, n->vertex);

                for(const MergeTree::Node::ValueVertex& x : n->vertices)
                    integrate(mi, x.first, x.second);

                return mi;
            }
        };

    private:
        const Decomposer&    decomposer;
        diy::Master&         pi_master;
        Real                 m;
        Real                 t;
        Real                 e;
        std::vector<Reader*> avg_var_readers;
        Reader*              density_reader;
        bool                 density_weighted;
};

struct OutputIntegrals
{
                    OutputIntegrals(std::string outfn_, bool density_weighted_, bool verbose_):
                        outfn(outfn_), density_weighted(density_weighted_), verbose(verbose_)   {}

       void         operator()(PersistentIntegralBlock* b, const diy::Master::ProxyWithLink& cp) const
       {
           PersistentIntegralBlock&  block = *b;

           std::string   dgm_fn = fmt::format("{}-b{}.comp", outfn, block.gid);
           std::ofstream ofs(dgm_fn.c_str());

           for(MinIntegral &mi : block.persistent_integrals)
           {
               Vertex v = block.global.position(mi.min_vtx);
               ofs << v[0] * block.cell_size[0] << " " << v[1] * block.cell_size[1] << " " << v[2] * block.cell_size[2] << " ";
               if (verbose)
                   ofs << v[0] << "x" << v[1] << "x" << v[2] << " (" << mi.min_vtx << ") ";
               ofs <<  mi.integral;
               if (verbose)
                   ofs << " " << mi.n_cells;
               for(Real sum : mi.add_sums)
                   ofs << " " << sum / (density_weighted ? mi.integral : mi.n_cells);
               ofs << std::endl;
#ifdef REEBER_PERSISTENT_INTEGRAL_TRACE_VTCS
               std::sort(mi.vertices.begin(), mi.vertices.end(), vv_cmp);
               for (std::vector< MergeTreeNode::ValueVertex >::const_iterator it = mi.vertices.begin(); it != mi.vertices.end(); ++it)
                   ofs << "   " << it->second << " (" << block.global.position(it->second) <<  ")" << std::endl;
#if 1
               // Consistency check for debugging
               for (size_t i = 0; i < mi.vertices.size() - 1; ++i)
                   if (mi.vertices[i].second == mi.vertices[i+1].second)
                       LOG_SEV(fatal) << "Duplicate vertex " << mi.vertices[i].second << " in component " << mi.min_vtx;
#endif
#endif
           }
       }

    private:
        std::string outfn;
        bool        density_weighted;
        bool        verbose;
};

int main(int argc, char** argv)
{
    diy::mpi::environment   env(argc, argv);
    diy::mpi::communicator  world;
#ifdef REEBER_USE_BOXLIB_READER
    reeber::io::BoxLib::environment boxlib_env(argc, argv, world);
#endif

    using namespace opts;

    std::string prefix      = "./DIY.XXXXXX";
    int         in_memory   = -1;
    int         threads     = 1;
    int         k           = 2;
    Real        m           = 200;
    Real        t           = 82;
    Real        e           = m - t;

    std::string profile_path;
    std::string log_level = "info";
    std::string avg_fn_str = "";
    std::string density_fn = "";

    Options ops(argc, argv);
    ops
        >> Option('m', "memory",    in_memory,    "maximum blocks to store in memory")
        >> Option('j', "jobs",      threads,      "threads to use during the computation")
        >> Option('k', "k",         k,            "use k-ary swap")
        >> Option('s', "storage",   prefix,       "storage prefix")
        >> Option('p', "profile",   profile_path, "path to keep the execution profile")
        >> Option('l', "log",       log_level,    "log level")
        >> Option('x', "max",       m,            "maximum threshold")
        >> Option('i', "iso",       t,            "isofind threshold")
        >> Option('e', "epsilon",   e,            "persistence threshold")
        >> Option('f', "mean",      avg_fn_str,   "list of additionals files/variables to average separated by ','")
        >> Option('q', "quotient",  density_fn,   "divide by density in file")
    ;
    bool absolute         = ops >> Present('a', "absolute", "use absolute values for thresholds (instead of multiples of mean)");
    bool verbose          = ops >> Present('v', "verbose",  "verbose output: logical coordiantes and number of cells");
    bool density_weighted = ops >> Present('w', "weight",   "compute density-weighted averages");
    bool split            = ops >> Present(     "split",    "use split IO");

    std::string infn, outfn;
    if (  ops >> Present('h', "help", "show help message") ||
        !(ops >> PosOption(infn) >> PosOption(outfn)))
    {
        if (world.rank() == 0)
        {
            fmt::print("Usage: {} IN.lgt OUT.pi\n{}", argv[0], ops);
        }
        return 1;
    }

    std::vector<std::string> avg_fn_list;
    if (!avg_fn_str.empty())
        boost::split(avg_fn_list, avg_fn_str, std::bind1st(std::equal_to<char>(), ','));

    dlog::add_stream(std::cerr, dlog::severity(log_level))
        << dlog::stamp() << dlog::aux_reporter(world.rank()) << dlog::color_pre() << dlog::level() << dlog::color_post() >> dlog::flush();

    std::ofstream   profile_stream;
    if (profile_path == "-")
        dlog::prof.add_stream(std::cerr);
    else if (!profile_path.empty())
    {
        std::string profile_fn = fmt::format("{}-r{}.prf", profile_path, world.rank());
        profile_stream.open(profile_fn.c_str());
        dlog::prof.add_stream(profile_stream);
    }

    world.barrier();
    dlog::Timer timer;
    LOG_SEV_IF(world.rank() == 0, info) << "Starting computation";

    diy::FileStorage            storage(prefix);

    diy::Master                 mt_master(world,
                                          threads,
                                          in_memory,
                                          &MergeTreeBlock::create,
                                          &MergeTreeBlock::destroy,
                                          &storage,
                                          &MergeTreeBlock::save,
                                          &MergeTreeBlock::load);

    diy::Master                 pi_master(world,
                                          threads,
                                          in_memory);

    diy::ContiguousAssigner     assigner(world.size(), 0);

    // load the trees
    LOG_SEV_IF(world.rank() == 0, debug) << "Reading blocks from " << infn;
    if (!split)
        diy::io::read_blocks(infn, world, assigner, mt_master);
    else
        diy::io::split::read_blocks(infn, world, assigner, mt_master);
    LOG_SEV_IF(world.rank() == 0, info) << "Blocks read: " << mt_master.size();

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to read data:                    " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    // get the domain bounds from any block that's in memory (they are all the same) and set up a decomposer
    MergeTreeBlock::Box global = static_cast<MergeTreeBlock*>(((const diy::Master&) mt_master).block(mt_master.loaded_block()))->global;
    diy::DiscreteBounds domain {3};
    for (unsigned i = 0; i < 3; ++i)
    {
        domain.min[i] = global.from()[i];
        domain.max[i] = global.to()[i];
    }
    diy::RegularDecomposer<diy::DiscreteBounds>     decomposer(3, domain, assigner.nblocks(), Decomposer::BoolVector(3, true));

    // Compute average
    if (!absolute)
    {
        mt_master.foreach(&MergeTreeBlock::compute_average);
        mt_master.exchange();

        const diy::Master::ProxyWithLink& proxy = mt_master.proxy(mt_master.loaded_block());
        double mean = proxy.get<double>() / proxy.get<size_t>();
        m *= mean;
        t *= mean;

        LOG_SEV_IF(world.rank() == 0, info) << "Average value is " << mean << ". Using isofind threshold of " << t << " and maximum threshold of " << m;
    }

    // Compute and combine persistent integrals
    diy::all_to_all(mt_master, assigner, TreeTracer(decomposer, pi_master, m, t, e, avg_fn_list, density_fn, density_weighted), k);

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to compute persistent integrals: " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    // Save persistent integrals to file
    pi_master.foreach(OutputIntegrals(outfn, density_weighted, verbose));

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to output persistent integrals:  " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    dlog::prof.flush();     // TODO: this is necessary because the profile file will close before
                            //       the global dlog::prof goes out of scope and flushes the events.
                            //       Need to eventually fix this.
    dlog::stats.flush();
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/rosenbrock4.hpp

 [begin_description]
 Implementation of the Rosenbrock 4 method for solving stiff ODEs. Note, that a
 controller and a dense-output stepper exist for this method,
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/*
 * ToDo:
 *
 * 2. Interfacing for odeint, check if controlled_error_stepper can be used
 * 3. dense output
 */

template< class Value >
struct default_rosenbrock_coefficients
{
    typedef Value value_type;
    typedef unsigned short order_type;

    default_rosenbrock_coefficients( void )
    : gamma ( static_cast< value_type >( 0.25 ) ) ,
      d1 ( static_cast< value_type >( 0.25 ) ) ,
      d2 ( static_cast< value_type >( -0.1043 ) ) ,
      d3 ( static_cast< value_type >( 0.1035 ) ) ,
      d4 ( static_cast< value_type >( 0.3620000000000023e-01 ) ) ,
      c2 ( static_cast< value_type >( 0.386 ) ) ,
      c3 ( static_cast< value_type >( 0.21 ) ) ,
      c4 ( static_cast< value_type >( 0.63 ) ) ,
      c21 ( static_cast< value_type >( -0.5668800000000000e+01 ) ) ,
      a21 ( static_cast< value_type >( 0.1544000000000000e+01 ) ) ,
      c31 ( static_cast< value_type >( -0.2430093356833875e+01 ) ) ,
      c32 ( static_cast< value_type >( -0.2063599157091915e+00 ) ) ,
      a31 ( static_cast< value_type >( 0.9466785280815826e+00 ) ) ,
      a32 ( static_cast< value_type >( 0.2557011698983284e+00 ) ) ,
      c41 ( static_cast< value_type >( -0.1073529058151375e+00 ) ) ,
      c42 ( static_cast< value_type >( -0.9594562251023355e+01 ) ) ,
      c43 ( static_cast< value_type >( -0.2047028614809616e+02 ) ) ,
      a41 ( static_cast< value_type >( 0.3314825187068521e+01 ) ) ,
      a42 ( static_cast< value_type >( 0.2896124015972201e+01 ) ) ,
      a43 ( static_cast< value_type >( 0.9986419139977817e+00 ) ) ,
      c51 ( static_cast< value_type >( 0.7496443313967647e+01 ) ) ,
      c52 ( static_cast< value_type >( -0.1024680431464352e+02 ) ) ,
      c53 ( static_cast< value_type >( -0.3399990352819905e+02 ) ) ,
      c54 ( static_cast< value_type >(  0.1170890893206160e+02 ) ) ,
      a51 ( static_cast< value_type >( 0.1221224509226641e+01 ) ) ,
      a52 ( static_cast< value_type >( 0.6019134481288629e+01 ) ) ,
      a53 ( static_cast< value_type >( 0.1253708332932087e+02 ) ) ,
      a54 ( static_cast< value_type >( -0.6878860361058950e+00 ) ) ,
      c61 ( static_cast< value_type >( 0.8083246795921522e+01 ) ) ,
      c62 ( static_cast< value_type >( -0.7981132988064893e+01 ) ) ,
      c63 ( static_cast< value_type >( -0.3152159432874371e+02 ) ) ,
      c64 ( static_cast< value_type >( 0.1631930543123136e+02 ) ) ,
      c65 ( static_cast< value_type >( -0.6058818238834054e+01 ) ) ,
      d21 ( static_cast< value_type >( 0.1012623508344586e+02 ) ) ,
      d22 ( static_cast< value_type >( -0.7487995877610167e+01 ) ) ,
      d23 ( static_cast< value_type >( -0.3480091861555747e+02 ) ) ,
      d24 ( static_cast< value_type >( -0.7992771707568823e+01 ) ) ,
      d25 ( static_cast< value_type >( 0.1025137723295662e+01 ) ) ,
      d31 ( static_cast< value_type >( -0.6762803392801253e+00 ) ) ,
      d32 ( static_cast< value_type >( 0.6087714651680015e+01 ) ) ,
      d33 ( static_cast< value_type >( 0.1643084320892478e+02 ) ) ,
      d34 ( static_cast< value_type >( 0.2476722511418386e+02 ) ) ,
      d35 ( static_cast< value_type >( -0.6594389125716872e+01 ) )
    {}

    const value_type gamma;
    const value_type d1 , d2 , d3 , d4;
    const value_type c2 , c3 , c4;
    const value_type c21 ;
    const value_type a21;
    const value_type c31 , c32;
    const value_type a31 , a32;
    const value_type c41 , c42 , c43;
    const value_type a41 , a42 , a43;
    const value_type c51 , c52 , c53 , c54;
    const value_type a51 , a52 , a53 , a54;
    const value_type c61 , c62 , c63 , c64 , c65;
    const value_type d21 , d22 , d23 , d24 , d25;
    const value_type d31 , d32 , d33 , d34 , d35;

    static const order_type stepper_order = 4;
    static const order_type error_order = 3;
};

template< class Value , class Coefficients = default_rosenbrock_coefficients< Value > , class Resizer = initially_resizer >
class rosenbrock4
{
private:

public:

    typedef Value value_type;
    typedef boost::numeric::ublas::vector< value_type > state_type;
    typedef state_type deriv_type;
    typedef value_type time_type;
    typedef boost::numeric::ublas::matrix< value_type > matrix_type;
    typedef boost::numeric::ublas::permutation_matrix< size_t > pmatrix_type;
    typedef Resizer resizer_type;
    typedef Coefficients rosenbrock_coefficients;
    typedef stepper_tag stepper_category;
    typedef unsigned short order_type;

    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef state_wrapper< matrix_type > wrapped_matrix_type;
    typedef state_wrapper< pmatrix_type > wrapped_pmatrix_type;

    typedef rosenbrock4< Value , Coefficients , Resizer > stepper_type;

    const static order_type stepper_order = rosenbrock_coefficients::stepper_order;
    const static order_type error_order = rosenbrock_coefficients::error_order;

    rosenbrock4( void )
    : m_resizer() , m_x_err_resizer() ,
      m_jac() , m_pm() ,
      m_dfdt() , m_dxdt() , m_dxdtnew() ,
      m_g1() , m_g2() , m_g3() , m_g4() , m_g5() ,
      m_cont3() , m_cont4() , m_xtmp() , m_x_err() ,
      m_coef()
    { }

    order_type order() const { return stepper_order; }

    template< class System >
    void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt , state_type &xerr )
    {
        // get the system and jacobi function
        typedef typename odeint::unwrap_reference< System >::type system_type;
        typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
        typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
        system_type &sys = system;
        deriv_func_type &deriv_func = sys.first;
        jacobi_func_type &jacobi_func = sys.second;

        const size_t n = x.size();

        m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );

        for( size_t i=0 ; i<n ; ++i )
            m_pm.m_v( i ) = i;

        deriv_func( x , m_dxdt.m_v , t );
        jacobi_func( x , m_jac.m_v , t , m_dfdt.m_v );

        m_jac.m_v *= -1.0;
        m_jac.m_v += 1.0 / m_coef.gamma / dt * boost::numeric::ublas::identity_matrix< value_type >( n );
        boost::numeric::ublas::lu_factorize( m_jac.m_v , m_pm.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_g1.m_v[i] = m_dxdt.m_v[i] + dt * m_coef.d1 * m_dfdt.m_v[i];
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g1.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a21 * m_g1.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c2 * dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g2.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d2 * m_dfdt.m_v[i] + m_coef.c21 * m_g1.m_v[i] / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g2.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a31 * m_g1.m_v[i] + m_coef.a32 * m_g2.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c3 * dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g3.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d3 * m_dfdt.m_v[i] + ( m_coef.c31 * m_g1.m_v[i] + m_coef.c32 * m_g2.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g3.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a41 * m_g1.m_v[i] + m_coef.a42 * m_g2.m_v[i] + m_coef.a43 * m_g3.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c4 * dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g4.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d4 * m_dfdt.m_v[i] + ( m_coef.c41 * m_g1.m_v[i] + m_coef.c42 * m_g2.m_v[i] + m_coef.c43 * m_g3.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g4.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a51 * m_g1.m_v[i] + m_coef.a52 * m_g2.m_v[i] + m_coef.a53 * m_g3.m_v[i] + m_coef.a54 * m_g4.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g5.m_v[i] = m_dxdtnew.m_v[i] + ( m_coef.c51 * m_g1.m_v[i] + m_coef.c52 * m_g2.m_v[i] + m_coef.c53 * m_g3.m_v[i] + m_coef.c54 * m_g4.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g5.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] += m_g5.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
        for( size_t i=0 ; i<n ; ++i )
            xerr[i] = m_dxdtnew.m_v[i] + ( m_coef.c61 * m_g1.m_v[i] + m_coef.c62 * m_g2.m_v[i] + m_coef.c63 * m_g3.m_v[i] + m_coef.c64 * m_g4.m_v[i] + m_coef.c65 * m_g5.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , xerr );

        for( size_t i=0 ; i<n ; ++i )
            xout[i] = m_xtmp.m_v[i] + xerr[i];
    }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt , state_type &xerr )
    {
        do_step( system , x , t , x , dt , xerr );
    }

    /*
     * do_step without error output - just calls above functions with and neglects the error estimate
     */
    template< class System >
    void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt )
    {
        m_x_err_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_x_err<state_type> , detail::ref( *this ) , detail::_1 ) );
        do_step( system , x , t , xout , dt , m_x_err.m_v );
    }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt )
    {
        m_x_err_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_x_err<state_type> , detail::ref( *this ) , detail::_1 ) );
        do_step( system , x , t , dt , m_x_err.m_v );
    }

    void prepare_dense_output()
    {
        const size_t n = m_g1.m_v.size();
        for( size_t i=0 ; i<n ; ++i )
        {
            m_cont3.m_v[i] = m_coef.d21 * m_g1.m_v[i] + m_coef.d22 * m_g2.m_v[i] + m_coef.d23 * m_g3.m_v[i] + m_coef.d24 * m_g4.m_v[i] + m_coef.d25 * m_g5.m_v[i];
            m_cont4.m_v[i] = m_coef.d31 * m_g1.m_v[i] + m_coef.d32 * m_g2.m_v[i] + m_coef.d33 * m_g3.m_v[i] + m_coef.d34 * m_g4.m_v[i] + m_coef.d35 * m_g5.m_v[i];
        }
    }

    void calc_state( time_type t , state_type &x ,
            const state_type &x_old , time_type t_old ,
            const state_type &x_new , time_type t_new )
    {
        const size_t n = m_g1.m_v.size();
        time_type dt = t_new - t_old;
        time_type s = ( t - t_old ) / dt;
        time_type s1 = 1.0 - s;
        for( size_t i=0 ; i<n ; ++i )
            x[i] = x_old[i] * s1 + s * ( x_new[i] + s1 * ( m_cont3.m_v[i] + s * m_cont4.m_v[i] ) );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
        resize_x_err( x );
    }

protected:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_dfdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdtnew , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_xtmp , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g2 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g3 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g4 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g5 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_cont3 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_cont4 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_jac , x , typename is_resizeable<matrix_type>::type() );
        resized |= adjust_size_by_resizeability( m_pm , x , typename is_resizeable<pmatrix_type>::type() );
        return resized;
    }

    template< class StateIn >
    bool resize_x_err( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_x_err , x , typename is_resizeable<state_type>::type() );
    }

private:

    resizer_type m_resizer;
    resizer_type m_x_err_resizer;

    wrapped_matrix_type m_jac;
    wrapped_pmatrix_type m_pm;
    wrapped_deriv_type m_dfdt , m_dxdt , m_dxdtnew;
    wrapped_state_type m_g1 , m_g2 , m_g3 , m_g4 , m_g5;
    wrapped_state_type m_cont3 , m_cont4;
    wrapped_state_type m_xtmp;
    wrapped_state_type m_x_err;

    const rosenbrock_coefficients m_coef;
};

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

#endif // BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED


#include "BinaryMetadataExtractor.h"
#include <boost/filesystem/path.hpp>
#include <google/protobuf/descriptor.pb.h>
#include <google/protobuf/io/coded_stream.h>
#include <fstream>

namespace
{
constexpr int kMaxVarintBytes = 10;

uint64_t DecodeVarint64(char* buffer, size_t* count)
{
    uint8_t* buffer_ = reinterpret_cast<uint8_t*>(buffer);
    uint64_t result = 0;
    uint32_t b;

    do
    {
        if (*count == kMaxVarintBytes)
            break;

        b = *buffer_;
        result |= static_cast<uint64_t>(b & 0x7F) << (7 * *count);
        ++buffer_;
        ++* count;
    }
    while (b & 0x80);

    return result;
}

class BinaryMetadata : public MetadataExtractor::Metadata
{
public:
    explicit BinaryMetadata(std::string id, uint8_t const* data, int32_t length) : Metadata(std::move(id)), Data(data), Length(length)
    {
    }

    std::shared_ptr<google::protobuf::io::CodedInputStream> CreateCodedInputStream() const override
    {
        return std::make_shared<google::protobuf::io::CodedInputStream>(Data, Length);
    }

private:
    uint8_t const* Data;
    int32_t Length;
};
}

void BinaryMetadataExtractor::Parse(boost::filesystem::path const& binaryPath)
{
    ReadFile(binaryPath);
    FindMetadata();
}

void BinaryMetadataExtractor::ReadFile(boost::filesystem::path const& binaryPath)
{
    std::ifstream ifs(binaryPath.string(), std::ifstream::binary);
    if (!ifs)
        throw std::runtime_error("could not open " + binaryPath.string());

    ifs >> std::noskipws;
    ifs.seekg(0, std::ios_base::end);
    _binary.reserve(ifs.tellg());
    ifs.seekg(0, std::ios_base::beg);

    std::copy(std::istream_iterator<char>(ifs), std::istream_iterator<char>(), std::back_inserter(_binary));
}

void BinaryMetadataExtractor::FindMetadata()
{
    size_t itr = 0;
    while (itr < _binary.size())
    {
        itr = _binary.find(".proto", itr);
        if (itr == std::string::npos)
            break;

        itr += 6;

        size_t start = _binary.rfind('\xA', itr);
        if (start == std::string::npos)
            continue;

        size_t decodedBytes = 0;
        uint64_t length = DecodeVarint64(&_binary[start + 1], &decodedBytes);
        size_t end = start + 1 + decodedBytes;
        if (itr - end != length)
            continue;

        google::protobuf::io::CodedInputStream stream(reinterpret_cast<uint8_t*>(&_binary[start]), _binary.size() - start);

        google::protobuf::FileDescriptorProto descriptor;
        if (!descriptor.MergeFromCodedStream(&stream))
            continue;

        _metadatas.emplace_back(new BinaryMetadata(std::to_string(_metadatas.size()), reinterpret_cast<uint8_t*>(&_binary[start]), stream.CurrentPosition() - 1));
    }
}


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

#include <amount.h>
#include <policy/fees.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(policy_fee_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(FeeRounder)
{
    FeeFilterRounder fee_rounder{CFeeRate{1000}};

    // check that 1000 rounds to 974 or 1071
    std::set<CAmount> results;
    while (results.size() < 2) {
        results.emplace(fee_rounder.round(1000));
    }
    BOOST_CHECK_EQUAL(*results.begin(), 974);
    BOOST_CHECK_EQUAL(*++results.begin(), 1071);

    // check that negative amounts rounds to 0
    BOOST_CHECK_EQUAL(fee_rounder.round(-0), 0);
    BOOST_CHECK_EQUAL(fee_rounder.round(-1), 0);

    // check that MAX_MONEY rounds to 9170997
    BOOST_CHECK_EQUAL(fee_rounder.round(MAX_MONEY), 9170997);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>

namespace merely3d
{
    // Unaligned types are needed to ensure safe usage when storing types
    // in containers or in structs.
    typedef Eigen::Matrix<float, 2, 1, Eigen::DontAlign> UnalignedVector2f;
    typedef Eigen::Quaternion<float, Eigen::DontAlign>   UnalignedQuaternionf;
}


/*!
@file
Defines `boost::hana::SearchableSet`.

@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_SANDBOX_SEARCHABLE_SET_HPP
#define BOOST_HANA_SANDBOX_SEARCHABLE_SET_HPP

#include <boost/hana/applicative.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/functor.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/monad.hpp>
#include <boost/hana/searchable.hpp>

namespace boost { namespace hana {
    //! @ingroup group-datatypes
    //! A `Monad` for searching infinite sets in finite time.
    //!
    //! Taken from http://math.andrej.com/2008/11/21/a-haskell-monad-for-infinite-search-in-finite-time/.
    struct SearchableSet {
        struct hana {
            struct operators
                : boost::hana::operators::of<Comparable, Monad>
            { };
        };
    };

    template <typename Find, typename = operators::adl>
    struct _sset {
        Find find;
        struct hana { using datatype = SearchableSet; };
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto searchable_set = [](auto pred) {
        return _sset<decltype(pred)>{pred};
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto singleton = [](auto x) {
        return searchable_set([=](auto p) { return x; });
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto doubleton = [](auto x, auto y) {
        return searchable_set([=](auto p) {
            return if_(p(x), x, y);
        });
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto union_ = [](auto xs, auto ys) {
        return flatten(doubleton(xs, ys));
    };

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<SearchableSet, SearchableSet> {
        template <typename Xs, typename Ys>
        static constexpr auto apply(Xs xs, Ys ys)
        { return and_(subset(xs, ys), subset(ys, xs)); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Functor
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct transform_impl<SearchableSet> {
        template <typename Set, typename F>
        static constexpr auto apply(Set set, F f) {
            return searchable_set([=](auto q) {
                return f(set.find([=](auto x) { return q(f(x)); }));
            });
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Applicative
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct lift_impl<SearchableSet> {
        template <typename X>
        static constexpr auto apply(X x)
        { return singleton(x); }
    };

    template <>
    struct ap_impl<SearchableSet> {
        template <typename F, typename Set>
        static constexpr auto apply(F fset, Set set) {
            return flatten(transform(fset, [=](auto f) {
                return transform(set, f);
            }));
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monad
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct flatten_impl<SearchableSet> {
        template <typename Set>
        static constexpr auto apply(Set set) {
            return searchable_set([=](auto p) {
                return set.find([=](auto set) {
                    return any_of(set, p);
                }).find(p);
            });
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Searchable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct find_if_impl<SearchableSet> {
        template <typename Set, typename Pred>
        static constexpr auto apply(Set set, Pred p) {
            auto x = set.find(p);
            return if_(p(x), just(x), nothing);
        }
    };

    template <>
    struct any_of_impl<SearchableSet> {
        template <typename Set, typename Pred>
        static constexpr auto apply(Set set, Pred p) {
            return p(set.find(p));
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_SANDBOX_SEARCHABLE_SET_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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 BOOST_SIMD_TOOLBOX_CONSTANT_INCLUDE_CONSTANTS_EPS_RELATED_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_INCLUDE_CONSTANTS_EPS_RELATED_HPP_INCLUDED

#include <boost/simd/toolbox/constant/include/constants/eps.hpp>
#include <boost/simd/toolbox/constant/include/constants/halfeps.hpp>
#include <boost/simd/toolbox/constant/include/constants/sqrteps.hpp>
#include <boost/simd/toolbox/constant/include/constants/mlogeps2.hpp>
#include <boost/simd/toolbox/constant/include/constants/threeeps.hpp>
#include <boost/simd/toolbox/constant/include/constants/fourthrooteps.hpp>
#include <boost/simd/toolbox/constant/include/constants/thirdrooteps.hpp>
#include <boost/simd/toolbox/constant/include/constants/smallestposval.hpp>
#include <boost/simd/toolbox/constant/include/constants/mindenormal.hpp>

#endif


#pragma once

#include <memory>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "../AgentPosition/agentposition.hh"
#include "../util/Maybe.hh"

namespace bold
{
  class CameraModel;
  class LineJunctionFinder;

  class Spatialiser
  {
  public:
    Spatialiser(std::shared_ptr<CameraModel> cameraModel);

    void updateZeroGroundPixelTransform();

    void updateZeroGroundPixelTransform(Eigen::Affine3d const& agentCameraTr);

    Eigen::Matrix3d getZeroGroundPixelTransform() const { return d_zeroGroundPixelTr; }

    Eigen::Matrix3d findGroundPixelTransform(double groundZ = 0.0) const;

    Eigen::Matrix3d findGroundPixelTransform(Eigen::Affine3d const& agentCameraTr,
                                             double groundZ = 0.0) const;

    // TODO rename these two functions to indicate they work in agent space

    /** Returns the ground-plane location, in agent space, for a given
     * camera pixel.
     *
     * @param pixel The x/y pixel location
     * @param groundZ The z coordinate of the ground plane that is used; default is 0
     */
    Maybe<Eigen::Vector3d> findGroundPointForPixel(Eigen::Vector2d const& pixel, double groundZ = 0.0) const;

    /** Returns the ground-plane location, in agent space, for a given
     * camera pixel and camera transformation.
     *
     * @param pixel The x/y pixel location
     * @param agentCameraTr The transformation from camera to agent frame
     * @param groundZ The z coordinate of the ground plane that is used; default is 0
     */
    Maybe<Eigen::Vector3d> findGroundPointForPixel(Eigen::Vector2d const& pixel,
                                                   Eigen::Affine3d const& agentCameraTr,
                                                   double groundZ = 0) const;

    /** Returns the ground-plane locations, in agent space, for a
     * given set of camera pixels.
     *
     * @param pixels A 2xN matrix, where each column denotes a pixel coordinate
     * @param groundZ The z coordinate of the ground plane that is used; default is 0
     * @returns a pair containing a 3xN matrix of ground locations and
     * an N-dimensional vector that denotes whether these are valid
     */
    std::pair<Eigen::MatrixXd,Eigen::VectorXi> findGroundPointsForPixels(Eigen::MatrixXd const& pixels,
                                                                         double groundZ = 0.0) const;

    /** Returns the ground-plane locations, in agent space, for a
     * given set of camera pixels and camera transfrmation.
     *
     * @param pixels A 2xN matrix, where each column denotes a pixel coordinate
     * @param agentCameraTr The transformation from camera to agent frame
     * @param groundZ The z coordinate of the ground plane that is used; default is 0
     */
    std::pair<Eigen::MatrixXd,Eigen::VectorXi> findGroundPointsForPixels(Eigen::MatrixXd const& pixels,
                                                                         Eigen::Affine3d const& agentCameraTr,
                                                                         double groundZ = 0.0) const;

    Maybe<Eigen::Vector2d> findPixelForAgentPoint(Eigen::Vector3d const& agentPoint) const;

    Maybe<Eigen::Vector2d> findPixelForAgentPoint(Eigen::Vector3d const& agentPoint,
                                                  Eigen::Affine3d const& agentCameraTr) const;

    int findHorizonForColumn(int column);
    int findHorizonForColumn(int column, Eigen::Affine3d const& agentCameraTr);

    void updateCameraToAgent();

    void updateAgentToWorld(AgentPosition position);

    std::shared_ptr<CameraModel> getCameraModel() const { return d_cameraModel; }

  private:
    Maybe<Eigen::Vector3d> findGroundPointForPixel(Eigen::Vector2d const& pixel,
                                                   Eigen::Matrix3d const& groundPixelTr) const;
    std::pair<Eigen::MatrixXd, Eigen::VectorXi> findGroundPointsForPixels(Eigen::MatrixXd const& pixels,
                                                                          Eigen::Matrix3d const& groundPixelTr) const;

    std::shared_ptr<CameraModel> d_cameraModel;
    std::shared_ptr<LineJunctionFinder> d_lineJunctionFinder;

    Eigen::Matrix3d d_zeroGroundPixelTr;
  };
}


/*
 * Copyright 2016,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
#include <pinocchio/fwd.hpp>
#include <sot/core/debug.hh>

#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>

#include <boost/version.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/multibody/model.hpp>

#include <dynamic-graph/all-commands.h>

#include "../src/dynamic-command.h"

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

const std::string dg::sot::DynamicPinocchio::CLASS_NAME = "DynamicPinocchio";

DynamicPinocchio::DynamicPinocchio(const std::string& name)
    : Entity(name),
      m_model(NULL),
      m_data(NULL)

      ,
      jointPositionSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::position"),
      freeFlyerPositionSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffposition"),
      jointVelocitySIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::velocity"),
      freeFlyerVelocitySIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffvelocity"),
      jointAccelerationSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::acceleration"),
      freeFlyerAccelerationSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffacceleration")

      ,
      pinocchioPosSINTERN(boost::bind(&DynamicPinocchio::getPinocchioPos, this, _1, _2),
                          jointPositionSIN << freeFlyerPositionSIN,
                          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioPos"),
      pinocchioVelSINTERN(boost::bind(&DynamicPinocchio::getPinocchioVel, this, _1, _2),
                          jointVelocitySIN << freeFlyerVelocitySIN,
                          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioVel"),
      pinocchioAccSINTERN(boost::bind(&DynamicPinocchio::getPinocchioAcc, this, _1, _2),
                          jointAccelerationSIN << freeFlyerAccelerationSIN,
                          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioAcc")

      ,
      newtonEulerSINTERN(boost::bind(&DynamicPinocchio::computeNewtonEuler, this, _1, _2),
                         pinocchioPosSINTERN << pinocchioVelSINTERN << pinocchioAccSINTERN,
                         "sotDynamicPinocchio(" + name + ")::intern(dummy)::newtoneuler"),
      jacobiansSINTERN(boost::bind(&DynamicPinocchio::computeJacobians, this, _1, _2), pinocchioPosSINTERN,
                       "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeJacobians"),
      forwardKinematicsSINTERN(boost::bind(&DynamicPinocchio::computeForwardKinematics, this, _1, _2),
                               pinocchioPosSINTERN << pinocchioVelSINTERN << pinocchioAccSINTERN,
                               "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeForwardKinematics"),
      ccrbaSINTERN(boost::bind(&DynamicPinocchio::computeCcrba, this, _1, _2),
                   pinocchioPosSINTERN << pinocchioVelSINTERN,
                   "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeCcrba"),
      zmpSOUT(boost::bind(&DynamicPinocchio::computeZmp, this, _1, _2), newtonEulerSINTERN,
              "sotDynamicPinocchio(" + name + ")::output(vector)::zmp"),
      JcomSOUT(boost::bind(&DynamicPinocchio::computeJcom, this, _1, _2), pinocchioPosSINTERN,
               "sotDynamicPinocchio(" + name + ")::output(matrix)::Jcom"),
      comSOUT(boost::bind(&DynamicPinocchio::computeCom, this, _1, _2), forwardKinematicsSINTERN,
              "sotDynamicPinocchio(" + name + ")::output(vector)::com"),
      inertiaSOUT(boost::bind(&DynamicPinocchio::computeInertia, this, _1, _2), pinocchioPosSINTERN,
                  "sotDynamicPinocchio(" + name + ")::output(matrix)::inertia"),
      footHeightSOUT(boost::bind(&DynamicPinocchio::computeFootHeight, this, _1, _2), pinocchioPosSINTERN,
                     "sotDynamicPinocchio(" + name + ")::output(double)::footHeight"),
      upperJlSOUT(boost::bind(&DynamicPinocchio::getUpperPositionLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::upperJl")

      ,
      lowerJlSOUT(boost::bind(&DynamicPinocchio::getLowerPositionLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::lowerJl")

      ,
      upperVlSOUT(boost::bind(&DynamicPinocchio::getUpperVelocityLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::upperVl")

      ,
      upperTlSOUT(boost::bind(&DynamicPinocchio::getMaxEffortLimits, this, _1, _2), sotNOSIGNAL,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::upperTl")

      ,
      inertiaRotorSOUT("sotDynamicPinocchio(" + name + ")::output(matrix)::inertiaRotor"),
      gearRatioSOUT("sotDynamicPinocchio(" + name + ")::output(matrix)::gearRatio"),
      inertiaRealSOUT(boost::bind(&DynamicPinocchio::computeInertiaReal, this, _1, _2),
                      inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
                      "sotDynamicPinocchio(" + name + ")::output(matrix)::inertiaReal"),
      MomentaSOUT(boost::bind(&DynamicPinocchio::computeMomenta, this, _1, _2), ccrbaSINTERN,
                  "sotDynamicPinocchio(" + name + ")::output(vector)::momenta"),
      AngularMomentumSOUT(boost::bind(&DynamicPinocchio::computeAngularMomentum, this, _1, _2), ccrbaSINTERN,
                          "sotDynamicPinocchio(" + name + ")::output(vector)::angularmomentum"),
      dynamicDriftSOUT(boost::bind(&DynamicPinocchio::computeTorqueDrift, this, _1, _2), newtonEulerSINTERN,
                       "sotDynamicPinocchio(" + name + ")::output(vector)::dynamicDrift") {
  sotDEBUGIN(5);

  // TODO-------------------------------------------

  // if( build ) buildModel();
  // firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
  // DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
  // endTODO--------------------------------------------

  signalRegistration(jointPositionSIN);
  signalRegistration(freeFlyerPositionSIN);
  signalRegistration(jointVelocitySIN);
  signalRegistration(freeFlyerVelocitySIN);
  signalRegistration(jointAccelerationSIN);
  signalRegistration(freeFlyerAccelerationSIN);
  signalRegistration(zmpSOUT);
  signalRegistration(comSOUT);
  signalRegistration(JcomSOUT);
  signalRegistration(footHeightSOUT);
  signalRegistration(upperJlSOUT);
  signalRegistration(lowerJlSOUT);
  signalRegistration(upperVlSOUT);
  signalRegistration(upperTlSOUT);
  signalRegistration(inertiaSOUT);
  signalRegistration(inertiaRealSOUT);
  signalRegistration(inertiaRotorSOUT);
  signalRegistration(gearRatioSOUT);
  signalRegistration(MomentaSOUT);
  signalRegistration(AngularMomentumSOUT);
  signalRegistration(dynamicDriftSOUT);

  //
  // Commands
  //
  std::string docstring;
  // setFiles

  docstring =
      "\n"
      "    Display the current robot configuration.\n"
      "\n"
      "      Input:\n"
      "        - none \n"
      "\n";
  addCommand("displayModel", new command::DisplayModel(*this, docstring));
  docstring =
      "    \n"
      "    Get the dimension of the robot configuration.\n"
      "    \n"
      "      Return:\n"
      "        an unsigned int: the dimension.\n"
      "    \n";
  addCommand("getDimension", new command::GetDimension(*this, docstring));

  {
    using namespace ::dg::command;
    // CreateOpPoint
    // TODO add operational joints
    docstring =
        "    \n"
        "    Create an operational point attached to a robot joint local frame.\n"
        "    \n"
        "      Input: \n"
        "        - a string: name of the operational point,\n"
        "        - a string: name the joint, or among (gaze, left-ankle, right ankle\n"
        "          , left-wrist, right-wrist, waist, chest).\n"
        "\n";
    addCommand("createOpPoint", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createOpPointSignals, docstring));

    docstring = docCommandVoid2("Create a jacobian (world frame) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createJacobian", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createJacobianWorldSignal, docstring));

    docstring = docCommandVoid2("Create a jacobian (endeff frame) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createJacobianEndEff",
               makeCommandVoid2(*this, &DynamicPinocchio::cmd_createJacobianEndEffectorSignal, docstring));

    docstring = docCommandVoid2(
        "Create a jacobian (endeff frame) signal only for one joint. "
        "The returned jacobian is placed at the joint position, but oriented with the world axis.",
        "string (signal name)", "string (joint name)");
    addCommand("createJacobianEndEffWorld",
               makeCommandVoid2(*this, &DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal, docstring));

    docstring = docCommandVoid2("Create a position (matrix homo) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createPosition", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createPositionSignal, docstring));

    docstring = docCommandVoid2("Create a velocity (vector) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createVelocity", makeCommandVoid2(*this, &DynamicPinocchio::cmd_createVelocitySignal, docstring));

    docstring = docCommandVoid2("Create an acceleration (vector) signal only for one joint.", "string (signal name)",
                                "string (joint name)");
    addCommand("createAcceleration",
               makeCommandVoid2(*this, &DynamicPinocchio::cmd_createAccelerationSignal, docstring));
    docstring =
        "\n"
        "  Return robot joint names.\n\n";
    addCommand("getJointNames", new command::GetJointNames(*this, docstring));
  }

  sphericalJoints.clear();

  sotDEBUG(10) << "Dynamic class_name address" << &CLASS_NAME << std::endl;
  sotDEBUGOUT(5);
}

DynamicPinocchio::~DynamicPinocchio(void) {
  sotDEBUGIN(15);
  // TODO currently, m_model and m_data are pointers owned by the Python interpreter
  // so we should not delete them.
  // I (Joseph Mirabel) think it would be wiser to make them belong to this class but
  // I do not know the impact it has.
  // if (0!=m_data ) { delete m_data ; m_data =NULL; }
  // if (0!=m_model) { delete m_model; m_model=NULL; }

  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    SignalBase<int>* sigPtr = *iter;
    delete sigPtr;
  }
  sotDEBUGOUT(15);
}

void DynamicPinocchio::setModel(pinocchio::Model* modelPtr) {
  this->m_model = modelPtr;

  if (this->m_model->nq > m_model->nv) {
    if (pinocchio::nv(this->m_model->joints[1]) == 6) sphericalJoints.push_back(3);  // FreeFlyer Orientation

    for (int i = 1; i < this->m_model->njoints; i++)     // 0: universe
      if (pinocchio::nq(this->m_model->joints[i]) == 4)  // Spherical Joint Only
        sphericalJoints.push_back(pinocchio::idx_v(this->m_model->joints[i]));
  }
}

void DynamicPinocchio::setData(pinocchio::Data* dataPtr) { this->m_data = dataPtr; }

/*--------------------------------GETTERS-------------------------------------------*/

dg::Vector& DynamicPinocchio::getLowerPositionLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  if (!sphericalJoints.empty()) {

    int fillingIndex = 0;  // SoTValue
    int origIndex = 0;     // PinocchioValue
    for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
      if (*it - fillingIndex > 0) {
        res.segment(fillingIndex, *it - fillingIndex) =
            m_model->lowerPositionLimit.segment(origIndex, *it - fillingIndex);

        // Don't Change this order
        origIndex += *it - fillingIndex;
        fillingIndex += *it - fillingIndex;
      }
      // Found a Spherical Joint.
      // Assuming that spherical joint limits are unset
      // Version C++11
      //res(fillingIndex) = std::numeric_limits<double>::lowest();
      //res(fillingIndex + 1) = std::numeric_limits<double>::lowest();
      //res(fillingIndex + 2) = std::numeric_limits<double>::lowest();
      // For now use C++98
      res(fillingIndex) = -std::numeric_limits<double>::max();
      res(fillingIndex + 1) = -std::numeric_limits<double>::max();
      res(fillingIndex + 2) = -std::numeric_limits<double>::max();

      fillingIndex += 3;
      origIndex += 4;
    }

    assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
    if (m_model->nv > fillingIndex)
      res.segment(fillingIndex, m_model->nv - fillingIndex) =
          m_model->lowerPositionLimit.segment(origIndex, m_model->nv - fillingIndex);
  } else {
    res = m_model->lowerPositionLimit;
  }
  sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
}

dg::Vector& DynamicPinocchio::getUpperPositionLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  if (!sphericalJoints.empty()) {
    int fillingIndex = 0;  // SoTValue
    int origIndex = 0;     // PinocchioValue
    for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
      if (*it - fillingIndex > 0) {
        res.segment(fillingIndex, *it - fillingIndex) =
            m_model->upperPositionLimit.segment(origIndex, *it - fillingIndex);

        // Don't Change this order
        origIndex += *it - fillingIndex;
        fillingIndex += *it - fillingIndex;
      }
      // Found a Spherical Joint.
      // Assuming that spherical joint limits are unset
      res(fillingIndex) = std::numeric_limits<double>::max();
      res(fillingIndex + 1) = std::numeric_limits<double>::max();
      res(fillingIndex + 2) = std::numeric_limits<double>::max();
      fillingIndex += 3;
      origIndex += 4;
    }
    assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
    if (m_model->nv > fillingIndex)
      res.segment(fillingIndex, m_model->nv - fillingIndex) =
          m_model->upperPositionLimit.segment(origIndex, m_model->nv - fillingIndex);
  } else {
    res = m_model->upperPositionLimit;
  }
  sotDEBUG(15) << "upperLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
}

dg::Vector& DynamicPinocchio::getUpperVelocityLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  res = m_model->velocityLimit;

  sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
}

dg::Vector& DynamicPinocchio::getMaxEffortLimits(dg::Vector& res, const int&) const {
  sotDEBUGIN(15);
  assert(m_model);

  res.resize(m_model->nv);
  res = m_model->effortLimit;

  sotDEBUGOUT(15);
  return res;
}

/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q, const int& time) {
  sotDEBUGIN(15);
  dg::Vector qJoints = jointPositionSIN.access(time);
  if (!sphericalJoints.empty()) {
    if (freeFlyerPositionSIN) {
      dg::Vector qFF = freeFlyerPositionSIN.access(time);
      qJoints.head<6>() = qFF;          // Overwrite qJoints ff with ffposition value
      assert(sphericalJoints[0] == 3);  // FreeFlyer should ideally be present.
    }
    q.resize(qJoints.size() + sphericalJoints.size());
    int fillingIndex = 0;
    int origIndex = 0;
    for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++) {
      if (*it - origIndex > 0) {
        q.segment(fillingIndex, *it - origIndex) = qJoints.segment(origIndex, *it - origIndex);
        fillingIndex += *it - origIndex;
        origIndex += *it - origIndex;
      }
      assert(*it == origIndex);
      Eigen::Quaternion<double> temp = Eigen::AngleAxisd(qJoints(origIndex + 2), Eigen::Vector3d::UnitZ()) *
                                       Eigen::AngleAxisd(qJoints(origIndex + 1), Eigen::Vector3d::UnitY()) *
                                       Eigen::AngleAxisd(qJoints(origIndex), Eigen::Vector3d::UnitX());
      q(fillingIndex) = temp.x();
      q(fillingIndex + 1) = temp.y();
      q(fillingIndex + 2) = temp.z();
      q(fillingIndex + 3) = temp.w();
      fillingIndex += 4;
      origIndex += 3;
    }
    if (qJoints.size() > origIndex)
      q.segment(fillingIndex, qJoints.size() - origIndex) = qJoints.tail(qJoints.size() - origIndex);
  } else {
    q.resize(qJoints.size());
    q = qJoints;
  }

  sotDEBUG(15) << "Position out" << q << std::endl;
  sotDEBUGOUT(15);
  return q;
}

dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const int& time) {
  const Eigen::VectorXd vJoints = jointVelocitySIN.access(time);
  if (freeFlyerVelocitySIN) {
    const Eigen::VectorXd vFF = freeFlyerVelocitySIN.access(time);
    if (v.size() != vJoints.size() + vFF.size()) v.resize(vJoints.size() + vFF.size());
    v << vFF, vJoints;
    return v;
  } else {
    v = vJoints;
    return v;
  }
}

dg::Vector& DynamicPinocchio::getPinocchioAcc(dg::Vector& a, const int& time) {
  const Eigen::VectorXd aJoints = jointAccelerationSIN.access(time);
  if (freeFlyerAccelerationSIN) {
    const Eigen::VectorXd aFF = freeFlyerAccelerationSIN.access(time);
    if (a.size() != aJoints.size() + aFF.size()) a.resize(aJoints.size() + aFF.size());
    a << aFF, aJoints;
    return a;
  } else {
    a = aJoints;
    return a;
  }
}

/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::createJacobianSignal(const std::string& signame,
                                                                                const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  dg::SignalTimeDependent<dg::Matrix, int>* sig;
  if (m_model->existFrame(jointName)) {
    long int frameId = m_model->getFrameId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericJacobian, this, true, frameId, _1, _2), jacobiansSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else if (m_model->existJointName(jointName)) {
    long int jointId = m_model->getJointId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericJacobian, this, false, jointId, _1, _2), jacobiansSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to " + jointName);

  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);
  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::createEndeffJacobianSignal(const std::string& signame,
                                                                                const std::string& jointName,
                                                                                const bool isLocal) {
  sotDEBUGIN(15);
  assert(m_model);
  dg::SignalTimeDependent<dg::Matrix, int>* sig;

  if (m_model->existFrame(jointName)) {
    long int frameId = m_model->getFrameId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian, this, true, isLocal, frameId, _1, _2),
        jacobiansSINTERN << forwardKinematicsSINTERN, "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else if (m_model->existJointName(jointName)) {
    long int jointId = m_model->getJointId(jointName);
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
        boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian, this, false, isLocal, jointId, _1, _2),
        jacobiansSINTERN << forwardKinematicsSINTERN, "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
  } else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to " + jointName);
  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);
  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent<MatrixHomogeneous, int>& DynamicPinocchio::createPositionSignal(const std::string& signame,
                                                                                const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  dg::SignalTimeDependent<MatrixHomogeneous, int>* sig;
  if (m_model->existFrame(jointName)) {
    long int frameId = m_model->getFrameId(jointName);
    sig = new dg::SignalTimeDependent<MatrixHomogeneous, int>(
        boost::bind(&DynamicPinocchio::computeGenericPosition, this, true, frameId, _1, _2), forwardKinematicsSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrixHomo)::" + signame);
  } else if (m_model->existJointName(jointName)) {
    long int jointId = m_model->getJointId(jointName);
    sig = new dg::SignalTimeDependent<MatrixHomogeneous, int>(
        boost::bind(&DynamicPinocchio::computeGenericPosition, this, false, jointId, _1, _2), forwardKinematicsSINTERN,
        "sotDynamicPinocchio(" + name + ")::output(matrixHomo)::" + signame);
  } else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to " + jointName);

  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);
  sotDEBUGOUT(15);
  return *sig;
}

SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::createVelocitySignal(const std::string& signame,
                                                                             const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  long int jointId = m_model->getJointId(jointName);

  SignalTimeDependent<dg::Vector, int>* sig = new SignalTimeDependent<dg::Vector, int>(
      boost::bind(&DynamicPinocchio::computeGenericVelocity, this, jointId, _1, _2), forwardKinematicsSINTERN,
      "sotDynamicPinocchio(" + name + ")::output(dg::Vector)::" + signame);
  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);

  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::createAccelerationSignal(const std::string& signame,
                                                                                const std::string& jointName) {
  sotDEBUGIN(15);
  assert(m_model);
  long int jointId = m_model->getJointId(jointName);
  dg::SignalTimeDependent<dg::Vector, int>* sig = new dg::SignalTimeDependent<dg::Vector, int>(
      boost::bind(&DynamicPinocchio::computeGenericAcceleration, this, jointId, _1, _2), forwardKinematicsSINTERN,
      "sotDynamicPinocchio(" + name + ")::output(dg::Vector)::" + signame);

  genericSignalRefs.push_back(sig);
  signalRegistration(*sig);

  sotDEBUGOUT(15);
  return *sig;
}

void DynamicPinocchio::destroyJacobianSignal(const std::string& signame) {
  sotDEBUGIN(15);

  bool deletable = false;
  dg::SignalTimeDependent<dg::Matrix, int>* sig = &jacobiansSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
                               " (while trying to remove generic jac. signal <%s>).", signame.c_str());
  }
  signalDeregistration(signame);
  delete sig;
}

void DynamicPinocchio::destroyPositionSignal(const std::string& signame) {
  sotDEBUGIN(15);
  bool deletable = false;
  dg::SignalTimeDependent<MatrixHomogeneous, int>* sig = &positionsSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
                               " (while trying to remove generic pos. signal <%s>).", signame.c_str());
  }

  signalDeregistration(signame);

  delete sig;
}

void DynamicPinocchio::destroyVelocitySignal(const std::string& signame) {
  sotDEBUGIN(15);
  bool deletable = false;
  SignalTimeDependent<dg::Vector, int>* sig = &velocitiesSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
                               " (while trying to remove generic pos. signal <%s>).", signame.c_str());
  }

  signalDeregistration(signame);

  delete sig;
}

void DynamicPinocchio::destroyAccelerationSignal(const std::string& signame) {
  sotDEBUGIN(15);
  bool deletable = false;
  dg::SignalTimeDependent<dg::Vector, int>* sig = &accelerationsSOUT(signame);
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin(); iter != genericSignalRefs.end();
       ++iter) {
    if ((*iter) == sig) {
      genericSignalRefs.erase(iter);
      deletable = true;
      break;
    }
  }

  if (!deletable) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL, getName() + ":cannot destroy signal",
                               " (while trying to remove generic acc "
                               "signal <%s>).",
                               signame.c_str());
  }

  signalDeregistration(signame);

  delete sig;
}

/* --------------------- COMPUTE ------------------------------------------------- */

dg::Vector& DynamicPinocchio::computeZmp(dg::Vector& res, const int& time) {
  // TODO: To be verified
  sotDEBUGIN(25);
  assert(m_data);
  if (res.size() != 3) res.resize(3);
  newtonEulerSINTERN(time);

  const pinocchio::Force& ftau = m_data->oMi[1].act(m_data->f[1]);
  const pinocchio::Force::Vector3& tau = ftau.angular();
  const pinocchio::Force::Vector3& f = ftau.linear();
  res(0) = -tau[1] / f[2];
  res(1) = tau[0] / f[2];
  res(2) = 0;

  sotDEBUGOUT(25);

  return res;
}

// In world coordinates

// Updates the jacobian matrix in m_data
int& DynamicPinocchio::computeJacobians(int& dummy, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  pinocchio::computeJointJacobians(*m_model, *m_data);
  sotDEBUG(25) << "Jacobians updated" << std::endl;
  sotDEBUGOUT(25);
  return dummy;
}
int& DynamicPinocchio::computeForwardKinematics(int& dummy, const int& time) {
  sotDEBUGIN(25);
  assert(m_model);
  assert(m_data);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
  const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
  pinocchio::forwardKinematics(*m_model, *m_data, q, v, a);
  sotDEBUG(25) << "Kinematics updated" << std::endl;
  sotDEBUGOUT(25);
  return dummy;
}

int& DynamicPinocchio::computeCcrba(int& dummy, const int& time) {
  sotDEBUGIN(25);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
  pinocchio::ccrba(*m_model, *m_data, q, v);
  sotDEBUG(25) << "Inertia and Momentum updated" << std::endl;
  sotDEBUGOUT(25);
  return dummy;
}

dg::Matrix& DynamicPinocchio::computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res,
                                                     const int& time) {
  sotDEBUGIN(25);
  assert(m_model);
  assert(m_data);
  if (res.rows() != 6 || res.cols() != m_model->nv) res = Matrix::Zero(6, m_model->nv);
  jacobiansSINTERN(time);

  pinocchio::JointIndex id =
      isFrame ? m_model->frames[(pinocchio::JointIndex)jointId].parent : (pinocchio::JointIndex)jointId;

  // Computes Jacobian in world coordinates.
  pinocchio::getJointJacobian(*m_model, *m_data, id, pinocchio::WORLD, res);
  sotDEBUGOUT(25);
  return res;
}

dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int id,
                                                           dg::Matrix& res, const int& time) {
  sotDEBUGIN(25);
  assert(m_model);
  assert(m_data);
  if (res.rows() != 6 || res.cols() != m_model->nv) res = Matrix::Zero(6, m_model->nv);

  jacobiansSINTERN(time);

  pinocchio::FrameIndex fid;
  pinocchio::JointIndex jid;
  bool changeFrame = !isLocal;
  pinocchio::SE3 M;

  // Computes Jacobian in end-eff coordinates.
  if (isFrame) {
    changeFrame = true;
    fid = (pinocchio::FrameIndex)id;
    const pinocchio::Frame& frame = m_model->frames[fid];
    jid = frame.parent;

    M = frame.placement.inverse();
    if (!isLocal)  // Express the jacobian is world coordinate system.
      M.rotation() = m_data->oMf[fid].rotation() * M.rotation();
  } else {
    jid = (pinocchio::JointIndex)id;
    if (!isLocal) {  // Express the jacobian is world coordinate system.
      M.rotation() = m_data->oMi[jid].rotation();
      M.translation().setZero();
    }
  }
  pinocchio::getJointJacobian(*m_model, *m_data, jid, pinocchio::LOCAL, res);

  if (changeFrame) pinocchio::motionSet::se3Action(M, res, res);

  sotDEBUGOUT(25);
  return res;
}

MatrixHomogeneous& DynamicPinocchio::computeGenericPosition(const bool isFrame, const int id, MatrixHomogeneous& res,
                                                            const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  if (isFrame) {
    const pinocchio::Frame& frame = m_model->frames[id];
    res.matrix() = (m_data->oMi[frame.parent] * frame.placement).toHomogeneousMatrix();
  } else {
    res.matrix() = m_data->oMi[id].toHomogeneousMatrix();
  }
  sotDEBUG(25) << "For " << (isFrame ? m_model->frames[id].name : m_model->names[id]) << " with id: " << id
               << " position is " << res << std::endl;
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& DynamicPinocchio::computeGenericVelocity(const int jointId, dg::Vector& res, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  res.resize(6);
  const pinocchio::Motion& aRV = m_data->v[jointId];
  res << aRV.linear(), aRV.angular();
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& DynamicPinocchio::computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  res.resize(6);
  const pinocchio::Motion& aRA = m_data->a[jointId];
  res << aRA.linear(), aRA.angular();
  sotDEBUGOUT(25);
  return res;
}

int& DynamicPinocchio::computeNewtonEuler(int& dummy, const int& time) {
  sotDEBUGIN(15);
  assert(m_model);
  assert(m_data);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
  const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
  pinocchio::rnea(*m_model, *m_data, q, v, a);

  sotDEBUG(1) << "pos = " << q << std::endl;
  sotDEBUG(1) << "vel = " << v << std::endl;
  sotDEBUG(1) << "acc = " << a << std::endl;

  sotDEBUGOUT(15);
  return dummy;
}

dg::Matrix& DynamicPinocchio::computeJcom(dg::Matrix& Jcom, const int& time) {
  sotDEBUGIN(25);
  forwardKinematicsSINTERN(time);
  Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data, false);
  sotDEBUGOUT(25);
  return Jcom;
}

dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com, const int& time) {
  sotDEBUGIN(25);
  if (JcomSOUT.needUpdate(time)) {
    forwardKinematicsSINTERN(time);
    pinocchio::centerOfMass(*m_model, *m_data, false);
  }
  com = m_data->com[0];
  sotDEBUGOUT(25);
  return com;
}

dg::Matrix& DynamicPinocchio::computeInertia(dg::Matrix& res, const int& time) {
  sotDEBUGIN(25);
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
  res = pinocchio::crba(*m_model, *m_data, q);
  res.triangularView<Eigen::StrictlyLower>() = res.transpose().triangularView<Eigen::StrictlyLower>();
  sotDEBUGOUT(25);
  return res;
}

dg::Matrix& DynamicPinocchio::computeInertiaReal(dg::Matrix& res, const int& time) {
  sotDEBUGIN(25);

  const dg::Matrix& A = inertiaSOUT(time);
  const dg::Vector& gearRatio = gearRatioSOUT(time);
  const dg::Vector& inertiaRotor = inertiaRotorSOUT(time);

  res = A;
  for (int i = 0; i < gearRatio.size(); ++i) res(i, i) += (gearRatio(i) * gearRatio(i) * inertiaRotor(i));

  sotDEBUGOUT(25);
  return res;
}

double& DynamicPinocchio::computeFootHeight(double& res, const int&) {
  // Ankle position in local foot frame
  // TODO: Confirm that it is in the foot frame
  sotDEBUGIN(25);
  if (!m_model->existJointName("r_sole_joint")) {
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "Robot has no joint corresponding to rigthFoot");
  }
  long int jointId = m_model->getJointId("r_sole_joint");
  Eigen::Vector3d anklePosInLocalRefFrame = m_data->liMi[jointId].translation();
  // TODO: positive or negative? Current output:negative
  res = anklePosInLocalRefFrame(2);
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& DynamicPinocchio::computeTorqueDrift(dg::Vector& tauDrift, const int& time) {
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
  tauDrift = m_data->tau;
  sotDEBUGOUT(25);
  return tauDrift;
}

dg::Vector& DynamicPinocchio::computeMomenta(dg::Vector& Momenta, const int& time) {
  sotDEBUGIN(25);
  ccrbaSINTERN(time);
  if (Momenta.size() != 6) Momenta.resize(6);

  Momenta = m_data->hg.toVector_impl();

  sotDEBUGOUT(25) << "Momenta :" << Momenta;
  return Momenta;
}

dg::Vector& DynamicPinocchio::computeAngularMomentum(dg::Vector& Momenta, const int& time) {
  sotDEBUGIN(25);
  ccrbaSINTERN(time);

  if (Momenta.size() != 3) Momenta.resize(3);
  Momenta = m_data->hg.angular_impl();

  sotDEBUGOUT(25) << "AngularMomenta :" << Momenta;
  return Momenta;
}

/* ------------------------ SIGNAL CASTING--------------------------------------- */

dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::jacobiansSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<dg::Matrix, int>& res = dynamic_cast<dg::SignalTimeDependent<dg::Matrix, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type matrix.", name.c_str());
  }
}
dg::SignalTimeDependent<MatrixHomogeneous, int>& DynamicPinocchio::positionsSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<MatrixHomogeneous, int>& res =
        dynamic_cast<dg::SignalTimeDependent<MatrixHomogeneous, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type matrixHomo.", name.c_str());
  }
}

dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::velocitiesSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<dg::Vector, int>& res = dynamic_cast<dg::SignalTimeDependent<dg::Vector, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type Vector.", name.c_str());
  }
}

dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::accelerationsSOUT(const std::string& name) {
  SignalBase<int>& sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<dg::Vector, int>& res = dynamic_cast<dg::SignalTimeDependent<dg::Vector, int>&>(sigabs);
    return res;
  } catch (std::bad_cast e) {
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
                              " (while getting signal <%s> of type Vector.", name.c_str());
  }
}

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

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

/* --- PARAMS --------------------------------------------------------------- */

// jointName is either a fixed-joint (pinocchio operational frame) or a
// movable joint (pinocchio joint-variant).
void DynamicPinocchio::cmd_createOpPointSignals(const std::string& opPointName, const std::string& jointName) {
  createEndeffJacobianSignal(std::string("J") + opPointName, jointName, true);
  createPositionSignal(opPointName, jointName);
}
void DynamicPinocchio::cmd_createJacobianWorldSignal(const std::string& signalName, const std::string& jointName) {
  createJacobianSignal(signalName, jointName);
}
void DynamicPinocchio::cmd_createJacobianEndEffectorSignal(const std::string& signalName,
                                                           const std::string& jointName) {
  createEndeffJacobianSignal(signalName, jointName, true);
}

void DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal(const std::string& signalName,
                                                                const std::string& jointName) {
  createEndeffJacobianSignal(signalName, jointName, false);
}

void DynamicPinocchio::cmd_createPositionSignal(const std::string& signalName, const std::string& jointName) {
  createPositionSignal(signalName, jointName);
}
void DynamicPinocchio::cmd_createVelocitySignal(const std::string& signalName, const std::string& jointName) {
  createVelocitySignal(signalName, jointName);
}
void DynamicPinocchio::cmd_createAccelerationSignal(const std::string& signalName, const std::string& jointName) {
  createAccelerationSignal(signalName, jointName);
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SQRT_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0  a0) const BOOST_NOEXCEPT
    {
      return ::sqrt(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      #ifdef BOOST_SIMD_HAS_SQRTF
      return ::sqrtf(a0);
      #else
      return std::sqrt(a0);
      #endif
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(is_gez(a0), "sqrt input is negative");

      return A0(bs::sqrt(double(a0)));
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return std::sqrt(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return if_else_zero(a0, a0 * fast_(rsqrt)(a0));
    }
  };

   BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return ::sqrt(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( is_positive(a0)
                      , "sqrt integer domain is restricted to positive integer."
                      );
      return sqrt(a0);
    }
  };

} } }

#endif


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ATAN2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ATAN2_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/arch/common/detail/scalar/f_invtrig.hpp>
#include <boost/simd/arch/common/detail/scalar/d_invtrig.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( atan2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      #ifndef BOOST_SIMD_NO_NANS
      if (is_nan(a0) || is_nan(a1)) return Nan<A0>();
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0) && is_inf(a1))
      {
        a0 = copysign(One<A0>(), a0);
        a1 = copysign(One<A0>(), a1);
      }
      #endif
      A0 q = bs::abs(a0/a1);
      A0 z = detail::invtrig_base<A0,tag::radian_tag, tag::not_simd_type>::kernel_atan(q, rec(q));
      A0 sgn = signnz(a0);
      z = (is_positive(a1)? z: Pi<A0>()-z)*sgn;
    return is_eqz(a0) ? if_else_zero(is_negative(a1), Pi<A0>()*sgn) : z;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( atan2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::atan2(a0, a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( atan2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::fast_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      A0 q = bs::abs(a0/a1);
      A0 z = detail::invtrig_base<A0,tag::radian_tag, tag::not_simd_type>::kernel_atan(q, bs::rec(q));
      return (is_positive(a1)? z: Pi<A0>()-z)*signnz(a0);
    }
  };
} } }

#endif