/************************************************************
 *
 * Copyright (c) 2021, University of California, Los Angeles
 *
 * Authors: Kenny J. Chen, Brett T. Lopez
 * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu
 *
 ***********************************************************/

/***********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020, SMRT-AIST
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

#include <boost/format.hpp>

#include <nano_gicp/lsq_registration.hpp>
#include <nano_gicp/gicp/so3.hpp>

namespace nano_gicp {

template <typename PointTarget, typename PointSource>
LsqRegistration<PointTarget, PointSource>::LsqRegistration() {
  this->reg_name_ = "LsqRegistration";
  max_iterations_ = 64;
  rotation_epsilon_ = 2e-3;
  transformation_epsilon_ = 5e-4;

  lsq_optimizer_type_ = LSQ_OPTIMIZER_TYPE::LevenbergMarquardt;
  lm_debug_print_ = false;
  lm_max_iterations_ = 10;
  lm_init_lambda_factor_ = 1e-9;
  lm_lambda_ = -1.0;

  final_hessian_.setIdentity();
}

template <typename PointTarget, typename PointSource>
LsqRegistration<PointTarget, PointSource>::~LsqRegistration() {}

template <typename PointTarget, typename PointSource>
void LsqRegistration<PointTarget, PointSource>::setRotationEpsilon(double eps) {
  rotation_epsilon_ = eps;
}

template <typename PointTarget, typename PointSource>
void LsqRegistration<PointTarget, PointSource>::setInitialLambdaFactor(double init_lambda_factor) {
  lm_init_lambda_factor_ = init_lambda_factor;
}

template <typename PointTarget, typename PointSource>
void LsqRegistration<PointTarget, PointSource>::setDebugPrint(bool lm_debug_print) {
  lm_debug_print_ = lm_debug_print;
}

template <typename PointTarget, typename PointSource>
const Eigen::Matrix<double, 6, 6>& LsqRegistration<PointTarget, PointSource>::getFinalHessian() const {
  return final_hessian_;
}

template <typename PointTarget, typename PointSource>
void LsqRegistration<PointTarget, PointSource>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
  Eigen::Isometry3d x0 = Eigen::Isometry3d(guess.template cast<double>());

  lm_lambda_ = -1.0;
  converged_ = false;

  if (lm_debug_print_) {
    std::cout << "********************************************" << std::endl;
    std::cout << "***************** optimize *****************" << std::endl;
    std::cout << "********************************************" << std::endl;
  }

  for (int i = 0; i < max_iterations_ && !converged_; i++) {
    nr_iterations_ = i;

    Eigen::Isometry3d delta;
    if (!step_optimize(x0, delta)) {
      std::cerr << "lm not converged!!" << std::endl;
      break;
    }

    converged_ = is_converged(delta);
  }

  final_transformation_ = x0.cast<float>().matrix();
  pcl::transformPointCloud(*input_, output, final_transformation_);
}

template <typename PointTarget, typename PointSource>
bool LsqRegistration<PointTarget, PointSource>::is_converged(const Eigen::Isometry3d& delta) const {
  double accum = 0.0;
  Eigen::Matrix3d R = delta.linear() - Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = delta.translation();

  Eigen::Matrix3d r_delta = 1.0 / rotation_epsilon_ * R.array().abs();
  Eigen::Vector3d t_delta = 1.0 / transformation_epsilon_ * t.array().abs();

  return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1;
}

template <typename PointTarget, typename PointSource>
bool LsqRegistration<PointTarget, PointSource>::step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) {
  switch (lsq_optimizer_type_) {
    case LSQ_OPTIMIZER_TYPE::LevenbergMarquardt:
      return step_lm(x0, delta);
    case LSQ_OPTIMIZER_TYPE::GaussNewton:
      return step_gn(x0, delta);
  }

  return step_lm(x0, delta);
}

template <typename PointTarget, typename PointSource>
bool LsqRegistration<PointTarget, PointSource>::step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) {
  Eigen::Matrix<double, 6, 6> H;
  Eigen::Matrix<double, 6, 1> b;
  double y0 = linearize(x0, &H, &b);

  Eigen::LDLT<Eigen::Matrix<double, 6, 6>> solver(H);
  Eigen::Matrix<double, 6, 1> d = solver.solve(-b);

  delta.setIdentity();
  delta.linear() = so3_exp(d.head<3>()).toRotationMatrix();
  delta.translation() = d.tail<3>();

  x0 = delta * x0;
  final_hessian_ = H;

  return true;
}

template <typename PointTarget, typename PointSource>
bool LsqRegistration<PointTarget, PointSource>::step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) {
  Eigen::Matrix<double, 6, 6> H;
  Eigen::Matrix<double, 6, 1> b;
  double y0 = linearize(x0, &H, &b);

  if (lm_lambda_ < 0.0) {
    lm_lambda_ = lm_init_lambda_factor_ * H.diagonal().array().abs().maxCoeff();
  }

  double nu = 2.0;
  for (int i = 0; i < lm_max_iterations_; i++) {
    Eigen::LDLT<Eigen::Matrix<double, 6, 6>> solver(H + lm_lambda_ * Eigen::Matrix<double, 6, 6>::Identity());
    Eigen::Matrix<double, 6, 1> d = solver.solve(-b);

    delta.setIdentity();
    delta.linear() = so3_exp(d.head<3>()).toRotationMatrix();
    delta.translation() = d.tail<3>();

    Eigen::Isometry3d xi = delta * x0;
    double yi = compute_error(xi);
    double rho = (y0 - yi) / (d.dot(lm_lambda_ * d - b));

    if (lm_debug_print_) {
      if (i == 0) {
        std::cout << boost::format("--- LM optimization ---\n%5s %15s %15s %15s %15s %15s %5s\n") % "i" % "y0" % "yi" % "rho" % "lambda" % "|delta|" % "dec";
      }
      char dec = rho > 0.0 ? 'x' : ' ';
      std::cout << boost::format("%5d %15g %15g %15g %15g %15g %5c") % i % y0 % yi % rho % lm_lambda_ % d.norm() % dec << std::endl;
    }

    if (rho < 0) {
      if (is_converged(delta)) {
        return true;
      }

      lm_lambda_ = nu * lm_lambda_;
      nu = 2 * nu;
      continue;
    }

    x0 = xi;
    lm_lambda_ = lm_lambda_ * std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3));
    final_hessian_ = H;
    return true;
  }

  return false;
}

}  // namespace nano_gicp


/*
 Copyright (c) 2018 Tuukka Norri
 This code is licensed under MIT license (see LICENSE for details).
 */

// XXX workaround for Boost 1.64.0.
#include <boost/serialization/array_wrapper.hpp>

#include <boost/function_output_iterator.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <cstdint>
//#include <execution>
#include <libbio/dispatch_fn.hh>
#include <libbio/file_handling.hh>
#include <libbio/infix_ostream_fn.hh>
#include <libbio/vcf_reader.hh>
#include <sdsl/int_vector.hpp>
#include <thread>
#include <valarray>
#include <vcfdistances/calculate_distances.hh>
#include <vector>

namespace bnu	= boost::numeric::ublas;
namespace ios	= boost::iostreams;
namespace lb	= libbio;
namespace vd	= vcfdistances;

typedef bnu::triangular_matrix <std::uint32_t, bnu::lower> triangular_uint32_matrix;
typedef bnu::triangular_matrix <float, bnu::lower> triangular_float_matrix;

namespace {

	class calculate_task;
	class calculate_context;

	// Divide a vector element-wise, store the result to lhs.
	// Used the idea from https://lists.boost.org/ublas/2009/05/3509.php
	template <typename t_lhs_el, typename t_rhs_el>
	void divide_vector(
		bnu::matrix_row <t_lhs_el> &lhs,
		bnu::matrix_row <t_rhs_el> &rhs
	)
	{
		lhs = bnu::element_div(lhs, rhs);
	}

	// Hadamard division for matrices.
	template <typename t_lhs_matrix, typename t_rhs_matrix>
	void hadamard_division(
		t_lhs_matrix &lhs,
		t_rhs_matrix &rhs
	)
	{
		// Check the dimensions.
		assert(lhs.size1() == rhs.size1());
		assert(lhs.size2() == rhs.size2());

		for (std::size_t i(0), count(lhs.size1()); i < count; ++i)
		{
			// Create proxies.
			auto lhs_row(bnu::matrix_row(lhs, i));
			auto rhs_row(bnu::matrix_row(rhs, i));
			divide_vector(lhs_row, rhs_row);
		}
	}

	// Base class for producing output based on the counts produced for calculate_context.
	class output_handler
	{
	protected:
		lb::file_ostream								m_stream;

	public:
		virtual ~output_handler() {}

		lb::file_ostream &stream() { return m_stream; }

		// Produce the output.
		virtual void output(
			std::uint32_t const total_variants,
			triangular_uint32_matrix const &mutual_set_values,
			triangular_uint32_matrix const &all_set_values,
			triangular_uint32_matrix const &different_values
		) = 0;

	protected:
		// Output a (triangular) matrix, values separated by tabs.
		template <typename t_matrix>
		void output_matrix(t_matrix const &matrix)
		{
			for (auto it(matrix.begin1()), end(matrix.end1()); it != end; ++it)
			{
				std::copy(
					it.begin(),
					it.end(),
					boost::make_function_output_iterator(lb::infix_ostream_fn <typename t_matrix::value_type>(m_stream, '\t'))
				);

				m_stream << '\n';
			}

			m_stream << std::flush;
		}
	};

	// Output Hamming distances.
	class hamming_output_handler final : public output_handler
	{
	public:
		virtual void output(
			std::uint32_t const total_variants,
			triangular_uint32_matrix const &mutual_set_values,
			triangular_uint32_matrix const &all_set_values,
			triangular_uint32_matrix const &different_values
		) override;
	};

	// Output Simple Matching Distances.
	class smd_output_handler final : public output_handler
	{
	public:
		virtual void output(
			std::uint32_t const total_haplotypes,
			triangular_uint32_matrix const &mutual_set_values,
			triangular_uint32_matrix const &all_set_values,
			triangular_uint32_matrix const &different_values
		) override;
	};

	// Output Jaccard distances.
	class jaccard_output_handler final : public output_handler
	{
	public:
		virtual void output(
			std::uint32_t const total_variants,
			triangular_uint32_matrix const &mutual_set_values,
			triangular_uint32_matrix const &all_set_values,
			triangular_uint32_matrix const &different_values
		) override;
	};

	// Output Jaccard similarity coefficient numerators.
	class intersection_size_output_handler final : public output_handler
	{
	public:
		virtual void output(
			std::uint32_t const total_variants,
			triangular_uint32_matrix const &mutual_set_values,
			triangular_uint32_matrix const &all_set_values,
			triangular_uint32_matrix const &different_values
		) override;
	};

	// Output Jaccard similarity coefficient denominators.
	class union_size_output_handler final : public output_handler
	{
	public:
		virtual void output(
			std::uint32_t const total_variants,
			triangular_uint32_matrix const &mutual_set_values,
			triangular_uint32_matrix const &all_set_values,
			triangular_uint32_matrix const &different_values
		) override;
	};

	// Task for comparing two haplotypes at the given indices i and j.
	class compare_two_task final
	{
	protected:
		calculate_task									*m_calculate_task{nullptr};
		std::size_t										m_i{};
		std::size_t										m_j{};

		std::uint32_t									m_same_value_present_in_both{};
		std::uint32_t									m_any_value_present_in_either{};
		std::uint32_t									m_different_values{};

	public:
		compare_two_task(
			calculate_task &calculate_task,
			std::size_t const i,
			std::size_t const j
		):
			m_calculate_task(&calculate_task),
			m_i(i),
			m_j(j)
		{
		}

		void execute();
		std::uint32_t same_value_present_in_both() const { return m_same_value_present_in_both; }
		std::uint32_t any_value_present_in_either() const { return m_any_value_present_in_either; }
		std::uint32_t different_values() const { return m_different_values; }

	protected:
		std::uint32_t count_ones(sdsl::bit_vector const &bv) const;
	};

	// Task for comparing all the haplotypes in a variant file.
	class calculate_task final
	{
	protected:
		calculate_context								*m_ctx{nullptr};
		lb::dispatch_ptr <dispatch_group_t>				m_group{};
		std::uint32_t									m_file_idx{0};
		std::vector <sdsl::bit_vector>					m_haplotypes;
		std::vector <std::vector <std::uint8_t>>		m_multi_alt_haplotypes;

	public:
		calculate_task(
			calculate_context &ctx,
			std::uint32_t const idx,
			std::vector <sdsl::bit_vector> &&haplotypes,
			std::vector <std::vector <std::uint8_t>> &&multi_alt_haplotypes
		):
			m_ctx(&ctx),
			m_file_idx(idx),
			m_haplotypes(std::move(haplotypes)),
			m_multi_alt_haplotypes(std::move(multi_alt_haplotypes))
		{
		}

		std::vector <sdsl::bit_vector> const &haplotypes() const { return m_haplotypes; }
		std::vector <std::vector <std::uint8_t>> const &multi_alt_haplotypes() const { return m_multi_alt_haplotypes; }
		bool operator<(calculate_task const &other) const { return m_file_idx < other.m_file_idx; }

		void prepare();
		void execute_comparison_tasks();
	};

	class calculate_context final
	{
	protected:
		vd::input_format								m_input_format{};
		lb::vcf_reader::sample_name_map					m_sample_names;
		std::vector <std::uint8_t>						m_ploidy;
		std::uint32_t									m_haplotype_count{0};
		std::atomic_size_t								m_variant_count{0};
		std::atomic_size_t								m_calculation_tasks{0};

		std::mutex										m_task_mutex;
		std::set <calculate_task>						m_tasks;
		lb::dispatch_ptr <dispatch_semaphore_t>			m_task_semaphore{};

		lb::dispatch_ptr <dispatch_queue_t>				m_input_queue;
		lb::dispatch_ptr <dispatch_queue_t>				m_task_queue{};

		std::mutex										m_distance_mutex;
		triangular_uint32_matrix						m_mutual_set_values;	// For Jaccard.
		triangular_uint32_matrix						m_all_set_values;		// For Jaccard.
		triangular_uint32_matrix						m_different_values;		// For Hamming and SMD.

		lb::file_ostream								m_sample_names_stream;
		std::vector <std::unique_ptr <output_handler>>	m_output_handlers;

	public:
		calculate_context(vd::input_format const fmt):
			m_input_format(fmt)
		{
		}

		calculate_context() = delete;
		calculate_context(calculate_context const &) = delete;
		calculate_context(calculate_context &&) = delete;

		std::uint32_t haplotype_count() const { return m_haplotype_count; }
		lb::dispatch_ptr <dispatch_queue_t> &task_queue() { return m_task_queue; }
		lb::dispatch_ptr <dispatch_semaphore_t> &task_semaphore() { return m_task_semaphore; }

		void cleanup();

		void output_sample_names(char const *dst_path) { lb::open_file_for_writing(dst_path, m_sample_names_stream, false); }
		void output_hamming_distances(char const *dst_path) { add_output_handler <hamming_output_handler>(dst_path); }
		void output_jaccard_distances(char const *dst_path) { add_output_handler <jaccard_output_handler>(dst_path); }
		void output_smd(char const *dst_path) { add_output_handler <smd_output_handler>(dst_path); }
		void output_intersection_sizes(char const *dst_path) { add_output_handler <intersection_size_output_handler>(dst_path); }
		void output_union_sizes(char const *dst_path) { add_output_handler <union_size_output_handler>(dst_path); }

		void prepare();
		void calculate_distances(std::vector <std::string> &&inputs);
		void update_matrices(
			std::size_t i,
			std::size_t j,
			std::uint32_t same_value_present_in_both,
			std::uint32_t any_value_present_in_either,
			std::uint32_t different_value
		);

		void report_progress(std::size_t const file_idx, std::size_t const comparisons);
		void finish_calculation_task(calculate_task &task);

	protected:
		void cleanup_mt() { delete this; }

		template <typename t_handler>
		void add_output_handler(char const *dst_path);

		void process_path(std::size_t const i, std::string const &path);
		void process_with_reader(std::size_t const i, lb::vcf_reader &reader);
		void read_headers(lb::vcf_reader &reader, bool const is_first);
		void check_ploidy(lb::vcf_reader &reader, std::vector <std::uint8_t> &ploidy);
		void read_haplotypes(
			lb::vcf_reader &reader,
			std::vector <sdsl::bit_vector> &haplotypes,
			std::vector <std::vector <std::uint8_t>> &multi_alt_haplotypes
		);
		void start_calculation_task(
			std::size_t const file_idx,
			std::vector <sdsl::bit_vector> &&haplotypes,
			std::vector <std::vector <std::uint8_t>> &&multi_alt_haplotypes
		);
		void output_sample_names_to_stream();
		void output_distances();
		void report_progress_mt(std::size_t const file_idx, std::size_t const comparisons);
	};

	void calculate_context::cleanup()
	{
		auto queue(dispatch_get_main_queue());
		lb::dispatch_async_fn(queue, [this, queue](){
			cleanup_mt();
			exit(EXIT_SUCCESS);
		});
	}

	void calculate_context::prepare()
	{
		m_input_queue.reset(dispatch_queue_create("fi.iki.tsnorri.vcfdistances.input_queue", DISPATCH_QUEUE_SERIAL));
		m_task_queue.reset(dispatch_queue_create("fi.iki.tsnorri.vcfdistances.task_queue", DISPATCH_QUEUE_CONCURRENT));
		//m_task_queue.reset(dispatch_queue_create("fi.iki.tsnorri.vcfdistances.task_queue", DISPATCH_QUEUE_SERIAL));
		m_task_semaphore.reset(dispatch_semaphore_create(2 * std::thread::hardware_concurrency())); // FIXME: some other value?
	}

	void calculate_context::calculate_distances(std::vector <std::string> &&inputs)
	{
		// Update the number of calculation tasks, used to determine when finished.
		m_calculation_tasks = inputs.size();

		// Use dispatch_async to start a processing task with each of the input paths.
		// The input queue is serial, though.
		std::size_t i(0);
		for (auto &path : inputs)
		{
			lb::dispatch_async_fn(*m_input_queue,
				[
					this,
					i,
					path{std::move(path)}
				]()
				{
					process_path(i, path);
				}
			);

			++i;
		}
	}

	void calculate_context::process_path(
		std::size_t const i,
		std::string const &path
	)
	{
		// Instantiate a VCF reader and a vcf_input.
		// Keep them in the current stack frame and call the processing functions.
		lb::vcf_reader reader;

		lb::dispatch_async_fn(dispatch_get_main_queue(), [path](){
			std::cerr << "Processing " << path << "…" << std::endl;
		});

		if (vd::input_format::UNCOMPRESSED == m_input_format)
		{
			lb::vcf_stream_input <lb::file_istream> vcf_stream;
			lb::open_file_for_reading(path.c_str(), vcf_stream.input_stream());
			reader.set_input(vcf_stream);
			process_with_reader(i, reader);
		}
		else if (vd::input_format::GZIP == m_input_format)
		{
			lb::file_istream gzip_stream;
			lb::open_file_for_reading(path.c_str(), gzip_stream);

			lb::vcf_stream_input <ios::filtering_istream> vcf_stream;
			auto &filtering_istream(vcf_stream.input_stream());
			filtering_istream.push(ios::gzip_decompressor());
			filtering_istream.push(gzip_stream);
			filtering_istream.exceptions(std::istream::badbit);
			reader.set_input(vcf_stream);
			process_with_reader(i, reader);
		}
	}

	void calculate_context::process_with_reader(std::size_t const i, lb::vcf_reader &reader)
	{
		// Read the variants and start a distance calculation task.

		std::vector <sdsl::bit_vector> haplotypes;
		std::vector <std::vector <std::uint8_t>> multi_alt_haplotypes;

		read_headers(reader, 0 == i);

		// For the first input, output the sample names if needed.
		if (0 == i && m_sample_names_stream.is_open())
			output_sample_names_to_stream();

		// Only calculate the distances if requested.
		if (m_output_handlers.size())
		{
			read_haplotypes(reader, haplotypes, multi_alt_haplotypes);
			// haplotypes now contains a bit vector for each haplotype and
			// multi_alt_haplotypes a vector for each variant that has more than one ALT.
			start_calculation_task(i, std::move(haplotypes), std::move(multi_alt_haplotypes));
		}
	}

	void calculate_context::read_headers(lb::vcf_reader &reader, bool const is_first)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Reading headers…" << std::endl;
		});

		reader.read_header();

		// Check that the sample names match.
		// Also check the ploidy but for the first position only.
		if (is_first)
		{
			m_sample_names = reader.sample_names();
			check_ploidy(reader, m_ploidy);
			m_haplotype_count = std::accumulate(m_ploidy.cbegin(), m_ploidy.cend(), static_cast <uint64_t>(0));

			// Allocate space for the distances.
			m_mutual_set_values.resize(m_haplotype_count, m_haplotype_count, false);
			m_all_set_values.resize(m_haplotype_count, m_haplotype_count, false);
			m_different_values.resize(m_haplotype_count, m_haplotype_count, false);

			// Zero the matrices.
			// FIXME: there is bound to be a faster way to do this but couldn't find one.
			for (std::size_t i(0); i < m_haplotype_count; ++i)
			{
				for (std::size_t j(0); j <= i; ++j)
				{
					m_mutual_set_values(i, j) = 0;
					m_all_set_values(i, j) = 0;
					m_different_values(i, j) = 0;
				}
			}

		}
		else
		{
			lb::always_assert(reader.sample_names() == m_sample_names);

			std::vector <std::uint8_t> ploidy;
			check_ploidy(reader, ploidy);
			lb::always_assert(
				std::equal(
					/* std::execution::parallel_unsequenced_policy, */ // FIXME: enable; not available in libc++ 5.0.
					std::begin(ploidy),
					std::end(ploidy),
					std::begin(m_ploidy),
					std::end(m_ploidy)
				)
			);
		}
	}

	void calculate_context::read_haplotypes(
		lb::vcf_reader &reader,
		std::vector <sdsl::bit_vector> &haplotypes,
		std::vector <std::vector <std::uint8_t>> &multi_alt_haplotypes
	)
	{
		// Count the lines by parsing as little as possible.
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Counting lines…" << std::flush;
		});
		{
			reader.reset();
			reader.set_parsed_fields(lb::vcf_field::CHROM);
			bool should_continue(false);
			do {
				reader.fill_buffer();
				should_continue = reader.parse([](lb::transient_variant const &var) -> bool { return true; });
			} while (should_continue);
		}

		auto const lines(reader.lineno() - reader.last_header_lineno());
		m_variant_count += lines;
		lb::dispatch_async_fn(dispatch_get_main_queue(), [lines](){
			std::cerr << " got " << lines << '.' << std::endl;
		});

		// Allocate bitvectors for each sample.
		// Also have a buffer to speed up reading; copy each 64-bit word to the bit vectors
		// after processing 64 variants.
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Allocating bitvectors…" << std::endl;
		});
		haplotypes.resize(m_haplotype_count);
		std::valarray <std::uint64_t> gt_buffer(static_cast <std::uint64_t>(0), m_haplotype_count);
		for (auto &vec : haplotypes)
		{
			sdsl::bit_vector bv(lines, 0);
			vec = std::move(bv);
		}

		std::uint32_t single_alt_variants(0);

		// Process the variants.
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Reading variants…" << std::endl;
		});
		{
			reader.reset();
			reader.set_parsed_fields(lb::vcf_field::ALL);
			bool should_continue(false);
			std::size_t i(0); // Bit index (from 0 to 63).
			std::size_t k(0); // Word index in hapotypes.
			std::size_t lineno(0);
			do {
				reader.fill_buffer();
				should_continue = reader.parse(
					[
						this,
						&single_alt_variants,
						&haplotypes,
						&multi_alt_haplotypes,
						&gt_buffer,
						&i,
						&k,
						&lineno
					](lb::transient_variant const &var) -> bool {

					// If there is just one ALT, use the bit vectors to store the variant position.
					// Otherwise, use the std::vector to store the ALT number with the variant position.
					if (1 < var.alts().size())
					{
						// Add a new vector for this variant.
						auto &vec(multi_alt_haplotypes.emplace_back(m_haplotype_count, 0));

						// Store the ALT numbers.
						std::size_t j(0);
						for (auto const &sample : var.samples())
						{
							for (auto const &gt : sample.get_genotype())
								vec[j++] = gt.alt;
						}
					}
					else
					{
						// Read the value of each genotype, set the corresponding bit in the corresponding buffer.
						{
							std::size_t j(0);
							for (auto const &sample : var.samples())
							{
								for (auto const &gt : sample.get_genotype())
								{
									uint64_t mask(gt.alt);
									mask <<= i;
									gt_buffer[j++] |= mask;
								}
							}
						}

						++i;
						++single_alt_variants;

						// Check if the buffer was filled.
						if (64 == i)
						{
							// Copy the words to the bit vectors.
							for (std::size_t j(0); j < m_haplotype_count; ++j)
							{
								auto const val(gt_buffer[j]);
								*(haplotypes[j].data() + k) = val;
							}

							i = 0;
							++k;
							gt_buffer = 0;
						}
					}

					++lineno;
					if (0 == lineno % 10000)
					{
						lb::dispatch_async_fn(dispatch_get_main_queue(), [lineno](){
							std::cerr << "Handled " << lineno << " lines…" << std::endl;
						});
					}

					return true;
				});
			} while (should_continue);

			// Copy the final words if needed.
			if (i)
			{
				for (std::size_t j(0); j < m_haplotype_count; ++j)
				{
					auto const val(gt_buffer[j]);
					*(haplotypes[j].data() + k) = val;
				}
			}
		}
	}

	void calculate_context::check_ploidy(lb::vcf_reader &reader, std::vector <std::uint8_t> &ploidy)
	{
		// Store the ploidies from the first variant.
		auto const sample_count(reader.sample_names().size());
		ploidy.resize(sample_count, 0);

		reader.reset();
		reader.set_parsed_fields(lb::vcf_field::ALL);

		reader.fill_buffer();
		if (!reader.parse([this, &reader, &ploidy](lb::transient_variant const &var) -> bool {
			for (auto const &kv : reader.sample_names())
			{
				auto const sample_no(kv.second);
				assert(sample_no);
				auto const &sample(var.sample(sample_no));
				auto const current_ploidy(sample.ploidy());
				ploidy[sample_no - 1] = current_ploidy;
			}

			return false;
		}))
		{
			lb::fail("Unable to read the first variant");
		}
	}

	void calculate_context::start_calculation_task(
		std::size_t const file_idx,
		std::vector <sdsl::bit_vector> &&haplotypes,
		std::vector <std::vector <std::uint8_t>> &&multi_alt_haplotypes
	)
	{
		// Create a calculate_task for comparing samples in one variant file, store the task and execute.
		calculate_task task(*this, file_idx, std::move(haplotypes), std::move(multi_alt_haplotypes));
		task.prepare();

		std::set <calculate_task>::iterator it;

		{
			std::lock_guard <std::mutex> guard(m_task_mutex);
			auto res(m_tasks.emplace(std::move(task)));
			lb::always_assert(res.second);
			it = res.first;
		}

		// execute_comparison_tasks is not going to affect the comparison order of calculate_tasks.
		const_cast <calculate_task &>(*it).execute_comparison_tasks();
	}

	void calculate_context::finish_calculation_task(calculate_task &task)
	{
		// Remove the finished task. If it was the last one,
		// write the distance matrices to the output streams and exit.

		{
			std::lock_guard <std::mutex> guard(m_task_mutex);
			m_tasks.erase(task);
		}

		if (0 == --m_calculation_tasks)
		{
			output_distances();
			cleanup();
		}
	}

	void calculate_context::update_matrices(
		std::size_t i,
		std::size_t j,
		std::uint32_t same_value_present_in_both,
		std::uint32_t any_value_present_in_either,
		std::uint32_t different_value
	)
	{
		// Update the difference matrices.
		// Called from the concurrent queue.
		std::lock_guard <std::mutex> guard(m_distance_mutex);
		m_mutual_set_values(i, j) += same_value_present_in_both;
		m_all_set_values(i, j) += any_value_present_in_either;
		m_different_values(i, j) += different_value;
	}

	void calculate_context::output_sample_names_to_stream()
	{
		// Write the sample names to the corresponding output stream.
		// Since the names are in an std::map with sample numbers as keys,
		// the order will be what we expect.
		std::size_t sample_idx(0);
		for (auto const &kv : m_sample_names)
		{
			auto const idx(kv.second);
			assert(idx);
			auto const sample_ploidy(m_ploidy[idx - 1]);
			for (std::size_t i(0); i < sample_ploidy; ++i)
				m_sample_names_stream << kv.first << '\t' << (sample_idx++) << '\n';
		}
		m_sample_names_stream << std::flush;
	}

	void calculate_context::output_distances()
	{
		// Call each output handler.
		for (auto &handler : m_output_handlers)
			handler->output(m_variant_count, m_mutual_set_values, m_all_set_values, m_different_values);
	}

	template <typename t_handler>
	void calculate_context::add_output_handler(char const *dst_path)
	{
		// Instantiate an output handler and open its stream for writing.
		auto &handler(m_output_handlers.emplace_back(new t_handler()));
		lb::open_file_for_writing(dst_path, handler->stream(), false);
	}

	void calculate_context::report_progress(std::size_t const file_idx, std::size_t const comparisons)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [this, file_idx, comparisons](){
			report_progress_mt(file_idx, comparisons);
		});
	}

	void calculate_context::report_progress_mt(std::size_t const file_idx, std::size_t const comparisons)
	{
		std::cerr << "File " << (1 + file_idx) << ": done " << comparisons << " comparisons." << std::endl;
	}

	void calculate_task::prepare()
	{
		m_group.reset(dispatch_group_create());
	}

	void calculate_task::execute_comparison_tasks()
	{
		// Instantiate a comparison task (in the lambda) and compare a pair of samples.
		auto queue(*(m_ctx->task_queue()));
		auto sema(*(m_ctx->task_semaphore()));
		std::size_t task_count(0);

		// Make sure that the group doesn't get notified immediately.
		dispatch_group_enter(*m_group);

		for (std::size_t i(0), haplotype_count(m_ctx->haplotype_count()); i < haplotype_count; ++i)
		{
			for (std::size_t j(0); j <= i; ++j)
			{
				++task_count;
				dispatch_semaphore_wait(sema, DISPATCH_TIME_FOREVER);
				lb::dispatch_group_async_fn(*m_group, queue,
					[this, i, j, task_count, sema](){
						// Execute a compare_two_task for (i, j).
						compare_two_task task(*this, i, j);
						task.execute();
						m_ctx->update_matrices(
							i,
							j,
							task.same_value_present_in_both(),
							task.any_value_present_in_either(),
							task.different_values()
						);

						if (0 == task_count % 100000)
							m_ctx->report_progress(m_file_idx, task_count);

						dispatch_semaphore_signal(sema);
					}
				);
			}
		}

		dispatch_group_leave(*m_group);
		lb::dispatch_group_notify_fn(*m_group, dispatch_get_global_queue(DISPATCH_QUEUE_PRIORITY_DEFAULT, 0), [this](){
			m_ctx->finish_calculation_task(*this);
		});
	}

	void compare_two_task::execute()
	{
		// Compare two samples by using bit operations in the case of single-ALT variants
		// and pairwise comparison of ALT field values otherwise.
		auto const &haplotypes(m_calculate_task->haplotypes());
		auto const &multi_alt_haplotypes(m_calculate_task->multi_alt_haplotypes());

		// Make the comparisons.
		auto const &lhs_bv(haplotypes[m_i]);
		auto const &rhs_bv(haplotypes[m_j]);
		sdsl::bit_vector or_target_bv(lhs_bv);
		sdsl::bit_vector and_target_bv(lhs_bv);
		sdsl::bit_vector xor_target_bv(lhs_bv);
		or_target_bv |= rhs_bv;
		and_target_bv &= rhs_bv;
		xor_target_bv ^= rhs_bv;

		// Count the ones in the result.
		auto const or_ones(count_ones(or_target_bv));
		auto const and_ones(count_ones(and_target_bv));
		auto const xor_ones(count_ones(xor_target_bv));

		// Handle the multi-alt vectors.
		std::uint32_t same_nonzero_mv(0);
		std::uint32_t any_two_nonzero_mv(0);
		std::uint32_t any_different_mv(0);
		for (auto const &mv_vec : multi_alt_haplotypes)
		{
			// For Jaccard distance, check if both samples have a non-zero ALT.
			if (mv_vec[m_i] && mv_vec[m_j])
			{
				++any_two_nonzero_mv;

				// Check if the values are equal.
				if (mv_vec[m_i] == mv_vec[m_j])
					++same_nonzero_mv;
			}

			// For Hamming and SMD, count different values.
			if (mv_vec[m_i] != mv_vec[m_j])
				++any_different_mv;
		}

		m_same_value_present_in_both = and_ones + same_nonzero_mv;
		m_any_value_present_in_either = or_ones + any_two_nonzero_mv;
		m_different_values = xor_ones + any_different_mv;
	}

	std::uint32_t compare_two_task::count_ones(sdsl::bit_vector const &bv) const
	{
		// Count ones in bit vectors by using an operation on each 64-bit word.
		auto const *data(bv.data());
		auto const bit_count(bv.size());
		auto const full_word_count(bit_count / 64);
		std::uint32_t retval(0);

		// The bit vector has 8 additional bytes if 0 == bit_count % 64.
		// In any case the padding is set to zero.
		// See https://github.com/xxsds/sdsl-lite/blob/master/include/sdsl/memory_management.hpp#L758
		// (sdsl::memory_manager::resize())
		for (std::size_t i(0); i <= full_word_count; ++i)
			retval += sdsl::bits_impl <>::cnt(data[i]);

		return retval;
	}

	void hamming_output_handler::output(
		std::uint32_t const total_variants,
		triangular_uint32_matrix const &mutual_set_values,
		triangular_uint32_matrix const &all_set_values,
		triangular_uint32_matrix const &different_values
	)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Writing Hamming distances…" << std::endl;
		});

		// Hamming distance is equal to the number of different values.
		output_matrix(different_values);
	}

	void smd_output_handler::output(
		std::uint32_t const total_variants,
		triangular_uint32_matrix const &mutual_set_values,
		triangular_uint32_matrix const &all_set_values,
		triangular_uint32_matrix const &different_values
	)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Writing Simple Matching Distances…" << std::endl;
		});

		// SMD is equal to the number of different values normalized.
		triangular_float_matrix mat(1.0 * different_values / total_variants);
		output_matrix(mat);
	}

	void jaccard_output_handler::output(
		std::uint32_t const total_variants,
		triangular_uint32_matrix const &mutual_set_values,
		triangular_uint32_matrix const &all_set_values,
		triangular_uint32_matrix const &different_values
	)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Writing Jaccard distances…" << std::endl;
		});

		// Jaccard distance of two sets A and B is
		// 1 - J(A, B) = 1 - |A ∩ B| / |A ∪ B|.
		// We consider only “non-missing” values, i.e. for
		// each pair the variants that are present in at least
		// one of the samples.
		triangular_float_matrix mat(mutual_set_values * -1.0);
		hadamard_division(mat, all_set_values);
		for (std::size_t i(0), count(mat.size1()); i < count; ++i)
		{
			for (std::size_t j(0); j <= i; ++j)
				mat(i, j) += 1.0;
		}

		output_matrix(mat);
	}

	void intersection_size_output_handler::output(
		std::uint32_t const total_variants,
		triangular_uint32_matrix const &mutual_set_values,
		triangular_uint32_matrix const &all_set_values,
		triangular_uint32_matrix const &different_values
	)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Writing intersection sizes…" << std::endl;
		});

		output_matrix(mutual_set_values);
	}

	void union_size_output_handler::output(
		std::uint32_t const total_variants,
		triangular_uint32_matrix const &mutual_set_values,
		triangular_uint32_matrix const &all_set_values,
		triangular_uint32_matrix const &different_values
	)
	{
		lb::dispatch_async_fn(dispatch_get_main_queue(), [](){
			std::cerr << "Writing union sizes…" << std::endl;
		});

		output_matrix(all_set_values);
	}
}

namespace vcfdistances {

	void calculate_distances(
		std::vector <std::string> &&inputs,
		input_format const fmt,
		char const *sample_names_dst_path,
		char const *hamming_dst_path,
		char const *jaccard_dst_path,
		char const *smd_dst_path,
		char const *intersection_sizes_dst_path,
		char const *union_sizes_dst_path
	)
	{
		calculate_context *ctx(new calculate_context(fmt));

		// Prepare to output matrices according to the given file names.
		if (sample_names_dst_path)
			ctx->output_sample_names(sample_names_dst_path);

		if (hamming_dst_path)
			ctx->output_hamming_distances(hamming_dst_path);

		if (jaccard_dst_path)
			ctx->output_jaccard_distances(jaccard_dst_path);

		if (smd_dst_path)
			ctx->output_smd(smd_dst_path);

		if (intersection_sizes_dst_path)
			ctx->output_intersection_sizes(intersection_sizes_dst_path);

		if (union_sizes_dst_path)
			ctx->output_union_sizes(union_sizes_dst_path);

		// Calculate the distances.
		ctx->prepare();
		ctx->calculate_distances(std::move(inputs));
	}
}


// Author:Nick Sauter
#include <cctbx/boost_python/flex_fwd.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/versa.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <algorithm>

namespace sx_clustering {

  struct Rodriguez_Laio_clustering_2014 {

    Rodriguez_Laio_clustering_2014( ):rho_set(false){}

    Rodriguez_Laio_clustering_2014(
      scitbx::af::flex_double distance_matrix,
      double d_c):
        distance_matrix( distance_matrix ),
        d_c(d_c),
        rho_set(false){
        NN = distance_matrix.accessor().focus()[0];
        SCITBX_ASSERT(NN == distance_matrix.accessor().focus()[1]);// square matrix
    }

    scitbx::af::shared<std::size_t>
    get_rho(){
      if (!rho_set) {
        rho = scitbx::af::shared<std::size_t>(NN);
        const double* Dij = distance_matrix.begin();
        for (int i=0; i < NN; ++i){
          for (int j=0; j < NN; ++j){
            if (Dij[i*NN + j] < d_c) { rho[i]+=1; }
          }
        }
        rho_set=true;
      }
      return rho;
    }

    scitbx::af::shared<double>
    get_delta(scitbx::af::shared<std::size_t> rho_order, double delta_i_max){
      //! delta_i is measured by computing the minimum distance between the point i
      /*! and any other point with higher OR EQUAL density (emphasis mine)
       */
      delta = scitbx::af::shared<double>(NN,delta_i_max);
      const double* Dij = distance_matrix.begin();
      for (int p=1; p < NN; ++p){ // first iteration through rho order
        int i = rho_order[p];
        for (int q=0; q < p; ++q){
          int j = rho_order[q];
          if (rho[j] >= rho[i] && Dij[i*NN+j] < delta[i]) {
            delta[i] = Dij[i*NN+j];
          }
        }
      }
      return delta;
    }

    void cluster_assignment(scitbx::af::shared<std::size_t> rho_order,
                            scitbx::af::shared<int> cluster_id){
      //! one-pass assignment of clusters
      /*! assign each point to its nearest neighbor (Dij) of higher density
          the cluster_id input array is modified, the return type is void
       */
      scitbx::af::flex_double::const_iterator B = distance_matrix.begin();
      scitbx::af::flex_double::const_iterator E = distance_matrix.end();
      scitbx::af::flex_double::const_iterator max_Dij = std::max_element(B,E);
      const double* Dij = distance_matrix.begin();
      std::size_t init_value = std::numeric_limits<std::size_t>().max(); //None
      for (int p=0; p < NN; ++p){
        int item_idx = rho_order[p];
        if (cluster_id[item_idx] == -1){ // still unassigned
          double trial_Dij = *max_Dij;
          std::size_t i_neighbor = init_value;
          for (int q=0; q < p; ++q){
            double examined_distance = Dij[item_idx*NN + rho_order[q]];
            if ( examined_distance < trial_Dij) {
              i_neighbor = rho_order[q];
              trial_Dij = examined_distance;
            }
          }
          SCITBX_ASSERT(i_neighbor != init_value);
          cluster_id[item_idx] = cluster_id[i_neighbor];
        }
      }
    }

    scitbx::af::shared<bool>
    get_border(scitbx::af::shared<int> cluster_id){
      //! find border
      /*! defined as points that are within d_c of those in another cluster
       */
      scitbx::af::shared<bool> result(NN,false);
      const double* Dij = distance_matrix.begin();
      for (int i=0; i < NN; ++i){
        for (int j=i+1; j < NN; ++j){
          if (Dij[i*NN+j] < d_c){
            if (cluster_id[i] != cluster_id[j]){
              result[i]=true; result[j]=true;
            }
          }
        }
      }
      return result;
    }

    public:
      scitbx::af::shared<std::size_t> rho;
      scitbx::af::shared<double> delta;

    private:
      scitbx::af::flex_double distance_matrix;
      double d_c;
      bool rho_set;
      std::size_t NN;

  };

}

using namespace boost::python;
namespace sx_clustering{
namespace boost_python { namespace {

  void
  sx_clustering_init_module() {
    using namespace boost::python;

    class_<Rodriguez_Laio_clustering_2014>("Rodriguez_Laio_clustering_2014",no_init)
      .def(init<>())
      .def(init<scitbx::af::flex_double, double >(
          (arg_("distance_matrix"),arg_("d_c"))))
      .def("get_rho",&Rodriguez_Laio_clustering_2014::get_rho)
      .def("get_delta",&Rodriguez_Laio_clustering_2014::get_delta,(
          (arg_("rho_order"),arg_("delta_i_max"))))
      .def("cluster_assignment",&Rodriguez_Laio_clustering_2014::cluster_assignment,(
          (arg_("rho_order"),arg_("cluster_id"))))
      .def("get_border",&Rodriguez_Laio_clustering_2014::get_border,(
          (arg_("cluster_id"))))
    ;

  }

}
}} // namespace sx_clustering::boost_python::<anonymous>

BOOST_PYTHON_MODULE(sx_clustering_ext)
{
  sx_clustering::boost_python::sx_clustering_init_module();
}


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "MohrCoulomb.h"

#include "BaseLib/Error.h"
#include "MathLib/MathTools.h"

#ifndef Q_MOC_RUN // to avoid Qt4 bug, https://bugreports.qt.io/browse/QTBUG-22829
#include <boost/math/special_functions/sign.hpp>
#endif

namespace MaterialLib
{
namespace Fracture
{

namespace
{

struct MaterialPropertyValues
{
    double Kn = 0.0;
    double Ks = 0.0;
    double phi = 0.0; // friction angle
    double psi = 0.0; // dilation angle
    double c = 0.0;

    template <typename MaterialProperties>
    MaterialPropertyValues(
            MaterialProperties const& mp,
            double const t,
            ProcessLib::SpatialPosition const& x)
    {
        Kn = mp.normal_stiffness(t,x)[0];
        Ks = mp.shear_stiffness(t,x)[0];
        phi = MathLib::to_radians(mp.friction_angle(t,x)[0]);
        psi = MathLib::to_radians(mp.dilatancy_angle(t,x)[0]);
        c = mp.cohesion(t,x)[0];
    }
};

} // no namespace

template <int DisplacementDim>
void MohrCoulomb<DisplacementDim>::computeConstitutiveRelation(
        double const t,
        ProcessLib::SpatialPosition const& x,
        Eigen::Ref<Eigen::VectorXd const> w_prev,
        Eigen::Ref<Eigen::VectorXd const> w,
        Eigen::Ref<Eigen::VectorXd const> sigma_prev,
        Eigen::Ref<Eigen::VectorXd> sigma,
        Eigen::Ref<Eigen::MatrixXd> Kep,
        typename FractureModelBase<DisplacementDim>::MaterialStateVariables&
        material_state_variables)
{
    if (DisplacementDim == 3)
    {
        OGS_FATAL("MohrCoulomb fracture model does not support 3D case.");
        return;
    }
    material_state_variables.reset();

    MaterialPropertyValues const mat(_mp, t, x);
    Eigen::VectorXd const dw = w - w_prev;

    Eigen::MatrixXd Ke(2,2);
    Ke.setZero();
    Ke(0,0) = mat.Ks;
    Ke(1,1) = mat.Kn;

    sigma.noalias() = sigma_prev + Ke * dw;

    // if opening
    if (sigma[1] > 0)
    {
        Kep.setZero();
        sigma.setZero();
        material_state_variables.setTensileStress(true);
        return;
    }

    // check shear yield function (Fs)
    double const Fs = std::abs(sigma[0]) + sigma[1] * std::tan(mat.phi) - mat.c;
    material_state_variables.setShearYieldFunctionValue(Fs);
    if (Fs < .0)
    {
        Kep = Ke;
        return;
    }

    Eigen::VectorXd dFs_dS(2);
    dFs_dS[0] = boost::math::sign(sigma[0]);
    dFs_dS[1] = std::tan(mat.phi);

    // plastic potential function: Qs = |tau| + Sn * tan da
    Eigen::VectorXd dQs_dS(2);
    dQs_dS[0] = boost::math::sign(sigma[0]);
    dQs_dS[1] = std::tan(mat.psi);

    // plastic multiplier
    Eigen::RowVectorXd const A = dFs_dS.transpose() * Ke / (dFs_dS.transpose() * Ke * dQs_dS);
    double const d_eta = A * dw;

    // plastic part of the dispalcement
    Eigen::VectorXd const dwp = dQs_dS * d_eta;

    // correct stress
    sigma.noalias() = sigma_prev + Ke * (dw - dwp);

    // Kep
    Kep = Ke - Ke * dQs_dS * A;
}

template class MohrCoulomb<2>;
template class MohrCoulomb<3>;

}   // namespace Fracture
}  // namespace MaterialLib


/*
 * Common.hpp
 *
 *  Created on: Dec 30, 2012
 *      Author: david
 */

#ifndef DASP_SPECTRAL_COMMON_HPP
#define DASP_SPECTRAL_COMMON_HPP

#include <boost/graph/adjacency_list.hpp>
#include <Eigen/Dense>
#include <vector>

namespace graphseg
{
	namespace detail
	{
		struct EigenComponent
		{
			float eigenvalue;
			Eigen::VectorXf eigenvector;
		};

		struct SparseEntry
		{
			unsigned int i; // row index
			unsigned int j; // column index
			float weight; // value
		};

		/** A symmetric lower triangle sparse matrix
		 * Assertion: i >= j
		 * Assertion: sorted by j then by i
		 * Entries are sorted s.t. i
		 */
		struct SparseMatrix
		{
			unsigned int dim;
			std::vector<SparseEntry> entries;
		};
	}

	/** A simple weighted undirected graph */
	typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
			boost::no_property,
			float
	> SpectralGraph;

}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_PIO2_MEDIUM_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_MEDIUM_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_pio2_medium capabilities

    Computes the remainder modulo \f$\pi/2\f$ with medium algorithm,
    and the angle quadrant between 0 and 3.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r, rc;
    as_integer<T> n;
    rem_pio2_cephes(x, n, r);
    @endcode

    is similar to:

    @code
    as_integer<T> n = idivround2even(x, Pio_2<T>());
    T r =  remainder(x, Pio_2<T>());
    @endcode

    @par Note:

    @c rem_pio2_medium compute the remainder modulo \f$\pi/2\f$ with medium algorithm,
    and the angle quadrant between 0 and 3.
    This is a medium_ version version accurate if the input is in:
     \f$[-2^6\pi,2^6\pi]\f$ for float,
     \f$[-2^{18}\pi,2^{18}\pi]\f$ for double.
    \par
    The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm is over costly and implies the knowledge
    of a few hundred \f$pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on smaller intervals.

    @see rem_pio2, rem_pio2_straight,rem_2pi,  rem_pio2_cephes,

  **/
  const boost::dispatch::functor<tag::rem_pio2_medium_> rem_pio2_medium = {};
} }
#endif

#include <boost/simd/function/scalar/rem_pio2_medium.hpp>
#include <boost/simd/function/simd/rem_pio2_medium.hpp>

#endif


//
//  multivector.hpp
//
//  Created by r. on 09/05/14
//

#ifndef round1_multivector_hpp
#define round1_multivector_hpp

// Standard libraries
#include <algorithm>    // std::random_shuffle
#include <numeric> // std::iota
#include <iostream>
#include <vector>
#include <queue>
#include <string>
#include <sstream>
#include <list>
#include <set>
#include <limits>

// Boost MPI
#include <boost/mpi.hpp>
#include <boost/serialization/vector.hpp>

// Boost ublas
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

//
#include "../include/stopwatch.hpp"

namespace mypara
{

	namespace ublas = boost::numeric::ublas;
    namespace mpi = boost::mpi;

	class multivector : public ublas::matrix<double, ublas::column_major>
	{
	public:
		typedef ublas::matrix<double, ublas::column_major> parent_type;
		typedef parent_type::value_type value_type;
		typedef parent_type::size_type size_type;

		typedef ublas::matrix_column<parent_type> matrix_column;
		typedef ublas::matrix_column<const parent_type> const_matrix_column;

		typedef ublas::matrix<value_type, ublas::column_major> dense_matrix;
	private:
		// MPI
		mpi::communicator world;
	private:
		// Total number of verticals to distribute (horizontal size)
		unsigned int total;
		// For each process, begin and end of its portion
		std::vector<unsigned int> p2gja;
		std::vector<unsigned int> p2gjb;
	public:
		const mpi::communicator& getcm() const { return world; }
	public:
        // Width of the whole multivector
		unsigned int getot() const { return total; }
        // Rank in the communicator
		unsigned int getrk() const { return world.rank(); }
        // Begin of portion (global)
		unsigned int getja() const { return p2gja[getrk()]; }
        // End of portion (global)
		unsigned int getjb() const { return p2gjb[getrk()]; }
        // Size of portion
		unsigned int getsz() const { return (getjb() - getja()); }
	public:
		unsigned int gj2pr(unsigned int j) const
        // Finds the owner process of global column j
		{
			for (auto p = 0; p < world.size(); ++p)
				if ((p2gja[p] <= j) && (j < p2gjb[p]))
					return p;
			return world.size();
		}

		bool ismyj(unsigned int j) const { return ((getja() <= j) && (j < getjb())); }
	public:
		multivector(const mpi::communicator& comm)
		: world(comm), total(0), parent_type(0,0)
		{
		}

		multivector(size_type size1, unsigned int total, const mpi::communicator& comm)
		: world(comm), total(total), parent_type(0,0)
		// Pre: the operation is requested by all processes in the communicator comm
		{
			stopwatch::Time time(stopwatch::watches, "multivector::multivector");

//			world.barrier();

			//cout << "ID: " << world.rank() << ": new multivector" << endl;

			unsigned int wsize = world.size();

			// Compute everybody's portion
			{
				unsigned int share = total; // Number of vectors still to be distributed
				unsigned int pleft = wsize; // Number of processes still unemployed
				while (pleft)
				{
					p2gjb.push_back(share);
					share -= (share / pleft);
					p2gja.push_back(share);
					pleft--;
				}
				assert(share == 0);
				assert(pleft == 0);
			}

			assert(p2gja.size() == wsize);
			assert(p2gjb.size() == wsize);

			parent_type& mymat = *this;
			mymat.resize(size1, getsz());
			mymat.clear();
		}
	public:
		multivector& operator=(const multivector&) = default;
	public:
		matrix_column
		colio_local(unsigned int j)
		// Provides access to global column #j
		// Pre: the column is owned by the local process
		{
			assert(ismyj(j));
			unsigned int loclj = (j - getja());
			parent_type& x = *this;
			ublas::matrix_column< parent_type > mc(x, loclj);
			return mc;
		}

		void
		setcolumn(unsigned int j, const ublas::vector<value_type>& colmn)
		// Replaces global column #j
		// Pre: the column is owned by the local process
		{
			matrix_column mc = this->colio_local(j);
			mc = colmn;
		}

        ublas::vector<value_type>
        getcolumn(unsigned int j) const
		// Gets a copy of global column #j
		// Pre: the column is owned by the local process
        {
			assert(ismyj(j));
			unsigned int loclj = (j - getja());
			const parent_type& x = *this;
            ublas::vector<value_type> v = column(x, loclj);
			return v;
        }

		dense_matrix
		getwhole() const
        // Combines all portions to one dense matrix available locally
		// Pre: requested by all processes in the communicator of u
		{
			getcm().barrier();

			std::vector<parent_type> parts;
			mpi::all_gather(getcm(), (parent_type&)(*this), parts);
			dense_matrix whole(size1(), getot());
			for (auto p = 0; p < parts.size(); ++p)
			{
				ublas::range vrang(0, parts[p].size1());
				ublas::range hrang(p2gja[p], p2gjb[p]);
				ublas::matrix_range<dense_matrix> subma(whole, vrang, hrang);
				subma = parts[p];
			}
			return whole;
		}
	public:
		bool
		shrink_communicator()
		// If the local has no capacity, it is removed from the communicator
		// Returns true iff the local has any capacity
		{
			multivector& me = *this;

			unsigned int isfat = ((me.size2() == 0) ? 0 : 1);
			mpi::communicator small = me.getcm().split(isfat);

			embrace_communicator(small);

			return (isfat != 0);
		}

		void
		embrace_communicator(const mpi::communicator& newcm)
		{
			multivector& me = *this;

			unsigned int newja = me.getja();
			unsigned int newjb = me.getjb();

			assert(me.size2() == (newjb - newja));

			me.world = newcm;

			me.p2gja.resize(0);
			me.p2gjb.resize(0);
			mpi::all_gather(me.world, newja, me.p2gja);
			mpi::all_gather(me.world, newjb, me.p2gjb);
		}

	private:
		void
		keepj(unsigned int ja, unsigned int jb)
		{
            stopwatch::Time time(stopwatch::watches, "mypara::multivector::keepj()");

			multivector& me = *this;
            assert((0 <= ja) && (ja <= jb) && (jb <= me.getot()));

            unsigned int np = me.getcm().size();
            assert((np == me.p2gja.size()) && (np == me.p2gjb.size()));

            for (unsigned int p = 0; p != np; ++p)
            {
                // Old range of process p
                unsigned int oldja = me.p2gja[p];
                unsigned int oldjb = me.p2gjb[p];

                // New range of process p
                unsigned int newja, newjb;
                {
                    newja = std::min(std::max(oldja, ja), oldjb);
                    newjb = std::max(std::min(oldjb, jb), oldja);
                    assert(newja <= newjb);
                    assert((oldja <= newja) && (newjb <= oldjb));
                }

                // If I am process p, then cut my data to the new range
                if (p == me.getcm().rank()) {
                    ublas::range hrang(newja - oldja, newjb - oldja);
                    ublas::range vrang(0, me.size1());
                    ((parent_type)me) = ublas::matrix_range<parent_type>(me, vrang, hrang);
                }

                // Compute p2gja and p2gjb of process p
                {
                    assert(newja <= newjb);
                    if (newja != newjb) {
                        assert((ja <= newja) && (newja <= newjb) && (newjb <= jb));
                        newja -= ja;
                        newjb -= ja;
                    } else {
                        newja = newjb = 0;
                    }

                    me.p2gja[p] = newja;
                    me.p2gjb[p] = newjb;
                }
            }

            me.total = (jb - ja);
		}

	public:
		void
		split(multivector& A, multivector& B, unsigned int j0) const
		// First j0 columns of *this go to a, the others to b
		// Pre: *this, a and b all have the same communicator
		{
            stopwatch::Time time(stopwatch::watches, "mypara::multivector::split()");

			const multivector& me = *this;
			assert(j0 <= me.getot());

			//
			B.p2gja = A.p2gja = me.p2gja;
			B.p2gjb = A.p2gjb = me.p2gjb;
            B.total = A.total = me.total;

            // Clean data of a and b, but make consistent
			A.resize(0, A.getjb() - A.getja());
			B.resize(0, B.getjb() - B.getja());

            //
			A.keepj(0, j0);
			B.keepj(j0, me.getot());

			// Copy data
			A.resize(me.size1(), A.getjb() - A.getja());
			B.resize(me.size1(), B.getjb() - B.getja());

			// This could be done more efficiently:

			for (unsigned int j = A.getja(); j != A.getjb(); ++j) {
				A.setcolumn(j, me.getcolumn(j));
			}

			for (unsigned int j = B.getja(); j != B.getjb(); ++j) {
				B.setcolumn(j, me.getcolumn(j0 + j));
			}
		}

		void
		merge(multivector& a, multivector& b)
		// Pre: a and b are the (possibly modified) result of split
		// Note: a, b, and this need not share the communicator
		{
			stopwatch::Time time(stopwatch::watches, "mypara::multivector::merge()");

			multivector& me = *this;
			assert(me.getot() == (a.getot() + b.getot()));
			assert((me.size1() == a.size1()) && (me.size1() == b.size1()));

			for (unsigned int j = a.getja(); j != a.getjb(); ++j)
			{
				me.setcolumn(j, a.getcolumn(j));
			}

			unsigned int j0 = a.getot();
			for (unsigned int j = b.getja(); j != b.getjb(); ++j)
			{
				me.setcolumn(j0 + j, b.getcolumn(j));
			}
		}

	};

	// Operators

	double
	inner_prod_max(const multivector& a, const multivector& b)
	// Pre: a and b share the mpi communicator
	// Pre: the operation is requested by all processes in the communicator of a / b
	{
		stopwatch::Time time(stopwatch::watches, "multivector::inner_prod");

		std::vector<double> local_vec;
		{
			assert(a.getja() == b.getja());
			assert(a.getjb() == b.getjb());

			for (unsigned int k = 0; k != a.getsz(); ++k)
				local_vec.push_back(ublas::inner_prod(ublas::column(a, k), ublas::column(b, k)));
		}

		// negative infinity
		double local = std::numeric_limits<double>::lowest();

		if (local_vec.size())
			local = *std::max_element(local_vec.begin(), local_vec.end());

		double global = 0;
		{
			mpi::all_reduce(a.getcm(), local, global, mpi::maximum<double>());
		}

		return global;
	}

	double
	inner_prod(const multivector& a, const multivector& b)
	// Pre: a and b share the mpi communicator
	// Pre: the operation is requested by all processes in the communicator of a / b
	{
        stopwatch::Time time(stopwatch::watches, "multivector::inner_prod");

		double local = 0;
		{
			assert(a.getja() == b.getja());
			assert(a.getjb() == b.getjb());

			for (unsigned int k = 0; k != a.getsz(); ++k)
			{
				local += ublas::inner_prod(ublas::column(a, k), ublas::column(b, k));
			}
		}

		double global = 0;
		{
			mpi::all_reduce(a.getcm(), local, global, std::plus<double>());
		}

		return global;
	}

    // RIGHT MULTIPLY

    void
    prod_ref(const multivector& u, const multivector::dense_matrix& m, multivector& v)
    // This is the reference implementation for the following:
    // Right matrix multiply u * m, assumes no structure in m
    // Compare with function prod(u, m, jplan, v)
    // Pre: the operation is requested by all processes in the communicator of u and v
    // Pre: u and v have the same communicator
    {
        stopwatch::Time time(stopwatch::watches, "multivector::prod(ref)");

        typedef multivector::value_type value_type;
        typedef multivector::const_matrix_column const_matrix_column;
        typedef ublas::vector<value_type> vector;

        for (unsigned int j = 0; j != v.getot(); ++j)
        {
            namespace ublas = boost::numeric::ublas;
            typedef ublas::vector<value_type> Vec;

            // From the j-th column of the right matrix m, get local subvector
            ublas::vector<value_type> submv(u.getsz());
            {
                ublas::range verrg(u.getja(), u.getjb());
                const_matrix_column colmj(m, j);
                submv = ublas::vector_range<const_matrix_column>(colmj, verrg);
            }

            // Am I receiving data and/or sending data?
            bool itodo = (v.ismyj(j) || (0 != ublas::norm_inf(submv)));

            // A. Who has something to contribute?
            // Note: cannot interchange order with B
            mpi::communicator activ = u.getcm().split(itodo ? 1 : 0);

            // B. Do I have something to contribute? If not, skip.
            // Note: cannot interchange order with A
            if (!itodo) continue;

            // Find the rank of the process in the activ communicator
            // that holds the j-th column of the result
            unsigned int trank;
            {
                unsigned int local = (v.ismyj(j) ? activ.rank() : 0);
                mpi::all_reduce(activ, local, trank, std::plus<unsigned int>());
            }

            // Local contribution to j-th column of the result
            Vec myvec(u.size1());
            myvec = ublas::prod(u, submv);

            // Am I to receive the product vector?
            if (activ.rank() == trank) {
                Vec resul(myvec.size());
                mpi::reduce(activ, myvec, resul, std::plus<Vec>(), trank);
                v.setcolumn(j, resul);
            } else {
                mpi::reduce(activ, myvec, std::plus<Vec>(), trank);
            }
        }

        v.getcm().barrier();
    }

	struct Jplan
	{
	public:
		enum Strategy { nnz = 1, jforward = 2, jreverse = 4 };
	private:
		Strategy strategy;
	public:
		typedef double value_type;
		typedef ublas::compressed_vector<value_type> sparse_vector;
		typedef ublas::compressed_matrix<value_type, ublas::column_major> sparse_matrix;
	public:
		std::vector<sparse_vector> M_local;
	public:
		typedef ublas::vector<unsigned int> GROUP;
		typedef ublas::vector<unsigned int> TARGT;
	public:
		std::vector<GROUP> group_plan;
		std::vector<TARGT> targt_plan;
	private:
		bool ready;
	public:
		bool is_ready() const { return ready; }
	public:
		Jplan() : ready(false), strategy(Strategy::nnz) { }

		Jplan& operator()(const Strategy& s)
		{
			strategy = s;
			return (*this);
		}

		void
		compute(const multivector& u, const sparse_matrix& m, const multivector& v)
		// Pre: u and v have the same communicator
		{
			stopwatch::Time time(stopwatch::watches, "Jplan::compute");

			assert(u.getot() == m.size1());
			assert(v.getot() == m.size2());

			// Horizontal size of the result multivector v
			unsigned int width = (unsigned int)(m.size2());

			// Clear
			{
				M_local.clear();
				group_plan.clear();
				targt_plan.clear();
			}

			std::vector<sparse_vector> M;

			// Step 1.
			// Convert the right multiplication matrix to a vector of vectors
			for (unsigned int j = 0; j != width; ++j)
			{
				sparse_vector v(ublas::column(m, j));
				M.push_back(v);

				// From the j-th column of the right matrix m, get local subvector
				sparse_vector submv(u.getsz());
				{
					ublas::range range(u.getja(), u.getjb());
					submv = ublas::vector_range<sparse_vector>(v, range);
				}
				assert(submv.size() == u.getsz());
				M_local.push_back(submv);
			}

			// Step 2.
			// Create sets of disjoint communication groups
			{
				std::list<unsigned int> jleft;
				{
					std::vector<unsigned int> order(M.size());

					// Default strategy: Strategy::jforward
					std::iota(order.begin(), order.end(), 0.);

					if (strategy == Strategy::nnz)
					{
						// Sort with decreasing nnz value
						struct R {
							typedef std::vector<sparse_vector> VOV;
							const VOV& M;
							R(const VOV& M) : M(M) { }
							bool operator()(unsigned int i, unsigned int j) const {
								return (M[i].nnz() > M[j].nnz());
							}
						};
						std::sort(order.begin(), order.end(), R(M));
					}

					if (strategy == Strategy::jreverse) {
						std::sort(order.rbegin(), order.rend());
					}

					for (unsigned int j = 0; j != width; ++j)
						jleft.push_back(order[j]);
				}

				// While there are target columns to process
				while (jleft.size())
				{
					GROUP group(u.getcm().size());
					// group[p] is the number of the communication group
					// where process p is involved
					// It is zero if the process has nothing to communicate

					TARGT targt(u.getcm().size());
					// targt[p] is the target column in the result
					// for the communication group masks[p]

					group.clear();
					targt.clear();

					unsigned int grpno = 0;
					auto pj = jleft.begin();
					while (pj != jleft.end())
					{
						// jmask[p] is one if process p is involved, and zero else
						ublas::vector<unsigned int> jmask(group.size());
						jmask.clear();

						unsigned int j = *pj;

						// conflict iff jmask has nontrivial overlap with group
						bool conflict = false;

						// Target index
						{
							unsigned int p = v.gj2pr(j);
							conflict = conflict || (group[p] != 0);
							jmask[p] = 1;
						}

						// Source indices
						{
							typedef ublas::compressed_vector<double> sparse_vector;
							const sparse_vector& vj = M[j];
							// (Does a non-const iterator work here?)
							for (sparse_vector::const_iterator i = vj.begin(); i != vj.end(); ++i)
							{
								unsigned int p = u.gj2pr((unsigned int)i.index());
								conflict = conflict || (group[p] != 0);
								jmask[p] = 1;
								if (conflict) break;
							}
						}

						if (conflict) {
							// Bad luck, try another column j
							pj++;
						} else {
							grpno++;
							group += (grpno * jmask);
							targt += (j * jmask);

							// The target column j has been scheduled for processing
							// Remove from queue and proceed to next column
							pj = jleft.erase(pj);
						}
					}

					group_plan.push_back(group);
					targt_plan.push_back(targt);
				}
			}

			this->ready = true;
		}
	}; // class Jplan

	void
	prod(const multivector& u, const Jplan::sparse_matrix& m, Jplan& jplan, multivector& v)
    // Right matrix multiply, u * m
	// Good for matrices m where each row is sparse
	// Pre: the operation is requested by all processes in the communicator of u and v
	// Pre: u and v have the same communicator
	// TODO: implement using MPI groups
	{
        stopwatch::Time time(stopwatch::watches, "multivector::prod(jplan)");

		typedef multivector::value_type value_type;
		//		typedef multivector::dense_matrix dense_matrix;
		typedef multivector::const_matrix_column const_matrix_column;
        typedef ublas::vector<value_type> vector;

		if (!jplan.is_ready()) jplan.compute(u, m, v);
		assert(jplan.is_ready());

		for (unsigned int k = 0; k != jplan.group_plan.size(); ++k)
		{
			auto& group = jplan.group_plan[k];
			auto& targt = jplan.targt_plan[k];

			//std::cout << "group: " << group << std::endl;
			//std::cout << "targt: " << targt << std::endl;

			// Partition the MPI communicator into disjoint groups

			unsigned int r = u.getcm().rank();
			unsigned int color = group[r];
			//std::cout << masks << std::endl;
			mpi::communicator activ = u.getcm().split(color);

			//std::cout << "Proc: " << r << "; color: " << color << "; targt: " << targt[r] << std::endl;

			// Am I in one of the communicating groups?
			if (!color) continue;

			// The truly parallel part:
			// Compute the partial matrix matrix product

			unsigned int j = targt[r];

			// From the j-th column of the right matrix m, get local subvector
			const Jplan::sparse_vector& submv = jplan.M_local[j];

			// Find the rank of the process in the activ communicator
			// that holds the j-th column of the result
			unsigned int jdest;
			{
				unsigned int local = (v.ismyj(j) ? activ.rank() : 0);
				mpi::all_reduce(activ, local, jdest, std::plus<unsigned int>());
			}

			//std::cout << "j: " << j << "; trank: " << trank << std::endl;

			// Local contribution to j-th column of the result
			typedef ublas::vector<value_type> Vec;
			Vec myvec(u.size1()); myvec.clear();
			myvec = ublas::prod(u, submv);

			// Am I to receive the product vector?
			if (activ.rank() == jdest) {
				Vec resul(myvec.size());
				mpi::reduce(activ, myvec, resul, std::plus<Vec>(), jdest);
				v.setcolumn(j, resul);
			} else {
				mpi::reduce(activ, myvec, std::plus<Vec>(), jdest);
			}
		}

        #ifndef NDEBUG
        /// Check correctness
        {
            multivector v_ref = v;
            mypara::prod_ref(u, m, v_ref);
            assert(v.size1() == v_ref.size1());
            assert(v.size2() == v_ref.size2());
            assert(ublas::norm_inf(v - v_ref) <= 1e-6);
        }//*/
        #endif
	}

	multivector
	prod(const multivector& u, const multivector::dense_matrix& m, Jplan& jplan)
    // Right matrix multiply. See void prod(u, m, jplan, v)
	{
		assert(u.getot() == m.size1());

		multivector v(u.size1(), (unsigned int)(m.size2()), u.getcm());
		mypara::prod(u, m, jplan, v);

		return v;
	}

	void
	prod(const multivector& u, const ublas::compressed_matrix<double, ublas::column_major>& m, multivector& v)
	// Asynchronous implementation of:
    // Right matrix multiply u * m
	// Compare with function prod(u, m, jplan, v)
    // Pre: m is sparse (send buffers created)
	// Pre: the operation is requested by all processes in the communicator of u and v
	// Pre: u and v have the same communicator
	{
        stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch)");

        v.clear();

		// Type of m
		typedef ublas::compressed_matrix<double, ublas::column_major> matrix;
		// Column of m
		typedef ublas::matrix_column<const matrix> cmc;
		// Vector type of column of m
		typedef ublas::compressed_vector<matrix::value_type> sparse_vector;

		// Step 1.
		// Convert the right multiplication matrix to a vector of vectors
		std::vector<sparse_vector> M, M_local;
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).Mlocal");

			for (unsigned int j = 0; j != m.size2(); ++j)
			{
				sparse_vector vcolj(ublas::column(m, j));

				// From the j-th column of the right matrix m, get local subvector
				sparse_vector submv(u.getsz());
				{
					ublas::range range(u.getja(), u.getjb());
					submv = ublas::vector_range<sparse_vector>(vcolj, range);
					assert(submv.size() == u.getsz());
				}

				M.push_back(vcolj);
				M_local.push_back(submv);
			}
		}

		// Vector for a column of u or v
		typedef ublas::vector<multivector::value_type> Vec;

        //
        struct Packet {
            int dest, src, tag;
            Vec myvec;
            //
            Packet(int dest, int tag, const Vec& myvec, int src) : dest(dest), tag(tag), myvec(myvec), src(src)
            { }
			//
			std::string str() const {
                std::stringstream s;
                s << "dest: " << dest << ", "
                  << "tag: " << tag << ", "
                  << "vecsize: " << myvec.size() << ", "
                  << "src: " << src;
                return s.str();
			}
        };

        // Queue in everything that has to be sent
        std::list<Packet> sendbuf;
        std::vector<mpi::request> sendreq;
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).sendbuf");

			// Prepare for send: iterate over target columns
			for (unsigned int j = 0; j != v.getot(); ++j)
			{
				// Is the target local?
				if (v.ismyj(j)) continue;

				// Local subvector from the j-th column of the right matrix m
				const sparse_vector& submv = M_local[j];

				// Do I have data to send?
				if (0 == submv.nnz()) continue;

				// Local contribution to j-th column of the result
				Vec myvec = ublas::prod(u, submv);
				assert(myvec.size() == v.size1());

				// Destination rank
				const unsigned int trank = v.gj2pr(j);
				// Source rank
				const unsigned int srank = u.getcm().rank();

				assert(trank != srank);
				const int tag = j;
				// Put data into send buffer
				sendbuf.push_back(Packet(trank, tag, myvec, srank));

				// Submit MPI send request
				const Packet& packet = sendbuf.back();
				//std::cout << "Sending packet " << packet.str() << std::endl;
				sendreq.push_back(u.getcm().isend(packet.dest, packet.tag, packet.myvec));
			}
		}

        // Queue in everything that has to be received
        std::list<Packet> recvbuf;
        std::vector<mpi::request> recvreq;
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).recvbuf");

			for (unsigned int j = v.getja(); j != v.getjb(); ++j)
			{
				// j-th column of m
				sparse_vector vcolj = M[j];

				// Identify senders
				std::set<unsigned int> ranks;
				for (sparse_vector::const_iterator i = vcolj.begin(); i != vcolj.end(); ++i)
				{
					// Is the source local?
					if (u.ismyj((unsigned int)i.index())) continue;

					unsigned int srank = u.gj2pr((unsigned int)i.index());
					ranks.insert(srank);
				}

				for (auto i = ranks.begin(); i != ranks.end(); ++i)
				{
					const unsigned int srank = (*i);
					const int tag = j;

					// Receive buffer
                    // The target vector will be automatically resized by boost::mpi
					recvbuf.push_back(Packet(v.getcm().rank(), tag, Vec(0), srank));

					// Submit MPI receive request
					Packet& packet = recvbuf.back();
					//std::cout << "Recving packet " << packet.str() << std::endl;
					recvreq.push_back(u.getcm().irecv(packet.src, packet.tag, packet.myvec));
				}
			}
		}

        // Do what is entirely local
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).local");

			for (unsigned int j = v.getja(); j != v.getjb(); ++j)
			{
				// Local contribution to j-th column of the result
				v.setcolumn(j, ublas::prod(u, M_local[j]));
			}
		}

		// Receive all data
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).recvall");

			mpi::wait_all(recvreq.begin(), recvreq.end());
			//std::cout << "#" << v.getcm().rank() << ": recieved" << std::endl;

			for (std::list<Packet>::const_iterator i = recvbuf.begin(); i != recvbuf.end(); ++i)
			{
				unsigned int j = i->tag;
				assert(v.ismyj(j));
				assert(i->myvec.size() == v.size1());
				v.colio_local(j) += i->myvec;
			}
		}

		// Wait for all sent data to be received (before destroying the send buffer)
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).sendall");

			mpi::wait_all(sendreq.begin(), sendreq.end());
			//std::cout << "#" << v.getcm().rank() << ": sent" << std::endl;
		}
//		v.getcm().barrier();

        #ifndef NDEBUG
        /// Check correctness
        {
            multivector v_ref = v;
            mypara::prod_ref(u, m, v_ref);
            assert(v.size1() == v_ref.size1());
            assert(v.size2() == v_ref.size2());
            assert(ublas::norm_inf(v - v_ref) <= 1e-6);
			std::cout << "Prod check ok (use -DNDEBUG to disable)" << std::endl;
        }//*/
        #endif
	}

	multivector
    prod(const multivector& u, const ublas::compressed_matrix<double, ublas::column_major>& m)
    // Right matrix multiply. See void prod(u, m, v)
	{
		assert(u.getot() == m.size1());

		multivector v(u.size1(), (unsigned int)(m.size2()), u.getcm());
		mypara::prod(u, m, v);

		return v;
	}

    // LEFT MULTIPLY

	multivector
	prod(const ublas::compressed_matrix<double, ublas::row_major>& m, const multivector& u)
    // Left matrix multiply
	{
        stopwatch::Time time(stopwatch::watches, "multivector::prod(left)");

//		u.getcm().barrier();

		assert(m.size2() == u.size1());

		multivector v(m.size1(), u.getot(), u.getcm());
		{
			//ublas::compressed_matrix<multivector::value_type, ublas::column_major> m1(m);
			//ublas::matrix<multivector::value_type, ublas::row_major> u1(u);
			stopwatch::Time time(stopwatch::watches, "multivector::prod(left)/ublas::prod");
			((multivector::parent_type&)v) = boost::numeric::ublas::prod(m, u);
		}

		return v;
	}
}

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "sparse_solver.h"
#include <Eigen/IterativeLinearSolvers>

template<typename T, typename I> void test_conjugate_gradient_T()
{
  typedef SparseMatrix<T,0,I> SparseMatrixType;
  ConjugateGradient<SparseMatrixType, Lower      > cg_colmajor_lower_diag;
  ConjugateGradient<SparseMatrixType, Upper      > cg_colmajor_upper_diag;
  ConjugateGradient<SparseMatrixType, Lower|Upper> cg_colmajor_loup_diag;
  ConjugateGradient<SparseMatrixType, Lower, IdentityPreconditioner> cg_colmajor_lower_I;
  ConjugateGradient<SparseMatrixType, Upper, IdentityPreconditioner> cg_colmajor_upper_I;

  CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_diag)  );
  CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_diag)  );
  CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_loup_diag)   );
  CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_lower_I)     );
  CALL_SUBTEST( check_sparse_spd_solving(cg_colmajor_upper_I)     );
}

void test_conjugate_gradient()
{
  CALL_SUBTEST_1(( test_conjugate_gradient_T<double,int>() ));
  CALL_SUBTEST_2(( test_conjugate_gradient_T<std::complex<double>, int>() ));
  CALL_SUBTEST_3(( test_conjugate_gradient_T<double,long int>() ));
}


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <vector>
#include <Eigen/Dense>
#include <vinecopulib/bicop/class.hpp>

namespace vinecopulib {
namespace tools_select {

std::vector <Bicop> create_candidate_bicops(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &data,
    const FitControlsBicop &controls);

std::vector <BicopFamily> get_candidate_families(
    const FitControlsBicop &controls);

void preselect_candidates(
    std::vector <Bicop> &bicops,
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &data,
    double tau,
    const Eigen::VectorXd& weights);

std::vector<double> get_c1c2(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &data,
    double tau,
    const Eigen::VectorXd& weights);

bool preselect_family(std::vector<double> c, double tau, const Bicop &bicop);

}
}

#include <vinecopulib/bicop/implementation/tools_select.ipp>


//  (C) Copyright John Maddock 2006.
//  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)

#ifndef BOOST_MATH_TOOLS_SOLVE_HPP
#define BOOST_MATH_TOOLS_SOLVE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/config.hpp>
#include <boost/math/tools/assert.hpp>

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable:4996 4267 4244)
#endif

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#ifdef _MSC_VER
#pragma warning(pop)
#endif

namespace boost{ namespace math{ namespace tools{

//
// Find x such that Ax = b
//
// Caution: this uses undocumented, and untested ublas code,
// however short of writing our own LU-decomposition code
// it's the only game in town.
//
template <class T>
boost::numeric::ublas::vector<T> solve(
          const boost::numeric::ublas::matrix<T>& A_,
          const boost::numeric::ublas::vector<T>& b_)
{
   //BOOST_MATH_ASSERT(A_.size() == b_.size());

   boost::numeric::ublas::matrix<T> A(A_);
   boost::numeric::ublas::vector<T> b(b_);
   boost::numeric::ublas::permutation_matrix<> piv(b.size());
   lu_factorize(A, piv);
   lu_substitute(A, piv, b);
   //
   // iterate to reduce error:
   //
   boost::numeric::ublas::vector<T> delta(b.size());
   for(unsigned k = 0; k < 1; ++k)
   {
      noalias(delta) = prod(A_, b);
      delta -= b_;
      lu_substitute(A, piv, delta);
      b -= delta;

      T max_error = 0;

      for(unsigned i = 0; i < delta.size(); ++i)
      {
         T err = fabs(delta[i] / b[i]);
         if(err > max_error)
            max_error = err;
      }
      //std::cout << "Max change in LU error correction: " << max_error << std::endl;
   }

   return b;
}

}}} // namespaces

#endif // BOOST_MATH_TOOLS_SOLVE_HPP


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120203    K. Kumar          File created.
 *      130118    K. Kumar          Added individual test tolerances and added note about possible
 *                                  problem with RKF78 coefficient set.
 *      130204    K. Kumar          Fixed bug in DOPRI8 coefficient set under Windows and added
 *                                  note about problem.
 *      130916    K. Kumar          Removed RKF56 integrator test.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      Montenbruck, O., Gill, E. Satellite Orbits: Models, Methods, Applications, Springer, 2005.
 *
 *    Notes
 *      There might be a problem with the RKF78 and DOPRI8 integrators, as the coefficients do not
 *      meet the required conditions to the tolerance achieved for all the other Runge-Kutta-type
 *      integrators tested (1.0e-14 versus 1.0e-15 for the other integrators). This should be
 *      looked into further to ensure that there are no bugs introduced in the coefficients
 *      used.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_runge_kutta_coefficients )

using numerical_integrators::RungeKuttaCoefficients;

void checkValidityOfCoefficientSet( const RungeKuttaCoefficients::CoefficientSets& coefficientSet,
                                    const double tolerance )
{
    // Declare coefficient set.
    RungeKuttaCoefficients coefficients;
    coefficients = coefficients.get( coefficientSet );

    // Check that the sum of the b-coefficients for both the integrated order and the
    // error-checking order is one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                Eigen::VectorXd::Constant( 2, 1.0 ),
                coefficients.bCoefficients.rowwise( ).sum( ), tolerance );

    // Check that the first c-coefficient is zero.
    BOOST_CHECK_SMALL( coefficients.cCoefficients( 0 ), tolerance );

    // Check that the c-coefficient/a-coefficient relation holds.
    for ( int i = 1; i < coefficients.cCoefficients.size( ); i++ )
    {
        if ( std::fabs( coefficients.cCoefficients( i ) ) < tolerance )
        {
            BOOST_CHECK_SMALL( coefficients.aCoefficients.row( i ).sum( ), tolerance );
        }

        else
        {
            BOOST_CHECK_CLOSE_FRACTION( coefficients.cCoefficients( i ),
                                        coefficients.aCoefficients.row( i ).sum( ), tolerance );
        }
    }
}

BOOST_AUTO_TEST_CASE( testRungeKuttaFehlberg45Coefficients )
{
    // Check validity of Runge-Kutta-Fehlberg 45 coefficients.
    checkValidityOfCoefficientSet( RungeKuttaCoefficients::rungeKuttaFehlberg45, 1.0e-15 );
}

BOOST_AUTO_TEST_CASE( testRungeKuttaFehlberg78Coefficients )
{
    // Check validity of Runge-Kutta-Fehlberg 78 coefficients.
    // Note, for some reason, the RKF78 set fails the unit test when the tolerance in
    // checkValidityOfCoefficientSet() is set to lower than this value (rows 8 and 9 of
    // aCoefficients matrix sum does not correspond to cCoefficient counterpart with tolerance less
    // than 1.0e-14).
    checkValidityOfCoefficientSet( RungeKuttaCoefficients::rungeKuttaFehlberg78, 1.0e-14 );
}

BOOST_AUTO_TEST_CASE( testRungeKutta87DormandAndPrinceCoefficients )
{
    // Check validity of Runge-Kutta 87 (Dormand and Prince) coefficients.
    // Note, for some reason, the DOPRI8 set fails the unit test when the tolerance in
    // checkValidityOfCoefficientSet() is set to lower than this value (row 10 of aCoefficients
    // matrix sum does not correspond to cCoefficient counterpart with tolerance less than
    // 1.0e-14).
    checkValidityOfCoefficientSet( RungeKuttaCoefficients::rungeKutta87DormandPrince, 1.0e-14 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      122027    A. Ronse          File created.
 *
 *    References
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/multi_array.hpp>

#include <limits>
#include <vector>
#include <cmath>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"

#include "Tudat/Mathematics/Interpolators/multiLinearInterpolator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_multi_linear_interpolation )

// Test 1: Comparison to MATLAB solution of the example provided in matlab's interp2 function
// documentation.
BOOST_AUTO_TEST_CASE( test2Dimensions )
{
    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 2 );
    for ( int i = 0; i < 5; i++ )
    {
        independentValues[ 0 ].push_back( 1950.0 + static_cast< double >( i ) * 10.0 );
    }

    for ( int i = 0; i < 3; i++ )
    {
        independentValues[ 1 ].push_back( 10.0 + static_cast< double >( i ) * 10.0 );
    }

    // Create two-dimensional array for dependent values.
    boost::multi_array< double, 2 > dependentValues;
    dependentValues.resize( boost::extents[ 5 ][ 3 ] );

    dependentValues[ 0 ][ 0 ] = 150.697;
    dependentValues[ 0 ][ 1 ] = 199.592;
    dependentValues[ 0 ][ 2 ] = 187.625;
    dependentValues[ 1 ][ 0 ] = 179.323;
    dependentValues[ 1 ][ 1 ] = 195.072;
    dependentValues[ 1 ][ 2 ] = 250.287;
    dependentValues[ 2 ][ 0 ] = 203.212;
    dependentValues[ 2 ][ 1 ] = 179.092;
    dependentValues[ 2 ][ 2 ] = 322.767;
    dependentValues[ 3 ][ 0 ] = 226.505;
    dependentValues[ 3 ][ 1 ] = 153.706;
    dependentValues[ 3 ][ 2 ] = 426.730;
    dependentValues[ 4 ][ 0 ] = 249.633;
    dependentValues[ 4 ][ 1 ] = 120.281;
    dependentValues[ 4 ][ 2 ] = 598.243;

    // Initialize interpolator.
    interpolators::MultiLinearInterpolator< double, double, 2 > twoDimensionalInterpolator(
            independentValues, dependentValues );

    // Set interpolation target's independent values.
    std::vector< double > targetValue( 2 );
    targetValue[ 0 ] = 1975.0;
    targetValue[ 1 ] = 15.0;

    // Declare interpolated dependent variable value and execute interpolation.
    const double interpolationResult = twoDimensionalInterpolator.interpolate( targetValue );

    // Check if equal to MATLAB result up to machine precision.
    BOOST_CHECK_CLOSE_FRACTION( 190.62875, interpolationResult,
                                std::numeric_limits< double >::epsilon( ) );
}

// Test 2: 4-dimensional test. Comparison to matlab interpolated solution of analytical function.
BOOST_AUTO_TEST_CASE( test4Dimensions )
{
    // Create independent variable vector.
    std::vector< std::vector< double > > independentValues;

    // Resize independent variable vector to two dimensions and assign values.
    independentValues.resize( 4 );
    for ( int i = 0; i < 11; i++ )
    {
        independentValues[ 0 ].push_back( -1.0 + static_cast< double >( i ) * 0.2 );
        independentValues[ 1 ].push_back( -1.0 + static_cast< double >( i ) * 0.2 );
        independentValues[ 2 ].push_back( -1.0 + static_cast< double >( i ) * 0.2 );
    }

    for ( int i = 0; i < 6; i++ )
    {
        independentValues[ 3 ].push_back( static_cast< double >( i ) * 2.0 );
    }

    // Create four-dimensional array for dependent values based on analytical function
    // f = t*e^(-x^2 - y^2 - z^2 ).
    boost::multi_array< double, 4 > dependentValues;
    dependentValues.resize( boost::extents[ 11 ][ 11 ][ 11 ][ 6 ] );

    for ( int i = 0; i < 11; i++ )
    {
        for ( int j = 0; j < 11; j++ )
        {
            for ( int k = 0; k < 11; k++ )
            {
                for ( int l = 0; l < 6; l++ )
                {
                    dependentValues[ i ][ j ][ k ][ l ] =
                            independentValues[ 3 ][ l ] *
                            std::exp( -std::pow( independentValues[ 0 ][ i ], 2 ) -
                                      std::pow( independentValues[ 1 ][ j ], 2 ) -
                                      std::pow( independentValues[ 2 ][ k ], 2 ) );
                }
            }
        }
    }

    // Initialize interpolator.
    interpolators::MultiLinearInterpolator< double, double, 4 > twoDimensionalInterpolator(
            independentValues, dependentValues );

    // Set interpolation target's independent values.
    std::vector< double > targetValue( 4 );
    targetValue[ 0 ] = -1.0;
    targetValue[ 1 ] = 0.1;
    targetValue[ 2 ] = 0.5;
    targetValue[ 3 ] = 7.0;

    // Declare interpolated dependent variable value and execute interpolation.
    const double interpolationResult = twoDimensionalInterpolator.interpolate( targetValue );

    // Check if equal to MATLAB result up to machine precision
    BOOST_CHECK_CLOSE_FRACTION( 1.956391733957447, interpolationResult,
                                std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat