#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


#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;
}


/**
 * 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;
}

}


/********************************************************************************
 * 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


#include <iostream>

#include <boost/bind/bind.hpp>
#include <boost/math/special_functions/pow.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/moment.hpp>

#include "Filtering.hpp"

#include "RadarCoordinatesTemplate.hpp"

using namespace boost::accumulators;

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

matrix<double> stateTransitionMatrix(const std::size_t N, double dt) {
	matrix<double> B = identity_matrix<double>(N);
	for (std::size_t i = 0; i < N; i++) {
		for (std::size_t j = i + 1; j < N; j++) {
			unsigned ji = (unsigned) (j - i);
			double fji = boost::math::factorial<double>(ji);
			B(i, j) = pow(dt, ji) / fji;
		}
	}
	return B;
}

int main() {
	using namespace boost::numeric::ublas;

	std::cout << boost::math::pow<3>(10) << std::endl;

	/*
	matrix<double> TntTn(3,3);
	TntTn(0, 0) = 11.0; TntTn(0, 1) = -5.5; TntTn(0, 2) = 3.85;
	TntTn(1, 0) = -5.5; TntTn(1, 1) = 3.85; TntTn(1, 2) = -3.025;
	TntTn(2, 0) = 3.85; TntTn(2, 1) = -3.025; TntTn(2, 2) = 2.5333;
	vector<double> TntYn(3);
	TntYn(0) = 45.99012356;
	TntYn(1) = -9.88611426;
	TntYn(2) = 0.36213461;

	std::cout << TntTn << std::endl;
	std::cout << TntYn << std::endl;

	permutation_matrix<std::size_t> pm(TntTn.size1());
	lu_factorize(TntTn, pm);
	lu_substitute(TntTn, pm, TntYn);
	std::cout << TntYn << std::endl;

	std::cout << stateTransitionMatrix(8, 0.1) << std::endl;
	*/
	RadarCoordinates rc;

	RealVector E(1), N(1), U(1);
	E(0) = 10;
	N(0) = 20;
	U(0) = 50;
	std::cout << rc.ENU2AER(E, N, U) << std::endl;
	return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <dpMM/global.hpp>
#include <dpMM/normal.hpp>
#include <dpMM/iw.hpp>
#include <dpMM/sphere.hpp>

#define LOG_2PI 1.8378770664093453

template<typename T>
class NormalSphere : public Distribution<T>
{
public:
  uint32_t D_; // dimension of the ambient space

  NormalSphere(const Matrix<T,Dynamic,1>& mu,
      const Matrix<T,Dynamic,Dynamic>& Sigma, boost::mt19937* pRndGen);
  NormalSphere(const Matrix<T,Dynamic,1>& mu,
      const Normal<T>& normal, boost::mt19937* pRndGen);
  NormalSphere(const NormalSphere& other);
  ~NormalSphere();

  /* for any point on sphere - maps into T_muS and rotates north before logPdf */
  T logPdf(const Matrix<T,Dynamic,1>& q_i) const;
  /* assumes x_i is already in T_northS */
  T logPdfNorth(const Matrix<T,Dynamic,1>& x_i) const;
  T logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter,
      const Matrix<T,Dynamic,1>& mean, T count) const;
//  T logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter, T count) const;

  Matrix<T,Dynamic,1> sample();

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normal_.Sigma();};
  void setSigma(const Matrix<T,Dynamic,Dynamic>& Sigma)
  {return normal_.setSigma(Sigma);};
  T logDetSigma() const {return normal_.logDetSigma();};
  T logNormalizer() const {return -0.5*(normal_.logDetSigma()+D_*LOG_2PI);};

  /* mean on sphere */
  void setMean( const Matrix<T,Dynamic,1>& mu);
  const Matrix<T,Dynamic,1>& getMean() const {return mu_;};

  /* mean in tangent plane */
  void setMuInTpS( const Matrix<T,Dynamic,1>& mu)
  {normal_.mu_ = mu;};

  void setNormal(const Normal<T>& normal) {normal_ = normal;};
  const Normal<T>& normal() const {return normal_;};

private:
  Normal<T> normal_; // zero-mean Gaussian in Tangent plane (dim: D-1)
  Sphere<T> S_;
  Matrix<T,Dynamic,1> mu_; // mean pointing to location in sphere
  Matrix<T,Dynamic,Dynamic> northR_;
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, uint32_t K);

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K);

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    const Matrix<T,Dynamic,Dynamic>& Delta, T nu,
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K);

// ---------------------------------------------------------------------------
template<typename T>
NormalSphere<T>::NormalSphere(const Matrix<T,Dynamic,1>& mu,
    const Matrix<T,Dynamic,Dynamic>& Sigma, boost::mt19937* pRndGen)
  : Distribution<T>(pRndGen),  D_(mu.size()),
    normal_(Sigma,pRndGen), S_(D_)
{
  setMean(mu);
};

template<typename T>
NormalSphere<T>::NormalSphere(const Matrix<T,Dynamic,1>& mu,
      const Normal<T>& normal, boost::mt19937* pRndGen)
  : Distribution<T>(pRndGen),  D_(mu.size()),
    normal_(normal), S_(D_)
{
  setMean(mu);
};

template<typename T>
NormalSphere<T>::NormalSphere(const NormalSphere& other)
  : Distribution<T>(other.pRndGen_), D_(other.mu_.size()),
    normal_(other.normal_), S_(other.D_)
{
  setMean(other.mu_);
};

template<typename T>
NormalSphere<T>::~NormalSphere()
{};

template<typename T>
void NormalSphere<T>::setMean( const Matrix<T,Dynamic,1>& mu)
{
  assert(mu.rows() == D_);
  mu_ = mu;
  northR_ = S_.north_R_TpS2(mu_);
}

template<typename T>
T NormalSphere<T>::logPdf(const Matrix<T,Dynamic,1>& q_i) const
{
//  cout<<q_i.transpose()<<endl;
  Matrix<T,Dynamic,1> x_i = S_.Log_p_single(mu_,q_i);
//  cout<<x_i.transpose()<<endl;
//  cout<<northR_<<endl;

#ifndef NDEBUG
  ASSERT(fabs(x_i.transpose()*mu_)<1e-6, x_i.transpose()*mu_);

  Matrix<T,Dynamic,1> xNorth = (northR_*x_i);
  ASSERT(fabs( xNorth(D_-1)) < 1e-6,
      xNorth.transpose() << endl
      << " northR_ "<<endl<<northR_<<endl
      << " recomputed"<<endl<<S_.north_R_TpS2(mu_)<<endl);
  return normal_.logPdf(xNorth.topRows(D_-1));
#else
  return normal_.logPdf((northR_*x_i).topRows(D_-1));
#endif
//  return normal_.logPdf( S_.Log_p_north(mu_,q_i) );
};

template<typename T>
T NormalSphere<T>::logPdfNorth(const Matrix<T,Dynamic,1>& x_i) const
{
  assert(x_i.rows() == D_-1);
#ifndef NDEBUG
  Matrix<T,Dynamic,1> x(D_);
  x.topRows(D_-1) = x_i;
  x(D_-1) = 1.0;
//  cout<<(x.transpose()*S_.north())<<endl;
  assert(fabs((x.transpose()*S_.north()).norm() -1.) < 1.e-5);
#endif
  return normal_.logPdf(x_i);
};

template<typename T>
T NormalSphere<T>::logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter,
    const Matrix<T,Dynamic,1>& mean, T count) const
{
  return normal_.logPdf(scatter,mean,count);
}

//template<typename T>
//T NormalSphere<T>::logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter,
//    T count) const
//{
//  return normal_.logPdf(scatter,count);
//}

template<typename T>
Matrix<T,Dynamic,1> NormalSphere<T>::sample()
{
  Matrix<T,Dynamic,1> xNorth(D_-1);
  xNorth = normal_.sample();
  // if outside radius of PI wrap around
  // TODO
  while(xNorth.norm() > PI)
  {
    cout<<"wrapping around! ---------------------------------------------"<<endl;
    xNorth -= (T(2*PI))*(xNorth/xNorth.norm());
  }
//  cout<<"xNorth = "<<xNorth.transpose()<<endl;
//  cout<<"mu = "<<mu_.transpose()<<endl;
  Matrix<T,Dynamic,1> x = S_.rotate_north2p(mu_,xNorth);
//  cout<<"x = "<<x.transpose()<<endl;
  return S_.Exp_p(mu_,S_.rotate_north2p(mu_,xNorth));
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    const Matrix<T,Dynamic,Dynamic>& Delta, T nu,
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K,
    T minAngle = static_cast<T>(6.))
{
  uint32_t N = x.cols();
  uint32_t D = x.rows();
  Sphere<T> S_(D);
  boost::mt19937 rndGen(9119);

  IW<T> iw(Delta,nu,&rndGen);
  Matrix<T,Dynamic,Dynamic> mus(D,K);
  for(uint32_t k=0; k<K; ++k)
  {
    Matrix<T,Dynamic,Dynamic> Sigma = iw.sample();
//    cout<<Sigma<<endl;
//    cout<<"nu "<<nu<<endl;
//    cout<<Delta<<endl;
    Matrix<T,Dynamic,1> mu = S_.sampleUnif(&rndGen);
    if(k>0)
    {
      bool done = false;
      while(!done)
      {
        mu = S_.sampleUnif(&rndGen);
        done = true;
        for(uint32_t j=0; j<k; ++j)
          done = done & (mu.transpose()*mus.col(j) < cos(minAngle*M_PI/180.0));
      }
    }
    cout<<"sampling data for k="<<k<<" around mu="<<mu.transpose()<<" Sigma:"<<endl;
    cout<<Sigma*(180.0/M_PI)*(180.0/M_PI)<<endl;
    NormalSphere<T> gauss_k(mu,Sigma,&rndGen);
    mus.col(k) = gauss_k.getMean();
    for (uint32_t i=k*(N/K); i<min(N,(k+1)*(N/K)+N%K); ++i)
    {
//      cout<<"--"<<endl;
//      cout<<mus.col(k).transpose()<<endl;
      do{
        x.col(i) = gauss_k.sample();
      }while(fabs(x.col(i).norm()-1.0) > 1e-3);
      if(fabs(x.col(i).norm()-1.0) > 1e-2)
        cout<<x.col(i).norm()<<endl;
      z(i) = k;
//        x.col(i) /= x.col(i).norm();
//      cout<<x.col(i).transpose()<<endl;
    }
  }
  return mus;
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K)
{
  uint32_t N = x.cols();
  uint32_t D = x.rows();
  Sphere<T> S_(D);
  boost::mt19937 rndGen(9119);

  Matrix<T,Dynamic,Dynamic> Sigma = Matrix<T,Dynamic,Dynamic>::Identity(D-1,D-1);
  Sigma *= 0.05;
  Matrix<T,Dynamic,Dynamic> mus(D,K);
  for(uint32_t k=0; k<K; ++k)
  {
    NormalSphere<T> gauss_k(S_.sampleUnif(&rndGen),Sigma,&rndGen);
    mus.col(k) = gauss_k.getMean();
    for (uint32_t i=k*(N/K); i<min(N,(k+1)*(N/K)+N%K); ++i)
    {
      do{
        x.col(i) = gauss_k.sample();
      }while(fabs(x.col(i).norm()-1.0) > 1e-3);
      if(fabs(x.col(i).norm()-1.0) > 1e-2)
        cout<<x.col(i).norm()<<endl;
      z(i) = k;
//        x.col(i) /= x.col(i).norm();
//        cout<<x.col(i).transpose()<<endl;
    }
  }
  return mus;
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, uint32_t K)
{
  VectorXu z(x.cols());
  return sampleClustersOnSphere<T>(x,z,K);
};


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file analyticcevengine.cpp */

#include <ql/exercise.hpp>
#include <ql/math/functional.hpp>
#include <ql/pricingengines/vanilla/analyticcevengine.hpp>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>

namespace QuantLib {

    CEVCalculator::CEVCalculator(Real f0, Real alpha, Real beta)
    : f0_(f0),
      alpha_(alpha),
      beta_(beta),
      delta_((1.0-2.0*beta)/(1.0-beta)),
      x0_(X(f0)) { }

    Real CEVCalculator::X(Real f) const {
        return std::pow(f, 2.0*(1.0-beta_))/square<Real>()(alpha_*(1.0-beta_));
    }

    Real CEVCalculator::value(
        Option::Type optionType, Real strike, Time t) const {

        typedef boost::math::non_central_chi_squared_distribution<Real>
            nc_chi2;

        const Real kTilde = X(strike);

        if (optionType == Option::Call) {
            if (delta_ < 2.0) {
                return f0_ * (1.0 - boost::math::cdf(
                         nc_chi2(4.0-delta_, x0_/t), kTilde/t))
                     - strike * boost::math::cdf(
                         nc_chi2(2.0-delta_, kTilde/t), x0_/t);
            }
            else {
                const Real g =
                    boost::math::gamma_p(0.5*delta_-1.0,x0_/(2.0*t));

                return f0_ * (g - boost::math::cdf(
                         nc_chi2(delta_-2.0, kTilde/t), x0_/t))
                     - strike * boost::math::cdf(
                         nc_chi2(delta_, x0_/t), kTilde/t);
            }
        }
        else if (optionType == Option::Put) {
            if (delta_ < 2.0) {
                return - f0_ * boost::math::cdf(
                           nc_chi2(4.0-delta_, x0_/t), kTilde/t)
                       + strike * (1.0 - boost::math::cdf(
                           nc_chi2(2.0-delta_, kTilde/t), x0_/t));
            }
            else {
                return - f0_ * boost::math::cdf(
                           nc_chi2(delta_-2.0, kTilde/t), x0_/t)
                       + strike * (1.0 - boost::math::cdf(
                           nc_chi2(delta_, x0_/t), kTilde/t));
            }
        }
        else
            QL_FAIL("unknown option type");

    }

    AnalyticCEVEngine::AnalyticCEVEngine(
        Real f0, Real alpha, Real beta,
        const Handle<YieldTermStructure>& discountCurve)
    : calculator_(ext::make_shared<CEVCalculator>(f0, alpha,beta)),
      discountCurve_(discountCurve) {
        registerWith(discountCurve_);
    }

    void AnalyticCEVEngine::calculate() const {

        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        ext::shared_ptr<StrikedTypePayoff> payoff =
            ext::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff);
        QL_REQUIRE(payoff, "non-striked payoff given");

        const Date exerciseDate = arguments_.exercise->lastDate();

        results_.value = calculator_->value(
                payoff->optionType(),
                payoff->strike(),
                discountCurve_->timeFromReference(exerciseDate))
            * discountCurve_->discount(exerciseDate);
    }

}


/**
 * \file SecondOrderSVFFilter.cpp
 */

#include <ATK/EQ/SecondOrderSVFFilter.h>

#include <cassert>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename SVFCoefficients>
  class SecondOrderSVFFilter<SVFCoefficients>::SVFState
  {
  public:
    typename SVFCoefficients::DataType iceq1 = 0;
    typename SVFCoefficients::DataType iceq2 = 0;
  };

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::SecondOrderSVFFilter(gsl::index nb_channels)
  :SVFCoefficients(nb_channels), state(std::make_unique<SVFState[]>(nb_channels))
  {
  }

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::~SecondOrderSVFFilter()
  {
  }

  template<typename SVFCoefficients>
  void SecondOrderSVFFilter<SVFCoefficients>::full_setup()
  {
    state = std::make_unique<SVFState[]>(nb_input_ports);
  }

  template<typename DataType>
  void SecondOrderSVFFilter<DataType>::process_impl(gsl::index size) const
  {
    assert(nb_input_ports == nb_output_ports);

    for(gsl::index j = 0; j < nb_input_ports; ++j)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[j];
      DataType* ATK_RESTRICT output = outputs[j];

      for(gsl::index i = 0; i < size; ++i)
      {
        DataType v3 = input[i] - state[j].iceq2;
        DataType v1 = a1 * state[j].iceq1 + a2 * v3;
        DataType v2 = state[j].iceq2 + a2 * state[j].iceq1 + a3 * v3;
        state[j].iceq1 = CoeffDataType(2) * v1 - state[j].iceq1;
        state[j].iceq2 = CoeffDataType(2) * v2 - state[j].iceq2;

        output[i] = m0 * input[i] + m1 * v1 + m2 * v2;
      }
    }
  }

  template<typename DataType>
  SecondOrderSVFBaseCoefficients<DataType>::SecondOrderSVFBaseCoefficients(gsl::index nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequencies must be positive");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFBaseCoefficients<DataType>::CoeffDataType SecondOrderSVFBaseCoefficients<DataType>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFBaseCoefficients<DataType>::CoeffDataType SecondOrderSVFBaseCoefficients<DataType>::get_Q() const
  {
    return Q;
  }

  template<typename DataType_>
  SecondOrderSVFLowPassCoefficients<DataType_>::SecondOrderSVFLowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFLowPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1/Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 0;
    m2 = 1;
  }

  template<typename DataType_>
  SecondOrderSVFBandPassCoefficients<DataType_>::SecondOrderSVFBandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFBandPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 1;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFHighPassCoefficients<DataType_>::SecondOrderSVFHighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFHighPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = -1;
  }

  template<typename DataType_>
  SecondOrderSVFNotchCoefficients<DataType_>::SecondOrderSVFNotchCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFNotchCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFPeakCoefficients<DataType_>::SecondOrderSVFPeakCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFPeakCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 2;
  }

  template<typename DataType_>
  SecondOrderSVFBellCoefficients<DataType_>::SecondOrderSVFBellCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {

  }

  template<typename DataType_>
  void SecondOrderSVFBellCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFBellCoefficients<DataType>::CoeffDataType SecondOrderSVFBellCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFBellCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q * gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain * gain - 1);
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFLowShelfCoefficients<DataType_>::SecondOrderSVFLowShelfCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFLowShelfCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFLowShelfCoefficients<DataType>::CoeffDataType SecondOrderSVFLowShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFLowShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain - 1);
    m2 = gain * gain - 1;
  }

  template<typename DataType_>
  SecondOrderSVFHighShelfCoefficients<DataType_>::SecondOrderSVFHighShelfCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFHighShelfCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFHighShelfCoefficients<DataType>::CoeffDataType SecondOrderSVFHighShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFHighShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q * gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = gain * gain;
    m1 = k * (1 - gain) * gain;
    m2 = 1 - gain * gain;
  }

#if ATK_ENABLE_INSTANTIATION
  template class SecondOrderSVFBaseCoefficients<float>;
  template class SecondOrderSVFBaseCoefficients<std::complex<float> >;
  template class SecondOrderSVFBaseCoefficients<std::complex<double> >;

  template class SecondOrderSVFLowPassCoefficients<float>;
  template class SecondOrderSVFLowPassCoefficients<std::complex<float> >;
  template class SecondOrderSVFLowPassCoefficients<std::complex<double> >;
  template class SecondOrderSVFBandPassCoefficients<float>;
  template class SecondOrderSVFBandPassCoefficients<std::complex<float> >;
  template class SecondOrderSVFBandPassCoefficients<std::complex<double> >;
  template class SecondOrderSVFHighPassCoefficients<float>;
  template class SecondOrderSVFHighPassCoefficients<std::complex<float> >;
  template class SecondOrderSVFHighPassCoefficients<std::complex<double> >;
  template class SecondOrderSVFNotchCoefficients<float>;
  template class SecondOrderSVFNotchCoefficients<std::complex<float> >;
  template class SecondOrderSVFNotchCoefficients<std::complex<double> >;
  template class SecondOrderSVFPeakCoefficients<float>;
  template class SecondOrderSVFPeakCoefficients<std::complex<float> >;
  template class SecondOrderSVFPeakCoefficients<std::complex<double> >;
  template class SecondOrderSVFBellCoefficients<float>;
  template class SecondOrderSVFBellCoefficients<std::complex<float> >;
  template class SecondOrderSVFBellCoefficients<std::complex<double> >;
  template class SecondOrderSVFLowShelfCoefficients<float>;
  template class SecondOrderSVFLowShelfCoefficients<std::complex<float> >;
  template class SecondOrderSVFLowShelfCoefficients<std::complex<double> >;
  template class SecondOrderSVFHighShelfCoefficients<float>;
  template class SecondOrderSVFHighShelfCoefficients<std::complex<float> >;
  template class SecondOrderSVFHighShelfCoefficients<std::complex<double> >;

  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<std::complex<double> > >;
#endif
  template class SecondOrderSVFBaseCoefficients<double>;

  template class SecondOrderSVFLowPassCoefficients<double>;
  template class SecondOrderSVFBandPassCoefficients<double>;
  template class SecondOrderSVFHighPassCoefficients<double>;
  template class SecondOrderSVFNotchCoefficients<double>;
  template class SecondOrderSVFPeakCoefficients<double>;
  template class SecondOrderSVFBellCoefficients<double>;
  template class SecondOrderSVFLowShelfCoefficients<double>;
  template class SecondOrderSVFHighShelfCoefficients<double>;

  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<double> >;
}


/**
 * File: random_var.cpp
 * Date: Mon Nov  9 10:31:35 CET 2020
 * Author: Open Risk  (www.openriskmanagement.com)
 *
 */

#include <cmath>
#include <iostream>
#include <cassert>

#include <Poco/JSON/JSON.h>
#include <Poco/JSON/Parser.h>
#include <armadillo>

#include "random_var.h"

using namespace Poco;

RandomVar &RandomVar::operator=(const RandomVar &R) {
    assert(R.size() == this->size()); // check that size matches
    for (size_t i = 0; i < R.size(); i++) {
        this->setX(i, R.getX(i));
        this->setP(i, R.getP(i));
        this->setC(i, R.getC(i));
    }
    return (*this);
};

/**
 * ... text ...
 */
void RandomVar::Sort() {
    arma::sort(m_S);
}

/**
 * ... text ...
 */
void RandomVar::Cumulative() {
    m_C[0] = m_P[0];
    for (size_t i = 1; i < m_P.size(); i++)
        m_C[i] = m_C[i - 1] + m_P[i];
}

/**
 * ... text ...
 */
void RandomVar::Probability() {
    m_P[0] = m_C[0];
    for (size_t i = 1; i < m_P.size(); i++)
        m_P[i] = m_C[i] - m_C[i - 1];
}

/**
 * ... text ...
 */
double RandomVar::Average() const {
    double expectation = 0.0;
    if (m_type == 0) {
        for (size_t i = 0; i < m_P.size(); i++) {
            expectation += m_P[i] * m_X[i];
        }
    } else if (m_type == 1) {
        for (size_t i = 0; i < m_S.size(); ++i) {
            expectation += m_S[i];
        }
        expectation /= m_S.size();
    }
    return expectation;
}

/**
 * ... text ...
 */
double RandomVar::Mean() const {
    return Average();
}

/**
 * ... text ...
 */
double RandomVar::Median() const {
    return Quantile(0.5);
}

/**
 * ... text ...
 */
double RandomVar::Variance() const {
    double var = 0;
    if (m_type == 0) {
        for (size_t i = 0; i < m_P.size(); i++)
            var += m_P[i] * m_X[i] * m_X[i];
        var -= Average() * Average();
    } else if (m_type == 1) {
        for (size_t i = 0; i < m_S.size(); i++)
            var += m_S[i] * m_S[i];
        var /= m_S.size();
        var -= Average() * Average();
    }
    return var;
}

/**
 * ... text ...
 */
double RandomVar::Vol() const {
    return sqrt(Variance());
}

/**
 * ... text ...
 */
double RandomVar::StandardDeviation() const {
    return Vol();
}

/**
 * ... text ...
 */
double RandomVar::Skeweness() const {
    double skew = 0;
    double mean = Average();
    for (size_t i = 0; i < m_P.size(); i++)
        skew += m_P[i] * pow(m_X[i] - mean, 3);
    skew = skew / pow(Variance(), 3 / 2);
    return skew;
}

/**
 * ... text ...
 */
double RandomVar::Kurtosis() const {
    double kurt = 0;
    double mean = Average();
    for (size_t i = 0; i < m_P.size(); i++)
        kurt += m_P[i] * pow(m_X[i] - mean, 4);
    kurt = kurt / pow(Variance(), 2);
    return kurt;
}

/**
 * ... text ...
 */
int RandomVar::Quantile_Index(double alpha) const {
    int index = 0;
    for (int i = 0; i < m_P.size(); i++) {
        if (m_C[i] > 1 - alpha) {
            index = i;
            break;
        }
    }
    return index;
}

/**
 * ... text ...
 */
double RandomVar::Quantile(double alpha) const {
    int index = this->Quantile_Index(alpha);
    return m_X[index];
}

/**
 * ... text ...
 */
double RandomVar::VaR(double alpha) const {
    return Quantile(1.0 - alpha);
}

/**
 * ... text ...
 */
double RandomVar::ExpectedShortFall(double alpha) const {
    int iVaR = this->Quantile_Index(alpha);
    double es = 0;
    for (int k = iVaR; k < m_P.size(); k++) {
        es += m_P[k] * m_X[k];
    }
    es /= alpha;
    return es;
}

/**
 * ... text ...
 */
double RandomVar::ExceedanceProbability(int index) const {
    double ep = 0;
    for (size_t k = index; k < m_P.size(); k++)
        ep += m_P[k];
    return ep;
}

/**
 * ... text ...
 */
double RandomVar::MeanExcess(int index) const {
    double alpha = ExceedanceProbability(index);
    double es = 0;
    for (size_t k = index; k < m_P.size(); k++)
        es += m_P[k] * m_X[k];
    es /= alpha;
    return es;
}

std::ostream &operator<<(std::ostream &os, const RandomVar &R) {
    std::ostringstream out;
    for (size_t k = 0; k < R.size(); k++)
        out << R.getX(k) << "\t" << R.getP(k) << "\t" << R.getC(k) << std::endl;
    return os << out.str();
};

void RandomVar::Print() {
    if (this->m_type == 1) {
        for (size_t s = 0; s < this->m_S.size(); s++) {
            cout << s << "\t" << this->m_S[s] << std::endl;
        }
    } else if (this->m_type == 0) {
        for (size_t s = 0; s < this->m_X.size(); s++) {
            cout << s << "\t" << this->m_X[s] << "\t" << this->m_P[s] << "\t" << this->m_C[s] << std::endl;
        }
    }
}

void RandomVar::ReadFromJSON(const char *fileName) {

    Poco::JSON::Parser loParser;
    std::ifstream t(fileName);
    std::stringstream buffer;
    buffer << t.rdbuf();
    std::string json = buffer.str();
    // Parse the JSON and get the Results
    Poco::Dynamic::Var loParsedJson = loParser.parse(json);
    Poco::Dynamic::Var loParsedJsonResult = loParser.result();

    // Random variable data are an array of objects
    //[
    // {"value": 1, "probability" : 0.2, "cumulative" : 0.2},
    // {"value": 2, "probability" : 0.2, "cumulative" : 0.4},
    // {"value": 3, "probability" : 0.2, "cumulative" : 0.6},
    // {"value": 4, "probability" : 0.2, "cumulative" : 0.8},
    // {"value": 5, "probability" : 0.2, "cumulative" : 1.0}
    //]

    Poco::JSON::Array::Ptr arr = loParsedJsonResult.extract<Poco::JSON::Array::Ptr>();
    size_t size = arr->size();
    cout << "Reading " << size << " records." << endl;

    m_P.resize(size);
    m_C.resize(size);
    m_X.resize(size);

    // Individual data rows
    Poco::JSON::Object::Ptr object;
    for (size_t i = 0; i < size; i++) {
        object = arr->getObject(i);
        this->setX(i, object->getValue<double>("value"));
        this->setP(i, object->getValue<double>("probability"));
        this->setC(i, object->getValue<double>("cumulative"));
    }

}