/*
Copyright(c) 2014 Akihiro Nishimura

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

#define _SCL_SECURE_NO_WARNINGS

#include "mrlda.h"
#include "../helper/mapreduce_module.h"
#include "SigUtil/lib/calculation/ublas.hpp"
#include "SigUtil/lib/calculation/for_each.hpp"
#include "SigUtil/lib/calculation/assign_operation.hpp"
#include "SigUtil/lib/distance/norm.hpp"
#include "SigUtil/lib/tools/convergence.hpp"
#include "SigUtil/lib/file.hpp"
#include <boost/numeric/ublas/io.hpp>

namespace sigtm
{
const uint MrLDA:: max_local_iteration_ = 100;
const double MrLDA::global_convergence_threshold = 0.0001;
const double alpha_convergence_threshold = 0.001;
const uint max_alpha_update_iteration = 100;

const auto resume_info_fname = SIG_TO_FPSTR("mrlda_info");
const auto resume_alpha_fname = SIG_TO_FPSTR("mrlda_alpha");
const auto resume_gamma_fname = SIG_TO_FPSTR("mrlda_gamma");
const auto resume_phi_fname = SIG_TO_FPSTR("mrlda_phi");

void MrLDA::MapTask::process(value_type const& value, VectorK<double>& gamma, MatrixVK<double>& omega) const
{
	uint const vnum = value.vnum_;
	uint const knum = value.knum_;
	VectorK<double> const& alpha = *value.alpha_;
	MatrixKV<double> const& phi = *value.phi_;
	VectorV<uint> const& word_ct = *value.word_ct_;
	//VectorK<double> prev_gamma(value.knum_, 0);
	VectorK<double> sigma(value.knum_, 0);

	//while (!is_convergence(gamma, prev_gamma)){
	for (uint i = 0; i < max_local_iteration_; ++i){
		const auto exp_digamma = sig::map([](double g){ return std::exp(digamma(g)); }, gamma);

		for (uint v = 0; v<vnum; ++v){
			for (uint k = 0; k<knum; ++k){
				// update omega
				omega[v][k] = phi[k][v] * exp_digamma[k];
			}
			sig::normalize_dist(omega[v]);
			const uint wn = word_ct[v];
			sig::compound_assignment([wn](double& s, double p){ s += wn * p; }, sigma, omega[v]);
		}
		//prev_gamma = std::move(gamma);
		gamma = sig::plus(alpha, sigma);
	}
}

void MrLDA::init(bool resume)
{
	doc_word_ct_ = SIG_INIT_MATRIX(uint, D, V, 0);
	for (auto const& t : input_data_->tokens_){
		++doc_word_ct_[t.doc_id][t.word_id];
	}

	term_score_ = SIG_INIT_MATRIX(double, K, V, 0);

	auto base_pass = sig::modify_dirpass_tail(input_data_->getWorkingDirectory(), true);

	if (resume){
		auto load_info = sig::load_line(base_pass + resume_info_fname);
		if (sig::isJust(load_info)){
			auto info = sig::fromJust(load_info);
			total_iter_ct_ = std::stoul(info[0]);
		}

		auto tmp_alpha = std::move(alpha_);
		auto load_alpha = sig::load_num<double, VectorK<double>>(base_pass + resume_alpha_fname, " ");
		if (sig::isJust(load_alpha)){
			alpha_ = std::move(sig::fromJust(load_alpha));
			std::cout << "resume alpha" << std::endl;
		}
		else{
			alpha_ = std::move(tmp_alpha);
			std::cout << "resume alpha error : alpha is set as default" << std::endl;
		}
	}

	auto load_gamma = resume ? sig::load_num2d<double, MatrixDK<double>>(base_pass + resume_gamma_fname, " ") : nothing;
	if (sig::isJust(load_gamma)){
		gamma_ = std::move(sig::fromJust(load_gamma));
		std::cout << "resume gamma" << std::endl;
	}
	else{
		gamma_ = MatrixDK<double>(D_, VectorK<double>(K_));
		for (DocumentId d = 0; d<D_; ++d){
			for (TopicId k=0; k<K_; ++k){
				gamma_[d][k] = alpha_[k] + rand_d_();
			}
			bool ck = sig::normalize_dist(gamma_[d]);
		}
		if (resume) std::cout << "resume gamma error : gamma is set by random" << std::endl;
	}

	auto load_beta = resume ? sig::load_num2d<double, MatrixKV<double>>(base_pass + resume_phi_fname, " ") : nothing;
	if (sig::isJust(load_beta)){
		phi_ = std::move(sig::fromJust(load_beta));
		std::cout << "resume phi" << std::endl;
	}
	else{
		phi_ = MatrixKV<double>(K_, VectorV<double>(V_));
		for (TopicId k = 0; k<K_; ++k){
			for (WordId v = 0; v<V_; ++v){
				phi_[k][v] = eta_[k][v] + rand_d_();
			}
			bool ck = sig::normalize_dist(phi_[k]);
		}
		if (resume) std::cout << "resume phi error : phi is set by random" << std::endl;
	}
}

void MrLDA::saveResumeData() const
{
	std::cout << "save resume data... ";

	auto base_pass = input_data_->getWorkingDirectory();

	sig::save_num(alpha_, base_pass + resume_alpha_fname, " ");
	sig::save_num(gamma_, base_pass + resume_gamma_fname, " ");
	sig::save_num(phi_, base_pass + resume_phi_fname, " ");

	sig::clear_file(base_pass + resume_info_fname);
	sig::save_line(total_iter_ct_, base_pass + resume_info_fname, sig::WriteMode::append);

	std::cout << "completed" << std::endl;
}

double MrLDA::calcLiklihood(double term2, double term4) const
{
	return D_ * calcModule0(alpha_) + term2 + term3_ + term4;
}

void updateAlpha(VectorK<double>& alpha, VectorK<double> const& sufficient_statistics, const int D)
{
	const uint K = alpha.size();

	const auto calc_gradient = [D](double old_alpha, double suff_stat, double old_alpha_sum_digamma)
	{
		std::cout << "grad:" << D*old_alpha_sum_digamma << "," << -D*digamma(old_alpha) << "," << suff_stat << std::endl;
		return D * (old_alpha_sum_digamma - digamma(old_alpha)) + suff_stat;
	};

	const auto calc_hessian = [D](uint row, uint col, VectorK<double> const& old_alpha, double old_alpha_sum_trigamma)
	{
		return row == col ? D * (trigamma(old_alpha[col]) - old_alpha_sum_trigamma) : D * (-old_alpha_sum_trigamma);
	};

	auto regulate = [&](VectorK<double> const& delta, uint& decay) ->VectorK<double>
	{
		while (true){
			bool f = true;
			auto corr_delta = sig::mult(std::pow(0.8, decay), delta);

			std::cout << "delta:";
			for (uint i=0; i<K; ++i){
				std::cout << corr_delta[i] << ", ";
				if (alpha[i] < std::abs(corr_delta[i])){
					f = false;
					++decay;
					break;
				}
			}
			std::cout << std::endl;
			if (f) return corr_delta;
		}
	};

	uint iteration_count = 0;
	uint decay = 0;
	sig::ManageConvergence<VectorK<double>> convergence(alpha_convergence_threshold, sig::norm_L2);

	while (true) {
		const double alpha_sum = sig::sum(alpha);
		if (++iteration_count > max_alpha_update_iteration || convergence.update(alpha)) break;

		sig::vector_u<double> gradient(K);
		sig::matrix_u<double> hessian(K, K);
		auto alpha_sum_digamma = digamma(alpha_sum);
		auto alpha_sum_trigamma = trigamma(alpha_sum);

		for (uint i = 0; i < K; ++i){
			gradient(i) = calc_gradient(alpha[i], sufficient_statistics[i], alpha_sum_digamma);
			for (uint j = 0; j < K; ++j){
				hessian(i, j) = calc_hessian(i, j, alpha, alpha_sum_trigamma);
			}
		}

		auto delta = sig::from_vector_ublas(boost::numeric::ublas::prod(sig::fromJust(sig::invert_matrix(hessian)), gradient));
		auto corr_delta = regulate(delta, decay);

		sig::compound_assignment(
			[](double& a, double d){ a -= d; },
			alpha, corr_delta
		);

		std::cout << "alpha:";
		for (auto e : alpha) std::cout << e << ", ";
		std::cout << std::endl;
	}
	std::cout << "ct:" << iteration_count << std::endl;
}

/*
void updateAlpha(VectorK<double>& alpha, VectorK<double> const& sufficient_statistics, const int D)
{
	const uint K = alpha.size();
	uint iteration_count = 0;
	sig::ManageConvergence<VectorK<double>, sig::RelativeError, sig::Norm<1>> convergence(alpha_convergence_threshold, sig::norm_L1);

	VectorK<double> hessian(K);
	VectorK<double> gradient(K);
	VectorK<double> alpha_new(K);

	int decay = 0;
	while (true) {
		double sumG_H = 0;
		double sum1_H = 0;

		const double alpha_sum = sig::sum(alpha);
		if (++iteration_count > max_alpha_update_iteration || convergence.update(alpha)) break;

		for (int i = 0; i < K; i++) {
			gradient[i] =K * (digamma(alpha_sum) - digamma(alpha[i])) + sufficient_statistics[i];

			hessian[i] = -D * trigamma(alpha[i]);

			sumG_H += gradient[i] / hessian[i];
			sum1_H += 1 / hessian[i];
		}

		double z = D * trigamma(alpha_sum);
		double c = sumG_H / (1 / z + sum1_H);

		while (true) {
			bool singular = false;

			std::cout << "stepsize:";
			for (int i = 0; i < K; i++) {
				double step_size = std::pow(0.8, decay) * (gradient[i] - c) / hessian[i];
				std::cout << step_size << ", ";
				if (alpha[i] < std::abs(step_size)){
					// the current hessian matrix is singular
					singular = true;
					break;
				}
				alpha_new[i] = alpha[i] - step_size;
			}
			std::cout << std::endl;

			if (singular) {
				decay++;
				alpha_new = alpha;
			}
			else {
				break;
			}
		}
		alpha = alpha_new;
		std::cout << "alpha:" << alpha[0] << "," << alpha[1] << std::endl;
	}
	std::cout << "ct:" << iteration_count << std::endl;
}
*/

double MrLDA::iteration()
{
	auto updatePhi = [&](TopicId k, WordId v, double delta){
		phi_[k][v] = eta_[k][v] + delta;
	};

	double term2;
	VectorK<double> alpha_sufficient_statistics(K_, 0);

	mapreduce_->run<mapreduce::schedule_policy::cpu_parallel<mr_job>>(performance_result_);

	for (auto it = mapreduce_->begin_results(), end = mapreduce_->end_results(); it != end; ++it){
		if (std::get<0>(it->first) == ReduceKeyType::Lambda){
			updatePhi(std::get<1>(it->first), std::get<2>(it->first), it->second);
		}
		else if (std::get<0>(it->first) == ReduceKeyType::Alpha){
			alpha_sufficient_statistics[std::get<2>(it->first)] = it->second;
		}
		else{
			term2 = it->second;
		}
	}
	//updateAlpha(alpha_, alpha_sufficient_statistics, D_);

	for(auto& b : phi_) sig::normalize_dist(b);

	saveResumeData();

	return 0;//calcLiklihood(term2, sig::sum(lambda_, [&](VectorV<double> const& row){ return calcModule0(row); }));
}

void MrLDA::train(uint iteration_num, std::function<void(LDA const*)> callback)
{
	sig::ManageConvergenceSimple convergence(global_convergence_threshold);

	/*while (convergence.update(iteration())){
		std::cout << convergence.get_value() << std::endl;
	}*/

	for (uint i = 0; i < iteration_num; ++i, ++total_iter_ct_){
		iteration();
		calcTermScore(getPhi(), term_score_);
		save(Distribution::TOPIC, L"./test data");
		save(Distribution::TERM_SCORE, L"./test data");
		save(Distribution::DOCUMENT, L"./test data");
		callback(this);
		initMR();
	}
}

void MrLDA::save(Distribution target, FilepassString save_folder, bool detail) const
{
	save_folder = sig::modify_dirpass_tail(save_folder, true);

	switch (target){
	case Distribution::DOCUMENT:
		printTopic(getTheta(), input_data_->getInputFileNames(), save_folder + SIG_TO_FPSTR("document_mrlda"));
		break;
	case Distribution::TOPIC:
		printWord(getPhi(), std::vector<FilepassString>(), input_data_->words_, detail ? nothing : sig::Just<uint>(20), save_folder + SIG_TO_FPSTR("topic_mrlda"));
		break;
	case Distribution::TERM_SCORE:
		printWord(getTermScore(), std::vector<FilepassString>(), input_data_->words_, detail ? nothing : sig::Just<uint>(20), save_folder + SIG_TO_FPSTR("term-score_mrlda"));
		break;
	default:
		std::cout << "MrLDA::save error" << std::endl;
		getchar();
	}
}

auto MrLDA::getTheta() const->MatrixDK<double>
{
	MatrixDK<double> theta;

	for (DocumentId d = 0; d < D_; ++d){
		theta.push_back(getTheta(d));
	}
	return theta;
}

auto MrLDA::getTheta(DocumentId d_id) const->VectorK<double>
{
	double sum = sig::sum(gamma_[d_id]);
	// computed from the variational distribution
	return sig::map([sum](double g){ return g / sum; }, gamma_[d_id]);
}

auto MrLDA::getPhi() const->MatrixKV<double>
{
	MatrixKV<double> phi;

	for (TopicId k = 0; k < K_; ++k){
		phi.push_back(getPhi(k));
	}
	return phi;
}

auto MrLDA::getPhi(TopicId k_id) const->VectorV<double>
{
	/*double sum = sig::sum(lambda_[k_id]);
	// computed from the variational distribution
	return sig::map([sum](double l){ return l / sum; }, lambda_[k_id]);*/
	return phi_[k_id];
}

auto MrLDA::getWordOfTopic(Distribution target, uint return_word_num) const->VectorK< std::vector< std::tuple<std::wstring, double> > >
{
	VectorK< std::vector< std::tuple<std::wstring, double> > > result;

	for (TopicId k = 0; k < K_; ++k){
		result.push_back(getWordOfTopic(target, return_word_num, k));
	}
	return result;
}

auto MrLDA::getWordOfTopic(Distribution target, uint return_word_num, TopicId k_id) const->std::vector< std::tuple<std::wstring, double> >
{
	std::vector< std::tuple<std::wstring, double> > result;
	std::vector<double> df;

	if (target == Distribution::TOPIC) df = getPhi(k_id);
	else if (target == Distribution::TERM_SCORE) df = getTermScore(k_id);
	else{
		std::cout << "MrLDA::getWordOfTopic : Distributionが無効" << std::endl;
		return result;
	}
	return getTopWords(df, return_word_num, input_data_->words_);
}

auto MrLDA::getWordOfDocument(uint return_word_num) const->VectorD< std::vector< std::tuple<std::wstring, double> > >
{
	VectorD< std::vector< std::tuple<std::wstring, double> > > result;

	for (DocumentId d = 0; d<D_; ++d){
		result.push_back(getWordOfDocument(return_word_num, d));
	}
	return result;
}

auto MrLDA::getWordOfDocument(uint return_word_num, DocumentId d_id) const->std::vector< std::tuple<std::wstring, double> >
{
	std::vector< std::tuple<std::wstring, double> > result;
	auto top_wscore = getTermScoreOfDocument(getTheta(d_id), getTermScore());

	for (uint i = 0; i<return_word_num; ++i){
		result.push_back(std::make_tuple(*input_data_->words_.getWord(std::get<0>(top_wscore[i])), std::get<1>(top_wscore[i])));
	}
	return std::move(result);
}

}


#ifndef ROBOTMODEL_H
#define ROBOTMODEL_H

#include <dart/dart.hpp>
#include <dart/utils/utils.hpp>
#include <dart/utils/urdf/urdf.hpp>
#include <Eigen/Dense>

class RobotModel
{
private:
    RobotModel();

    /*
     * Update Ig, Ag, Jg
     * , where \dot{h} = I_{cm} * \dot{x}_{cm}
     *                 = A_{cm} * \dot{q}
     *          J_{cm} = I_{cm}^{-1} * A_{cm}
     *    \dot{x}_{cm} = J_{cm} * \dot{q}
     */
    void _updateCentroidFrame(const Eigen::VectorXd & q_,
                              const Eigen::VectorXd & qdot_);

    dart::dynamics::SkeletonPtr mSkel;
    int mNumDof;
    Eigen::VectorXd mInitialPositions;
    Eigen::MatrixXd mIcent;
    Eigen::MatrixXd mJcent;
    Eigen::MatrixXd mAcent;

public:
    static RobotModel* getRobotModel();
    virtual ~RobotModel();

    void updateModel(const Eigen::VectorXd & q_, const Eigen::VectorXd & qdot_);
    int getNumDofs();
    dart::dynamics::SkeletonPtr getSkeleton(); // will be depricated
    Eigen::MatrixXd getMassMatrix();
    double getTotalMass();
    Eigen::VectorXd getCOM(const dart::dynamics::Frame* _wrt=dart::dynamics::Frame::World());
    Eigen::VectorXd getCoriolisAndGravityForces();
    Eigen::MatrixXd getCentroidInertia();
    Eigen::MatrixXd getCentroidJacobian();
    Eigen::VectorXd getInitialPositions();
    std::pair<Eigen::VectorXd, Eigen::VectorXd> getJointPositionLimits();
    std::pair<Eigen::VectorXd, Eigen::VectorXd> getJointVelocityLimits();
    std::pair<Eigen::VectorXd, Eigen::VectorXd> getJointAccelerationLimits();
    std::pair<Eigen::VectorXd, Eigen::VectorXd> getJointForceLimits();

};

#endif /* ROBOTMODEL_H */


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_y0.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_y0"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_y0(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_y0(v_t(0)), v_t);
};

EVE_TEST( "Check behavior of cyl_bessel_y0 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 20000.0))
        )
  <typename T>(T const& a0)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_y0 =  [](auto x) { return eve::cyl_bessel_y0(x); };
  auto std__cyl_bessel_y0 =  [](auto x)->v_t { return boost::math::cyl_neumann(v_t(0), x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_y0(eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y0(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y0(eve::minf(eve::as< T>())), eve::nan(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y0(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_y0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(1500)), std__cyl_bessel_y0(v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(500)), std__cyl_bessel_y0(v_t(500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(10)), std__cyl_bessel_y0(v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(5)),  std__cyl_bessel_y0(v_t(5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(2)),  std__cyl_bessel_y0(v_t(2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(1.5)),std__cyl_bessel_y0(v_t(1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(0.5)),std__cyl_bessel_y0(v_t(0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(1)),  std__cyl_bessel_y0(v_t(1))   , 2.5);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0(v_t(0)),  eve::minf(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(1500)), T(std__cyl_bessel_y0(v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(500)),  T(std__cyl_bessel_y0(v_t(500)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(10)) ,  T(std__cyl_bessel_y0( v_t(10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(5))  ,  T(std__cyl_bessel_y0( v_t(5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(2))  ,  T(std__cyl_bessel_y0( v_t(2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(1.5)),  T(std__cyl_bessel_y0( v_t(1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(0.5)),  T(std__cyl_bessel_y0( v_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(1))  ,  T(std__cyl_bessel_y0( v_t(1))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_y0( T(0))  , eve::minf(eve::as< T>()), 0.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_y0(a0), map(std__cyl_bessel_y0, a0), 0.001);
};


//
//   Project Name:        Kratos
//   Last modified by:    $Author: pooyan $
//   Date:                $Date: 2008-07-09 13:14:17 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes

#if defined(KRATOS_PYTHON)
// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "petsc_application.h"
#include "petsc_space.h"
#include "spaces/ublas_space.h"
#include "add_petsc_linear_solvers_to_python.h"
#include "custom_strategies/builder_and_solvers/petsc_residualbased_elimination_builder_and_solver.h"
#include "includes/model_part.h"

//strategies
#include "solving_strategies/strategies/solving_strategy.h"
#include "solving_strategies/strategies/residualbased_linear_strategy.h"
#include "solving_strategies/strategies/residualbased_newton_raphson_strategy.h"

//schemes
#include "solving_strategies/schemes/scheme.h"
#include "custom_strategies/schemes/petsc_residualbased_incrementalupdate_static_scheme.h"

//convergence criterias
#include "solving_strategies/convergencecriterias/convergence_criteria.h"
#include "solving_strategies/convergencecriterias/displacement_criteria.h"
//#include "solving_strategies/convergencecriterias/residual_criteria.h"
//#include "solving_strategies/convergencecriterias/and_criteria.h"

//Builder And Solver
#include "solving_strategies/builder_and_solvers/builder_and_solver.h"

//linear solvers
#include "linear_solvers/linear_solver.h"

//utilities
#include "python/pointer_vector_set_python_interface.h"

namespace Kratos
{

namespace Python
{

  using namespace boost::python;

  void EraseAll(std::string& ThisString, std::string ToBeRemoved)
  {
    int position;
    while( (position = ThisString.find_first_of(ToBeRemoved)) >= 0 )
      {
	ThisString.erase(position ,ToBeRemoved.size() );
      }

  }

  std::string ErrorCleaner(std::string const& Input)
  {
    std::string output(Input);

    EraseAll(output, "boost::numeric::");

    return output;
  }

  typedef PetscSpace<Mat, Vec> PetscSparseSpaceType;
  typedef UblasSpace<double, Matrix, Vector> PetscLocalSpaceType;

	//ADDED BY PAOLO (next two)
	double Dot(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType& rX, PetscSparseSpaceType::VectorType& rY)
	{   return dummy.Dot(rX,rY);}

	void ScaleAndAdd(PetscSparseSpaceType& dummy, const double A, const PetscSparseSpaceType::VectorType& rX, const double B, PetscSparseSpaceType::VectorType& rY)
	//(const double A,const  VectorType& rX, const double B, VectorType& rY) // rY = (A * rX) + (B * rY)
	{	dummy.ScaleAndAdd(A,rX,B, rY);	}

	void Mult(PetscSparseSpaceType& dummy, PetscSparseSpaceType::MatrixType& rA, PetscSparseSpaceType::VectorType& rX, PetscSparseSpaceType::VectorType& rY)
	//rY=A*rX (the product is stored inside the rY)
	{   dummy.Mult(rA, rX, rY);}

// 	void TransposeMult(PetscSparseSpaceType& dummy, PetscSparseSpaceType::MatrixType& rA, PetscSparseSpaceType::VectorType& rX, PetscSparseSpaceType::VectorType& rY)
// 	//rY=A*rX (the product is stored inside the rY)
// 	{   dummy.TransposeMult(rA, rX, rY);}

	PetscSparseSpaceType::IndexType Size(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType const& rV)
	{return dummy.Size(rV);}

    void ResizeMatrix(PetscSparseSpaceType& dummy, PetscSparseSpaceType::MatrixType& A, unsigned int i1, unsigned int i2)
	{	dummy.Resize(A,i1,i2);	}

	void ResizeVector(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType& x, unsigned int i1)
	{	dummy.Resize(x,i1);	}

	void SetToZeroMatrix(PetscSparseSpaceType& dummy, PetscSparseSpaceType::MatrixType& A)
	{	dummy.SetToZero(A);	}

	void SetToZeroVector(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType& x)
	{	dummy.SetToZero(x);	}

	void ClearMatrix(PetscSparseSpaceType& dummy, PetscSparseSpaceType::MatrixType& A)
	{	dummy.Clear(A);	}

	void ClearVector(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType& x)
	{	dummy.Clear(x);	}

	double TwoNorm(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType& x)
	{	return dummy.TwoNorm(x);	}

	void UnaliasedAdd(PetscSparseSpaceType& dummy, PetscSparseSpaceType::VectorType& x, const double A, const PetscSparseSpaceType::VectorType& rY) // x+= a*Y
	{	dummy.UnaliasedAdd(x,A,rY);	}

  void MoveMesh( Scheme< PetscSparseSpaceType, PetscLocalSpaceType >& dummy, ModelPart::NodesContainerType& rNodes)
  {
    for(ModelPart::NodeIterator i = rNodes.begin() ; i != rNodes.end() ; ++i)
      {
	const array_1d<double,3>& disp = i->FastGetSolutionStepValue(DISPLACEMENT);
	(i)->X() = (i)->X0() + disp[0];
	(i)->Y() = (i)->Y0() + disp[1];
	(i)->Z() = (i)->Z0() + disp[2];
      }
  }

  BOOST_PYTHON_MODULE(KratosPetscApplication)
  {

	  class_<KratosPetscApplication,
			  KratosPetscApplication::Pointer,
			  bases<KratosApplication>, boost::noncopyable >("KratosPetscApplication")
			;

	  typedef LinearSolver<PetscSparseSpaceType, PetscLocalSpaceType > PetscLinearSolverType;
	bool (PetscLinearSolverType::*pointer_to_solve)(PetscLinearSolverType::SparseMatrixType& rA, PetscLinearSolverType::VectorType& rX, PetscLinearSolverType::VectorType& rB) = &PetscLinearSolverType::Solve;
	  class_<PetscLinearSolverType, PetscLinearSolverType::Pointer>("PetscLinearSolver")
	    .def("Initialize",&PetscLinearSolverType::Initialize)
	    .def("Solve",pointer_to_solve)
		  //.def("",&LinearSolverType::)
		  .def(self_ns::str(self))
		  ;

	  typedef SolvingStrategy< PetscSparseSpaceType, PetscLocalSpaceType, PetscLinearSolverType > PetscBaseSolvingStrategyType;
	  typedef Scheme< PetscSparseSpaceType, PetscLocalSpaceType > PetscBaseSchemeType;
	  typedef BuilderAndSolver< PetscSparseSpaceType, PetscLocalSpaceType, PetscLinearSolverType > BuilderAndSolverType;

	  //********************************************************************
	  //********************************************************************
	  //strategy base class
	  class_< PetscBaseSolvingStrategyType, boost::noncopyable >("PetscSolvingStrategy", init< ModelPart&, bool >() )
	    .def("Predict", &PetscBaseSolvingStrategyType::Predict )
	    .def("Solve", &PetscBaseSolvingStrategyType::Solve )
				.def("IsConverged", &PetscBaseSolvingStrategyType::IsConverged )
				.def("CalculateOutputData", &PetscBaseSolvingStrategyType::CalculateOutputData )
				.def("SetEchoLevel", &PetscBaseSolvingStrategyType::SetEchoLevel )
				.def("GetEchoLevel", &PetscBaseSolvingStrategyType::GetEchoLevel )
				.def("SetRebuildLevel", &PetscBaseSolvingStrategyType::SetRebuildLevel )
				.def("GetRebuildLevel", &PetscBaseSolvingStrategyType::GetRebuildLevel )
				.def("SetMoveMeshFlag", &PetscBaseSolvingStrategyType::SetMoveMeshFlag )
				.def("MoveMeshFlag", &PetscBaseSolvingStrategyType::MoveMeshFlag )
				.def("MoveMesh", &PetscBaseSolvingStrategyType::MoveMesh )
				.def("Clear", &PetscBaseSolvingStrategyType::Clear )
				//.def("GetModelPart", &PetscBaseSolvingStrategyType::GetModelPart )
				;

			class_< ResidualBasedLinearStrategy< PetscSparseSpaceType, PetscLocalSpaceType, PetscLinearSolverType >,bases< PetscBaseSolvingStrategyType >,  boost::noncopyable >
				("PetscResidualBasedLinearStrategy",
				 init<ModelPart&,PetscBaseSchemeType::Pointer, PetscLinearSolverType::Pointer, BuilderAndSolverType::Pointer, bool, bool, bool, bool	>() )
				;

 			typedef ConvergenceCriteria< PetscSparseSpaceType, PetscLocalSpaceType > TConvergenceCriteriaType;

 			class_< ResidualBasedNewtonRaphsonStrategy< PetscSparseSpaceType, PetscLocalSpaceType, PetscLinearSolverType >,bases< PetscBaseSolvingStrategyType >,  boost::noncopyable >
 				("PetscResidualBasedNewtonRaphsonStrategy",
 				init<ModelPart&, PetscBaseSchemeType::Pointer, PetscLinearSolverType::Pointer, TConvergenceCriteriaType::Pointer, BuilderAndSolverType::Pointer, int, bool, bool, bool
 				>() )
 				;

			//********************************************************************
			//********************************************************************
			class_< PetscBaseSchemeType, boost::noncopyable >
			("PetscScheme", init< >() )
			.def("Initialize", &PetscBaseSchemeType::Initialize )
			.def("SchemeIsInitialized", &PetscBaseSchemeType::SchemeIsInitialized )
			.def("ElementsAreInitialized", &PetscBaseSchemeType::ElementsAreInitialized )
			.def("InitializeElements", &PetscBaseSchemeType::InitializeElements )
			.def("InitializeSolutionStep", &PetscBaseSchemeType::InitializeSolutionStep )
			.def("FinalizeSolutionStep", &PetscBaseSchemeType::FinalizeSolutionStep )
			.def("InitializeNonLinIteration", &PetscBaseSchemeType::InitializeNonLinIteration )
			.def("FinalizeNonLinIteration", &PetscBaseSchemeType::FinalizeNonLinIteration )
			.def("Predict", &PetscBaseSchemeType::Predict )
			.def("Update", &PetscBaseSchemeType::Update )
			.def("CalculateOutputData", &PetscBaseSchemeType::CalculateOutputData )
			.def("Clean", &PetscBaseSchemeType::Clean )
			.def("MoveMesh", MoveMesh )
			;

			class_< PetscResidualBasedIncrementalUpdateStaticScheme< PetscSparseSpaceType, PetscLocalSpaceType>,
					bases< PetscBaseSchemeType >,  boost::noncopyable >
				(
					"PetscResidualBasedIncrementalUpdateStaticScheme", init< >()
				);

			//********************************************************************
			//********************************************************************
			//convergence criteria base class
			 class_< ConvergenceCriteria< PetscSparseSpaceType, PetscLocalSpaceType >, boost::noncopyable >("PetscConvergenceCriteria", init<>() )
 				.def("SetActualizeRHSFlag", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::SetActualizeRHSFlag )
 			 .def("GetActualizeRHSflag", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::GetActualizeRHSflag )
// 			 .def("PreCriteria", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::PreCriteria )
// 			 .def("PostCriteria", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::PostCriteria )
// 			 .def("Initialize", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::Initialize )
// 			 .def("InitializeSolutionStep", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::InitializeSolutionStep )
// 			 .def("FinalizeSolutionStep", &ConvergenceCriteria<PetscSparseSpaceType, PetscLocalSpaceType >::FinalizeSolutionStep )
			 ;

			class_< DisplacementCriteria<PetscSparseSpaceType, PetscLocalSpaceType >,
			         bases<ConvergenceCriteria< PetscSparseSpaceType, PetscLocalSpaceType > >,
			         boost::noncopyable >
			        ("PetscDisplacementCriteria", init< double, double>() );

/*			class_< ResidualCriteria< PetscSparseSpaceType >,
			         bases<ConvergenceCriteria< PetscSparseSpaceType > >,
			         boost::noncopyable >
			        ("PetscResidualCriteria", init<Model::Pointer, double >() );

			class_< AndCriteria< PetscSparseSpaceType >,
			         bases<ConvergenceCriteria< PetscSparseSpaceType > >,
			         boost::noncopyable >
			        ("PetscAndCriteria", init<Model::Pointer, ConvergenceCriteria< PetscSparseSpaceType >::Pointer, ConvergenceCriteria< PetscSparseSpaceType >::Pointer >()*/
			//);

			//********************************************************************
			//********************************************************************
			//

			//Builder and Solver

			class_< BuilderAndSolverType::DofsArrayType, boost::noncopyable >("PetscDofsArrayType",	init<>() );

			class_< BuilderAndSolverType, boost::noncopyable >("PetscBuilderAndSolver",	init<PetscLinearSolverType::Pointer>() )
			 .def("SetCalculateReactionsFlag", &BuilderAndSolverType::SetCalculateReactionsFlag )
			 .def("GetCalculateReactionsFlag", &BuilderAndSolverType::GetCalculateReactionsFlag )
			 .def("SetDofSetIsInitializedFlag", &BuilderAndSolverType::SetDofSetIsInitializedFlag )
			 .def("GetDofSetIsInitializedFlag", &BuilderAndSolverType::GetDofSetIsInitializedFlag )
			 .def("SetReshapeMatrixFlag", &BuilderAndSolverType::SetReshapeMatrixFlag )
			 .def("GetReshapeMatrixFlag", &BuilderAndSolverType::GetReshapeMatrixFlag )
			 .def("GetEquationSystemSize", &BuilderAndSolverType::GetEquationSystemSize )
			 .def("BuildLHS", &BuilderAndSolverType::BuildLHS )
			 .def("BuildRHS", &BuilderAndSolverType::BuildRHS )
			 .def("Build", &BuilderAndSolverType::Build )
			 .def("SystemSolve", &BuilderAndSolverType::SystemSolve )
			 .def("BuildAndSolve", &BuilderAndSolverType::BuildAndSolve )
			 .def("BuildRHSAndSolve", &BuilderAndSolverType::BuildRHSAndSolve )
			 .def("ApplyDirichletConditions", &BuilderAndSolverType::ApplyDirichletConditions )
			 .def("SetUpDofSet", &BuilderAndSolverType::SetUpDofSet )
			 .def("GetDofSet", &BuilderAndSolverType::GetDofSet, return_internal_reference<>() )
			 .def("SetUpSystem", &BuilderAndSolverType::SetUpSystem )
			 .def("ResizeAndInitializeVectors", &BuilderAndSolverType::ResizeAndInitializeVectors )
			 .def("InitializeSolutionStep", &BuilderAndSolverType::InitializeSolutionStep )
			 .def("FinalizeSolutionStep", &BuilderAndSolverType::FinalizeSolutionStep )
			 .def("CalculateReactions", &BuilderAndSolverType::CalculateReactions )
			 .def("Clear", &BuilderAndSolverType::Clear )
			 .def("SetEchoLevel", &BuilderAndSolverType::SetEchoLevel )
			 .def("GetEchoLevel", &BuilderAndSolverType::GetEchoLevel )
			 ;

			//********************************************************************
			//********************************************************************

			class_< PetscSparseSpaceType, boost::noncopyable >("PetscSparseSpace",	init<>() )
			 .def("ClearMatrix", ClearMatrix )
			 .def("ClearVector", ClearVector )
			 .def("ResizeMatrix", ResizeMatrix )
			 .def("ResizeVector", ResizeVector )
			 .def("SetToZeroMatrix", SetToZeroMatrix )
			 .def("SetToZeroVector", SetToZeroVector )
			 .def("TwoNorm", TwoNorm )
			 //the dot product of two vectors
			 .def("Dot", Dot)
			 //the matrix-vector multiplication
			 .def("Mult", Mult)
// 			 .def("TransposeMult", TransposeMult)
			 .def("Size", Size)
			 .def("UnaliasedAdd",UnaliasedAdd)
			 .def("ScaleAndAdd",ScaleAndAdd)
			;
	  typedef PetscResidualBasedEliminationBuilderAndSolver< PetscSparseSpaceType, PetscLocalSpaceType> PetscResidualBasedEliminationBuilderAndSolverType;

	  class_< PetscResidualBasedEliminationBuilderAndSolverType, bases<BuilderAndSolverType>, boost::noncopyable> ("PetscResidualBasedEliminationBuilderAndSolver", init<>() );

	AddPetscLinearSolversToPython();

	def("ErrorCleaner", ErrorCleaner);

  }

}  // namespace Python.

}  // namespace Kratos.

#endif // KRATOS_PYTHON defined


// SNCOperatorsTest.cpp - SNCOperators test code.
//
// Copyright (c) 2016 Timothy Zhu.
// Licensed under the MIT License. See LICENSE file for details.
//

#include <cassert>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include "../SNC-Library/ProcessedTrace.hpp"
#include "../SNC-Library/Estimator.hpp"
#include "../SNC-Library/time.hpp"
#include "../SNC-Library/SNCOperators.hpp"
#include "SNC-LibraryTest.hpp"

using namespace std;

class TestArrival : public SNCArrival
{
private:
    double _sigma;
    double _rho;

public:
    TestArrival(double sigma, double rho)
        : _sigma(sigma),
          _rho(rho)
    {
    }
    virtual ~TestArrival()
    {
    }

    virtual void calcBound(double theta, double* sigma, double* rho) const
    {
        *sigma = _sigma + theta;
        *rho = _rho + theta;
    }
};

class TestService : public SNCService
{
private:
    double _sigma;
    double _rho;

public:
    TestService(double sigma, double rho)
        : _sigma(sigma),
          _rho(rho)
    {
    }
    virtual ~TestService()
    {
    }

    virtual void calcBound(double theta, double* sigma, double* rho) const
    {
        *sigma = _sigma + theta;
        *rho = _rho + theta;
    }
};

class TestSNCOperators
{
public:
    static void DependencyParamsTest();
    static ProcessedTrace* MMBPArrivalTestTrace();
    static double MMBPArrivalSpectralRadiusV1(const MMBPArrival& arrival, double theta);
    static double MMBPArrivalSpectralRadiusV2(const MMBPArrival& arrival, double theta);
    static void MMBPArrivalTest();
    static void ConstantServiceTest();
    static void AggregateArrivalTest();
    static void ConvolutionServiceTest();
    static void OutputArrivalTest();
    static void LeftoverServiceTest();
    static void LatencyBoundTest(double epsilon);
    static void LatencyBoundTest();
};

void TestSNCOperators::DependencyParamsTest()
{
    DependencyParams dp;
    // Test setP, setQ
    dp.setP(1.5);
    assert(approxEqual(dp.getP(), 1.5));
    assert(approxEqual(dp.getQ(), 3.0));
    dp.setQ(1.5);
    assert(approxEqual(dp.getP(), 3.0));
    assert(approxEqual(dp.getQ(), 1.5));
    // Test setLowerP, setUpperP, setLowerQ, setUpperQ, setDependent
    dp.setLowerP(1.2);
    assert(approxEqual(dp.getLowerP(), 1.2));
    dp.setUpperP(1.3);
    assert(approxEqual(dp.getUpperP(), 1.3));
    dp.setLowerQ(1.4);
    assert(approxEqual(dp.getLowerQ(), 1.4));
    dp.setUpperQ(1.5);
    assert(approxEqual(dp.getUpperQ(), 1.5));
    assert(dp.getDependentBounds().empty());
    dp.setDependent();
    assert(dp.getDependentBounds().size() == 1);
    assert(dp.getDependentBounds()[0] == &dp);
    // Test addDependencies, getDependencies, checkDependence
    assert(dp.getDependencies().empty());
    set<FlowId> newDependencies;
    newDependencies.insert(3);
    newDependencies.insert(7);
    dp.addDependencies(newDependencies);
    const set<FlowId>& dependencies = dp.getDependencies();
    assert(dependencies.size() == 2);
    assert(dependencies.find(3) != dependencies.end());
    assert(dependencies.find(7) != dependencies.end());
    DependencyParams dp2;
    set<FlowId> newDependencies2;
    newDependencies2.insert(4);
    newDependencies2.insert(8);
    dp2.addDependencies(newDependencies2);
    assert(dp.checkDependence(&dp2) == false);
    assert(dp2.checkDependence(&dp) == false);
    newDependencies2.insert(7);
    dp2.addDependencies(newDependencies2);
    assert(dp.checkDependence(&dp2) == true);
    assert(dp2.checkDependence(&dp) == true);
    // Test getDependentBounds
    assert(dp2.getDependentBounds().empty());
    dp2.setDependent();
    assert(dp2.getDependentBounds().size() == 1);
    assert(dp2.getDependentBounds()[0] == &dp2);
    dp2.addDependencies(&dp);
    const set<FlowId>& dependencies2 = dp2.getDependencies();
    assert(dependencies2.size() == 4);
    assert(dependencies2.find(3) != dependencies2.end());
    assert(dependencies2.find(4) != dependencies2.end());
    assert(dependencies2.find(7) != dependencies2.end());
    assert(dependencies2.find(8) != dependencies2.end());
    assert(dp2.getDependentBounds().size() == 2);
    assert(dp2.getDependentBounds()[0] == &dp2);
    assert(dp2.getDependentBounds()[1] == &dp);
    cout << "PASS DependencyParamsTest" << endl;
}

class MMBPArrivalTestProcessedTrace : public ProcessedTrace
{
private:
    vector<ProcessedTraceEntry> _trace;
    unsigned int _curIndex;

public:
    MMBPArrivalTestProcessedTrace(const vector<ProcessedTraceEntry>& trace, Json::Value& estimatorInfo)
        : ProcessedTrace("testTrace.txt", Estimator::Create(estimatorInfo)),
          _trace(trace),
          _curIndex(0)
    {}
    virtual ~MMBPArrivalTestProcessedTrace()
    {}

    virtual bool nextEntry(ProcessedTraceEntry& entry)
    {
        if (_curIndex < _trace.size()) {
            entry = _trace[_curIndex++];
            return true;
        }
        return false;
    }

    virtual void reset()
    {
        _curIndex = 0;
    }
};

void MMBPArrivalTestAddTraceEntry(vector<ProcessedTraceEntry>& trace, uint64_t arrivalTime, double work, bool isGet)
{
    ProcessedTraceEntry entry = {arrivalTime, work, isGet};
    trace.push_back(entry);
}
ProcessedTrace* TestSNCOperators::MMBPArrivalTestTrace()
{
    vector<ProcessedTraceEntry> trace;
    uint64_t arrivalTime = 0;
    MMBPArrivalTestAddTraceEntry(trace, arrivalTime, 0.2, true);
    MMBPArrivalTestAddTraceEntry(trace, arrivalTime, 0.1, true);
    arrivalTime += MMBPArrival::intervalWidth;
    for (int i = 0; i < 24; i++) {
        MMBPArrivalTestAddTraceEntry(trace, arrivalTime, 0.3, true);
    }
    arrivalTime += MMBPArrival::intervalWidth;
    MMBPArrivalTestAddTraceEntry(trace, arrivalTime, 0.3, true);
    Json::Value estimatorInfo;
    setEstimatorInfoProcessedTraceTest(estimatorInfo);
    return new MMBPArrivalTestProcessedTrace(trace, estimatorInfo);
}

double TestSNCOperators::MMBPArrivalSpectralRadiusV1(const MMBPArrival& arrival, double theta)
{
    // Hand-solved solution for 2 states
    double MGF0 = arrival._MGFs[0]->calcMGF(theta);
    double MGF1 = arrival._MGFs[1]->calcMGF(theta);
    double L1 = (arrival._transitionMatrix[0][0] * MGF0 + arrival._transitionMatrix[1][1] * MGF1 + sqrt((arrival._transitionMatrix[0][0] * MGF0 - arrival._transitionMatrix[1][1] * MGF1) * (arrival._transitionMatrix[0][0] * MGF0 - arrival._transitionMatrix[1][1] * MGF1) + 4.0 * arrival._transitionMatrix[0][1] * arrival._transitionMatrix[1][0] * MGF0 * MGF1)) / 2.0;
    double L2 = (arrival._transitionMatrix[0][0] * MGF0 + arrival._transitionMatrix[1][1] * MGF1 - sqrt((arrival._transitionMatrix[0][0] * MGF0 - arrival._transitionMatrix[1][1] * MGF1) * (arrival._transitionMatrix[0][0] * MGF0 - arrival._transitionMatrix[1][1] * MGF1) + 4.0 * arrival._transitionMatrix[0][1] * arrival._transitionMatrix[1][0] * MGF0 * MGF1)) / 2.0;
    return max(fabs(L1), fabs(L2));
}

double TestSNCOperators::MMBPArrivalSpectralRadiusV2(const MMBPArrival& arrival, double theta)
{
    // Generic solution for n states
    Eigen::MatrixXd m(arrival._MGFs.size(), arrival._MGFs.size());
    for (unsigned int fromState = 0; fromState < arrival._transitionMatrix.size(); fromState++) {
        double stateMGF = arrival._MGFs[fromState]->calcMGF(theta);
        if (!isfinite(stateMGF)) {
            return numeric_limits<double>::infinity();
        }
        for (unsigned int toState = 0; toState < arrival._transitionMatrix[fromState].size(); toState++) {
            m(fromState, toState) = stateMGF * arrival._transitionMatrix[fromState][toState];
        }
    }
    return m.eigenvalues().cwiseAbs().maxCoeff();
}

void TestSNCOperators::MMBPArrivalTest()
{
    ProcessedTrace* pTrace = MMBPArrivalTestTrace();
    MMBPArrival arrival(pTrace);
    arrival._transitionMatrix.clear();
    while (!arrival._MGFs.empty()) {
        delete arrival._MGFs.back();
        arrival._MGFs.pop_back();
    }
    // Test countArrivalIntervals
    vector<double> intervals;
    arrival.countArrivalIntervals(pTrace, intervals);
    assert(intervals.size() == 3);
    assert(approxEqual(intervals[0], 2));
    assert(approxEqual(intervals[1], 24));
    assert(approxEqual(intervals[2], 1));
    // Test determineStatesLAMBDA
    vector<unsigned int> states;
    vector<double> lambdas;
    unsigned int numStates = arrival.determineStatesLAMBDA(intervals, states, lambdas, 2.0);
    assert(numStates == 2);
    assert(states.size() == 3);
    assert(states[0] == states[2]);
    assert(states[1] != states[2]);
    assert(lambdas.size() == 2);
    assert(approxEqual(lambdas[0], 4.0));
    assert(approxEqual(lambdas[1], 16.0));
    // Test initTransitionMatrix
    arrival.initTransitionMatrix(numStates, states);
    assert(arrival._transitionMatrix.size() == 2);
    assert(arrival._transitionMatrix[0].size() == 2);
    assert(arrival._transitionMatrix[1].size() == 2);
    assert(approxEqual(arrival._transitionMatrix[0][0], 1.0 - 1.0 / floor(ConvertTimeToSeconds(2.0 * MMBPArrival::intervalWidth) / stepSize)));
    assert(approxEqual(arrival._transitionMatrix[0][1], 1.0 / floor(ConvertTimeToSeconds(2.0 * MMBPArrival::intervalWidth) / stepSize)));
    assert(approxEqual(arrival._transitionMatrix[1][0], 1.0 / floor(ConvertTimeToSeconds(1.0 * MMBPArrival::intervalWidth) / stepSize)));
    assert(approxEqual(arrival._transitionMatrix[1][1], 1.0 - 1.0 / floor(ConvertTimeToSeconds(1.0 * MMBPArrival::intervalWidth) / stepSize)));
    // Test initMGFs
    arrival.initMGFs(pTrace, numStates, states, lambdas);
    assert(arrival._MGFs.size() == 2);
    MGF* state0 = arrival.createMMBPStateMGF();
    MGF* state1 = arrival.createMMBPStateMGF();
    pTrace->reset();
    ProcessedTraceEntry entry;
    pTrace->nextEntry(entry);
    state0->addSampleRequest(entry);
    pTrace->nextEntry(entry);
    state0->addSampleRequest(entry);
    for (int i = 0; i < 24; i++) {
        pTrace->nextEntry(entry);
        state1->addSampleRequest(entry);
    }
    pTrace->nextEntry(entry);
    state0->addSampleRequest(entry);
    state0->setProbRequest(4.0 * stepSize / ConvertTimeToSeconds(MMBPArrival::intervalWidth));
    state1->setProbRequest(16.0 * stepSize / ConvertTimeToSeconds(MMBPArrival::intervalWidth));
    assert(approxEqual(arrival._MGFs[0]->calcMGF(0.1), state0->calcMGF(0.1)));
    assert(approxEqual(arrival._MGFs[0]->calcMGF(0.2), state0->calcMGF(0.2)));
    assert(approxEqual(arrival._MGFs[0]->calcMGF(2.0), state0->calcMGF(2.0)));
    assert(approxEqual(arrival._MGFs[1]->calcMGF(0.1), state1->calcMGF(0.1)));
    assert(approxEqual(arrival._MGFs[1]->calcMGF(0.2), state1->calcMGF(0.2)));
    assert(approxEqual(arrival._MGFs[1]->calcMGF(2.0), state1->calcMGF(2.0)));
    delete state0;
    delete state1;
    // Test calcSpectralRadius
    assert(approxEqual(MMBPArrivalSpectralRadiusV1(arrival, 0.1), MMBPArrivalSpectralRadiusV2(arrival, 0.1)));
    assert(approxEqual(MMBPArrivalSpectralRadiusV1(arrival, 0.2), MMBPArrivalSpectralRadiusV2(arrival, 0.2)));
    assert(approxEqual(MMBPArrivalSpectralRadiusV1(arrival, 2.0), MMBPArrivalSpectralRadiusV2(arrival, 2.0)));
    double spectralRadius0_1 = MMBPArrivalSpectralRadiusV2(arrival, 0.1);
    double spectralRadius0_2 = MMBPArrivalSpectralRadiusV2(arrival, 0.2);
    double spectralRadius2_0 = MMBPArrivalSpectralRadiusV2(arrival, 2.0);
    assert(approxEqual(arrival.calcSpectralRadius(0.1), spectralRadius0_1));
    assert(approxEqual(arrival.calcSpectralRadius(0.2), spectralRadius0_2));
    assert(approxEqual(arrival.calcSpectralRadius(2.0), spectralRadius2_0));
    // Test calcBound
    double sigma = 0;
    double rho = 0;
    arrival.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, log(spectralRadius0_1) / 0.1));
    arrival.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, log(spectralRadius0_2) / 0.2));
    arrival.calcBound(2.0, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, log(spectralRadius2_0) / 2.0));
    // Test serialization/deserialization
    Json::Value data;
    arrival.serialize(data);
    MMBPArrival arrival2(data);
    arrival2.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, log(spectralRadius0_1) / 0.1));
    arrival2.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, log(spectralRadius0_2) / 0.2));
    arrival2.calcBound(2.0, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, log(spectralRadius2_0) / 2.0));
    cout << "PASS MMBPArrivalTest" << endl;
}

void TestSNCOperators::ConstantServiceTest()
{
    ConstantService c0(3);
    double sigma = 0;
    double rho = 0;
    c0.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -3 * stepSize));
    c0.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -3 * stepSize));
    c0.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -3 * stepSize));
    ConstantService c1(4);
    c1.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -4 * stepSize));
    c1.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -4 * stepSize));
    c1.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -4 * stepSize));
    ConstantService c2(5);
    c2.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -5 * stepSize));
    c2.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -5 * stepSize));
    c2.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 0));
    assert(approxEqual(rho, -5 * stepSize));
    cout << "PASS ConstantServiceTest" << endl;
}

void TestSNCOperators::AggregateArrivalTest()
{
    TestArrival a0(1, 2);
    TestArrival a1(3, 4);
    AggregateArrival aggregateArrival(&a0, &a1);
    double sigma = 0;
    double rho = 0;
    // Independent
    aggregateArrival.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 4.2));
    assert(approxEqual(rho, 6.2));
    aggregateArrival.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 4.4));
    assert(approxEqual(rho, 6.4));
    aggregateArrival.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 8));
    assert(approxEqual(rho, 10));
    // Dependent p = 2
    aggregateArrival.setDependent();
    aggregateArrival.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 4.4));
    assert(approxEqual(rho, 6.4));
    aggregateArrival.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 4.8));
    assert(approxEqual(rho, 6.8));
    aggregateArrival.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 12));
    assert(approxEqual(rho, 14));
    // Dependent p = 1.5
    aggregateArrival.setP(1.5);
    aggregateArrival.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 4.45));
    assert(approxEqual(rho, 6.45));
    aggregateArrival.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 4.9));
    assert(approxEqual(rho, 6.9));
    aggregateArrival.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 13));
    assert(approxEqual(rho, 15));
    cout << "PASS AggregateArrivalTest" << endl;
}

void TestSNCOperators::ConvolutionServiceTest()
{
    TestService s0(2, -3);
    TestService s1(4, -5);
    ConvolutionService convolutionService(&s0, &s1);
    double sigma = 0;
    double rho = 0;
    // Independent
    convolutionService.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 6.2 - log(1.0 - exp(-0.2)) / 0.1));
    assert(approxEqual(rho, -2.9));
    convolutionService.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 6.4 - log(1.0 - exp(-0.4)) / 0.2));
    assert(approxEqual(rho, -2.8));
    convolutionService.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 10 - log(1.0 - exp(-4)) / 2.0));
    assert(approxEqual(rho, -1));
    // Dependent p = 2
    convolutionService.setDependent();
    convolutionService.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 6.4 - log(1.0 - exp(-0.2)) / 0.1));
    assert(approxEqual(rho, -2.8));
    convolutionService.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 6.8 - log(1.0 - exp(-0.4)) / 0.2));
    assert(approxEqual(rho, -2.6));
    convolutionService.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 14 - log(1.0 - exp(-4)) / 2.0));
    assert(approxEqual(rho, 1));
    // Dependent p = 1.5
    convolutionService.setP(1.5);
    convolutionService.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 6.45 - log(1.0 - exp(-0.185)) / 0.1));
    assert(approxEqual(rho, -2.85));
    convolutionService.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 6.9 - log(1.0 - exp(-0.34)) / 0.2));
    assert(approxEqual(rho, -2.7));
    convolutionService.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 15 - log(1.0 - exp(-2)) / 2.0));
    assert(approxEqual(rho, 1));
    // Equal rho
    TestService s2(5, -10);
    TestService s3(5, -10);
    ConvolutionService convolutionServiceEqual(&s2, &s3);
    convolutionServiceEqual.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 10.2 - log(1.0 - exp(-0.0099)) / 0.1));
    assert(approxEqual(rho, -9.801));
    convolutionServiceEqual.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 10.4 - log(1.0 - exp(-0.0196)) / 0.2));
    assert(approxEqual(rho, -9.702));
    convolutionServiceEqual.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 14 - log(1.0 - exp(-0.16)) / 2.0));
    assert(approxEqual(rho, -7.92));
    cout << "PASS ConvolutionServiceTest" << endl;
}

void TestSNCOperators::OutputArrivalTest()
{
    TestArrival a(1, 2);
    TestService s(3, -8);
    OutputArrival outputArrival(&a, &s);
    double sigma = 0;
    double rho = 0;
    // Independent
    outputArrival.calcBound(0.25, &sigma, &rho);
    assert(approxEqual(sigma, 4.5 - log(1.0 - exp(-1.375)) / 0.25));
    assert(approxEqual(rho, 2.25));
    outputArrival.calcBound(0.5, &sigma, &rho);
    assert(approxEqual(sigma, 5.0 - log(1.0 - exp(-2.5)) / 0.5));
    assert(approxEqual(rho, 2.5));
    outputArrival.calcBound(1, &sigma, &rho);
    assert(approxEqual(sigma, 6.0 - log(1.0 - exp(-4.0)) / 1.0));
    assert(approxEqual(rho, 3));
    // Dependent p = 2
    outputArrival.setDependent();
    outputArrival.calcBound(0.25, &sigma, &rho);
    assert(approxEqual(sigma, 5.0 - log(1.0 - exp(-1.25)) / 0.25));
    assert(approxEqual(rho, 2.5));
    outputArrival.calcBound(0.5, &sigma, &rho);
    assert(approxEqual(sigma, 6.0 - log(1.0 - exp(-2.0)) / 0.5));
    assert(approxEqual(rho, 3));
    outputArrival.calcBound(1, &sigma, &rho);
    assert(approxEqual(sigma, 8.0 - log(1.0 - exp(-2.0)) / 1.0));
    assert(approxEqual(rho, 4));
    // Dependent p = 1.5
    outputArrival.setP(1.5);
    outputArrival.calcBound(0.25, &sigma, &rho);
    assert(approxEqual(sigma, 5.125 - log(1.0 - exp(-1.21875)) / 0.25));
    assert(approxEqual(rho, 2.375));
    outputArrival.calcBound(0.5, &sigma, &rho);
    assert(approxEqual(sigma, 6.25 - log(1.0 - exp(-1.875)) / 0.5));
    assert(approxEqual(rho, 2.75));
    outputArrival.calcBound(1, &sigma, &rho);
    assert(approxEqual(sigma, 8.5 - log(1.0 - exp(-1.5)) / 1.0));
    assert(approxEqual(rho, 3.5));
    cout << "PASS OutputArrivalTest" << endl;
}

void TestSNCOperators::LeftoverServiceTest()
{
    TestArrival a(1, 2);
    TestService s(3, 4);
    LeftoverService leftoverService(&a, &s);
    double sigma = 0;
    double rho = 0;
    // Independent
    leftoverService.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 4.2));
    assert(approxEqual(rho, 6.2));
    leftoverService.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 4.4));
    assert(approxEqual(rho, 6.4));
    leftoverService.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 8));
    assert(approxEqual(rho, 10));
    // Dependent p = 2
    leftoverService.setDependent();
    leftoverService.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 4.4));
    assert(approxEqual(rho, 6.4));
    leftoverService.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 4.8));
    assert(approxEqual(rho, 6.8));
    leftoverService.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 12));
    assert(approxEqual(rho, 14));
    // Dependent p = 1.5
    leftoverService.setP(1.5);
    leftoverService.calcBound(0.1, &sigma, &rho);
    assert(approxEqual(sigma, 4.45));
    assert(approxEqual(rho, 6.45));
    leftoverService.calcBound(0.2, &sigma, &rho);
    assert(approxEqual(sigma, 4.9));
    assert(approxEqual(rho, 6.9));
    leftoverService.calcBound(2, &sigma, &rho);
    assert(approxEqual(sigma, 13));
    assert(approxEqual(rho, 15));
    cout << "PASS LeftoverServiceTest" << endl;
}

void TestSNCOperators::LatencyBoundTest(double epsilon)
{
    TestArrival a(1, 2);
    TestService s(3, -8);
    LatencyBound latencyBound(&a, &s, epsilon);
    // Independent
    assert(approxEqual(latencyBound.calcLatency(0.25), stepSize * ((log(epsilon * (1.0 - exp(-1.375))) / 0.25) - 4.5) / (-7.75)));
    assert(approxEqual(latencyBound.calcLatency(0.5), stepSize * ((log(epsilon * (1.0 - exp(-2.5))) / 0.5) - 5.0) / (-7.5)));
    assert(approxEqual(latencyBound.calcLatency(1), stepSize * ((log(epsilon * (1.0 - exp(-4.0))) / 1.0) - 6.0) / (-7.0)));
    // Dependent p = 2
    latencyBound.setDependent();
    assert(approxEqual(latencyBound.calcLatency(0.25), stepSize * ((log(epsilon * (1.0 - exp(-1.25))) / 0.25) - 5.0) / (-7.5)));
    assert(approxEqual(latencyBound.calcLatency(0.5), stepSize * ((log(epsilon * (1.0 - exp(-2.0))) / 0.5) - 6.0) / (-7.0)));
    assert(approxEqual(latencyBound.calcLatency(1), stepSize * ((log(epsilon * (1.0 - exp(-2.0))) / 1.0) - 8.0) / (-6.0)));
    // Dependent p = 1.5
    latencyBound.setP(1.5);
    assert(approxEqual(latencyBound.calcLatency(0.25), stepSize * ((log(epsilon * (1.0 - exp(-1.21875))) / 0.25) - 5.125) / (-7.25)));
    assert(approxEqual(latencyBound.calcLatency(0.5), stepSize * ((log(epsilon * (1.0 - exp(-1.875))) / 0.5) - 6.25) / (-6.5)));
    assert(approxEqual(latencyBound.calcLatency(1), stepSize * ((log(epsilon * (1.0 - exp(-1.5))) / 1.0) - 8.5) / (-5.0)));
}

void TestSNCOperators::LatencyBoundTest()
{
    // Epsilon 0.01
    LatencyBoundTest(0.01);
    // Epsilon 0.001
    LatencyBoundTest(0.001);
    // Epsilon 0.0001
    LatencyBoundTest(0.0001);
    cout << "PASS LatencyBoundTest" << endl;
}

void SNCOperatorsTest()
{
    TestSNCOperators::DependencyParamsTest();
    TestSNCOperators::MMBPArrivalTest();
    TestSNCOperators::ConstantServiceTest();
    TestSNCOperators::AggregateArrivalTest();
    TestSNCOperators::ConvolutionServiceTest();
    TestSNCOperators::OutputArrivalTest();
    TestSNCOperators::LeftoverServiceTest();
    TestSNCOperators::LatencyBoundTest();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2017 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/detail/overlay/sort_by_side.hpp>

#include <boost/geometry/strategies/strategies.hpp>  // for equals/within
#include <boost/geometry/algorithms/detail/overlay/get_turns.hpp>
#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>

#if defined(TEST_WITH_SVG)
#include "debug_sort_by_side_svg.hpp"
#endif

namespace
{

template <typename T>
std::string as_string(std::vector<T> const& v)
{
    std::stringstream out;
    bool first = true;
    BOOST_FOREACH(T const& value, v)
    {
        out << (first ? "[" : " , ") << value;
        first = false;
    }
    out << (first ? "" : "]");
    return out.str();
}

}

template
<
    typename Geometry, typename Point,
    typename RobustPolicy, typename Strategy
>
std::vector<std::size_t> apply_get_turns(std::string const& case_id,
            Geometry const& geometry1, Geometry const& geometry2,
            Point const& turn_point, Point const& origin_point,
            RobustPolicy const& robust_policy,
            Strategy const& strategy,
            std::size_t expected_open_count,
            std::size_t expected_max_rank,
            std::vector<bg::signed_size_type> const& expected_right_count)
{
    using namespace boost::geometry;

    std::vector<std::size_t> result;

//todo: maybe should be enriched to count left/right - but can also be counted from ranks
    typedef typename bg::point_type<Geometry>::type point_type;
    typedef bg::detail::overlay::turn_info
    <
        point_type,
        typename bg::detail::segment_ratio_type<point_type, RobustPolicy>::type
    > turn_info;
    typedef std::deque<turn_info> turn_container_type;

    turn_container_type turns;

    detail::get_turns::no_interrupt_policy policy;
    bg::get_turns
        <
            false, false,
            detail::overlay::assign_null_policy
        >(geometry1, geometry2, strategy, robust_policy, turns, policy);

    // Define sorter, sorting counter-clockwise such that polygons are on the
    // right side
    typedef typename Strategy::side_strategy_type side_strategy;
    typedef bg::detail::overlay::sort_by_side::side_sorter
        <
            false, false, overlay_union,
            point_type, side_strategy, std::less<int>
        > sbs_type;

    sbs_type sbs(strategy.get_side_strategy());

    std::cout << "Case: " << case_id << std::endl;

    bool has_origin = false;
    for (std::size_t turn_index = 0; turn_index < turns.size(); turn_index++)
    {
        turn_info const& turn = turns[turn_index];

        if (bg::equals(turn.point, turn_point))
        {
//            std::cout << "Found turn: " << turn_index << std::endl;
            for (int i = 0; i < 2; i++)
            {
                Point point1, point2, point3;
                bg::copy_segment_points<false, false>(geometry1, geometry2,
                        turn.operations[i].seg_id, point1, point2, point3);
                bool const is_origin = ! has_origin && bg::equals(point1, origin_point);
                if (is_origin)
                {
                    has_origin = true;
                }

                sbs.add(turn.operations[i], turn_index, i,
                        geometry1, geometry2, is_origin);

            }
        }
    }

    BOOST_CHECK_MESSAGE(has_origin,
                        "  caseid="  << case_id
                        << " origin not found");

    if (!has_origin)
    {
        for (std::size_t turn_index = 0; turn_index < turns.size(); turn_index++)
        {
            turn_info const& turn = turns[turn_index];
            if (bg::equals(turn.point, turn_point))
            {
                for (int i = 0; i < 2; i++)
                {
                    Point point1, point2, point3;
                    bg::copy_segment_points<false, false>(geometry1, geometry2,
                            turn.operations[i].seg_id, point1, point2, point3);
//                    std::cout << "Turn " << turn_index << " op " << i << " point1=" << bg::wkt(point1) << std::endl;
                }
            }
        }
        return result;
    }

    sbs.apply(turn_point);

    sbs.find_open();
    sbs.assign_zones(detail::overlay::operation_union);

    std::size_t const open_count = sbs.open_count(detail::overlay::operation_union);
    std::size_t const max_rank = sbs.m_ranked_points.back().rank;
    std::vector<bg::signed_size_type> right_count(max_rank + 1, -1);

    int previous_rank = -1;
    int previous_to_rank = -1;
    for (std::size_t i = 0; i < sbs.m_ranked_points.size(); i++)
    {
        typename sbs_type::rp const& ranked_point = sbs.m_ranked_points[i];

        int const rank = static_cast<int>(ranked_point.rank);
        bool const set_right = rank != previous_to_rank;
        if (rank != previous_rank)
        {
            BOOST_CHECK_MESSAGE(rank == previous_rank + 1,
                                "  caseid="  << case_id
                                << " ranks: conflict in ranks="  << ranked_point.rank
                                << " vs " << previous_rank + 1);
            previous_rank = rank;
        }

        if (ranked_point.direction != bg::detail::overlay::sort_by_side::dir_to)
        {
            continue;
        }

        previous_to_rank = rank;
        if (set_right)
        {
            right_count[rank] = ranked_point.count_right;
        }
        else
        {
            BOOST_CHECK_MESSAGE(right_count[rank] == int(ranked_point.count_right),
                                "  caseid="  << case_id
                                << " ranks: conflict in right_count="  << ranked_point.count_right
                                << " vs " << right_count[rank]);
        }

    }
    BOOST_CHECK_MESSAGE(open_count == expected_open_count,
                        "  caseid="  << case_id
                        << " open_count: expected="  << expected_open_count
                        << " detected="  << open_count);

    BOOST_CHECK_MESSAGE(max_rank == expected_max_rank,
                        "  caseid="  << case_id
                        << " max_rank: expected="  << expected_max_rank
                        << " detected="  << max_rank);

    BOOST_CHECK_MESSAGE(right_count == expected_right_count,
                        "  caseid="  << case_id
                        << " right count: expected="  << as_string(expected_right_count)
                        << " detected="  << as_string(right_count));

#if defined(TEST_WITH_SVG)
    debug::sorted_side_map(case_id, sbs, turn_point, geometry1, geometry2);
#endif

    return result;
}

template <typename T>
void test_basic(std::string const& case_id,
                std::string const& wkt1,
                std::string const& wkt2,
                std::string const& wkt_turn_point,
                std::string const& wkt_origin_point,
                std::size_t expected_open_count,
                std::size_t expected_max_rank,
                std::vector<bg::signed_size_type> const& expected_right_count)
{
    typedef bg::model::point<T, 2, bg::cs::cartesian> point_type;
    typedef bg::model::polygon<point_type> polygon;
    typedef bg::model::multi_polygon<polygon> multi_polygon;

    multi_polygon g1;
    bg::read_wkt(wkt1, g1);

    multi_polygon g2;
    bg::read_wkt(wkt2, g2);

    point_type turn_point, origin_point;
    bg::read_wkt(wkt_turn_point, turn_point);
    bg::read_wkt(wkt_origin_point, origin_point);

    // Reverse if necessary
    bg::correct(g1);
    bg::correct(g2);

    typedef typename bg::rescale_overlay_policy_type
    <
        multi_polygon,
        multi_polygon
    >::type rescale_policy_type;

    rescale_policy_type robust_policy
        = bg::get_rescale_policy<rescale_policy_type>(g1, g2);

    typedef typename bg::strategy::intersection::services::default_strategy
        <
            typename bg::cs_tag<point_type>::type
        >::type strategy_type;

    strategy_type strategy;

    apply_get_turns(case_id, g1, g2, turn_point, origin_point,
        robust_policy, strategy,
        expected_open_count, expected_max_rank, expected_right_count);
}

using boost::assign::list_of;

template <typename T>
void test_all()
{
    test_basic<T>("simplex",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 0,1, 0)))",
      "POINT(1 1)", "POINT(1 0)",
      2, 3, list_of(-1)(1)(-1)(1));

    test_basic<T>("dup1",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 0,1, 0)),((0 2,1 2,1 1,0 1,0 2)))",
      "POINT(1 1)", "POINT(1 0)",
      2, 3, list_of(-1)(1)(-1)(2));

    test_basic<T>("dup2",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)),((1 0,1 1,2 1,2 0,1, 0)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 0,1, 0)))",
      "POINT(1 1)", "POINT(1 0)",
      2, 3, list_of(-1)(2)(-1)(1));

    test_basic<T>("dup3",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)),((1 0,1 1,2 1,2 0,1, 0)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 0,1, 0)),((0 2,1 2,1 1,0 1,0 2)))",
      "POINT(1 1)", "POINT(1 0)",
      2, 3, list_of(-1)(2)(-1)(2));

    test_basic<T>("threequart1",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)),((1 0,1 1,2 1,2 0,1, 0)))",
      "MULTIPOLYGON(((1 2,2 2,2 1,1 1,1 2)))",
      "POINT(1 1)", "POINT(1 0)",
      1, 3, list_of(-1)(1)(1)(1));

    test_basic<T>("threequart2",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)),((1 0,1 1,2 1,2 0,1, 0)))",
      "MULTIPOLYGON(((1 2,2 2,2 1,1 1,1 2)),((2 0,1 0,1 1,2 0)))",
      "POINT(1 1)", "POINT(1 0)",
      1, 4, list_of(-1)(2)(1)(1)(1));

    test_basic<T>("threequart3",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)),((1 0,1 1,2 1,2 0,1, 0)))",
      "MULTIPOLYGON(((1 2,2 2,2 1,1 1,1 2)),((2 0,1 0,1 1,2 0)),((0 1,0 2,1 1,0 1)))",
      "POINT(1 1)", "POINT(1 0)",
      1, 5, list_of(-1)(2)(1)(1)(-1)(2));

    test_basic<T>("full1",
      "MULTIPOLYGON(((0 2,1 2,1 1,0 1,0 2)),((1 0,1 1,2 1,2 0,1, 0)))",
      "MULTIPOLYGON(((1 2,2 2,2 1,1 1,1 2)),((0 0,0 1,1 1,1 0,0 0)))",
      "POINT(1 1)", "POINT(1 0)",
      0, 3, list_of(1)(1)(1)(1));

    test_basic<T>("hole1",
      "MULTIPOLYGON(((0 0,0 3,2 3,2 2,3 2,3 0,0 0),(1 1,2 1,2 2,1 2,1 1)),((4 2,3 2,3 3,4 3,4 2)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 2,1 2,1 4,4 4,4 0,1, 0),(3 2,3 3,2 3,2 2,3 2)))",
      "POINT(1 2)", "POINT(2 2)",
      1, 2, list_of(-1)(2)(1));

    test_basic<T>("hole2",
      "MULTIPOLYGON(((0 0,0 3,2 3,2 2,3 2,3 0,0 0),(1 1,2 1,2 2,1 2,1 1)),((4 2,3 2,3 3,4 3,4 2)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 2,1 2,1 4,4 4,4 0,1, 0),(3 2,3 3,2 3,2 2,3 2)))",
      "POINT(2 2)", "POINT(2 1)",
      2, 3, list_of(-1)(2)(-1)(2));

    test_basic<T>("hole3",
      "MULTIPOLYGON(((0 0,0 3,2 3,2 2,3 2,3 0,0 0),(1 1,2 1,2 2,1 2,1 1)),((4 2,3 2,3 3,4 3,4 2)))",
      "MULTIPOLYGON(((1 0,1 1,2 1,2 2,1 2,1 4,4 4,4 0,1, 0),(3 2,3 3,2 3,2 2,3 2)))",
      "POINT(3 2)", "POINT(2 2)",
      1, 3, list_of(-1)(2)(-1)(2));
}

int test_main(int, char* [])
{
    test_all<double>();
    return 0;
}


//          Copyright John McFarlane 2015 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file ../../LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#if defined(CNL_BOOST_ENABLED) && !defined(_WIN64)

#include <cnl/auxiliary/boost.multiprecision.h>

#include <cnl/_impl/type_traits/assert_same.h>
#include <cnl/_impl/type_traits/identical.h>
#include <cnl/scaled_integer.h>

#include <boost/version.hpp>
#include <gtest/gtest.h>

#include "boost.throw_exception.h"

using cnl::multiprecision;
using cnl::scaled_integer;
using cnl::set_digits_t;
using cnl::signed_multiprecision;
using cnl::unsigned_multiprecision;
using cnl::_impl::assert_same;
using cnl::_impl::identical;

////////////////////////////////////////////////////////////////////////////////
// cnl::multiprecision traits

namespace test_is_boost_multiprecision {
    static_assert(
            cnl::_impl::is_boost_multiprecision<cnl::unsigned_multiprecision<987654321>>::value,
            "cnl::is_boost_multiprecision<>");
}

// test cnl::numbers::signedness<{un}signed_multiprecision>::value
static_assert(
        cnl::numbers::signedness<signed_multiprecision<1>>::value,
        "cnl::numbers::signedness<signed_multiprecision<>>::value test failed");
static_assert(
        cnl::numbers::signedness<signed_multiprecision<3>>::value,
        "cnl::numbers::signedness<signed_multiprecision<>>::value test failed");
static_assert(
        !cnl::numbers::signedness<unsigned_multiprecision<5>>::value,
        "cnl::numbers::signedness<unsigned_multiprecision<>>::value test failed");
static_assert(
        !cnl::numbers::signedness<unsigned_multiprecision<7>>::value,
        "cnl::numbers::signedness<unsigned_multiprecision<>>::value test failed");

// test cnl::numbers::set_signedness_t<{un}signed_multiprecision>
static_assert(
        cnl::numbers::signedness<cnl::numbers::set_signedness_t<signed_multiprecision<9>, true>>::value);
static_assert(
        cnl::numbers::signedness<cnl::numbers::set_signedness_t<unsigned_multiprecision<10>, true>>::value);
static_assert(
        cnl::numbers::signedness<cnl::numbers::set_signedness_t<signed_multiprecision<9>, true>>::value);
static_assert(
        cnl::numbers::signedness<cnl::numbers::set_signedness_t<unsigned_multiprecision<10>, true>>::value);

static_assert(
        !cnl::numbers::signedness<cnl::numbers::set_signedness_t<signed_multiprecision<9>, false>>::value);
static_assert(
        !cnl::numbers::signedness<cnl::numbers::set_signedness_t<unsigned_multiprecision<10>, false>>::value);
static_assert(
        !cnl::numbers::signedness<cnl::numbers::set_signedness_t<signed_multiprecision<9>, false>>::value);
static_assert(
        !cnl::numbers::signedness<cnl::numbers::set_signedness_t<unsigned_multiprecision<10>, false>>::value);

// test cnl::width<{un}signed_multiprecision>
static_assert(cnl::digits<signed_multiprecision<11>> >= 11);
static_assert(cnl::digits<unsigned_multiprecision<12>> >= 12);

// test cnl::set_digits_t<{un}signed_multiprecision>
static_assert(cnl::digits<set_digits_t<signed_multiprecision<13>, 57>> >= 57);
static_assert(cnl::digits<set_digits_t<unsigned_multiprecision<14>, 3>> >= 3);

static_assert(cnl::digits<set_digits_t<signed_multiprecision<120>, 3>> <= cnl::digits<signed_multiprecision<120>>);
static_assert(cnl::digits<unsigned_multiprecision<128>> >= cnl::digits<set_digits_t<unsigned_multiprecision<128>, 16>>);

namespace test_to_rep {
    static_assert(
            assert_same<
                    decltype(std::declval<unsigned_multiprecision<12>>()),
                    decltype(cnl::to_rep<scaled_integer<unsigned_multiprecision<12>>>{}(
                            std::declval<unsigned_multiprecision<12>>()))>::value,
            "cnl::_impl::depth<scaled_integer<boost::multiprecision>>");
}

TEST(multiprecision, to_rep)  // NOLINT
{
    auto const expected = unsigned_multiprecision<5432>{123};
    auto const actual = cnl::to_rep<unsigned_multiprecision<5432>>{}(123);
    ASSERT_EQ(expected, actual);
    ASSERT_TRUE(identical(expected, actual));
}

namespace test_impl_to_rep {
    static_assert(
            assert_same<
                    unsigned_multiprecision<987654321>,
                    cnl::_impl::rep_of_t<scaled_integer<unsigned_multiprecision<987654321>>>>::value,
            "cnl::_impl::rep_of_t<scaled_integer<boost::multiprecision>>");
}

TEST(multiprecision, from_value)  // NOLINT
{
    auto const expected = unsigned_multiprecision<43212>{53};
    auto const actual =
            cnl::from_value<signed_multiprecision<47>, unsigned_multiprecision<43212>>{}(
                    unsigned_multiprecision<786>{53});
    ASSERT_EQ(expected, actual);
    ASSERT_TRUE(identical(expected, actual));
}

TEST(multiprecision, scale_positive)  // NOLINT
{
    auto const expected = signed_multiprecision<321>{768 * (1 << 5)};
    auto const actual = cnl::_impl::scale<5, 2>(signed_multiprecision<321>{768});
    ASSERT_EQ(expected, actual);
    ASSERT_TRUE(identical(expected, actual));
}

TEST(multiprecision, scale_negative)  // NOLINT
{
    auto const expected = unsigned_multiprecision<321>{768 / (1 << 5)};
    auto const actual = cnl::_impl::scale<-5, 2>(unsigned_multiprecision<321>{768});
    ASSERT_EQ(expected, actual);
    ASSERT_TRUE(identical(expected, actual));
}

////////////////////////////////////////////////////////////////////////////////
// cnl::multiprecision arithmetic

TEST(multiprecision, add)  // NOLINT
{
    using int64 = unsigned_multiprecision<64>;

    auto augend = int64{123456789012345678LL};
    auto addend = int64{876543210987654321LL};

    auto sum = augend + addend;
    auto expected = int64{999999999999999999LL};

    ASSERT_EQ(sum, expected);
}

TEST(multiprecision, subtract)  // NOLINT
{
    using int64 = cnl::unsigned_multiprecision<64>;

    auto minuend = int64{999999999999999999LL};
    auto subtrahend = int64{876543210987654321LL};

    auto difference = minuend - subtrahend;
    auto expected = int64{123456789012345678LL};

    ASSERT_EQ(difference, expected);
}

TEST(multiprecision, multiply)  // NOLINT
{
    using int64 = unsigned_multiprecision<64>;
    using int128 = unsigned_multiprecision<128>;

    auto factor = int64{123456789012345678LL};

    auto product = static_cast<int128>(factor) * static_cast<int128>(factor);
    ASSERT_GT(product, factor);

    auto quotient = product / factor;
    ASSERT_EQ(factor, quotient);
}

TEST(multiprecision, divide)  // NOLINT
{
    using int64 = unsigned_multiprecision<64>;

    auto div = int64{123456789012345678LL};

    auto quotient = div / div;  // NOLINT(misc-redundant-expression)
    auto expected = 1;

    ASSERT_EQ(quotient, expected);
}

////////////////////////////////////////////////////////////////////////////////
// cnl::scaled_integer<cnl::unsigned_multiprecision<>> arithmetic

TEST(scaled_integer_multiprecision, add)  // NOLINT
{
    using int64 = scaled_integer<unsigned_multiprecision<64>>;

    auto augend = int64{123456789012345678LL};
    auto addend = int64{876543210987654321LL};

    auto sum = augend + addend;
    auto expected = int64{999999999999999999LL};

    ASSERT_EQ(sum, expected);
}

TEST(scaled_integer_multiprecision, subtract)  // NOLINT
{
    using int64 = scaled_integer<unsigned_multiprecision<64>>;

    auto minuend = int64{999999999999999999LL};
    auto subtrahend = int64{876543210987654321LL};

    auto difference = minuend - subtrahend;
    auto expected = int64{123456789012345678LL};

    ASSERT_EQ(difference, expected);
}

TEST(scaled_integer_multiprecision, multiply)  // NOLINT
{
    using int64 = scaled_integer<unsigned_multiprecision<64>>;
    using int128 = scaled_integer<unsigned_multiprecision<128>>;

    auto factor = int64{123456789012345678LL};

    auto product = static_cast<int128>(factor) * static_cast<int128>(factor);
    ASSERT_GT(product, factor);

    auto quotient = product / factor;
    ASSERT_EQ(factor, quotient);
}

TEST(scaled_integer_multiprecision, divide)  // NOLINT
{
    using int64 = scaled_integer<unsigned_multiprecision<64>>;

    auto div = int64{123456789012345678LL};

    auto quotient = div / div;  // NOLINT(misc-redundant-expression)
    auto expected = 1;

    ASSERT_EQ(quotient, expected);
}

////////////////////////////////////////////////////////////////////////////////
// cnl::digits<boost::multiprecision::number<>>

static_assert(cnl::digits<unsigned_multiprecision<24>> == 24);

static_assert(cnl::digits<signed_multiprecision<24>> == 24);

////////////////////////////////////////////////////////////////////////////////
// bitwise shift with boost::multiprecision and cnl::constant

// Boost.Multiprecision in 1.58 known to have constexpr support
#if (BOOST_VERSION >= 105800)

TEST(scaled_integer_multiprecision, shift_left_constant)  // NOLINT
{
    auto lhs = cnl::unsigned_multiprecision<15>{1536};
    auto rhs = cnl::constant<3>{};
    auto actual = lhs << rhs;
    auto expected = cnl::unsigned_multiprecision<15>{1536 << 3};
    ASSERT_EQ(expected, actual);
}

TEST(scaled_integer_multiprecision, shift_right_constant)  // NOLINT
{
    auto lhs = cnl::unsigned_multiprecision<15>{1536};
    auto rhs = cnl::constant<3>{};
    auto actual = lhs >> rhs;
    auto expected = cnl::unsigned_multiprecision<15>{1536 >> 3};
    ASSERT_EQ(expected, actual);
}

#endif

////////////////////////////////////////////////////////////////////////////////
// cnl::used_digits<boost::multiprecision::number<>>

TEST(scaled_integer_multiprecision, unsigned_multiprecision_used_digits)  // NOLINT
{
    using q4_20 = scaled_integer<unsigned_multiprecision<24>, cnl::power<-20>>;
    q4_20 a = 3.051757812500000e-05;
    ASSERT_EQ(6, cnl::used_digits(a));
}

TEST(scaled_integer_multiprecision, signed_multiprecision_used_digits)  // NOLINT
{
    using q4_20 = scaled_integer<signed_multiprecision<24>, cnl::power<-20>>;
    q4_20 a = 3.051757812500000e-05;
    ASSERT_EQ(6, cnl::used_digits(a));
}

////////////////////////////////////////////////////////////////////////////////
// cnl::leading_bits<boost::multiprecision::number<>>

TEST(scaled_integer_multiprecision, unsigned_multiprecision_leading_bits)  // NOLINT
{
    using q4_20 = scaled_integer<unsigned_multiprecision<24>, cnl::power<-20>>;
    q4_20 a = 3.051757812500000e-05;
    ASSERT_EQ(18, leading_bits(a));
}

TEST(scaled_integer_multiprecision, signed_multiprecision_leading_bits)  // NOLINT
{
    using q4_20 = scaled_integer<signed_multiprecision<24>, cnl::power<-20>>;
    q4_20 a = 3.051757812500000e-05;
    ASSERT_EQ(18, leading_bits(a));
}

TEST(scaled_integer_multiprecision, sqrt)  // NOLINT
{
    using length_t = cnl::scaled_integer<boost::multiprecision::int128_t>;
    length_t x = length_t{25};
    auto y = cnl::sqrt(x);
    ASSERT_EQ(y, 5);
}

#endif  // CNL_BOOST_ENABLED


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
#include "cor_data_structure/sources/geometry/r_tree_pool_tmpl_impl.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(r_tree_near)

BOOST_AUTO_TEST_CASE(r_tree_near_basic)
{
    class T
    {
    public:
        cor::type::Box2F box;

        cor::type::Box2F get_box()
        {
            return box;
        }

        cor::RFloat get_distance(const cor::type::Vector2F& p)
        {
            return box.get_distance(p);
        }

        T* operator->()
        {
            return this;
        }

        T()
        {
            box.set(-1, -1, 3, 3);
        }

        T(cor::RFloat px, cor::RFloat py, cor::RFloat wx, cor::RFloat wy)
        {
            box.set(px, py, wx, wy);
        }

    };

    typedef cor::data_structure::RTreePoolTmpl<cor::RFloat, cor::type::Vector2F, cor::type::Vector2I, T> TP;

    TP tp;

    cor::algorithm::times(4, [&](cor::RSize i){
        cor::algorithm::times(4, [&](cor::RSize j){
            auto r = tp.create(T(j * 2, i * 2, 1.0f, 1.0f));
        });
    });

    tp.update_leaf_box();
    tp.construct();
    tp.fit();

    tp.find_near(cor::type::Vector2F(-1.0f, -1.0f), [&](const cor::RFloat& d, TP::Ref& r){
        BOOST_CHECK_CLOSE(d, sqrtf(2.0f), 0.00001f);
        BOOST_CHECK_CLOSE(r->box.p.x, 0.0f, 0.00001f);
        BOOST_CHECK_CLOSE(r->box.p.y, 0.0f, 0.00001f);
        BOOST_CHECK_CLOSE(r->box.w.x, 1.0f, 0.00001f);
        BOOST_CHECK_CLOSE(r->box.w.y, 1.0f, 0.00001f);
        return cor::rtrue;
    });

    tp.find_near(cor::type::Vector2F(8.0f, 8.0f), [&](const cor::RFloat& d, TP::Ref& r){
        BOOST_CHECK_CLOSE(d, sqrtf(2.0f), 0.00001f);
        BOOST_CHECK_CLOSE(r->box.p.x, 6.0f, 0.00001f);
        BOOST_CHECK_CLOSE(r->box.p.y, 6.0f, 0.00001f);
        BOOST_CHECK_CLOSE(r->box.w.x, 1.0f, 0.00001f);
        BOOST_CHECK_CLOSE(r->box.w.y, 1.0f, 0.00001f);
        return cor::rtrue;
    });
}

BOOST_AUTO_TEST_SUITE_END()