// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_LBFGS_HPP
#define MCL_LBFGS_HPP 1

#include <Eigen/Dense>
#include <vector>
#include <functional>

namespace mcl
{

// L-BFGS implementation based on Nocedal & Wright Numerical Optimization book (Section 7.2)
//
// Function pointers are meant to be used with lambdas, e.g.
//   LBFGS<MatrixXd> lbfgs;
//   lbfgs.gradient = [&](const MatrixXd &x, MatrixXd &g)->Scalar { return ... };
//   double obj = lbfgs.minimize(x);
//
// If the g arg is not sized, don't compute gradient
//
template <typename MatrixType>
class LBFGS
{
public:
    typedef typename MatrixType::Scalar Scalar;

	struct Options
	{
		int min_iters;
		int max_iters;
		Scalar abs_tol; // absolute tol if converged(...) not set
		Scalar rel_tol; // relative tol if converged(...) not set
		int M; // history window size
		Scalar gamma; // init Hessian = gamma * I
		Options() :
			min_iters(0),
			max_iters(100),
			abs_tol(1e-5),
			rel_tol(0),
			M(6),
			gamma(1)
			{}
	} options;

	LBFGS();

	// Output from the last call to minimize(...)
	int iters() const { return num_iters; }
	Scalar gamma() const { return gamma_k; }

	// Resizes buffers and sets to zero.
	// Called during minimize(...) ONLY if there is a change in dof or M.
	// Otherwise it's assumed you're picking up where you left off
	// on the previous call to minimize(...)
	void reset(int rows, int cols);

	// Required:
	// computes objective value and gradient
	//   obj = gradient(x, g)
	// If the g arg is not sized, don't compute gradient
	std::function<Scalar(const MatrixType&, MatrixType&)> gradient;

	// Optional:
	// Returns true if the solver should exit, default uses ||g||<abs_tol or ||g||<||x||rel_tol
	//   is_converged = converged(obj, x_prev, x, grad)
	std::function<bool(Scalar obj, const MatrixType&, const MatrixType&, const MatrixType&)> converged;

	// Optional:
	// Linesearch function, default uses bracketing weak wolfe (slow!)
	//   obj_k1 = (x, grad, descent, alpha)
	// Returns new objective value and updates both x AND gradient
	std::function<Scalar(MatrixType&, MatrixType&, const MatrixType&, Scalar&)> linesearch;

	// Optional:
	// Filter descent direction, p = B(p)
	// Otherwise p = gamma_k * p is used.
	std::function<void(MatrixType&)> filter;

	// Calls initialize(x) once and iterate(x) until converged
	Scalar minimize(MatrixType& x);

	// Initialize the solver
	void initialize(MatrixType& x);

	// Take an iteration
	// Returns objective
	Scalar iterate(MatrixType& x);

	// i.e. bisection with weak Wolfe conditions
	Scalar bracketing_weakwolfe(
		MatrixType& x,
		MatrixType& grad,
		const MatrixType& p,
		Scalar &alpha) const;

	// Used if converged not set
	// Returns true if:
	// grad.norm <= abs_tol
	// or
	// grad.norm() <= rel_tol * x.norm()
	bool default_converged(
		Scalar curr_obj,
		const MatrixType& xprev,
		const MatrixType& x,
		const MatrixType& grad) const;

	Scalar inner(
		const MatrixType &a,
		const MatrixType &b) const;

protected:
	bool initialized;
	int num_iters, max_iters, k;
	Scalar gamma_k, obj_0, obj_k;
	std::vector<MatrixType> s;
	std::vector<MatrixType> y;
	Eigen::Matrix<Scalar,Eigen::Dynamic,1> alpha;
	Eigen::Matrix<Scalar,Eigen::Dynamic,1> rho;
	MatrixType grad;
	MatrixType q;
	MatrixType descent;
	MatrixType grad_old;
	MatrixType x_old;
	MatrixType x_last;
	MatrixType s_temp;
	MatrixType y_temp;

}; // end class LBFGS

//
// Implementation
//

template <typename MatrixType>
LBFGS<MatrixType>::LBFGS() :
	initialized(false),
	num_iters(0),
	max_iters(0),
	k(0),
	gamma_k(1),
	obj_0(0),
	obj_k(0)
	{}

template <typename MatrixType>
void LBFGS<MatrixType>::reset(int rows, int cols)
{
    using namespace Eigen;
	num_iters = 0;
	max_iters = 0;
	k = 0;
	gamma_k = 1;
	obj_0 = std::numeric_limits<Scalar>::max();
	obj_k = std::numeric_limits<Scalar>::max();
	int M = options.M;
	s = std::vector<MatrixType>(M, MatrixType::Zero(rows,cols));
	y = std::vector<MatrixType>(M, MatrixType::Zero(rows,cols));
	alpha = VectorXd::Zero(M);
	rho = VectorXd::Zero(M);
	grad = MatrixType::Zero(rows, cols);
	q = MatrixType::Zero(rows, cols); // inv descent
	descent = q;
	grad_old = MatrixType::Zero(rows, cols);
	x_old = MatrixType::Zero(rows, cols);
	x_last = MatrixType::Zero(rows, cols);
	s_temp = MatrixType::Zero(rows, cols);
	y_temp = MatrixType::Zero(rows, cols);
}

// Returns number of iterations used
template <typename MatrixType>
typename LBFGS<MatrixType>::Scalar
LBFGS<MatrixType>::minimize(MatrixType &x)
{
	initialize(x);

	// Did we start at the initializer?
	if (num_iters >= options.min_iters &&
		converged(obj_k,x_last,x,grad))
	{
		num_iters = 1;
		return obj_k;
	}

	for (; k<max_iters; ++k)
	{
		iterate(x);
	} // end loop lbfgs iters

	return obj_k;

} // end minimize

template <typename MatrixType>
void LBFGS<MatrixType>::initialize(MatrixType& x)
{
	initialized = true;

	if (gradient == nullptr) {
	    throw std::runtime_error("no gradient function");
	}

	if (converged == nullptr)
	{
		using namespace std::placeholders;
		converged = std::bind(&LBFGS::default_converged, this, _1, _2, _3, _4);
	}

	if (linesearch == nullptr)
	{
		using namespace std::placeholders;
		linesearch = std::bind(&LBFGS::bracketing_weakwolfe, this, _1, _2, _3, _4);
	}

	// Resize/resize variables?
	if (alpha.rows() != options.M ||
		grad.rows() != x.rows() ||
		grad.cols() != x.cols()) {
		reset(x.rows(), x.cols());
	}

	num_iters = 0;
	max_iters = std::max(options.min_iters, options.max_iters);
	gamma_k = options.gamma;
	obj_0 = gradient(x, grad);
	obj_k = obj_0;
	k = 0;
}

template <typename MatrixType>
typename LBFGS<MatrixType>::Scalar
LBFGS<MatrixType>::iterate(MatrixType& x)
{
	if (!initialized) {
		throw std::runtime_error("not initialized");
	}

	x_old = x;
	grad_old = grad;
	q = grad;
	num_iters++;

	//
	// Two-loop recursion
	//
	{
		// L-BFGS first - loop recursion
		int iter = std::min(options.M, k);
		for(int i = iter - 1; i >= 0; --i)
		{
			Scalar denom = inner(s[i], y[i]);
			if (std::abs(denom) <= 0.0)
			{
				rho(i) = 0;
				alpha(i) = 0;
				continue;
			}
			rho(i) = 1.0 / denom;
			alpha(i) = rho(i)*inner(s[i], q);
			q -= alpha(i) * y[i];
		}

		if (filter != nullptr) { filter(q); }
		else { q = gamma_k*q; }

		// L-BFGS second - loop recursion
		for(int i = 0; i < iter; ++i)
		{
			Scalar beta = rho(i)*inner(q, y[i]);
			q += (alpha(i) - beta)*s[i];
		}
	}

	//
	// Perform step
	//
	{
		// If our hess approx is bad and we start going in
		// the wrong direction, restart memory
		Scalar step_size = 1.0;
		Scalar dir = inner(q, grad);
		if (dir <= 0)
		{
			q = grad;
			max_iters -= k; // Restart memory
			k = 0;
			step_size = std::min(1.0, 1.0 / grad.template lpNorm<Eigen::Infinity>() );
		}

		// We've hit local minima, we have to exit
		if (q.squaredNorm() <= 0.0) {
			return obj_k;
		}

		descent = -q;
		x_last = x;
		obj_k = linesearch(x, grad, descent, step_size);
		if (num_iters >= options.min_iters && converged(obj_k,x_last,x,grad)) {
			return obj_k;
		}
	}

	//
	// Correction term
	//
	{
		s_temp = x - x_old;
		y_temp = grad - grad_old;

		// update the history
		if (k < options.M)
		{
			s[k] = s_temp;
			y[k] = y_temp;
		}
		else
		{
			for (int i=0; i<options.M - 1; ++i)
			{
				s[i] = s[i+1];
				y[i] = y[i+1];
			}
			s.back() = s_temp;
			y.back() = y_temp;
		}

		Scalar denom = inner(y_temp, y_temp);
		if (std::abs(denom) > 0.0)
		{
			gamma_k = inner(s_temp, y_temp) / denom;
		}
	}

	return obj_k;
}

template <typename MatrixType>
typename LBFGS<MatrixType>::Scalar
LBFGS<MatrixType>::bracketing_weakwolfe(
		MatrixType& x,
		MatrixType& g,
		const MatrixType &drt,
		Scalar &step) const
{
    using namespace Eigen;

	Scalar c1 = 10e-4;
	Scalar c2 = 0.9;
	if (step <= 0.0) { step = 1.0; }
    int x_cols = x.cols();

	g = MatrixType::Zero(x.rows(), x_cols);
	MatrixType xp = x;
	const Scalar fx_init = gradient(x,g);
	Scalar fx = fx_init;

	Scalar dg_init = inner(g, drt);
    if (dg_init > 0) {
        throw std::runtime_error("direction increases objective");
    }

	const Scalar test_decr = c1 * dg_init;
	Scalar lower = 0;
	Scalar upper = std::numeric_limits<Scalar>::infinity();
	int maxiter = 200;
	int iter = 0;
	for (iter = 0; iter < maxiter; iter++)
	{
		x = xp + step * drt;
		fx = gradient(x, g);
		if (fx > fx_init + step * test_decr) { // Armijo rule
			upper = step;
		}
		else
		{
			Scalar dg = inner(g, drt);
			if(dg < c2 * dg_init){ // Weak wolfe
				lower = step;
			} else {
				break; // both met
			}
		}
		step = std::isinf(upper) ? 2*step : lower/2 + upper/2;
	}
	return fx;

} // end linesearch

template <typename MatrixType>
bool LBFGS<MatrixType>::default_converged(
		Scalar curr_obj,
		const MatrixType& xprev,
		const MatrixType& x,
		const MatrixType& g) const
{
	(void)(curr_obj);
	(void)(xprev);
	Scalar gnorm = g.norm();
	if (gnorm <= options.abs_tol) {
	    return true;
	}
	Scalar xnorm = x.norm();
	if (gnorm < options.rel_tol * xnorm) {
	    return true;
	}
	return false;
}

template <typename MatrixType>
typename LBFGS<MatrixType>::Scalar
LBFGS<MatrixType>::inner(
    const MatrixType &a,
    const MatrixType &b) const
{
    int cols = std::min(a.cols(), b.cols());
    Scalar dot = 0;
    for (int i=0; i<cols; ++i) {
        dot += a.col(i).dot(b.col(i));
    }
    return dot;
} // end inner product

} // end ns mcl

#endif


#ifndef PLAN_H
#define PLAN_H

#include <cmath>
#include "nubot/core/core.hpp"
#include "nubot/nubot_control/behaviour.hpp"
#include <boost/ptr_container/ptr_list.hpp>

using namespace std;
namespace nubot{
class WolrdModeliInfo
{
    public:
           WolrdModeliInfo()
               :active_robot_num_(0),is_robot_stuck_(false),ball_info_state_(0),
                 game_ctrl_(CTRL_STOP),indist_(0)
           {
               robot_pos_ = DPoint(0.0,0.0);
               robot_ori_ = Angle(0.0);
               robot_vel_ = DPoint(0.0,0.0);
               ball_pos_  = DPoint(0.0,0.0);
               ball_vel_  = DPoint(0.0,0.0);
               obs_pos_.reserve(10);
               target_    = DPoint(0.0,0.0);
               target_orientation_ = 0.0;
           }
           ~WolrdModeliInfo()
           {}
    public:

           int    active_robot_num_;   //current active robot number

           DPoint robot_pos_;          //robot position
           Angle  robot_ori_;          //robot orientation
           DPoint robot_vel_;          //robot velocity
           bool   is_robot_stuck_;     //stuck check

           DPoint ball_pos_;           //ball position
           DPoint ball_vel_;           //ball velocity
           int    ball_info_state_;    //ball information state    local ?  share ? or cannot see

           std::vector<DPoint> obs_pos_;            //obstacle information

           unsigned char game_ctrl_;   //current order

           double    indist_;
           DPoint    target_;             //target from coach
           float     target_orientation_; //the orientation of the specified target

};

    class Plan
    {
    public:
        Plan();
        /*******************catch ball******************/
        void
        traceBall();      // trace
        void
        interceptBall();  // intercept

        void
        catchBall();
        void
        catchBallSlowly();
        void
        catchBallForCoop();
        void
        catchMovingBall();
        void
        catchMotionlessBall();

        void
        positionAvoidObs(DPoint target, float theta, float stopdis, float stoptheta);

        DPoint
        avoidRelOble(DPoint target, double theta, double ro, double vel, bool includeball);
        void
        driblleControl(DPoint target,double acc,double sacc,double lvel,double maxvel);

        void
            move2Positionwithobs(DPoint target, float maxvel, float maxacc);  // move to the target point with obstacles avoidance
        void
            move2Positionwithobs_noball(DPoint target, float maxvel, float maxacc, bool avoid_ball=false);
        //subtargets
        int
        Min_num(int n,double *q);
        double
        Min(int n,double *q);
        int
        Max_num(int n,double *q);
        double
        Max(int n,double *q);
        void
        subtarget(double *obs,double *pos_robot,double *pos_target,double *pos_subtarget);
        void
        subtargets(DPoint target,DPoint robotpos);
        void
        subtargets_withball(DPoint target,DPoint robotpos);
        void
        subtargets2(DPoint target,DPoint robotpos);

        double
               CaculatediffS(double &s, double &rho,double &phid,
                             double v0,double c ,  double alpha ,
                             double p,double q,double  diffp,double diffq);
               double
               CaculatediffS(double &s, double &rho,double &phid,
                             double lamda,double v0,double p,
                             double q,double  diffp,double diffq);
               double
               CaculatePhid(double  rho,double basic_phid ,
                            double thetar , double eps);

              double
               Curvature(double diffp , double diffq, double double_diffp,double double_diffq);

           boost::ptr_list<DPoint> Active_ControlPointList;
               boost::ptr_list<DPoint> Passive_ControlPointList;

               double
               Bernstein(double s , int i , int N);
               void
               Bezier(double s,boost::ptr_list<DPoint> &controlpoint,
                      double &p ,double &q, double &diffp,double &diffq,
                      double &double_diffp, double &double_diffq); //  ¿ØÖÆµã £¬ ¿ŽÊÇŒžœ×µÄbezierÇúÏß ...

               void
               Bezier(double s,boost::ptr_list<DPoint> &controlpoint,
                      double &p ,double &q, double &diffp,double &diffq);
               bool
               BezierPathFollow( boost::ptr_list<DPoint> &controlpoint ,
                                 double v0,double c , double alpha ,double k,
                                 double lamda ,double eps); //s
               void
               FromPath2Trajectory(double &s,double &rtheta ,double vr,
                                   double wr,double diffp,double diffq,
                                   double double_diffp,double double_diffq,
                                   double vprofile ,double aprofile);//
               //bool
               //BezierTrajectoryTracking4Pass(boost::ptr_list<DPoint> &controlpoint ,double v0,double a);
                bool
                BezierTrajectoryTracking(boost::ptr_list<DPoint> &controlpoint ,double v0,double a);

                double
                PECrossBackMIdlleLine(double direction);
                double
                PEOutField(double diretion);
                double
                PEInOurPenaty(double direction);
                double
                PObleDirection4OurField(double direction, double predictlen,double cobledirection,
                                               double kobledirection);

                bool
                SearchMinPE4PassThroughforOurField(double &direction,double pridictlen,
                                                          DPoint trap[4],double step,int flg);

                bool
                SearchMinPE4PassThrough(double &direction,double pridictlen,
                                        DPoint trap[4],double step,int flg);

                bool
                IsNullInTrap(double direction,double swidth,double lwidth,double len);
                bool
                checkinOurField(DPoint mypos);
                bool
                checkinOppField(DPoint mypos);
                bool
                checkinOurPenalty(DPoint object);
                double
                PObleDirection(double direction, double predictlen, double cobledirection,double kobledirection);

                DPoint
                FindBstDirectionForAvoid(DPoint target);
                double
                FindBstDirectionForAvoid();
                double
                FindBstDirectionForAvoid2(DPoint target);
                double
                SearchDirectionforMinPEPoint(double oridirection,double step,int lefttime,int righttime);
                int
                GetAvoidState();

    public:
        Behaviour m_behaviour_;
        WolrdModeliInfo* worldmodelinfo_;

        DPoint   subtargets_pos_;
        DPoint   subtargets2_pos_;

        bool inourfield_;
        bool inoppfield_;
        double lastdirection;
        double pe1_;
        double pe2_;
        bool   isnull_;

        double bezier_s;
        bool   bezier_updateflag;
        double bezier_vr ,bezier_wr ;
        double bezier_vm ;
        double bezier_p ,bezier_q ,bezier_rtheta ;  //  ŽËŽŠÓÃµÄÊÇstatic £¬±ØÐëžÄ±ä²ÅÐÐ
        double last_bezier_rtheta;

        float kp;
        float kalpha;
        float kbeta;

        vector<DPoint> target_;

    public:
        bool   isinposition_;

    };
}
#endif // PLAN_H


#include <bitset>

#include <boost/utility/enable_if.hpp>
#include <boost/integer.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/integer/static_min_max.hpp>
#include <boost/integer_traits.hpp>

namespace boost {
	template <class T>
	class integer_traits<const T> : public integer_traits<T> { };
}

template <size_t N, class Type = void, class Enable = void>
struct is_power_of_two {
	static const bool value = false;
};
template <size_t N, class Type>
struct is_power_of_two<N, Type, typename boost::enable_if_c<(N & (N - 1)) == 0>::type> {
	static const bool value = true;
	typedef Type type;
};

template <size_t N, class Enable = void>
struct log_next_power_of_two {
	static const size_t value = boost::static_log2<N>::value + 1;
};
template <size_t N>
struct log_next_power_of_two<N, typename boost::enable_if<is_power_of_two<N> >::type> {
	static const size_t value = boost::static_log2<N>::value;
};

template <size_t N, class Enable = void>
struct next_power_of_two {
	static const size_t value = size_t(1) << (boost::static_log2<N>::value + 1);
};
template <size_t N>
struct next_power_of_two<N, typename boost::enable_if<is_power_of_two<N> >::type> {
	static const size_t value = N;
};

struct fast_integers {

private:

	template <size_t Bits, class Dummy = void> struct _impl { };
	template <class Dummy> struct _impl<8, Dummy> { typedef uint_fast8_t type; };
	template <class Dummy> struct _impl<16, Dummy> { typedef uint_fast16_t type; };
	template <class Dummy> struct _impl<32, Dummy> { typedef uint_fast32_t type; };
	template <class Dummy> struct _impl<64, Dummy> { typedef uint_fast64_t type; };
	template <class Dummy> struct _impl<128, Dummy> { typedef __uint128_t type; };

public:

	template <size_t Bits>
	struct bits : public _impl<boost::static_unsigned_max<8, next_power_of_two<Bits>::value>::value> { };

};

struct exact_integers {

private:

	template <size_t Bits, class Dummy = void> struct _impl { };
	template <class Dummy> struct _impl<8, Dummy> { typedef uint8_t type; };
	template <class Dummy> struct _impl<16, Dummy> { typedef uint16_t type; };
	template <class Dummy> struct _impl<32, Dummy> { typedef uint32_t type; };
	template <class Dummy> struct _impl<64, Dummy> { typedef uint64_t type; };
	template <class Dummy> struct _impl<128, Dummy> { typedef __uint128_t type; };

public:

	template <size_t Bits>
	struct bits : public _impl<boost::static_unsigned_max<8, next_power_of_two<Bits>::value>::value> { };

};

struct bitset_types {

	template <size_t Bits>
	struct bits {
		typedef std::bitset<Bits> type;
	};

};

/*!
 * Converter that rearranges bits in an integer.
 *
 * Conversion is reduced to a minimal number of mask & shift operations at compile-time.
 *
 * Usage:
 *
 *  bitset_converter<> is an empty converter list (cannot be used without adding at least one mappings).
 *  (list)::add::map<from, to&gt maps the from'th input bit to the to'th output bit.
 *
 *  Convenience function to add a continous region of mappings:
 *  bitset_converter<>::add::value<to2> is equivalent to bitset_converter<>::add::map<0, to2>
 *  (list)::add::map<from, to&gt::add::value<to2> is equivalent to ::add::map<from, to&gt::add::map<from + 1, to2>
 *
 *  Inut bits without a corresponding "from" entry are ignored.
 *  Output bit without a corresponding "to" entry are always zero.
 *
 *  The same input/output bit can appear in multiple mappings.
 *
 *  Invoke the converter: (list)::convert(integer)
 *
 * Limitations:
 *
 *  Input bits must fit in a native integer type provided by in_types::bits<bits>.
 *
 *  Output bits must fit in an integer type selected by out_types::bits<bits>.
 *
 * Example:
 *
 *  // Create a converter that swaps the first two bits, keeps the next one and ignores all others.
 *  typedef bitset_converter<>::add::map<0, 1>::add::map<1, 0>::add::value<2> Converter;
 *
 *  // Convert something.
 *  Converter::convert(3);
 *
 */
template <class out_types = fast_integers, class in_types = fast_integers>
struct bitset_converter {

private:

	typedef ptrdiff_t shift_type;
	typedef size_t index_type;

	template <class Combiner, class Entry>
	struct IterateEntries {
		static const typename Combiner::type value = Combiner::template combine<Entry, (IterateEntries<Combiner, typename Entry::next>::value)>::value;
	};
	template <class Combiner> struct IterateEntries<Combiner, void> { static const typename Combiner::type value = Combiner::base; };
	template <class Type, Type Base> struct Combiner { typedef Type type; static const Type base = Base; };

	template<class Getter, class Type>
	struct MaxCombiner : public Combiner<Type, boost::integer_traits<Type>::const_min> {
		template <class Entry, Type accumulator>
		struct combine { static const Type value = boost::static_signed_max<Getter::template get<Entry>::value, accumulator>::value; };
	};

	template<class Getter, class Type>
	struct MinCombiner : public Combiner<Type, boost::integer_traits<Type>::const_max> {
		template <class Entry, Type accumulator>
		struct combine { static const Type value = boost::static_signed_min<Getter::template get<Entry>::value, accumulator>::value; };
	};

	struct ShiftGetter { template<class Entry> struct get { static const shift_type value = Entry::shift; }; };
	struct FromGetter { template<class Entry> struct get { static const index_type value = Entry::from; }; };
	struct ToGetter { template<class Entry> struct get { static const index_type value = Entry::to; }; };

	template<shift_type Shift, class Type>
	struct ShiftMaskCombiner : public Combiner<Type, Type(0)> {
		template <class Entry, Type mask>
		struct combine { static const Type value = mask | ( (Entry::shift == Shift) ? (Type(1) << Entry::from) : Type(0) ); };
	};

	template<class List>
	struct Builder;

	template<index_type From, index_type To, class Next = void>
	struct Entry {

		typedef Entry<From, To, Next> This;

		static const index_type from = From;
		static const index_type to = To;
		typedef Next next;

		static const shift_type shift = shift_type(from) - shift_type(to);

		static const shift_type max_shift = IterateEntries<MaxCombiner<ShiftGetter, shift_type>, This>::value;
		static const shift_type min_shift = IterateEntries<MinCombiner<ShiftGetter, shift_type>, This>::value;

		static const index_type in_bits = IterateEntries<MaxCombiner<FromGetter, index_type>, This>::value + 1;
		typedef typename in_types::template bits<in_bits>::type in_type;

		static const index_type out_bits = IterateEntries<MaxCombiner<ToGetter, index_type>, This>::value + 1;
		typedef typename out_types::template bits<out_bits>::type out_type;

		template<shift_type Shift>
		struct ShiftMask { static const in_type value = IterateEntries<ShiftMaskCombiner<Shift, in_type>, This>::value; };

		template <shift_type Shift>
		inline static typename boost::enable_if_c<(Shift >= shift_type(0)), out_type>::type evaluate(in_type value) {
			return out_type((value & ShiftMask<Shift>::value) >> Shift);
		}
		template <shift_type Shift>
		inline static typename boost::enable_if_c<(Shift < shift_type(0)), out_type>::type evaluate(in_type value) {
			return out_type(value & ShiftMask<Shift>::value) << (-Shift);
		}

		template<shift_type Shift, class Enable = void>
		struct NextShift { static const shift_type value = Shift + 1; };
		template<shift_type Shift>
		struct NextShift<Shift, typename boost::enable_if_c<Shift != max_shift && ShiftMask<Shift + 1>::value == in_type(0)>::type > {
			static const shift_type value = NextShift<Shift + 1>::value;
		};

		template <shift_type Shift>
		inline static typename boost::enable_if_c<(NextShift<Shift>::value != max_shift + 1), out_type>::type map(in_type value) {
			return evaluate<Shift>(value) | (map<NextShift<Shift>::value>(value));
		}
		template <shift_type Shift>
		inline static typename boost::enable_if_c<(NextShift<Shift>::value == max_shift + 1), out_type>::type map(in_type value) {
			return evaluate<Shift>(value);
		}

	public:

		typedef Builder<This> add;

		static out_type convert(in_type value) {
			return map<min_shift>(value);
		}

	};

	template<class List>
	struct Builder {

		template<index_type From, index_type To>
		struct map : public Entry<From, To, List> { };

		template<index_type To, class Current = List>
		struct value : public Entry<Current::from + 1, To, Current> { };

		template<index_type To>
		struct value<To, void> : public Entry<0, To> { };

	};

public:

	typedef Builder<void> add;

};


#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <time.h>

// OpenCV headers
#include <cv.h>
#include <highgui.h>

#include <opencv2/contrib/contrib.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

#include <opencv_workbench/syllo/syllo.h>
//
#include <opencv_workbench/utils/AnnotationParser.h>
#include <opencv_workbench/plot/Plot.h>
#include <opencv_workbench/track/Dynamics.h>
#include <opencv_workbench/track/KalmanFilter.h>
#include <opencv_workbench/track/EKF.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

using std::cout;
using std::endl;

int main(int argc, char *argv[])
{
     cout << "Kalman Filter Test" << endl;

     double t0 = 0;
     double dt = 0.1;
     double tend = 10;

     // Setup boost RNG
     boost::mt19937 rng;
     boost::normal_distribution<> nd(0,1.0);
     boost::variate_generator<boost::mt19937&,
                              boost::normal_distribution<> > var_nor(rng,nd);

     ///////////////////////////////////////////////////////
     // Setup Cart Model
     Dynamics::state_5d_type input;
     input[0] = 5;
     input[1] = 3.14159265359/20;//0

     Dynamics dyn;
     dyn.set_time(t0, dt, tend);
     dyn.set_model(Dynamics::cart);
     dyn.set_input(input);
     dyn.set_process_noise(0.0);
     dyn.set_measurement_noise(0.1);

     dyn.compute_trajectory();
     std::vector<cv::Point2d> truth = dyn.truth_points();
     std::vector<cv::Point2d> measured = dyn.measured_points();

     ////////////////////////////////////////////////////////
     // Setup Kalman Filter
     // Kalman filter setup...

     syllo::KalmanFilter kf;
     Eigen::MatrixXf A;     // system matrix
     Eigen::MatrixXf B;     // input matrix
     Eigen::MatrixXf H;     // measurement matrix
     Eigen::MatrixXf Q;     // process noise matrix
     Eigen::MatrixXf R;     // measurement noise matrix
     Eigen::MatrixXf x0;    // initial position
     Eigen::MatrixXf covar; // initial state covariance matrix

     A.resize(4,4);
     B.resize(4,2);
     H.resize(2,4);
     Q.resize(4,4);
     R.resize(2,2);
     x0.resize(4,1);
     covar.resize(4,4);

     double T = dt;
     A << 1, 0, T, 0,
          0, 1, 0, T,
          0, 0, 1, 0,
          0, 0, 0, 1;

     B << 0, 0,
          1, 0,
          0, 0,
          0, 1;

     H << 1, 0, 0, 0,
          0, 1, 0, 0;

     Q = Eigen::MatrixXf::Identity(A.rows(), A.cols()) * 1e-4;

     //R << 10, 0,
     //     0, 10;
     R << .001, 0,
          0, .001;

     //x = [x, y, x_dot, y_dot]
     x0 << 0,
           0,
           0,
           0;

     covar << 0.1, 0, 0, 0,
              0, 0.1, 0, 0,
              0, 0, 0.1, 0,
              0, 0, 0, 0.1;

     kf.setModel(A,B,H,Q,R);
     kf.init(x0, covar);

     // Setup OpenCV's kalman filter tracker
     cv::KalmanFilter opencv_kf = cv::KalmanFilter(4, 2, 0);
     cv::Mat_<float> transition_matrix = cv::Mat_<float>(4,4);
     transition_matrix  << 1,0,T,0,
                           0,1,0,T,
                           0,0,1,0,
                           0,0,0,1;

     opencv_kf.transitionMatrix = transition_matrix;

     opencv_kf.statePre.at<float>(0) = 0;
     opencv_kf.statePre.at<float>(1) = 0;
     opencv_kf.statePre.at<float>(2) = 2;
     opencv_kf.statePre.at<float>(3) = 0;
     cv::setIdentity(opencv_kf.measurementMatrix);
     cv::setIdentity(opencv_kf.processNoiseCov, cv::Scalar::all(1e-4));
     cv::setIdentity(opencv_kf.measurementNoiseCov, cv::Scalar::all(0.001));
     cv::setIdentity(opencv_kf.errorCovPost, cv::Scalar::all(0.1));

     /////////////////////////////////////////////////////////
     // Process the measured points with the kalman filter
     std::vector<cv::Point2d> kf_points;
     std::vector<cv::Point2d> opencv_kf_points;
     std::vector<cv::Point2d> kf_var_x;
     std::vector<cv::Point2d> kf_var_y;
     std::vector<cv::Point2d> error;
     std::vector<cv::Point2d>::iterator it = measured.begin();
     std::vector<cv::Point2d>::iterator it_truth = truth.begin();
     double t = t0;
     for(; it != measured.end(); it++) {
          Eigen::MatrixXf u, z;
          u.resize(2,1);
          u << 0,
               0;

          z.resize(2,1);
          z << it->x , it->y;

          kf.predict(u);
          kf.update(z);

          //////////////////////////////////////////
          // Opencv
          cv::Mat prediction = opencv_kf.predict();
          cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

          // correct tracker update using the newly computed centroid.
          cv::Mat_<float> measurement(2,1);
          measurement(0) = it->x;
          measurement(1) = it->y;

          cv::Mat estimated = opencv_kf.correct(measurement);
          cv::Point2d est_centroid = cv::Point2d(estimated.at<float>(0),estimated.at<float>(1));
          opencv_kf_points.push_back(est_centroid);
          ////////////////////////////////////////////

          Eigen::MatrixXf state = kf.state();
          kf_points.push_back(cv::Point2d(state(0,0),state(1,0)));

          Eigen::MatrixXf covar = kf.covariance();
          kf_var_x.push_back(cv::Point2d(t,covar(0,0)));
          kf_var_y.push_back(cv::Point2d(t,covar(1,1)));

          double err = sqrt( pow(state(0,0) - it_truth->x, 2) + pow(state(1,0) - it_truth->y, 2) );
          error.push_back(cv::Point2d(t,err));

          it_truth++;
          t += dt;

#if 0
          std::string temp;
          std::cin >> temp;
#endif
     }

     /////////////////////////////////////////////////////////
     // Plot the tracks
     std::vector< std::vector<cv::Point2d> > vectors;
     const std::string title = "Tracks";
     std::vector<std::string> labels;
     std::vector<std::string> styles;

     vectors.push_back(truth);
     labels.push_back("Truth");
     styles.push_back("points");

     vectors.push_back(measured);
     labels.push_back("Measured");
     styles.push_back("points");

     vectors.push_back(kf_points);
     labels.push_back("KF");
     styles.push_back("linespoints");

     vectors.push_back(opencv_kf_points);
     labels.push_back("OPENCV KF");
     styles.push_back("linespoints");

     syllo::Plot plot;
     plot.plot(vectors, title, labels, styles);

     ////////////////////////////////////////////////////
     vectors.clear() ; labels.clear(); styles.clear();
     const std::string title_covar = "Covariance";

     vectors.push_back(kf_var_x);
     labels.push_back("X Var");
     styles.push_back("points");

     vectors.push_back(kf_var_y);
     labels.push_back("Y Var");
     styles.push_back("points");

     vectors.push_back(error);
     labels.push_back("Error");
     styles.push_back("points");

     syllo::Plot plot_covar;
     plot_covar.plot(vectors, title_covar, labels, styles);
     return 0;
}


#pragma once

#include <Eigen/Core>
#include <memory>
#include "fastenvelope/FastEnvelope.h"

namespace GEO {
class MeshFacetsAABBWithEps;
class Mesh;
} // namespace GEO

namespace sample_envelope {
class SampleEnvelope
{
public:
    SampleEnvelope(bool exact = false)
        : use_exact(exact){};
    double eps2 = 1e-6;
    double sampling_dist = 1e-3;
    bool use_exact = false;
    void init(
        const std::vector<Eigen::Vector3d>& m_ver,
        const std::vector<Eigen::Vector3i>& m_faces,
        const double);
    bool is_outside(const std::array<Eigen::Vector3d, 3>& tris);
    bool is_outside(const Eigen::Vector3d& pts);

private:
    std::shared_ptr<GEO::MeshFacetsAABBWithEps> geo_tree_ptr_;
    std::shared_ptr<GEO::Mesh> geo_polyhedron_ptr_;
    std::vector<int> geo_vertex_ind;
    std::vector<int> geo_face_ind;
private:
    fastEnvelope::FastEnvelope exact_envelope;
};
} // namespace sample_envelope


#include <algorithm>
#include <cmath>
#include <vector>
#include "kws.h"
#include "ctc_utils.h"

namespace {

static const float neginf = -std::numeric_limits<float>::infinity();

inline float log_add(float a, float b) {
    if (a == neginf) return b;
    if (b == neginf) return a;
    if (a > b)
        return log1p(exp(b-a)) + a;
    else
        return log1p(exp(a-b)) + b;
}

}

/* Computes forward probabilities for a given keyword.
 * Scores keyword as *keyword* allowing for arbitrary
 * pre and post characters.
 * *NB* undefined if keyword is empty string.
 */
float cscore_kws(const float* probs,
                 const int T, const int alphabet_size,
                 const int blank,
                 const std::vector<int>& labels) {

    std::vector<int> labels_w_blanks;
    std::vector<int> e_inc;
    std::vector<int> s_inc;
    int repeats = setup_labels(labels, blank, labels_w_blanks,
                               s_inc, e_inc);

    const int S = labels_w_blanks.size();
    float* prev_alphas = new float[S];
    float* next_alphas = new float[S];

    std::fill(prev_alphas, prev_alphas + S, neginf);

    int start =  (((S /2) + repeats - T) < 0) ? 0 : 1,
            end = S > 1 ? 2 : 1;

    for (int i = start; i < end; ++i) {
        if (i == 0) {
            prev_alphas[i] = std::log(1 - probs[labels_w_blanks[1]]);
        } else {
            int l = labels_w_blanks[i];
            prev_alphas[i] = std::log(probs[l]);
        }
    }

    for(int t = 1; t < T; ++t) {
        std::fill(next_alphas, next_alphas + S, neginf);

        int remain = (S / 2) + repeats - (T - t);
        if(remain >= 0)
            start += s_inc[remain];
        if(t <= (S / 2) + repeats)
            end += e_inc[t - 1];
        int startloop = start;
        int idx = t * alphabet_size;

        if (start == 0) {
            float star_score = std::log(1 - probs[idx + labels_w_blanks[1]]);
            next_alphas[0] = prev_alphas[0] + star_score;
            startloop += 1;
        }

        for(int i = startloop; i < end; ++i) {
            int l = labels_w_blanks[i];
            float prev_sum = log_add(prev_alphas[i], prev_alphas[i-1]);

            // Skip two if not on blank and not on repeat.
            if (l != blank && i != 1 &&
                    l != labels_w_blanks[i-2])
                prev_sum = log_add(prev_sum, prev_alphas[i-2]);

            next_alphas[i] = prev_sum;
            if (i == labels_w_blanks.size() - 1) {
                float nl_score = probs[idx + labels_w_blanks[i-1]];
                next_alphas[i] += std::log(1 - nl_score);
            } else {
                next_alphas[i] += std::log(probs[l + idx]);
            }
        }
        std::swap(prev_alphas, next_alphas);
    }

    float loglike = neginf;
    for(int i = start; i < end; ++i) {
        loglike = log_add(loglike, prev_alphas[i]);
    }

    // Cleanup
    delete[] prev_alphas;
    delete[] next_alphas;

    return -loglike;
}

#ifdef PYTHON
#include <boost/python.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/numeric.hpp>
#include <numpy/noprefix.h>

namespace py = boost::python;
namespace np = boost::python::numeric;

float score_kws(np::array probs, py::list labels,
                const int blank) {
    // *NB* logits must be type float and row-major.
    py::object shape = probs.attr("shape");

    unsigned int time = py::extract<unsigned int>(shape[0]);
    unsigned int num_classes = py::extract<unsigned int>(shape[1]);

    float* data = static_cast<float*>(PyArray_DATA(probs.ptr()));

    std::vector<int> label_vec;
    for (int i = 0; i < len(labels); i++)
        label_vec.push_back(py::extract<int>(labels[i]));

    return cscore_kws(data, time, num_classes,
                      blank, label_vec);
}

BOOST_PYTHON_MODULE(kws) {
    np::array::set_module_and_type("numpy", "ndarray");
    py::def("score_kws", &score_kws);
}
#endif


#include <vector>
#include <string>
#include <iostream>
#include <limits>
#include <typeinfo>
#include <iomanip>
std::string demangle(const char *name);

#ifdef __GNUC__
#include <cxxabi.h>
std::string demangle(const char *name) {
    int     status = -1;
    char   *realname;
    realname = abi::__cxa_demangle(name, 0, 0, &status);

    std::string res(name);

    if(status == 0){
        res.assign(realname);
        delete realname;
    };

    return res;
}
#else
#include <boost/core/demangle.hpp>

std::string demangle(const char *name) {
    return boost::core::demangle(name);
}
#endif

#define type_desc2(t)  #t
#define type_desc(t) type_desc2(t)

#define TYPE size_t

template<class T>
size_t type_string_length_info(std::ostream& os, const char* name_from_preprocessor=0)
{
    os << "type: ";
    if(name_from_preprocessor)
        os<<name_from_preprocessor<<" ==> ";
    os<<typeid(T).name()<<" ==> "<<demangle(typeid(T).name());
    os <<"\n  size in bytes: "<<sizeof(T);
    if(std::numeric_limits<T>::is_specialized) {

        size_t max_string_representation_length = ((std::numeric_limits<T>::is_integer)?std::numeric_limits<T>::digits10:std::numeric_limits<T>::max_digits10)+1+std::numeric_limits<T>::is_signed;

        os
        <<"\n                "<<'<'<<std::string(max_string_representation_length-2,'-')<<'>'
        <<"\n  min value:    "<<std::numeric_limits<T>::min()
        <<"\n  max value:    "<<std::numeric_limits<T>::max()
        <<"\n  is signed:    "<<(std::numeric_limits<T>::is_signed?"yes":"no")
        <<"\n  digits10:     "<<std::numeric_limits<T>::digits10
        <<"\n  digits:       "<<std::numeric_limits<T>::digits
        <<"\n  max digits10: "<<std::numeric_limits<T>::max_digits10
        <<"\n  radix:        "<<std::numeric_limits<T>::radix
        << '\n'
        << '\n';
        return max_string_representation_length;
    } else {
        os<<" have no info about type\n";
    }
    return 0;
}

#define  TYPE_STR_INFO(t) type_string_length_info<t>(std::cout, type_desc(t));

int main()
{
    TYPE_STR_INFO(int);
    TYPE_STR_INFO(char);
    TYPE_STR_INFO(unsigned char);
    TYPE_STR_INFO(unsigned long long);
    TYPE_STR_INFO(size_t);
    TYPE_STR_INFO(ptrdiff_t);
    //TYPE_STR_INFO(std::nullptr_t);

    return 0;
}


#ifndef COVMAT_HPP
#define COVMAT_HPP

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;

/*
 * This class stores a precision matrix, along with some functionals
 * of it used to compute the pdf of a normal r.v.
 */
class PrecMat
{
protected:
    MatrixXd prec;
    LLT<MatrixXd> cho_factor;
    MatrixXd cho_factor_eval;
    double log_det;

public:
    PrecMat() {}
    ~PrecMat() {}

    PrecMat(const MatrixXd &prec);

    MatrixXd get_prec() const;

    LLT<MatrixXd> get_cho_factor() const;

    const MatrixXd &get_cho_factor_eval() const;

    double get_log_det() const;
};

std::ostream &operator<<(std::ostream &output, const PrecMat &p);

#endif


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

/**
 * @date 2015
 * @author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * Max-Planck-Institute for Intelligent Systems
 */

#include <gtest/gtest.h>

#include <Eigen/Core>

#include <fl/util/math/linear_algebra.hpp>

#include <fl/filter/particle/particle_filter.hpp>
#include <fl/filter/gaussian/gaussian_filter_linear.hpp>
#include <fl/model/transition/linear_transition.hpp>
#include <fl/model/sensor/linear_gaussian_sensor.hpp>

template<typename Vector, typename Matrix>
bool moments_are_similar(Vector mean_a, Matrix cov_a,
                         Vector mean_b, Matrix cov_b, fl::Real epsilon = 0.1)
{
    Matrix cov_delta = cov_a.inverse() * cov_b;
    bool are_similar = cov_delta.isApprox(Matrix::Identity(), epsilon);

    Matrix square_root = fl::matrix_sqrt(cov_a);
    fl::Real max_mean_delta =
            (square_root.inverse() * (mean_a-mean_b)).cwiseAbs().maxCoeff();

    are_similar = are_similar && max_mean_delta < epsilon;

    return are_similar;
}

Eigen::Matrix<fl::Real, 3, 3> some_rotation()
{
    fl::Real angle = 2 * M_PI * fl::Real(rand()) / fl::Real(RAND_MAX);

    Eigen::Matrix<fl::Real, 3, 3> R = Eigen::Matrix<fl::Real, 3, 3>::Identity();

    R = R * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
    R = R * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
    R = R * Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY());
    return R;
}

class ParticleFilterTest
    : public ::testing::Test
{
protected:

    typedef Eigen::Matrix<fl::Real, 3, 1> State;
    typedef Eigen::Matrix<fl::Real, 3, 1> Observation;
    typedef Eigen::Matrix<fl::Real, 3, 1> Input;

    typedef Eigen::Matrix<fl::Real, 3, 3> Matrix;

    typedef fl::LinearTransition<State, State, Input> Transition;
    typedef fl::LinearGaussianSensor<Observation, State> Sensor;

    // particle filter
    typedef fl::ParticleFilter<Transition, Sensor> ParticleFilter;
    typedef ParticleFilter::Belief ParticleBelief;

    // gaussian filter
    typedef fl::GaussianFilter<Transition, Sensor> GaussianFilter;
    typedef GaussianFilter::Belief GaussianBelief;

    ParticleFilterTest()
        : transition(create_transition()),
          sensor(create_sensor()),
          particle_filter(transition, sensor),
          gaussian_filter(transition, sensor)

    {
        N_particles = 10000;
        N_steps = 10;
        delta_time = 1;

        // create intial beliefs
        gaussian_belief.set_standard();
        particle_belief.from_distribution(gaussian_belief, N_particles);
    }

    Transition create_transition()
    {
        srand(0);

        Transition transition;

        transition.dynamics_matrix(some_rotation());

        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<fl::Real, 3>(1, 3.5, 1.2);

        transition.noise_matrix(R*D);

        return transition;
    }

    Sensor create_sensor()
    {
        srand(0);

        Sensor sensor;

        sensor.sensor_matrix(some_rotation());

        Matrix R = some_rotation();
        Matrix D = Eigen::DiagonalMatrix<fl::Real, 3>(3.1, 1.0, 1.3);

        sensor.noise_covariance(R*D*R.transpose());

        return sensor;
    }

    Transition transition;
    Sensor sensor;

    ParticleFilter particle_filter;
    GaussianFilter gaussian_filter;

    GaussianBelief gaussian_belief;
    ParticleBelief particle_belief;

    size_t N_particles;
    size_t N_steps;
    size_t delta_time;

};

TEST_F(ParticleFilterTest, predict)
{
    // run prediction
    for(size_t i = 0; i < N_steps; i++)
    {
        particle_filter.predict(particle_belief, Input::Zero(), particle_belief);
        gaussian_filter.predict(gaussian_belief, Input::Zero(), gaussian_belief);

        EXPECT_TRUE(moments_are_similar(
                        particle_belief.mean(), particle_belief.covariance(),
                        gaussian_belief.mean(), gaussian_belief.covariance()));
    }
}

TEST_F(ParticleFilterTest, update)
{
    // run prediction
    for(size_t i = 0; i < N_steps; i++)
    {
        Observation observation(0.5, 0.5, 0.5);

        particle_filter.update(particle_belief, observation, particle_belief);
        gaussian_filter.update(gaussian_belief, observation, gaussian_belief);

        EXPECT_TRUE(moments_are_similar(
                        particle_belief.mean(), particle_belief.covariance(),
                        gaussian_belief.mean(), gaussian_belief.covariance()));
    }
}

TEST_F(ParticleFilterTest, predict_and_update)
{
    fl::StandardGaussian<State> standard_gaussian;
    State state = gaussian_belief.sample();
    // run prediction
    for(size_t i = 0; i < N_steps; i++)
    {
        // simulate system
        state = transition.state(state,
                                    standard_gaussian.sample(),
                                    Input::Zero());
        Observation observation =
                sensor.observation(state, standard_gaussian.sample());

        // predict
        particle_filter.predict(particle_belief, State::Zero(), particle_belief);
        gaussian_filter.predict(gaussian_belief, State::Zero(), gaussian_belief);

        // update
        particle_filter.update(particle_belief, observation, particle_belief);
        gaussian_filter.update(gaussian_belief, observation, gaussian_belief);
    }

    State delta = particle_belief.mean() - gaussian_belief.mean();
    fl::Real mh_distance = delta.transpose() * gaussian_belief.precision() * delta;

    // make sure that the estimate of the pf is within one std dev
    EXPECT_TRUE(std::sqrt(mh_distance) <= 1.0);
}


/**************************************************************************
** This file is a part of our work (Siggraph'16 paper, binary, code and dataset):
**
** Roto++: Accelerating Professional Rotoscoping using Shape Manifolds
** Wenbin Li, Fabio Viola, Jonathan Starck, Gabriel J. Brostow and Neill D.F. Campbell
**
** w.li AT cs.ucl.ac.uk
** http://visual.cs.ucl.ac.uk/pubs/rotopp
**
** Copyright (c) 2016, Wenbin Li
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions are met:
**
** -- Redistributions of source code and data must retain the above
**    copyright notice, this list of conditions and the following disclaimer.
** -- Redistributions in binary form must reproduce the above copyright
**    notice, this list of conditions and the following disclaimer in the
**    documentation and/or other materials provided with the distribution.
**
** THIS WORK AND THE RELATED SOFTWARE, SOURCE CODE AND DATA IS PROVIDED BY
** THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
** WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
** MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
** NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
** INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
** BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
** USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
** NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
** EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#ifndef GPLVM_HPP
#define GPLVM_HPP

#include "ceres/ceres.h"
#include "glog/logging.h"
#include "ceres/solver.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/Eigenvalues>

#include <array>
#include <random>
#include <iostream>
#include <cmath>
#include <fstream>
#include <cassert>
#include <chrono>

#include "include/rotoSolver/kernel.hpp"
#include "include/rotoSolver/baseDefs.hpp"

using namespace std;

class NoiseHyperPriorFunction
{
public:
    virtual double getHyperPrior(const double logBeta) const = 0;
    virtual double getHyperPriorDerivative(const double logBeta) const = 0;
};

class GaussianNoiseHyperPriorFunction : public NoiseHyperPriorFunction
{
private:
    double _logOffset;
    double _logScaleSq;

public:
    GaussianNoiseHyperPriorFunction(double logOffset = 0.0, double logScale = 1.0) :
        _logOffset(logOffset), _logScaleSq(logScale * logScale)
    {}

    virtual double getHyperPrior(const double logBeta) const
    {
        double b = logBeta - _logOffset;
        return (0.5 * _logScaleSq * b * b);
    }

    virtual double getHyperPriorDerivative(const double logBeta) const
    {
        return (_logScaleSq * logBeta);
    }
};

class GPLVM {
public:
    typedef Eigen::VectorXd Vector;
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::SparseMatrix<double> SparseMatrix;
    explicit GPLVM(const Matrix& Y, const int Q,
                   shared_ptr<Kernel> kernel,
                   const double initBeta = 1,
                   shared_ptr<NoiseHyperPriorFunction> noisePriorFunc =
     shared_ptr<NoiseHyperPriorFunction>(new GaussianNoiseHyperPriorFunction())) :
        _Y(Y), _N(_Y.rows()), _Q(Q), _kernel(kernel), _beta(initBeta),
        _noisePriorFunc(noisePriorFunc)
    {
        assert (_N > 0);
        assert (_Q > 0);

        const double N = double(_Y.rows());

        _Ymean = _Y.array().colwise().mean();
        _Y = _Y.array().rowwise() - _Ymean.array().colwise().mean();
        _Ystd = (_Y.array().pow(2.0).colwise().sum() / (N-1.0)).sqrt();
        _Y = _Y.array().rowwise() / _Ystd.array().colwise().mean();

#ifdef CHECK_NORMALISATION
        {
            vdbg(_Ymean);
            vdbg(_Ystd);
            vdbg(_Y.transpose());

            Matrix yy_mean = _Y;
            yy_mean = yy_mean.array().rowwise() * _Ystd.array().colwise().mean();
            yy_mean = yy_mean.array().rowwise() + _Ymean.array().colwise().mean();

            vdbg((yy_mean - Y).norm());
        }
#endif // CHECK_NORMALISATION

        _X = GetDefaultXInit();

        resetCachedValues();
    }

    void Print() const
    {
        std::cout << "GPLVM: ";
        std::cout << " N = " << _N;
        std::cout << " Q = " << _Q;
        std::cout << " beta = " << _beta << std::endl;
        _kernel->Print();
    }

    void SetInternalsFromMatlabStructure(std::map<string, shared_ptr<Eigen::MatrixXd> >& S)
    {
        _Y = *S["Y"];
        _Ymean = *S["Y_mean"];
        _Ystd = *S["Y_std"];

        _X = *S["X"];

        resetCachedValues();
    }

    inline int NumParameters() const
    {
        return NumPreDataParameters() + (_N * _Q);
    }

    inline int NumPreDataParameters() const
    {
        return (_kernel->NumberOfParameters() + 1);
    }

    int GetN() const { return _N; }

    int GetQ() const { return _Q; }

    // Cannot return ref due to normalisation so don't call too often..
    Matrix GetY() const
    {
        Matrix Y(_Y);
        Y = Y.array().rowwise() * _Ystd.array().colwise().mean();
        Y = Y.array().rowwise() + _Ymean.array().colwise().mean();
        return Y;
    }

    Matrix GetX() const { return _X; }

    double GetBeta() const { return _beta; }

    void SetBeta(const double newBeta)
    {
        _beta = newBeta;

        resetCachedValues();
    }

    template<typename Derived>
    Matrix Predict(const Eigen::MatrixBase<Derived>& x,
                   Vector* y_var_ptr = NULL,
                   std::vector<Matrix>* dy_dx_vec_ptr = NULL) const
    {
        assert (x.cols() == _Q);
        assert (x.rows() > 0);

        Matrix K_x_X = _kernel->GetKernelMatrix(x, _X, NULL);

        const Matrix& KinvY = GetKinvY();

        Matrix y_mean = K_x_X * KinvY;

        if (y_var_ptr) {
            Vector& y_var = *(y_var_ptr);
            y_var.resize(x.rows());

            const Matrix& Kinv = GetKinv();

            Vector K_xx_xx_diag = _kernel->GetDiagonalOfKernel(x);

            y_var = K_xx_xx_diag.array() -
                    (K_x_X.array() * (K_x_X * Kinv.transpose()).array()).rowwise().sum().array();
            y_var.array() += (1.0 / _beta);
        }

        y_mean = y_mean.array().rowwise() * _Ystd.array().colwise().mean();
        y_mean = y_mean.array().rowwise() + _Ymean.array().colwise().mean();

        if (dy_dx_vec_ptr)
        {
            std::vector<Matrix>& dy_dx_vec = *(dy_dx_vec_ptr);
            dy_dx_vec.clear();

            const Matrix& Kinv = GetKinv();

            const int D = _Y.cols();
            const int P = x.rows();

            dy_dx_vec.reserve(P);

            for (int p = 0; p < P; ++p)
            {
                shared_ptr<SparseMatrix> dK_dx = _kernel->GetGradientWrtSecondTerm(_X, x.row(p), K_x_X.row(p).transpose());

                Matrix dy_dx = (*dK_dx) * KinvY;

                dy_dx = dy_dx.array().rowwise() * _Ystd.array().colwise().mean();

                dy_dx_vec.push_back(dy_dx);
            }
        }

        return y_mean;
    }

    void SaveToMatFile(std::string matFilename) const
    {
        std::cerr << "UNSUPPORTED: UNABLE TO WRITE GPLVM MATLAB DATA TO \"" << matFilename << "\"." << std::endl;
    }

private:
    Matrix _Y;
    Matrix _Ymean;
    Matrix _Ystd;
    const int _N;
    const int _Q;
    shared_ptr<Kernel> _kernel;
    double _beta;
    Matrix _X;
    shared_ptr<NoiseHyperPriorFunction> _noisePriorFunc;

private:
    // Cached values (hence mutable)..
    mutable shared_ptr<Matrix> _KinvY_ptr;
    mutable shared_ptr<Matrix> _Kinv_ptr;

    void resetCachedValues()
    {
        _KinvY_ptr.reset();
        _Kinv_ptr.reset();
    }

protected:

    class CostFunction : public ceres::FirstOrderFunction
    {
    private:
        GPLVM* _gplvm;
        shared_ptr<Kernel> _kernel;

    public:
        explicit CostFunction(GPLVM* gplvm) :
            _gplvm(gplvm),
            _kernel(gplvm->_kernel->createCopy())
        {
        }

        virtual bool Evaluate(const double* parameters,
                              double* cost,
                              double* gradient) const {
            const int numParams = _gplvm->NumPreDataParameters();
            const int numKernelParams = _kernel->NumberOfParameters();
            const int N = _gplvm->_N;
            const int Q = _gplvm->_Q;
            const Matrix& Y = _gplvm->_Y;

            const double logBeta = parameters[0];
            const double beta = exp(logBeta);
            const Eigen::Map< const Vector > logKernelParams(&parameters[1], numKernelParams);

            _kernel->SetParameters(logKernelParams);

            const Eigen::Map< const Matrix > X(&parameters[numParams], N, Q);
            const Eigen::Map< const Vector > Xvec(&parameters[numParams], N * Q);

            // Create noise free copy of K and prepare extra data
            Matrix DDmat;
            Matrix KK = _kernel->GetKernelMatrix(X, X, &DDmat);
            Matrix K(KK);

            const double Nd = N;
            const double Dd = Y.cols();

            K.diagonal().array() += (1.0 / beta);
            Matrix L = K.llt().matrixL();

            double logDet = 2.0 * (L.diagonal().array().log()).sum();

            Matrix LinvY = K.llt().matrixL().solve(Y);
            double trace_Kinv_YYt = LinvY.squaredNorm();

            double hyperPriorNoise = _gplvm->_noisePriorFunc->getHyperPrior(logBeta);

            double hyperPriorParams = 0.5 * (logKernelParams.array() * logKernelParams.array()).sum();

            double hyperPriorData = 0.5 * X.squaredNorm();

            double LogLike = (0.5 * Dd * Nd) * log(2.0 * M_PI);
            LogLike += 0.5 * Dd * logDet;
            LogLike += 0.5 * trace_Kinv_YYt;
            LogLike += hyperPriorParams;
            LogLike += hyperPriorNoise;

            LogLike += hyperPriorData;

            cost[0] = LogLike;

            if (gradient != NULL)
            {
                // The beta gradient
                double& dL_dlogBeta = gradient[0];

                // The kernel params gradient
                Eigen::Map< Eigen::VectorXd > dL_dlogKernParams(&gradient[1], numKernelParams);

                // The final data gradient
                Eigen::Map< Eigen::VectorXd > dL_dX(&gradient[numParams], N*Q);

                Matrix dK_dbeta(Matrix::Identity(N, N));
                dK_dbeta.diagonal() /= - beta;

                // std::vector< shared_ptr< Matrix > >
                auto dK_dlogKernParams = _kernel->GetGradientWrtParams(X, KK, &DDmat);

                // shared_ptr< Eigen::SparseMatrix<double> >
                auto dK_dX_ptr = _kernel->GetGradientWrtData(X, KK, &DDmat);

                Matrix KinvY = K.llt().matrixL().transpose().solve(LinvY);

                Matrix dL_dK = - 0.5 * (KinvY * KinvY.transpose() - (Dd * K.llt().solve(Matrix::Identity(N, N))));

                dL_dlogBeta = (dL_dK.array() * dK_dbeta.array()).sum();
                dL_dlogBeta += _gplvm->_noisePriorFunc->getHyperPriorDerivative(logBeta);

                for (int i = 0; i < numKernelParams; ++i)
                {
                    dL_dlogKernParams[i] = (dL_dK.array() * (*(dK_dlogKernParams[i])).array()).sum() + logKernelParams[i];
                }

                Eigen::Map< Vector > dL_dK_vec(dL_dK.data(), N*N);

                dL_dX = (*dK_dX_ptr) * dL_dK_vec;

                // Add hyperPriorData gradient
                dL_dX += Xvec;
            }

            return true;
        }

        virtual int NumParameters() const
        {
            return _gplvm->NumParameters();
        }
    };

private:

    Matrix GetDefaultXInit() const
    {
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(_Y * _Y.transpose());
        Matrix X0 = es.eigenvectors().rowwise().reverse().block(0, 0, _N, _Q);

        const double scaling = ((X0.array().pow(2.0).colwise().sum() / (double(_N)-1.0)).sqrt()).mean();
        X0 /= scaling;

        return X0;
    }

    Vector GetParameterVector() const
    {
        Vector p(NumParameters());

        p[0] = log(_beta);

        Vector logKernParams;
        _kernel->GetParameters(logKernParams);
        p.block(1, 0, _kernel->NumberOfParameters(), 1) = logKernParams;

        const Eigen::Map< const Vector > Xvec(_X.data(), _N*_Q);
        p.block(NumPreDataParameters(), 0, _N*_Q, 1) = Xvec;

        return p;
    }

    void SetFromParameterVector(const Vector& p)
    {
        _beta = exp(p[0]);

        Vector logKernParams = p.block(1, 0, _kernel->NumberOfParameters(), 1);
        _kernel->SetParameters(logKernParams);

        Eigen::Map< Vector > Xvec(_X.data(), _N*_Q);
        Xvec = p.block(NumPreDataParameters(), 0, _N*_Q, 1);

        resetCachedValues();
    }

    const Matrix& GetKinvY() const
    {
        if (!_KinvY_ptr.get())
        {
            std::cout << "Computing KinvY cached value.." << std::endl;

            Matrix DDmat;
            Matrix K = _kernel->GetKernelMatrix(_X, _X, &DDmat);

            K.diagonal().array() += (1.0 / _beta);

            Matrix LinvY = K.llt().matrixL().solve(_Y);

            shared_ptr<Matrix> tmp(new Matrix(K.llt().matrixL().transpose().solve(LinvY)));

            _KinvY_ptr.swap(tmp);
        }

        return (*_KinvY_ptr);
    }

    const Matrix& GetKinv() const
    {
        if (!_Kinv_ptr.get()) {
            std::cout << "Computing Kinv cached value.." << std::endl;

            Matrix DDmat;
            Matrix K = _kernel->GetKernelMatrix(_X, _X, &DDmat);

            K.diagonal().array() += (1.0 / _beta);

            Matrix Linv = K.llt().matrixL().solve(Matrix::Identity(_N, _N));
            shared_ptr<Matrix> tmp(new Matrix(Linv.transpose() * Linv));

            _Kinv_ptr.swap(tmp);

#ifdef TEST_OTHER_INV_METHOD
//            vdbg(K * (*_Kinv_ptr));
//            vdbg((*_Kinv_ptr) * K);

//            vdbg((K * (*_Kinv_ptr) - Matrix::Identity(_N,_N)).norm());
//            vdbg(((*_Kinv_ptr) * K - Matrix::Identity(_N,_N)).norm());
            vdbg((K * Kinv - Matrix::Identity(_N,_N)).norm());
            vdbg((Kinv * K - Matrix::Identity(_N,_N)).norm());

            start = std::chrono::high_resolution_clock::now();
            Matrix Kinv2 = K.inverse();
            stop = std::chrono::high_resolution_clock::now();
            vdbg(std::chrono::duration_cast<std::chrono::microseconds>(stop-start).count());

//            vdbg(K * Kinv);
//            vdbg(Kinv * K);

            vdbg((K * Kinv2 - Matrix::Identity(_N,_N)).norm());
            vdbg((Kinv2 * K - Matrix::Identity(_N,_N)).norm());
#endif // TEST_OTHER_INV_METHOD
        }

        return (*_Kinv_ptr);
    }

public:

    void TestGplvmGradients(const double step = 1e-8)
    {
        using std::cout; using std::endl;
        cout << endl << " TESTING GPLVM GRADIENTS ";
        cout << endl << " ======================= " << endl;

        CostFunction costFunc(this);

        Vector parameters(GetParameterVector());

        parameters.setRandom();

        double C0 = 0.0;
        Vector gradients(NumParameters());

        bool r = costFunc.Evaluate(parameters.data(), &C0, gradients.data());
        assert (r);

        vdbg(C0);
        vdbg(gradients.transpose());

        Vector estGradients(NumParameters());
        estGradients.setZero();

        for (int i = 0, I = NumParameters(); i < I; ++i)
        {
            Vector p(parameters);
            p[i] += step;
            double C = 0;

            bool r = costFunc.Evaluate(p.data(), &C, NULL);
            assert (r);

            estGradients[i] = (C - C0) / step;
        }

        vdbg(estGradients.transpose());

        vdbg((estGradients - gradients).transpose());
        vdbg((estGradients - gradients).norm() / gradients.norm());

        assert ((estGradients - gradients).norm() / gradients.norm() < 1e-4);

        cout << " ======================= " << endl << endl;
    }

    void TestPredictionGradients(Matrix x = Matrix(0,0), const double step = 1e-8)
    {
        using std::cout; using std::endl;
        cout << endl << " TESTING PREDICTION GRADIENTS ";
        cout << endl << " ============================ " << endl;

        if (x.size() == 0)
        {
            x = GetX();
        }

        Vector yVar;
        std::vector<Matrix> dy_dx_vec;
        Matrix y = Predict(x, &yVar, &dy_dx_vec);

        const int P = x.rows();

        for (int p = 0; p < P; ++p)
        {
            Matrix xx = x.row(p);
            xx = xx + Matrix::Constant(xx.rows(), xx.cols(), step);
            Matrix yy = (Predict(xx) - y.row(p));
            yy /= step;

            vdbg((yy - dy_dx_vec[p]).norm());

            assert ((yy - dy_dx_vec[p]).norm() / (dy_dx_vec[p]).norm() < (10 * step));
        }

        cout << " ============================ " << endl << endl;
    }

    // Optimise to convergence or until the first of the specified limits is reached..
    void LearnParameters(const double maxSolverTimeInSeconds = 1.0,
                         const int maxNumIterations = 10000,
                         const bool showProgress = false)
    {
        ceres::GradientProblem problem(new CostFunction(this));

        Vector parameters(GetParameterVector());

        ceres::GradientProblemSolver::Options options;
        options.minimizer_progress_to_stdout = showProgress;

        options.function_tolerance = 1e-6;
        options.gradient_tolerance = 1e-4;

        options.max_num_iterations = maxNumIterations;
        options.max_solver_time_in_seconds = maxSolverTimeInSeconds;

        ceres::GradientProblemSolver::Summary summary;
        ceres::Solve(options, problem, parameters.data(), &summary);

        SetFromParameterVector(parameters);
    }

};

#endif // GPLVM_HPP


/*
 *
 * Copyright (c) Kresimir Fresl 2002, 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS2_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS2_OVERLOADS_HPP

#include <complex>
#include <boost/numeric/bindings/atlas/cblas_inc.hpp>
#include <boost/numeric/bindings/traits/type.hpp>

namespace boost { namespace numeric { namespace bindings {

  namespace atlas { namespace detail {

    // y <- alpha * op (A) * x + beta * y

    inline
    void gemv (CBLAS_ORDER const Order,
               CBLAS_TRANSPOSE const TransA, int const M, int const N,
               float const alpha, float const* A, int const lda,
               float const* X, int const incX,
               float const beta, float* Y, int const incY)
    {
      cblas_sgemv (Order, TransA, M, N, alpha, A, lda,
                   X, incX,
                   beta, Y, incY);
    }

    inline
    void gemv (CBLAS_ORDER const Order,
               CBLAS_TRANSPOSE const TransA, int const M, int const N,
               double const alpha, double const* A, int const lda,
               double const* X, int const incX,
               double const beta, double* Y, int const incY)
    {
      cblas_dgemv (Order, TransA, M, N, alpha, A, lda,
                   X, incX,
                   beta, Y, incY);
    }

    inline
    void gemv (CBLAS_ORDER const Order,
               CBLAS_TRANSPOSE const TransA, int const M, int const N,
               traits::complex_f const& alpha,
               traits::complex_f const* A, int const lda,
               traits::complex_f const* X, int const incX,
               traits::complex_f const& beta,
               traits::complex_f* Y, int const incY)
    {
      cblas_cgemv (Order, TransA, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (A), lda,
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (&beta),
                   static_cast<void*> (Y), incY);
    }

    inline
    void gemv (CBLAS_ORDER const Order,
               CBLAS_TRANSPOSE const TransA, int const M, int const N,
               traits::complex_d const& alpha,
               traits::complex_d const* A, int const lda,
               traits::complex_d const* X, int const incX,
               traits::complex_d const& beta,
               traits::complex_d* Y, int const incY)
    {
      cblas_zgemv (Order, TransA, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (A), lda,
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (&beta),
                   static_cast<void*> (Y), incY);
    }

    // y <- alpha * A * x + beta * y
    // A real symmetric

    inline
    void symv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, float const alpha, float const* A,
               int const lda, float const* X, int const incX,
               float const beta, float* Y, int const incY)
    {
      cblas_ssymv (Order, Uplo, N, alpha, A, lda,
                   X, incX, beta, Y, incY);
    }

    inline
    void symv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, double const alpha, double const* A,
               int const lda, double const* X, int const incX,
               double const beta, double* Y, int const incY)
    {
      cblas_dsymv (Order, Uplo, N, alpha, A, lda,
                   X, incX, beta, Y, incY);
    }

    // y <- alpha * A * x + beta * y
    // A real symmetric in packed form

    inline
    void spmv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, float const alpha, float const* Ap,
               float const* X, int const incX,
               float const beta, float* Y, int const incY)
    {
      cblas_sspmv (Order, Uplo, N, alpha, Ap, X, incX, beta, Y, incY);
    }

    inline
    void spmv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, double const alpha, double const* Ap,
               double const* X, int const incX,
               double const beta, double* Y, int const incY)
    {
      cblas_dspmv (Order, Uplo, N, alpha, Ap, X, incX, beta, Y, incY);
    }

    // y <- alpha * A * x + beta * y
    // A complex hermitian

    inline
    void hemv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_f const& alpha,
               traits::complex_f const* A, int const lda,
               traits::complex_f const* X, int const incX,
               traits::complex_f const& beta,
               traits::complex_f* Y, int const incY)
    {
      cblas_chemv (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (A), lda,
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (&beta),
                   static_cast<void*> (Y), incY);
    }

    inline
    void hemv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_d const& alpha,
               traits::complex_d const* A, int const lda,
               traits::complex_d const* X, int const incX,
               traits::complex_d const& beta,
               traits::complex_d* Y, int const incY)
    {
      cblas_zhemv (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (A), lda,
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (&beta),
                   static_cast<void*> (Y), incY);
    }

    // y <- alpha * A * x + beta * y
    // A complex hermitian in packed form

    inline
    void hpmv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_f const& alpha,
               traits::complex_f const* Ap,
               traits::complex_f const* X, int const incX,
               traits::complex_f const& beta,
               traits::complex_f* Y, int const incY)
    {
      cblas_chpmv (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (Ap),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (&beta),
                   static_cast<void*> (Y), incY);
    }

    inline
    void hpmv (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_d const& alpha,
               traits::complex_d const* Ap,
               traits::complex_d const* X, int const incX,
               traits::complex_d const& beta,
               traits::complex_d* Y, int const incY)
    {
      cblas_zhpmv (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (Ap),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (&beta),
                   static_cast<void*> (Y), incY);
    }

    // A <- alpha * x * y^T + A
    // .. real types:    calls cblas_xger
    // .. complex types: calls cblas_xgeru

    inline
    void ger (CBLAS_ORDER const Order, int const M, int const N,
              float const alpha, float const* X, int const incX,
              float const* Y, int const incY, float* A, int const lda)
    {
      cblas_sger (Order, M, N, alpha, X, incX, Y, incY, A, lda);
    }

    inline
    void ger (CBLAS_ORDER const Order, int const M, int const N,
              double const alpha, double const* X, int const incX,
              double const* Y, int const incY, double* A, int const lda)
    {
      cblas_dger (Order, M, N, alpha, X, incX, Y, incY, A, lda);
    }

    inline
    void ger (CBLAS_ORDER const Order, int const M, int const N,
              traits::complex_f const& alpha,
              traits::complex_f const* X, int const incX,
              traits::complex_f const* Y, int const incY,
              traits::complex_f* A, int const lda)
    {
      cblas_cgeru (Order, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    inline
    void ger (CBLAS_ORDER const Order, int const M, int const N,
              traits::complex_d const& alpha,
              traits::complex_d const* X, int const incX,
              traits::complex_d const* Y, int const incY,
              traits::complex_d* A, int const lda)
    {
      cblas_zgeru (Order, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    // A <- alpha * x * y^T + A
    // .. complex types only

    inline
    void geru (CBLAS_ORDER const Order, int const M, int const N,
               traits::complex_f const& alpha,
               traits::complex_f const* X, int const incX,
               traits::complex_f const* Y, int const incY,
               traits::complex_f* A, int const lda)
    {
      cblas_cgeru (Order, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    inline
    void geru (CBLAS_ORDER const Order, int const M, int const N,
               traits::complex_d const& alpha,
               traits::complex_d const* X, int const incX,
               traits::complex_d const* Y, int const incY,
               traits::complex_d* A, int const lda)
    {
      cblas_zgeru (Order, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    // A <- alpha * x * y^H + A
    // .. complex types only

    inline
    void gerc (CBLAS_ORDER const Order, int const M, int const N,
               traits::complex_f const& alpha,
               traits::complex_f const* X, int const incX,
               traits::complex_f const* Y, int const incY,
               traits::complex_f* A, int const lda)
    {
      cblas_cgerc (Order, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    inline
    void gerc (CBLAS_ORDER const Order, int const M, int const N,
               traits::complex_d const& alpha,
               traits::complex_d const* X, int const incX,
               traits::complex_d const* Y, int const incY,
               traits::complex_d* A, int const lda)
    {
      cblas_zgerc (Order, M, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    // A <- alpha * x * x^T + A
    // A real symmetric

    inline
    void syr (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, float const alpha,
              float const* X, int const incX, float* A, int const lda)
    {
      cblas_ssyr (Order, Uplo, N, alpha, X, incX, A, lda);
    }

    inline
    void syr (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, double const alpha,
              double const* X, int const incX, double* A, int const lda)
    {
      cblas_dsyr (Order, Uplo, N, alpha, X, incX, A, lda);
    }

    // A <- alpha * x * x^T + A
    // A real symmetric in packed form

    inline
    void spr (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, float const alpha,
              float const* X, int const incX, float* Ap)
    {
      cblas_sspr (Order, Uplo, N, alpha, X, incX, Ap);
    }

    inline
    void spr (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, double const alpha,
              double const* X, int const incX, double* Ap)
    {
      cblas_dspr (Order, Uplo, N, alpha, X, incX, Ap);
    }

    // A <- alpha * x * y^T + alpha * y * x^T + A
    // A real symmetric

    inline
    void syr2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, float const alpha,
               float const* X, int const incX,
               float const* Y, int const incY,
               float* A, int const lda)
    {
      cblas_ssyr2 (Order, Uplo, N, alpha, X, incX, Y, incY, A, lda);
    }

    inline
    void syr2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, double const alpha,
               double const* X, int const incX,
               double const* Y, int const incY,
               double* A, int const lda)
    {
      cblas_dsyr2 (Order, Uplo, N, alpha, X, incX, Y, incY, A, lda);
    }

    // A <- alpha * x * y^T + alpha * y * x^T + A
    // A real symmetric in packed form

    inline
    void spr2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, float const alpha,
               float const* X, int const incX,
               float const* Y, int const incY, float* Ap)
    {
      cblas_sspr2 (Order, Uplo, N, alpha, X, incX, Y, incY, Ap);
    }

    inline
    void spr2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, double const alpha,
               double const* X, int const incX,
               double const* Y, int const incY, double* Ap)
    {
      cblas_dspr2 (Order, Uplo, N, alpha, X, incX, Y, incY, Ap);
    }

    // A <- alpha * x * x^H + A
    // A hermitian

    inline
    void her (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, float const alpha,
              traits::complex_f const* X, int const incX,
              traits::complex_f* A, int const lda)
    {
      cblas_cher (Order, Uplo, N, alpha,
                  static_cast<void const*> (X), incX,
                  static_cast<void*> (A), lda);
    }

    inline
    void her (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, double const alpha,
              traits::complex_d const* X, int const incX,
              traits::complex_d* A, int const lda)
    {
      cblas_zher (Order, Uplo, N, alpha,
                  static_cast<void const*> (X), incX,
                  static_cast<void*> (A), lda);
    }

    // A <- alpha * x * x^H + A
    // A hermitian in packed form

    inline
    void hpr (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, float const alpha,
              traits::complex_f const* X, int const incX,
              traits::complex_f* Ap)
    {
      cblas_chpr (Order, Uplo, N, alpha,
                  static_cast<void const*> (X), incX,
                  static_cast<void*> (Ap));
    }

    inline
    void hpr (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
              int const N, double const alpha,
              traits::complex_d const* X, int const incX,
              traits::complex_d* Ap)
    {
      cblas_zhpr (Order, Uplo, N, alpha,
                  static_cast<void const*> (X), incX,
                  static_cast<void*> (Ap));
    }

    // A <- alpha * x * y^H + y * (alpha * x)^H + A
    // A hermitian

    inline
    void her2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_f const& alpha,
               traits::complex_f const* X, int const incX,
               traits::complex_f const* Y, int const incY,
               traits::complex_f* A, int const lda)
    {
      cblas_cher2 (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    inline
    void her2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_d const& alpha,
               traits::complex_d const* X, int const incX,
               traits::complex_d const* Y, int const incY,
               traits::complex_d* A, int const lda)
    {
      cblas_zher2 (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (A), lda);
    }

    // A <- alpha * x * y^H + y * (alpha * x)^H + A
    // A hermitian in packed form

    inline
    void hpr2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_f const& alpha,
               traits::complex_f const* X, int const incX,
               traits::complex_f const* Y, int const incY,
               traits::complex_f* Ap)
    {
      cblas_chpr2 (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (Ap));
    }

    inline
    void hpr2 (CBLAS_ORDER const Order, CBLAS_UPLO const Uplo,
               int const N, traits::complex_d const& alpha,
               traits::complex_d const* X, int const incX,
               traits::complex_d const* Y, int const incY,
               traits::complex_d* Ap)
    {
      cblas_zhpr2 (Order, Uplo, N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void const*> (Y), incY,
                   static_cast<void*> (Ap));
    }

  }} // namepaces detail & atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CBLAS2_OVERLOADS_HPP


#include <iostream>
#include <Eigen/Sparse>
#include <functional>

template <class func, class Vector>
void fixed_point_step(func&& A, const Vector & b, const
Vector & x, Vector & x_new){
	int  n= x.size();
	Eigen::SparseMatrix<double> A_eval(n,n) = A(x);
	x_new = x - ( A_eval + x*x.transposed()/x.norm()).cwiseInverse()*(A_eval*x-b);
};

int main(){
	Eigen::VectorXd x, b, x_new, x_old;
	b=Eigen::VectorXd::Random(10,10);
	x_new = b;
	double tol =2e-15;

	auto A = [ x_new ] (const Eigen::VectorXd & x) ->
	Eigen::SparseMatrix<double> & {
	int const n = x.size();
	Eigen::SparseMatrix<double, n, n> A;
	double x_norm = x.norm();
	x_norm +=3;
	A(0,0)=x_norm;A(0,1)=1;
	A(n-1,n-1)=x_norm;A(n-1,n-2)=1;
	for (int i= 1; i<n-1; i++){
		A(i,i)=x_norm;
		A(i+1,i)=1;
		A(i,i+1)=1;
	}
	return A;
	};

	int j=0;
	while (std::abs(x_old.norm()-x.norm())<tol){
		i++
		fixed_point_step(A, b, x, x_new)
		x_old=x;
		x=x_new;
	}
	std::cout << x << std::endl;

	return 0;

}

Nummerical Methods for CSE/PS5/quasilin.cpp