/**
 * \file boost/numeric/ublasx/operation/reshape.hpp
 *
 * \brief Reshape a matrix.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_RESHAPE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_RESHAPE_HPP

//#include <boost/mpl/has_xxx.hpp>
//#include <boost/mpl/if.hpp>
#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/detail/temporary.hpp>
#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/tags.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/detail/compiler.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
//#include <boost/utility/enable_if.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

template <typename T>
struct reshape_traits;

namespace detail { namespace /*<unnamed>*/ {

// /// Define a \c has_matrix_temp_type trait class.
//BOOST_MPL_HAS_XXX_TRAIT_DEF(matrix_temp_type)

///**
// * \brief Wrapper type-traits used in \c boost::lazy_enabled_if for getting the
// *  matrix temporary type (see below).
// * \tparam MatrixT A matrix type.
// */
//template <typename MatrixT>
//struct matrix_temp_type
//{
//	/// The matrix temporary type.
//	typedef typename matrix_temporary_traits<MatrixT>::type type;
//};

/**
 * \brief The implementation of the \em reshape operation.
 * \tparam MatrixExprT The type of the input matrix expression.
 * \param A The input matrix expression.
 * \param nr The number of rows of the reshaped matrix.
 * \param nr The number of columns of the reshaped matrix.
 * \param colw Tells of elements are taken from the input matrix either in a
 *  column-wise way (\c true value) or in a row-wise way (\c false value).
 * \return The reshaped matrix.
 */
template <typename VectorExprT>
typename reshape_traits<VectorExprT>::result_type reshape_impl(vector_expression<VectorExprT> const& ve,
															   ::std::size_t nr,
															   ::std::size_t nc)
{
	typedef typename reshape_traits<VectorExprT>::result_type result_matrix_type;
	typedef typename vector_traits<VectorExprT>::size_type size_type;

	size_type n(size(ve));

	// pre: to reshape, the number of elements must not change.
	// NOTE: assertion used to mimic the MATLAB 'reshape' function.
	BOOST_UBLAS_CHECK(
			nr*nc == n,
			bad_size()
		);

	// NOTE: this is needed to avoid this kind of warning when one compiles
	//       without debug information.
	BOOST_UBLASX_SUPPRESS_UNUSED_VARIABLE_WARNING( n );

	result_matrix_type res(nr, nc);

	size_type k(0);

	for (size_type i = 0; i < nr; ++i)
	{
		for (size_type j = 0; j < nc; ++j)
		{
			res(i,j) = ve()(k);

			++k;
		}
	}

	return res;
}

/**
 * \brief The implementation of the \em reshape operation.
 * \tparam MatrixExprT The type of the input matrix expression.
 * \param A The input matrix expression.
 * \param nr The number of rows of the reshaped matrix.
 * \param nr The number of columns of the reshaped matrix.
 * \param colw Tells if elements are taken from the input matrix either in a
 *  column-wise way (\c true value) or in a row-wise way (\c false value).
 * \return The reshaped matrix.
 */
template <typename MatrixExprT>
typename reshape_traits<MatrixExprT>::result_type reshape_impl(matrix_expression<MatrixExprT> const& me,
															   ::std::size_t nr,
															   ::std::size_t nc,
															   bool colw)
{
	// pre: to reshape, the number of elements must not change.
	// NOTE: assertion used to mimic the MATLAB 'reshape' function.
	BOOST_UBLAS_CHECK(
			nr*nc == num_rows(me)*num_columns(me),
			bad_size()
		);

//	typedef typename matrix_temporary_traits<MatrixExprT>::type result_matrix_type;
	typedef typename reshape_traits<MatrixExprT>::result_type result_matrix_type;
	typedef typename matrix_traits<MatrixExprT>::size_type size_type;

	result_matrix_type res(nr, nc);

	size_type r(0);
	size_type c(0);

	if (colw)
	{
		// Reshape by taking the elements in a column-wise way.
		size_type nr_me(num_rows(me));
		for (size_type j = 0; j < nc; ++j)
		{
			for (size_type i = 0; i < nr; ++i)
			{
				if (r == nr_me)
				{
					r = size_type/*zero*/();
					++c;
				}

				res(i,j) = me()(r,c);

				++r;
			}
		}
	}
	else
	{
		// Reshape by taking the elements in a row-wise way.
		size_type nc_me(num_columns(me));
		for (size_type j = 0; j < nc; ++j)
		{
			for (size_type i = 0; i < nr; ++i)
			{
				if (c == nc_me)
				{
					c = size_type/*zero*/();
					++r;
				}

				res(i,j) = me()(r,c);

				++c;
			}
		}
	}

	return res;
}

/// Auxiliary class for the implementation of the by-dim \c reshape operation.
template <std::size_t Dim>
struct reshape_by_dim_impl;

template <>
struct reshape_by_dim_impl<1>
{
    /**
	 * \brief Reshape the given matrix taking its elements in a column-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
    template <typename ExprT>
    BOOST_UBLAS_INLINE
    static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
    {
        return reshape_impl(me, nr, nc, true);
    }
};

template <>
struct reshape_by_dim_impl<2>
{
    /**
	 * \brief Reshape the given matrix taking its elements in a row-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
    template <typename ExprT>
    BOOST_UBLAS_INLINE
    static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
    {
        return reshape_impl(me, nr, nc, false);
    }
};

/// Auxiliary class for the implementation of the by-tag \c reshape operation.
template <typename TagT, typename OrientationT>
struct reshape_by_tag_impl;

template <>
struct reshape_by_tag_impl<tag::major, row_major_tag>
{
	/**
	 * \brief Reshape the given matrix taken its elements in a column-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
	template <typename ExprT>
	BOOST_UBLAS_INLINE
	static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
	{
		return reshape_impl(me, nr, nc, true);
	}
};

template <>
struct reshape_by_tag_impl<tag::major, column_major_tag>
{
	/**
	 * \brief Reshape the given matrix taken its elements in a row-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
	template <typename ExprT>
	BOOST_UBLAS_INLINE
	static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
	{
		return reshape_impl(me, nr, nc, false);
	}
};

template <>
struct reshape_by_tag_impl<tag::minor, row_major_tag>
{
	/**
	 * \brief Reshape the given matrix taken its elements in a row-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
	template <typename ExprT>
	BOOST_UBLAS_INLINE
	static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
	{
		return reshape_impl(me, nr, nc, false);
	}
};

template <>
struct reshape_by_tag_impl<tag::minor, column_major_tag>
{
	/**
	 * \brief Reshape the given matrix taken its elements in a column-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
	template <typename ExprT>
	BOOST_UBLAS_INLINE
	static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
	{
		return reshape_impl(me, nr, nc, true);
	}
};

template <>
struct reshape_by_tag_impl<tag::leading, row_major_tag>
{
	/**
	 * \brief Reshape the given matrix taken its elements in a row-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
	template <typename ExprT>
	BOOST_UBLAS_INLINE
	static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
	{
		return reshape_impl(me, nr, nc, false);
	}
};

template <>
struct reshape_by_tag_impl<tag::leading, column_major_tag>
{
	/**
	 * \brief Reshape the given matrix taken its elements in a column-wise way.
	 * \tparam ExprT A matrix expression type.
	 * \pre ExprT must be a model of MatrixExpression.
	 */
	template <typename ExprT>
	BOOST_UBLAS_INLINE
	static typename reshape_traits<ExprT>::result_type apply(matrix_expression<ExprT> const& me, ::std::size_t nr, ::std::size_t nc)
	{
		return reshape_impl(me, nr, nc, true);
	}
};

}} // Namespace detail::<unnamed>

template <typename T>
struct reshape_traits
{
	typedef matrix<typename T::value_type> result_type;
};

template <typename VectorExprT>
struct reshape_traits< vector_expression<VectorExprT> >
{
	typedef matrix<typename vector_traits<VectorExprT>::value_type> result_type;
};

template <typename MatrixExprT>
struct reshape_traits< matrix_expression<MatrixExprT> >
{
//	typedef typename matrix_temporary_traits<MatrixExprT>::type result_type;
	typedef matrix<typename matrix_traits<MatrixExprT>::value_type> result_type;
};

template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename reshape_traits<VectorExprT>::result_type reshape(vector_expression<VectorExprT> const& ve,
														  ::std::size_t nr,
														  ::std::size_t nc)
{
	return detail::reshape_impl(ve, nr, nc);
}

template <std::size_t Dim, typename MatrixExprT>
BOOST_UBLAS_INLINE
typename reshape_traits<MatrixExprT>::result_type reshape(matrix_expression<MatrixExprT> const& me, ::std::size_t nr, ::std::size_t nc)
{
    return detail::reshape_by_dim_impl<Dim>::template apply(me, nr, nc);
}

/*
 * [BEGIN] Does Not Work!
 *
template <typename TagT, typename MatrixExprT>
BOOST_UBLAS_INLINE
typename ::boost::lazy_enable_if_c<
	detail::has_matrix_temp_type<MatrixExprT>::value,
	detail::matrix_temp_type<MatrixExprT>
>::type reshape(matrix_expression<MatrixExprT> const& me,
				::std::size_t nr,
				::std::size_t nc)
{
	return detail::reshape_by_tag_impl<TagT, typename matrix_traits<MatrixExprT>::orientation_category>::template apply(me);
}
 *
 * [END] Does Not Work!
 */

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename reshape_traits<MatrixExprT>::result_type reshape(matrix_expression<MatrixExprT> const& me,
															::std::size_t nr,
															::std::size_t nc)
{
	return detail::reshape_by_dim_impl<1>::template apply(me, nr, nc);
}

template <std::size_t Dim, typename MatrixExprT>
BOOST_UBLAS_INLINE
void reshape_inplace(matrix_container<MatrixExprT>& mc, ::std::size_t nr, ::std::size_t nc)
{
	typedef typename reshape_traits<MatrixExprT>::result_type work_matrix_type;

	work_matrix_type res;
	res = reshape<Dim>(mc, nr, nc);
	mc().resize(nr, nc, false);
	mc() = res;
}

/*
 * [BEGIN] Does Not Work!
 *
template <typename TagT, typename MatrixExprT>
BOOST_UBLAS_INLINE
void reshape_inplace(matrix_container<MatrixExprT>& mc,
					 ::std::size_t nr,
					 ::std::size_t nc)
{
	typedef typename reshape_traits<MatrixExprT>::result_type work_matrix_type;

	work_matrix_type res;
	res = reshape<TagT>(mc, nr, nc);
	mc().resize(nr, nc, false);
	mc() = res;
}
 *
 * [END] Does Not Work!
 */

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
void reshape_inplace(matrix_container<MatrixExprT>& mc,
					 ::std::size_t nr,
					 ::std::size_t nc)
{
	typedef typename reshape_traits<MatrixExprT>::result_type work_matrix_type;

	work_matrix_type res;
	res = reshape(mc, nr, nc);
	mc().resize(nr, nc, false);
	mc() = res;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_RESHAPE_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief radar_beam_height.cpp

\details
Demonstrate library usage for user test cases suggested by Michael Fawcett.

Output:
@verbatim

//[radar_beam_height_output
radar range        : 300 nmi
earth radius       : 6.37101e+06 m
beam height 1      : 18169.7 m
beam height 2      : 9.81085 nmi
beam height 3      : 18169.7 m
beam height 4      : 9.81085 nmi
beam height approx : 59488.4 ft
beam height approx : 18132.1 m
//]

@endverbatim
**/

#include <iostream>

#include <boost/units/conversion.hpp>
#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/prefixes.hpp>

using boost::units::length_dimension;
using boost::units::pow;
using boost::units::root;
using boost::units::quantity;
using boost::units::unit;

//[radar_beam_height_class_snippet_1
namespace nautical {

struct length_base_unit :
    boost::units::base_unit<length_base_unit, length_dimension, 1>
{
    static std::string name()       { return "nautical mile"; }
    static std::string symbol()     { return "nmi"; }
};

typedef boost::units::make_system<length_base_unit>::type system;

/// unit typedefs
typedef unit<length_dimension,system>    length;

static const length mile,miles;

} // namespace nautical

// helper for conversions between nautical length and si length
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(nautical::length_base_unit,
                                     boost::units::si::meter_base_unit,
                                     double, 1.852e3);
//]

//[radar_beam_height_class_snippet_2
namespace imperial {

struct length_base_unit :
    boost::units::base_unit<length_base_unit, length_dimension, 2>
{
    static std::string name()       { return "foot"; }
    static std::string symbol()     { return "ft"; }
};

typedef boost::units::make_system<length_base_unit>::type system;

/// unit typedefs
typedef unit<length_dimension,system>    length;

static const length foot,feet;

} // imperial

// helper for conversions between imperial length and si length
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(imperial::length_base_unit,
                                     boost::units::si::meter_base_unit,
                                     double, 1.0/3.28083989501312);
//]

// radar beam height functions
//[radar_beam_height_function_snippet_1
template<class System,typename T>
constexpr
quantity<unit<boost::units::length_dimension,System>,T>
radar_beam_height(const quantity<unit<length_dimension,System>,T>& radar_range,
                  const quantity<unit<length_dimension,System>,T>& earth_radius,
                  T k = 4.0/3.0)
{
    return quantity<unit<length_dimension,System>,T>
        (pow<2>(radar_range)/(2.0*k*earth_radius));
}
//]

//[radar_beam_height_function_snippet_2
template<class return_type,class System1,class System2,typename T>
constexpr
return_type
radar_beam_height(const quantity<unit<length_dimension,System1>,T>& radar_range,
                  const quantity<unit<length_dimension,System2>,T>& earth_radius,
                  T k = 4.0/3.0)
{
    // need to decide which system to use for calculation
    return pow<2>(static_cast<return_type>(radar_range))
            / (2.0*k*static_cast<return_type>(earth_radius));
}
//]

//[radar_beam_height_function_snippet_3
constexpr
quantity<imperial::length>
radar_beam_height(const quantity<nautical::length>& range)
{
    return quantity<imperial::length>
        (pow<2>(range/(1.23*nautical::miles/root<2>(imperial::feet))));
}
//]

int main(void)
{
    using namespace boost::units;
    using namespace boost::units::si;
    using namespace nautical;

    //[radar_beam_height_snippet_1
    const quantity<nautical::length> radar_range(300.0*miles);
    const quantity<si::length>       earth_radius(6371.0087714*kilo*meters);

    const quantity<si::length>       beam_height_1(radar_beam_height(quantity<si::length>(radar_range),earth_radius));
    const quantity<nautical::length> beam_height_2(radar_beam_height(radar_range,quantity<nautical::length>(earth_radius)));
    const quantity<si::length>       beam_height_3(radar_beam_height< quantity<si::length> >(radar_range,earth_radius));
    const quantity<nautical::length> beam_height_4(radar_beam_height< quantity<nautical::length> >(radar_range,earth_radius));
    //]

    std::cout << "radar range        : " << radar_range << std::endl
              << "earth radius       : " << earth_radius << std::endl
              << "beam height 1      : " << beam_height_1 << std::endl
              << "beam height 2      : " << beam_height_2 << std::endl
              << "beam height 3      : " << beam_height_3 << std::endl
              << "beam height 4      : " << beam_height_4 << std::endl
              << "beam height approx : " << radar_beam_height(radar_range)
              << std::endl
              << "beam height approx : "
              << quantity<si::length>(radar_beam_height(radar_range))
              << std::endl << std::endl;

    return 0;
}


#include <hdl_graph_slam/keyframe.hpp>

#include <boost/filesystem.hpp>

#include <pcl/io/pcd_io.h>
#include <g2o/types/slam3d/vertex_se3.h>

namespace hdl_graph_slam {

KeyFrame::KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud<PointT>::ConstPtr& cloud)
  : stamp(stamp),
    odom(odom),
    accum_distance(accum_distance),
    cloud(cloud),
    node(nullptr)
{}

KeyFrame::~KeyFrame() {

}

void KeyFrame::dump(const std::string& directory) {
  if(!boost::filesystem::is_directory(directory)) {
    boost::filesystem::create_directory(directory);
  }

  std::ofstream ofs(directory + "/data");
  ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n";

  ofs << "odom\n";
  ofs << odom.matrix() << "\n";

  ofs << "accum_distance " << accum_distance << "\n";

  if(floor_coeffs) {
    ofs << "floor_coeffs " << floor_coeffs->transpose() << "\n";
  }

  if(node) {
    ofs << "id " << node->id() << "\n";
  }

  pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud);

}

KeyFrameSnapshot::KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud<PointT>::ConstPtr& cloud)
  : pose(pose),
    cloud(cloud)
{}

KeyFrameSnapshot::KeyFrameSnapshot(const KeyFrame::Ptr& key)
  : pose(key->node->estimate()),
    cloud(key->cloud)
{}

KeyFrameSnapshot::~KeyFrameSnapshot() {

}

}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file joint_sensor_id.hpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <tuple>
#include <memory>

#include <fl/util/traits.hpp>
#include <fl/util/meta.hpp>

#include <fl/model/sensor/interface/sensor_function.hpp>

namespace fl
{

// Forward declarations
template <typename...Models> class JointSensor;

/**
 * Traits of JointSensor
 */
template <typename...Models>
struct Traits<
           JointSensor<Models...>
       >
{
    enum : signed int
    {
        ObsrvDim = JoinSizes<Traits<Models>::Obsrv::SizeAtCompileTime...>::Size,
        NoiseDim = JoinSizes<Traits<Models>::Noise::SizeAtCompileTime...>::Size,
        ParamDim = JoinSizes<Traits<Models>::Param::SizeAtCompileTime...>::Size,
        StateDim = Traits<
                       typename FirstTypeIn<Models...>::Type
                    >::State::SizeAtCompileTime
    };

    typedef typename Traits<
                         typename FirstTypeIn<Models...>::Type
                     >::State::Scalar Scalar;

    typedef Eigen::Matrix<Scalar, ObsrvDim, 1> Obsrv;
    typedef Eigen::Matrix<Scalar, StateDim, 1> State;
    typedef Eigen::Matrix<Scalar, NoiseDim, 1> Noise;
    typedef Eigen::Matrix<Scalar, ParamDim, 1> Param;

    typedef SensorInterface<
                Obsrv,
                State,
                Noise
            > SensorBase;

    typedef AdaptiveModel<Param> AdaptiveModelBase;
};

/**
 * \ingroup sensors
 *
 * \brief JointSensor itself is an observation model which contains
 * internally multiple models. Each of the models must operate on the same state
 * space. The joint model can be simply summarized as
 * \f$ h(x, \theta, w) = [ h_1(x, \theta_1, w_1), h_2(x, \theta_2, w_2),
 * \ldots, h_n(x, \theta_n, w_n) ]^T \f$
 *
 * where \f$x\f$ is the state variate, \f$\theta = [ \theta_1, \theta_2, \ldots,
 * \theta_n ]^T\f$ and \f$w = [ w_1, w_2, \ldots, w_n ]^T\f$  are the the
 * parameters and noise terms of each of the \f$n\f$ models.
 *
 * JointSensor implements the SensorInterface and the
 * AdaptiveModel interface. That being said, the JointSensor can be
 * used as a regular observation model or even as an adaptive observation model
 * which provides a set of parameters that can be changed at any time. This
 * implies that all sub-models must implement the the AdaptiveModel
 * interface. However, if a sub-model is not adaptive, JointSensor
 * applies a decorator call the NotAdaptive operator on the sub-model. This
 * operator enables any model to be treated as if it is adaptive without
 * effecting the model behaviour.
 */
template <typename ... Models>
class JointSensor
    : public Traits<JointSensor<Models...>>::SensorBase
{
private:
    /** Typdef of \c This for #from_traits(TypeName) helper */
    typedef JointSensor<Models...> This;

public:
    typedef from_traits(State);
    typedef from_traits(Noise);
    typedef from_traits(Obsrv);
    typedef from_traits(Param);

public:
    /**
     * Constructor a JointSensor which is a composition of mixture of
     * fixed and dynamic-size observation models.
     *
     * \param models    Variadic list of shared pointers of the models
     */
    explicit
    JointSensor(std::shared_ptr<Models>...models)
        : models_(models...)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~JointSensor() noexcept { }

    /**
     * \copydoc SensorInterface::predict_obsrv
     */
    virtual Obsrv predict_obsrv(const State& state,
                                            const Noise& noise,
                                            double delta_time)
    {
        Obsrv prediction = Obsrv(obsrv_dimension(), 1);

        predict_obsrv<sizeof...(Models)>(
            models_,
            delta_time,
            state,
            noise,
            prediction);

        return prediction;
    }

    /**
     * \copydoc SensorInterface::state_dimension
     *
     * Determines the joint size of the state vector of all models
     */
    virtual int state_dimension() const
    {
        return expand_state_dimension(CreateIndexSequence<sizeof...(Models)>());
    }

    /**
     * \copydoc SensorInterface::noise_dimension
     *
     * Determines the joint size of the noise vector of all models
     */
    virtual int noise_dimension() const
    {
        return expand_noise_dimension(CreateIndexSequence<sizeof...(Models)>());
    }

    /**
     * \copydoc SensorInterface::input_dimension
     *
     * Determines the joint size of the observation vector of all models
     */
    virtual int obsrv_dimension() const
    {
        return expand_obsrv_dimension(CreateIndexSequence<sizeof...(Models)>());
    }

protected:
    /**
     * \brief Contains the points to the sub-models which this joint model
     *        is composed of.
     */
    std::tuple<std::shared_ptr<Models>...> models_;

private:
    /** \cond internal */
    template <int...Indices>
    constexpr int expand_state_dimension(IndexSequence<Indices...>) const
    {
        const auto& dims = { std::get<Indices>(models_)->state_dimension()... };

        int joint_dim = 0;
        for (auto dim : dims) { joint_dim += dim; }

        return joint_dim;
    }

    template <int...Indices>
    constexpr int expand_noise_dimension(IndexSequence<Indices...>) const
    {
        const auto& dims = { std::get<Indices>(models_)->noise_dimension()... };

        int joint_dim = 0;
        for (auto dim : dims) { joint_dim += dim; }

        return joint_dim;
    }

    template <int...Indices>
    constexpr int expand_obsrv_dimension(IndexSequence<Indices...>) const
    {
        const auto& dims =
            { std::get<Indices>(models_)->obsrv_dimension()... };

        int joint_dim = 0;
        for (auto dim : dims) { joint_dim += dim; }

        return joint_dim;
    }

    template <
        int Size,
        int k = 0,
        typename Tuple,
        typename State,
        typename Noise,
        typename Obsrv
    >
    void predict_obsrv(Tuple& models_tuple,
                       double delta_time,
                       const State& state,
                       const Noise& noise,
                       Obsrv& prediction,
                       const int obsrv_offset = 0,
                       const int state_offset = 0,
                       const int noise_offset = 0)
    {
        auto&& model = std::get<k>(models_tuple);

        const auto obsrv_dim = model->obsrv_dimension();
        const auto state_dim = model->state_dimension();
        const auto noise_dim = model->noise_dimension();

        prediction.middleRows(obsrv_offset, obsrv_dim) =
            model->predict_obsrv(
                state.middleRows(state_offset, state_dim),
                noise.middleRows(noise_offset, noise_dim),
                delta_time
            );

        if (Size == k + 1) return;

        predict_obsrv<Size, k + (k + 1 < Size ? 1 : 0)>(
                    models_tuple,
                    delta_time,
                    state,
                    noise,
                    prediction,
                    obsrv_offset + obsrv_dim,
                    state_offset + state_dim,
                    noise_offset + noise_dim);
    }
    /** \endcond */
};

}


#pragma once

// image magick
#define MAGICKCORE_EXCLUDE_DEPRECATED
#include <Magick++.h>
#include <iostream>
#include <map>
#include <vector>
// glew
#include <GL/glew.h>
// assimp
#include <assimp/Importer.hpp>      // C++ importer interface
#include <assimp/scene.h>       // Output data structure
#include <assimp/postprocess.h> // Post processing flags
// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
// std
#include <string>

#define INVALID_OGL_VALUE 0xffffffff
#define SAFE_DELETE(p) if (p) { delete p; p = NULL; }

using namespace Eigen;
using namespace std;

struct Vertex {
    Vector3f m_pos;
    Vector2f m_tex;
    Vector3f m_normal;

    Vertex() { }

    Vertex(const Vector3f &pos, const Vector2f &tex, const Vector3f &normal) {
        m_pos = pos;
        m_tex = tex;
        m_normal = normal;
    }
};

class Texture {
public:
    Texture(GLenum TextureTarget, const std::string &FileName);

    bool Load();

    void Bind(GLenum TextureUnit);

private:
    std::string m_fileName;
    GLenum m_textureTarget;
    GLuint m_textureObj;
    Magick::Image m_image;
    Magick::Blob m_blob;
};

class Mesh {
public:
    Mesh();

    ~Mesh();

    bool LoadMesh(const std::string &Filename);
    bool LoadMesh(const std::string &Filename, VectorXf &pose);

    void Render();

    string name = "default name";

    Matrix4f ModelMatrix;

    std::vector<Vertex> Vertices;

private:
    bool InitFromScene(const aiScene *pScene, const std::string &Filename);

    void InitMesh(unsigned int Index, const aiMesh *paiMesh);

    bool InitMaterials(const aiScene *pScene, const std::string &Filename);

    void Clear();

#define INVALID_MATERIAL 0xFFFFFFFF

    struct MeshEntry {
        MeshEntry();

        ~MeshEntry();

        void Init(const std::vector<Vertex> &Vertices,
                  const std::vector<unsigned int> &Indices);

        GLuint VB;
        GLuint IB;
        unsigned int NumIndices;
        unsigned int MaterialIndex;
    };

    std::vector<MeshEntry> m_Entries;
    std::vector<Texture *> m_Textures;
};


#pragma once

#include <iostream>

#include <boost/graph/adjacency_list.hpp>

#include "protocol.hpp"

struct ProtoMsg;
struct Vertex;
struct Edge;

/* There is no need to use boost::listS since we're not inserting nor removing
 * nodes, thus the performance gain is useless. Because of that we've choosen
 * boost::vectS which provides better space optimization. */
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
	Vertex, Edge> graph_t;
typedef boost::graph_traits<graph_t>::vertex_descriptor vertex_desc_t;

/* Route vector contains [<total cost>, <vector of vertices>] */
typedef std::pair<Edge, std::vector<Vertex *>> route_t;
typedef std::pair<Edge, std::pair<Vertex *, Vertex *>> edge_t;

struct Vertex {
	unsigned int id;
	struct coord {
		unsigned int x;
		unsigned int y;
	} coord = {};

	vertex_desc_t self = 0;
	vertex_desc_t prev = 0;
	std::vector<route_t> routing_table{};
	std::vector<ProtoMsg> msg_table{};
};

struct Edge {
	unsigned int cost;
};

class NetworkGraph
{
public:
	NetworkGraph(void);
	void graph_file(std::string);
	std::string graph_file(void) const;
	void sqr_weight(unsigned int);
	unsigned int grid_dim(void) const;
	unsigned int sqr_weight(void) const;

	void add_routes_from_file(void);
	void add_routes_random(void);
	int calc_routing_graph(void);
	route_t find_shortest_path(unsigned int, unsigned int);
	std::vector<edge_t> graph_edges(void) const;
	void clear_graph(void);
	Vertex * vertex(unsigned int);

private:
	void _calc_adjacent_list(std::vector<unsigned int>);

	graph_t _graph;
	// input filename for a graph already generated
	std::string _graph_filename;

	// how much a sqr represents as graph weight
	unsigned int _sqr_weight = 10;
	unsigned int _grid_dim = 5;
	// adjacent list to handle random nodes generation
	std::vector<std::pair<unsigned int,
			std::pair<unsigned int, unsigned int>>> _adj_list;
	// grid is a collection of "pixels" which might store a vertex or not:
	// in case a vertex is there present, it'll be added to busy_pixel vector,
	// easing the traverse when calculating the euclidean distance.
	std::vector<Vertex *> _grid;

	// vertex id <-> graph vertex ref lookup table
	std::vector<std::pair<unsigned int, Vertex *>> _vertex_lut;
	// node (vertex) id <-> node ref lookup table
};


#ifndef NETKET_COMMON_TYPES_HPP
#define NETKET_COMMON_TYPES_HPP
/**
 * This header contains standard type aliases to be used throughout the NetKet
 * codebase.
 */

#include <complex>
#include <cstddef>

#include <Eigen/Core>

namespace netket {

using Index = std::ptrdiff_t;
using Complex = std::complex<double>;

using VectorXd = Eigen::VectorXd;
using VectorXcd = Eigen::VectorXcd;
using MatrixXd = Eigen::MatrixXd;
using MatrixXcd = Eigen::MatrixXcd;

}  // namespace netket

#endif  // NETKET_COMMON_TYPES_HPP


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

#include "UnitTest.h"

#include <Eigen/Core>
#include <iostream>

using namespace std;

// ----------------------------------------------------------------------------
//
// ----------------------------------------------------------------------------
void unit_test::NotImplemented()
{
    cout << "\033[0;32m"
         << "[          ] "
         << "\033[0;0m";
    cout << "\033[0;31m"
         << "Not implemented."
         << "\033[0;0m" << endl;

    // FAIL();
    // ADD_FAILURE();
    GTEST_NONFATAL_FAILURE_("Not implemented");
}

// ----------------------------------------------------------------------------
// Equal test over Eigen::Vector2d components.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const Eigen::Vector2d& v0, const Eigen::Vector2d& v1)
{
    EXPECT_NEAR(v0(0, 0), v1(0, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(1, 0), v1(1, 0), unit_test::THRESHOLD_1E_6);
}
void unit_test::ExpectEQ(const double& v00, const double& v01, const Eigen::Vector2d& v1)
{
    EXPECT_NEAR(v00, v1(0, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v01, v1(1, 0), unit_test::THRESHOLD_1E_6);
}

// ----------------------------------------------------------------------------
// Equal test over Eigen::Vector3d components.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const Eigen::Vector3d& v0, const Eigen::Vector3d& v1)
{
    EXPECT_NEAR(v0(0, 0), v1(0, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(1, 0), v1(1, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(2, 0), v1(2, 0), unit_test::THRESHOLD_1E_6);
}
void unit_test::ExpectEQ(const double& v00, const double& v01, const double& v02, const Eigen::Vector3d& v1)
{
    EXPECT_NEAR(v00, v1(0, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v01, v1(1, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v02, v1(2, 0), unit_test::THRESHOLD_1E_6);
}

// ----------------------------------------------------------------------------
// Equal test over Eigen::Matrix3d components.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const Eigen::Matrix3d& v0, const Eigen::Matrix3d& v1)
{
    EXPECT_NEAR(v0(0, 0), v1(0, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(1, 0), v1(1, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(2, 0), v1(2, 0), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(0, 1), v1(0, 1), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(1, 1), v1(1, 1), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(2, 1), v1(2, 1), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(0, 2), v1(0, 2), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(1, 2), v1(1, 2), unit_test::THRESHOLD_1E_6);
    EXPECT_NEAR(v0(2, 2), v1(2, 2), unit_test::THRESHOLD_1E_6);
}

// ----------------------------------------------------------------------------
// Equal test over Eigen::Vector2i components.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const Eigen::Vector2i& v0, const Eigen::Vector2i& v1)
{
    EXPECT_EQ(v0(0, 0), v1(0, 0));
    EXPECT_EQ(v0(1, 0), v1(1, 0));
}
void unit_test::ExpectEQ(const int& v00, const int& v01, const Eigen::Vector2i& v1)
{
    EXPECT_EQ(v00, v1(0, 0));
    EXPECT_EQ(v01, v1(1, 0));
}

// ----------------------------------------------------------------------------
// Equal test over Eigen::Vector3i components.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const Eigen::Vector3i& v0, const Eigen::Vector3i& v1)
{
    EXPECT_EQ(v0(0, 0), v1(0, 0));
    EXPECT_EQ(v0(1, 0), v1(1, 0));
    EXPECT_EQ(v0(2, 0), v1(2, 0));
}
void unit_test::ExpectEQ(const int& v00, const int& v01, const int& v02, const Eigen::Vector3i& v1)
{
    EXPECT_EQ(v00, v1(0, 0));
    EXPECT_EQ(v01, v1(1, 0));
    EXPECT_EQ(v02, v1(2, 0));
}

// ----------------------------------------------------------------------------
// Less than or Equal test over Eigen::Vector3d components.
// ----------------------------------------------------------------------------
void unit_test::ExpectLE(const Eigen::Vector3d& v0, const Eigen::Vector3d& v1)
{
    EXPECT_LE(v0(0, 0), v1(0, 0));
    EXPECT_LE(v0(1, 0), v1(1, 0));
    EXPECT_LE(v0(2, 0), v1(2, 0));
}
void unit_test::ExpectLE(const double& v00, const double& v01, const double& v02, const Eigen::Vector3d& v1)
{
    EXPECT_LE(v00, v1(0, 0));
    EXPECT_LE(v01, v1(1, 0));
    EXPECT_LE(v02, v1(2, 0));
}

// ----------------------------------------------------------------------------
// Greater than or Equal test over Eigen::Vector3d components.
// ----------------------------------------------------------------------------
void unit_test::ExpectGE(const Eigen::Vector3d& v0, const Eigen::Vector3d& v1)
{
    EXPECT_GE(v0(0, 0), v1(0, 0));
    EXPECT_GE(v0(1, 0), v1(1, 0));
    EXPECT_GE(v0(2, 0), v1(2, 0));
}
void unit_test::ExpectGE(const double& v00, const double& v01, const double& v02, const Eigen::Vector3d& v1)
{
    EXPECT_GE(v00, v1(0, 0));
    EXPECT_GE(v01, v1(1, 0));
    EXPECT_GE(v02, v1(2, 0));
}

// ----------------------------------------------------------------------------
// Test equality of two arrays of uint8_t.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const uint8_t* const v0,
              const uint8_t* const v1,
              const size_t& size)
{
    for (int i = 0; i < size; i++)
        EXPECT_EQ(v0[i], v1[i]);
}

// ----------------------------------------------------------------------------
// Test equality of two vectors of uint8_t.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const vector<uint8_t>& v0, const vector<uint8_t>& v1)
{
    EXPECT_EQ(v0.size(), v1.size());
    ExpectEQ(&v0[0], &v1[0], v0.size());
}

// ----------------------------------------------------------------------------
// Test equality of two arrays of int.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const int* const v0,
              const int* const v1,
              const size_t& size)
{
    for (int i = 0; i < size; i++)
        EXPECT_EQ(v0[i], v1[i]);
}

// ----------------------------------------------------------------------------
// Test equality of two vectors of int.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const vector<int>& v0, const vector<int>& v1)
{
    EXPECT_EQ(v0.size(), v1.size());
    ExpectEQ(&v0[0], &v1[0], v0.size());
}

// ----------------------------------------------------------------------------
// Test equality of two arrays of double.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const double* const v0,
              const double* const v1,
              const size_t& size)
{
    for (int i = 0; i < size; i++)
        EXPECT_NEAR(v0[i], v1[i], THRESHOLD_1E_6);
}

// ----------------------------------------------------------------------------
// Test equality of two vectors of double.
// ----------------------------------------------------------------------------
void unit_test::ExpectEQ(const vector<double>& v0, const vector<double>& v1)
{
    EXPECT_EQ(v0.size(), v1.size());
    ExpectEQ(&v0[0], &v1[0], v0.size());
}


#include <iostream>

#include "imu_tk/io_utils.h"
#include "imu_tk/calibration.h"
#include "imu_tk/filters.h"
#include "imu_tk/integration.h"
#include "imu_tk/visualization.h"

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/algorithm/string/replace.hpp>

namespace fs = boost::filesystem;
namespace po = boost::program_options;

using namespace std;
using namespace imu_tk;
using namespace Eigen;

int main(int argc, char** argv)
{

  vector< TriadData > acc_data, gyro_data;

  string acc_file, gyr_file, suffix;
  double g_mag, init_interval_duration_, gyr_dt_, nominal_1g_norm_, init_acc_bias_, init_gyr_scale_, alpha_;
  int min_num_intervals_, interval_n_samples_, max_num_iterations_;
  bool acc_use_means_, optimize_gyro_bias_, verbose_output_, use_acc_G_, use_gyr_G_;

  po::options_description po("Options for imu_tk2");
  po.add_options()
      ("help", "print options")
      ("acc_file", po::value<string>(& acc_file)->default_value("acc"), "file containing acc readings")
      ("gyr_file", po::value<string>(& gyr_file)->default_value("gyr"), "file containing gyro readings")

      // double params
      ("g_mag, g", po::value<double>(& g_mag)->default_value(9.805622), "magntute of local gravity")
      ("init_interval_duration", po::value<double>(&init_interval_duration_)->default_value(50.0), "duration of the initial interval")
      ("gyr_dt", po::value<double>(& gyr_dt_)->default_value(-1), "gyro rate, -1 automatic")
      ("nominal_1g_norm", po::value<double>(& nominal_1g_norm_)->default_value(16384.0), "Datasheet value for 1g acc")
      ("alpha", po::value<double>(& alpha_)->default_value(0.75), "weight for bias minimesation objective. Set to 1 if not 0.5 < alpha < 1")

      // int params
      ("min_num_intervals", po::value<int>(& min_num_intervals_)->default_value(9), "Minimum numbr of intervals to perform calibration")
      ("interval_n_samples", po::value<int>(& interval_n_samples_)->default_value(300), "Minimum number of samples for static intervals")
      ("max_iter", po::value<int>(& max_num_iterations_)->default_value(100), "Maximum number of iterations for Ceres Solver")

      // bool params
      ("acc_use_means", po::value<bool>(& acc_use_means_)->default_value(false), "Use means for acc")
      ("opt_gyr_b", po::value<bool>(& optimize_gyro_bias_)->default_value(false), "if false calculates the gyro biases bases on the initial static interval")
      ("use_acc_G", po::value<bool>(& use_acc_G_)->default_value(false), "calculate acc proportional bias for accelerometer")
      ("use_gyr_G", po::value<bool>(& use_gyr_G_)->default_value(false), "calculate acc proportional bias for gyroscope")
      ("verbose", po::value<bool>(& verbose_output_)->default_value(true), "Print a lot of stuff")

      // Initial calibration guesses
      ("init_acc_bias", po::value<double>(& init_acc_bias_)->default_value(1), "Initial guess of the accelerometer bias")
      ("init_gyr_scale", po::value<double>(& init_gyr_scale_)->default_value((250 * M_PI / (2.0 * 180.0 * 16384.0))), "initial guess for gyro scale - from datasheet")
      ("suffix", po::value<string>(& suffix)->default_value("params"), "sufix for calibration results files")
      ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, po), vm);
  po::notify(vm);

  if (vm.count("help"))
  {
      cout << "Usage: options_description [options]\n";
      cout << po << std::endl;
      return 0;
  }

  // Handle file paths
  boost::replace_all(acc_file, "~", getenv ("HOME"));
  boost::replace_all(gyr_file, "~", getenv ("HOME"));

  cout<<"Importing IMU data from the Matlab matrix file : "<< acc_file << endl;
  importAsciiData( acc_file.c_str(), acc_data, imu_tk::TIMESTAMP_UNIT_SEC );
  cout<<"Importing IMU data from the Matlab matrix file : "<< gyr_file << endl;
  importAsciiData( gyr_file.c_str(), gyro_data, imu_tk::TIMESTAMP_UNIT_SEC  );

  if(acc_data.size()==0)
  {
    cout << "\nNo data in the files... exiting" << endl;
    return 1;
  }

  MultiPosCalibration mp_calib;

  //double params
  mp_calib.setGravityMagnitude(g_mag);
  mp_calib.setInitStaticIntervalDuration(init_interval_duration_);
  mp_calib.setGyroDataPeriod(gyr_dt_);
  mp_calib.set1g(nominal_1g_norm_);

  // int params
  mp_calib.setMinNumIntervals(min_num_intervals_);
  mp_calib.setIntarvalsNumSamples(interval_n_samples_);
  mp_calib.setMaxIte(max_num_iterations_);

  // bool params
  mp_calib.enableAccUseMeans(acc_use_means_);
  mp_calib.enableAccG(use_acc_G_);
  mp_calib.enableGyrG(use_gyr_G_);
  mp_calib.enableGyroBiasOptimization(optimize_gyro_bias_);
  mp_calib.enableVerboseOutput(verbose_output_);

  // string params
  mp_calib.setPlotFile(acc_file + "_" + suffix + ".png");

  //init_acc_calib.setBias( Vector3d(32768, 32768, 32768) );
  //init_gyro_calib.setScale( Vector3d(1.0/6258.0, 1.0/6258.0, 1.0/6258.0) );

  // Set initial guess for calibration
  CalibratedTriad init_acc_calib, init_gyro_calib;
  init_acc_calib.setBias( Vector3d(init_acc_bias_, init_acc_bias_, init_acc_bias_) );
  init_gyro_calib.setScale( Vector3d(init_gyr_scale_, init_gyr_scale_, init_gyr_scale_) );
  mp_calib.setInitAccCalibration( init_acc_calib );
  mp_calib.setInitGyroCalibration( init_gyro_calib );

  // Perform the calibration
  mp_calib.calibrateAccGyro(acc_data, gyro_data );

  // Save results
  string acc_params_name = acc_file + "." + suffix;
  string gyr_params_name = gyr_file + "." + suffix;

  cout << "file names:\n" << acc_params_name << "\n" << gyr_params_name << "\n\n";
  mp_calib.getAccCalib().saveAcc(acc_params_name);
  mp_calib.getGyroCalib().saveGyr(gyr_params_name);

  return 0;
}


#include <blitzcommon/HashUtils.h>

namespace blitz
{
    hash hashString(const char* const str)
    {
        unsigned int h;

        h = 0;
        for (size_t idx = 0; str[idx] != '\0'; ++idx)
            h = 37 * h + str[idx];

        return h;
    }
} // namespace blitz


#include <boost/math/tools/precision.hpp>


/**
BSD 3-Clause License

This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git

Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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
@brief Definition of static IMU biases used for calibration
*/

#pragma once

#include <Eigen/Dense>

namespace basalt {

/// @brief Static calibration for accelerometer.
///
/// Calibrates axis scaling and misalignment and has 9 parameters \f$ [b_x,
/// b_y, b_z, s_1, s_2, s_3, s_4, s_5, s_6]^T \f$.
/// \f[
/// a_c = \begin{bmatrix} s_1 + 1 & 0 & 0 \\ s_2 & s_4 + 1 & 0 \\ s_3 & s_5 &
/// s_6 + 1 \\  \end{bmatrix} a_r -  \begin{bmatrix} b_x \\ b_y \\ b_z
/// \end{bmatrix}
/// \f] where  \f$ a_c \f$ is a calibrated measurement and \f$ a_r \f$ is a
/// raw measurement. When all elements are zero applying calibration results in
/// Identity mapping.
template <typename Scalar>
class CalibAccelBias {
 public:
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
  using Mat33 = Eigen::Matrix<Scalar, 3, 3>;

  /// @brief Default constructor with zero initialization.
  inline CalibAccelBias() { accel_bias_full_.setZero(); }

  /// @brief  Set calibration to random values (used in unit-tests).
  inline void setRandom() {
    accel_bias_full_.setRandom();
    accel_bias_full_.template head<3>() /= 10;
    accel_bias_full_.template tail<6>() /= 100;
  }

  /// @brief Return const vector of parameters.
  /// See detailed description in \ref CalibAccelBias.
  inline const Eigen::Matrix<Scalar, 9, 1>& getParam() const {
    return accel_bias_full_;
  }

  /// @brief Return vector of parameters. See detailed description in \ref
  /// CalibAccelBias.
  inline Eigen::Matrix<Scalar, 9, 1>& getParam() { return accel_bias_full_; }

  /// @brief Increment the calibration vector
  ///
  /// @param inc increment vector
  inline void operator+=(const Eigen::Matrix<Scalar, 9, 1>& inc) {
    accel_bias_full_ += inc;
  }

  /// @brief Return bias vector and scale matrix. See detailed description in
  /// \ref CalibAccelBias.
  inline void getBiasAndScale(Vec3& accel_bias, Mat33& accel_scale) const {
    accel_bias = accel_bias_full_.template head<3>();

    accel_scale.setZero();
    accel_scale.col(0) = accel_bias_full_.template segment<3>(3);
    accel_scale(1, 1) = accel_bias_full_(6);
    accel_scale(2, 1) = accel_bias_full_(7);
    accel_scale(2, 2) = accel_bias_full_(8);
  }

  /// @brief Calibrate the measurement. See detailed description in
  /// \ref CalibAccelBias.
  ///
  /// @param raw_measurement
  /// @return calibrated measurement
  inline Vec3 getCalibrated(const Vec3& raw_measurement) const {
    Vec3 accel_bias;
    Mat33 accel_scale;

    getBiasAndScale(accel_bias, accel_scale);

    return (raw_measurement + accel_scale * raw_measurement - accel_bias);
  }

  /// @brief Invert calibration (used in unit-tests).
  ///
  /// @param calibrated_measurement
  /// @return raw measurement
  inline Vec3 invertCalibration(const Vec3& calibrated_measurement) const {
    Vec3 accel_bias;
    Mat33 accel_scale;

    getBiasAndScale(accel_bias, accel_scale);

    Mat33 accel_scale_inv =
        (Eigen::Matrix3d::Identity() + accel_scale).inverse();

    return accel_scale_inv * (calibrated_measurement + accel_bias);
  }

 private:
  Eigen::Matrix<Scalar, 9, 1> accel_bias_full_;
};

/// @brief Static calibration for gyroscope.
///
/// Calibrates rotation, axis scaling and misalignment and has 12 parameters \f$
/// [b_x, b_y, b_z, s_1, s_2, s_3, s_4, s_5, s_6, s_7, s_8, s_9]^T \f$. \f[
/// \omega_c = \begin{bmatrix} s_1 + 1 & s_4 & s_7 \\ s_2 & s_5 + 1 & s_8 \\ s_3
/// & s_6 & s_9 +1 \\  \end{bmatrix} \omega_r -  \begin{bmatrix} b_x \\ b_y
/// \\ b_z \end{bmatrix} \f] where  \f$ \omega_c \f$ is a calibrated measurement
/// and \f$ \omega_r \f$ is a raw measurement. When all elements are zero
/// applying calibration results in Identity mapping.
template <typename Scalar>
class CalibGyroBias {
 public:
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
  using Mat33 = Eigen::Matrix<Scalar, 3, 3>;

  /// @brief Default constructor with zero initialization.
  inline CalibGyroBias() { gyro_bias_full_.setZero(); }

  /// @brief  Set calibration to random values (used in unit-tests).
  inline void setRandom() {
    gyro_bias_full_.setRandom();
    gyro_bias_full_.template head<3>() /= 10;
    gyro_bias_full_.template tail<9>() /= 100;
  }

  /// @brief Return const vector of parameters.
  /// See detailed description in \ref CalibGyroBias.
  inline const Eigen::Matrix<Scalar, 12, 1>& getParam() const {
    return gyro_bias_full_;
  }

  /// @brief Return vector of parameters.
  /// See detailed description in \ref CalibGyroBias.
  inline Eigen::Matrix<Scalar, 12, 1>& getParam() { return gyro_bias_full_; }

  /// @brief Increment the calibration vector
  ///
  /// @param inc increment vector
  inline void operator+=(const Eigen::Matrix<Scalar, 12, 1>& inc) {
    gyro_bias_full_ += inc;
  }

  /// @brief Return bias vector and scale matrix. See detailed description in
  /// \ref CalibGyroBias.
  inline void getBiasAndScale(Vec3& gyro_bias, Mat33& gyro_scale) const {
    gyro_bias = gyro_bias_full_.template head<3>();
    gyro_scale.col(0) = gyro_bias_full_.template segment<3>(3);
    gyro_scale.col(1) = gyro_bias_full_.template segment<3>(6);
    gyro_scale.col(2) = gyro_bias_full_.template segment<3>(9);
  }

  /// @brief Calibrate the measurement. See detailed description in
  /// \ref CalibGyroBias.
  ///
  /// @param raw_measurement
  /// @return calibrated measurement
  inline Vec3 getCalibrated(const Vec3& raw_measurement) const {
    Vec3 gyro_bias;
    Mat33 gyro_scale;

    getBiasAndScale(gyro_bias, gyro_scale);

    return (raw_measurement + gyro_scale * raw_measurement - gyro_bias);
  }

  /// @brief Invert calibration (used in unit-tests).
  ///
  /// @param calibrated_measurement
  /// @return raw measurement
  inline Vec3 invertCalibration(const Vec3& calibrated_measurement) const {
    Vec3 gyro_bias;
    Mat33 gyro_scale;

    getBiasAndScale(gyro_bias, gyro_scale);

    Mat33 gyro_scale_inv = (Eigen::Matrix3d::Identity() + gyro_scale).inverse();

    return gyro_scale_inv * (calibrated_measurement + gyro_bias);
  }

 private:
  Eigen::Matrix<Scalar, 12, 1> gyro_bias_full_;
};

}  // namespace basalt


//==================================================================================================
/*
  Copyright 2012 - 2015 NumScale SAS

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

//! [hello]
#include <boost/simd/pack.hpp>
#include <boost/simd/io.hpp>
#include <boost/simd/functions/splat.hpp>
#include <boost/simd/functions/plus.hpp>
#include <boost/simd/functions/multiplies.hpp>
#include <iostream>

int main()
{
  using p_t = boost::simd::pack<float>;

  //! [hello-def-ctor]
  p_t res;
  //! [hello-def-ctor]

  //! [hello-splat-ctor]
  p_t u{10};
  //! [hello-splat-ctor]

  //! [hello-enum-ctor]
  p_t r{11,11,11,11};
  //! [hello-enum-ctor]

  //! [hello-ops]
  res = (u + r) * 2.f;
  //! [hello-ops]

  //! [hello-io]
  std::cout << res << std::endl;
  //! [hello-io]

  return 0;
}
//! [hello]


#include <spatialops/SpatialOpsTools.h>

#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/OperatorDatabase.h>

#include <spatialops/structured/stencil/FVStaggeredOperatorTypes.h>
#include <spatialops/structured/stencil/StencilBuilder.h>

#include "ReferenceStencil.h"
#include <test/TestHelper.h>

#include <spatialops/Nebo.h>

#include <spatialops/structured/FieldComparisons.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace SpatialOps;

#include <stdexcept>
using std::cout;
using std::endl;

//--------------------------------------------------------------------

#ifndef NEBO_GPU_TEST
#define ACCEPTABLE_ERROR 0.0
#else
#define ACCEPTABLE_ERROR 1.0e-10
#endif

//--------------------------------------------------------------------

template<typename Field>
void initialize_field(Field & f)
{
  typename Field::iterator i = f.begin();
  MemoryWindow mw = f.window_with_ghost();
  int xLength = mw.extent(0);
  int yLength = mw.extent(1);
  int zLength = mw.extent(2);

  for(int x = 1; x <= xLength; x++) {
    for(int y = 1; y <= yLength; y++) {
      for(int z = 1; z <= zLength; z++) {
        *i = sin(x +
            y * xLength +
            z * xLength * yLength);
        i++;
      };
    };
  };
};

//--------------------------------------------------------------------

template<typename OpType, typename SrcType, typename DestType>
bool test_stencil2( const IntVec& npts,
                    const OperatorDatabase& opdb,
                    const IntVec& bc )
{
  //basic definitions:
  const GhostData  srcGhost(1);
  const GhostData destGhost(1);
  const BoundaryCellInfo  srcBC = BoundaryCellInfo::build< SrcType>(bc,bc);
  const BoundaryCellInfo destBC = BoundaryCellInfo::build<DestType>(bc,bc);
  const MemoryWindow mwSrc  = get_window_with_ghost(npts,  srcGhost,  srcBC);
  const MemoryWindow mwDest = get_window_with_ghost(npts, destGhost, destBC);

  SrcType  src ( mwSrc,   srcBC,  srcGhost, NULL );
  DestType ref ( mwDest, destBC, destGhost, NULL );
  DestType test( mwDest, destBC, destGhost, NULL );

  //initialize source field / zero out result fields:
  initialize_field(src);
  ref <<= 0.0;
  test <<= 0.0;

  //get operator:
  typedef typename SpatialOps::OperatorTypeBuilder<OpType,SrcType,DestType>::type Op;
  const Op* const op = opdb.retrieve_operator<Op>();

  //run reference:
  ref_stencil2_apply_to_field(op->coefs().get_coef(0),
                              op->coefs().get_coef(1),
                              src,
                              ref);

  //run operator:
  op->apply_to_field(src, test);

  return (field_equal(test, ref, ACCEPTABLE_ERROR));
};

//--------------------------------------------------------------------

template<typename OpType, typename SrcType, typename DestType>
bool test_stencil4(const IntVec npts,
                   const OperatorDatabase & opdb,
                   const IntVec& bc )
{
  //basic definitions:
  const GhostData  srcGhost(1);
  const GhostData destGhost(1);
  const BoundaryCellInfo  srcBC = BoundaryCellInfo::build< SrcType>(bc,bc);
  const BoundaryCellInfo destBC = BoundaryCellInfo::build<DestType>(bc,bc);
  const MemoryWindow mwSrc  = get_window_with_ghost(npts,  srcGhost,  srcBC);
  const MemoryWindow mwDest = get_window_with_ghost(npts, destGhost, destBC);
  SrcType  src (mwSrc,   srcBC,  srcGhost, NULL);
  DestType ref (mwDest, destBC, destGhost, NULL);
  DestType test(mwDest, destBC, destGhost, NULL);

  //initialize source field / zero out result fields:
  initialize_field(src);
  ref <<= 0.0;
  test <<= 0.0;

  //get operator:
  typedef typename SpatialOps::OperatorTypeBuilder<OpType,SrcType,DestType>::type Op;
  const Op* const op = opdb.retrieve_operator<Op>();

  //run reference:
  ref_stencil4_apply_to_field(op->coefs().get_coef(0),
                              op->coefs().get_coef(1),
                              op->coefs().get_coef(2),
                              op->coefs().get_coef(3),
                              src,
                              ref);

  //run operator:
  op->apply_to_field(src, test);

  return (field_equal(test, ref, ACCEPTABLE_ERROR));
};

//--------------------------------------------------------------------

template<typename OpType, typename FieldType>
bool test_fd_stencil2(const IntVec npts,
                      const OperatorDatabase & opdb,
                      const IntVec& bc )
{
  //basic definitions:
  const GhostData ghost(1);
  const BoundaryCellInfo bcinfo = BoundaryCellInfo::build<FieldType>(bc,bc);
  const MemoryWindow mw  = get_window_with_ghost(npts, ghost, bcinfo);
  FieldType src (mw, bcinfo, ghost, NULL);
  FieldType ref (mw, bcinfo, ghost, NULL);
  FieldType test(mw, bcinfo, ghost, NULL);

  //initialize source field / zero out result fields:
  initialize_field(src);
  ref <<= 0.0;
  test <<= 0.0;

  //get operator:
  typedef typename SpatialOps::OperatorTypeBuilder<OpType,FieldType,FieldType>::type Op;
  const Op* const op = opdb.retrieve_operator<Op>();

  //run reference:
  ref_fd_stencil2_apply_to_field<OpType,FieldType>(op->coefs().get_coef(0),
                                                   op->coefs().get_coef(1),
                                                   src,
                                                   ref);

  //run operator:
  op->apply_to_field(src, test);

  return (field_equal(test, ref, ACCEPTABLE_ERROR));
};

//--------------------------------------------------------------------

template<typename OpType, typename SrcType, typename DestType>
bool test_null_stencil(const IntVec npts,
                       const OperatorDatabase & opdb,
                       const IntVec& bc )
{
  //basic definitions:
  const GhostData  srcGhost(1);
  const GhostData destGhost(1);
  const BoundaryCellInfo  srcBC = BoundaryCellInfo::build< SrcType>(bc,bc);
  const BoundaryCellInfo destBC = BoundaryCellInfo::build<DestType>(bc,bc);
  const MemoryWindow mwSrc  = get_window_with_ghost(npts,  srcGhost,  srcBC);
  const MemoryWindow mwDest = get_window_with_ghost(npts, destGhost, destBC);
  SrcType  src (mwSrc,   srcBC,  srcGhost, NULL);
  DestType ref (mwDest, destBC, destGhost, NULL);
  DestType test(mwDest, destBC, destGhost, NULL);

  //initialize source field / zero out result fields:
  initialize_field(src);
  ref <<= 0.0;
  test <<= 0.0;

  //get operator:
  typedef typename SpatialOps::OperatorTypeBuilder<OpType,SrcType,DestType>::type Op;
  const Op* const op = opdb.retrieve_operator<Op>();

  //run reference:
  ref_null_stencil_apply_to_field(src, ref);

  //run operator:
  op->apply_to_field(src, test);

  return (field_equal(test, ref, ACCEPTABLE_ERROR));
};

//--------------------------------------------------------------------

template<typename OpType, typename FieldType>
bool test_box_filter_stencil(const IntVec npts,
                             const OperatorDatabase & opdb,
                             const IntVec& bc )
{
  //basic definitions:
  const GhostData ghost(1);
  const BoundaryCellInfo bcinfo = BoundaryCellInfo::build<FieldType>(bc,bc);
  const MemoryWindow mw = get_window_with_ghost(npts, ghost, bcinfo);
  FieldType src (mw, bcinfo, ghost, NULL);
  FieldType ref (mw, bcinfo, ghost, NULL);
  FieldType test(mw, bcinfo, ghost, NULL);

  //initialize source field / zero out result fields:
  initialize_field(src);
  ref <<= 0.0;
  test <<= 0.0;

  //get operator:
  typedef typename SpatialOps::OperatorTypeBuilder<OpType,FieldType,FieldType>::type Op;
  const Op* const op = opdb.retrieve_operator<Op>();

  //run reference:
  ref_box_filter_stencil_apply_to_field(src, ref);

  //run operator:
  op->apply_to_field(src, test);

  return (field_equal(test, ref, ACCEPTABLE_ERROR));
};

//--------------------------------------------------------------------

template<typename VolField>
inline bool test_basic_stencils( const IntVec npts,
                                 const OperatorDatabase & opdb,
                                 const IntVec& bc )
{
  typedef typename FaceTypes<VolField>::XFace SurfXField;
  typedef typename FaceTypes<VolField>::YFace SurfYField;
  typedef typename FaceTypes<VolField>::ZFace SurfZField;

  TestHelper status(true);

  if( npts[0] > 1) {
    status( test_stencil2<Interpolant, VolField,   SurfXField>(npts, opdb, bc), "Interpolant VolField -> SurfXField (2)" );
    status( test_stencil2<Gradient,    VolField,   SurfXField>(npts, opdb, bc), "Gradient    VolField -> SurfXField (2)" );
    status( test_stencil2<Divergence,  SurfXField, VolField>  (npts, opdb, bc), "Divergence  SurfXField -> VolField (2)" );
  };

  if( npts[1] > 1) {
    status( test_stencil2<Interpolant, VolField,   SurfYField>(npts, opdb, bc), "Interpolant VolField -> SurfYField (2)" );
    status( test_stencil2<Gradient,    VolField,   SurfYField>(npts, opdb, bc), "Gradient    VolField -> SurfYField (2)" );
    status( test_stencil2<Divergence,  SurfYField, VolField>  (npts, opdb, bc), "Divergence  SurfYField -> VolField (2)" );
  };

  if( npts[2] > 1) {
    status( test_stencil2<Interpolant, VolField,   SurfZField>(npts, opdb, bc), "Interpolant VolField -> SurfZField (2)" );
    status( test_stencil2<Gradient,    VolField,   SurfZField>(npts, opdb, bc), "Gradient    VolField -> SurfZField (2)" );
    status( test_stencil2<Divergence,  SurfZField, VolField>  (npts, opdb, bc), "Divergence  SurfZField -> VolField (2)" );
  };

  return status.ok();
};

//--------------------------------------------------------------------

#define TEST_EXTENTS( SRC, DEST,                                                                                \
                      DirT,                                                                                \
                      S2Ox,  S2Oy,  S2Oz,                                                                                \
                      DOx,   DOy,   DOz,                                                                                \
                      ULx,   ULy,   ULz,                                                                                \
                      name )                                                                                \
    {                                                                                \
      using std::string;                                                                                \
      typedef ExtentsAndOffsets<SRC,DEST> Extents;                                                                               \
      status( IsSameType< Extents::Dir,        DirT                          >::result, string(name) + string(" dir"       ) );  \
      status( IsSameType< Extents::Src2Offset, IndexTriplet<S2Ox,S2Oy,S2Oz > >::result, string(name) + string(" s2 offset" ) );  \
      status( IsSameType< Extents::DestOffset, IndexTriplet<DOx, DOy, DOz  > >::result, string(name) + string(" d  offset" ) );  \
      status( IsSameType< Extents::Src1Extent, IndexTriplet<ULx, ULy, ULz  > >::result, string(name) + string(" UB"        ) );  \
    }
//-------------------------------------------------------------------

bool test_compile_time()
{
  using namespace SpatialOps;
  using namespace RefStencil2Detail;

  TestHelper status(false);

  status( IsSameType< ActiveDir< YVolField, SVolField   >::type, YDIR >::result, "YVol->SVol (y)" );
  status( IsSameType< ActiveDir< YVolField, XSurfYField >::type, XDIR >::result, "YVol->ZSY  (x)" );
  status( IsSameType< ActiveDir< YVolField, ZSurfYField >::type, ZDIR >::result, "YVol->ZSY  (z)" );

  status( IsSameType< ActiveDir< ZVolField, SVolField   >::type, ZDIR >::result, "ZVol->SVol (z)" );
  status( IsSameType< ActiveDir< ZVolField, XSurfZField >::type, XDIR >::result, "ZVol->XSZ  (x)" );
  status( IsSameType< ActiveDir< ZVolField, YSurfZField >::type, YDIR >::result, "ZVol->YSZ  (y)" );

  status( IsSameType< ActiveDir< SVolField, XVolField >::type, XDIR >::result, "SVol->XVol (x)" );
  status( IsSameType< ActiveDir< SVolField, YVolField >::type, YDIR >::result, "SVol->YVol (y)" );
  status( IsSameType< ActiveDir< SVolField, ZVolField >::type, ZDIR >::result, "SVol->ZVol (z)" );

  TEST_EXTENTS( SVolField, SSurfXField, XDIR,  1,0,0,  1,0,0,  -1, 0, 0,  "SVol->SSX" )
  TEST_EXTENTS( SVolField, SSurfYField, YDIR,  0,1,0,  0,1,0,   0,-1, 0,  "SVol->SSY" )
  TEST_EXTENTS( SVolField, SSurfZField, ZDIR,  0,0,1,  0,0,1,   0, 0,-1,  "SVol->SSZ" )
  TEST_EXTENTS( SSurfXField, SVolField, XDIR,  1,0,0,  0,0,0,  -1, 0, 0,  "SSX->SVol" )
  TEST_EXTENTS( SSurfYField, SVolField, YDIR,  0,1,0,  0,0,0,   0,-1, 0,  "SSY->SVol" )
  TEST_EXTENTS( SSurfZField, SVolField, ZDIR,  0,0,1,  0,0,0,   0, 0,-1,  "SSZ->SVol" )

  TEST_EXTENTS( XVolField, XSurfXField, XDIR,  1,0,0,  0,0,0,  -1, 0, 0,  "XVol->XSX" )
  TEST_EXTENTS( XVolField, XSurfYField, YDIR,  0,1,0,  0,1,0,   0,-1, 0,  "XVol->XSY" )
  TEST_EXTENTS( XVolField, XSurfZField, ZDIR,  0,0,1,  0,0,1,   0, 0,-1,  "XVol->XSZ" )

  TEST_EXTENTS( XSurfXField, XVolField, XDIR,  1,0,0,  1,0,0,  -1, 0, 0,  "XSX->XVol" )
  TEST_EXTENTS( XSurfYField, XVolField, YDIR,  0,1,0,  0,0,0,   0,-1, 0,  "XSY->XVol" )
  TEST_EXTENTS( XSurfZField, XVolField, ZDIR,  0,0,1,  0,0,0,   0, 0,-1,  "XSZ->XVol" )

  TEST_EXTENTS( YVolField, YSurfXField, XDIR,  1,0,0,  1,0,0,  -1, 0, 0,  "YVol->YSX" )
  TEST_EXTENTS( YVolField, YSurfYField, YDIR,  0,1,0,  0,0,0,   0,-1, 0,  "YVol->YSY" )
  TEST_EXTENTS( YVolField, YSurfZField, ZDIR,  0,0,1,  0,0,1,   0, 0,-1,  "YVol->YSZ" )
  TEST_EXTENTS( YSurfXField, YVolField, XDIR,  1,0,0,  0,0,0,  -1, 0, 0,  "YSX->YVol" )
  TEST_EXTENTS( YSurfYField, YVolField, YDIR,  0,1,0,  0,1,0,   0,-1, 0,  "YSY->YVol" )
  TEST_EXTENTS( YSurfZField, YVolField, ZDIR,  0,0,1,  0,0,0,   0, 0,-1,  "YSZ->YVol" )

  TEST_EXTENTS( ZVolField, ZSurfXField, XDIR,  1,0,0,  1,0,0,  -1, 0, 0,  "ZVol->ZSX" )
  TEST_EXTENTS( ZVolField, ZSurfYField, YDIR,  0,1,0,  0,1,0,   0,-1, 0,  "ZVol->ZSY" )
  TEST_EXTENTS( ZVolField, ZSurfZField, ZDIR,  0,0,1,  0,0,0,   0, 0,-1,  "ZVol->ZSZ" )
  TEST_EXTENTS( ZSurfXField, ZVolField, XDIR,  1,0,0,  0,0,0,  -1, 0, 0,  "ZSX->ZVol" )
  TEST_EXTENTS( ZSurfYField, ZVolField, YDIR,  0,1,0,  0,0,0,   0,-1, 0,  "ZSY->ZVol" )
  TEST_EXTENTS( ZSurfZField, ZVolField, ZDIR,  0,0,1,  0,0,1,   0, 0,-1,  "ZSZ->ZVol" )

  TEST_EXTENTS( XVolField, SVolField,   XDIR,  1,0,0,  0,0,0,  -1, 0, 0,  "XVol->SVol" )
  TEST_EXTENTS( XVolField, YSurfXField, YDIR,  0,1,0,  0,1,0,   0,-1, 0,  "XVol->YSX"  )
  TEST_EXTENTS( XVolField, ZSurfXField, ZDIR,  0,0,1,  0,0,1,   0, 0,-1,  "XVol->ZSX"  )

  return status.ok();
}

//--------------------------------------------------------------------

int main( int iarg, char* carg[] )
{
  int nx, ny, nz;
  IntVec bc( false, false, false );

  {
    po::options_description desc("Supported Options");
    desc.add_options()
      ( "help", "print help message\n" )
      ( "nx",  po::value<int>(&nx)->default_value(11), "number of points in x-dir for base mesh" )
      ( "ny",  po::value<int>(&ny)->default_value(11), "number of points in y-dir for base mesh" )
      ( "nz",  po::value<int>(&nz)->default_value(11), "number of points in z-dir for base mesh" )
      ( "bcx", "physical boundary on +x side?" )
      ( "bcy", "physical boundary on +y side?" )
      ( "bcz", "physical boundary on +z side?" );

    po::variables_map args;
    po::store( po::parse_command_line(iarg,carg,desc), args );
    po::notify(args);

    if( args.count("bcx") ) bc[0] = true;
    if( args.count("bcy") ) bc[1] = true;
    if( args.count("bcz") ) bc[2] = true;

    if( args.count("help") ){
      cout << desc << endl
           << "Examples:" << endl
           << " test_stencil --nx 5 --ny 10 --nz 3 --bcx" << endl
           << " test_stencil --bcx --bcy --bcz" << endl
           << " test_stencil --nx 50 --bcz" << endl
           << endl;
      return -1;
    }
  }

  TestHelper status( true );
  const IntVec npts(nx,ny,nz);

  {
    const std::string bcx = bc[0] ? "ON" : "OFF";
    const std::string bcy = bc[1] ? "ON" : "OFF";
    const std::string bcz = bc[2] ? "ON" : "OFF";
    cout << "Run information: " << endl
         << "  bcx    : " << bcx << endl
         << "  bcy    : " << bcy << endl
         << "  bcz    : " << bcz << endl
         << "  domain : " << npts << endl
         << endl;
  }

  status( test_compile_time(), "Compile time type introspection tests" );
  cout << endl;

  const double length = 10.0;

  OperatorDatabase opdb;
  build_stencils( npts[0], npts[1], npts[2], length, length, length, opdb );

  try{
      //Stencil2 tests:

      status( test_basic_stencils<SVolField>(npts, opdb, bc), "SVol operators");
      if( npts[0]>1 ) { status( test_basic_stencils<XVolField>(npts, opdb, bc), "XVol operators"); };
      if( npts[1]>1 ) { status( test_basic_stencils<YVolField>(npts, opdb, bc), "YVol operators"); };
      if( npts[2]>1 ) { status( test_basic_stencils<ZVolField>(npts, opdb, bc), "ZVol operators"); };

      if( npts[0]>1 && npts[1]>1 ) status( test_stencil2<Interpolant, XVolField, YSurfXField>(npts, opdb, bc), "Interpolant XVolField -> YSurfXField (2)" );
      if( npts[0]>1 && npts[1]>1 ) status( test_stencil2<Gradient,    XVolField, YSurfXField>(npts, opdb, bc), "Gradient    XVolField -> YSurfXField (2)" );

      if( npts[0]>1 && npts[2]>1 ) status( test_stencil2<Interpolant, XVolField, ZSurfXField>(npts, opdb, bc), "Interpolant XVolField -> ZSurfXField (2)" );
      if( npts[0]>1 && npts[2]>1 ) status( test_stencil2<Gradient,    XVolField, ZSurfXField>(npts, opdb, bc), "Gradient    XVolField -> ZSurfXField (2)" );

      if( npts[1]>1 && npts[0]>1 ) status( test_stencil2<Interpolant, YVolField, XSurfYField>(npts, opdb, bc), "Interpolant YVolField -> XSurfYField (2)" );
      if( npts[1]>1 && npts[0]>1 ) status( test_stencil2<Gradient,    YVolField, XSurfYField>(npts, opdb, bc), "Gradient    YVolField -> XSurfYField (2)" );

      if( npts[1]>1 && npts[2]>1 ) status( test_stencil2<Interpolant, YVolField, ZSurfYField>(npts, opdb, bc), "Interpolant YVolField -> ZSurfYField (2)" );
      if( npts[1]>1 && npts[2]>1 ) status( test_stencil2<Gradient,    YVolField, ZSurfYField>(npts, opdb, bc), "Gradient    YVolField -> ZSurfYField (2)" );

      if( npts[2]>1 && npts[0]>1 ) status( test_stencil2<Interpolant, ZVolField, XSurfZField>(npts, opdb, bc), "Interpolant ZVolField -> XSurfZField (2)" );
      if( npts[2]>1 && npts[0]>1 ) status( test_stencil2<Gradient,    ZVolField, XSurfZField>(npts, opdb, bc), "Gradient    ZVolField -> XSurfZField (2)" );

      if( npts[2]>1 && npts[1]>1 ) status( test_stencil2<Interpolant, ZVolField, YSurfZField>(npts, opdb, bc), "Interpolant ZVolField -> YSurfZField (2)" );
      if( npts[2]>1 && npts[1]>1 ) status( test_stencil2<Gradient,    ZVolField, YSurfZField>(npts, opdb, bc), "Gradient    ZVolField -> YSurfZField (2)" );

      if( npts[0]>1 ) status( test_stencil2<Interpolant, SVolField, XVolField>(npts, opdb, bc), "Interpolant SVolField -> XVolField (2)" );
      if( npts[0]>1 ) status( test_stencil2<Gradient,    SVolField, XVolField>(npts, opdb, bc), "Gradient    SVolField -> XVolField (2)" );

      if( npts[1]>1 ) status( test_stencil2<Interpolant, SVolField, YVolField>(npts, opdb, bc), "Interpolant SVolField -> YVolField (2)" );
      if( npts[1]>1 ) status( test_stencil2<Gradient,    SVolField, YVolField>(npts, opdb, bc), "Gradient    SVolField -> YVolField (2)" );

      if( npts[2]>1 ) status( test_stencil2<Interpolant, SVolField, ZVolField>(npts, opdb, bc), "Interpolant SVolField -> ZVolField (2)" );
      if( npts[2]>1 ) status( test_stencil2<Gradient,    SVolField, ZVolField>(npts, opdb, bc), "Gradient    SVolField -> ZVolField (2)" );

      if( npts[0]>1 ) status( test_stencil2<Interpolant, XVolField, SVolField>(npts, opdb, bc), "Interpolant XVolField -> SVolField (2)" );
      if( npts[0]>1 ) status( test_stencil2<Gradient,    XVolField, SVolField>(npts, opdb, bc), "Gradient    XVolField -> SVolField (2)" );

      if( npts[1]>1 ) status( test_stencil2<Interpolant, YVolField, SVolField>(npts, opdb, bc), "Interpolant YVolField -> SVolField (2)" );
      if( npts[1]>1 ) status( test_stencil2<Gradient,    YVolField, SVolField>(npts, opdb, bc), "Gradient    YVolField -> SVolField (2)" );

      if( npts[2]>1 ) status( test_stencil2<Interpolant, ZVolField, SVolField>(npts, opdb, bc), "Interpolant ZVolField -> SVolField (2)" );
      if( npts[2]>1 ) status( test_stencil2<Gradient,    ZVolField, SVolField>(npts, opdb, bc), "Gradient    ZVolField -> SVolField (2)" );

      if( npts[0]>1 ) status( test_stencil2<Interpolant, XSurfXField, XVolField>(npts, opdb, bc), "Interpolant XSurfXField -> XVolField (2)" );
      if( npts[1]>1 ) status( test_stencil2<Interpolant, XSurfYField, XVolField>(npts, opdb, bc), "Interpolant XSurfYField -> XVolField (2)" );
      if( npts[2]>1 ) status( test_stencil2<Interpolant, XSurfZField, XVolField>(npts, opdb, bc), "Interpolant XSurfZField -> XVolField (2)" );

      if( npts[0]>1 ) status( test_stencil2<Interpolant, YSurfXField, YVolField>(npts, opdb, bc), "Interpolant YSurfXField -> YVolField (2)" );
      if( npts[1]>1 ) status( test_stencil2<Interpolant, YSurfYField, YVolField>(npts, opdb, bc), "Interpolant YSurfYField -> YVolField (2)" );
      if( npts[2]>1 ) status( test_stencil2<Interpolant, YSurfZField, YVolField>(npts, opdb, bc), "Interpolant YSurfZField -> YVolField (2)" );

      if( npts[0]>1 ) status( test_stencil2<Interpolant, ZSurfXField, ZVolField>(npts, opdb, bc), "Interpolant ZSurfXField -> ZVolField (2)" );
      if( npts[1]>1 ) status( test_stencil2<Interpolant, ZSurfYField, ZVolField>(npts, opdb, bc), "Interpolant ZSurfYField -> ZVolField (2)" );
      if( npts[2]>1 ) status( test_stencil2<Interpolant, ZSurfZField, ZVolField>(npts, opdb, bc), "Interpolant ZSurfZField -> ZVolField (2)" );

      //Stencil4 tests:

      if( npts[0]>1 && npts[1]>1 ) status( test_stencil4<Interpolant, SVolField, XSurfYField>(npts, opdb, bc), "Interpolant SVolField -> XSurfYField (4)" );
      if( npts[0]>1 && npts[2]>1 ) status( test_stencil4<Interpolant, SVolField, XSurfZField>(npts, opdb, bc), "Interpolant SVolField -> XSurfZField (4)" );

      if( npts[1]>1 && npts[0]>1 ) status( test_stencil4<Interpolant, SVolField, YSurfXField>(npts, opdb, bc), "Interpolant SVolField -> YSurfXField (4)" );
      if( npts[1]>1 && npts[2]>1 ) status( test_stencil4<Interpolant, SVolField, YSurfZField>(npts, opdb, bc), "Interpolant SVolField -> YSurfZField (4)" );

      if( npts[2]>1 && npts[0]>1 ) status( test_stencil4<Interpolant, SVolField, ZSurfXField>(npts, opdb, bc), "Interpolant SVolField -> ZSurfXField (4)" );
      if( npts[2]>1 && npts[1]>1 ) status( test_stencil4<Interpolant, SVolField, ZSurfYField>(npts, opdb, bc), "Interpolant SVolField -> ZSurfYField (4)" );

      if( npts[0]>1 && npts[1]>1 ) status( test_stencil4<Interpolant, XSurfYField, SVolField>(npts, opdb, bc), "Interpolant XSurfYField -> SVolField (4)" );
      if( npts[0]>1 && npts[2]>1 ) status( test_stencil4<Interpolant, XSurfZField, SVolField>(npts, opdb, bc), "Interpolant XSurfZField -> SVolField (4)" );

      if( npts[1]>1 && npts[0]>1 ) status( test_stencil4<Interpolant, YSurfXField, SVolField>(npts, opdb, bc), "Interpolant YSurfXField -> SVolField (4)" );
      if( npts[1]>1 && npts[2]>1 ) status( test_stencil4<Interpolant, YSurfZField, SVolField>(npts, opdb, bc), "Interpolant YSurfZField -> SVolField (4)" );

      if( npts[2]>1 && npts[0]>1 ) status( test_stencil4<Interpolant, ZSurfXField, SVolField>(npts, opdb, bc), "Interpolant ZSurfXField -> SVolField (4)" );
      if( npts[2]>1 && npts[1]>1 ) status( test_stencil4<Interpolant, ZSurfYField, SVolField>(npts, opdb, bc), "Interpolant ZSurfYField -> SVolField (4)" );

      if( npts[0]>1 && npts[1]>1 ) status( test_stencil4<Interpolant, XVolField, YVolField>(npts, opdb, bc), "Interpolant XVolField -> YVolField (4)" );
      if( npts[0]>1 && npts[2]>1 ) status( test_stencil4<Interpolant, XVolField, ZVolField>(npts, opdb, bc), "Interpolant XVolField -> ZVolField (4)" );

      if( npts[1]>1 && npts[0]>1 ) status( test_stencil4<Interpolant, YVolField, XVolField>(npts, opdb, bc), "Interpolant YVolField -> XVolField (4)" );
      if( npts[1]>1 && npts[2]>1 ) status( test_stencil4<Interpolant, YVolField, ZVolField>(npts, opdb, bc), "Interpolant YVolField -> ZVolField (4)" );

      if( npts[2]>1 && npts[0]>1 ) status( test_stencil4<Interpolant, ZVolField, XVolField>(npts, opdb, bc), "Interpolant ZVolField -> XVolField (4)" );
      if( npts[2]>1 && npts[1]>1 ) status( test_stencil4<Interpolant, ZVolField, YVolField>(npts, opdb, bc), "Interpolant ZVolField -> YVolField (4)" );

      //Finite Difference (FDStencil2) tests:

      if( npts[0]>1 ) status( test_fd_stencil2<InterpolantX, SVolField>(npts, opdb, bc), "InterpolantX SVolField -> SVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<InterpolantY, SVolField>(npts, opdb, bc), "InterpolantY SVolField -> SVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<InterpolantZ, SVolField>(npts, opdb, bc), "InterpolantZ SVolField -> SVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<GradientX, SVolField>(npts, opdb, bc), "GradientX    SVolField -> SVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<GradientY, SVolField>(npts, opdb, bc), "GradientY    SVolField -> SVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<GradientZ, SVolField>(npts, opdb, bc), "GradientZ    SVolField -> SVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<InterpolantX, XVolField>(npts, opdb, bc), "InterpolantX XVolField -> XVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<InterpolantY, XVolField>(npts, opdb, bc), "InterpolantY XVolField -> XVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<InterpolantZ, XVolField>(npts, opdb, bc), "InterpolantZ XVolField -> XVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<GradientX, XVolField>(npts, opdb, bc), "GradientX    XVolField -> XVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<GradientY, XVolField>(npts, opdb, bc), "GradientY    XVolField -> XVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<GradientZ, XVolField>(npts, opdb, bc), "GradientZ    XVolField -> XVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<InterpolantX, YVolField>(npts, opdb, bc), "InterpolantX YVolField -> YVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<InterpolantY, YVolField>(npts, opdb, bc), "InterpolantY YVolField -> YVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<InterpolantZ, YVolField>(npts, opdb, bc), "InterpolantZ YVolField -> YVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<GradientX, YVolField>(npts, opdb, bc), "GradientX    YVolField -> YVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<GradientY, YVolField>(npts, opdb, bc), "GradientY    YVolField -> YVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<GradientZ, YVolField>(npts, opdb, bc), "GradientZ    YVolField -> YVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<InterpolantX, ZVolField>(npts, opdb, bc), "InterpolantX ZVolField -> ZVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<InterpolantY, ZVolField>(npts, opdb, bc), "InterpolantY ZVolField -> ZVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<InterpolantZ, ZVolField>(npts, opdb, bc), "InterpolantZ ZVolField -> ZVolField (FD 2)" );

      if( npts[0]>1 ) status( test_fd_stencil2<GradientX, ZVolField>(npts, opdb, bc), "GradientX    ZVolField -> ZVolField (FD 2)" );
      if( npts[1]>1 ) status( test_fd_stencil2<GradientY, ZVolField>(npts, opdb, bc), "GradientY    ZVolField -> ZVolField (FD 2)" );
      if( npts[2]>1 ) status( test_fd_stencil2<GradientZ, ZVolField>(npts, opdb, bc), "GradientZ    ZVolField -> ZVolField (FD 2)" );

      //NullStencil tests:
      status( test_null_stencil<Interpolant, SVolField, SVolField>(npts, opdb, bc), "Interpolant SVolField -> SVolField (Null)" );
      status( test_null_stencil<Interpolant, XVolField, XVolField>(npts, opdb, bc), "Interpolant XVolField -> XVolField (Null)" );
      status( test_null_stencil<Interpolant, YVolField, YVolField>(npts, opdb, bc), "Interpolant YVolField -> YVolField (Null)" );
      status( test_null_stencil<Interpolant, ZVolField, ZVolField>(npts, opdb, bc), "Interpolant ZVolField -> ZVolField (Null)" );

      status( test_null_stencil<Interpolant, XVolField, SSurfXField>(npts, opdb, bc), "Interpolant XVolField -> SSurfXField (Null)" );
      status( test_null_stencil<Interpolant, YVolField, SSurfYField>(npts, opdb, bc), "Interpolant YVolField -> SSurfYField (Null)" );
      status( test_null_stencil<Interpolant, ZVolField, SSurfZField>(npts, opdb, bc), "Interpolant ZVolField -> SSurfZField (Null)" );

      status( test_null_stencil<Interpolant, SVolField, XSurfXField>(npts, opdb, bc), "Interpolant SVolField -> XSurfXField (Null)" );
      status( test_null_stencil<Interpolant, SVolField, YSurfYField>(npts, opdb, bc), "Interpolant SVolField -> YSurfYField (Null)" );
      status( test_null_stencil<Interpolant, SVolField, ZSurfZField>(npts, opdb, bc), "Interpolant SVolField -> ZSurfZField (Null)" );

      //Box filter tests:
      if( npts[0]>1 && npts[1]>1 && npts[2] ) {
          status( test_box_filter_stencil<Filter, SVolField>(npts, opdb, bc), "Filter SVolField -> SVolField (box filter)" );
          status( test_box_filter_stencil<Filter, XVolField>(npts, opdb, bc), "Filter XVolField -> XVolField (box filter)" );
          status( test_box_filter_stencil<Filter, YVolField>(npts, opdb, bc), "Filter YVolField -> YVolField (box filter)" );
          status( test_box_filter_stencil<Filter, ZVolField>(npts, opdb, bc), "Filter ZVolField -> ZVolField (box filter)" );
      }

      if( status.ok() ){
          cout << "ALL TESTS PASSED :)" << endl;
          return 0;
      }
  }
  catch( std::exception& e ){
      cout << e.what() << endl;
  }

  cout << "******************************" << endl
       << " At least one test FAILED! :(" << endl
       << "******************************" << endl;
  return -1;
}

//--------------------------------------------------------------------


#define BOOST_TEST_MODULE "test_common_boundary"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <mill/common/BoundaryCondition.hpp>

BOOST_AUTO_TEST_CASE(unlimited)
{
    using namespace std::literals::string_literals;
    using vector_type             = mill::Vector<double, 3>;
    using boundary_condition_type = mill::BoundaryCondition;
    using unlimited_boundary_type = mill::UnlimitedBoundary;
    {
        boundary_condition_type b(unlimited_boundary_type{});

        BOOST_TEST(std::isinf(b.volume()));

        vector_type r(1.0, 2.0, 3.0);
        BOOST_TEST(b.adjust_position(r)[0] == r[0]);
        BOOST_TEST(b.adjust_position(r)[1] == r[1]);
        BOOST_TEST(b.adjust_position(r)[2] == r[2]);

        vector_type dr(1.0, 2.0, 3.0);
        BOOST_TEST(b.adjust_direction(dr)[0] == dr[0]);
        BOOST_TEST(b.adjust_direction(dr)[1] == dr[1]);
        BOOST_TEST(b.adjust_direction(dr)[2] == dr[2]);
    }
}

BOOST_AUTO_TEST_CASE(periodic)
{
    using namespace std::literals::string_literals;
    using vector_type             = mill::Vector<double, 3>;
    using boundary_condition_type = mill::BoundaryCondition;
    using periodic_boundary_type  = mill::CuboidalPeriodicBoundary;
    {
        vector_type upper( 2.0,  2.0,  2.0);
        vector_type lower(-1.0, -1.0, -1.0);

        boundary_condition_type b(periodic_boundary_type(lower, upper));

        BOOST_TEST(b.volume() == 27.0);

        vector_type r(1.0, 2.0, 3.0);
        BOOST_TEST(b.adjust_position(r)[0] ==  1.0);
        BOOST_TEST(b.adjust_position(r)[1] == -1.0);
        BOOST_TEST(b.adjust_position(r)[2] ==  0.0);

        vector_type dr(1.0, 2.0, 3.0);
        BOOST_TEST(b.adjust_direction(dr)[0] ==  1.0);
        BOOST_TEST(b.adjust_direction(dr)[1] == -1.0);
        BOOST_TEST(b.adjust_direction(dr)[2] ==  0.0);
    }
}