#include "problemes.h"
#include "arithmetique.h"
#include "matrice.h"

#include <boost/numeric/ublas/assignment.hpp>

typedef long long nombre;
typedef std::vector<nombre> vecteur;

ENREGISTRER_PROBLEME(237, "Tours on a 4 x n playing board") {
    // Let T(n) be the number of tours over a 4 × n playing board such that:
    //
    // The tour starts in the top left corner.
    // The tour consists of moves that are up, down, left, or right one square.
    // The tour visits each square exactly once.
    // The tour ends in the bottom left corner.
    // The diagram shows one tour over a 4 × 10 board:
    //
    //					https://projecteuler.net/project/images/p237.gif
    //
    //T(10) is 2329. What is T(1012) modulo 108?
    size_t n = 1000000000000LL;
    // T[n]=2*T[n-1]+2*T[n-2]-2*T[n-3]+T[n-4]
    matrice::matrice<nombre> m(4, 4, 0);
    m <<= 2, 2, -2, 1,
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0;

    matrice::vecteur<nombre> i(4);
    i <<= 8, 4, 1, 1;

    auto m_n = matrice::puissance_matrice(m, n - 4, 100000000LL);
    nombre resultat = boost::numeric::ublas::prod(m_n, i)(0);

    return std::to_string(resultat);
}


/// @file bealab/extensions/control/baytrack.hpp
/// Implementation of Bayesian tracking techniques.

#ifndef _BEALAB_BAYTRACK_
#define	_BEALAB_BAYTRACK_

#include <bealab/core/blas.hpp>
#include <bealab/scilib/stats.hpp>
#include <bealab/scilib/optimization.hpp>
#include <bealab/extensions/control/linsys.hpp>
#include <boost/circular_buffer.hpp>

namespace bealab
{
namespace control
{
//------------------------------------------------------------------------------
/// @defgroup baytrack Bayesian tracking
/// Implementation of Bayesian tracking techniques.
/// @{

using boost::circular_buffer;

/// Kalman filter
class kalman_filter : public state_space {
public:

	rvec x;																		///< Initial state mean
	rmat P;																		///< Initial state covariance

	/// Constructor
	kalman_filter( const rmat& A, const rmat& B, const rmat& C, const rmat& D,
			const rmat& Q, const rmat& R, const rmat& P_ = rmat() ) :
				state_space(A,B,C,D,Q,R)
	{
		int N = A.size1();
		x     = zeros(N);
		if( P_.size1() == 0 )
			P = zeros(N,N);
		else
			P = P_;
	}

	/// Constructor
	kalman_filter( const state_space& ss, const rmat& P = rmat() ) :
		kalman_filter( ss.A, ss.B, ss.C, ss.D, ss.Q, ss.R, P) {}

	/// Prediction step
	rvec prediction( const rvec& u )
	{
		x = A * x + B * u;
		P = noproxy(A * P) * trans(A) + Q;
		return x;
	}

	/// Update step
	rvec update( const rvec& y )
	{
		int N  = A.size1();
		rmat K = trans( linsolve( noproxy(C * P) * trans(C) + R, C * P ) );
		x      = x + K * ( y - C * x );
		P      = (eye(N) - K * C) * P;
		return x;
	}

	/// Filter one pair of input and output samples
	rvec operator()( const rvec& u, const rvec& y )
	{
		prediction( u );
		update( y );
		return x;
	}

	/// Filter an output sample, assuming zero input.
	rvec operator()( const rvec& y )
	{
		int M  = B.size2();
		rvec u = zeros(M);
		return (*this)( u, y );
	}

	/// Filter one pair of sequences of input and output samples
	vec<rvec> operator()( const vec<rvec>& U, const vec<rvec>& Y )
	{
		assert( U.size() == Y.size() );
		int T = U.size();
		vec<rvec> Xh(T);
		for( int t = 0; t < T; t++ )
			Xh(t) = (*this)( U(t), Y(t) );
		return Xh;
	}

	/// Filter a sequences of output samples, assuming zero input.
	vec<rvec> operator()( const vec<rvec>& Y )
	{
		int T = Y.size();
		vec<rvec> Xh(T);
		for( int t = 0; t < T; t++ )
			Xh(t) = (*this)( Y(t) );
		return Xh;
	}
};

/// Base class for implementing a particle filter.
/// The virtual member functions  need to be implemented in a derived class.
/// Need to implement either state_transition() or particle_prediction(), and
/// either likelihood_function() or particle_update().
template< class state_t = rvec, class obs_t = state_t, class out_t = obs_t >
class particle_filter_b {
protected:

	/// A particles path with it's associated weight
	struct particle_path {
		circular_buffer<state_t> trajectory;
		double weight;
	};

	rvec old_weights;															///< Weights before the last update step
	bool init_f = false;														///< Flag to indicate that the filter was initialized
	int time    = 0;															///< The current time used when running the filter

	/// A particles with it's associated weight
	struct particle {
		state_t state;
		double weight;
	};

	/// Draw a sample from the initial distribution
	virtual
	state_t draw_initial_sample() = 0;

	/// State transition function
	virtual
	state_t state_transition( const state_t& state ) { return state; }

	/// Likelihood of the state given the observation
	virtual
	double likelihood_function( const obs_t& obs, const state_t& state ) { return 1; }

	/// Default particle prediction function. Uses state_transition().
	virtual
	particle particle_prediction( const particle& p )
	{
		particle pp = p;
		pp.state    = state_transition( p.state );
		return pp;
	}

	/// Default particle update function. Uses likelihood_function().
	virtual
	particle particle_update( const obs_t& obs, const particle& p )
	{
		particle pu = p;
		pu.weight  *= likelihood_function( obs, p.state );
		return pu;
	}

	/// Output function
	virtual
	out_t output_function( const state_t& state ) = 0;

public:

	vector<particle_path> x;													///< Particles of the current predicted or updated state
	const int I;																///< Number of particles
	const int lag;																///< Amount of lag used for smoothing.
	double resample_period = 1;

	/// Constructor
	particle_filter_b( int I_, int lag_=0 ) :
		old_weights(I_), x(I_), I(I_), lag(lag_)
	{
		assert( I > 0 );
		for( int i = 0; i < I; i++ )
			x[i].trajectory = circular_buffer<state_t>(lag+1);
	}

	/// Destructor
	virtual
	~particle_filter_b() {}

	/// Implements the prediction step
	virtual
	void prediction()
	{
		#pragma omp parallel for schedule(dynamic)
		for( int i = 0; i < I; i++ ) {
			particle p  = { x[i].trajectory.back(), x[i].weight };
			particle pp = particle_prediction( p );
			x[i].trajectory.push_back( pp.state );
			x[i].weight = pp.weight;
		}
	}

	/// Implements the update step
	virtual
	void update( const obs_t& z )
	{
		#pragma omp parallel for schedule(dynamic)
		for( int i = 0; i < I; i++ ) {
			particle p             = { x[i].trajectory.back(), x[i].weight };
			particle pu            = particle_update( z, p );
			x[i].trajectory.back() = pu.state;
			old_weights(i)         = x[i].weight;
			x[i].weight            = pu.weight;
		}
		normalize();
	}

	/// Implements the normalization step
	virtual
	void normalize()
	{
		double K = 0;
		for( int i = 0; i < I; i++ )
			K += x[i].weight;
//		if( K == 0 ) {
//			warning("Ran out of particles");
//			for( int i = 0; i < I; i++ )
//				x[i].weight = old_weights(i);
//		}
//		else {
//			for( int i = 0; i < I; i++ )
//				x[i].weight /= K;
//		}
		for( int i = 0; i < I; i++ )
			x[i].weight /= K;
		if( K == 0 )
			warning("Ran out of particles");
	}

	/// Implements the re-sampling step
	virtual
	void resample()
	{
		// Store current particles
		auto x0 = x;

		// Build the distribution
		rvec weights(I);
		for( int i = 0; i < I; i++ )
			weights(i) = x[i].weight;
		std::discrete_distribution<> d( weights.begin(), weights.end() );

		// Re-sample
		for( int i = 0; i < I; i++ ) {
			int idx = d(_stats::engine);
			x[i].trajectory = x0[idx].trajectory;
			x[i].weight     = 1./I;
		}
	}

	/// Generates the output
	virtual
	out_t output()
	{
		vec<out_t> o(I);
		#pragma omp parallel for schedule(dynamic)
		for( int i = 0; i < I; i++ )
			o(i) = output_function( x[i].trajectory.front() ) * x[i].weight;
		return sum(o);
	}

	/// Draw the particles of the initial state
	void initialize()
	{
		init_f = true;
		for( int i = 0; i < I; i++ ) {
			x[i].trajectory.push_back( draw_initial_sample() );
			x[i].weight = 1./I;
		}
	}

	/// Filter one sample
	out_t operator()( const obs_t& z )
	{
		if( !init_f )
			initialize();
		update( z );
		out_t o = output();
		if( mod( time+1, resample_period ) == 0 )
			resample();
		prediction();
		time++;
		return o;
	}

	/// Filter a vector of samples
	vec<out_t> operator()( const vec<obs_t>& Z )
	{
		int T = Z.size();
		vec<out_t> Yh(T);
		for( int t = 0; t < T; t++ )
			Yh(t) = (*this)( Z(t) );
		return Yh;
	}
};

/// Particle approximation to a Kalman filter (for testing purposes)
class particle_kalman_filter : public particle_filter_b<rvec,rvec,rvec>, public kalman_filter {

	int N;
	rmat Qh, Rh, Ph;

	rvec draw_initial_sample() override
	{
		return Ph * randn(N);
	}

	rvec state_transition( const rvec& x ) override
	{
		return A * x + Qh * randn(N);
	}

	double likelihood_function( const rvec& y, const rvec& x ) override
	{
		return pdf( multivariate_normal( C*x, Rh ), y );
	}

	rvec output_function( const rvec& x ) override
	{
		return C*x;
	}

public:

	using particle_filter_b::prediction;
	using particle_filter_b::update;
	using particle_filter_b::operator();

	particle_kalman_filter( int I, const rmat& A, const rmat& B, const rmat& C, const rmat& D,
			const rmat& Q, const rmat& R, const rmat& P_ = rmat() ) :
				particle_filter_b(I),
				kalman_filter(A,B,C,D,Q,R,P_)
	{
		N  = A.size1();
		Qh = real(msqrt(Q));
		Rh = real(msqrt(R));
		Ph = real(msqrt(P));
	}

	particle_kalman_filter( int I, const state_space& ss, const rmat& P = rmat() ) :
		particle_kalman_filter(I,ss.A,ss.B,ss.C,ss.D,ss.Q,ss.R,P)
	{}
};

/// State associated to a Rao-Blackwellized particle filter.
struct RB_state_t {
	rvec position;
	rvec mean;
	rmat covariance;
};

/// Base class for implementing Rao-Blackwellized particle filters.
/// The virtual member functions  need to be implemented in a derived class.
class RB_particle_filter_b : public particle_filter_b< RB_state_t, rvec, rvec > {

	/// Non-linear state transition function
	virtual
	rvec f( const rvec& x ) = 0;

	/// Linear state transition function
	virtual
	rvec g( const rvec& x ) = 0;

	/// Measurement non-linear component
	virtual
	rvec h( const rvec& x ) = 0;

	/// Output non-linear component
	virtual
	rvec o( const rvec& x ) = 0;

	/// Non-linear state transition matrix
	virtual
	rmat F( const rvec& x ) = 0;

	/// Linear state transition matrix
	virtual
	rmat G( const rvec& x ) = 0;

	/// Measurement linear component
	virtual
	rmat H( const rvec& x ) = 0;

	/// Output linear component
	virtual
	rmat O( const rvec& x ) = 0;

	/// Non-linear process noise covariance
	virtual
	rmat U( const rvec& x ) = 0;

	/// Linear process noise covariance
	virtual
	rmat V( const rvec& x ) = 0;

	/// Output noise covariance
	virtual
	rmat W( const rvec& x ) = 0;

	/// Override
	particle particle_prediction( const particle& p ) override
	{
		// Predicted state
		particle pp;

		// Non-linear elements
		rvec f_ = f( p.state.position );
		rmat F_ = F( p.state.position );
		rmat U_ = U( p.state.position );
		rvec g_ = g( p.state.position );
		rmat G_ = G( p.state.position );
		rmat V_ = V( p.state.position );

		// Non-linear prediction
		rvec nlp_mean   = f_ + F_ * p.state.mean;
		rmat nlp_cov    = noproxy(F_ * p.state.covariance) * trans(F_) + U_;
		rmat nlp_cov_h  = real(msqrt(nlp_cov));
		rvec nlp        = rand( multivariate_normal( nlp_mean, nlp_cov_h ) );
		pp.state.position = nlp;

		// Linear prediction
		if( det(nlp_cov) != 0 ) {

			// Linear false update
			rvec lfu_obs  = nlp - f_;
			rmat K        = trans( linsolve( nlp_cov, F_ * p.state.covariance ) );
			rvec lfu_mean = p.state.mean + K * ( lfu_obs - F_ * p.state.mean );
			rmat lfu_cov  = (eye(K.size1()) - K * F_) * p.state.covariance;

			// Prediction
			pp.state.mean       = g_ + G_ * lfu_mean;
			pp.state.covariance = noproxy(G_ * lfu_cov) * trans(G_) + V_;
		}
		else {
			pp.state.mean       = g_ + G_ * p.state.mean;
			pp.state.covariance = noproxy(G_ * p.state.covariance) * trans(G_) + V_;
		}

		// Weight
		pp.weight = p.weight;

		return pp;
	}

	/// Override
	particle particle_update( const rvec& obs, const particle& p ) override
	{
		// Update particles
		particle pu;

		// Non-linear elements
		rvec h_ = h( p.state.position );
		rmat H_ = H( p.state.position );
		rmat W_ = W( p.state.position );

		// Linear output prediction
		rvec lop_mean = H_ * p.state.mean + h_;
		rmat lop_cov  = noproxy(H_ * p.state.covariance) * trans(H_) + W_;

		// Check the singularity of lop_cov
		double singular = (det(lop_cov) == 0);

		// Non-linear state update
		pu.state.position = p.state.position;
		rvec delta        = obs - lop_mean;
		if( !singular ) {
			int N     = lop_mean.size();
			double a  = inner_prod( delta, linsolve( lop_cov, delta ) );
			double b  = log( pow(2*pi,N) * det(lop_cov) );
			pu.weight = exp( -0.5 * (a+b) ) * p.weight;
			if( isnan(pu.weight) )
				error("Weight is nan");
		}
		else
			pu.weight = ( norm(delta) == 0 ? 1 : 0) * p.weight;

		// Linear state update
		if( !singular ) {
			rvec lsu_obs        = obs - h_;
			rmat K              = trans( linsolve( lop_cov, H_ * p.state.covariance ) );
			pu.state.mean       = p.state.mean + K * ( lsu_obs - H_ * p.state.mean );
			pu.state.covariance = (eye(K.size1()) - K * H_) * p.state.covariance;
		}
		else {
			pu.state.mean       = p.state.mean;
			pu.state.covariance = p.state.covariance;
		}

		return pu;
	}

	/// Override
	rvec output_function( const RB_state_t& state ) override
	{
		rvec o_ = o( state.position );
		rmat O_ = O( state.position );
		return O_ * state.mean + o_;
	}

public:

	/// Constructor
	RB_particle_filter_b( int I, int lag=0 ) :
		particle_filter_b<RB_state_t,rvec,rvec>( I, lag ) {}

	/// Destructor
	virtual
	~RB_particle_filter_b() {}
};

/// Base class for implementing a particle smoother.
/// The virtual member functions  need to be implemented in a derived class.
//template< class pos_t = rvec >
//class particle_smoother_b : public particle_filter_b<pos_t> {
//
//	int L;																		///< Lag size
//	particles current;															///< Current updated particles
//	deque<particles> lagbuffer;													///< Buffer with the updated particles within the lag
//
//	/// Run one lag step
//	particles smooth_one_step( const particles& filtered, const particles& lagged )
//	{
////		particles rv = filtered;
////		for( int i = 0; i < I; i++ )
////			rv[i].weight *=
//		return lagged;
//	}
//
//	/// Run the backwards smoothing process
//	particles smooth_backwards()
//	{
//		particles lagged = current;
//		for( int l = 0; l < L; l++ )
//			lagged = smooth_one_step( lagbuffer[l], lagged );
//		return lagged;
//	}
//
//public:
//
//	/// Constructor
//	particle_smoother_b( int I_, int L_ ) :
//		particle_filter_b(I_), L(L_) {};
//
//	/// Filter one sample
//	pos_t operator()( const rvec& z ) override
//	{
//		// Initialize
//		if( !init_f )
//			initialize();
//
//		// Update buffer
//		if( (int)lagbuffer.size() == L )
//			lagbuffer.pop_back();
//		lagbuffer.emplace_front( current );
//
//		// Filter
//		current             = update( x, z );
//		particles resampled = resample( current );
//		x                   = prediction( resampled );
//		time++;
//
//		// Smoothing
//		particles xs = smooth_backwards();
//		return mean( output(xs) );
//	}
//};

/// ML Kalman filter
class maximum_likelihood_kalman_filter_b {

	/// State type
	struct state {
		rvec x;
		rmat P;
	};

	rmat C;
	rmat C_pseudoinverse;

	virtual
	double logLF( const rvec& z, const rvec& x ) = 0;

	virtual
	rvec logLF_gradient( const rvec& z, const rvec& x )
	{
		auto loglf = [this,&z](const rvec&x){return logLF(z,x);};
		return gradient( loglf, x );
	}

	virtual
	rvec output( const rvec& x )
	{
		return x;
	}

	/// Maximum likelihood estimation.
	/// Returns the ML estimate in rv.x and the Hessian of the logLF in rv.P
	state ml_estimate( const rvec& z, const rvec& guess=rvec() )
	{
		// Detect Rao-Blackwellization
		int L = s.x.size();
		int N = s.x.size();
		if( C_pseudoinverse.size1() * C_pseudoinverse.size2() > 0 )
			N = C_pseudoinverse.size2();

		// ML Estimation
		auto objfun  = [this,&z](const rvec&x){return -logLF(z,x);};
		auto objgrad = [this,&z](const rvec&x){return -logLF_gradient(z,x);};
		optimization::bfgs opt(N);
		opt.set_objective( objfun, objgrad );
		if( guess.size() == 0 )
			opt.guess = zeros(N);
		else
			opt.guess = guess;

//		// Test derivatives
//		rvec xt = randn(N);
//		cout << "xtest = " << xt << endl;
//		opt.test_derivatives( xt );
//		cin.get();

//		opt.stop_fincrement_relative = 1e-2;
//		opt.stop_xincrement_relative = 1e-2;
		rvec xh = opt.optimize();

		// Return value
		state ml;
		ml.x = { xh, zeros(L-N) };
		ml.P = jacobian( objgrad, xh );
		ml.P = ( ml.P + trans(ml.P) ) / 2;
		ml.P = { { ml.P,         zeros(N,L-N)   },
				 { zeros(L-N,N), zeros(L-N,L-N) } };
		return ml;
	}

	/// Prediction step
	state prediction( const state& s )
	{
		state p;
		p.x = A * s.x;
		p.P = noproxy(A * s.P) * trans(A) + Q;
		return p;
	}

	/// Update step
	state update( const state& s, const rvec& z )
	{
		// ML estimate
		state ml = ml_estimate( z, C*s.x );

		// Update the state
		state u;
		u.P = inv( inv(s.P) + ml.P );
		u.x = u.P * ( linsolve( s.P, s.x ) + ml.P * ml.x );
		return u;
	}

public:

	state s;																	///< Current state
	rmat A;																		///< State transition matrix
	rmat Q;																		///< Process noise covariance

	/// Destructor
	virtual
	~maximum_likelihood_kalman_filter_b() {}

	void set_C( const rmat& C_ )
	{
		C = C_;
		C_pseudoinverse = real(pinv(C));
	}

	/// Filter the sample z
	rvec operator()( const rvec& z )
	{
		s = prediction( s );
		s = update( s, z );
		return output(s.x);
	}

	/// Filter a vector of samples
	vec<rvec> operator()( const vec<rvec>& Z )
	{
		int T = Z.size();
		vec<rvec> Yh(T);
		for( int t = 0; t < T; t++ )
			Yh(t) = (*this)( Z(t) );
		return Yh;
	}
};

/// @}
}
}
#endif


#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/ceres.h"
#include "glog/logging.h"

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

const double DT = 1.0;
const Eigen::Vector3d GRAVITY{0, 0, -9.8};

struct State {
  Eigen::Vector3d pos = Eigen::Vector3d::Random(); // position
  Eigen::Vector3d vel = Eigen::Vector3d::Random(); // velocity
  Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); // pose Qwr

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct Measurement{
  Eigen::Matrix3d Rwr;
  Eigen::Quaterniond qwr;
  Eigen::Vector3d twr;
  Eigen::Vector3d acc;
  Eigen::Vector3d omega;

  Measurement(Eigen::Matrix3d Rwr,
              Eigen::Quaterniond qwr,
              Eigen::Vector3d twr,
              Eigen::Vector3d acc,
              Eigen::Vector3d omega)
    : Rwr(Rwr), qwr(qwr), twr(twr), acc(acc), omega(omega) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct PositionError {
	PositionError(const Eigen::Vector3d& pos_measured)
		: pos_measured_(pos_measured) {}

	template <typename T>
	bool operator()(const T* const pos_hat_ptr,
									T* residuals_ptr) const {
		Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
		// Eigen::Matrix<T, 3, 1> residuals(residuals_ptr);
		// residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();
		Eigen::Matrix<T, 3, 1> pos_delta = pos_hat - pos_measured_.template cast<T>();

    for (int i = 0; i < 3; i++) {
      residuals_ptr[i] = pos_delta[i];
    }
		return true;
	}

	static CostFunction* Create(const Eigen::Vector3d& pos_measured) {
		return new AutoDiffCostFunction<PositionError, 3, 3>(
			new PositionError(pos_measured)
		);
	}

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
	const Eigen::Vector3d pos_measured_;
};

struct PoseError {
  PoseError(const Eigen::Vector3d& pos_measured,
            const Eigen::Quaterniond& q_measured)
    : pos_measured_(pos_measured), q_measured_(q_measured) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const q_hat_ptr,
                  T* residuals_ptr) const {
    // Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
		// Eigen::Matrix<T, 3, 1> pos_delta = pos_hat - pos_measured_.template cast<T>();

    // Eigen::Quaternion<T> q_hat(q_hat_ptr);
    // Eigen::Quaternion<T> q_delta = q_hat.conjugate() * q_measured_.template cast<T>();
		// residuals_ptr[0] = pos_delta.norm() + T(2.0) * q_delta.vec().norm();

		Eigen::Matrix<T, 6, 1> residuals;
    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
		residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();

    Eigen::Quaternion<T> q_hat(q_hat_ptr);
    Eigen::Quaternion<T> q_delta = q_hat.conjugate() * q_measured_.template cast<T>();
		residuals.template block<3, 1>(3, 0) = T(2.0) * q_delta.vec();
    for (int i = 0; i < 6; i++) {
      residuals_ptr[i] = residuals[i];
    }

		return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& pos_measured,
                              const Eigen::Quaterniond& q_measured) {
    return new AutoDiffCostFunction<PoseError, 6, 3, 4>(
      new PoseError(pos_measured, q_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d pos_measured_;
  const Eigen::Quaterniond q_measured_;
};

std::vector<Measurement, Eigen::aligned_allocator<Measurement>> readSensorData(std::string path) {
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    Eigen::Matrix3d Rwr;
    Eigen::Quaterniond qwr;
    Eigen::Vector3d twr;
    Eigen::Vector3d acc;
    Eigen::Vector3d omega;
    Rwr << row[0], row[1], row[2],
          row[4], row[5], row[6],
          row[8], row[9], row[10];
    qwr = Rwr;
    twr << row[3], row[7], row[11];
    acc << row[16], row[17], row[18];
    omega << row[19], row[20], row[21];
    std::cout << "Rwr: " << Rwr << std::endl;
    std::cout << "qwr: " << qwr.w() << " " << qwr.vec() << std::endl;
    std::cout << "twr: " << twr << std::endl;
    std::cout << "acc: " << acc << std::endl;
    std::cout << "omega: " << omega << std::endl;

    ret.push_back(Measurement(Rwr, qwr, twr, acc, omega));
  }

  return ret;
}

void output_pose(const Eigen::Vector3d& pos,
								 const Eigen::Quaterniond& q) {
	Eigen::AngleAxisd ori(q);

	std::cout << "Location: " << pos << std::endl;
	std::cout << "Orientation: " << ori.angle() << " * " << std::endl << ori.axis() << std::endl;
}

void output_measurement(const Measurement& data) {
  std::cout << "\nData State: \n" << "R: \n" << data.Rwr << "\nt: \n" << data.twr \
            << "\nacc: \n" << data.acc << "\nomega: \n" << data.omega << std::endl;
}

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "missing arg for the csv file" << std::endl;
  }

  std::string path = argv[1];
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> data = readSensorData(path);

  output_measurement(data[0]);
  output_measurement(data[1]);
  output_measurement(data[2]);

  return 0;

  // int cnt = data.size();
  int cnt = 2;

  Eigen::Vector3d bias = Eigen::Vector3d::Random();
  std::vector<State, Eigen::aligned_allocator<State>> states(cnt);
  std::cout << "states size: " << states.size() << std::endl;
  Problem problem;

  ceres::LossFunction* loss_function = nullptr;
  ceres::LocalParameterization* quaternion_local_parameterization =
      new ceres::EigenQuaternionParameterization;

  cnt = 0;

  Eigen::Vector3d pos1 = Eigen::Vector3d::Random(); // position
	Eigen::Quaterniond q1 = Eigen::Quaterniond::UnitRandom(); // orientation

	std::cout << "Initial state: " << std::endl;
	output_pose(pos1, q1);

	ceres::CostFunction* pose_cost_function = PoseError::Create(data[0].twr,
																															data[0].qwr);
	problem.AddResidualBlock(pose_cost_function,
													 loss_function,
													 pos1.data(),
													 q1.coeffs().data());

	// for (auto& measure : data) {
  //   ceres::CostFunction* pos_cost_function = PoseError::Create(measure.twr, measure.qwr);
  //   problem.AddResidualBlock(pos_cost_function,
  //                            loss_function,
  //                            states[cnt].pos.data(),
  //                            states[cnt].q.coeffs().data());
  //   problem.SetParameterization(states[cnt].q.coeffs().data(),
  //                               quaternion_local_parameterization);

	// 	// ceres::CostFunction* position_cost_function = PositionError::Create(measure.twr);
	// 	// problem.AddResidualBlock(position_cost_function, loss_function, states[cnt].pos.data());
	//   cnt++;
  //   if (cnt >= states.size()) {
  //     break;
  //   }
  // }

  ceres::Solver::Options options;
	options.max_num_iterations = 20;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

	std::cout << "Ground Truth: " << std::endl;
	output_pose(data[0].twr, data[0].qwr);
	std::cout << "Final State: " << std::endl;
	output_pose(pos1, q1);

  output_measurement(data[0]);
  output_measurement(data[1]);
  output_measurement(data[2]);

  return 0;
}


#include <cmath>
#include <cassert>
#include <boost/math/special_functions/gamma.hpp>

#include "StudentTPrior.h"

namespace Grante {

const double StudentTPrior::pi = 3.14159265358979323846;

StudentTPrior::StudentTPrior(double dof, double sigma, unsigned int dim)
	: dof(dof), sigma(sigma) {
	// Precompute two constants
	double d = static_cast<double>(dim);
	logp_constant1 = -boost::math::lgamma(0.5*(dof+d))
		+ boost::math::lgamma(0.5*dof) + 0.5*d*std::log(dof*pi)
		+ d*log(sigma);
	logp_constant2 = dof + d;
}

StudentTPrior::~StudentTPrior() {
}

// -log p(w) = -log Gamma((dof + dim)/2) + log Gamma(dof/2)
//             + (dim/2)*log(dof * pi) + dim*log(sigma)
//             + ((dof + dim)/2)*log(1 + (1/(dof*sigma^2))*w'*w)
// \nabla_w -log p(w) = ((dof+dim)/(dof*sigma^2))
//             * (1/(1+(1/(dof*sigma^2))*w'*w)) * w.
double StudentTPrior::EvaluateNegLogP(const std::vector<double>& w,
	std::vector<double>& grad, double scale) const {
	if (grad.empty() == false) {
		assert(w.size() == grad.size());
	}
	double xnorm = 0.0;
	for (unsigned int d = 0; d < w.size(); ++d)
		xnorm += w[d]*w[d];

	double nlogp = 1.0 + xnorm/(dof*sigma*sigma);
	double scale2 = (logp_constant2 / (dof*sigma*sigma)) / nlogp;
	if (grad.empty() == false) {
		for (unsigned int d = 0; d < w.size(); ++d)
			grad[d] += scale * scale2 * w[d];
	}

	double res = 0.5*logp_constant2*std::log(nlogp) + logp_constant1;
	return (scale * res);
}

}


// find_root_example.cpp

// Copyright Paul A. Bristow 2007, 2010.

// Use, modification and distribution are subject to 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)

// Example of using root finding.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[root_find1
/*`
First we need some includes to access the normal distribution
(and some std output of course).
*/

#include <boost/math/tools/roots.hpp> // root finding.

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
#include <stdexcept>
  using std::exception;

//] //[/root_find1]

int main()
{
  cout << "Example: Normal distribution, root finding.";
  try
  {

//[root_find2

/*`A machine is set to pack 3 kg of ground beef per pack.
Over a long period of time it is found that the average packed was 3 kg
with a standard deviation of 0.1 kg.
Assuming the packing is normally distributed,
we can find the fraction (or %) of packages that weigh more than 3.1 kg.
*/

double mean = 3.; // kg
double standard_deviation = 0.1; // kg
normal packs(mean, standard_deviation);

double max_weight = 3.1; // kg
cout << "Percentage of packs > " << max_weight << " is "
<< cdf(complement(packs, max_weight)) << endl; // P(X > 3.1)

double under_weight = 2.9;
cout <<"fraction of packs <= " << under_weight << " with a mean of " << mean
  << " is " << cdf(complement(packs, under_weight)) << endl;
// fraction of packs <= 2.9 with a mean of 3 is 0.841345
// This is 0.84 - more than the target 0.95
// Want 95% to be over this weight, so what should we set the mean weight to be?
// KK StatCalc says:
double over_mean = 3.0664;
normal xpacks(over_mean, standard_deviation);
cout << "fraction of packs >= " << under_weight
<< " with a mean of " << xpacks.mean()
  << " is " << cdf(complement(xpacks, under_weight)) << endl;
// fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005
double under_fraction = 0.05;  // so 95% are above the minimum weight mean - sd = 2.9
double low_limit = standard_deviation;
double offset = mean - low_limit - quantile(packs, under_fraction);
double nominal_mean = mean + offset;

normal nominal_packs(nominal_mean, standard_deviation);
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << under_weight
  << " is " << cdf(complement(nominal_packs, under_weight)) << endl;

/*`
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95.

Setting the packer to 3.13263 will mean that fraction of packs >= 2.9 is 0.99,
but will more than double the mean loss from 0.0644 to 0.133.

Alternatively, we could invest in a better (more precise) packer with a lower standard deviation.

To estimate how much better (how much smaller standard deviation) it would have to be,
we need to get the 5% quantile to be located at the under_weight limit, 2.9
*/
double p = 0.05; // wanted p th quantile.
cout << "Quantile of " << p << " = " << quantile(packs, p)
  << ", mean = " << packs.mean() << ", sd = " << packs.standard_deviation() << endl; //
/*`
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1

With the current packer (mean = 3, sd = 0.1), the 5% quantile is at 2.8551 kg,
a little below our target of 2.9 kg.
So we know that the standard deviation is going to have to be smaller.

Let's start by guessing that it (now 0.1) needs to be halved, to a standard deviation of 0.05
*/
normal pack05(mean, 0.05);
cout << "Quantile of " << p << " = " << quantile(pack05, p)
  << ", mean = " << pack05.mean() << ", sd = " << pack05.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack05.standard_deviation()
  << " is " << cdf(complement(pack05, under_weight)) << endl;
//
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.9772

So 0.05 was quite a good guess, but we are a little over the 2.9 target,
so the standard deviation could be a tiny bit more. So we could do some
more guessing to get closer, say by increasing to 0.06
*/

normal pack06(mean, 0.06);
cout << "Quantile of " << p << " = " << quantile(pack06, p)
  << ", mean = " << pack06.mean() << ", sd = " << pack06.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack06.standard_deviation()
  << " is " << cdf(complement(pack06, under_weight)) << endl;
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.9522

Now we are getting really close, but to do the job properly,
we could use root finding method, for example the tools provided, and used elsewhere,
in the Math Toolkit, see
[link math_toolkit.toolkit.internals1.roots2  Root Finding Without Derivatives].

But in this normal distribution case, we could be even smarter and make a direct calculation.
*/
//] [/root_find2]

  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

/*
Output is:

//[root_find_output

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\find_root_example.exe"
Example: Normal distribution, root finding.Percentage of packs > 3.1 is 0.158655
fraction of packs <= 2.9 with a mean of 3 is 0.841345
fraction of packs >= 2.9 with a mean of 3.0664 is 0.951944
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1
Quantile of 0.05 = 2.91776, mean = 3, sd = 0.05
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.97725
Quantile of 0.05 = 2.90131, mean = 3, sd = 0.06
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.95221

//] [/root_find_output]
*/


#ifndef MLT_UTILS_LINEAR_ALGEBRA_HPP
#define MLT_UTILS_LINEAR_ALGEBRA_HPP

#include <algorithm>
#include <limits>

#include <Eigen/Core>
#include <Eigen/SVD>

#include "../defs.hpp"

namespace mlt {
namespace utils {
namespace linear_algebra {
	// Moore-Penrose pseudoinverse
	inline auto pseudo_inverse(MatrixXdRef x) {
		auto svd = x.jacobiSvd(ComputeThinU | ComputeThinV);

		auto tolerance = numeric_limits<double>::epsilon() * max(x.rows(), x.cols()) * svd.singularValues().maxCoeff();

		return (svd.matrixV() * svd.singularValues().unaryExpr([=](double s) { return (s < tolerance) ? 0 : 1 / s; }).eval().asDiagonal() * svd.matrixU().transpose()).eval();
	}

	inline auto covariance(MatrixXdRef x, MatrixXdRef y) {
		assert(x.cols() == y.cols());
		const auto num_observations = static_cast<double>(x.cols());
		return ((x.colwise() - (x.rowwise().sum() / num_observations)) * (y.colwise() - (y.rowwise().sum() / num_observations)).transpose() / num_observations).eval();
	}

	inline auto linear_transformation(MatrixXdRef x, MatrixXdRef w) {
		return (w * x).eval();
	}

	inline auto linear_transformation(MatrixXdRef x, MatrixXdRef w, VectorXdRef b) {
		return ((w * x).colwise() + b).eval();
	}
}
}
}
#endif


#ifndef MOCHIMOCHI_SCW_HPP_
#define MOCHIMOCHI_SCW_HPP_

#include <Eigen/Dense>
#include <boost/math/special_functions/erf.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include "../../functions/enumerate.hpp"

class SCW {
private :
  const std::size_t kDim;
  const double kC;
  const double kPhi;

private :
  Eigen::VectorXd _covariances;
  Eigen::VectorXd _means;

private :
  inline double cdf(const double x) const {
    return 0.5 * (1.0 + boost::math::erf(x / std::sqrt(2.0)));
  }

public :
  SCW(const std::size_t dim, const double c, const double eta)
    : kDim(dim),
      kC(c),
      kPhi(cdf(eta)),
      _covariances(Eigen::VectorXd::Ones(kDim)),
      _means(Eigen::VectorXd::Zero(kDim)) {

    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(c)>::max() > 0, "Hyper Parameter Error. (c > 0)");
    static_assert(std::numeric_limits<decltype(eta)>::max() > 0, "Hyper Parameter Error. (η > 0)");
    assert(dim > 0);
    assert(c > 0);
    assert(eta > 0);
  }

  virtual ~SCW() { }

private :

  double suffer_loss(const Eigen::VectorXd& f, const int label) const {
    const auto confidence = compute_confidence(f);
    return std::max(0.0, kPhi * std::sqrt(confidence) - label * _means.dot(f));
  }

  //Proposition 1
  double compute_alpha(const double m, const double n, const double v, const double ganma) const {
    const auto psi = 1.0 + kPhi * kPhi / 2.0;
    const auto zeta = 1.0 + kPhi * kPhi;
    const auto tmp1 = -m * psi + std::sqrt(m * m * std::pow(kPhi, 4.0) / 4.0 + v * kPhi * kPhi * zeta);
    const auto tmp2 = 1.0 / v * zeta * tmp1;
    return std::min(kC, std::max(0.0, tmp2));
  }

  double compute_beta(const double alpha, const double v) const {
    const auto u = std::pow(-alpha * v * kPhi + std::sqrt(alpha * alpha * v * v * kPhi * kPhi + 4.0 * v), 2.0) / 4.0;
    return alpha * kPhi / (std::sqrt(u) + v * alpha * kPhi);
  }

  double compute_confidence(const Eigen::VectorXd& f) const {
    auto confidence = 0.0;
    functions::enumerate(f.data(), f.data() + f.size(), 0,
                       [&](const int index, const double value) {
                         confidence += _covariances[index] * value * value;
                       });
    return confidence;
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    const auto v = compute_confidence(feature);
    const auto m = label * _means.dot(feature);
    const auto n = v + 1.0 / 2.0 * kC;
    const auto ganma = kPhi * std::sqrt(kPhi * kPhi * m * m * v * v + 4.0 * n * v * (n + v * kPhi * kPhi));
    const auto alpha = compute_alpha(m, n, v, ganma);
    const auto beta = compute_beta(alpha, ganma);

    if (suffer_loss(feature, label) <= 0.0) { return false; }

    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                       [&](const int index, const double value) {
                         const auto v = _covariances[index] * value;
                         _means[index] += alpha * label * v;
                         _covariances[index] -= beta * v * v;
                       });

    return true;
  }

  int predict(const Eigen::VectorXd& x) {
    return _means.dot(x) < 0.0 ? -1 : 1;
  }

  Eigen::VectorXd get_means(void) const {
    return _means;
  }

  void save(const std::string& filename) {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> covariances_vector(_covariances.data(), _covariances.data() + _covariances.size());
    std::vector<double> means_vector(_means.data(), _means.data() + _means.size());
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("phi", const_cast<double&>(kPhi));
    ar & boost::serialization::make_nvp("c", const_cast<double&>(kC));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> covariances_vector;
    std::vector<double> means_vector;
    ar & boost::serialization::make_nvp("covariances", covariances_vector);
    ar & boost::serialization::make_nvp("means", means_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("phi", const_cast<double&>(kPhi));
    ar & boost::serialization::make_nvp("c", const_cast<double&>(kC));
    _covariances = Eigen::Map<Eigen::VectorXd>(&covariances_vector[0], covariances_vector.size());
    _means = Eigen::Map<Eigen::VectorXd>(&means_vector[0], means_vector.size());
  }

};

#endif //MOCHIMOCHI_SCW_HPP_


#include <iostream>
#include <numeric>
#include <type.hpp>

#include <Eigen/Eigen>

#include "FEM/FEM1DApp.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"

#include <numeric>

using Linear = PolynomialFEMApp<1>;
using Quadratic = PolynomialFEMApp<2>;

using LinearDG = PolynomialFEMAppSD<1>;
using QuadraticDG = PolynomialFEMAppSD<2>;

class FEM1DVisualizer :public Visualizer
{
protected:

	void evaluate()
	{
		int segement_ = segemnt;

		//Homework 2
		//auto rhs = [](Float x) {return  -4 - x + Power(x, 2) - Power(x, 3) + Power(x, 4) + Cos(x) - 2 * x * Cos(x) - 2 * Sin(x); };
		//auto a = [](Float x) {return sin(x) + 2; };
		//auto c = [](Float x) {return x * x + 1; };
		auto rhs = [](Float x) {return  x; };
		auto d = [this](Float x) {return epsilon; };
		auto b = [](Float x) {return 1;	};
		auto c = [](Float x) {return 0; };

		Interval interval(0.0, 1.0);

		if (use_shishkin)
		{
			Interval interval1(0.0, 1 - 2 * epsilon * log(segement_));
			interval1.SetPartitionCount(segement_);
			Interval interval2(1 - 2 * epsilon * log(segement_), 1.0);
			interval2.SetPartitionCount(segement_);

			std::vector<Float> knot_vector(2 * segement_ - 1);

			for (int i = 0; i < segement_ - 1; ++i)
			{
				knot_vector[i] = interval1.SubInterval(i).lerp(1.0);
				knot_vector[segement_ + i] = interval2.SubInterval(i).lerp(1.0);
			}
			if (segement_ > 1)
				knot_vector[segement_ - 1] = interval1.SubInterval(segement_ - 1).lerp(1.0);

			interval.SetSubIntervalKnots(knot_vector);
		}
		else
			interval.SetPartitionCount(segement_);

		LinearDG linear(rhs, d, b, c, interval);
		QuadraticDG quadratic(rhs, d, b, c, interval);
		linear.evaluate();
		quadratic.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			quadratic_val[i] = quadratic.Value(1.0 / (Length - 1) * i);
			linear_val[i] = linear.Value(1.0 / (Length - 1) * i);

			quadratic_diff[i] = quadratic_val[i] - precise_val[i];
			linear_diff[i] = linear_val[i] - precise_val[i];
		}
	}

	void Control_UI();
	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	bool updated = true;
	int segemnt = 16;
	float epsilon = 1E-7;
	bool use_shishkin = false;

	void error(std::vector<float>& ref, std::vector<float>& eval, Float& L_1, Float& L_2, Float& L_inf)
	{
		assert(ref.size() == eval.size());

		std::vector<Float> minus(ref.size());

		for (int i = 0; i < ref.size(); ++i)
		{
			minus[i] = abs(ref[i] - eval[i]);
		}

		L_inf = *std::max_element(minus.begin(), minus.end(), [](Float a, Float b) {return a < b; });
		L_2 = sqrt(std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a * a; }) / Float(ref.size()));
		L_1 = std::accumulate(minus.begin(), minus.end(), static_cast<Float>(0), [](Float r, Float a) {return r + a; }) / Float(ref.size());
	}

public:
	void CalcAccurateRst()
	{
		Float h = 1.0 / (Length - 1);
		for (int i = 0; i < Length; ++i)
		{
			xs[i] = i * h;

			auto accurate_func = [this](Float x) {return -(-1 + exp(x / epsilon) + pow(x, 2) - exp(1 / epsilon) * pow(x, 2) + 2 * epsilon * (-1 + exp(x / epsilon) + x - exp(1 / epsilon) * x)) / (2. * (-1 + exp(1 / epsilon))); };

			if (epsilon < 1E-2)
			{
				auto accurate_func = [this](Float x) {return -exp(1 / epsilon * (x - 1)) / 2. + x * epsilon + x * x / 2.; };
				precise_val[i] = accurate_func(xs[i]);
			}
			else
			{
				precise_val[i] = accurate_func(xs[i]);
			}
		}
	}

	FEM1DVisualizer() {
		CalcAccurateRst();
		segemnt = 16;
		Float L1, L2, L_inf;
		do
		{
			evaluate();

			error(precise_val, linear_val, L1, L2, L_inf);

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

			if (segemnt == 16)
			{
				cout << segemnt << '&' << L1 << '&' << '-' << '&' << L2 << '&' << '-' << '&' << L_inf << '&' << '-' << "\\\\" << endl;
			}
			else
				cout << segemnt << '&' << L1 << '&' <<- log2(L1 / linear_L1.back()) << '&' << L2 << '&' << -log2(L2 / linear_L2.back()) << '&' << L_inf << '&' << -log2(L_inf / linear_Linf.back()) << "\\\\" << endl;

			pointcount.push_back(segemnt);
			linear_L1.push_back(L1);
			linear_L2.push_back(L2);
			linear_Linf.push_back(L_inf);
			error(precise_val, quadratic_val, L1, L2, L_inf);

			//if (segemnt == 16)
			//{
			//	cout << segemnt << '&' << L1 << '&' << '-' << '&' << L2 << '&' << '-' << '&' << L_inf << '&' << '-' << "\\\\" << endl;
			//}
			//else
			//	cout << segemnt << '&' << L1 << '&' << -log2(L1 / quadratic_L1.back()) << '&' << L2 << '&' << -log2(L2 / quadratic_L2.back()) << '&' << L_inf << '&' << -log2(L_inf / quadratic_Linf.back()) << "\\\\" << endl;

			quadratic_L1.push_back(L1);
			quadratic_L2.push_back(L2);
			quadratic_Linf.push_back(L_inf);

			segemnt *= 2;
		} while (segemnt != 8192);
		segemnt = 16;

		evaluate();
	}
	const size_t Length = 20001;

	std::vector<float> xs = std::vector<float>(Length);
	std::vector<float> precise_val = std::vector<float>(Length);
	std::vector<float> quadratic_val = std::vector<float>(Length);
	std::vector<float> linear_val = std::vector<float>(Length);
	std::vector<float> quadratic_diff = std::vector<float>(Length);
	std::vector<float> linear_diff = std::vector<float>(Length);

	std::vector<float> pointcount;
	std::vector<float> linear_L1;
	std::vector<float> linear_L2;
	std::vector<float> linear_Linf;

	std::vector<float> quadratic_L1;
	std::vector<float> quadratic_L2;
	std::vector<float> quadratic_Linf;
};

static inline ImVec2 operator-(const ImVec2& lhs, const ImVec2& rhs) { return ImVec2(lhs.x - rhs.x, lhs.y - rhs.y); }

void FEM1DVisualizer::Control_UI()
{
	if (ImGui::SliderInt("Number of segments", &segemnt, 2, 200))
	{
		segemnt = segemnt < 2 ? 2 : segemnt;
		evaluate();
	}
	if (ImGui::SliderFloat("Epsilon", &epsilon, 1E-7, 1E-1, "%.8f", ImGuiSliderFlags_Logarithmic))
	{
		evaluate();
		CalcAccurateRst();
	}
	if (ImGui::Checkbox("Use Shishkin", &use_shishkin))
	{
		evaluate();
	}
}

void FEM1DVisualizer::draw(bool* p_open)
{
	if (ImGui::BeginTabBar("FEM 1D App")) {
		if (ImGui::BeginTabItem("FEM1D"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				ImPlot::PlotLine("Precise solution", &xs[0], &precise_val[0], Length);
				ImPlot::PlotLine("FEM Quadratic", &xs[0], &quadratic_val[0], Length);
				ImPlot::PlotLine("FEM Linear", &xs[0], &linear_val[0], Length);

				ImPlot::EndPlot();
			}
			Control_UI();
			ImGui::EndTabItem();
		}
		if (ImGui::BeginTabItem("FEM1D difference"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				ImPlot::PlotLine("FEM diff Linear", &xs[0], &linear_diff[0], Length);
				ImPlot::PlotLine("FEM diff Quadratic", &xs[0], &quadratic_diff[0], Length);

				ImPlot::EndPlot();
			}
			Control_UI();
			ImGui::EndTabItem();
		}

		if (ImGui::BeginTabItem("Error Plot"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail() - ImVec2(0, 100), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus, ImPlotAxisFlags_LogScale, ImPlotAxisFlags_LogScale)) {
				ImPlot::PlotLine("Linear L1    Error", &pointcount[0], &linear_L1[0], pointcount.size());
				ImPlot::PlotLine("Linear L2    Error", &pointcount[0], &linear_L2[0], pointcount.size());
				ImPlot::PlotLine("Linear L_inf Error", &pointcount[0], &linear_Linf[0], pointcount.size());

				ImPlot::PlotLine("Quadratic L1    Error", &pointcount[0], &quadratic_L1[0], pointcount.size());
				ImPlot::PlotLine("Quadratic L2    Error", &pointcount[0], &quadratic_L2[0], pointcount.size());
				ImPlot::PlotLine("Quadratic L_inf Error", &pointcount[0], &quadratic_Linf[0], pointcount.size());

				ImPlot::EndPlot();
			}

			ImGui::EndTabItem();
		}
		ImGui::EndTabBar();
	}

	ImGui::End();
}

int main()
{
	FEM1DVisualizer visualizer;
	visualizer.RenderLoop();
}


#pragma once
#include "settings.hpp"
#if USE_EIGEN == 1
#include <Eigen/Dense>
#endif

namespace nn
{
	enum class CostType
	{
		kQuadratic,
		kCrossEntropy
	};

#if USE_EIGEN == 1
	using MatrixType = Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>;
	using CostFunction = real(*)(const MatrixType& output, const MatrixType& truth);
	using CostDerivativeFunction = MatrixType(*)(const MatrixType& output, const MatrixType& truth);

	template<CostType t> real cost(const MatrixType& output, const MatrixType& truth);
	template<CostType t> MatrixType cost_derivative(const MatrixType& output, const MatrixType& truth);

	// mse
	template<>
	real cost<CostType::kQuadratic>(const MatrixType& output, const MatrixType& truth)
	{
		if (output.rows() != truth.rows() || output.cols() != truth.cols())
			throw std::logic_error("Cost functions require equally sized matrices");
		const real c = real((truth - output).squaredNorm());
		return real(0.5) * c;
	}
	template<>
	MatrixType cost_derivative<CostType::kQuadratic>(const MatrixType& output, const MatrixType& truth)
	{
		if (output.rows() != truth.rows() || output.cols() != truth.cols())
			throw std::logic_error("Cost functions require equally sized matrices");
		return output - truth;
	}

	// cross-entropy
	template<>
	real cost<CostType::kCrossEntropy>(const MatrixType& output, const MatrixType& truth)
	{
		if (output.rows() != truth.rows() || output.cols() != truth.cols())
			throw std::logic_error("Cost functions require equally sized matrices");
		const MatrixType one = MatrixType::Ones(truth.rows(), truth.cols());
		MatrixType tmp = -truth.array() * output.unaryExpr(&logf).array() - (one - truth).array() * ((one - output).unaryExpr(&logf).array());
		auto& arr = tmp.array();
		for (int i = 0; i < arr.size(); ++i)
			if (!std::isfinite(arr(i)))
				arr(i) = 0.0f;
		return tmp.sum();
	}
	template<>
	MatrixType cost_derivative<CostType::kCrossEntropy>(const MatrixType& output, const MatrixType& truth)
	{
		if (output.rows() != truth.rows() || output.cols() != truth.cols())
			throw std::logic_error("Cost functions require equally sized matrices");
		return (output - truth);
	}
#endif
}


#include <iostream>
#include <ql/quantlib.hpp>
#include <boost/format.hpp>
#include <functional>
#include <numeric>
#include <fstream>

using namespace QuantLib;

double calculatePortfolioReturn (double proportionA, double expectedReturnA,
                                 double expectedReturnB) {
  return proportionA * expectedReturnA +
         (1 - proportionA) * expectedReturnB;
}

Volatility
calculatePortfolioRisk (double proportionA, Volatility volatilityA,
                        Volatility volatilityB,
                        double covarianceAB) {
  return std::sqrt (std::pow (proportionA, 2) * std::pow (volatilityA, 2) +
                    std::pow (1 - proportionA, 2) *
                    std::pow (volatilityB, 2) +
                    (2 * proportionA * (1 - proportionA) * covarianceAB));
}

int main () {

  Matrix covarianceMatrix (4, 4);

//row 1
  covarianceMatrix[0][0] = .40;  //Equity1-Equity1
  covarianceMatrix[0][1] = .03;  //Equity1-Equity2
  covarianceMatrix[0][2] = .02;  //Equity1-Equity3
  covarianceMatrix[0][3] = .06;  //Equity1-Equity4
//row 2
  covarianceMatrix[1][0] = .03;  //Equity2-Equity1
  covarianceMatrix[1][1] = .20;  //Equity2-Equity2
  covarianceMatrix[1][2] = .01;  //Equity2-Equity3
  covarianceMatrix[1][3] = -.06; //Equity2-Equity4
//row 3
  covarianceMatrix[2][0] = .02;  //Equity3-Equity1
  covarianceMatrix[2][1] = .01;  //Equity3-Equity2
  covarianceMatrix[2][2] = .30;  //Equity3-Equity3
  covarianceMatrix[2][3] = .03;  //Equity3-Equity4
//row 4
  covarianceMatrix[3][0] = .06;  //Equity4-Equity1
  covarianceMatrix[3][1] = -.06; //Equity4-Equity2
  covarianceMatrix[3][2] = .03;  //Equity4-Equity3
  covarianceMatrix[3][3] = .15;  //Equity4-Equity4

  std::cout << "Covariance matrix of returns: " << std::endl;
  std::cout << covarianceMatrix << std::endl;

//portfolio return vector
  Matrix portfolioReturnVector (4, 1);
  portfolioReturnVector[0][0] = .19; //Equity1
  portfolioReturnVector[1][0] = .11; //Equity2
  portfolioReturnVector[2][0] = .07; //Equity3
  portfolioReturnVector[3][0] = .08; //Equity4

  std::cout << "Portfolio return vector" << std::endl;
  std::cout << portfolioReturnVector << std::endl;

// Constant
  Rate c = .05;

// Portfolio return vector minus constant rate
  Matrix portfolioReturnVectorMinusC (4, 1);
  for (int i = 0; i < 4; ++i) {
    portfolioReturnVectorMinusC[i][0] = portfolioReturnVector[i][0] - c;
  }

  std::cout
    << boost::format ("Portfolio return vector minus constantrate (c = %f)") % c
    << std::endl;
  std::cout << portfolioReturnVectorMinusC << std::endl;

// Inverse of covariance matrix
  const Matrix &inverseOfCovarienceMatrix = inverse (covarianceMatrix);

// Z vectors
  const Matrix &portfolioAz = inverseOfCovarienceMatrix * portfolioReturnVector;
  std::cout << "Portfolio A z vector" << std::endl;
  std::cout << portfolioAz << std::endl;
  double sumOfPortfolioAz = 0.0;
  std::for_each (portfolioAz.begin (), portfolioAz.end (), [&] (Real n) {
      sumOfPortfolioAz += n;
  });

  const Matrix &portfolioBz =
    inverseOfCovarienceMatrix * portfolioReturnVectorMinusC;
  std::cout << "Portfolio B z vector" << std::endl;
  std::cout << portfolioBz << std::endl;
  double sumOfPortfolioBz = 0.0;
  std::for_each (portfolioBz.begin (), portfolioBz.end (), [&] (Real n) {
      sumOfPortfolioBz += n;
  });

// Portfolio weights
  Matrix weightsPortfolioA (4, 1);
  for (int i = 0; i < 4; ++i) {
    weightsPortfolioA[i][0] = portfolioAz[i][0] / sumOfPortfolioAz;
  }

  std::cout << "Portfolio A weights" << std::endl;
  std::cout << weightsPortfolioA << std::endl;

  Matrix weightsPortfolioB (4, 1);
  for (int i = 0; i < 4; ++i) {
    weightsPortfolioB[i][0] = portfolioBz[i][0] / sumOfPortfolioBz;
  }

  std::cout << "Portfolio B weights" << std::endl;
  std::cout << weightsPortfolioB << std::endl;

// Portfolio risk and return
  const Matrix &expectedReturnPortfolioAMatrix =
    transpose (weightsPortfolioA) * portfolioReturnVector;
  double expectedReturnPortfolioA = expectedReturnPortfolioAMatrix[0][0];
  const Matrix &variancePortfolioAMatrix =
    transpose (weightsPortfolioA) * covarianceMatrix * weightsPortfolioA;
  double variancePortfolioA = variancePortfolioAMatrix[0][0];
  double stdDeviationPortfolioA = std::sqrt (variancePortfolioA);
  std::cout << boost::format ("Portfolio A expected return: %f") %
               expectedReturnPortfolioA << std::endl;
  std::cout << boost::format ("Portfolio A variance: %f") % variancePortfolioA
            << std::endl;
  std::cout << boost::format ("Portfolio A standard deviation: %f") %
               stdDeviationPortfolioA << std::endl;

  const Matrix &expectedReturnPortfolioBMatrix =
    transpose (weightsPortfolioB) * portfolioReturnVector;
  double expectedReturnPortfolioB = expectedReturnPortfolioBMatrix[0][0];
  const Matrix &variancePortfolioBMatrix =
    transpose (weightsPortfolioB) * covarianceMatrix * weightsPortfolioB;
  double variancePortfolioB = variancePortfolioBMatrix[0][0];
  double stdDeviationPortfolioB = std::sqrt (variancePortfolioB);
  std::cout << boost::format ("Portfolio B expected return: %f") %
               expectedReturnPortfolioB << std::endl;
  std::cout << boost::format ("Portfolio B variance: %f") % variancePortfolioB
            << std::endl;
  std::cout << boost::format ("Portfolio B standard deviation: %f") %
               stdDeviationPortfolioB << std::endl;

// Covariance and correlation of returns
  const Matrix &covarianceABMatrix =
    transpose (weightsPortfolioA) * covarianceMatrix * weightsPortfolioB;
  double covarianceAB = covarianceABMatrix[0][0];
  double correlationAB =
    covarianceAB / (stdDeviationPortfolioA * stdDeviationPortfolioB);
  std::cout
    << boost::format ("Covariance of portfolio A and B: %f") % covarianceAB
    << std::endl;
  std::cout
    << boost::format ("Correlation of portfolio A and B: %f") % correlationAB
    << std::endl;

// Generate envelope set of portfolios
  double startingProportion = -.40;
  double increment = .10;
  std::map<double, std::pair<Volatility, double> > mapOfProportionToRiskAndReturn;
  std::map<Volatility, double> mapOfVolatilityToReturn;
  for (int i = 0; i < 21; ++i) {
    double proportionA = startingProportion + i * increment;
    Volatility risk_frontier = calculatePortfolioRisk (proportionA,
                                                       stdDeviationPortfolioA,
                                                       stdDeviationPortfolioB,
                                                       covarianceAB);
    double returnEF = calculatePortfolioReturn (proportionA,
                                                expectedReturnPortfolioA,
                                                expectedReturnPortfolioB);
    mapOfProportionToRiskAndReturn[proportionA] = std::make_pair (risk_frontier,
                                                                  returnEF);
    mapOfVolatilityToReturn[risk_frontier] = returnEF;
  }

// Write data to a file for plotting latter
  std::ofstream envelopeSetFile;
  envelopeSetFile.open ("./envelope.dat", std::ios::out);
  for (std::map<double, std::pair<Volatility, double> >::const_iterator i = mapOfProportionToRiskAndReturn.begin ();
       i != mapOfProportionToRiskAndReturn.end (); ++i) {
    envelopeSetFile << boost::format ("%f %f %f") % i->first % i->second.first %
                       i->second.second << std::endl;
  }
  envelopeSetFile.close ();

// Find minimum risk portfolio on efficient frontier
  std::pair<Volatility, double> minimumVariancePortfolioRiskAndReturn = *mapOfVolatilityToReturn.begin ();
  Volatility minimumRisk = minimumVariancePortfolioRiskAndReturn.first;
  double maximumReturn = minimumVariancePortfolioRiskAndReturn.second;
  std::cout << boost::format ("Maximum portfolio return for risk of %f is %f") %
               minimumRisk % maximumReturn << std::endl;

// Generate efficient frontier
  std::map<Volatility, double> efficientFrontier;
  for (std::map<double, std::pair<Volatility, double> >::const_iterator i = mapOfProportionToRiskAndReturn.begin ();
       i != mapOfProportionToRiskAndReturn.end (); ++i) {
    efficientFrontier[i->second.first] = i->second.second;
    if (i->second.first == minimumRisk) break;
  }

// Write efficient frontier to file
  std::ofstream efficient_frontier_file;
  efficient_frontier_file.open ("./efficient_frontier.dat", std::ios::out);
  for (std::map<Volatility, double>::const_iterator i = efficientFrontier.begin ();
       i != efficientFrontier.end (); ++i) {
    efficient_frontier_file << boost::format ("%f %f") % i->first % i->second
                            << std::endl;
  }
  efficient_frontier_file.close ();
  return 0;
}


#ifndef HBM_PERIODICITY_HPP
#define HBM_PERIODICITY_HPP

#include "ukp_common.hpp"
#include "wrapper.hpp"
#include "type_name.hpp"

#include <type_traits>  // For is_integral, is_same
#include <limits> // for numeric_limits
#include <boost/multiprecision/cpp_int.hpp> // For cpp_rational (infinite
                                            // precision boost rational)
                                            // used at y_star
#include <boost/math/common_factor_rt.hpp>  // For boost::math::lcm used
                                            // at huangtang

namespace hbm {
  template <typename W, typename P, typename I>
  struct per_extra_info_t : extra_info_t {
    W original_cap;
    W y_cap;

    per_extra_info_t(W original_cap, W y_cap) :
                     original_cap(original_cap),  y_cap(y_cap) { }

    virtual std::string gen_info(void) {
      return std::string("algorithm_name: y_star_periodicity_bound\n")
           + "git_head_at_compilation: " + HBM_GIT_HEAD_AT_COMPILATION + "\n"
           + "type_W: " + hbm::type_name<W>::get() + "\n"
           + "type_P: " + hbm::type_name<P>::get() + "\n"
           + "type_I: " + hbm::type_name<I>::get() + "\n"
           + "Original capacity: " + std::to_string(original_cap) + "\n"
           + "y* capacity: " + std::to_string(y_cap) + "\n";
    }
  };

  namespace hbm_periodicity_impl {
    using namespace std;
    using namespace boost;
    using namespace boost::multiprecision;

    // From "A constructive periodicity bound for the unbounded
    //  knapsack problem"
    // Its complexity is bigger than O(n^2), and it seems worse than
    // y*. The overall impression is that it is terrible.
    template <typename W, typename P>
    W huangtang(instance_t<W, P> &ukpi, bool already_sorted = false) {
      auto &items = ukpi.items;
      if (!already_sorted) sort_by_eff(items);

      size_t n = items.size();
      W h0 = 0;
      for (size_t j = 1; j < n; ++j) {
        W min = boost::math::lcm(items[0].w, items[j].w) - items[j].w;
        for (size_t i = 1; i < j; ++i) {
          W x = boost::math::lcm(items[i].w, items[j].w) - items[j].w;
          if (x < min) min = x;
        }
        h0 += min;
      }

      return h0 + 1;
    }

    template <typename W, typename P>
    W y_star(const item_t<W, P> &b, const item_t<W, P> &b2) {
      assert(b < b2);
      if (std::is_integral<W>::value && std::is_same<W, P>::value) {
        // Without the castings, the compiler gives conversion warnings
        // that don't will ever happen. If P is a floating point number
        // this 'if' never executes. The castings have literally no
        // effect since inside this if W and P are the same type.
        W w1 = b.w, w2 = b2.w;
        W p1 = static_cast<W>(b.p), p2 = static_cast<W>(b2.p);

        cpp_rational r_p1 = static_cast<W>(b.p);
        cpp_rational r1(p1, w1);
        cpp_rational r2(p2, w2);

        // If the two numbers are equal we would divide by
        // zero later. The closest value we have to infinity
        // is the best return.
        if (r1 == r2) return numeric_limits<W>::max();

        // Always positive: the r1 efficiency is bigger than the r2 efficiency
        cpp_rational d = r1 - r2;
        // Final value, the "plus one" is to avoid getting 1 less
        // than the real value when we cast back to W
        cpp_rational y = (r_p1 / d) + 1;

        return y <= numeric_limits<W>::max() ? static_cast<W>(y) : numeric_limits<W>::max();
      } else if (std::is_floating_point<P>::value) {
        W w1 = b.w, w2 = b2.w;
        P p1 = b.p, p2 = b2.p;

        P e1 = p1 / static_cast<P>(w1);
        P e2 = p2 / static_cast<P>(w2);

        if (e1 - e2 < numeric_limits<P>::epsilon())
          return numeric_limits<W>::max();

        return static_cast<W>(p1/(e1 - e2)) + 1;
      } else {
        cerr << __func__ << ": W and P aren't valid types. " << endl;
        exit(EXIT_FAILURE);
      }
    }

    template <typename W>
    W refine_y_star(W y_, W c, W w_b) {
      if (y_ > c) return c;
      W qt_b = ((c - y_) / w_b) + 1;
      return c - qt_b*w_b;
    }

    template <typename W, typename P>
    W y_star(vector< item_t<W, P> > &items, bool already_sorted = false) {
      if (!already_sorted) sort_by_eff(items, 2u);

      return hbm_periodicity_impl::y_star(items[0], items[1]);
    }

    template <typename W, typename P>
    W y_star(instance_t<W, P> &ukpi, bool already_sorted = false) {
      return y_star(ukpi.items, already_sorted);
    }

//    template <typename W, typename P, typename I>
//    W run_with_y_star(void(*ukp_solver)(instance_t<W, P> &, solution_t<W, P, I> &, void*),
//      instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, void* ukp_solver_extra_params) {
//      W y_ = y_star(ukpi, false);
//
//      if (y_ >= ukpi.c) {
//        (*ukp_solver)(ukpi, sol, ukp_solver_extra_params);
//        return y_;
//      }
//
//      vector< item_t<W, P> > &items(ukpi.items);
//
//      W old_c = ukpi.c;
//
//      W w1, p1;
//      w1 = items[0].w;
//      p1 = items[0].p;
//
//      W qt_best_item_used = (old_c - y_)/w1;
//      P profit_generated_by_best_item = static_cast<P>(qt_best_item_used)*p1;
//      W space_used_by_best_item = qt_best_item_used*w1;
//
//      ukpi.c = old_c - space_used_by_best_item;
//
//      (*ukp_solver)(ukpi, sol, true);
//
//      sol.opt += profit_generated_by_best_item;
//
//      return y_;
//    }

    template <typename W, typename P, typename I>
    void y_star_wrapper(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted = false) {
      sol.show_only_extra_info = true;
      I y_star_cap = hbm_periodicity_impl::y_star(ukpi, already_sorted);

      per_extra_info_t<W, P, I>* ptr =
        new per_extra_info_t<W, P, I>(ukpi.c, y_star_cap);

      extra_info_t* upcast_ptr = dynamic_cast<extra_info_t*>(ptr);
      sol.extra_info = std::shared_ptr<extra_info_t>(upcast_ptr);

      return;
    }

    template<typename W, typename P, typename I>
    struct y_star_wrap : wrapper_t<W, P, I> {
      virtual void operator()(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) const {
        // Calls the overloaded version with the third argument as a bool
        hbm_periodicity_impl::y_star_wrapper(ukpi, sol, already_sorted);

        return;
      }

      virtual const std::string& name(void) const {
        static const std::string name = "y_star";
        return name;
      }
    };

    template<typename W, typename P, typename I = size_t>
    void y_star_wrapper(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, int argc, argv_t argv) {
      simple_wrapper(y_star_wrap<W, P, I>(), ukpi, sol, argc, argv);
    }
  }

  /// Computes the y* periodicity bound. It's guaranteed that any optimal
  /// solution for a capacity bigger than this bound have at least one copy of
  /// the best item. Taken from Garfinkel and Nemhauser at "Integer
  /// Programming", p. 223.
  ///
  /// @param b The best item, i.e. the most efficient one.
  /// @param b2 The second best item, i.e. the second
  ///   most efficient one.
  ///
  /// @return The first capacity value that have guarantee that any optimal
  ///   solution will contain a copy of the best item.
  template <typename W, typename P>
  W y_star(const item_t<W, P> &b, const item_t<W, P> &b2) {
    return hbm_periodicity_impl::y_star(b, b2);
  }

  /// Based on the y* bound, gives you the capacity for what you
  /// should compute the UKP solution, to after fill with remaining
  /// space with copies of the best item.
  ///
  /// Don't use the y* bound as capacity. Use the value
  /// returned by this function. This value is guaranteed to be
  /// equal to or smaller than y*, and the difference between it
  /// and c is always a multiple of w_b (if y_ > c it will be c,
  /// and the difference will be zero, that is a multiple of any
  /// number). This way, to know how many copies of the best item
  /// you should add to the solution you only need to compute this
  /// value divided by w_b.
  ///
  /// @param y_ The value obtained by y_star.
  /// @param c The original capacity value of the instance.
  /// @param w_b The weight of the best item, i.e the most
  ///   efficient one.
  ///
  /// @return A safe capacity to compute the result, and then fill the
  ///   remaining space with exactly (c - <this return value>)/best_item.w
  ///   copies of the best item.
  template <typename W>
  W refine_y_star(W y_, W c, W w_b) {
    return hbm_periodicity_impl::refine_y_star(y_, c, w_b);
  }

  /// A terrible bound only implemented to access that it is indeed
  /// terrible. Kept only for comparison.
  template <typename W, typename P>
  W huangtang(instance_t<W, P> &ukpi, bool already_sorted = false) {
    return hbm_periodicity_impl::huangtang(ukpi, already_sorted);
  }

  /// Executes y_star over the two best items of ukpi.items. Assumes that ukpi
  /// has at least two items, and that if already_sorted is true, the items are
  /// ordered by non-increasing efficiency.
  ///
  /// @see y_star(const item_t<W, P> &b, const item_t<W, P> &b2)
  template <typename W, typename P>
  W y_star(instance_t<W, P> &ukpi, bool already_sorted = false) {
    return hbm_periodicity_impl::y_star(ukpi, already_sorted);
  }

//  template <typename W, typename P, typename I = size_t>
//  W run_with_y_star(void(*ukp_solver)(instance_t<W, P> &, solution_t<W, P, I> &, bool),
//    instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted = false) {
//    return hbm_periodicity_impl::run_with_y_star(ukp_solver, ukpi, sol, already_sorted);
//  }

  /// Convenience overload, executes y_star over ukpi.items and saves the
  /// "solution" (the result of y_star, not an optimal solution) to sol. A hack
  /// used to allow y_star to make use of main_take_path and benchmark_pyasukp
  /// procedures.
  ///
  /// @see main_take_path
  /// @see benchmark_pyasukp
  /// @see per_extra_info_t
  template <typename W, typename P, typename I = size_t>
  void y_star_wrapper(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted = false) {
    hbm_periodicity_impl::y_star_wrapper(ukpi, sol, already_sorted);
  }

  /// Other convenience overload, executes y_star over ukpi.items and saves the
  /// "solution" (the result of y_star, not an optimal solution) to sol. A hack
  /// used to allow y_star to make use of main_take_path and benchmark_pyasukp
  /// procedures.
  ///
  /// @see main_take_path
  /// @see benchmark_pyasukp
  /// @see per_extra_info_t
  template<typename W, typename P, typename I = size_t>
  void y_star_wrapper(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, int argc, argv_t argv) {
    hbm_periodicity_impl::y_star_wrapper(ukpi, sol, argc, argv);
  }
}

#endif //HBM_PERIODICITY_HPP


#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/ZZXFactoring.h>

#include "RSAkey.h"

using namespace std;
using namespace NTL;

// Create RSA public key from modulus and public exponent
RSAkey::RSAkey (ZZ modulus, ZZ pub_exp) {
	n = modulus;
	e = pub_exp;
}

// Create RSA private key from modulus' factors and public exponent
RSAkey::RSAkey (vector<ZZ> factors, ZZ pub_exp) {
	vector<ZZ>::iterator iter;

	f = factors;
	n = 1;
	for(iter = f.begin(); iter != f.end(); iter++)
		n *= (*iter);

	e = pub_exp;

	// get phi and set it as modulus for e*d = 1 (mod phi)
	phi = 1;
	for(iter = f.begin(); iter != f.end(); iter++)
		phi *= ((*iter) - 1);
	InvMod(d, pub_exp, phi);
}

// Create RSA private key from modulus, public exponent and private exponent
RSAkey::RSAkey (ZZ modulus, ZZ pub_exp, ZZ priv_exp) {
	n = modulus;
	e = pub_exp;
	d = priv_exp;

	// Calculate factors from d
	// Based on a generalization of pycryptodome's implementation:
	// https://github.com/Legrandin/pycryptodome/blob/master/lib/Crypto/PublicKey/RSA.py
	ZZ x, t, g, cand, fac, r;
	vector<ZZ>::iterator iter;
	x = e * d - 1;
	g = 2;

	t = x;
	while(t % 2 == 0) t/=2;

	// r: remaining composite factors
	r = n;

	while(!ProbPrime(r) && g < 100) {
		ZZ k = t;
		while(k < x) {
			PowerMod(cand, g, k, r);
			if(cand != 1 && cand != (r - 1) && SqrMod(cand, r) == 1) {
				fac = GCD(cand + 1, r);
				if(ProbPrime(fac)) {
					f.push_back(fac);
					r = r / fac;
					g = 2;
					break;
				}
			}
			k*=2;
		}
		g += 2;
	}
	if(!ProbPrime(r)) throw domain_error("Unable to compute p and q from d.");

	f.push_back(r);

	// phi = (f1 - 1)*(f2 - 1)*...*(fk - 1) for all k factors of n
	phi = 1;
	for(iter = f.begin(); iter < f.end(); iter++)
		mul(phi, phi, (*iter) - 1);
}

bool RSAkey::is_private() const {
	if (!IsZero(phi)) {
		return true;
	} else {
		return false;
	}
}

ZZ RSAkey::get_param(string param) const {
	char ch = param[0];
	if(param == "phi") {
		// o for order
		ch = 'o';
	}

	switch(ch) {
		case 'n':
			return n;
			break;
		case 'e':
			return e;
			break;
		case 'd':
			return d;
			break;
		case 'o':
			return phi;
			break;
	}
	throw domain_error("Invalid RSA parameter specified!");
}

vector<ZZ> RSAkey::get_factors() const {
	return f;
}


/*
Problem 28 - Number spiral diagonals
Starting with the number 1 and moving to the right in a clockwise direction a 5 by 5 spiral is formed as follows:

21 22 23 24 25
20  7  8  9 10
19  6  1  2 11
18  5  4  3 12
17 16 15 14 13

It can be verified that the sum of the numbers on the diagonals is 101.

What is the sum of the numbers on the diagonals in a 1001 by 1001 spiral formed in the same way?
*/

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

long long int table_size = 1001;
long long int current_step = 1;

int main() {

	Eigen::Matrix<long long int, Eigen::Dynamic, Eigen::Dynamic> table(table_size, table_size);
	for (int i = 0; i < table_size; i++) {
		for (int j = 0; j < table_size; j++) {
			table(i, j) = 0;
		}
	}
	Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> is_visited(table_size, table_size);
	for (int i = 0; i < table_size; i++) {
		for (int j = 0; j < table_size; j++) {
			is_visited(i, j) = false;
		}
	}

	// We populate the matrix
	long long int last_i = table_size / 2, last_j = table_size / 2;
	table(last_i, last_j) = current_step;
	is_visited(last_i, last_j) = true;
	long long int current_i = last_i, current_j = last_j + 1;
	while ((current_j < table_size - 1) || (current_i >0)) { // The last number is in the upper right corner
		current_step += 1;
		table(current_i, current_j) = current_step;
		is_visited(current_i, current_j) = true;
		// std::cout << current_step << " i: " << current_i << " j: " << current_j << endl;
		if ((current_j - last_j) == 1) {
			if (is_visited(current_i + 1, current_j)) {
				last_i = current_i;
				last_j = current_j;
				current_j += 1;
				// std::cout << "Right" << endl;
			}
			else {
				last_i = current_i;
				last_j = current_j;
				current_i += 1;
				// std::cout << "Down" << endl;
			}
			continue;
		}
		if ((current_i - last_i) == 1) {
			if (is_visited(current_i, current_j - 1)) {
				last_i = current_i;
				last_j = current_j;
				current_i += 1;
				// std::cout << "Down" << endl;
			}
			else {
				last_i = current_i;
				last_j = current_j;
				current_j -= 1;
				// std::cout << "Left" << endl;
			}
			continue;
		}
		if ((current_j - last_j) == -1) {
			if (is_visited(current_i - 1, current_j)) {
				last_i = current_i;
				last_j = current_j;
				current_j -= 1;
				// std::cout << "Left" << endl;
			}
			else {
				last_i = current_i;
				last_j = current_j;
				current_i -= 1;
				// std::cout << "Up" << endl;
			}
			continue;
		}
		if ((current_i - last_i) == -1) {
			if (is_visited(current_i, current_j + 1)) {
				last_i = current_i;
				last_j = current_j;
				current_i -= 1;
				// std::cout << "Up" << endl;
			}
			else {
				last_i = current_i;
				last_j = current_j;
				current_j += 1;
				// std::cout << "Right" << endl;
			}
			continue;
		}
	}
	current_step += 1;
	table(current_i, current_j) = current_step;
	is_visited(current_i, current_j) = true;
	// std::cout << current_step << " i: " << current_i << " j: " << current_j << endl;

	//Summation
	long long int result = 0;
	for (int i = 0; i < table_size; i++) {
		result += table(i, i);
	}
	// std::cout << "OK" << endl;
	for (int i = 0; i < table_size; i++) {
		result += table(table_size - i - 1, i);
	}
	// std::cout << "OK" << endl;
	result -= 1;

	std::cout << result << endl;

}


//
// Created by tr on 21-10-14.
//
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

using namespace std;

int main(){
    Eigen::Matrix3d R=Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotationVector(M_PI/4, Eigen::Vector3d(0,0,1));
    R=rotationVector.toRotationMatrix();
    cout<<"R: "<<R<<endl;

    //3points
    Eigen::Vector3d p1 =Eigen::Vector3d(1,2,3);
    Eigen::Vector3d p2 =Eigen::Vector3d(6,5,4);
    Eigen::Vector3d p3 =Eigen::Vector3d(8,7,9);

    vector<Eigen::Vector3d> points1={p1,p2,p3};

    vector<Eigen::Vector3d> points2;

    for(auto p:points1)
    {
        points2.push_back(R*p);
        cout<<"p1:"<<p<<endl;
    }

//    for (auto p2:points2)
//    {
//        cout<<"p2: "<<p2<<endl;
//    }

Eigen::Vector3d qa=Eigen::Vector3d(0,0,0);
    Eigen::Vector3d qb=Eigen::Vector3d(0,0,0);

    for(int i =0; i<points1.size();i++)
    {
        for(int j=0;j<3; j++)
        {
            qa[j]+=points1[i][j];
            qb[j]+=points2[i][j];
        }
    }
qa=qa/points1.size();qb=qb/points2.size();
cout<<"qa:"<<qa<<endl;
cout<<"qb"<<qb<<endl;

    for (int i=0; i<points1.size();i++)
    {
points1[i]=points1[i]-qa;
points2[i]=points2[i]-qb;
    }
    Eigen::Matrix3d W=Eigen::Matrix3d::Zero();
    for(int i=0;i<points1.size();i++)
    {
        W+=points1[i]*points2[i].transpose();
    }
    cout<<"W:"<<W<<endl;
//svd

Eigen::JacobiSVD<Eigen::Matrix3d> svd (W,Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d U= svd.matrixU();
    Eigen::Matrix3d V=svd.matrixV();

    Eigen::Matrix3d Rr=U*V.transpose();
    cout <<"Rr"<<Rr<<endl;

    return 0;

}


// negative_binomial_example1.cpp

// Copyright Paul A. Bristow 2007.

// Use, modification and distribution are subject to 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)

// Example 1 of using negative_binomial distribution.

//[negative_binomial_eg1_1

/*`
Based on [@http://en.wikipedia.org/wiki/Negative_binomial_distribution
a problem by Dr. Diane Evans,
Professor of Mathematics at Rose-Hulman Institute of Technology].

Pat is required to sell candy bars to raise money for the 6th grade field trip.
There are thirty houses in the neighborhood,
and Pat is not supposed to return home until five candy bars have been sold.
So the child goes door to door, selling candy bars.
At each house, there is a 0.4 probability (40%) of selling one candy bar
and a 0.6 probability (60%) of selling nothing.

What is the probability mass (density) function (pdf) for selling the last (fifth)
candy bar at the nth house?

The Negative Binomial(r, p) distribution describes the probability of k failures
and r successes in k+r Bernoulli(p) trials with success on the last trial.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and p is the probability of success).
See also [@ http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli distribution]
and [@http://www.math.uah.edu/stat/bernoulli/Introduction.xhtml Bernoulli applications].

In this example, we will deliberately produce a variety of calculations
and outputs to demonstrate the ways that the negative binomial distribution
can be implemented with this library: it is also deliberately over-commented.

First we need to #define macros to control the error and discrete handling policies.
For this simple example, we want to avoid throwing
an exception (the default policy) and just return infinity.
We want to treat the distribution as if it was continuous,
so we choose a discrete_quantile policy of real,
rather than the default policy integer_round_outwards.
*/
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real
/*`
After that we need some includes to provide easy access to the negative binomial distribution,
[caution It is vital to #include distributions etc *after* the above #defines]
and we need some std library iostream, of course.
*/
#include <boost/math/distributions/negative_binomial.hpp>
  // for negative_binomial_distribution
  using boost::math::negative_binomial; // typedef provides default type is double.
  using  ::boost::math::pdf; // Probability mass function.
  using  ::boost::math::cdf; // Cumulative density function.
  using  ::boost::math::quantile;

#include <iostream>
  using std::cout; using std::endl;
  using std::noshowpoint; using std::fixed; using std::right; using std::left;
#include <iomanip>
  using std::setprecision; using std::setw;

#include <limits>
  using std::numeric_limits;
//] [negative_binomial_eg1_1]

int main()
{
  cout <<"Selling candy bars - using the negative binomial distribution."
    << "\nby Dr. Diane Evans,"
    "\nProfessor of Mathematics at Rose-Hulman Institute of Technology,"
    << "\nsee http://en.wikipedia.org/wiki/Negative_binomial_distribution\n"
    << endl;
  cout << endl;
  cout.precision(5);
  // None of the values calculated have a useful accuracy as great this, but
  // INF shows wrongly with < 5 !
  // https://connect.microsoft.com/VisualStudio/feedback/ViewFeedback.aspx?FeedbackID=240227
//[negative_binomial_eg1_2
/*`
It is always sensible to use try and catch blocks because defaults policies are to
throw an exception if anything goes wrong.

A simple catch block (see below) will ensure that you get a
helpful error message instead of an abrupt program abort.
*/
  try
  {
/*`
Selling five candy bars means getting five successes, so successes r = 5.
The total number of trials (n, in this case, houses visited) this takes is therefore
  = sucesses + failures or k + r = k + 5.
*/
    double sales_quota = 5; // Pat's sales quota - successes (r).
/*`
At each house, there is a 0.4 probability (40%) of selling one candy bar
and a 0.6 probability (60%) of selling nothing.
*/
    double success_fraction = 0.4; // success_fraction (p) - so failure_fraction is 0.6.
/*`
The Negative Binomial(r, p) distribution describes the probability of k failures
and r successes in k+r Bernoulli(p) trials with success on the last trial.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and p is the probability of success).

We therefore start by constructing a negative binomial distribution
with parameters sales_quota (required successes) and probability of success.
*/
    negative_binomial nb(sales_quota, success_fraction); // type double by default.
/*`
To confirm, display the success_fraction & successes parameters of the distribution.
*/
    cout << "Pat has a sales per house success rate of " << success_fraction
      << ".\nTherefore he would, on average, sell " << nb.success_fraction() * 100
      << " bars after trying 100 houses." << endl;

    int all_houses = 30; // The number of houses on the estate.

    cout << "With a success rate of " << nb.success_fraction()
      << ", he might expect, on average,\n"
        "to need to visit about " << success_fraction * all_houses
        << " houses in order to sell all " << nb.successes() << " bars. " << endl;
/*`
[pre
Pat has a sales per house success rate of 0.4.
Therefore he would, on average, sell 40 bars after trying 100 houses.
With a success rate of 0.4, he might expect, on average,
to need to visit about 12 houses in order to sell all 5 bars.
]

The random variable of interest is the number of houses
that must be visited to sell five candy bars,
so we substitute k = n - 5 into a negative_binomial(5, 0.4)
and obtain the [link math.dist.pdf probability mass (density) function (pdf or pmf)]
of the distribution of houses visited.
Obviously, the best possible case is that Pat makes sales on all the first five houses.

We calculate this using the pdf function:
*/
    cout << "Probability that Pat finishes on the " << sales_quota << "th house is "
      << pdf(nb, 5 - sales_quota) << endl; // == pdf(nb, 0)
/*`
Of course, he could not finish on fewer than 5 houses because he must sell 5 candy bars.
So the 5th house is the first that he could possibly finish on.

To finish on or before the 8th house, Pat must finish at the 5th, 6th, 7th or 8th house.
The probability that he will finish on *exactly* ( == ) on any house
is the Probability Density Function (pdf).
*/
    cout << "Probability that Pat finishes on the 6th house is "
      << pdf(nb, 6 - sales_quota) << endl;
    cout << "Probability that Pat finishes on the 7th house is "
      << pdf(nb, 7 - sales_quota) << endl;
    cout << "Probability that Pat finishes on the 8th house is "
      << pdf(nb, 8 - sales_quota) << endl;
/*`
[pre
Probability that Pat finishes on the 6th house is 0.03072
Probability that Pat finishes on the 7th house is 0.055296
Probability that Pat finishes on the 8th house is 0.077414
]

The sum of the probabilities for these houses is the Cumulative Distribution Function (cdf).
We can calculate it by adding the individual probabilities.
*/
    cout << "Probability that Pat finishes on or before the 8th house is sum "
      "\n" << "pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = "
      // Sum each of the mass/density probabilities for houses sales_quota = 5, 6, 7, & 8.
      << pdf(nb, 5 - sales_quota) // 0 failures.
        + pdf(nb, 6 - sales_quota) // 1 failure.
        + pdf(nb, 7 - sales_quota) // 2 failures.
        + pdf(nb, 8 - sales_quota) // 3 failures.
      << endl;
/*`[pre
pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = 0.17367
]

Or, usually better, by using the negative binomial *cumulative* distribution function.
*/
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 8 << "th house is "
      << cdf(nb, 8 - sales_quota) << endl;
/*`[pre
Probability of selling his quota of 5 bars on or before the 8th house is 0.17367
]*/
    cout << "\nProbability that Pat finishes exactly on the 10th house is "
      << pdf(nb, 10 - sales_quota) << endl;
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 10 << "th house is "
      << cdf(nb, 10 - sales_quota) << endl;
/*`
[pre
Probability that Pat finishes exactly on the 10th house is 0.10033
Probability of selling his quota of 5 bars on or before the 10th house is 0.3669
]*/
    cout << "Probability that Pat finishes exactly on the 11th house is "
      << pdf(nb, 11 - sales_quota) << endl;
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 11 << "th house is "
      << cdf(nb, 11 - sales_quota) << endl;
/*`[pre
Probability that Pat finishes on the 11th house is 0.10033
Probability of selling his quota of 5 candy bars
on or before the 11th house is 0.46723
]*/
    cout << "Probability that Pat finishes exactly on the 12th house is "
      << pdf(nb, 12 - sales_quota) << endl;

    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 12 << "th house is "
      << cdf(nb, 12 - sales_quota) << endl;
/*`[pre
Probability that Pat finishes on the 12th house is 0.094596
Probability of selling his quota of 5 candy bars
on or before the 12th house is 0.56182
]
Finally consider the risk of Pat not selling his quota of 5 bars
even after visiting all the houses.
Calculate the probability that he /will/ sell on
or before the last house:
Calculate the probability that he would sell all his quota on the very last house.
*/
    cout << "Probability that Pat finishes on the " << all_houses
      << " house is " << pdf(nb, all_houses - sales_quota) << endl;
/*`
Probability of selling his quota of 5 bars on the 30th house is
[pre
Probability that Pat finishes on the 30 house is 0.00069145
]
when he'd be very unlucky indeed!

What is the probability that Pat exhausts all 30 houses in the neighborhood,
and *still* doesn't sell the required 5 candy bars?
*/
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << all_houses << "th house is "
      << cdf(nb, all_houses - sales_quota) << endl;
/*`
[pre
Probability of selling his quota of 5 bars
on or before the 30th house is 0.99849
]

/*`So the risk of failing even after visiting all the houses is 1 - this probability,
  ``1 - cdf(nb, all_houses - sales_quota``
But using this expression may cause serious inaccuracy,
so it would be much better to use the complement of the cdf:
So the risk of failing even at, or after, the 31th (non-existent) houses is 1 - this probability,
  ``1 - cdf(nb, all_houses - sales_quota)``
But using this expression may cause serious inaccuracy.
So it would be much better to use the complement of the cdf.
[link why_complements Why complements?]
*/
    cout << "\nProbability of failing to sell his quota of " << sales_quota
      << " bars\neven after visiting all " << all_houses << " houses is "
      << cdf(complement(nb, all_houses - sales_quota)) << endl;
/*`
[pre
Probability of failing to sell his quota of 5 bars
even after visiting all 30 houses is 0.0015101
]
We can also use the quantile (percentile), the inverse of the cdf, to
predict which house Pat will finish on.  So for the 8th house:
*/
 double p = cdf(nb, (8 - sales_quota));
 cout << "Probability of meeting sales quota on or before 8th house is "<< p << endl;
/*`
[pre
Probability of meeting sales quota on or before 8th house is 0.174
]
*/
    cout << "If the confidence of meeting sales quota is " << p
        << ", then the finishing house is " << quantile(nb, p) + sales_quota << endl;

    cout<< " quantile(nb, p) = " << quantile(nb, p) << endl;
/*`
[pre
If the confidence of meeting sales quota is 0.17367, then the finishing house is 8
]
Demanding absolute certainty that all 5 will be sold,
implies an infinite number of trials.
(Of course, there are only 30 houses on the estate,
so he can't ever be *certain* of selling his quota).
*/
    cout << "If the confidence of meeting sales quota is " << 1.
        << ", then the finishing house is " << quantile(nb, 1) + sales_quota << endl;
    //  1.#INF == infinity.
/*`[pre
If the confidence of meeting sales quota is 1, then the finishing house is 1.#INF
]
And similarly for a few other probabilities:
*/
    cout << "If the confidence of meeting sales quota is " << 0.
        << ", then the finishing house is " << quantile(nb, 0.) + sales_quota << endl;

    cout << "If the confidence of meeting sales quota is " << 0.5
        << ", then the finishing house is " << quantile(nb, 0.5) + sales_quota << endl;

    cout << "If the confidence of meeting sales quota is " << 1 - 0.00151 // 30 th
        << ", then the finishing house is " << quantile(nb, 1 - 0.00151) + sales_quota << endl;
/*`
[pre
If the confidence of meeting sales quota is 0, then the finishing house is 5
If the confidence of meeting sales quota is 0.5, then the finishing house is 11.337
If the confidence of meeting sales quota is 0.99849, then the finishing house is 30
]

Notice that because we chose a discrete quantile policy of real,
the result can be an 'unreal' fractional house.

If the opposite is true, we don't want to assume any confidence, then this is tantamount
to assuming that all the first sales_quota trials will be successful sales.
*/
    cout << "If confidence of meeting quota is zero\n(we assume all houses are successful sales)"
      ", then finishing house is " << sales_quota << endl;
/*`
[pre
If confidence of meeting quota is zero (we assume all houses are successful sales), then finishing house is 5
If confidence of meeting quota is 0, then finishing house is 5
]
We can list quantiles for a few probabilities:
*/

    double ps[] = {0., 0.001, 0.01, 0.05, 0.1, 0.5, 0.9, 0.95, 0.99, 0.999, 1.};
    // Confidence as fraction = 1-alpha, as percent =  100 * (1-alpha[i]) %
    cout.precision(3);
    for (int i = 0; i < sizeof(ps)/sizeof(ps[0]); i++)
    {
      cout << "If confidence of meeting quota is " << ps[i]
        << ", then finishing house is " << quantile(nb, ps[i]) + sales_quota
        << endl;
   }

/*`
[pre
If confidence of meeting quota is 0, then finishing house is 5
If confidence of meeting quota is 0.001, then finishing house is 5
If confidence of meeting quota is 0.01, then finishing house is 5
If confidence of meeting quota is 0.05, then finishing house is 6.2
If confidence of meeting quota is 0.1, then finishing house is 7.06
If confidence of meeting quota is 0.5, then finishing house is 11.3
If confidence of meeting quota is 0.9, then finishing house is 17.8
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.99, then finishing house is 24.8
If confidence of meeting quota is 0.999, then finishing house is 31.1
If confidence of meeting quota is 1, then finishing house is 1.#INF
]

We could have applied a ceil function to obtain a 'worst case' integer value for house.
``ceil(quantile(nb, ps[i]))``

Or, if we had used the default discrete quantile policy, integer_outside, by omitting
``#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real``
we would have achieved the same effect.

The real result gives some suggestion which house is most likely.
For example, compare the real and integer_outside for 95% confidence.

[pre
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.95, then finishing house is 21
]
The real value 20.1 is much closer to 20 than 21, so integer_outside is pessimistic.
We could also use integer_round_nearest policy to suggest that 20 is more likely.

Finally, we can tabulate the probability for the last sale being exactly on each house.
*/
   cout << "\nHouse for " << sales_quota << "th (last) sale.  Probability (%)" << endl;
   cout.precision(5);
   for (int i = (int)sales_quota; i < all_houses+1; i++)
   {
     cout << left << setw(3) << i << "                             " << setw(8) << cdf(nb, i - sales_quota)  << endl;
   }
   cout << endl;
/*`
[pre
House for 5 th (last) sale.  Probability (%)
5                               0.01024
6                               0.04096
7                               0.096256
8                               0.17367
9                               0.26657
10                              0.3669
11                              0.46723
12                              0.56182
13                              0.64696
14                              0.72074
15                              0.78272
16                              0.83343
17                              0.874
18                              0.90583
19                              0.93039
20                              0.94905
21                              0.96304
22                              0.97342
23                              0.98103
24                              0.98655
25                              0.99053
26                              0.99337
27                              0.99539
28                              0.99681
29                              0.9978
30                              0.99849
]

As noted above, using a catch block is always a good idea, even if you do not expect to use it.
*/
  }
  catch(const std::exception& e)
  { // Since we have set an overflow policy of ignore_error,
    // an overflow exception should never be thrown.
     std::cout << "\nMessage from thrown exception was:\n " << e.what() << std::endl;
/*`
For example, without a ignore domain error policy, if we asked for ``pdf(nb, -1)`` for example, we would get:
[pre
Message from thrown exception was:
 Error in function boost::math::pdf(const negative_binomial_distribution<double>&, double):
 Number of failures argument is -1, but must be >= 0 !
]
*/
//] [/ negative_binomial_eg1_2]
  }
   return 0;
}  // int main()

/*

Output is:

Selling candy bars - using the negative binomial distribution.
by Dr. Diane Evans,
Professor of Mathematics at Rose-Hulman Institute of Technology,
see http://en.wikipedia.org/wiki/Negative_binomial_distribution
Pat has a sales per house success rate of 0.4.
Therefore he would, on average, sell 40 bars after trying 100 houses.
With a success rate of 0.4, he might expect, on average,
to need to visit about 12 houses in order to sell all 5 bars.
Probability that Pat finishes on the 5th house is 0.01024
Probability that Pat finishes on the 6th house is 0.03072
Probability that Pat finishes on the 7th house is 0.055296
Probability that Pat finishes on the 8th house is 0.077414
Probability that Pat finishes on or before the 8th house is sum
pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = 0.17367
Probability of selling his quota of 5 bars
on or before the 8th house is 0.17367
Probability that Pat finishes exactly on the 10th house is 0.10033
Probability of selling his quota of 5 bars
on or before the 10th house is 0.3669
Probability that Pat finishes exactly on the 11th house is 0.10033
Probability of selling his quota of 5 bars
on or before the 11th house is 0.46723
Probability that Pat finishes exactly on the 12th house is 0.094596
Probability of selling his quota of 5 bars
on or before the 12th house is 0.56182
Probability that Pat finishes on the 30 house is 0.00069145
Probability of selling his quota of 5 bars
on or before the 30th house is 0.99849
Probability of failing to sell his quota of 5 bars
even after visiting all 30 houses is 0.0015101
Probability of meeting sales quota on or before 8th house is 0.17367
If the confidence of meeting sales quota is 0.17367, then the finishing house is 8
 quantile(nb, p) = 3
If the confidence of meeting sales quota is 1, then the finishing house is 1.#INF
If the confidence of meeting sales quota is 0, then the finishing house is 5
If the confidence of meeting sales quota is 0.5, then the finishing house is 11.337
If the confidence of meeting sales quota is 0.99849, then the finishing house is 30
If confidence of meeting quota is zero
(we assume all houses are successful sales), then finishing house is 5
If confidence of meeting quota is 0, then finishing house is 5
If confidence of meeting quota is 0.001, then finishing house is 5
If confidence of meeting quota is 0.01, then finishing house is 5
If confidence of meeting quota is 0.05, then finishing house is 6.2
If confidence of meeting quota is 0.1, then finishing house is 7.06
If confidence of meeting quota is 0.5, then finishing house is 11.3
If confidence of meeting quota is 0.9, then finishing house is 17.8
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.99, then finishing house is 24.8
If confidence of meeting quota is 0.999, then finishing house is 31.1
If confidence of meeting quota is 1, then finishing house is 1.#J
House for 5th (last) sale.  Probability (%)
5                               0.01024
6                               0.04096
7                               0.096256
8                               0.17367
9                               0.26657
10                              0.3669
11                              0.46723
12                              0.56182
13                              0.64696
14                              0.72074
15                              0.78272
16                              0.83343
17                              0.874
18                              0.90583
19                              0.93039
20                              0.94905
21                              0.96304
22                              0.97342
23                              0.98103
24                              0.98655
25                              0.99053
26                              0.99337
27                              0.99539
28                              0.99681
29                              0.9978
30                              0.99849

*/