#pragma once
#include <cmath>
#include"cnpy.h"

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

namespace boost {
namespace numeric {
namespace odeint {
    namespace detail{
        template< typename Value, class Iterator1 >
        inline Value norm_l2(Iterator1 first1, Iterator1 last1, Value init, size_t n)
        {
            using std::max;
            using std::abs;
            for (; first1 != last1; first1++)
                init += std::pow(*first1, 2);
            return std::sqrt(init / n);
        }
    }
    template< typename S >
    static typename norm_result_type<S>::type norm_l2(const S& s)
    {
        size_t n = boost::size(s);
        return detail::norm_l2(boost::begin(s), boost::end(s),
            static_cast<typename norm_result_type<S>::type>(0), n);
    }

    template< class Fac1 = double >
    struct rel_error
    {
        const Fac1 m_eps_abs, m_eps_rel, m_a_x;

        rel_error(Fac1 eps_abs, Fac1 eps_rel, Fac1 a_x)
            : m_eps_abs(eps_abs), m_eps_rel(eps_rel), m_a_x(a_x) { }

        template< class T1, class T2, class T3 >
        void operator()(T3& t3, const T1& t1, const T2& t2) const
        {
            using std::abs;
            set_unit_value(t3, abs(get_unit_value(t3)) / (m_eps_abs + m_eps_rel * (m_a_x * std::max(abs(get_unit_value(t1)), abs(get_unit_value(t2))) )));
        }

        typedef void result_type;
    };

    double scale_norm(const std::vector<double>& y, const std::vector<double>& scale) {
        size_t n = boost::size(y);
        double _norm = 0.0;
        for (int i = 0; i < n; i++) {
            _norm += std::pow(y[i] / scale[i], 2.0);
        }
        _norm = sqrt(_norm / n);
        return _norm;
    }
    /*
    template<class value_type, class State>
    value_type scale_norm(State& y, State& scale) {
        value_type _norm = 0.0;
        detail::for_each2(boost::begin(y), boost::end(y),
            boost::begin(scale), [](auto& y_i, auto& scale_i) { _norm += y_i / scale_i });
    }
    */
    template< class Fac1 = double >
    struct custom_scale_sum
    {
        const Fac1 m_alpha1, m_alpha2;

        custom_scale_sum(Fac1 alpha1, Fac1 alpha2) : m_alpha1(alpha1), m_alpha2(alpha2) { }

        template< class T1, class T2 >
        void operator()(T1& t1, const T2& t2) const
        {
            t1 = m_alpha1 * std::abs(t2) + m_alpha2;
        }

        typedef void result_type;
    };

    template<class value_type, class System, class State>
    value_type select_initial_step(System fun, value_type t0, State& y0, size_t order, value_type rtol, value_type atol) {
        // create local State to hold values
        state_wrapper< State > f0, f1, scale, tmp_err;
        range_algebra algebra;
        // resize it
        adjust_size_by_resizeability(scale, y0, typename is_resizeable<State>::type());
        adjust_size_by_resizeability(tmp_err, y0, typename is_resizeable<State>::type());
        adjust_size_by_resizeability(f0, y0, typename is_resizeable<State>::type());
        adjust_size_by_resizeability(f1, y0, typename is_resizeable<State>::type());
        // compute f0
        fun(y0, f0.m_v, t0);
        // using for_each to compute the scale vector
        algebra.for_each2(scale.m_v, y0,
            custom_scale_sum< value_type >(rtol, atol));
        // compute d0, d1
        value_type d0 = scale_norm(y0, scale.m_v);
        value_type d1 = scale_norm(f0.m_v, scale.m_v);
        // h0
        value_type h0 = 0.01 * d0 / d1;
        if (d0 < 1e-5 || d1 < 1e-5) {
            h0 = 1e-6;
        }
        algebra.for_each3(tmp_err.m_v, y0, f0.m_v,
            default_operations::scale_sum2< value_type >(1.0, h0));
        // tmp_err.m_v becomes y1 now
        State& y1 = tmp_err.m_v;
        // compute f1
        fun(y1, f1.m_v, t0 + h0);
        algebra.for_each3(tmp_err.m_v, f1.m_v, f0.m_v,
            default_operations::scale_sum2< value_type >(1.0, -1.0));
        value_type d2 = scale_norm(tmp_err.m_v, scale.m_v) / h0;
        value_type h1;
        if (d1 <= 1e-15 || d2 <= 1e-15) {
            h1 = std::max(1e-6, h0 * 1e-3);
        }
        else {
            h1 = std::pow(0.01 / std::max(d1, d2), 1.0 / (order + 1));
        }
        return std::min(100 * h0, h1);
    }

template
    <
    class Value,
    class Algebra,
    class Operations
    >
    class custom_error_checker
{
public:

    typedef Value value_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;

    custom_error_checker(
        value_type eps_abs = static_cast<value_type>(1.0e-6),
        value_type eps_rel = static_cast<value_type>(1.0e-6),
        value_type a_x = static_cast<value_type>(1),
        value_type a_dxdt = static_cast<value_type>(1))
        : m_eps_abs(eps_abs), m_eps_rel(eps_rel), m_a_x(a_x), m_a_dxdt(a_dxdt)
    { }

    template< class State, class Deriv, class Err, class Time >
    value_type error(const State& x_old, const Deriv& dxdt_old, Err& x_err, Time dt) const
    {
        return error(algebra_type(), x_old, dxdt_old, x_err, dt);
    }

    template< class State, class Deriv, class Err, class Time >
    value_type error(algebra_type& algebra, const State& x_old, const Deriv& dxdt_old, Err& x_err, Time dt) const
    {
        using std::abs;
        // this overwrites x_err !
        algebra.for_each3(x_err, x_old, dxdt_old,
            rel_error< value_type >(m_eps_abs, m_eps_rel, m_a_x));

        // value_type res = algebra.reduce( x_err ,
        //        typename operations_type::template maximum< value_type >() , static_cast< value_type >( 0 ) );
        return norm_l2(x_err);
    }
    double eps_abs() {
        return m_eps_abs;
    }
private:

    value_type m_eps_abs;
    value_type m_eps_rel;
    value_type m_a_x;
    value_type m_a_dxdt;

};

// standard IController
template< typename Value, typename Time >
class custom_step_adjuster
{
public:
    typedef Time time_type;
    typedef Value value_type;

    custom_step_adjuster(const time_type max_dt=static_cast<time_type>(0))
            : m_max_dt(max_dt)
    {}

    time_type decrease_step(time_type dt, const value_type error, const int stepper_order) const
    {
        // returns the decreased time step
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        dt *= max
        BOOST_PREVENT_MACRO_SUBSTITUTION(
                static_cast<value_type>( static_cast<value_type>(9) / static_cast<value_type>(10) *
                                         pow(error, static_cast<value_type>(-1) / (stepper_order))),
                static_cast<value_type>( static_cast<value_type>(1) / static_cast<value_type> (5)));
        if(m_max_dt != static_cast<time_type >(0))
            // limit to maximal stepsize even when decreasing
            dt = detail::min_abs(dt, m_max_dt);
        return dt;
    }

    time_type adjust_step(time_type dt, value_type error, const int stepper_order) const
    {
        // returns the increased time step
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;
        time_type factor;
        factor = max
            BOOST_PREVENT_MACRO_SUBSTITUTION(
                static_cast<value_type>(static_cast<value_type>(9) / static_cast<value_type>(10) *
                    pow(error, static_cast<value_type>(-1) / (stepper_order))),
                static_cast<value_type>(static_cast<value_type>(1) / static_cast<value_type> (5)));
        factor = std::min(factor, 10.0);
        dt *= factor;
        if (m_max_dt != static_cast<time_type>(0))
            // limit to maximal stepsize even when decreasing
            dt = detail::min_abs(dt, m_max_dt);
        return dt;
    }

    bool check_step_size_limit(const time_type dt)
    {
        if(m_max_dt != static_cast<time_type >(0))
            return detail::less_eq_with_sign(dt, m_max_dt, dt);
        return true;
    }

    time_type get_max_dt() { return m_max_dt; }

protected:
    time_type m_max_dt;
};

// only support FSAL
template<
    class ErrorStepper,
    class ErrorChecker = default_error_checker< typename ErrorStepper::value_type,
    typename ErrorStepper::algebra_type,
    typename ErrorStepper::operations_type >,
    class StepAdjuster = default_step_adjuster< typename ErrorStepper::value_type,
    typename ErrorStepper::time_type >,
    class Resizer = typename ErrorStepper::resizer_type,
    class ErrorStepperCategory = typename ErrorStepper::stepper_category
>
class custom_controlled_runge_kutta;

template<
    class ErrorStepper,
    class ErrorChecker,
    class StepAdjuster,
    class Resizer
>
class custom_controlled_runge_kutta< ErrorStepper, ErrorChecker, StepAdjuster, Resizer, explicit_error_stepper_fsal_tag >
{

public:

    typedef ErrorStepper stepper_type;
    typedef typename stepper_type::state_type state_type;
    typedef typename stepper_type::value_type value_type;
    typedef typename stepper_type::deriv_type deriv_type;
    typedef typename stepper_type::time_type time_type;
    typedef typename stepper_type::algebra_type algebra_type;
    typedef typename stepper_type::operations_type operations_type;
    typedef Resizer resizer_type;
    typedef ErrorChecker error_checker_type;
    typedef StepAdjuster step_adjuster_type;
    typedef explicit_controlled_stepper_fsal_tag stepper_category;

#ifndef DOXYGEN_SKIP
    typedef typename stepper_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;

    typedef custom_controlled_runge_kutta< ErrorStepper, ErrorChecker, StepAdjuster, Resizer, explicit_error_stepper_tag > controlled_stepper_type;
#endif // DOXYGEN_SKIP

    /**
     * \brief Constructs the controlled Runge-Kutta stepper.
     * \param error_checker An instance of the error checker.
     * \param stepper An instance of the underlying stepper.
     */
    custom_controlled_runge_kutta(
        const error_checker_type& error_checker = error_checker_type(),
        const step_adjuster_type& step_adjuster = step_adjuster_type(),
        const stepper_type& stepper = stepper_type(),
        std::string model_file_name = "",
        bool is_fixed_stepsize = false
    )
        : m_stepper(stepper), m_error_checker(error_checker), m_step_adjuster(step_adjuster),
        m_first_call(true), fixed_stepsize(is_fixed_stepsize)
    {
        m_use_nn = !(model_file_name == "");
        if (m_use_nn) {
            // construct W1, b1, W2, b2
            cnpy::npz_t _npz = cnpy::npz_load(model_file_name);
            size_t w1_size = _npz["W1"].shape[0];
            hidden_num = _npz["b1"].shape[0];
            double* _W1 = _npz["W1"].data<double>();
            double* _W2 = _npz["W2"].data<double>();
            double* _b1 = _npz["b1"].data<double>();
            b2 = *_npz["b2"].data<double>();
            if (_npz.count("k") > 0) {
                k = *_npz["k"].data<double>();
            }
            else {
                k = -1.0;
            }

            std::copy(_W1, _W1 + w1_size, back_inserter(W1));
            std::copy(_W2, _W2 + hidden_num, back_inserter(W2));
            std::copy(_b1, _b1 + hidden_num, back_inserter(b1));

            tmp_vec.resize(hidden_num);
        }
    }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The two overloads are needed in order to solve the forwarding problem
     */
     /**
      * \brief Tries to perform one step.
      *
      * This method tries to do one step with step size dt. If the error estimate
      * is to large, the step is rejected and the method returns fail and the
      * step size dt is reduced. If the error estimate is acceptably small, the
      * step is performed, success is returned and dt might be increased to make
      * the steps as large as possible. This method also updates t if a step is
      * performed.
      *
      * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
      *               Simple System concept.
      * \param x The state of the ODE which should be solved. Overwritten if
      * the step is successful.
      * \param t The value of the time. Updated if the step is successful.
      * \param dt The step size. Updated.
      * \return success if the step was accepted, fail otherwise.
      */
    template< class System, class StateInOut >
    controlled_step_result try_step(System system, StateInOut& x, time_type& t, time_type& dt)
    {
        return try_step_v1(system, x, t, dt);
    }

    /**
     * \brief Tries to perform one step. Solves the forwarding problem and
     * allows for using boost range as state_type.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful. Can be a boost range.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */
    template< class System, class StateInOut >
    controlled_step_result try_step(System system, const StateInOut& x, time_type& t, time_type& dt)
    {
        return try_step_v1(system, x, t, dt);
    }

    /*
     * Version 2 : try_step( sys , in , t , out , dt );
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     *
     * The disabler is needed to solve ambiguous overloads
     */
     /**
      * \brief Tries to perform one step.
      *
      * \note This method is disabled if state_type=time_type to avoid ambiguity.
      *
      * This method tries to do one step with step size dt. If the error estimate
      * is to large, the step is rejected and the method returns fail and the
      * step size dt is reduced. If the error estimate is acceptably small, the
      * step is performed, success is returned and dt might be increased to make
      * the steps as large as possible. This method also updates t if a step is
      * performed.
      *
      * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
      *               Simple System concept.
      * \param in The state of the ODE which should be solved.
      * \param t The value of the time. Updated if the step is successful.
      * \param out Used to store the result of the step.
      * \param dt The step size. Updated.
      * \return success if the step was accepted, fail otherwise.
      */
    template< class System, class StateIn, class StateOut >
    typename boost::disable_if< boost::is_same< StateIn, time_type >, controlled_step_result >::type
        try_step(System system, const StateIn& in, time_type& t, StateOut& out, time_type& dt)
    {
        if (m_dxdt_resizer.adjust_size(in, detail::bind(&custom_controlled_runge_kutta::template resize_m_dxdt_impl< StateIn >, detail::ref(*this), detail::_1)) || m_first_call)
        {
            initialize(system, in, t);
        }
        return try_step(system, in, m_dxdt.m_v, t, out, dt);
    }

    /*
     * Version 3 : try_step( sys , x , dxdt , t , dt )
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     */
     /**
      * \brief Tries to perform one step.
      *
      * This method tries to do one step with step size dt. If the error estimate
      * is to large, the step is rejected and the method returns fail and the
      * step size dt is reduced. If the error estimate is acceptably small, the
      * step is performed, success is returned and dt might be increased to make
      * the steps as large as possible. This method also updates t if a step is
      * performed.
      *
      * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
      *               Simple System concept.
      * \param x The state of the ODE which should be solved. Overwritten if
      * the step is successful.
      * \param dxdt The derivative of state.
      * \param t The value of the time. Updated if the step is successful.
      * \param dt The step size. Updated.
      * \return success if the step was accepted, fail otherwise.
      */
    template< class System, class StateInOut, class DerivInOut >
    controlled_step_result try_step(System system, StateInOut& x, DerivInOut& dxdt, time_type& t, time_type& dt)
    {
        m_xnew_resizer.adjust_size(x, detail::bind(&custom_controlled_runge_kutta::template resize_m_xnew_impl< StateInOut >, detail::ref(*this), detail::_1));
        m_dxdt_new_resizer.adjust_size(x, detail::bind(&custom_controlled_runge_kutta::template resize_m_dxdt_new_impl< StateInOut >, detail::ref(*this), detail::_1));
        controlled_step_result res = try_step(system, x, dxdt, t, m_xnew.m_v, m_dxdtnew.m_v, dt);
        if (res == success)
        {
            boost::numeric::odeint::copy(m_xnew.m_v, x);
            boost::numeric::odeint::copy(m_dxdtnew.m_v, dxdt);
        }
        return res;
    }

    /*
     * Version 4 : try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
     *
     * This version does not solve the forwarding problem, boost::range can not be used.
     */
     /**
      * \brief Tries to perform one step.
      *
      * This method tries to do one step with step size dt. If the error estimate
      * is to large, the step is rejected and the method returns fail and the
      * step size dt is reduced. If the error estimate is acceptably small, the
      * step is performed, success is returned and dt might be increased to make
      * the steps as large as possible. This method also updates t if a step is
      * performed.
      *
      * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
      *               Simple System concept.
      * \param in The state of the ODE which should be solved.
      * \param dxdt The derivative of state.
      * \param t The value of the time. Updated if the step is successful.
      * \param out Used to store the result of the step.
      * \param dt The step size. Updated.
      * \return success if the step was accepted, fail otherwise.
      */
    template< class System, class StateIn, class DerivIn, class StateOut, class DerivOut >
    controlled_step_result try_step(System system, const StateIn& in, const DerivIn& dxdt_in, time_type& t,
        StateOut& out, DerivOut& dxdt_out, time_type& dt)
    {
        unwrapped_step_adjuster& step_adjuster = m_step_adjuster;
        if (!step_adjuster.check_step_size_limit(dt))
        {
            // given dt was above step size limit - adjust and return fail;
            dt = step_adjuster.get_max_dt();
            return fail;
        }
        value_type max_rel_err;
        time_type dt_tmp;
        if (!m_use_nn && !fixed_stepsize) {
            m_xerr_resizer.adjust_size(in, detail::bind(&custom_controlled_runge_kutta::template resize_m_xerr_impl< StateIn >, detail::ref(*this), detail::_1));

            //fsal: m_stepper.get_dxdt( dxdt );
            //fsal: m_stepper.do_step( sys , x , dxdt , t , dt , m_x_err );
            m_stepper.do_step(system, in, dxdt_in, t, out, dxdt_out, dt, m_xerr.m_v);

            // this potentially overwrites m_x_err! (standard_error_checker does, at least)
            max_rel_err = m_error_checker.error(m_stepper.algebra(), in, out, m_xerr.m_v, dt);

            if (max_rel_err > 1.0)
            {
                // error too big, decrease step size and reject this step
                dt = step_adjuster.adjust_step(dt, max_rel_err, m_stepper.stepper_order());
                return fail;
            }
        }
        else if(!fixed_stepsize) {

            input_vec[0] = t;
            for (int i = 1; i <= in.size(); i++)
                input_vec[i] = in[i - 1];

            dt_tmp = 0;
            // computation
            for (int i = 0; i < hidden_num; i++) {
                for (int j = 0; j < m; j++) {
                    tmp_vec[i] += W1[i * m + j] * input_vec[j];
                }
                tmp_vec[i] += b1[i];
                tmp_vec[i] = std::max(0.0, tmp_vec[i]);
            }
            for (int i = 0; i < hidden_num; i++) {
                dt_tmp += tmp_vec[i] * W2[i];
                tmp_vec[i] = 0;
            }
            dt_tmp += b2;
            if (k > 0) {
                dt_tmp += k * std::log(m_error_checker.eps_abs());
            }
            dt = std::exp(dt_tmp);
            m_stepper.do_step(system, in, dxdt_in, t, out, dxdt_out, dt);

        }
        else {
            m_stepper.do_step(system, in, dxdt_in, t, out, dxdt_out, dt);
        }
        // otherwise, increase step size and accept
        t += dt;
        if (!m_use_nn && !fixed_stepsize) {
            dt = step_adjuster.adjust_step(dt, max_rel_err, m_stepper.stepper_order());
        }
        return success;
    }

    /**
     * \brief Resets the internal state of the underlying FSAL stepper.
     */
    void reset(void)
    {
        m_first_call = true;
    }

    /**
     * \brief Initializes the internal state storing an internal copy of the derivative.
     *
     * \param deriv The initial derivative of the ODE.
     */
    template< class DerivIn >
    void initialize(const DerivIn& deriv)
    {
        boost::numeric::odeint::copy(deriv, m_dxdt.m_v);
        m_first_call = false;
    }

    /**
     * \brief Initializes the internal state storing an internal copy of the derivative.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param x The initial state of the ODE which should be solved.
     * \param t The initial time.
     */
    template< class System, class StateIn >
    void initialize(System system, const StateIn& x, time_type t)
    {
        typename odeint::unwrap_reference< System >::type& sys = system;
        sys(x, m_dxdt.m_v, t);
        size_t n = boost::size(x);
        if (k < 0) {
            m = n + 2;
        }
        else {
            m = n + 1;
        }
        input_vec.resize(m);
        if (k < 0) {
            input_vec[m - 1] = std::log(m_error_checker.eps_abs());
        }
        m_first_call = false;
    }

    /**
     * \brief Returns true if the stepper has been initialized, false otherwise.
     *
     * \return true, if the stepper has been initialized, false otherwise.
     */
    bool is_initialized(void) const
    {
        return !m_first_call;
    }

    /**
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */
    template< class StateType >
    void adjust_size(const StateType& x)
    {
        resize_m_xerr_impl(x);
        resize_m_dxdt_impl(x);
        resize_m_dxdt_new_impl(x);
        resize_m_xnew_impl(x);
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    stepper_type& stepper(void)
    {
        return m_stepper;
    }

    /**
     * \brief Returns the instance of the underlying stepper.
     * \returns The instance of the underlying stepper.
     */
    const stepper_type& stepper(void) const
    {
        return m_stepper;
    }

private:

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

    template< class StateIn >
    bool resize_m_dxdt_impl(const StateIn& x)
    {
        return adjust_size_by_resizeability(m_dxdt, x, typename is_resizeable<deriv_type>::type());
    }

    template< class StateIn >
    bool resize_m_dxdt_new_impl(const StateIn& x)
    {
        return adjust_size_by_resizeability(m_dxdtnew, x, typename is_resizeable<deriv_type>::type());
    }

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

    template< class System, class StateInOut >
    controlled_step_result try_step_v1(System system, StateInOut& x, time_type& t, time_type& dt)
    {
        if (m_dxdt_resizer.adjust_size(x, detail::bind(&custom_controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut >, detail::ref(*this), detail::_1)) || m_first_call)
        {
            initialize(system, x, t);
        }
        return try_step(system, x, m_dxdt.m_v, t, dt);
    }

    stepper_type m_stepper;
    error_checker_type m_error_checker;
    step_adjuster_type m_step_adjuster;
    typedef typename unwrap_reference< step_adjuster_type >::type unwrapped_step_adjuster;

    resizer_type m_dxdt_resizer;
    resizer_type m_xerr_resizer;
    resizer_type m_xnew_resizer;
    resizer_type m_dxdt_new_resizer;

    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_xerr;
    wrapped_state_type m_xnew;
    wrapped_deriv_type m_dxdtnew;
    bool m_first_call;
    bool m_use_nn;
    bool fixed_stepsize;
    size_t hidden_num;
    size_t m;
    std::vector<double> W1;
    std::vector<double> W2;
    std::vector<double> b1;
    double b2;
    double k;
    std::vector<double> input_vec;
    std::vector<double> tmp_vec;
};

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


//
// 	Copyright (c) 2018  Cem Bassoy, cem.bassoy@gmail.com
// 	Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>

BOOST_AUTO_TEST_SUITE ( test_extents_dynamic )

struct fixture
{
  using extents = boost::numeric::ublas::extents<>;

//  static inline auto n     = extents{};
  static inline auto n1    = extents{1};
  static inline auto n2    = extents{2};
  static inline auto n11   = extents{1,1};
  static inline auto n12   = extents{1,2};
  static inline auto n21   = extents{2,1};
  static inline auto n22   = extents{2,2};
  static inline auto n32   = extents{3,2};
  static inline auto n111  = extents{1,1,1};
  static inline auto n211  = extents{2,1,1};
  static inline auto n121  = extents{1,2,1};
  static inline auto n112  = extents{1,1,2};
  static inline auto n123  = extents{1,2,3};
  static inline auto n321  = extents{3,2,1};
  static inline auto n213  = extents{2,1,3};
  static inline auto n432  = extents{4,3,2};
};

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_empty,
                        fixture,
                        *boost::unit_test::label("dynamic_extents") *boost::unit_test::label("empty"))
{
  namespace ublas = boost::numeric::ublas;

//  BOOST_CHECK( ublas::empty(n   ));
  BOOST_CHECK(!ublas::empty(n1  ));
  BOOST_CHECK(!ublas::empty(n2  ));
  BOOST_CHECK(!ublas::empty(n11 ));
  BOOST_CHECK(!ublas::empty(n12 ));
  BOOST_CHECK(!ublas::empty(n21 ));
  BOOST_CHECK(!ublas::empty(n22 ));
  BOOST_CHECK(!ublas::empty(n32 ));
  BOOST_CHECK(!ublas::empty(n111));
  BOOST_CHECK(!ublas::empty(n211));
  BOOST_CHECK(!ublas::empty(n121));
  BOOST_CHECK(!ublas::empty(n112));
  BOOST_CHECK(!ublas::empty(n123));
  BOOST_CHECK(!ublas::empty(n321));
  BOOST_CHECK(!ublas::empty(n213));
  BOOST_CHECK(!ublas::empty(n432));

  BOOST_CHECK_THROW( extents({1,1,0}), std::invalid_argument);
  BOOST_CHECK_THROW( extents({1,0})  , std::invalid_argument);
  BOOST_CHECK_THROW( extents({0}  )  , std::invalid_argument);
  BOOST_CHECK_THROW( extents({0,1})  , std::invalid_argument);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_size,
                        fixture,
                        *boost::unit_test::label("dynamic_extents") *boost::unit_test::label("size"))
{
  namespace ublas = boost::numeric::ublas;

//  BOOST_CHECK_EQUAL(ublas::size(n   ),0);
  BOOST_CHECK_EQUAL(ublas::size(n1  ),1);
  BOOST_CHECK_EQUAL(ublas::size(n2  ),1);
  BOOST_CHECK_EQUAL(ublas::size(n11 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n12 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n21 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n22 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n32 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n111),3);
  BOOST_CHECK_EQUAL(ublas::size(n211),3);
  BOOST_CHECK_EQUAL(ublas::size(n121),3);
  BOOST_CHECK_EQUAL(ublas::size(n112),3);
  BOOST_CHECK_EQUAL(ublas::size(n123),3);
  BOOST_CHECK_EQUAL(ublas::size(n321),3);
  BOOST_CHECK_EQUAL(ublas::size(n213),3);
  BOOST_CHECK_EQUAL(ublas::size(n432),3);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_at_read,
                       fixture,
                       *boost::unit_test::label("dynamic_extents") *boost::unit_test::label("at_read"))
{
  BOOST_CHECK_EQUAL(n1  .at(0),1);
  BOOST_CHECK_EQUAL(n2  .at(0),2);

  BOOST_CHECK_EQUAL(n11 .at(0),1);
  BOOST_CHECK_EQUAL(n11 .at(1),1);

  BOOST_CHECK_EQUAL(n12 .at(0),1);
  BOOST_CHECK_EQUAL(n12 .at(1),2);

  BOOST_CHECK_EQUAL(n21 .at(0),2);
  BOOST_CHECK_EQUAL(n21 .at(1),1);

  BOOST_CHECK_EQUAL(n22 .at(0),2);
  BOOST_CHECK_EQUAL(n22 .at(1),2);

  BOOST_CHECK_EQUAL(n32 .at(0),3);
  BOOST_CHECK_EQUAL(n32 .at(1),2);

  BOOST_CHECK_EQUAL(n432.at(0),4);
  BOOST_CHECK_EQUAL(n432.at(1),3);
  BOOST_CHECK_EQUAL(n432.at(2),2);

//  BOOST_CHECK_THROW( (void)n  .at(0), std::out_of_range);
  BOOST_CHECK_THROW( (void)n32.at(2), std::out_of_range);
  BOOST_CHECK_THROW( (void)n32.at(5), std::out_of_range);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_at_write,
                        fixture,
                        *boost::unit_test::label("dynamic_extents") *boost::unit_test::label("at_write"))
{
  auto n3 = extents{1};
  n3 = extents{3};
  BOOST_CHECK_EQUAL(n3.at(0),3);

  auto n34 = extents{1,1};
  n34 = extents{3,4};
  BOOST_CHECK_EQUAL(n34.at(0),3);
  BOOST_CHECK_EQUAL(n34.at(1),4);

  auto n345 = extents{1,1,1};
  n345 = extents{3,4,5};
  BOOST_CHECK_EQUAL(n345.at(0),3);
  BOOST_CHECK_EQUAL(n345.at(1),4);
  BOOST_CHECK_EQUAL(n345.at(2),5);

  auto n5432 = extents{1,1,1,1};
  n5432 = extents{5,4,3,2};
  BOOST_CHECK_EQUAL(n5432.at(0),5);
  BOOST_CHECK_EQUAL(n5432.at(1),4);
  BOOST_CHECK_EQUAL(n5432.at(2),3);
  BOOST_CHECK_EQUAL(n5432.at(3),2);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_operator_access_read,
                        fixture,
                        *boost::unit_test::label("dynamic_extents") *boost::unit_test::label("operator_access_read"))
{
  BOOST_CHECK_EQUAL(n1  [0],1);
  BOOST_CHECK_EQUAL(n2  [0],2);

  BOOST_CHECK_EQUAL(n11 [0],1);
  BOOST_CHECK_EQUAL(n11 [1],1);

  BOOST_CHECK_EQUAL(n12 [0],1);
  BOOST_CHECK_EQUAL(n12 [1],2);

  BOOST_CHECK_EQUAL(n21 [0],2);
  BOOST_CHECK_EQUAL(n21 [1],1);

  BOOST_CHECK_EQUAL(n22 [0],2);
  BOOST_CHECK_EQUAL(n22 [1],2);

  BOOST_CHECK_EQUAL(n32 [0],3);
  BOOST_CHECK_EQUAL(n32 [1],2);

  BOOST_CHECK_EQUAL(n432[0],4);
  BOOST_CHECK_EQUAL(n432[1],3);
  BOOST_CHECK_EQUAL(n432[2],2);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <sophus/se3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

using namespace Sophus;
// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
class LocalParameterizationSE3 : public ceres::LocalParameterization {
 public:
  virtual ~LocalParameterizationSE3() {}

  // SE3 plus operation for Ceres
  //
  //  T * exp(x)
  //
  virtual bool Plus(double const* T_raw, double const* delta_raw,
                    double* T_plus_delta_raw) const {
    Eigen::Map<SE3d const> const T(T_raw);
    Eigen::Map<Vector6d const> const delta(delta_raw);
    Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * SE3d::exp(delta);
    return true;
  }

  // Jacobian of SE3 plus operation for Ceres
  //
  // Dx T * exp(x)  with  x=0
  //
  virtual bool ComputeJacobian(double const* T_raw,
                               double* jacobian_raw) const {
    Eigen::Map<SE3d const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
        jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  virtual bool MultiplyByJacobian(const double* x, const int n_rows, const double* global_matrix, double *local_matrix) const
  {
    return ceres::LocalParameterization::MultiplyByJacobian(x, n_rows, global_matrix, local_matrix);
  }

  virtual int GlobalSize() const { return SE3d::num_parameters; }

  virtual int LocalSize() const { return SE3d::DoF; }
};

struct ProjectionError
{
  ProjectionError(const Eigen::Vector2d& measurement, const Eigen::Vector3d& point,
                  const Eigen::Matrix3d& K) : _x(measurement), _X(point), _K(K)
    {}

  template <class T>
  bool operator() (const T* const params, T* residuals) const
  {
    const Eigen::Map<const Sophus::SE3<T>> Rt(params);
    Eigen::Matrix<T, 3, 1> X(T(_X.x()), T(_X.y()), T(_X.z()));
    Eigen::Matrix<T, 3, 1> uv = _K * (Rt * X);
    residuals[0] = _x[0] - uv.x() / uv.z();
    residuals[1] = _x[1] - uv.y() / uv.z();
    // std::cout << "residuals: \n";
    // std::cout << residuals[0] << std::endl;
    // std::cout << residuals[1] << std::endl << std::endl;;

    // std::cout << T(residuals[0]) << "\n";
    // std::cout << T(residuals[1]) << "\n";
    return true;
  }

  private:
    Eigen::Vector2d _x;
    Eigen::Vector3d _X;
    Eigen::Matrix3d _K;
};

void pose_refinement_ceres(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose)
{
  ceres::Problem problem;
  for (int i = 0; i < points_3d.size(); ++i)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<ProjectionError, 2, 7>(
        new ProjectionError(points_2d[i], points_3d[i], K)
      ),
      nullptr,
      pose.data()
    );
  }
  problem.AddParameterBlock(pose.data(), 7, new LocalParameterizationSE3());

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = false;

  ceres::Solver::Summary summary;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
  cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;
  std::cout << summary.BriefReport() << std::endl;
}

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  Mat d1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

  cout << "3d-2d pairs: " << pts_3d.size() << endl;

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
  Mat R;
  cv::Rodrigues(r, R);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1,
             0, 521.0, 249.7,
             0, 0, 1;

  // Ceres
  cout << "Custom Ceres" << endl;
  Sophus::SE3d pose_ceres;
  pose_refinement_ceres(pts_3d_eigen, pts_2d_eigen, K_eigen, pose_ceres);
  Eigen::Vector3d t_ceres = pose_ceres.translation();
  Eigen::Matrix3d R_ceres = pose_ceres.so3().unit_quaternion().toRotationMatrix();
  cout << "R_ceres = " << endl << R_ceres << endl;
  cout << "t_ceres = " << endl << t_ceres << endl;

  return 0;
}


//
// Created by 叶璨铭 on 2022/4/2.
//
#include "cn/edu/SUSTech/YeCanming/Algs/DivideAndConquer/BinarySearch.hpp"
#include "gtest/gtest.h"
#include <algorithm>
#include <boost/log/trivial.hpp>
#include <numeric>
#include <vector>
namespace cn::edu::SUSTech::YeCanming::Algs::DivideAndConquer {
    namespace ThisPackage = cn::edu::SUSTech::YeCanming::Algs::DivideAndConquer;
    TEST(GTestBinarySearch, UsedToFindElement){
        std::vector<int> a(10);
        std::iota(a.begin(), a.end(), 0); //0,1,2,...,9
      // binary_search_for_last_satisfies
        auto result = ThisPackage::binary_search_for_last_satisfies(a.begin(), a.end(), [](const int& a){
            return a<=5;
        });
        EXPECT_EQ(*result, 5);
        auto resultC = ThisPackage::binary_search_for_last_satisfies(a.cbegin(), a.cend(), [](const int& a){
            return a<=5;
        });
        EXPECT_EQ(*resultC, 5);
        // binary_search_for_first_satisfies
        result = ThisPackage::binary_search_for_first_satisfies(a.begin(), a.end(), [](const int& a){
            return a>=5;
        });
        EXPECT_EQ(*result, 5);
        resultC = ThisPackage::binary_search_for_first_satisfies(a.cbegin(), a.cend(), [](const int& a){
            return a>=5;
        });
        EXPECT_EQ(*resultC, 5);
    }
    TEST(GTestBinarySearch, WhenSatifactionSequenceAreAllZero){
        std::vector<int> a(10);
        std::fill(a.begin(), a.end(), 10);
        // binary_search_for_last_satisfies
        auto result = ThisPackage::binary_search_for_last_satisfies(a.begin(), a.end(), [](const int& a){
            return a>11;
        });
        EXPECT_EQ(result, a.end());
        auto resultC = ThisPackage::binary_search_for_last_satisfies(a.cbegin(), a.cend(), [](const int& a){
            return a>11;
        });
        EXPECT_EQ(result, a.cend());
        // binary_search_for_first_satisfies
        result = ThisPackage::binary_search_for_first_satisfies(a.begin(), a.end(), [](const int& a){
            return a>11;
        });
        EXPECT_EQ(result, a.end());
        resultC = ThisPackage::binary_search_for_first_satisfies(a.cbegin(), a.cend(), [](const int& a){
            return a>11;
        });
        EXPECT_EQ(result, a.cend());
    }
}
int main(int argc, char* argv[]){
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/**
 * Authors:
 * 		André Potes (andre.potes@gmail.com)
 *      Marcelo Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
 * Maintained by: Marcelo Fialho Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
 * Last Update: 14/12/2021
 * License: MIT
 * File: frames.hpp
 * Brief: Defines all functions related to conversions between ENU do NED frames and vice-versa
 *
 * NOTE: Most of this code is adapted from mavros
 * https://github.com/mavlink/mavros/blob/master/mavros/src/lib/ftf_frame_conversions.cpp
 * which had as authors Nuno Marques (n.marques21@hotmail.com) and Eddy Scott (scott.edward@aurora.aero)
 */
#pragma once

#include <Eigen/Dense>
#include "rotations.hpp"

namespace DSOR{

/**
 * @brief Static quaternion to convert a rotation expressed in ENU to a rotation expressed in NED (Z->Y->X convention) on
 * 			the inertial frame
 * Rotate PI/2 about Z-axis -> Rotate 0 about Y-axis -> Rotate PI about X-axis
 *
 * NOTE: this quaternion is as valid as the quaternion representing the rotation from NED to ENU (quaternion ambiguity) on
 * 			the inertial frame
 */
template <typename T>
static const Eigen::Quaternion<T> ENU_NED_INERTIAL_Q = euler_to_quaternion(Eigen::Matrix<T, 3, 1>(M_PI, 0.0, M_PI_2));

/**
 * @brief Static quaternion to convert a rotation expressed in ENU body frame (ROS base_link) to
 * 			 a rotation expressed in NED body frame  (Z->Y->X convention)
 * Rotate 0 about Z-axis -> Rotate 0 about Y-axis -> Rotate PI about X-axis
 *
 * NOTE: this quaternion is as valid as the quaternion representing the rotation from NED to ENU (quaternion ambiguity) on
 * 			the body frame
*/
template <typename T>
static const Eigen::Quaternion<T> ENU_NED_BODY_Q = euler_to_quaternion(Eigen::Matrix<T, 3, 1>(M_PI, 0.0, 0.0));

/**
 * @brief Static quaternion needed for rotating vectors in body frames between ENU and NED
 * +PI rotation around X (Forward) axis transforms from Forward, Right, Down (body frame in NED)
 * Fto Forward, Left, Up (body frame in ENU).
 */
template <typename T>
static const Eigen::Quaternion<T> BODY_ENU_NED_Q = euler_to_quaternion(Eigen::Matrix<T, 3, 1>(M_PI, 0.0, 0.0));

/**
 * @brief Static affine matrix to roate vectors ENU (or NED) -> NED (or ENU) expressed in body frame
 * +PI rotation around X (Forward) axis transforms from Forward, Right, Down (body frame in NED)
 * Fto Forward, Left, Up (body frame in ENU).
 */
template <typename T>
static const Eigen::Transform<T, 3, Eigen::Affine> BODY_ENU_NED_TF = Eigen::Transform<T, 3, Eigen::Affine>(BODY_ENU_NED_Q<T>);
//template <typename T>
//static const Eigen::Matrix<T, 3, 3> BODY_ENU_NED_AXIS = BODY_ENU_NED_Q<T>.toRotationMatrix();

/**
 * @brief Use reflections instead of rotations for NED <-> ENU transformation
 * to avoid NaN/Inf floating point pollution across different axes
 * since in NED <-> ENU the axes are perfectly aligned.
 */
static const Eigen::PermutationMatrix<3> NED_ENU_REFLECTION_XY(Eigen::Vector3i(1, 0, 2));
template <typename T>
static const Eigen::DiagonalMatrix<T, 3> NED_ENU_REFLECTION_Z(1, 1, -1);

/**
 * @brief Transform a rotation (as a quaternion) from body expressed in ENU (or NED) to inertial frame
 * 			to a similar rotation (as quaternion) from body expressed in NED (or ENU) to inertial frame.
 *
 * NOTE: Check http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
 * 	for more details behind these type of transformations towards obtaining rotations in different
 * 	frames of reference.
 *
 * @param q quaternion representing a rotation: body frame ENU (or NED) -> inertial frame (in arbitrary convention)
 * @return quaternion represeting a rotation: body frame NED (or ENU) -> inertial frame (in arbitrary convention)
 */
template <typename T>
inline Eigen::Quaternion<T> rot_body_rotation(const Eigen::Quaternion<T> &q) {
	return q * ENU_NED_BODY_Q<T>;
}

/**
 * @brief Transform a rotation (as a quaternion) from body to inertial frame expressed in ENU (or NED)
 * 			to a similar rotation (as quaternion) from body to inertial frame expressed in NED (or ENU)
 *
 * NOTE: Check http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
 * 	for more details behind these type of transformations towards obtaining rotations in different
 * 	frames of reference.
 *
 * @param q quaternion representing a rotation: body frame (in arbitrary convention) -> inertial frame ENU (or NED)
 * @return quaternion represeting a rotation: body frame (in arbitrary convention) -> inertial frame NED (or ENU)
 */
template <typename T>
inline Eigen::Quaternion<T> rot_inertial_rotation(const Eigen::Quaternion<T> &q) {
	return ENU_NED_INERTIAL_Q<T> * q;
}

/**
 * @brief Transform a rotation of a rigid body (as a quaternion) from body (ENU or NED) to inertial frame (ENU or NED)
 * 			to a similar rotation (as quaternion) from body (NED or ENU) to inertial frame (NED or ENU)
 *
 * NOTE: This function is usefull to convert the attitude of a vehicle from "ROS" quaternion to a typicall literature
 * quaternion (where both the body frame and inertial frames are in ENU). If you are converting a quaternion that expresses
 * the orientation of a sensor with respect to a rigid body's body frame (and not the inertial frame), then you DO NOT WANT TO USE THIS FUNCTION.
 * Body-FRAME NED is not the same as INERTIAL-FRAME NED (this comes once again from the fact that in ned body
 * the x-y axis don't switch like in inertial frame) as explained in the documentation.
 *
 * Essencial only use this if you are representing a body in inertial frame!
 *
 * NOTE: Check http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
 * 	for more details behind these type of transformations towards obtaining rotations in different
 * 	frames of reference.
 *
 * @param q quaternion representing a rotation: body frame (ENU or NED) -> inertial frame (ENU or NED)
 * @return quaternion representing a rotation: body frame (NED or ENU) -> inertial frame (NED or ENU)
 */
template <typename T>
inline Eigen::Quaternion<T> rot_body_to_inertial(const Eigen::Quaternion<T> &q) {
	return rot_inertial_rotation(rot_body_rotation(q));
}

/**
 * @brief Transform vector in ENU (or NED) to NED (or ENU), expressed in body-frame.
 * 	+PI rotation around X (Forward) axis transforms from Forward, Right, Down (body frame in NED)
 * 	Fto Forward, Left, Up (body frame in ENU).
 *
 * @param vec Vector expressed in body-frame (ENU or NED)
 * @return Vector expressed in body-frame (NED or ENU)
 */
template <typename T>
inline Eigen::Matrix<T, 3, 1> transform_vect_body_enu_ned(const Eigen::Matrix<T,3,1> &vec) {
	return BODY_ENU_NED_TF<T> * vec;
}

/**
 * @brief Transform a vector in a given frame of reference to another frame of reference.
 *
 * @param vec Vector expressed in the original frame of reference
 * @param q Quaternion that expresses the orientation of the original frame of reference with respect to the final frame of reference
 * @return Vector expressed in the new frame of reference
 */
template <typename T>
inline Eigen::Matrix<T, 3, 1> transform_vect_between_arbitrary_ref(const Eigen::Matrix<T, 3, 1> &vec, const Eigen::Quaternion<T> &q) {

	// Create an Affine3D transform with the rotation between the reference frames
	const Eigen::Transform<T, 3, Eigen::Affine> frame_conversion = Eigen::Transform<T, 3, Eigen::Affine>(q);
	return frame_conversion * vec;
}

/**
 * @brief Transform vector in ENU (or NED) to NED (or ENU), expressed in inertial-frame.
 *  ENU <---> NED - Invert the Z axis and switch the XY axis
 *
 * @param vec Vector expressed in inertial-frame (ENU or NED)
 * @return Vector expressed in inertial-frame (NED or ENU)
 */
template <typename T>
inline Eigen::Matrix<T, 3, 1> transform_vect_inertial_enu_ned(const Eigen::Matrix<T,3,1> &vec) {
	return NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z<T> * vec);
}

/**
 * @brief Transform 3x3 covariance matrix in ENU (or NED) to NED (or ENU), expressed in body-frame.
 *
 * NOTE: Check https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance for a detailed
 * 	explanation of the actual conversion proof for covariance matrices
 *
 * @param cov_in Covariance matrix expressed in body-frame (ENU or NED)
 * @return Covariance matrix expressed in body-frame (NED or ENU)
 */
template <typename T>
inline Eigen::Matrix<T, 3, 3> transform_cov3_body_enu_ned(const Eigen::Matrix<T, 3, 3> &cov_in) {
	return cov_in * BODY_ENU_NED_Q<T>;
}

/**
 * @brief Transform 3x3 covariance matrix in ENU (or NED) to NED (or ENU), expressed in inertial-frame.
 *
 * NOTE: Check https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance for a detailed
 * 	explanation of the actual conversion proof for covariance matrices
 *
 * @param cov_in Covariance matrix expressed in inertial-frame (ENU or NED)
 * @return Covariance matrix expressed in inertial-frame (NED or ENU)
 */
template <typename T>
inline Eigen::Matrix<T, 3, 3> transform_cov3_inertial_enu_ned(const Eigen::Matrix<T, 3, 3> &cov_in) {
	Eigen::Matrix<T, 3, 3> cov_out;

	cov_out = NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z<T> * cov_in * NED_ENU_REFLECTION_Z<T> ) *
        NED_ENU_REFLECTION_XY.transpose();

	return cov_out;
}

}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the kernel ridge regression
    object from the dlib C++ Library.

    This example will train on data from the sinc function.

*/

#include <iostream>
#include <vector>

#include <dlib/svm.h>

using namespace std;
using namespace dlib;

// Here is the sinc function we will be trying to learn with kernel ridge regression
double sinc(double x)
{
    if (x == 0)
        return 1;
    return sin(x)/x;
}

int main()
{
    // Here we declare that our samples will be 1 dimensional column vectors.
    typedef matrix<double,1,1> sample_type;

    // Now sample some points from the sinc() function
    sample_type m;
    std::vector<sample_type> samples;
    std::vector<double> labels;
    for (double x = -10; x <= 4; x += 1)
    {
        m(0) = x;
        samples.push_back(m);
        labels.push_back(sinc(x));
    }

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the krr_trainer object.  This is the
    // object that we will later use to do the training.
    krr_trainer<kernel_type> trainer;

    // Here we set the kernel we want to use for training.   The radial_basis_kernel
    // has a parameter called gamma that we need to determine.  As a rule of thumb, a good
    // gamma to try is 1.0/(mean squared distance between your sample points).  So
    // below we are using a similar value computed from at most 2000 randomly selected
    // samples.
    const double gamma = 3.0/compute_mean_squared_distance(randomly_subsample(samples, 2000));
    cout << "using gamma of " << gamma << endl;
    trainer.set_kernel(kernel_type(gamma));

    // now train a function based on our sample points
    decision_function<kernel_type> test = trainer.train(samples, labels);

    // now we output the value of the sinc function for a few test points as well as the
    // value predicted by our regression.
    m(0) = 2.5; cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = 0.1; cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = -4;  cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = 5.0; cout << sinc(m(0)) << "   " << test(m) << endl;

    // The output is as follows:
    //using gamma of 0.075
    //    0.239389   0.239389
    //    0.998334   0.998362
    //    -0.189201   -0.189254
    //    -0.191785   -0.186618

    // The first column is the true value of the sinc function and the second
    // column is the output from the krr estimate.

    // Note that the krr_trainer has the ability to tell us the leave-one-out predictions
    // for each sample.
    std::vector<double> loo_values;
    trainer.train(samples, labels, loo_values);
    cout << "mean squared LOO error: " << mean_squared_error(labels,loo_values) << endl;
    cout << "R^2 LOO value:          " << r_squared(labels,loo_values) << endl;
    // Which outputs the following:
    // mean squared LOO error: 8.29575e-07
    // R^2 LOO value:          0.999995

    // Another thing that is worth knowing is that just about everything in dlib is serializable.
    // So for example, you can save the test object to disk and recall it later like so:
    ofstream fout("saved_function.dat",ios::binary);
    serialize(test,fout);
    fout.close();

    // now lets open that file back up and load the function object it contains
    ifstream fin("saved_function.dat",ios::binary);
    deserialize(test, fin);

}


/**
 * @file expfittedupwind.cc
 * @brief NPDE homework ExpFittedUpwind
 * @author Amélie Loher, Philippe Peter
 * @date 07.01.2021
 * @copyright Developed at ETH Zurich
 */

#include "expfittedupwind.h"

#include <lf/base/base.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <cmath>
#include <memory>
#include <vector>

namespace ExpFittedUpwind {

/**
 * @brief Computes the Bernoulli function B(tau)
 **/
double Bernoulli(double tau) {
  //====================
  // Your code goes here
  //====================
  return tau;
}

/**
 * @brief computes the quantities \beta(e) for all the edges e of a mesh
 * @param mesh_p underlying mesh
 * @param mu vector of nodal values of a potential Psi
 * @return  Mesh Data set containing the quantities \beta(e)
 */
std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<double>> CompBeta(
    std::shared_ptr<const lf::mesh::Mesh> mesh_p, const Eigen::VectorXd& mu) {
  // data set over all edges of the mesh.
  auto beta_p = lf::mesh::utils::make_CodimMeshDataSet(mesh_p, 1, 1.0);

  //====================
  // Your code goes here
  //====================

  return beta_p;
}

/**
 * @brief actual computation of the element matrix
 * @param cell reference to the triangle for which the matrix is evaluated
 * @return 3x3 dense matrix containg the element matrix
 */
Eigen::Matrix3d ExpFittedEMP::Eval(const lf::mesh::Entity& cell) {
  LF_VERIFY_MSG(cell.RefEl() == lf::base::RefEl::kTria(),
                "Only 2D triangles are supported.");

  // Evaluate the element matrix A_K
  Eigen::Matrix3d AK = laplace_provider_.Eval(cell).block<3, 3>(0, 0);

  Eigen::Matrix3d result;

  //====================
  // Your code goes here
  //====================

  return std::move(result);
}

/**
 * @brief returns the quanties beta(e) for  the
 * three edges e_0, e_1 and e_2 of a triangle.
 * @param cell reference to the triangle for which the quantities are needed
 * @return vector  [beta(e_0),beta(e_1),beta(e_2)]'
 **/
Eigen::Vector3d ExpFittedEMP::beta_loc(const lf::mesh::Entity& cell) {
  Eigen::Vector3d b;
  auto edges = cell.SubEntities(1);
  for (int i = 0; i < 3; ++i) {
    b(i) = (*beta_)(*(edges[i]));
  }
  return b;
}

/** @brief returns the nodal values of the potential Psi for the
 * three vertices a_1, a_2 and a_3 of a triangle
 * @param cell reference to the triangle for which the quantities are needed
 * @return vector [Psi(a_1), Psi(a_2), Psi(a_3)]'
 **/
Eigen::Vector3d ExpFittedEMP::mu_loc(const lf::mesh::Entity& cell) {
  Eigen::Vector3d m;
  auto mesh_p = fe_space_->Mesh();
  auto vertices = cell.SubEntities(2);
  for (int i = 0; i < 3; ++i) {
    int index = mesh_p->Index(*(vertices[i]));
    m(i) = mu_(index);
  }
  return m;
}

} /* namespace ExpFittedUpwind */


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                       */
/*  This file is part of the library KASKADE 7                 */
/*    see http://www.zib.de/en/numerik/software/kaskade-7.html         */
/*                                       */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin              */
/*                                       */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.  */
/*    see $KASKADE/academic.txt                        */
/*                                       */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/**
 * @file
 * @ingroup tests
 * @brief  Test with embedded error estimation on a domain with reentrant corner.
 *
 * Testprogram for Kaskade: tests wether the error between calculated and exact solution behaves as expected.
 * This test was built by using the embedded error estimation example (2D) and changing the domain.
 *
 * Test uses direct solver or IterateType::CG with preconditioner PrecondType::ICC, PrecondType::ICC0 or PrecondType::BOOMERAMG.
 * Uses ContinuousLagrangeMapper.
 *
 * Since the exact solution is not known, the solution, that is obtained by the finest refinement, is taken for error calculation.
 */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/embedded_errorest.hh"
#include "fem/norms.hh"
// #include "fem/hierarchicspace.hh"
#include "fem/lagrangespace.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/iccprecond.hh"
#include "linalg/icc0precond.hh"
#include "linalg/hyprecond.hh"       // BoomerAMG
#include "linalg/cg.hh"
#include "utilities/enums.hh"
#include "utilities/kaskopt.hh"

using namespace Kaskade;
#include "reentrantCorner.hh"

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  int verbosityOpt = 0;
  bool dump = false;
  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  // using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,1,0> >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = ReentrantCornerFunctional<double,VariableSet>;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  using Rhs = Assembler::TestVariableSet::CoefficientVectorRepresentation<>::type;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  std::cout << "Start heat program using embedded error estimation on a domain with reentrant corner (einspringender Ecke)" << std::endl;

  int  refinements = 2;
  int verbosity   = getParameter(pt, "verbosity", 1);
  // if true, then the test result will be written in a file
  bool result = getParameter(pt, "result",0);

  constexpr int orderLength = 5;
  constexpr int maxRefSteps = 45;
  //ansatz orders to be tested
  int order[orderLength] = {1,2,3,4,5};
  //error values for corresponding ansatz order
  //double errors[orderLength] = {3.4e-4,1.3e-4,6.0e-6,9.0e-7,1.5e-7};
  double errors[orderLength] = {1.4e-3,3.1e-4,4.2e-05,5.0e-05,6.7e-05};
  // former values for error limits
  //double errors[orderLength] = {1e-3,3e-4,2e-5,3e-6,1.5e-7};
  // values for order of convergence for corresponding ansatz order
  double conOrder[orderLength] = {0.7,0.9,1.2,1.4,1.47};

  double atol = 1e-7;
  double rtol = 1e-7;

  bool valid = true;
  std::stringstream message("Test succeeded", std::stringstream::out);

  DirectType directType;
  MatrixProperties property;
  PrecondType precondType = PrecondType::NONE;
  std::string empty;

  int direct, onlyLowerTriangle = false;

  std::string s("names.type.");
  std::string solverTypeI = getParameter(pt, "solver.type", empty);
  std::string solverTypeII;
  s += solverTypeI;
  direct = getParameter(pt, s, 0);

  std::string directSolver = getParameter(pt, "solver.direct", empty);
  s = "names.direct." + directSolver;
  directType = static_cast<DirectType>(getParameter(pt, s, 2));

  // Remark: in this example only PrecondType::NONE,PrecondType::JACOBI, PrecondType::ICC, PrecondType::ICC0, PrecondType::BOOMERAMG are used.
  std::string preconditioner = getParameter(pt, "solver.preconditioner", empty);
  s = "names.preconditioner." + preconditioner;
  precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;
  if(verbosity > 0) {
    std::cout << "discretization is symmetric" << std::endl;
  }

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || (precondType == PrecondType::ICC) )
  {
    onlyLowerTriangle = true;
    std::cout <<
      "Note: direct solver MUMPS/PARADISO or PrecondType::ICC preconditioner ===> onlyLowerTriangle is set to true!"
      << std::endl;
  }

  //iterate over the ansatz orders
  for(int i = 0;i < orderLength && valid;i++) {
    if(verbosity > 0) {
      std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
      std::cout << "discretization order         : " << order[i] << std::endl;
      std::cout << "output level (verbosity)     : " << verbosity << std::endl;
    }

    //   two-dimensional space: dim=2
    Dune::GridFactory<Grid> factory;

    // create Grid on a domain with reentrant corner
    // vertex coordinates v[0], v[1]
    Dune::FieldVector<double,dim> v;
    v[0]=0; v[1]=0; factory.insertVertex(v);
    v[0]=0.5; v[1]=0; factory.insertVertex(v);
    v[0]=1; v[1]=0; factory.insertVertex(v);
    v[0]=1; v[1]=0.499; factory.insertVertex(v);
    v[0]=0.5; v[1]=0.5; factory.insertVertex(v);
    v[0]=1; v[1]=0.501; factory.insertVertex(v);
    v[0]=1; v[1]=1; factory.insertVertex(v);
    v[0]=0.5; v[1]=1; factory.insertVertex(v);
    v[0]=0; v[1]=1; factory.insertVertex(v);
    v[0]=0; v[1]=0.5; factory.insertVertex(v);
    // triangle defined by 3 vertex indices
    std::vector<unsigned int> vid(3);
    Dune::GeometryType gt(Dune::GeometryType::simplex,2);
    vid[0]=0; vid[1]=1; vid[2]=9; factory.insertElement(gt,vid);
    vid[0]=1; vid[1]=4; vid[2]=9; factory.insertElement(gt,vid);
    vid[0]=1; vid[1]=3; vid[2]=4; factory.insertElement(gt,vid);
    vid[0]=1; vid[1]=2; vid[2]=3; factory.insertElement(gt,vid);
    vid[0]=5; vid[1]=6; vid[2]=7; factory.insertElement(gt,vid);
    vid[0]=4; vid[1]=5; vid[2]=7; factory.insertElement(gt,vid);
    vid[0]=4; vid[1]=7; vid[2]=9; factory.insertElement(gt,vid);
    vid[0]=7; vid[1]=8; vid[2]=9; factory.insertElement(gt,vid);
    std::unique_ptr<Grid> grid( factory.createGrid() ) ;
    // the coarse grid will be refined
    grid->globalRefine(refinements);
    // some information on the refined mesh
    if(verbosity > 0) {
      std::cout << std::endl;
      std::cout << "Grid: " << grid->size(0) << " triangles, " << std::endl;
      std::cout << "      " << grid->size(1) << " edges, " << std::endl;
      std::cout << "      " << grid->size(2) << " points" << std::endl << std::endl;
    }

    // a gridmanager is constructed
    // as connector between geometric and algebraic information
    GridManager<Grid> gridManager(std::move(grid));
    gridManager.setVerbosity(verbosity);
    gridManager.enforceConcurrentReads(true);

    // construction of finite element space for the scalar solution T
    H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
    order[i]);
    Spaces spaces(&temperatureSpace);
    // VariableDescription<int spaceId, int components, int Id>
    // spaceId: number of associated FEFunctionSpace
    // components: number of components in this variable
    // Id: number of this variable
    std::string varNames[1] = { "T" };
    VariableSet variableSet(spaces,varNames);
    Functional F;

    if(verbosity > 0) {
      std::cout << "no of variables = " << nvars << std::endl;
      std::cout << "no of equations = " << neq   << std::endl;
    }

    //construct Galerkin representation
    Assembler assembler(gridManager,spaces);

    // keep the solutions of different refinements
    std::vector<VariableSet::VariableSet> xx;
    xx.reserve(maxRefSteps);
    for(int i=0;i<maxRefSteps;i++) {
      xx.push_back(VariableSet::VariableSet(variableSet));
    }

    size_t nnz  = assembler.nnz(0,neq,0,nvars,onlyLowerTriangle);
    size_t size = variableSet.degreesOfFreedom(0,nvars);
    if ( verbosity>0) std::cout << "init mesh: nnz = " << nnz << ", dof = " << size << std::endl;

    std::vector<std::pair<double,double> > tol(1);
    tol[0] = std::make_pair(atol,rtol);
    if(verbosity > 0) {
      std::cout << std::endl << "Accuracy: atol = " << atol << ",  rtol = " << rtol << std::endl;
    }

    bool accurate = true;
    int refSteps = -1;

    //keep degrees of freedom
    std::vector<double> dofs;
    dofs.reserve(maxRefSteps);

    do {
      refSteps++;
      if(refSteps >= maxRefSteps || size > 2e5)
      {
        refSteps--;
        std::cout << "maxRefSteps = " << maxRefSteps << ",  size = " << size << std::endl;
        break;
      }

      boost::timer::cpu_timer assembTimer;
      VariableSet::VariableSet x(variableSet);
      assembler.assemble(linearization(F,x));
      CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
      solution = 0;
      CoefficientVectors rhs(assembler.rhs());
      AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);
      MatrixAsTriplet<double> tri = A.get<MatrixAsTriplet<double> >();
      if ( verbosity>0) std::cout << "assemble: " << (double)assembTimer.elapsed().user/1e9 << "s\n";

      if (direct)
      {
        solverTypeII = directSolver;
        boost::timer::cpu_timer directTimer;
        directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
        x.data = solution.data;

        if ( verbosity>0) std::cout << "direct solve: " << (double)(directTimer.elapsed().user)/1e9 << "s\n";
      }
      else
      {
        solverTypeII = "CG, " + preconditioner;
        int iteSteps = getParameter(pt, "solver.iteMax", 1000);
        double iteEps = getParameter(pt, "solver.iteEps", 1.0e-10);
        //if ( verbosity>0) std::cout << "iterative solver: steps = " << iteSteps <<
        //   ", eps = " << iteEps << std::endl;
        boost::timer::cpu_timer iteTimer;
        Dune::InverseOperatorResult res;
        const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
        StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
        int lookAhead = getParameter(pt, "solver.lookAhead", 3);
        termination.setLookAhead(lookAhead);

        switch (precondType)
        {
          case PrecondType::ICC0:
          {
            std::cout << "selected preconditioner: ICC0" << std::endl;
            ICC_0Preconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > icc0(A);
            CG<LinearSpace,LinearSpace> cg(A,icc0,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::ICC:
          {
            std::cout << "selected preconditioner: ICC" << std::endl;
            if (property != MatrixProperties::SYMMETRIC)
            {
              std::cout << "PrecondType::ICC preconditioner of TAUCS lib has to be used with matrix.property==MatrixProperties::SYMMETRIC\n";
              std::cout << "i.e., call the executable with option --solver.property MatrixProperties::SYMMETRIC\n\n";
            }
            double dropTol = getParameter(pt, "solver.ICC.dropTol", 0.01);;
            ICCPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > icc(A,dropTol);
            CG<LinearSpace,LinearSpace> cg(A,icc,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
          case PrecondType::BOOMERAMG:
          default:
          {
            int steps = getParameter(pt, "solver.BOOMERAMG.steps", iteSteps);
            int coarsentype = getParameter(pt, "solver.BOOMERAMG.coarsentype", 21);
            int interpoltype = getParameter(pt, "solver.BOOMERAMG.interpoltype", 0);
            int cycleType = getParameter(pt, "solver.BOOMERAMG.cycleType", 1);
            int relaxType = getParameter(pt, "solver.BOOMERAMG.relaxType", 3);
            int variant = getParameter(pt, "solver.BOOMERAMG.variant", 0);
            int overlap = getParameter(pt, "solver.BOOMERAMG.overlap", 1);
            double tol = getParameter(pt, "solver.BOOMERAMG.tol", iteEps);
            double strongThreshold = getParameter(pt, "solver.BOOMERAMG.strongThreshold", (dim==2)?0.25:0.6);
            BoomerAMG<AssembledGalerkinOperator<Assembler,0,1,0,1> >
            BoomerAMGPrecon(A,steps,coarsentype,interpoltype,tol,cycleType,relaxType,
            strongThreshold,variant,overlap,1,verbosity);
            //Dune::LoopSolver<LinearSpace> cg(A,BoomerAMGPrecon,iteEps,iteSteps,verbosity);
            CG<LinearSpace,LinearSpace> cg(A,BoomerAMGPrecon,defaultScalarProduct,termination,verbosity);
            cg.apply(solution,rhs,res);
          }
          break;
        }
        solution *= -1.0;
        x.data = solution.data;

        if ( verbosity>0) std::cout << "iterative solve eps= " << iteEps << ": "
            << (res.converged?"converged":"failed") << " after "
            << res.iterations << " steps, rate="
            << res.conv_rate << ", time=" << (double)(iteTimer.elapsed().user)/1e9 << "s\n";
      }
      // VariableSet::VariableSet xx may be used beyond the do...while loop
      xx[refSteps].data = x.data;
      dofs.push_back(size);

      VariableSet::VariableSet e = x;
      projectHierarchically(variableSet,e);
      e -= x;

      accurate = embeddedErrorEstimator(variableSet,e,x,IdentityScaling(),tol,gridManager,verbosity);
      nnz = assembler.nnz(0,1,0,1,onlyLowerTriangle);;
      size = variableSet.degreesOfFreedom(0,1);
      if ( verbosity>0) std::cout << "new mesh: nnz = " << nnz << ", dof = " << size << std::endl;

    }  while (!accurate);

    //calculate and keep errors
    if ( refSteps < 3 ) {
      message << "Test failed: refSteps = " << refSteps << " is too small" ;
      break;}
    else {

      std::vector<double> ress(refSteps-2);
      for(int i = 0; i < refSteps-2;i++) {
        L2Norm l2;
        xx[i] -= xx[refSteps];
        ress[i] = l2( boost::fusion::at_c<0>(xx[i].data) );
      }

      if(verbosity > 0) {
        std::cout << "error = " << ress.back() << std::endl;
      }

      if(verbosity > 0) {
      std::cout << "error in l2norm: " << ress.back() << "  has to be smaller than  "
                << errors[i] << std::endl;
      }

      // query whether error is low enough
      if(ress.back() > errors[i]) {
        valid = false;
        message << "Test failed: The error was too high at the test with ansatz functions of order "
                << order[i] << ".";
        message << "\n             err = " << ress.back() << ", threshold = " << errors[i] << ".";
      }

      // calculate order of convergence by linear regression
      double a11=0, a12=0, a22=ress.size(), b1=0,b2=0;
      for(int j=0;j<ress.size();j++) {
        dofs[j] = std::log(dofs[j]);
        ress[j] = std::log(ress[j]);
        a11 += dofs[j]*dofs[j];
        a12 += dofs[j];
        b1 += dofs[j]*ress[j];
        b2 += ress[j];
      }
      double det = 1.0/(a11*a22-a12*a12);
      double convOrd = det*(a22*b1-a12*b2);
      double logc = det*(a11*b2-a12*b1);
      convOrd = -convOrd;
      if(verbosity > 0) {
        std::cout << "error = c*(1/N)^p with" << std::endl;
        std::cout << "p = " << convOrd << std::endl;
        std::cout << "log(c) = " << logc << std::endl;
      }
      if(convOrd < conOrder[i]) {
        valid = false;
        message << "Test failed: The order of convergence was too low at the test with ansatz functions of order " << order[i] << ".";
      }
    }
  }

  std::cout << "End test program" << std::endl;
  std::cout << message.str() << std::endl;
  if(result) {
    std::string description = "Test with embedded error estimation, einspringende Ecke. Used "
                              + solverTypeI + " solver " + solverTypeII + ":";
    std::ofstream outfile("../testResult.txt", std::ofstream::out | std::ofstream::app);
    outfile << description << std::endl << message.str() << std::endl << std::endl;
    outfile.close();
  }
}


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

#ifndef RW_MATH_TRANSFORM3D_HPP
#define RW_MATH_TRANSFORM3D_HPP

/**
 * @file Transform3D.hpp
 */

#if !defined(SWIG)
#include "Rotation3D.hpp"
#include "Rotation3DVector.hpp"
#include "Vector3D.hpp"

#include <Eigen/Core>
#include <cassert>
#include <limits>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/
#if !defined(SWIGJAVA)
    /**
     * @brief A 4x4 homogeneous transform matrix @f$ \mathbf{T}\in SE(3) @f$
     *
     * @f$
     * \mathbf{T} =
     * \left[
     *  \begin{array}{cc}
     *  \mathbf{R} & \mathbf{d} \\
     *  \begin{array}{ccc}0 & 0 & 0\end{array} & 1
     *  \end{array}
     * \right]
     * @f$
     *
     */

     #endif
    template< class T = double > class Transform3D
    {
      public:
        //! Value type.
        typedef T value_type;

        //! @brief Type for the internal Eigen matrix.
        typedef Eigen::Matrix< T, 4, 4 > EigenMatrix4x4;

        /**
         * @brief Default Constructor.
         *
         * Initializes with 0 translation and Identity matrix as rotation
         */
        Transform3D () : _d (), _R (rw::math::Rotation3D< T >::identity ()) {}

        /**
         * @brief Constructs a homogeneous transform
         * @param d [in] @f$ \mathbf{d} @f$ A 3x1 translation vector
         * @param R [in] @f$ \mathbf{R} @f$ A 3x3 rotation matrix
         */
        Transform3D (const rw::math::Vector3D< T >& d, const rw::math::Rotation3D< T >& R) : _d (d), _R (R) {}

        /**
           @brief A homogeneous transform with a rotation of \b R and a
           translation of zero.
        */
        explicit Transform3D (const rw::math::Rotation3D< T >& R) : _d (0, 0, 0), _R (R) {}

        /**
           @brief A homogeneous transform with a rotation of zero and a
           translation of \b d.
        */
        explicit Transform3D (const rw::math::Vector3D< T >& d) : _d (d), _R (rw::math::Rotation3D< T >::identity ()) {}

        /**
         * @brief Constructs a homogeneous transform
         *
         * Calling this constructor is equivalent to the transform
         * Transform3D(d, r.toRotation3D()).
         *
         * @param d [in] @f$ \mathbf{d} @f$ A 3x1 translation vector
         * @param r [in] @f$ \mathbf{r} @f$ A 3x1 rotation vector
         */
        Transform3D (const rw::math::Vector3D< T >& d, const rw::math::Rotation3DVector< T >& r) :
            _d (d), _R (r.toRotation3D ())
        {}

        /**
         * @brief Creates a Transform3D from matrix_expression
         * @param r [in] an Eigen Vector
         */
        template< class R > explicit Transform3D (const Eigen::MatrixBase< R >& r)
        {
            _d[0] = T(r.row (0) (3));
            _d[1] = T(r.row (1) (3));
            _d[2] = T(r.row (2) (3));
            _R = Rotation3D<T>(r.block(0,0,3,3));
        }
#if !defined(SWIGJAVA)
        /**
         * @brief Constructs a homogeneous transform using the original
         * Denavit-Hartenberg notation
         *
         * @param alpha [in] @f$ \alpha_i @f$
         * @param a [in] @f$ a_i @f$
         * @param d [in] @f$ d_i @f$
         * @param theta [in] @f$ \theta_i @f$
         * @return @f$ ^{i-1}\mathbf{T}_i @f$
         *
         * @f$
         *  \robabx{i-1}{i}{\mathbf{T}}=
         *  \left[
         *    \begin{array}{cccc}
         *      c\theta_i & -s\theta_i c\alpha_i &  s\theta_i s\alpha_i & a_i c\theta_i \\
         *      s\theta_i &  c\theta_i c\alpha_i & -c\theta_i s\alpha_i & a_i s\theta_i \\
         *      0         &  s\alpha_i           &  c\alpha_i           & d_i \\
         *      0         &  0                   & 0                    & 1
         *    \end{array}
         *  \right]
         * @f$
         */

         #endif
        static const Transform3D DH (T alpha, T a, T d, T theta);

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs a homogeneous transform using the Craig (modified)
         * Denavit-Hartenberg notation
         *
         * @param alpha [in] @f$ \alpha_{i-1} @f$
         * @param a [in] \f$ a_{i-1} \f$
         * @param d [in] \f$ d_i \f$
         * @param theta [in] \f$ \theta_i \f$
         * @return @f$ \robabx{i-1}{i}{\mathbf{T}} @f$
         *
         * @note The Craig (modified) Denavit-Hartenberg notation differs from
         * the original Denavit-Hartenberg notation and is given as
         *
         * @f$
         * \robabx{i-1}{i}{\mathbf{T}} =
         * \left[
         * \begin{array}{cccc}
         * c\theta_i & -s\theta_i & 0 & a_{i-1} \\
         * s\theta_i c\alpha_{i-1} & c\theta_i c\alpha_{i-1} & -s\alpha_{i-1} & -s\alpha_{i-1}d_i \\
         * s\theta_i s\alpha_{i-1} & c\theta_i s\alpha_{i-1} &  c\alpha_{i-1} &  c\alpha_{i-1}d_i \\
         * 0 & 0 & 0 & 1
         * \end{array}
         * \right]
         * @f$
         *
         */

         #endif
        static const Transform3D craigDH (T alpha, T a, T d, T theta);

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs a homogeneous transform using the Gordon (modified)
         * Denavit-Hartenberg notation
         *
         * @param alpha [in] @f$ \alpha_i @f$
         * @param a [in] @f$ a_i @f$
         * @param beta [in] @f$ \beta_i @f$
         * @param b [in] @f$ b_i @f$
         * @return @f$ ^{i-1}\mathbf{T}_i @f$
         *
         * @note The Gordon (modified) Denavit-Hartenberg differs from
         * the original Denavit-Hartenberg as it branches between parallel
         * and non-parallel z-axes.
         *
         * @f$ z_{i-1} @f$ is close to parallel to @f$ z_i @f$
         * @f$
         *  \robabx{i-1}{i}{\mathbf{T}}=
         *  \left[
         *    \begin{array}{cccc}
         *       c\beta_i & s\alpha_i s\beta_i &  c\alpha_i s\beta_i &  a_i c\beta_i \\
         *       0        & c\alpha_i          & -s\alpha_i          &  b_i \\
         *      -s\beta_i & s\alpha_i c\beta_i &  c\alpha_i c\beta_i & -a_i s\beta \\
         *      0         & 0                  & 0                    & 1
         *    \end{array}
         *  \right]
         * @f$
         */

         #endif
        static const Transform3D DHHGP (T alpha, T a, T beta, T b);

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs the identity transform
         * @return the identity transform
         *
         * @f$
         * \mathbf{T} =
         * \left[
         * \begin{array}{cccc}
         * 1 & 0 & 0 & 0\\
         * 0 & 1 & 0 & 0\\
         * 0 & 0 & 1 & 0\\
         * 0 & 0 & 0 & 1
         * \end{array}
         * \right]
         * @f$
         */

         #endif
        static const Transform3D& identity ();

#if !defined(SWIG)
        /**
         * @brief Returns matrix element reference
         * @param row [in] row, row must be @f$ < 3 @f$
         * @param col [in] col, col must be @f$ < 4 @f$
         * @return reference to matrix element
         */
        T& operator() (std::size_t row, std::size_t col)
        {
            assert (row < 3);
            assert (col < 4);
            if (row < 3 && col < 3)
                return _R (row, col);
            else
                return _d (row);
        }

        /**
         * @brief Returns const matrix element reference
         * @param row [in] row, row must be @f$ < 3 @f$
         * @param col [in] col, col must be @f$ < 4 @f$
         * @return const reference to matrix element
         */
        const T& operator() (std::size_t row, std::size_t col) const
        {
            assert (row < 3);
            assert (col < 4);
            if (row < 3 && col < 3)
                return _R (row, col);
            else
                return _d (row);
        }
#else
        MATRIXOPERATOR (T);
#endif

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] Transform to compare with
         * @return True if equal.
         */
        bool operator== (const Transform3D< T >& rhs) const
        {
            return (R () == rhs.R ()) && (P () == rhs.P ());
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] Transform to compare with
         * @return True if not equal.
         */
        bool operator!= (const Transform3D< T >& rhs) const { return !(*this == rhs); }

        /**
         * @brief Compares the transformations with a given precision
         *
         * Performs an element wise comparison. Two elements are considered equal if the difference
         * are less than \b precision.
         *
         * @param t3d [in] Transform to compare with
         * @param precision [in] The precision to use for testing
         * @return True if all elements are less than \b precision apart.
         */
        bool equal (const Transform3D< T >& t3d,
                    const T precision = std::numeric_limits< T >::epsilon ()) const
        {
            if (!R ().equal (t3d.R (), precision))
                return false;
            for (size_t i = 0; i < 3; i++)
                if (fabs (P ()[i] - t3d.P ()[i]) > precision)
                    return false;
            return true;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Calculates @f$ \robabx{a}{c}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}
         * \robabx{b}{c}{\mathbf{T}} @f$
         * @param bTc [in] @f$ \robabx{b}{c}{\mathbf{T}} @f$
         * @return @f$ \robabx{a}{c}{\mathbf{T}} @f$
         *
         * @f$
         * \robabx{a}{c}{\mathbf{T}} =
         * \left[
         *  \begin{array}{cc}
         *  \robabx{a}{b}{\mathbf{R}}\robabx{b}{c}{\mathbf{R}} & \robabx{a}{b}{\mathbf{d}} +
         * \robabx{a}{b}{\mathbf{R}}\robabx{b}{c}{\mathbf{d}} \\ \begin{array}{ccc}0 & 0 &
         * 0\end{array} & 1 \end{array} \right]
         * @f$
         */

         #endif
        const Transform3D operator* (const Transform3D& bTc) const
        {
            return Transform3D (_d + _R * bTc._d, _R * bTc._R);
        }

        /**
         * @brief Calculates @f$ \robax{a}{\mathbf{p}} = \robabx{a}{b}{\mathbf{T}}
         * \robax{b}{\mathbf{p}} \f$ thus transforming point @f$ \mathbf{p} @f$ from frame @f$ b @f$
         * to frame @f$ a @f$
         * @param bP [in] @f$ \robax{b}{\mathbf{p}} @f$
         * @return @f$ \robax{a}{\mathbf{p}} @f$
         */
        const rw::math::Vector3D< T > operator* (const rw::math::Vector3D< T >& bP) const { return _R * bP + _d; }

        /**
         * @brief Gets the rotation part @f$ \mathbf{R} @f$ from @f$ \mathbf{T} @f$
         * @return @f$ \mathbf{R} @f$
         */
        rw::math::Rotation3D< T >& R () { return _R; }

        /**
         * @brief Gets the rotation part @f$ \mathbf{R} @f$ from @f$ \mathbf{T} @f$
         * @return @f$ \mathbf{R} @f$
         */
        const rw::math::Rotation3D< T >& R () const { return _R; }

        /**
         * \brief Gets the position part @f$ \mathbf{d} @f$ from @f$ \mathbf{T} @f$
         * \return @f$ \mathbf{d} @f$
         */
        rw::math::Vector3D< T >& P () { return _d; }

        /**
         * @brief Gets the position part @f$ \mathbf{d} @f$ from @f$ \mathbf{T} @f$
         * @return @f$ \mathbf{d} @f$
         */
        const rw::math::Vector3D< T >& P () const { return _d; }

#if !defined(SWIG)
        /**
         * @brief Outputs transform to stream
         * @param os [in/out] an output stream
         * @param t [in] the transform that is to be sent to the output stream
         * @return os
         */
        friend std::ostream& operator<< (std::ostream& os, const Transform3D< T >& t)
        {
            // This format matches the Lua notation.
            return os << "Transform3D(" << t.P () << ", " << t.R () << ")";
        }
#else
        TOSTRING (rw::math::Transform3D< T >);
#endif

        /**
           @brief Write to \b result the product \b a * \b b.
        */
        static inline void multiply (const Transform3D< T >& a, const Transform3D< T >& b,
                                     Transform3D< T >& result)
        {
            rw::math::Rotation3D< T >::multiply (a.R (), b.R (), result.R ());
            rw::math::Rotation3D< T >::multiply (a.R (), b.P (), result.P ());
            result.P () += a.P ();
        }

        /**
         * @brief computes the inverse of t1 and multiplies it with t2.
         * The result is saved in t1. t1 = inv(t1) * t2
         */
        static inline Transform3D< T >& invMult (Transform3D< T >& t1, const Transform3D< T >& t2)
        {
            const T p0 = t1.P () (0), p1 = t1.P () (1), p2 = t1.P () (2);

            const T r01 = t1.R () (0, 1);
            const T r12 = t1.R () (1, 2);
            const T r02 = t1.R () (0, 2);

            t1.P () (0) = (-p0 + t2.P () (0)) * t1.R () (0, 0) +
                          (-p1 + t2.P () (1)) * t1.R () (1, 0) +
                          (-p2 + t2.P () (2)) * t1.R () (2, 0);

            t1.P () (1) = (-p0 + t2.P () (0)) * r01 + (-p1 + t2.P () (1)) * t1.R () (1, 1) +
                          (-p2 + t2.P () (2)) * t1.R () (2, 1);

            t1.P () (2) = (-p0 + t2.P () (0)) * r02 + (-p1 + t2.P () (1)) * r12 +
                          (-p2 + t2.P () (2)) * t1.R () (2, 2);

            t1.R () (0, 1) = t1.R () (0, 0) * t2.R () (0, 1) + t1.R () (1, 0) * t2.R () (1, 1) +
                             t1.R () (2, 0) * t2.R () (2, 1);
            t1.R () (0, 2) = t1.R () (0, 0) * t2.R () (0, 2) + t1.R () (1, 0) * t2.R () (1, 2) +
                             t1.R () (2, 0) * t2.R () (2, 2);
            t1.R () (0, 0) = t1.R () (0, 0) * t2.R () (0, 0) + t1.R () (1, 0) * t2.R () (1, 0) +
                             t1.R () (2, 0) * t2.R () (2, 0);

            t1.R () (1, 0) = r01 * t2.R () (0, 0) + t1.R () (1, 1) * t2.R () (1, 0) +
                             t1.R () (2, 1) * t2.R () (2, 0);
            t1.R () (1, 2) = r01 * t2.R () (0, 2) + t1.R () (1, 1) * t2.R () (1, 2) +
                             t1.R () (2, 1) * t2.R () (2, 2);
            t1.R () (1, 1) = r01 * t2.R () (0, 1) + t1.R () (1, 1) * t2.R () (1, 1) +
                             t1.R () (2, 1) * t2.R () (2, 1);

            t1.R () (2, 0) =
                r02 * t2.R () (0, 0) + r12 * t2.R () (1, 0) + t1.R () (2, 2) * t2.R () (2, 0);
            t1.R () (2, 1) =
                r02 * t2.R () (0, 1) + r12 * t2.R () (1, 1) + t1.R () (2, 2) * t2.R () (2, 1);
            t1.R () (2, 2) =
                r02 * t2.R () (0, 2) + r12 * t2.R () (1, 2) + t1.R () (2, 2) * t2.R () (2, 2);
            return t1;
        }

        /**
         * @brief computes the inverse of t1 and multiplies it with t2.
         * The result is saved in t1. t1 = inv(t1) * t2
         */
        static inline Transform3D< T >& invMult (const Transform3D< T >& t1,
                                                 const Transform3D< T >& t2, Transform3D< T >& t3)
        {
            const T p0 = t1.P () (0), p1 = t1.P () (1), p2 = t1.P () (2);

            const T r01 = t1.R () (0, 1);
            const T r12 = t1.R () (1, 2);
            const T r02 = t1.R () (0, 2);

            t3.P () (0) = (-p0 + t2.P () (0)) * t1.R () (0, 0) +
                          (-p1 + t2.P () (1)) * t1.R () (1, 0) +
                          (-p2 + t2.P () (2)) * t1.R () (2, 0);

            t3.P () (1) = (-p0 + t2.P () (0)) * r01 + (-p1 + t2.P () (1)) * t1.R () (1, 1) +
                          (-p2 + t2.P () (2)) * t1.R () (2, 1);

            t3.P () (2) = (-p0 + t2.P () (0)) * r02 + (-p1 + t2.P () (1)) * r12 +
                          (-p2 + t2.P () (2)) * t1.R () (2, 2);

            t3.R () (0, 1) = t1.R () (0, 0) * t2.R () (0, 1) + t1.R () (1, 0) * t2.R () (1, 1) +
                             t1.R () (2, 0) * t2.R () (2, 1);
            t3.R () (0, 2) = t1.R () (0, 0) * t2.R () (0, 2) + t1.R () (1, 0) * t2.R () (1, 2) +
                             t1.R () (2, 0) * t2.R () (2, 2);
            t3.R () (0, 0) = t1.R () (0, 0) * t2.R () (0, 0) + t1.R () (1, 0) * t2.R () (1, 0) +
                             t1.R () (2, 0) * t2.R () (2, 0);

            t3.R () (1, 0) = r01 * t2.R () (0, 0) + t1.R () (1, 1) * t2.R () (1, 0) +
                             t1.R () (2, 1) * t2.R () (2, 0);
            t3.R () (1, 2) = r01 * t2.R () (0, 2) + t1.R () (1, 1) * t2.R () (1, 2) +
                             t1.R () (2, 1) * t2.R () (2, 2);
            t3.R () (1, 1) = r01 * t2.R () (0, 1) + t1.R () (1, 1) * t2.R () (1, 1) +
                             t1.R () (2, 1) * t2.R () (2, 1);

            t3.R () (2, 0) =
                r02 * t2.R () (0, 0) + r12 * t2.R () (1, 0) + t1.R () (2, 2) * t2.R () (2, 0);
            t3.R () (2, 1) =
                r02 * t2.R () (0, 1) + r12 * t2.R () (1, 1) + t1.R () (2, 2) * t2.R () (2, 1);
            t3.R () (2, 2) =
                r02 * t2.R () (0, 2) + r12 * t2.R () (1, 2) + t1.R () (2, 2) * t2.R () (2, 2);
            return t3;
        }

        /**
         * @brief creates a transformation that is positioned in \b eye and looking toward
         * \b center along -z where \b up indicates the upward direction along which the y-axis
         * is placed. Same convention as for gluLookAt
         * and is handy for placing a cameraview.
         * @param eye [in] position of view
         * @param center [in] point to look toward
         * @param up [in] the upward direction (the
         * @return Transformation
         */
        static Transform3D< T > makeLookAt (const rw::math::Vector3D< T >& eye, const rw::math::Vector3D< T >& center,
                                            const rw::math::Vector3D< T >& up)
        {
            rw::math::Vector3D< T > f (center - eye);
            f = normalize (f);
            rw::math::Vector3D< T > s (cross (f, up));
            s = normalize (s);
            rw::math::Vector3D< T > u (cross (s, f));
            u = normalize (u);

            rw::math::Rotation3D< T > R (s[0], s[1], s[2], u[0], u[1], u[2], -f[0], -f[1], -f[2]);

            return inverse (Transform3D (R * -eye, R));
        }

        /**
         * @brief Returns a Eigen 4x4 matrix @f$ \mathbf{M}\in SE(3)
         * @f$ that represents this homogeneous transformation
         *
         * @return @f$ \mathbf{M}\in SE(3) @f$
         */
        Eigen::Matrix<T,4,4> e () const;

      private:
        rw::math::Vector3D< T > _d;
        rw::math::Rotation3D< T > _R;
    };

// Explicit template specifications.
#if !defined(SWIG)
    extern template class rw::math::Transform3D< double >;
    extern template class rw::math::Transform3D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Transform3Dd, rw::math::Transform3D< double >);
    SWIG_DECLARE_TEMPLATE (Transform3Df, rw::math::Transform3D< float >);
#endif

    using Transform3Dd = Transform3D< double >;
    using Transform3Df = Transform3D< float >;

#if !defined(SWIGJAVA)
    /**
     * @brief Calculates
     * @f$ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} @f$
     *
     * @relates Transform3D
     *
     * @param aTb [in] the transform matrix @f$ \robabx{a}{b}{\mathbf{T}} @f$
     * @return @f$ \robabx{b}{a}{\mathbf{T}} = \robabx{a}{b}{\mathbf{T}}^{-1} @f$
     *
     * @f$
     * \robabx{a}{b}{\mathbf{T}}^{-1} =
     * \left[
     *  \begin{array}{cc}
     *  \robabx{a}{b}{\mathbf{R}}^{T} & - \robabx{a}{b}{\mathbf{R}}^{T} \robabx{a}{b}{\mathbf{d}} \\
     *  \begin{array}{ccc}0 & 0 & 0\end{array} & 1
     *  \end{array}
     * \right]
     *
     * @f$
     */

     #endif
    template< class T > const Transform3D< T > inverse (const Transform3D< T >& aTb)
    {
        return Transform3D< T > (-(inverse (aTb.R ()) * aTb.P ()), inverse (aTb.R ()));
    }

    /**
     * @brief Cast Transform3D<T> to Transform3D<Q>
     * @param trans [in] Transform3D with type T
     * @return Transform3D with type Q
     */
    template< class Q, class T > const Transform3D< Q > cast (const Transform3D< T >& trans)
    {
        Transform3D< Q > res;
        for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 4; j++)
                res (i, j) = static_cast< Q > (trans (i, j));
        return res;
    }

    /*@}*/

}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Transform3D
         */
        template<>
        void write (const rw::math::Transform3D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Transform3D
         */
        template<>
        void write (const rw::math::Transform3D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Transform3D
         */
        template<>
        void read (rw::math::Transform3D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Transform3D
         */
        template<>
        void read (rw::math::Transform3D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

namespace boost { namespace serialization {
    /**
     * @brief Boost serialization.
     * @param archive [in] the boost archive to read from or write to.
     * @param transform [in/out] the transformation to read/write.
     * @param version [in] class version (currently version 0).
     * @relatedalso rw::math::Transform3D
     */
    template< class Archive, class T >
    void serialize (Archive& archive, rw::math::Transform3D< T >& transform,
                    const unsigned int version)
    {
        archive& transform.P ();
        archive& transform.R ();
    }
}}    // namespace boost::serialization

#endif    // end include guard