#include <stdint.h>

#include <iostream>
#include <vector>
#include <cstdlib>
#include <limits>

//#include <boost/random.hpp>
//#include <boost/random/linear_congruential.hpp>
//#include <boost/random/minstd_rand.hpp>
//#include <boost/random/normal_distribution.hpp>
//#include <boost/random/variate_generator.hpp>
//#include <boost/generator_iterator.hpp>

//#include <boost/random.hpp>
//#include <boost/random/linear_congruential.hpp>
//#include <boost/random/minstd_rand.hpp>
//#include <boost/random/normal_distribution.hpp>
//#include <boost/random/variate_generator.hpp>
//#include <boost/generator_iterator.hpp>

using namespace std;

void flopTest()
{
    float a,b,c,d,e,f,g,h;

    a = 9.9 + 50;
    b = 1.0;
    c = a /b;
    d = a * b - c;
}

void longFunction(long bla);

void longFunction(long);

void unknownFunction();

namespace myNameSpace
{
    class classInNameSpace
    {

    };

    void funcInNamespace() {}

    namespace namespaceInNameSpace
    {
        void test() {};
    }
}

class C1
{
    class InnerClass
    {
        public:
            int f1();
    };

    public:
          void  memberFunc() { }

    protected:
        int memberVar;

};

int C1::InnerClass::f1()
{
    int a=5;
    return a/2;
}

/*template< typename Engine, typename Distribution >
void randomTest( Engine eng, const Distribution& dist )
{
    typedef typename Distribution::result_type result_type;
    typedef typename boost::variate_generator<Engine&, Distribution> gen_type;

    gen_type gen( eng, dist );

    double res( 0.0 );

    boost::generator_iterator<gen_type> die(&gen);

    const double start( dist.mean() - 4*dist.sigma() );
    const double end( dist.mean() + 4*dist.sigma() );

    //const double num( (end-start)/10.0 + 1 );

    int distr[20];// = new double[num];
    double intervall( (end-start)/20.0 );

    std::fill( distr, distr+20, 0.0 );

    for( int i = 0; i < std::numeric_limits<int>::max()/10000; ++i )
    {
        double current_number( *die++ );

        res += current_number;

        if( current_number < start+intervall )
        {
            distr[0] += 1;
            continue;
        }
        else if( current_number < start + 2 * intervall )
        {
            distr[1] += 1;
            continue;
        }
        else if( current_number < start + 3 * intervall )
        {
            distr[2] += 1;
            continue;
        }
        else if( current_number < start + 4 * intervall )
        {
            distr[3] += 1;
            continue;
        }
        else if( current_number < start + 5 * intervall )
        {
            distr[4] += 1;
            continue;
        }
        else if( current_number < start + 6 * intervall )
        {
            distr[5] += 1;
            continue;
        }
        else if( current_number < start + 7 * intervall )
        {
            distr[6] += 1;
            continue;
        }
        else if( current_number < start + 8 * intervall )
        {
            distr[7] += 1;
            continue;
        }
        else if( current_number < start + 9 * intervall )
        {
            distr[8] += 1;
            continue;
        }
        else if( current_number < start + 10 * intervall )
        {
            distr[9] += 1;
            continue;
        }
        else if( current_number < start + 11 * intervall )
        {
            distr[10] += 1;
            continue;
        }
        else if( current_number < start + 12 * intervall )
        {
            distr[11] += 1;
            continue;
        }
        else if( current_number < start + 13 * intervall )
        {
            distr[12] += 1;
            continue;
        }
        else if( current_number < start + 14 * intervall )
        {
            distr[13] += 1;
            continue;
        }
        else if( current_number < start + 15 * intervall )
        {
            distr[14] += 1;
            continue;
        }
        else if( current_number < start + 16 * intervall )
        {
            distr[15] += 1;
            continue;
        }
        else if( current_number < start + 17 * intervall )
        {
            distr[16] += 1;
            continue;
        }
        else if( current_number < start + 18 * intervall )
        {
            distr[17] += 1;
            continue;
        }
        else if( current_number < start + 19 * intervall )
        {
            distr[18] += 1;
            continue;
        }
        else
        {
            distr[19] += 1;
            continue;
        }
    }

    cout << endl << res << " ";
    std::copy( distr, distr+20, ostream_iterator<double, char>( cout, " " ) );
}

template< typename Generator, typename T >
void randomTest( T seed )
{
    Generator generator;

    {
        boost::normal_distribution<> distribution;
        randomTest( generator, distribution );
    }
    std::cout << std::endl;
}*/
void longFunction(long iterations)
{
	double d=1000;
	for(long int i=0;i<iterations; i++)
	{
	    for(int j=0; j<10; j++)
	    {
	        d=d/2+17;
	    }
	}
	//cout<< "d=" << d << std::endl;
}

void copyVector(vector<int>&a, vector<int>& b)
{
	for(unsigned int i=0; i<a.size(); i++)
	{
		b.push_back(a[i]);
	}
}

void fillVector(vector<int> & v, int size)
{
	v.clear();
	for(int i=0; i<size; i++)
		v.push_back(i);
}

int main(int argc, char**argv)
{
	int size=100;

	longFunction(size*1000);

        //randomTest<boost::minstd_rand, unsigned int>( 42u );
        /*randomTest<boost::rand48>( (uint64_t)42u );
        randomTest<boost::ecuyer1988>( 42u );
        randomTest<boost::kreutzer1986>( 42u );
        randomTest<boost::hellekalek1995>( 42u );
        randomTest<boost::mt11213b>( 42u );
        randomTest<boost::mt19937>( 42u );
        randomTest<boost::lagged_fibonacci607>( 42u );
        randomTest<boost::lagged_fibonacci1279>( 42u );
        randomTest<boost::lagged_fibonacci2281>( 42u );
        randomTest<boost::lagged_fibonacci3217>( 42u );
        randomTest<boost::lagged_fibonacci4423>( 42u );
        randomTest<boost::lagged_fibonacci9689>( 42u );
        randomTest<boost::lagged_fibonacci19937>( 42u );
        randomTest<boost::lagged_fibonacci23209>( 42u );
        randomTest<boost::lagged_fibonacci44497>( 42u );*/

	return 0;
}


// Copyright 2010-2014, D. E. Shaw Research.
// 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_RANDOM_M128COMMON_PRF_HPP
#define BOOST_RANDOM_M128COMMON_PRF_HPP

#include <x86intrin.h>
#include <boost/throw_exception.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/random/detail/counter_traits.hpp>
#include <boost/limits.hpp>

#include <stdexcept>
#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)
#include <initializer_list>
#endif

#if (defined(__ICC) && __ICC<1210) || (defined(_MSC_VER) && !defined(_WIN64))
/* Is there an intrinsic to assemble an __m128i from two 64-bit words?
   If not, use the 4x32-bit intrisic instead.  N.B.  It looks like Intel
   added _mm_set_epi64x to icc version 12.1 in Jan 2012.
*/
inline __m128i _mm_set_epi64x(uint64_t v1, uint64_t v0){
    union{
        uint64_t u64;
        uint32_t u32[2];
    } u1, u0;
    u1.u64 = v1;
    u0.u64 = v0;
    return _mm_set_epi32(u1.u32[1], u1.u32[0], u0.u32[1], u0.u32[0]);
}
#endif

namespace boost{
namespace random{
namespace detail{

// This is a SKETCH of a possible specialization of the
// counter_traits<__m128i> for the Intel SSE __m128i data type.  It's
// only complete enough to demonstrate a hardware-specific
// implementation of the ARS generator in ars_example.cpp.  IT IS NOT
// WELL TESTED AND HAS KNOWN DEFECTS (e.g., overflows and carries are
// not detected in incr()!!).

template<>
struct counter_traits<__m128i>{
    BOOST_STATIC_CONSTANT(unsigned, Nbits = 128u);

    template<class CharT, class Traits>
    static std::basic_ostream<CharT, Traits>& insert(std::basic_ostream<CharT, Traits>& os, const __m128i& f){
        union{
            uint64_t u64[2];
            __m128i m;
        }u;
        _mm_storeu_si128(&u.m, f);
        return os << u.u64[0] << " " << u.u64[1];
    }

    template<class CharT, class Traits>
    static std::basic_istream<CharT, Traits>& extract(std::basic_istream<CharT, Traits>& is, __m128i& f){
        uint64_t u64[2];
        is >> u64[0] >> u64[1];
        f = _mm_set_epi64x(u64[1], u64[0]);
        return is;
    }

    static bool is_equal(const __m128i& rhs, const __m128i& lhs){
        return 0xf==_mm_movemask_ps(_mm_castsi128_ps(_mm_cmpeq_epi32(lhs, rhs)));
    }

    static __m128i make_counter(){
        return _mm_setzero_si128();
    }

    static __m128i make_counter(uintmax_t v){
        __m128i ret = _mm_set_epi32(v, 0, 0, 0);
        return ret;
    }

    template <typename It>
    static __m128i make_counter(It first, It last, It* endp=0){
        union{
            uint32_t a[4];
            __m128i m;
        } u;
        detail::fill_array_int<32>(first, last, u.a);
        if(endp)
            *endp = first;
        else{
            // If the caller isn't interested in endp, then don't
            // permit non-zero values in the leftover range.
            while( first != last ){
                if( *first++ )
                    BOOST_THROW_EXCEPTION(std::invalid_argument("non-zero values in range ignored"));
            }
        }
        return u.m;
    }

#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)
    template <typename V>
    static __m128i make_counter(std::initializer_list<V> il){
        return make_counter(il.begin(), il.end());
    }
#endif

private:
    template <typename SeedSeq>
    static __m128i _make_counter_from_seed_seq(SeedSeq& seq){
        union{
            uint32_t a[4];
            __m128i m;
        } u;
        detail::seed_array_int<32>(seq, u.a);
        return u.m;
    }
    template <typename Tt, typename Prf, unsigned CtrBits, unsigned w, typename Dtraits, typename Rtraits, typename Ktrats>
    friend class counter_based_engine;

public:

    template<unsigned CtrBitsBits>
    static bool clr_highbits(__m128i& k){
        BOOST_STATIC_ASSERT(CtrBitsBits <= 32);
        BOOST_STATIC_CONSTANT(uint32_t, CtrBitsMask = low_bits_mask_t<32-CtrBitsBits>::sig_bits );
        uint32_t k3 = _mm_extract_epi32(k, 3);
        bool ret = !!( k3 & ~CtrBitsMask );
        k3 &= CtrBitsMask;
        k = _mm_insert_epi32(k, k3, 3);
        return ret;
    }

    template <unsigned CtrBits>
    static __m128i incr(__m128i c){
        BOOST_STATIC_ASSERT(CtrBits <= 128);
        BOOST_STATIC_ASSERT(CtrBits > 0);
        __m128i zeroone = _mm_set_epi64x(UINT64_C(0), UINT64_C(1));
        c = _mm_add_epi64(c, zeroone);
        return c;
    }

    template <unsigned CtrBits>
    static __m128i incr(__m128i d, boost::uintmax_t n){
        BOOST_STATIC_ASSERT(CtrBits <= 128);
        BOOST_STATIC_ASSERT(CtrBits > 0);
        __m128i incr128 = _mm_set_epi64x(0, n);
        d = _mm_add_epi64(d, incr128);
        return d;
    }

    template<unsigned w>
    static std::size_t size(){
        BOOST_STATIC_ASSERT(w==32 || w==64);
        return 128/w;
    }

    template <typename result_type, unsigned w>
    static result_type at(std::size_t n, __m128i v){
        BOOST_STATIC_ASSERT(w==32 || w==64);
        switch(w){
        case 32:
            switch(n){
            case 0: return _mm_extract_epi32(v, 0);
            case 1: return _mm_extract_epi32(v, 1);
            case 2: return _mm_extract_epi32(v, 2);
            case 3: return _mm_extract_epi32(v, 3);
            }
            BOOST_THROW_EXCEPTION(std::out_of_range("nth_result(n)"));
        case 64:
            switch(n){
            case 0: return _mm_extract_epi64(v, 0);
            case 1: return _mm_extract_epi64(v, 1);
            }
            BOOST_THROW_EXCEPTION(std::out_of_range("nth_result(n)"));
        default:
            // Silence a warning from icpc:
            BOOST_THROW_EXCEPTION(std::out_of_range("can't get here"));
        }
    }
};

} // namespace detail
} // namespace random
} // namespace boost

#endif


#include <boost/bind.hpp>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>

#include <mrsl_quadrotor_simulator/Quadrotor.h>
#include <mrsl_quadrotor_simulator/QuadrotorSO3AttitudeControl.h>

#include <ros/callback_queue.h>
#include <ros/ros.h>

#include <stdio.h>

namespace mrsl_quadrotor_simulator {
class QuadrotorPropulsion : public gazebo::ModelPlugin {
public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  QuadrotorPropulsion() : initialized_(false) { robot_name_ = std::string(""); }

  ~QuadrotorPropulsion() {
    node_handle_->shutdown();
    delete node_handle_;
  }

  void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
    // Make sure the ROS node for Gazebo has already been initialized
    if (!ros::isInitialized()) {
      ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable "
                       "to load plugin. "
                       << "Load the Gazebo system plugin "
                          "'libgazebo_ros_api_plugin.so' in the gazebo_ros "
                          "package)");
      return;
    }

    if (_sdf->HasElement("robotNamespace"))
      robot_name_ = _sdf->GetElement("robotNamespace")->Get<std::string>();

    node_handle_ = new ros::NodeHandle(robot_name_);
    link = _parent->GetLink();

    gazebo::physics::InertialPtr I = link->GetInertial();
    float Im[3][3];
#if GAZEBO_MAJOR_VERSION >= 8
    Im[0][0] = I->IXX();
    Im[0][1] = I->IXY();
    Im[1][0] = I->IXY();
    Im[1][1] = I->IYY();
    Im[1][2] = I->IYZ();
    Im[2][1] = I->IYZ();
    Im[2][2] = I->IZZ();
    Im[2][0] = I->IXZ();
    Im[0][2] = I->IXZ();
#else
    Im[0][0] = I->GetIXX();
    Im[0][1] = I->GetIXY();
    Im[1][0] = I->GetIXY();
    Im[1][1] = I->GetIYY();
    Im[1][2] = I->GetIYZ();
    Im[2][1] = I->GetIYZ();
    Im[2][2] = I->GetIZZ();
    Im[2][0] = I->GetIXZ();
    Im[0][2] = I->GetIXZ();
#endif

    quad_attitude_control.init(quad, Im);

    if (_sdf->HasElement("command_topic")) {
      command_topic_ = _sdf->GetElement("command_topic")->Get<std::string>();

      ros::SubscribeOptions ops;
      ops.callback_queue = &callback_queue_;
      ops.init<QuadrotorSO3AttitudeControl::CmdMsg>(
          command_topic_, 10,
          boost::bind(&QuadrotorSO3AttitudeControl::cmdCallback,
                      &quad_attitude_control, _1));
      command_sub_ = node_handle_->subscribe(ops);
    } else
      ROS_ERROR("command_topic has not been set up correctly, use either "
                "'so3_cmd' or 'trpy_cmd'");

    updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
        boost::bind(&QuadrotorPropulsion::OnUpdate, this, _1));
  }

  void OnUpdate(const gazebo::common::UpdateInfo &_info) {
    gazebo::common::Time current_time = _info.simTime;
    if (!initialized_) {
      last_time_ = current_time;
      initialized_ = true;
      return;
    }

    double dt = (current_time - last_time_).Double();
    last_time_ = current_time;

    callback_queue_.callAvailable();

    Quadrotor::State quad_state;
#if GAZEBO_MAJOR_VERSION >= 8
    ignition::math::Pose3d pose = link->WorldPose();
    ignition::math::Vector3d angular_velocity = link->RelativeAngularVel();
#else
    ignition::math::Pose3d pose = link->GetWorldPose().Ign();
    ignition::math::Vector3d angular_velocity = link->GetRelativeAngularVel().Ign();
#endif
    quad_state.omega(0) = angular_velocity.X();
    quad_state.omega(1) = angular_velocity.Y();
    quad_state.omega(2) = angular_velocity.Z();
    quad_state.ypr(0) = pose.Rot().Yaw();
    quad_state.ypr(1) = pose.Rot().Pitch();
    quad_state.ypr(2) = pose.Rot().Roll();

    Quadrotor::MotorState des_rpm =
        quad_attitude_control.getControl(quad_state);
    Quadrotor::Wrench wrench = quad.update(des_rpm, dt);

    link->AddRelativeForce(ignition::math::Vector3d(0, 0, wrench(0)));

    link->AddRelativeTorque(
        ignition::math::Vector3d(wrench(1), wrench(2), wrench(3)));
  }

private:
  // Pointer to the model
  gazebo::event::ConnectionPtr updateConnection;
  gazebo::physics::LinkPtr link; // Link for robot
  gazebo::common::Time last_time_;
  bool initialized_; // initialize time stamp
  std::string robot_name_;
  std::string command_topic_; // command that robot subscribe to

  ros::NodeHandle *node_handle_;
  ros::Subscriber command_sub_;
  ros::CallbackQueue callback_queue_;

  Quadrotor quad;
  QuadrotorSO3AttitudeControl quad_attitude_control;
};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(QuadrotorPropulsion)
}


/**
 * \file FFTCheckerFilter.cpp
 */

#include "FFTCheckerFilter.h"

#include <cmath>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

namespace ATK
{
  template<class DataType_>
  FFTCheckerFilter<DataType_>::FFTCheckerFilter()
  :TypedBaseFilter<DataType_>(1, 0)
  {
    FFTimpl.reset(new FFT<DataType_>);
  }

  template<class DataType_>
  FFTCheckerFilter<DataType_>::~FFTCheckerFilter()
  {
  }

  template<class DataType_>
  void FFTCheckerFilter<DataType_>::set_checks(const std::vector<std::pair<int, DataType> >& frequency_checks)
  {
    this->frequency_checks = frequency_checks;
  }

  template<class DataType_>
  void FFTCheckerFilter<DataType_>::setup()
  {
    FFTimpl->set_size(input_sampling_rate);
  }

  template<class DataType_>
  void FFTCheckerFilter<DataType_>::process_impl(int64_t size) const
  {
    for(int64_t i = 0; i < size/input_sampling_rate; ++i)
    {
      FFTimpl->process(converted_inputs[0] + i * input_sampling_rate, input_sampling_rate);
      std::vector<DataType_> output_freqs;
      FFTimpl->get_amp(output_freqs);

      for(int j = 0; j < frequency_checks.size(); ++j)
      {
        DataType amp = output_freqs[frequency_checks[j].first];
        DataType amp_check = frequency_checks[j].second * frequency_checks[j].second;
        if(amp_check < 0.001)
        {
          BOOST_REQUIRE_SMALL(amp, 0.001);
        }
        else
        {
          BOOST_REQUIRE_CLOSE(2*amp, amp_check, 0.1);
        }
      }
    }
  }

  template class FFTCheckerFilter<double>;
}


/*
 *  vec_scal_div_asgn_expr.h
 *  MTL4
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#ifndef MTL_VEC_SCAL_DIV_ASGN_EXPR_INCLUDE
#define MTL_VEC_SCAL_DIV_ASGN_EXPR_INCLUDE

#include <boost/static_assert.hpp>

#include <boost/numeric/mtl/vector/vec_scal_aop_expr.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/operation/sfunctor.hpp>

namespace mtl { namespace vector {

	// Model of VectorExpression
	template <class E1, class E2>
	struct vec_scal_div_asgn_expr
		: public vec_scal_aop_expr< E1, E2, mtl::sfunctor::divide_assign<typename E1::value_type, E2> >
	{
		typedef vec_scal_aop_expr< E1, E2, mtl::sfunctor::divide_assign<typename E1::value_type, E2> > base;
		vec_scal_div_asgn_expr( E1& v1, E2 const& v2 )
		: base( v1, v2 )
		{}
	};

} } // Namespace mtl::vector

#endif


#pragma once

#include <supereight/shared/commons.h>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include <cassert>
#include <memory>

namespace se {

template<typename T>
class ImageAccessor;

template<typename T, typename AllocatorT = Eigen::aligned_allocator<T>>
class Image {
public:
    using value_type = T;
    using size_type  = std::size_t;

    using accessor_type = ImageAccessor<value_type>;

    Image() = default;

    Image(const size_type width, const size_type height)
        : width_(width), height_(height) {
        assert(width_ > 0 && height_ > 0);
        data_ = allocator_.allocate(width_ * height_);
    }

    Image(const Eigen::Vector2i& size) : Image(size.x(), size.y()) {}
    Image(const size_type size) : Image(size, 1) {}

    Image(const Image& other) = delete;

    Image(Image&& other)
        : width_{other.width_}, height_{other.height_}, data_{other.data_} {
        other.width_  = 0;
        other.height_ = 0;
        other.data_   = nullptr;
    }

    ~Image() { allocator_.deallocate(data_, width_ * height_); };

    size_type width() const { return width_; };
    size_type height() const { return height_; };

    size_type size() const { return width_ * height_; };
    Eigen::Vector2i dim() const { return Eigen::Vector2i(width(), height()); }

    void resize(const size_type width, const size_type height) {
        if (width == width_ && height == height_) return;

        if (width * height == size()) {
            width_  = width;
            height_ = height;

            return;
        }

        allocator_.deallocate(data_, size());

        width_  = width;
        height_ = height;

        data_ = allocator_.allocate(size());
    }

    void resize(const size_type size) { resize(size, 1); }

    value_type* data() { return data_; }
    const value_type* data() const { return data_; }

    value_type& operator[](const size_type idx) { return data_[idx]; }
    const value_type& operator[](const size_type idx) const {
        return data_[idx];
    }

    value_type& operator()(const size_type x, const size_type y) {
        return data_[x + y * width_];
    }

    const value_type& operator()(const size_type x, const size_type y) const {
        return data_[x + y * width_];
    }

    accessor_type accessor() { return accessor_type(data_, width_, height_); }

    const accessor_type accessor() const {
        return accessor_type(data_, width_, height_);
    }

private:
    size_type width_  = 0;
    size_type height_ = 0;

    value_type* data_ = nullptr;

    AllocatorT allocator_;
};

template<typename T>
class ImageAccessor {
public:
    using value_type = T;
    using size_type  = std::size_t;

    ImageAccessor() = delete;

    SE_DEVICE_FUNC
    size_type width() const { return width_; };

    SE_DEVICE_FUNC
    size_type height() const { return height_; };

    SE_DEVICE_FUNC
    size_type size() const { return width_ * height_; };

    SE_DEVICE_FUNC
    Eigen::Vector2i dim() const { return Eigen::Vector2i(width(), height()); }

    SE_DEVICE_FUNC
    value_type* data() { return data_; }

    SE_DEVICE_FUNC
    const value_type* data() const { return data_; }

    SE_DEVICE_FUNC
    value_type& operator[](size_type idx) { return data_[idx]; }

    SE_DEVICE_FUNC
    const value_type& operator[](size_type idx) const { return data_[idx]; }

    SE_DEVICE_FUNC
    value_type& operator()(const size_type x, const size_type y) {
        return data_[x + y * width_];
    }

    SE_DEVICE_FUNC
    const value_type& operator()(const size_type x, const size_type y) const {
        return data_[x + y * width_];
    }

private:
    ImageAccessor(
        value_type* const data, const size_type width, const size_type height)
        : data_{data}, width_{width}, height_{height} {}

    ImageAccessor(const value_type* const data, const size_type width,
        const size_type height)
        : data_{const_cast<value_type*>(data)}, width_{width}, height_{height} {
    }

    value_type* const data_ = nullptr;

    size_type width_  = 0;
    size_type height_ = 0;

    template<typename, typename>
    friend class Image;
};

} // end namespace se


/*
 * NOTE:
 *
 * Linker flags required for Poco C++:
 *
 * -lPocoFoundation
 *
 */
#include <algorithm>
#include <iostream>
#include <limits>

#include <boost/range/irange.hpp>

#include <Poco/DigestEngine.h>
#include <Poco/DigestStream.h>
#include <Poco/MD5Engine.h>

auto get_value() {
	const auto key = "ckczppom";

	auto md5 = Poco::MD5Engine{};
	auto out = Poco::DigestOutputStream{md5};

	using uint32 = std::uint32_t;

	const auto prefix = boost::irange(uint32{3});

	const auto begin = uint32{117946 + 1}; // avoid repeating computations from Part 1
	const auto end   = std::numeric_limits<uint32>::max();

	const auto range = boost::irange(begin, end);

	auto result = std::find_if(range.begin(), range.end(), [&] (auto i) {

		// computes the digest of (key + i) to be retrieved by the engine later
		out << key << i;
		out.close();

		const auto& hash = md5.digest();

		// each element in hash is one-byte-sized (char) and represents two hexadecimal characters -
		// in other words, an all-zero hash[0-2] (3 elements) is equal to "000000"
		const auto has_prefix = std::all_of(prefix.begin(), prefix.end(), [&hash] (auto i) {
			return (hash[i] == 0);
		});

		return has_prefix;
	});

	return *result;
}

int main() {

	std::cout << get_value() << std::endl;

	return 0;
}


#include <boost/hana/permutations.hpp>


/**
 * @file backward_visitor.hpp
 * @author Marcus Edel
 *
 * This file provides an abstraction for the Backward() function for different
 * layers and automatically directs any parameter to the right layer type.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_VISITOR_BACKWARD_VISITOR_HPP
#define MLPACK_METHODS_ANN_VISITOR_BACKWARD_VISITOR_HPP

#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>

#include <boost/variant.hpp>

namespace mlpack {
namespace ann {

/**
 * BackwardVisitor executes the Backward() function given the input, error and
 * delta parameter.
 */
class BackwardVisitor : public boost::static_visitor<void>
{
 public:
  //! Execute the Backward() function given the input, error and delta
  //! parameter.
  BackwardVisitor(arma::mat&& input, arma::mat&& error, arma::mat&& delta);

  //! Execute the Backward() function for the layer with the specified index.
  BackwardVisitor(arma::mat&& input, arma::mat&& error, arma::mat&& delta,
      const size_t index);

  //! Execute the Backward() function.
  template<typename LayerType>
  void operator()(LayerType* layer) const;

  void operator()(MoreTypes layer) const;

 private:
  //! The input parameter set.
  arma::mat&& input;

  //! The error parameter.
  arma::mat&& error;

  //! The delta parameter.
  arma::mat&& delta;

  //! The index of the layer to run.
  size_t index;

  //! Indicates whether to use index or not
  bool hasIndex;

  //! Execute the Backward() function if the module does not have Run()
  //! check.
  template<typename T>
  typename std::enable_if<
      !HasRunCheck<T, bool&(T::*)(void)>::value, void>::type
  LayerBackward(T* layer, arma::mat& input) const;

  //! Execute the Backward() function if the module is has Run() function.
  template<typename T>
  typename std::enable_if<
      HasRunCheck<T, bool&(T::*)(void)>::value, void>::type
  LayerBackward(T* layer, arma::mat& input) const;
};

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "backward_visitor_impl.hpp"

#endif


#include "v4r/registration/MultiSessionModelling.h"
#include "v4r/common/visibility_reasoning.h"
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <v4r/common/miscellaneous.h>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

template<class PointT>
v4r::Registration::MultiSessionModelling<PointT>::MultiSessionModelling()
{

}

template<class PointT>
float
v4r::Registration::MultiSessionModelling<PointT>::computeFSV (PointCloudTPtr & cloud,
                                                              pcl::PointCloud<pcl::Normal>::Ptr & normals,
                                                              std::vector<int> & indices,
                                                              Eigen::Matrix4f & pose,
                                                              PointCloudTPtr & range_image)
{
    v4r::VisibilityReasoning<PointT> vr (525.f, 640, 480);
    vr.setThresholdTSS (0.01f);

    PointCloudTPtr model(new pcl::PointCloud<PointT>());
    pcl::transformPointCloud(*cloud, indices, *model, pose);

    pcl::PointCloud<pcl::Normal>::Ptr model_normals (new pcl::PointCloud<pcl::Normal>);
    v4r::transformNormals(*normals, *model_normals, indices, pose);

    //Eigen::Matrix4f identity = Eigen::Matrix4f::Identity();

    float fsv = vr.computeFSVWithNormals (range_image, model, model_normals);

    //the more points on the same surface the more reliable this is, so increase fsv if not many points
    int ss = vr.getFSVUsedPoints();
    float ratio = 1.f - ss / static_cast<float>(model->points.size());

    //std::cout << model->points.size() << " " << indices.size() << " fsv:" << fsv << std::endl;

    /*if(fsv > 0.1)
    {*/
    /*    ///debug
        pcl::visualization::PCLVisualizer vis("correspondences");
        //int v1,v2;
        //vis.createViewPort(0,0,0.5,1,v1);
        //vis.createViewPort(0.5,0,1,1,v2);

        pcl::visualization::PointCloudColorHandlerCustom<PointT> handler(model, 255, 0, 0);
        vis.addPointCloud(range_image, "cloud_2");
        vis.addPointCloud(model, handler, "cloud_1");

        vis.spin();
    */
    /*}*/

    return fsv * ratio;
}

template<class PointT>
void
v4r::Registration::MultiSessionModelling<PointT>::computeCost(EdgeBetweenPartialModels & edge)
{
    //fsv computation
    //take all views of edge_i with respective indices and compute FSV with respect to the views of j_
    //do i need normals?

    float fsv = 0.f;
    int non_valid = 0;
    int total = 0;
    for(size_t ii=session_ranges_[edge.i_].first; ii <= session_ranges_[edge.i_].second; ii++)
    {
        PointCloudTPtr cloud = clouds_[ii];
        std::vector<int> & indices = getIndices(ii);
        Eigen::Matrix4f pose = edge.transformation_.inverse() * getPose(ii);

        for(size_t jj=session_ranges_[edge.j_].first; jj <= session_ranges_[edge.j_].second; jj++)
        {
            Eigen::Matrix4f pose_to_jj = poses_[jj].inverse() * pose;
            float fsv_loc = computeFSV(cloud, normals_[ii], indices, pose_to_jj, clouds_[jj]);
            if(fsv_loc < 0)
            {
                non_valid++;
            }
            else
            {
                fsv += fsv_loc;
            }

            total++;
        }
    }

    if(non_valid == total)
    {
        std::cout << "No valid edge..." << std::endl;
        edge.cost_ = std::numeric_limits<float>::infinity();
    }
    else
    {
        edge.cost_ = fsv / (total - non_valid);
    }
}

template<class PointT>
void
v4r::Registration::MultiSessionModelling<PointT>::compute()
{

    for(size_t a=0; a < reg_algos_.size(); a++)
    {
        reg_algos_[a]->setMSM(this);
        reg_algos_[a]->initialize(session_ranges_);
    }

    std::vector< std::vector< std::vector<EdgeBetweenPartialModels> > > edges;
    edges.resize(session_ranges_.size());
    for(size_t i=0; i < session_ranges_.size(); i++)
    {
        edges[i].resize(session_ranges_.size());
    }

    //for each session pair, call the class that registers two partial models and returns a set of poses
    for(size_t i=0; i < session_ranges_.size(); i++)
    {
        std::pair<int, int> pair_i = session_ranges_[i];
        for(size_t j=(i+1); j < session_ranges_.size(); j++)
        {
            std::pair<int, int> pair_j = session_ranges_[j];

            for(size_t a=0; a < reg_algos_.size(); a++)
            {
                reg_algos_[a]->setSessions(pair_i, pair_j);
                reg_algos_[a]->compute(i,j);

                //poses transform the RF of pair_j to the RF of pair_i
                std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > poses;
                reg_algos_[a]->getPoses(poses);

                std::cout << "poses between " << j << " and " << i << " :" << poses.size() << std::endl;

                for(size_t p=0; p < poses.size(); p++)
                {
                    EdgeBetweenPartialModels ebpm;
                    ebpm.transformation_ = poses[p];
                    ebpm.cost_ = -1;
                    ebpm.i_ = i;
                    ebpm.j_ = j;

                    edges[i][j].push_back(ebpm);
                }
            }
        }
    }

    //for each edge in the graph, compute a score quantifying quality of alignment
    //overlap? number of outliers? fsv average?

    for(size_t i=0; i < session_ranges_.size(); i++)
    {
        for(size_t j=(i+1); j < session_ranges_.size(); j++)
        {
            if(edges[i][j].size() > 0)
            {
                std::cout << "Number of edges:" << edges[i][j].size() << std::endl;
                //iterate over the edges and compute cost_ (the higher, the worse)

#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
                for(size_t k=0; k < edges[i][j].size(); k++)
                {
                    computeCost(edges[i][j][k]);
                }
            }
        }
    }

    //final alignment between partial models? MST
    int final_alignment = 0; //0-MST, 1-something fancy?

    switch(final_alignment)
    {
    case 0:
    {

        std::vector<PointCloudTPtr> partial_model_clouds(session_ranges_.size());

        for(size_t i=0; i < session_ranges_.size(); i++)
        {
            partial_model_clouds[i].reset(new pcl::PointCloud<PointT>);

            for(int t=session_ranges_[i].first; t <= session_ranges_[i].second; t++)
            {
                typename pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>);
                pcl::copyPointCloud(*getCloud(static_cast<size_t>(t)), getIndices(static_cast<size_t>(t)), *transformed);
                Eigen::Matrix4f pose_inv = getPose(static_cast<size_t>(t));
                pcl::transformPointCloud(*transformed, *transformed, pose_inv);
                *partial_model_clouds[i] += *transformed;
            }

        }

        Graph G;
        for(size_t i=0; i < session_ranges_.size(); i++)
        {
            boost::add_vertex((int)i, G);
        }

        for(size_t i=0; i < session_ranges_.size(); i++)
        {
            for(size_t j=(i+1); j < session_ranges_.size(); j++)
            {
                if(edges[i][j].size() > 0)
                {
                    float min_cost = std::numeric_limits<float>::infinity();
                    int min_k = -1;

                    for(size_t k=0; k < edges[i][j].size(); k++)
                    {
                        //std::cout << "cost:" << edges[i][j][k].cost_ << std::endl;
                        if(edges[i][j][k].cost_ < min_cost)
                        {
                            min_cost = edges[i][j][k].cost_;
                            min_k = static_cast<int>(k);
                        }
                    }

                    std::cout << j << " => " << i << " " << min_cost << " " << min_k << std::endl;
                    //add edge to graph... transformation maps from j to i (otherwise invert)
                    myEdge e;
                    e.edge_weight = edges[i][j][min_k].cost_;
                    e.transformation = edges[i][j][min_k].transformation_;
                    e.source_id = j;
                    e.target_id = i;
                    boost::add_edge ((int)j, (int)i, e, G);

                    /*std::stringstream title_str;
                    title_str << "best pw from " << j << " " << i;
                    pcl::visualization::PCLVisualizer vis(title_str.str().c_str());
                    vis.addCoordinateSystem(0.1);
                    for(size_t k=0; k < edges[i][j].size(); k++)
                    {
                        if(edges[i][j][k].cost_ * 0.25 <= min_cost)
                        {
                            std::cout << "cost:" << edges[i][j][k].cost_ << std::endl;

                            {
                                pcl::visualization::PointCloudColorHandlerRGBField<PointT> handler(partial_model_clouds[i]);
                                vis.addPointCloud(partial_model_clouds[i], handler, "cloud_i");
                            }

                            typename pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>);
                            pcl::transformPointCloud(*partial_model_clouds[j], *transformed, edges[i][j][k].transformation_);

                            {
                                pcl::visualization::PointCloudColorHandlerRGBField<PointT> handler(transformed);
                                vis.addPointCloud(transformed, handler, "cloud_j");
                            }

                            vis.spin();
                            vis.removeAllPointClouds();
                        }
                    }*/

                }
            }
        }

        boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, G);
        Graph MST;
        for(size_t i=0; i < session_ranges_.size(); i++)
            boost::add_vertex((int)i, MST);

        //there is a bug in boost 1.54 that causes negative weights error (even though weights are positive)
        /*std::vector < boost::graph_traits<Graph>::vertex_descriptor > p (boost::num_vertices (G));
        boost::prim_minimum_spanning_tree (G, &p[0]);

        for (std::size_t i = 0; i != p.size (); ++i)
        {
            if (p[i] != i)
                std::cout << "parent[" << i << "] = " << p[i] << std::endl;
            else
                std::cout << "parent[" << i << "] = no parent" << std::endl;
        }

        typedef typename Graph::edge_iterator EdgeIterator;
        std::pair<EdgeIterator, EdgeIterator> edges = boost::edges(G);
        EdgeIterator edge;

        for (edge = edges.first; edge != edges.second; edge++)
        {
            typename boost::graph_traits<Graph>::vertex_descriptor s, t;
            s = boost::source(*edge, G);
            t = boost::target(*edge, G);

            if(p[s] == t || p[t] == s)
            {
                //edge in prim
                boost::add_edge ((int)s, (int)t, weightmap[*edge], MST);
            }
        }*/

        std::vector < Edge > spanning_tree;
        boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));

        std::cout << "Print the edges in the MST:" << std::endl;
        for (std::vector < Edge >::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei)
        {
            std::cout << source(*ei, G) << " " << target(*ei, G) << std::endl;
            boost::add_edge(source(*ei, G), target(*ei, G), weightmap[*ei], MST);
        }

        std::cout << boost::num_edges(MST) << " " << boost::num_vertices(MST) << std::endl;
        output_session_poses_.resize(boost::num_vertices(MST));

        computeAbsolutePoses(MST, output_session_poses_);

        //finally, define output_cloud_poses_
        output_cloud_poses_.resize(clouds_.size());

        for(size_t i=0; i < output_session_poses_.size(); i++)
        {
            std::cout << output_session_poses_[i] << std::endl;
            for(int j=session_ranges_[i].first; j <= session_ranges_[i].second; j++)
            {
                Eigen::Matrix4f trans = output_session_poses_[i] * poses_[j];
                output_cloud_poses_[j] = trans;
            }
        }

        break;
    }

    default:
    {
        break;
    }
    }
}

template class V4R_EXPORTS v4r::Registration::MultiSessionModelling<pcl::PointXYZRGB>;


//          Copyright Gavin Band 2008 - 2012.
// 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)

#include <string>
#include <deque>
#include <iostream>
#include <algorithm>
#include <boost/math/distributions/chi_squared.hpp>
#include "genfile/string_utils.hpp"
#include "appcontext/OptionProcessor.hpp"
#include "appcontext/ApplicationContext.hpp"
#include "appcontext/get_current_time_as_string.hpp"
#include "appcontext/ProgramFlow.hpp"
#include "config/qctool_version_autogenerated.hpp"
#include "config/config.hpp"
#if HAVE_MGL
	#include "mgl/mgl.h"
	#include "mgl/mgl_zb.h"
#endif

namespace globals {
	std::string const program_name = "inflation" ;
	std::string const program_version = qctool_revision ;
}

struct InflationApplication: public appcontext::ApplicationContext
{
public:
	static void declare_options( appcontext::OptionProcessor& options ) {
		options.set_help_option( "-help" ) ;
		options[ "-analysis-name" ]
			.set_description( "Specify a name to label results from this analysis with.  (This applies to modules which store their results in a qcdb file.)" )
			.set_takes_single_value()
			.set_default_value( "inflation analysis, started " + appcontext::get_current_time_as_string() ) ;
		options [ "-qq-plot" ]
			.set_description( "Specify that " + globals::program_name + " should make a qq-plot in the file with the given name." )
			.set_takes_single_value() ;
		options [ "-log" ]
			.set_description( "Specify that " + globals::program_name + " should write a log file to the given file." )
			.set_takes_single_value() ;
	}

public:
	InflationApplication( appcontext::OptionProcessor::UniquePtr options, int argc, char** argv ):
		appcontext::ApplicationContext( globals::program_name, globals::program_version , options, argc, argv, "-log" )
	{
	}

	void process() {
		unsafe_process();
	}
private:

	void unsafe_process() {
		get_ui_context().logger() << application_name() << ": reading..." ;

		std::string line ;
		std::deque< float > numbers ;
		std::size_t missing_count = 0 ;
		while( std::getline( std::cin, line )) {
			if( line.empty() || line.find_first_not_of( "0123456789.E+-" ) != std::string::npos ) {
				++missing_count ;
			} else {
				numbers.push_back( genfile::string_utils::to_repr< float >( line ) ) ;
			}

			if( ( numbers.size() + missing_count ) % 1000000 == 0 ) {
				get_ui_context().logger() << ( numbers.size() + missing_count ) << ".." ;
			}
		}

		get_ui_context().logger() << ( numbers.size() / 1000000 ) << ".\n" ;
		get_ui_context().logger() << "inflation.cpp: read " << numbers.size() << " P-values from std::cin.\n" ;
		get_ui_context().logger() << "inflation.cpp: converting to chi-squared statistics...\n" ;
		using namespace boost::math ;
		chi_squared_distribution< float > chi_square( 1 ) ;
		for( std::size_t i = 0; i < numbers.size(); ++i ) {
			numbers[i] = quantile( complement( chi_square, numbers[i] )) ;
		}

		get_ui_context().logger() << "inflation.cpp: finding median...\n" ;

		std::cout << "analysis,N,missing,median,lambda,1pc_adjusted_median,1pc_adjusted_lambda\n" ;
		std::cout
			<< options().get< std::string >( "-analysis-name" ) << ","
			<< numbers.size() << ","
			<< missing_count ;
		// Compute basic lambda
		{
			std::size_t const mid = numbers.size() / 2 ;
			std::nth_element( numbers.begin(), numbers.begin() + mid, numbers.end() ) ;
			double const median = numbers[ mid ] ;
			std::cout << "," << median << "," << (median / quantile( complement( chi_square, 0.5 ))) ;
		}

		if( options().check( "-qq-plot" )) {
#if HAVE_MGL
			get_ui_context().logger() << "inflation.cpp: making qq-plot...\n" ;
			plot( numbers, options().get< std::string >( "-qq-plot" ), options().get< std::string >( "-analysis-name" ) ) ;
#else
			get_ui_context().logger() << "!! InflationApplication::unsafe_process(): you must build with MathGL support for -qq-plot to be supported.  Skipping plots...\n" ;
#endif
		}

		// Compute lambda after removing top 99% of signal
		{
			std::size_t const top_one_percent = numbers.size() * 0.99 ;
			std::deque< float >::iterator top_one_percent_i = numbers.begin() + top_one_percent ;
			numbers.erase( top_one_percent_i, numbers.end() ) ;
		}

		{
			std::size_t const mid = numbers.size() / 2 ;
			std::nth_element( numbers.begin(), numbers.begin() + mid, numbers.end() ) ;
			double const median = numbers[ mid ] ;
			std::cout << "," << median << "," << (median / quantile( complement( chi_square, 0.5 ))) ;
		}
		std::cout << "\n" ;
	}

#if HAVE_MGL
	void plot( std::deque< float >& numbers, std::string const& filename, std::string const& title ) {
		std::sort( numbers.begin(), numbers.end() ) ;

		mglData
			observed( numbers.size() ),
			expected( numbers.size() )
		;

		using namespace boost::math ;
		{
			chi_squared_distribution< float > chi_squared( 1 ) ;
			for( std::size_t i = 0; i < numbers.size(); ++i ) {
				observed.a[i] = numbers[i] ;
				expected.a[i] = quantile( complement( chi_squared, float( numbers.size() - i ) / ( numbers.size() + 1 ) ) ) ;
			}
		}
		mglGraphZB graph( 800, 800 ) ;
		graph.Light( true ) ;
		graph.Clf( mglColor( 1.0, 1.0, 1.0 ) ) ;
		graph.Title( title.c_str(), 0, 4 ) ;
		graph.SetTickLen( 0.04 ) ;
		graph.SetRanges(
			0.0, 30,
			0.0, 30
		) ;
		graph.Axis( "x", true ) ;
		graph.Axis( "y", true ) ;
		graph.CAxis( 0.0, 3.0 ) ;

		graph.Plot( expected, observed ) ; //, observed ) ;
		graph.WritePNG( filename.c_str(), "", false ) ;
	}
#endif
} ;

int main( int argc, char** argv ) {
	try {
		appcontext::OptionProcessor::UniquePtr options( new appcontext::OptionProcessor ) ;
		InflationApplication::declare_options( *options ) ;
		InflationApplication app( options, argc, argv ) ;
		app.process() ;
	} catch ( appcontext::HaltProgramWithReturnCode const& e ) {
		return e.return_code() ;
	}
	return 0 ;
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <metaCommand.h>
#include <mitkDiffusionCommandLineParser.h>
#include <usAny.h>
#include <mitkIOUtil.h>
#include <mitkLexicalCast.h>
#include <itksys/SystemTools.hxx>
#include <itkDirectory.h>
#include <mitkFiberBundle.h>
#include <vtkTransformPolyDataFilter.h>
#include <fstream>
#include <chrono>
#include <boost/progress.hpp>
#include <itkTractsToFiberEndingsImageFilter.h>
#include <itkTractDensityImageFilter.h>
#include <itkImageRegionConstIterator.h>

typedef itksys::SystemTools ist;
typedef itk::Image<unsigned char, 3>    ItkUcharImgType;
typedef itk::Image<unsigned int, 3>    ItkUIntImgType;

/*!
\brief
*/
int main(int argc, char* argv[])
{
  mitkDiffusionCommandLineParser parser;

  parser.setTitle("Calculate Overlap");
  parser.setCategory("Fiber Tracking Evaluation");
  parser.setDescription("");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("tractogram", "", mitkDiffusionCommandLineParser::String, "", "file", us::Any(), false, false, false, mitkDiffusionCommandLineParser::Input);
  parser.addArgument("overlap_image", "", mitkDiffusionCommandLineParser::String, "", "file", us::Any(), false, false, false, mitkDiffusionCommandLineParser::Input);
  parser.addArgument("ep_image", "", mitkDiffusionCommandLineParser::String, "", "file", us::Any(), false, false, false, mitkDiffusionCommandLineParser::Input);

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string input_tractogram = us::any_cast<std::string>(parsedArgs["tractogram"]);
  std::string mask_file = us::any_cast<std::string>(parsedArgs["overlap_image"]);
  std::string mask_file2 = us::any_cast<std::string>(parsedArgs["ep_image"]);

  try
  {
    mitk::FiberBundle::Pointer fib = mitk::IOUtil::Load<mitk::FiberBundle>(input_tractogram);
    mitk::Image::Pointer mask = mitk::IOUtil::Load<mitk::Image>(mask_file);
    mitk::Image::Pointer mask2 = mitk::IOUtil::Load<mitk::Image>(mask_file2);

    ItkUcharImgType::Pointer itk_mask;
    mitk::CastToItkImage(mask, itk_mask);

    ItkUcharImgType::Pointer itk_mask2;
    mitk::CastToItkImage(mask2, itk_mask2);

    float ol = fib->GetOverlap(itk_mask);
    float ep = fib->GetNumEpFractionInMask(itk_mask2, true);
    MITK_INFO << "Overlap<<" << ol;
    MITK_INFO << "EP-Fraction<<" << ep;
  }
  catch (const itk::ExceptionObject& e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (std::exception& e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


#include <pointmatcher/PointMatcher.h>
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <Eigen/Dense>
#include <fstream>

#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/point_types.h>

namespace py = pybind11;
typedef PointMatcher<float> PM;
typedef PM::Matrix Matrix;
typedef PM::IntMatrix IntMatrix;
typedef PM::DataPoints DP;
typedef std::shared_ptr<PM::DataPoints> DPPtr;

DPPtr fromEigen(const Matrix &mat)
{
  DP::Labels labels;
  labels.push_back(DP::Label("x", 1));
  labels.push_back(DP::Label("y", 1));
  if (mat.cols() == 3)
    labels.push_back(DP::Label("z", 1));
  labels.push_back(DP::Label("pad", 1));

  Matrix padded_mat = Matrix::Ones(labels.size(), mat.rows());
  padded_mat.block(0, 0, labels.size() - 1, mat.rows()) = mat.transpose();

  DPPtr pc(new DP(padded_mat, labels));
  return pc;
}

DPPtr fromEigen(const Matrix &mat, const Matrix &desc)
{
  DP::Labels labels;
  labels.push_back(DP::Label("x", 1));
  labels.push_back(DP::Label("y", 1));
  if (mat.cols() == 3)
    labels.push_back(DP::Label("z", 1));
  labels.push_back(DP::Label("pad", 1));

  Matrix padded_mat = Matrix::Ones(labels.size(), mat.rows());
  padded_mat.block(0, 0, labels.size() - 1, mat.rows()) = mat.transpose();

  DP::Labels desc_labels;
  for (int col = 0; col < desc.cols(); ++col)
    desc_labels.push_back(DP::Label("desc" + std::to_string(col), 1));

  DPPtr pc(new DP(padded_mat, labels, desc.transpose(), desc_labels));
  return pc;
}

Matrix remove_outlier(const Matrix &mat_in, double radius, int min_points)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(mat_in.rows(), 1));
  for (int row = 0; row < mat_in.rows(); ++row)
  {
    cloud_in->at(row).x = mat_in(row, 0);
    cloud_in->at(row).y = mat_in(row, 1);
    if (mat_in.cols() == 3)
      cloud_in->at(row).z = mat_in(row, 2);
  }

  pcl::RadiusOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud_in);
  sor.setRadiusSearch(radius);
  sor.setMinNeighborsInRadius(min_points);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
  sor.filter(*cloud_out);
  Matrix mat_out = cloud_out->getMatrixXfMap();
  return mat_out.topRows(mat_in.cols()).transpose();
}

Matrix density_filter(const Matrix &mat_in, int knn, float min_density, float max_density)
{
  if (mat_in.rows() == 0)
    return mat_in;

  PointMatcherSupport::Parametrizable::Parameters params1;
  params1["knn"] = std::to_string(knn);
  params1["keepNormals"] = "0";
  params1["keepDensities"] = "1";

  std::shared_ptr<PM::DataPointsFilter> filter =
      PM::get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", params1);
  DPPtr cloud_in = fromEigen(mat_in);
  filter->inPlaceFilter(*cloud_in);

  PointMatcherSupport::Parametrizable::Parameters params2;
  params2["minDensity"] = std::to_string(min_density);
  params2["maxDensity"] = std::to_string(max_density);

  filter = PM::get().DataPointsFilterRegistrar.create("MaxDensityDataPointsFilter", params2);
  filter->inPlaceFilter(*cloud_in);
  return cloud_in->features.topRows(mat_in.cols()).transpose();
}

std::pair<Matrix, Matrix> density_filter(const Matrix &mat_in, const Matrix &desc_in, int knn, float min_density,
                                         float max_density)
{
  if (mat_in.rows() == 0)
    return std::make_pair(mat_in, desc_in);

  PointMatcherSupport::Parametrizable::Parameters params1;
  params1["knn"] = std::to_string(knn);
  params1["keepNormals"] = "0";
  params1["keepDensities"] = "1";

  std::shared_ptr<PM::DataPointsFilter> filter =
      PM::get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", params1);
  DPPtr cloud_in = fromEigen(mat_in, desc_in);
  filter->inPlaceFilter(*cloud_in);

  PointMatcherSupport::Parametrizable::Parameters params2;
  params2["minDensity"] = std::to_string(min_density);
  params2["maxDensity"] = std::to_string(max_density);

  filter = PM::get().DataPointsFilterRegistrar.create("MaxDensityDataPointsFilter", params2);
  filter->inPlaceFilter(*cloud_in);

  Matrix mat_out = cloud_in->features.topRows(mat_in.cols()).transpose();
  Matrix desc_out = cloud_in->descriptors.transpose();
  return std::make_pair(mat_out, desc_out);
}

Matrix downsample(const Matrix &mat_in, float resolution)
{
  if (mat_in.rows() == 0)
    return mat_in;

  PointMatcherSupport::Parametrizable::Parameters params;
  params["maxSizeByNode"] = std::to_string(resolution);
  params["samplingMethod"] = "3";
  std::shared_ptr<PM::DataPointsFilter> filter =
      PM::get().DataPointsFilterRegistrar.create("OctreeGridDataPointsFilter", params);
  DPPtr cloud_in = fromEigen(mat_in);
  filter->inPlaceFilter(*cloud_in);
  return cloud_in->features.topRows(mat_in.cols()).transpose();
}

std::pair<Matrix, Matrix> downsample(const Matrix &mat_in, const Matrix &desc_in, float resolution)
{
  if (mat_in.rows() == 0)
    return std::make_pair(mat_in, desc_in);

  PointMatcherSupport::Parametrizable::Parameters params;
  params["maxSizeByNode"] = std::to_string(resolution);
  params["samplingMethod"] = "3";
  std::shared_ptr<PM::DataPointsFilter> filter =
      PM::get().DataPointsFilterRegistrar.create("OctreeGridDataPointsFilter", params);
  DPPtr cloud_in = fromEigen(mat_in, desc_in);
  filter->inPlaceFilter(*cloud_in);

  Matrix mat_out = cloud_in->features.topRows(mat_in.cols()).transpose();
  Matrix desc_out = cloud_in->descriptors.transpose();
  return std::make_pair(mat_out, desc_out);
}

std::pair<IntMatrix, Matrix> match(const Matrix &mat_ref, const Matrix &mat_in, int knn, float max_dist)
{
  PointMatcherSupport::Parametrizable::Parameters params;
  params["knn"] = std::to_string(knn);
  params["maxDist"] = std::to_string(max_dist);
  std::shared_ptr<PM::Matcher> matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);

  DPPtr cloud_ref = fromEigen(mat_ref);
  DPPtr cloud_in = fromEigen(mat_in);

  matcher->init(*cloud_ref);
  PM::Matches matches = matcher->findClosests(*cloud_in);
  return std::make_pair(matches.ids, matches.dists);
}

PYBIND11_MODULE(pcl, m)
{
  m.def("remove_outlier", &remove_outlier);
  m.def("density_filter", (Matrix(*)(const Matrix &, int, float, float)) & density_filter);
  m.def("density_filter",
        (std::pair<Matrix, Matrix>(*)(const Matrix &, const Matrix &, int, float, float)) & density_filter);
  m.def("downsample", (Matrix(*)(const Matrix &, float)) & downsample);
  m.def("downsample", (std::pair<Matrix, Matrix>(*)(const Matrix &, const Matrix &, float)) & downsample);
  m.def("match", &match);
  py::class_<PM::ICP>(m, "ICP")
      .def(py::init<>())
      .def("loadFromYaml",
           [](PM::ICP &icp, const std::string &filename) {
             std::ifstream ifs(filename);
             if (!ifs.is_open())
             {
               std::cout << "Failed to load " << filename << ". Use default configuration." << std::endl;
               icp.setDefault();
             }
             else
               icp.loadFromYaml(ifs);
           })
      .def("compute",
           [](PM::ICP &icp, const Matrix &source, const Matrix &target, const Matrix &guess) {
             DPPtr pc_source = fromEigen(source);
             DPPtr pc_target = fromEigen(target);
             Matrix T = guess;
             try
             {
               T = icp(*pc_source, *pc_target, guess);
             }
             catch (PM::ConvergenceError &e)
             {
               return std::make_pair(e.what(), T);
             }
             return std::make_pair("success", T);
           })
      .def("getCovariance", [](const PM::ICP &icp) { return icp.errorMinimizer->getCovariance(); });
}


#include "focal_grid.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <math.h>
#include <chrono>
#include <vector>
#include <map>
#include <unordered_map>
#include <iterator>
#include <future>
#include <thread>
#include <algorithm>
#include <dirent.h>
#include <armadillo>
#include <boost/python/call.hpp>

using namespace std;

FocalGrid::FocalGrid() {
	// empty
}

bool FocalGrid::SetGridParameters(session_data &session, frame_data &frame, int NX, int NY, int NZ, double DS, double X0, double Y0, double Z0) {

	bool grid_loaded = true;

	// Set the focal grid parameters:
	try {

		// grid parameters:
		nx = NX;
		ny = NY;
		nz = NZ;
		ds = DS;
		x0 = X0;
		y0 = Y0;
		z0 = Z0;

		// load calibration file:
		string calib_file = session.session_loc + "/" + session.cal_loc + "/" + session.cal_name;

		arma::Mat<double> CalibMatrix;

		CalibMatrix.load(calib_file);

		// Clear vectors:
		image_size.clear();
		calib_mat.clear();
		X_xyz.clear();
		X_uv.clear();
		uv_offset.clear();

		// Set N_cam
		N_cam = session.N_cam;

		for (int i=0; i<N_cam; i++) {
			image_size.push_back(make_tuple(get<0>(frame.image_size[i]),get<1>(frame.image_size[i])));
			calib_mat.push_back(CalibMatrix.col(i));
			X_xyz.push_back(FocalGrid::Camera2WorldMatrix(CalibMatrix.col(i)));
			X_uv.push_back(FocalGrid::World2CameraMatrix(CalibMatrix.col(i)));
			arma::Col<double> uv_off_i = {CalibMatrix(10,i)/2.0-CalibMatrix(12,i)/2.0, CalibMatrix(9,i)/2.0-CalibMatrix(11,i)/2.0, 0.0};
			uv_offset.push_back(uv_off_i);
		}

	}
	catch (...) {
		cout << "Could not set focal grid parameters." << endl;
		grid_loaded = false;
	}

	return grid_loaded;
}

bool FocalGrid::ConstructFocalGrid(PyObject* progress_func) {

	bool grid_build = true;

	int voxel_ind = 0;

	int progress = 0;

	try {

		pix2vox.clear();
		vox2pix.clear();

		for (int k=0; k<nz; k++) {
			progress = (int) 100.0*(((k+1)*1.0)/(nz*1.0));
			boost::python::call<void>(progress_func,progress);
			for (int j=0; j<ny; j++) {
				for (int i=0; i<nx; i++) {
					vector<int> uv_voxel = FocalGrid::CheckVoxel(i, j, k);
					if (uv_voxel.size()==N_cam) {
						voxel_ind = k*nx*ny+j*nx+i;
						pix2vox.insert(pair<int,int>(uv_voxel[0],voxel_ind));
						vox2pix.insert(pair<int,vector<int>>(voxel_ind,uv_voxel));
					}
				}
			}
		}
		cout << "focal grid size: " << pix2vox.size() << endl;
		cout << "focal grid has been constructed" << endl;
	}
	catch (...) {
		grid_build = false;
		cout << "could not construct focal grid" << endl;
	}

	return grid_build;
}

vector<int> FocalGrid::CheckVoxel(int i, int j, int k) {

	vector<int> uv_out;

	int uv_ind = -1;

	arma::Col<double> xyz(4);

	xyz = {x0-((nx-1)/2.0)*ds+i*ds,
		y0-((ny-1)/2.0)*ds+j*ds,
		z0-((nz-1)/2.0)*ds+k*ds,
		1.0};

	arma::Col<double> uv(3);

	for (int n=0; n<N_cam; n++) {
		uv = X_uv[n]*xyz-uv_offset[n];
		if (uv(0)>=0.0 && uv(0)<(1.0*get<1>(image_size[n]))) {
			if (uv(1)>=0.0 && uv(1)<(1.0*get<0>(image_size[n]))) {
				uv_ind = ((int) uv(1))*get<1>(image_size[n])+((int) uv(0));
				uv_out.push_back(uv_ind);
			}
		}
	}

	return uv_out;
}

vector<arma::Col<int>> FocalGrid::ProjectCloud2Frames(arma::Mat<double> &cloud_in) {

	vector<arma::Col<int>> frame_now;

	int N_vox = cloud_in.n_cols;

	for (int n=0; n<N_cam; n++) {
		arma::Col<int> frame_n;
		frame_n.zeros(get<0>(image_size[n])*get<1>(image_size[n]));
		frame_now.push_back(frame_n);
	}

	arma::Col<double> uv;
	arma::Col<double> xyz;
	int u = 0;
	int v = 0;

	for (int i=0; i<N_vox; i++) {
		for (int j=0; j<N_cam; j++) {
			xyz = {cloud_in(0,i),cloud_in(1,i),cloud_in(2,i),1.0};
			uv = X_uv[j]*xyz-uv_offset[j];
			if (uv(0)>=0 && uv(0)<(get<1>(image_size[j]))) {
				if (uv(1)>=0 && uv(1)<(get<0>(image_size[j]))) {
					u = (int) uv(0);
					v = (int) uv(1);
					if (frame_now[j](get<1>(image_size[j])*v+u)==0) {
						frame_now[j](get<1>(image_size[j])*v+u) = (int) cloud_in(3,i);
					}
					else {
						if (frame_now[j](get<1>(image_size[j])*v+u) > cloud_in(3,i)) {
							frame_now[j](get<1>(image_size[j])*v+u) = (int) cloud_in(3,i);
						}
					}
				}
			}
		}
	}
	return frame_now;
}

arma::Mat<double> FocalGrid::ProjectCloud2UVdouble(arma::Mat<double> &cloud_in, int cam_nr) {

	int N_vox = cloud_in.n_cols;

	arma::Mat<double> uv_doubles(3,N_vox);

	arma::Mat<double> xyz_coords = cloud_in;
	xyz_coords.row(3).fill(1.0);
	arma::Mat<double> M_mat =  X_uv[cam_nr];
	M_mat.col(3) += -uv_offset[cam_nr];
	uv_doubles = M_mat*xyz_coords;

	return uv_doubles;
}

vector<tuple<double,double,double,int>> FocalGrid::ProjectFrames2Cloud(vector<arma::Col<int>> &frame_in) {

	vector<tuple<double,double,double,int>> pcl_now;

	unordered_map<int,int> pcl_voxels;

	int N_row = get<0>(image_size[0]);
	int N_col = get<1>(image_size[1]);

	pair<multimap<int,int>::iterator, multimap<int,int>::iterator> voxels_i;

	int vox_now;
	vector<int> uv_now;

	int n = 0;
	bool is_voxel = true;

	int frame_val_0 = 0;
	int frame_val_n = 0;

	int code_now = 0;

	int count = 0;

	// Insert voxels into an unordered map and give them a segment code:
	for (int i=0; i<(N_row*N_col); i++) {
		frame_val_0 = frame_in[0](i);
		if (frame_val_0>0) {
			voxels_i = pix2vox.equal_range(i);
			for (multimap<int,int>::iterator it=voxels_i.first; it != voxels_i.second; ++it) {
				vox_now = it->second;
				uv_now = vox2pix[vox_now];
				n = 1;
				is_voxel = true;
				code_now = frame_val_0;
				int body_view_count = 0;
				if (frame_val_0 == 1) {
					body_view_count = 1;
				}
				while (is_voxel==true && n < N_cam) {
					frame_val_n = frame_in[n](uv_now[n]);
					if (frame_val_n>0) {
						code_now = code_now+pow(max_n_seg,n)*frame_val_n;
					}
					else {
						is_voxel = false;
					}
					// Reject voxels which have N_cam-1 views formed by the body:
					if (frame_val_n==1) {
						body_view_count++;
					}
					n++;
				}
				if (body_view_count == (N_cam-1)) {
					is_voxel = false;
				}
				if (is_voxel==true) {
					count++;
					pcl_voxels.insert(pair<int,int>(vox_now,code_now));
				}
			}
		}
	}

	// For each voxel in the pcl_voxels map:
	// -> find the neighboring voxels
	// -> add the voxel to pcl_now if it has more than 1 neighbor and less tan 6 neighbors

	unordered_map<int,int>::iterator nb_it;

	arma::Col<int> neighbors(27);

	int nb_sum;

	arma::Col<double> xyz_pos(3);

	for (nb_it = pcl_voxels.begin(); nb_it != pcl_voxels.end(); nb_it++) {
		vox_now = nb_it->first;
		code_now = nb_it->second;
		neighbors = FocalGrid::FindNeighbors(vox_now);
		if (neighbors(13)>0) {
			nb_sum = 0;
			for (int m=0; m<27; m++) {
				if (pcl_voxels.find(neighbors(m)) != pcl_voxels.end()) {
					// Do nothing
				}
				else {
					nb_sum++;
				}
			}
			if (nb_sum > 2 && nb_sum < 27) {
				xyz_pos = FocalGrid::CalculatePosition(vox_now);
				pcl_now.push_back(make_tuple(xyz_pos(0),xyz_pos(1),xyz_pos(2),code_now));
			}
		}
	}

	if (pcl_now.size() == 0) {
		pcl_now.push_back(make_tuple(0.0,0.0,0.0,0));
	}

	return pcl_now;
}

arma::Mat<int> FocalGrid::FindConnectedPointClouds(arma::Mat<double> &pcl_in) {

	int N_pts = pcl_in.n_cols;

	arma::Row<double> seg_ids = arma::unique(pcl_in.row(3));
	int N_seg = seg_ids.n_cols;

	arma::Mat<int> connectivity_mat;

	// Set the diagonals of the connectivity mat to the unique codes:
	if (N_seg>1) {

		connectivity_mat.zeros(N_seg,N_seg);

		for (int j=0; j<N_seg; j++) {
			connectivity_mat(j,j) = seg_ids(j);
		}

		double central_x;
		double central_y;
		double central_z;
		double central_code;

		arma::uvec dx_ids;
		arma::Mat<double> dx_points;

		arma::uvec dxy_ids;
		arma::Mat<double> dxy_points;

		arma::uvec dxyz_ids;
		arma::Mat<double> dxyz_points;

		arma::Row<double> neighbor_codes;
		int N_codes;

		arma::uvec central_code_ind;
		arma::uvec neighbor_code_ind;

		for (int i=0; i<N_pts; i++) {
			// Iterate through the voxels, find any points which are at (+dx,-dx) subsequently (+dy,-dy) and finally (+dz,-dz).
			// Points which show up in all 3 conditions are neighboring points.
			// Than check the codes of the neighboring points and add scores to the connectivity mat if their codes differ from the central point.
			central_x = pcl_in(0,i);
			central_y = pcl_in(1,i);
			central_z = pcl_in(2,i);
			central_code = pcl_in(3,i);

			dx_ids = arma::find((pcl_in.row(0)>(central_x-(1.1*ds))) && (pcl_in.row(0)<(central_x+(1.1*ds))));
			if (dx_ids.n_rows>1) {
				dx_points = pcl_in.cols(dx_ids);
				dxy_ids = arma::find((dx_points.row(1)>(central_y-(1.1*ds))) && (dx_points.row(1)<(central_y+(1.1*ds))));
				if (dxy_ids.n_rows>1) {
					dxy_points = dx_points.cols(dxy_ids);
					dxyz_ids = arma::find((dxy_points.row(2)>(central_z-(1.1*ds))) && (dxy_points.row(2)<(central_z+(1.1*ds))));
					if (dxyz_ids.n_rows>1) {
						dxyz_points = dxy_points.cols(dxyz_ids);
						neighbor_codes = arma::unique(dxyz_points.row(3));
						N_codes = neighbor_codes.n_cols;
						central_code_ind = arma::find(seg_ids == central_code);
						for (int k=0; k<N_codes; k++) {
							neighbor_code_ind = arma::find(seg_ids == neighbor_codes(k));
							if (central_code_ind(0) != neighbor_code_ind(0)) {
								connectivity_mat(central_code_ind,neighbor_code_ind) += 1;
								connectivity_mat(neighbor_code_ind,central_code_ind) += 1;
							}
						}
					}
				}
			}
		}
	}
	else {
		connectivity_mat.zeros(1,1);
	}
	return connectivity_mat;
}

arma::Mat<double> FocalGrid::ConvertVector2Mat(vector<tuple<double,double,double,int>> pcl_in) {
	int N_pts = pcl_in.size();
	arma::Mat<double> pcl_out(4,N_pts);
	for (int j=0; j<N_pts; j++) {
		pcl_out(0,j) = get<0>(pcl_in[j]);
		pcl_out(1,j) = get<1>(pcl_in[j]);
		pcl_out(2,j) = get<2>(pcl_in[j]);
		pcl_out(3,j) = get<3>(pcl_in[j]);
	}
	return pcl_out;
}

arma::Col<int> FocalGrid::FindNeighbors(int vox_ind) {

	// find the neighboring voxels:

	arma::Col<int> vox_int_mat(27);

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	if ((i>0 && i<(nx-1)) && (j>0 && j<(ny-1)) && (k>0 && k<(nz-1))) {

		vox_int_mat(0) = vox_ind-nx*ny-nx-1;
		vox_int_mat(1) = vox_ind-nx*ny-nx;
		vox_int_mat(2) = vox_ind-nx*ny-nx+1;
		vox_int_mat(3) = vox_ind-nx*ny-1;
		vox_int_mat(4) = vox_ind-nx*ny;
		vox_int_mat(5) = vox_ind-nx*ny+1;
		vox_int_mat(6) = vox_ind-nx*ny+nx-1;
		vox_int_mat(7) = vox_ind-nx*ny+nx;
		vox_int_mat(8) = vox_ind-nx*ny+nx+1;
		vox_int_mat(9) = vox_ind-nx-1;
		vox_int_mat(10) = vox_ind-nx;
		vox_int_mat(11) = vox_ind-nx+1;
		vox_int_mat(12) = vox_ind-1;
		vox_int_mat(13) = vox_ind;
		vox_int_mat(14) = vox_ind+1;
		vox_int_mat(15) = vox_ind+nx-1;
		vox_int_mat(16) = vox_ind+nx;
		vox_int_mat(17) = vox_ind+nx+1;
		vox_int_mat(18) = vox_ind+nx*ny-nx-1;
		vox_int_mat(19) = vox_ind+nx*ny-nx;
		vox_int_mat(20) = vox_ind+nx*ny-nx+1;
		vox_int_mat(21) = vox_ind+nx*ny-1;
		vox_int_mat(22) = vox_ind+nx*ny;
		vox_int_mat(23) = vox_ind+nx*ny+1;
		vox_int_mat(24) = vox_ind+nx*ny+nx-1;
		vox_int_mat(25) = vox_ind+nx*ny+nx;
		vox_int_mat(26) = vox_ind+nx*ny+nx+1;

	}
	else {
		vox_int_mat.zeros();
	}

	return vox_int_mat;
}

arma::Col<double> FocalGrid::CalculatePosition(int vox_ind) {

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	arma::Col<double> xyz_pos(3);

	xyz_pos(0) = x0-((nx-1)/2.0)*ds+i*ds;
	xyz_pos(1) = y0-((ny-1)/2.0)*ds+j*ds;
	xyz_pos(2) = z0-((nz-1)/2.0)*ds+k*ds;

	return xyz_pos;
}

arma::Mat<int> FocalGrid::TransformXYZ2UV(arma::Col<double> xyz_pos) {

	arma::Mat<int> uv_mat(2,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos-uv_offset[n];
		uv_mat(0,n) = int(uv(0));
		uv_mat(1,n) = int(uv(1));
	}

	return uv_mat;
}

tuple<arma::Mat<int>, arma::Col<double>> FocalGrid::RayCasting(int cam_nr, arma::Col<double> xyz_pos_prev, arma::Col<double> uv_pos_prev, arma::Col<double> uv_pos_now) {

	// Calculate the 3D translation vector:
	arma::Col<double> xyz_uv_prev = X_xyz[cam_nr]*(uv_pos_prev+uv_offset[cam_nr]);
	arma::Col<double> xyz_uv_now = X_xyz[cam_nr]*(uv_pos_now+uv_offset[cam_nr]);
	arma::Col<double> trans_vec = xyz_uv_now-xyz_uv_prev;
	trans_vec(3) = 0.0;

	// Add the translation to the xyz position:
	arma::Col<double> xyz_pos_now = xyz_pos_prev + trans_vec;

	// Project the new position back to the camera views:

	arma::Mat<int> uv_mat(3,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos_now-uv_offset[n];
		if (uv(0)>=0 && uv(0)<(get<1>(image_size[n])) && uv(1)>=0 && uv(1)<(get<0>(image_size[n]))) {
			uv_mat(0,n) = int(uv(0));
			uv_mat(1,n) = int(uv(1));
			uv_mat(2,n) = int(uv(2));
		}
		else {
			arma::Col<double> uv_old = X_uv[n]*xyz_pos_prev-uv_offset[n];
			uv_mat(0,n) = int(uv_old(0));
			uv_mat(1,n) = int(uv_old(1));
			uv_mat(2,n) = int(uv_old(2));
		}
	}

	return make_tuple(uv_mat,xyz_pos_now);
}

arma::Col<double> FocalGrid::CalculateViewVector(int cam_nr) {

	arma::Col<double> u_vec = {1.0,0.0,0.0};
	arma::Col<double> v_vec = {0.0,1.0,0.0};

	arma::Col<double> u_vec_world = X_xyz[cam_nr]*u_vec;
	arma::Col<double> v_vec_world = X_xyz[cam_nr]*v_vec;

	arma::Col<double> view_vec = arma::cross(v_vec_world.rows(0,2),u_vec_world.rows(0,2))/(arma::norm(u_vec_world.rows(0,2))*arma::norm(v_vec_world.rows(0,2)));

	return arma::normalise(view_vec);

}

arma::Mat<double> FocalGrid::Camera2WorldMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return arma::inv(K)*arma::pinv(C);

}

arma::Mat<double> FocalGrid::World2CameraMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return C*K;

}


#pragma once

#include "../external_library/glm.hxx"
#include "program.hxx"

// note: 先に boost/variant.hpp がロードされる場合はそれよりも前に定義される必要がある。
#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_LIST_SIZE 40
#define BOOST_MPL_LIMIT_VECTOR_SIZE 40

#include <boost/variant.hpp>

namespace usagi
{
  namespace gl
  {

    using uniform_key_type = std::string;

    using uniform_value_type = boost::variant
    < glm::f32vec4, glm::f32vec3, glm::f32vec2, glm::f32vec1
    , float

    , std::vector< glm::f32vec4 >
    , std::vector< glm::f32vec3 >
    , std::vector< glm::f32vec2 >
    , std::vector< glm::f32vec1 >
    , std::vector< float >

    , glm::i32vec4, glm::i32vec3, glm::i32vec2, glm::i32vec1
    , std::int32_t
    , std::vector< glm::i32vec4 >
    , std::vector< glm::i32vec3 >
    , std::vector< glm::i32vec2 >
    , std::vector< glm::i32vec1 >
    , std::vector< std::int32_t >

    , glm::f32mat4x4
    , glm::f32mat4x3, glm::f32mat3x4
    , glm::f32mat4x2, glm::f32mat2x4
    , glm::f32mat3x3
    , glm::f32mat3x2, glm::f32mat2x3
    , glm::f32mat2x2

    , std::vector< glm::f32mat4x4 >
    , std::vector< glm::f32mat4x3 >
    , std::vector< glm::f32mat3x4 >
    , std::vector< glm::f32mat4x2 >
    , std::vector< glm::f32mat2x4 >
    , std::vector< glm::f32mat3x3 >
    , std::vector< glm::f32mat3x2 >
    , std::vector< glm::f32mat2x3 >
    , std::vector< glm::f32mat2x2 >

    >;

    static inline auto get_uniform_location( const uniform_key_type& key )
    {
      return glGetUniformLocation( get_current_program(), key.c_str() );
    }

    // f32

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32vec4& value )
    {
      glUniform4fv( get_uniform_location( key ), 1, &value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32vec3& value )
    {
      glUniform3fv( get_uniform_location( key ), 1, &value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32vec2& value )
    {
      glUniform2fv( get_uniform_location( key ), 1, &value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32vec1& value )
    {
      glUniform1f( get_uniform_location( key ), value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const float value )
    {
      glUniform1f( get_uniform_location( key ), value );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32vec4 >& value )
    {
      glUniform4fv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32vec3 >& value )
    {
      glUniform3fv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32vec2 >& value )
    {
      glUniform2fv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32vec1 >& value )
    {
      glUniform1fv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< float >& value )
    {
      glUniform1fv( get_uniform_location( key ), value.size(), &value[0] );
    }

    // i32

    static inline auto set_uniform( const uniform_key_type& key, const glm::i32vec4& value )
    {
      glUniform4iv( get_uniform_location( key ), 1, &value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::i32vec3& value )
    {
      glUniform3iv( get_uniform_location( key ), 1, &value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::i32vec2& value )
    {
      glUniform2iv( get_uniform_location( key ), 1, &value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::i32vec1& value )
    {
      glUniform1i( get_uniform_location( key ), value[0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::int32_t value )
    {
      glUniform1i( get_uniform_location( key ), value );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::i32vec4 >& value )
    {
      glUniform4iv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::i32vec3 >& value )
    {
      glUniform3iv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::i32vec2 >& value )
    {
      glUniform2iv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::i32vec1 >& value )
    {
      glUniform1iv( get_uniform_location( key ), value.size(), &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< std::int32_t >& value )
    {
      glUniform1iv( get_uniform_location( key ), value.size(), &value[0] );
    }

    // mat

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat4x4& value, const GLboolean transpose )
    {
      glUniformMatrix4fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat4x3& value, const GLboolean transpose )
    {
      glUniformMatrix4x3fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat3x4& value, const GLboolean transpose )
    {
      glUniformMatrix3x4fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat4x2& value, const GLboolean transpose )
    {
      glUniformMatrix4x2fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat2x4& value, const GLboolean transpose )
    {
      glUniformMatrix2x4fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat3x3& value, const GLboolean transpose )
    {
      glUniformMatrix3fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat3x2& value, const GLboolean transpose )
    {
      glUniformMatrix3x2fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat2x3& value, const GLboolean transpose )
    {
      glUniformMatrix2x3fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat2x2& value, const GLboolean transpose )
    {
      glUniformMatrix2fv( get_uniform_location( key ), 1, transpose, &value[0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat4x4 >& value, const GLboolean transpose )
    {
      glUniformMatrix4fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat4x3 >& value, const GLboolean transpose )
    {
      glUniformMatrix4x3fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat3x4 >& value, const GLboolean transpose )
    {
      glUniformMatrix3x4fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat4x2 >& value, const GLboolean transpose )
    {
      glUniformMatrix4x2fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat2x4 >& value, const GLboolean transpose )
    {
      glUniformMatrix2x4fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat3x3 >& value, const GLboolean transpose )
    {
      glUniformMatrix3fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat3x2 >& value, const GLboolean transpose )
    {
      glUniformMatrix3x2fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat2x3 >& value, const GLboolean transpose )
    {
      glUniformMatrix2x3fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    static inline auto set_uniform( const uniform_key_type& key, const std::vector< glm::f32mat2x2 >& value, const GLboolean transpose )
    {
      glUniformMatrix2fv( get_uniform_location( key ), value.size(), transpose, &value[0][0][0] );
    }

    // omit transpose version

    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat4x4& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat4x3& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat3x4& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat4x2& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat2x4& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat3x3& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat3x2& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat2x3& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type& key, const glm::f32mat2x2& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }

    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat4x4 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat4x3 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat3x4 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat4x2 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat2x4 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat3x3 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat3x2 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat2x3 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }
    static inline auto set_uniform( const uniform_key_type key, const std::vector< glm::f32mat2x2 >& value ) { set_uniform( key, value, static_cast< GLboolean >( false ) ); }

    // variant

    static inline auto set_uniform( const uniform_key_type& key, const uniform_value_type& value )
    {
      apply_visitor( [&]( const auto& value ) { set_uniform( key, value ); }, value );
    }

  }
}


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

//-----------------------------------------------------------------------------
/*!\file    ForwardDynamicsSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2016/02/14
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/ForwardDynamicsSolver.h>

// other
#include <map>
#include <sstream>
#include <boost/algorithm/clamp.hpp>
#include <eigen_conversions/eigen_kdl.h>

// KDL
#include <kdl/jntarrayvel.hpp>
#include <kdl/framevel.hpp>

// DEBUG

namespace cartesian_controller_base{

  ForwardDynamicsSolver::ForwardDynamicsSolver()
  {
  }

  ForwardDynamicsSolver::~ForwardDynamicsSolver(){}

  trajectory_msgs::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {

    // Compute joint space inertia matrix
    m_jnt_space_inertia_solver->JntToMass(m_current_positions,m_jnt_space_inertia);

    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
    m_current_accelerations.data = m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.toSec();

    // Integrate twice, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    for (int i = 0; i < m_number_joints; ++i)
    {
      m_current_positions(i) = boost::algorithm::clamp(
          m_current_positions(i),m_lower_pos_limits(i),m_upper_pos_limits(i));
    }

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  const KDL::Frame& ForwardDynamicsSolver::getEndEffectorPose() const
  {
    return m_end_effector_pose;
  }

  const ctrl::Vector6D& ForwardDynamicsSolver::getEndEffectorVel() const
  {
    return m_end_effector_vel;
  }

  const KDL::JntArray& ForwardDynamicsSolver::getPositions() const
  {
    return m_current_positions;
  }

  bool ForwardDynamicsSolver::setStartState(
      const std::vector<hardware_interface::JointHandle>& joint_handles)
  {
    // Copy into internal buffers.
    for (int i = 0; i < joint_handles.size(); ++i)
    {
      m_current_positions(i)      = joint_handles[i].getPosition();
      m_current_velocities(i)     = joint_handles[i].getVelocity();
      m_current_accelerations(i)  = 0.0;
      m_last_positions(i)         = m_current_positions(i);
    }
    return true;
  }

  bool ForwardDynamicsSolver::init(
      const KDL::Chain& chain,
      const KDL::JntArray& upper_pos_limits,
      const KDL::JntArray& lower_pos_limits)
  {
    if (!buildGenericModel(chain))
    {
      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
      return false;
    }

    // Initialize
    m_number_joints              = m_chain.getNrOfJoints();
    m_current_positions.data     = ctrl::VectorND::Zero(m_number_joints);
    m_current_velocities.data    = ctrl::VectorND::Zero(m_number_joints);
    m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints);
    m_last_positions.data        = ctrl::VectorND::Zero(m_number_joints);
    m_upper_pos_limits           = upper_pos_limits;
    m_lower_pos_limits           = lower_pos_limits;

    // Forward kinematics
    m_fk_pos_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain));
    m_fk_vel_solver.reset(new KDL::ChainFkSolverVel_recursive(m_chain));

    // Forward dynamics
    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
    m_jnt_jacobian.resize(m_number_joints);
    m_jnt_space_inertia.resize(m_number_joints);

    ROS_INFO("Forward dynamics solver initialized");
    ROS_INFO("Forward dynamics solver has control over %i joints", m_number_joints);

    return true;
  }

  bool ForwardDynamicsSolver::buildGenericModel(const KDL::Chain& input_chain)
  {
    m_chain = input_chain;

    // Set all masses and inertias to minimal (yet stable) values.
    double m_min = 0.001;
    double ip_min = 0.000001;
    for (size_t i = 0; i < m_chain.segments.size(); ++i)
    {
      // Fixed joint segment
      if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None)
      {
        m_chain.segments[i].setInertia(
            KDL::RigidBodyInertia::Zero());
      }
      else  // relatively moving segment
      {
        m_chain.segments[i].setInertia(
            KDL::RigidBodyInertia(
              m_min,                // mass
              KDL::Vector::Zero(),  // center of gravity
              KDL::RotationalInertia(
                ip_min,             // ixx
                ip_min,             // iyy
                ip_min              // izz
                // ixy, ixy, iyz default to 0.0
                )));
      }
    }

    // Only give the last segment a generic mass and inertia.
    // See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting.
    double m = 1;
    double ip = 1;
    m_chain.segments[m_chain.segments.size()-1].setInertia(
        KDL::RigidBodyInertia(
          m,
          KDL::Vector::Zero(),
          KDL::RotationalInertia(ip, ip, ip)));

    return true;
  }

} // namespace


//          Copyright Rein Halbersma 2010-2021.
// 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)

#include <core/board/sequence.hpp>      // nano, micro, checkers, Roman, spantsiretti, international, frisian, ktar<10, 11>,
                                        // ktar<10, 12>, Compact_10_12, Compact_12_10, basic_board<12, 10>, canadian, srilankan, dumm
#include <boost/range/irange.hpp>       // irange
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE
#include <algorithm>                    // all_of

using namespace dctl::core;

BOOST_AUTO_TEST_SUITE(TestBoard)

BOOST_AUTO_TEST_CASE_TEMPLATE(Bit2SquareIsInverseSquare2Bit, T, BoardSequence)
{
        auto const squares = boost::irange(0, T::size());
        BOOST_CHECK(
                std::all_of(squares.begin(), squares.end(), [](auto sq) {
                        return T::numeric0(T::embedding0(sq)) == sq;
                })
        );
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include "mtl/maybe.hpp"
#include <cstddef>
#include <cstdint>
#include <mtl/dyn_array.hpp>
#include <mtl/linked_list.hpp>
#include <mtl/types.hpp>

namespace mtl {
using Hash = u32;

/// @brief Pulled from https://en.wikipedia.org/wiki/Jenkins_hash_function
class Hasher {
  public:
    // Creates hash values from raw data
    static Hash JenkinsOneAtATime(const u8* raw_bytes, usize size) {
        usize i = 0;
        Hash hash = 0;
        while (i != size) {
            hash += raw_bytes[i++];
            hash += hash << 10;
            hash ^= hash >> 6;
        }
        hash += hash << 3;
        hash ^= hash >> 11;
        hash += hash << 15;
        return hash;
    }
    static Hash JenkinsOneAtATime(const char* raw_bytes, usize size) {
        usize i = 0;
        Hash hash = 0;
        while (i != size) {
            hash += static_cast<u32>(raw_bytes[i++]);
            hash += hash << 10;
            hash ^= hash >> 6;
        }
        hash += hash << 3;
        hash ^= hash >> 11;
        hash += hash << 15;
        return hash;
    }
};

template <class KeyType, class ValueType>
class HashMap {
  public:
    struct KeyAndValue {
        KeyType key;
        ValueType value;
    };
    HashMap() = default;

    /// @brief Maybe return the previously existing key/pair
    mtl::Maybe<KeyAndValue> insert(KeyType key, ValueType value) {
        const auto key_ptr = &key;
        const auto hash =
            Hasher::JenkinsOneAtATime(static_cast<const u8*>(key_ptr), sizeof(KeyType));
        const usize idx = hash % values_.length();
        keys_.push_back(std::move(key));
        /// @todo add to map values_[idx].emplace_back
        static_cast<void>(value);
        static_cast<void>(idx);
    }

    /// @brief Number of key:value pairs in the map
    usize count() const noexcept {
        // Keys and values are entered together so either can be used
        return keys_.size();
    }

  private:
    DynArray<KeyType> keys_;
    DynArray<LinkedList<ValueType>> values_;
};
} // namespace mtl