#include "linear_elasticity.h"

#include <deal.II/base/multithread_info.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe_tools.h>

#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>

#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/trilinos_precondition.h>

#include <deal.II/meshworker/copy_data.h>
#include <deal.II/meshworker/scratch_data.h>

#include <deal.II/numerics/error_estimator.h>

using namespace dealii;

template<int dim>
LinearElasticity<dim>::LinearElasticity() :
		mpi_communicator(MPI_COMM_WORLD), pcout(std::cout,
				(Utilities::MPI::this_mpi_process(mpi_communicator) == 0)), timer(
				pcout, TimerOutput::summary, TimerOutput::cpu_and_wall_times), n_components(
				dim), triangulation(mpi_communicator,
				typename Triangulation<dim>::MeshSmoothing(
						Triangulation<dim>::smoothing_on_refinement
								| Triangulation<dim>::smoothing_on_coarsening)), dof_handler(
				triangulation), forcing_term(n_components), exact_solution(
				n_components), dirichlet_boundary_condition(n_components), neumann_boundary_condition(
				n_components), error_table(
				std::vector<std::string>(n_components, "u")), solver_control(
				"Solver control", 1000, 1e-12, 1e-12), velocity(0) {
	TimerOutput::Scope timer_section(timer, "constructor");
	add_parameter("Finite element space", fe_name);
	add_parameter("Mapping degree", mapping_degree);
	add_parameter("Number of global refinements", n_refinements);
	add_parameter("Output filename", output_filename);
	add_parameter("Forcing term expression", forcing_term_expression);
	add_parameter("Dirichlet boundary condition expression",
			dirichlet_boundary_conditions_expression);
	add_parameter("Coefficient expression", coefficient_expression);
	add_parameter("Number of threads", number_of_threads);
	add_parameter("Exact solution expression", exact_solution_expression);
	add_parameter("Neumann boundary condition expression",
			neumann_boundary_conditions_expression);

	add_parameter("Local pre-refinement grid size expression",
			pre_refinement_expression);

	add_parameter("Dirichlet boundary ids", dirichlet_ids);
	add_parameter("Neumann boundary ids", neumann_ids);

	add_parameter("Problem constants", constants);
	add_parameter("Grid generator function", grid_generator_function);
	add_parameter("Grid generator arguments", grid_generator_arguments);
	add_parameter("Number of refinement cycles", n_refinement_cycles);

	add_parameter("Estimator type", estimator_type, "", this->prm,
			Patterns::Selection("exact|kelly|residual"));

	add_parameter("Marking strategy", marking_strategy, "", this->prm,
			Patterns::Selection("global|fixed_fraction|fixed_number"));

	add_parameter("Coarsening and refinement factors",
			coarsening_and_refinement_factors);

	add_parameter("Use direct solver", use_direct_solver);
	add_parameter("Linear elasticity mu", mu);
	add_parameter("Linear elasticity lambda", lambda);

	this->prm.enter_subsection("Error table");
	error_table.add_parameters(this->prm);
	this->prm.leave_subsection();
}

template<int dim>
void LinearElasticity<dim>::initialize(const std::string &filename) {
	TimerOutput::Scope timer_section(timer, "initialize");
	ParameterAcceptor::initialize(filename, "last_used_parameters.prm",
			ParameterHandler::Short);
}

template<int dim>
void LinearElasticity<dim>::parse_string(const std::string &parameters) {
	TimerOutput::Scope timer_section(timer, "parse_string");
	ParameterAcceptor::prm.parse_input_from_string(parameters);
	ParameterAcceptor::parse_all_parameters();
}

template<int dim>
void LinearElasticity<dim>::make_grid() {
	TimerOutput::Scope timer_section(timer, "make_grid");

	constants["mu"] = mu;
	constants["lambda"] = lambda;

	const auto vars = dim == 1 ? "x" : dim == 2 ? "x,y" : "x,y,z";
	pre_refinement.initialize(vars, pre_refinement_expression, constants);
	GridGenerator::generate_from_name_and_arguments(triangulation,
			grid_generator_function, grid_generator_arguments);

	for (unsigned int i = 0; i < n_refinements; ++i) {
		for (const auto &cell : triangulation.active_cell_iterators())
			if (pre_refinement.value(cell->center()) < cell->diameter())
				cell->set_refine_flag();
		triangulation.execute_coarsening_and_refinement();
	}

	std::cout << "Number of active cells: " << triangulation.n_active_cells()
			<< std::endl;
}

template<int dim>
void LinearElasticity<dim>::refine_grid() {
	TimerOutput::Scope timer_section(timer, "refine_grid");
	// Cells have been marked in the mark() method.
	triangulation.execute_coarsening_and_refinement();
}

template<int dim>
void LinearElasticity<dim>::setup_system() {
	TimerOutput::Scope timer_section(timer, "setup_system");
	if (!fe) {
		fe = FETools::get_fe_by_name<dim>(fe_name);
		mapping = std::make_unique < MappingQGeneric < dim >> (mapping_degree);
		const auto vars = dim == 1 ? "x" : dim == 2 ? "x,y" : "x,y,z";
		forcing_term.initialize(vars, forcing_term_expression, constants);
		coefficient.initialize(vars, coefficient_expression, constants);
		exact_solution.initialize(vars, exact_solution_expression, constants);

		dirichlet_boundary_condition.initialize(vars,
				dirichlet_boundary_conditions_expression, constants);

		neumann_boundary_condition.initialize(vars,
				neumann_boundary_conditions_expression, constants);
	}

	dof_handler.distribute_dofs(*fe);

	locally_owned_dofs = dof_handler.locally_owned_dofs();
	DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);

	pcout << "Number of degrees of freedom: " << dof_handler.n_dofs()
			<< std::endl;

	constraints.clear();
	constraints.reinit(locally_relevant_dofs);
	DoFTools::make_hanging_node_constraints(dof_handler, constraints);

	for (const auto &id : dirichlet_ids)
		VectorTools::interpolate_boundary_values(*mapping, dof_handler, id,
				dirichlet_boundary_condition, constraints);
	constraints.close();

	DynamicSparsityPattern dsp(dof_handler.n_dofs());
	DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints, false);
	SparsityTools::distribute_sparsity_pattern(dsp, locally_owned_dofs,
			mpi_communicator, locally_relevant_dofs);

	system_matrix.reinit(locally_owned_dofs, locally_owned_dofs, dsp,
			mpi_communicator);

	solution.reinit(locally_owned_dofs, mpi_communicator);
	system_rhs.reinit(locally_owned_dofs, mpi_communicator);

	locally_relevant_solution.reinit(locally_owned_dofs, locally_relevant_dofs,
			mpi_communicator);

	error_per_cell.reinit(triangulation.n_active_cells());
}

template<int dim>
void LinearElasticity<dim>::assemble_system_one_cell(
		const typename DoFHandler<dim>::active_cell_iterator &cell,
		ScratchData &scratch, CopyData &copy) {
	auto &cell_matrix = copy.matrices[0];
	auto &cell_rhs = copy.vectors[0];

	cell->get_dof_indices(copy.local_dof_indices[0]);

	const auto &fe_values = scratch.reinit(cell);
	cell_matrix = 0;
	cell_rhs = 0;

	for (const unsigned int q_index : fe_values.quadrature_point_indices()) {
		for (const unsigned int i : fe_values.dof_indices()) {
			const auto eps_v = fe_values[velocity].symmetric_gradient(i,
					q_index); // SymmetricTensor<2,dim>
			const auto div_v = fe_values[velocity].divergence(i, q_index); // double

			for (const unsigned int j : fe_values.dof_indices()) {
				const auto eps_u = fe_values[velocity].symmetric_gradient(j,
						q_index); // SymmetricTensor<2,dim>
				const auto div_u = fe_values[velocity].divergence(j, q_index); // double

				cell_matrix(i, j) += (mu * scalar_product(eps_v, eps_u)
						+ lambda * div_u * div_v) * fe_values.JxW(q_index); // dx
			}
			for (const unsigned int i : fe_values.dof_indices()) {
				const auto comp_i = this->fe->system_to_component_index(i).first;
				cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
						this->forcing_term.value(
								fe_values.quadrature_point(q_index), comp_i) * // f(x_q)
						fe_values.JxW(q_index));           // dx
			}
		}
	}

	if (cell->at_boundary())
		//  for(const auto face: cell->face_indices())
		for (unsigned int f = 0; f < GeometryInfo<dim>::faces_per_cell; ++f)
			if (neumann_ids.find(cell->face(f)->boundary_id())
					!= neumann_ids.end()) {
				auto &fe_face_values = scratch.reinit(cell, f);
				for (const unsigned int q_index : fe_face_values.quadrature_point_indices())
					for (const unsigned int i : fe_face_values.dof_indices())
						cell_rhs(i) += fe_face_values.shape_value(i, q_index)
								* neumann_boundary_condition.value(
										fe_face_values.quadrature_point(
												q_index))
								* fe_face_values.JxW(q_index);
			}
}

template<int dim>
void LinearElasticity<dim>::copy_one_cell(const CopyData &copy) {
	constraints.distribute_local_to_global(copy.matrices[0], copy.vectors[0],
			copy.local_dof_indices[0], system_matrix, system_rhs);
}

template<int dim>
void LinearElasticity<dim>::assemble_system_on_range(
		const typename DoFHandler<dim>::active_cell_iterator &begin,
		const typename DoFHandler<dim>::active_cell_iterator &end) {
	QGauss<dim> quadrature_formula(fe->degree + 1);
	QGauss<dim - 1> face_quadrature_formula(fe->degree + 1);

	ScratchData scratch(*mapping, *fe, quadrature_formula,
			update_values | update_gradients | update_quadrature_points
					| update_JxW_values, face_quadrature_formula,
			update_values | update_quadrature_points | update_JxW_values);

	CopyData copy(fe->n_dofs_per_cell());

	static Threads::Mutex assemble_mutex;

	for (auto cell = begin; cell != end; ++cell) {
		assemble_system_one_cell(cell, scratch, copy);
		assemble_mutex.lock();
		copy_one_cell(copy);
		assemble_mutex.unlock();
	}
}

template<int dim>
void LinearElasticity<dim>::assemble_system() {
	TimerOutput::Scope timer_section(timer, "assemble_system");
	QGauss<dim> quadrature_formula(fe->degree + 1);
	QGauss<dim - 1> face_quadrature_formula(fe->degree + 1);

	ScratchData scratch(*mapping, *fe, quadrature_formula,
			update_values | update_gradients | update_quadrature_points
					| update_JxW_values, face_quadrature_formula,
			update_values | update_quadrature_points | update_JxW_values);

	CopyData copy(fe->n_dofs_per_cell());

	for (const auto &cell : dof_handler.active_cell_iterators())
		if (cell->is_locally_owned()) {
			assemble_system_one_cell(cell, scratch, copy);
			copy_one_cell(copy);
		}

	system_matrix.compress(VectorOperation::add);
	system_rhs.compress(VectorOperation::add);
}

template<int dim>
void LinearElasticity<dim>::solve() {
	TimerOutput::Scope timer_section(timer, "solve");
	// if (use_direct_solver == true)
	//   {
	//     SparseDirectUMFPACK system_matrix_inverse;
	//     system_matrix_inverse.initialize(system_matrix);
	//     system_matrix_inverse.vmult(solution, system_rhs);
	//   }
	// else
	//   {
	SolverCG<LA::MPI::Vector> solver(solver_control);
	LA::MPI::PreconditionAMG amg;
	amg.initialize(system_matrix);
	solver.solve(system_matrix, solution, system_rhs, amg);
	constraints.distribute(solution);

	locally_relevant_solution = solution;
}

template<int dim>
void LinearElasticity<dim>::estimate() {
	TimerOutput::Scope timer_section(timer, "estimate");
	if (estimator_type == "exact") {
		error_per_cell = 0;
		QGauss<dim> quad(fe->degree + 1);
		VectorTools::integrate_difference(*mapping, dof_handler,
				locally_relevant_solution, exact_solution, error_per_cell, quad,
				VectorTools::H1_seminorm);
	} else if (estimator_type == "kelly") {
		std::map<types::boundary_id, const Function<dim>*> neumann;
		for (const auto id : neumann_ids)
			neumann[id] = &neumann_boundary_condition;

		QGauss<dim - 1> face_quad(fe->degree + 1);
		KellyErrorEstimator<dim>::estimate(*mapping, dof_handler, face_quad,
				neumann, locally_relevant_solution, error_per_cell,
				ComponentMask(), &coefficient);
	} else if (estimator_type == "residual") {
		// h_T || f+\Delta u_h ||_0,T
		// + \sum over faces
		// 1/2 (h_F)^{1/2} || [n.\nabla u_h] ||_0,F

		QGauss<dim - 1> face_quad(fe->degree + 1);
		QGauss<dim> quad(fe->degree + 1);

		std::map<types::boundary_id, const Function<dim>*> neumann;
		for (const auto id : neumann_ids)
			neumann[id] = &neumann_boundary_condition;

		KellyErrorEstimator<dim>::estimate(*mapping, dof_handler, face_quad,
				neumann, locally_relevant_solution, error_per_cell,
				ComponentMask(), &coefficient);

		FEValues<dim> fe_values(*mapping, *fe, quad,
				update_hessians | update_JxW_values | update_quadrature_points);

		std::vector<double> local_laplacians(quad.size());

		double residual_L2_norm = 0;

		unsigned int cell_index = 0;
		for (const auto &cell : dof_handler.active_cell_iterators()) {
			fe_values.reinit(cell);

			fe_values.get_function_laplacians(locally_relevant_solution,
					local_laplacians);
			residual_L2_norm = 0;
			for (const auto q_index : fe_values.quadrature_point_indices()) {
				const auto arg = (local_laplacians[q_index]
						+ forcing_term.value(
								fe_values.quadrature_point(q_index)));
				residual_L2_norm += arg * arg * fe_values.JxW(q_index);
			}
			error_per_cell[cell_index] += cell->diameter()
					* std::sqrt(residual_L2_norm);

			++cell_index;
		}
	} else {
		AssertThrow(false, ExcNotImplemented());
	}
	auto global_estimator = error_per_cell.l2_norm();
	error_table.add_extra_column("estimator", [global_estimator]() {
		return global_estimator;
	});
	error_table.error_from_exact(*mapping, dof_handler,
			locally_relevant_solution, exact_solution);
}

template<int dim>
void LinearElasticity<dim>::mark() {
	TimerOutput::Scope timer_section(timer, "mark");
	if (marking_strategy == "global") {
		for (const auto &cell : triangulation.active_cell_iterators())
			cell->set_refine_flag();
	} else if (marking_strategy == "fixed_fraction") {
		parallel::distributed::GridRefinement::refine_and_coarsen_fixed_fraction(
				triangulation, error_per_cell,
				coarsening_and_refinement_factors.second,
				coarsening_and_refinement_factors.first);
	} else if (marking_strategy == "fixed_number") {
		parallel::distributed::GridRefinement::refine_and_coarsen_fixed_number(
				triangulation, error_per_cell,
				coarsening_and_refinement_factors.second,
				coarsening_and_refinement_factors.first);
	} else {
		Assert(false, ExcInternalError());
	}
}

template<int dim>
void LinearElasticity<dim>::output_results(const unsigned cycle) const {
	TimerOutput::Scope timer_section(timer, "output_results");
	DataOut<dim> data_out;
	DataOutBase::VtkFlags flags;
	flags.write_higher_order_cells = true;
	data_out.set_flags(flags);
	data_out.attach_dof_handler(dof_handler);
	std::vector<std::string> names(n_components, "u");
	std::vector<DataComponentInterpretation::DataComponentInterpretation> interpretation(
			n_components,
			DataComponentInterpretation::component_is_part_of_vector);

	data_out.add_data_vector(locally_relevant_solution, names,
			DataOut<dim>::type_dof_data, interpretation);
	// auto interpolated_exact = solution;
	// VectorTools::interpolate(*mapping,
	//                          dof_handler,
	//                          exact_solution,
	//                          interpolated_exact);
	// auto locally_interpolated_exact = interpolated_exact;

	// data_out.add_data_vector(interpolated_exact, "exact");
	data_out.add_data_vector(error_per_cell, "estimator");
	data_out.build_patches(*mapping, std::max(mapping_degree, fe->degree),
			DataOut<dim>::curved_inner_cells);
	std::string fname = output_filename + "_" + std::to_string(cycle) + ".vtu";
	data_out.write_vtu_in_parallel(fname, mpi_communicator);

	GridOut go;
	go.write_mesh_per_processor_as_vtu(triangulation,
			"tria_" + std::to_string(cycle), false, true);
}

template<int dim>
void LinearElasticity<dim>::print_system_info() {
	if (number_of_threads != -1 && number_of_threads > 0)
		MultithreadInfo::set_thread_limit(
				static_cast<unsigned int>(number_of_threads));

	std::cout << "Number of cores  : " << MultithreadInfo::n_cores()
			<< std::endl << "Number of threads: "
			<< MultithreadInfo::n_threads() << std::endl;
}

template<int dim>
void LinearElasticity<dim>::run() {
	print_system_info();
	make_grid();
	for (unsigned int cycle = 0; cycle < n_refinement_cycles; ++cycle) {
		setup_system();
		assemble_system();
		solve();
		estimate();
		output_results(cycle);
		if (cycle < n_refinement_cycles - 1) {
			mark();
			refine_grid();
		}
	}
	if (pcout.is_active())
		error_table.output_table(std::cout);
}

template class LinearElasticity<1> ;
template class LinearElasticity<2> ;
template class LinearElasticity<3> ;


// Copyright (c) 2007–2018 The scikit-learn developers. All rights reserved.
// Copyright 2018 Martin Krasser. All Rights Reserved.
// Modifications copyright (C) 2018 Uber Technologies, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// =============================================================================

#include "gaussian_process.h"

#include <cmath>
#include <iostream>

#include <Eigen/LU>

#include "LBFGS.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

namespace horovod {
namespace common {

// Returns true if any of the elements in the vectors is not a number.
bool isnan(const VectorXd& x) {
  for (int i = 0; i < x.size(); ++i) {
    if (std::isnan(x[i])) {
      return true;
    }
  }
  return false;
}

GaussianProcessRegressor::GaussianProcessRegressor(double alpha) : alpha_(alpha) {}

void GaussianProcessRegressor::Fit(MatrixXd* x_train, MatrixXd* y_train) {
  // Cache the last used training inputs and outputs for later prediction
  x_train_ = x_train;
  y_train_ = y_train;

  // This function will apply the natural logarithm element-wise to a matrix
  auto ln = [](double x) {
    return std::log(x);
  };

  // f(x): the objective function to be minimized by our optimizer.
  // Computes the negative log-likelihood for training data x_train and y_train and given noise level.
  double a2 = alpha_ * alpha_;
  double d3 = 0.5 * x_train_->rows() * std::log(2 * M_PI);
  auto f = [&, a2, d3](const VectorXd& x) {
    int64_t m = x_train_->rows();
    MatrixXd k = Kernel(*x_train_, *x_train_, x[0], x[1]) + (a2 * MatrixXd::Identity(m, m));
    MatrixXd k_inv = k.inverse();

    // Compute determinant via Cholesky decomposition
    MatrixXd l = k.llt().matrixL().toDenseMatrix();
    double d1 = l.diagonal().unaryExpr(ln).sum();
    MatrixXd d2 = 0.5 * (y_train_->transpose() * (k_inv * (*y_train_)));
    MatrixXd cov = d2.array() + (d1 + d3);

    return cov(0, 0);
  };

  // We wish to minimize the negative log-likelihood of f(x) above by evaluating it at a given point, then
  // empirically approximating the derivative to follow the gradient downwards.
  double f_min = std::numeric_limits<double>::max();
  VectorXd x_min;
  auto nll_fn = [&](const VectorXd& x, VectorXd& grad) {
    // f(x) computed at the current point x
    double fx = f(x);

    // Update the best value observed so far, if x is a valid point
    if (!isnan(x) && fx < f_min) {
      f_min = fx;
      x_min = x;
    }

    // Update the gradient vector in place and return the f(x) value computed
    ApproxFPrime(x, f, fx, grad);
    return fx;
  };

  // Use the L-BFGS method for minimizing the objective, limit our search to a set number of iterations
  // if convergence has not be reached within the threshold (epsilon).
  LBFGSpp::LBFGSParam<double> param;
  param.epsilon = 1e-5;
  param.max_iterations = 100;
  LBFGSpp::LBFGSSolver<double> solver(param);

  // Find the kernel parameter values for length_ and sigma_f_ that maximize the likelihood of the training data.
  VectorXd x = VectorXd::Ones(2);
  double fx;
  solver.minimize(nll_fn, x, fx);

  // If the returned value is NaN, then we short-circuit the optimizer by returning the cached best values found
  if (!isnan(x)) {
    length_ = x[0];
    sigma_f_ = x[1];
  } else {
    length_ = x_min[0];
    sigma_f_ = x_min[1];
  }
}

void GaussianProcessRegressor::Predict(const MatrixXd& x, VectorXd& mu, VectorXd* sigma) const {
  MatrixXd cov;
  PosteriorPrediction(x, *x_train_, *y_train_, mu, cov, length_, sigma_f_, alpha_);

  // Only compute standard deviation if it was requested
  if (sigma != nullptr) {
    // Extract the standard deviation from the covariance matrix
    auto sqrt = [](double x) {
      return std::sqrt(x);
    };
    *sigma = cov.diagonal().unaryExpr(sqrt);
  }
}

void GaussianProcessRegressor::PosteriorPrediction(
    const MatrixXd& x_s, const MatrixXd& x_train, const MatrixXd& y_train, VectorXd& mu_s, MatrixXd& cov_s,
    double l, double sigma_f, double sigma_y) const {
  // With m training data and n new input data. sy2 is the noise term in the diagonal of k. It is set to 0 if
  // observations are noisy.
  int64_t n = x_s.rows();
  int64_t m = x_train.rows();
  double sy2 = sigma_y * sigma_y;

  // The posterior predictive distribution is Gaussian with mean mu_s and covariance cov_s. By definition of the
  // Gaussian Process, the joint distribution of observed data x_train and predictions y_train is distributed
  // normally with mean 0 and standard deviation [[k, k_s], [k_s^T, k_ss]].
  MatrixXd k = Kernel(x_train, x_train, l, sigma_f) + (sy2 * MatrixXd::Identity(m, m));
  MatrixXd k_s = Kernel(x_train, x_s, l, sigma_f);
  MatrixXd k_ss = Kernel(x_s, x_s, l, sigma_f) + (1e-8 * MatrixXd::Identity(n, n));
  MatrixXd k_inv = k.inverse();

  // Compute sufficient statistics of the posterior predictive distribution: mean and covariance.
  mu_s = (k_s.transpose() * k_inv) * y_train;
  cov_s = k_ss - (k_s.transpose() * k_inv) * k_s;
}

void GaussianProcessRegressor::ApproxFPrime(const VectorXd& x, const std::function<double(const VectorXd&)>& f,
                                            double f0, VectorXd& grad, double epsilon) {
  VectorXd ei = VectorXd::Zero(x.size());
  for (int k = 0; k < x.size(); ++k) {
    ei[k] = 1.0;
    VectorXd d = epsilon * ei;
    grad[k] = (f(x + d) - f0) / d[k];
    ei[k] = 0.0;
  }
}

MatrixXd GaussianProcessRegressor::Kernel(const MatrixXd& x1, const MatrixXd& x2,
                                          double l, double sigma_f) const {
  // Squared Exponential Kernel, also known as the Gaussian or RBF Kernel.
  auto x1_vec = x1.cwiseProduct(x1).rowwise().sum();
  auto x2_vec = x2.cwiseProduct(x2).rowwise().sum();
  auto x1_x2 = x1_vec.replicate(1, x2_vec.size()).rowwise() + x2_vec.transpose();

  auto& dot = x1 * x2.transpose();
  auto sqdist = x1_x2 - (dot.array() * 2).matrix();

  // The length parameter l controls the smoothness of the function and sigma_f the vertical variation. We use
  // the same l for all input dimensions (isotropic kernel).
  double sigma_f2 = sigma_f * sigma_f;
  double l2 = l * l;
  auto op = [sigma_f2, l2](double x) {
    return sigma_f2 * std::exp(-0.5 / l2 * x);
  };

  return sqdist.unaryExpr(op);
}

} // namespace common
} // namespace horovod


// *****************************************************************************
//
// Copyright (c) 2015, Southwest Research Institute® (SwRI®)
// 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 Southwest Research Institute® (SwRI®) 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 <COPYRIGHT HOLDER> 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 <swri_geometry_util/cubic_spline.h>

#include <Eigen/Dense>

namespace swri_geometry_util
{

  bool CubicSplineInterpolation(
    const std::vector<cv::Vec2d>& points,
    double delta,
    std::vector<std::vector<cv::Vec2d> >& splines)
  {
    if (delta <= 0)
    {
      return false;
    }

    size_t num_points = points.size();

    splines.clear();
    splines.resize(num_points - 1);

    // Accumulated distance along linear path.
    std::vector<double> s = std::vector<double>(num_points);

    // Distance between consecutive points on path.
    std::vector<double> ds = std::vector<double>(num_points);

    s[0] = 0;
    for (size_t i = 1; i < num_points; i++)
    {
      double dx = points[i][0] - points[i - 1][0];
      double dy = points[i][1] - points[i - 1][1];

      ds[i - 1] = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
      s[i] = s[i - 1] + ds[i - 1];
    }

    // Normalize s to be [0:1]
    double totalDistance = s[num_points - 1];
    for (size_t i = 0; i < num_points; i++)
    {
      s[i] /= totalDistance;
    }

    Eigen::MatrixX2d u(num_points, 2);
    Eigen::MatrixXd A(num_points, num_points);
    Eigen::MatrixX2d z(num_points, 2);

    // Set up the z0 equation.
    u(0, 0) = 0;
    u(0, 1) = 0;
    A(0, 0) = 1;
    for (size_t i = 1; i < num_points; i++)
    {
      A(0, i) = 0;
    }

    // Set up the z1 to zn-1 equations.
    for (size_t i = 1; i < num_points - 1; i++)
    {
      double hi = s[i + 1] - s[i];
      double himl = s[i] - s[i - 1];

      u(i, 0) = 6.0 * ((points[i + 1][0] - points[i][0]) / hi - (points[i][0]
          - points[i - 1][0]) / himl);
      u(i, 1) = 6.0 * ((points[i + 1][1] - points[i][1]) / hi - (points[i][1]
          - points[i - 1][1]) / himl);

      for (size_t j = 0; j < i - 1; j++)
      {
        A(i, j) = 0;
      }
      A(i, i - 1) = himl;
      A(i, i) = 2 * (himl + hi);
      A(i, i + 1) = hi;
      for (size_t j = i + 2; j < num_points; j++)
      {
        A(i, j) = 0;
      }
    }

    // Set up the zn equation
    u(num_points - 1, 0) = 0;
    u(num_points - 1, 1) = 0;
    for (size_t j = 0; j < num_points - 1; j++)
    {
      A(num_points - 1, j) = 0;
    }
    A(num_points - 1, num_points - 1) = 1;

    // Solve Az = u
    z = A.colPivHouseholderQr().solve(u);

    for (size_t i = 0; i < num_points - 1; i++)
    {
      std::vector<cv::Vec2d>& spline = splines[i];

      spline.push_back(points[i]);

      double hi = s[i + 1] - s[i];
      double act_dist = ds[i];
      int32_t num_to_add = static_cast<int32_t> (act_dist / delta);
      double normalizedDelta = hi / (num_to_add + 1);

      // Add the additional points between waypoints
      for (int32_t j = 1; j <= num_to_add; j++)
      {
        // Calculate the (x,y) value of the interpolated point
        double si = s[i] + j * normalizedDelta;
        double xx = (z(i + 1, 0) * pow(si - s[i], 3) + z(i, 0) * pow(s[i + 1]
            - si, 3)) / (6.0 * hi) + (points[i + 1][0] / hi - hi * z(i + 1, 0)
            / 6.0) * (si - s[i]) + (points[i][0] / hi - hi * z(i, 0) / 6.0)
            * (s[i + 1] - si);
        double yy = (z(i + 1, 1) * pow(si - s[i], 3) + z(i, 1) * pow(s[i + 1]
            - si, 3)) / (6.0 * hi) + (points[i + 1][1] / hi - hi * z(i + 1, 1)
            / 6.0) * (si - s[i]) + (points[i][1] / hi - hi * z(i, 1) / 6.0)
            * (s[i + 1] - si);

        // Add the point to the interpolated array
        spline.push_back(cv::Vec2d(xx, yy));
      }

      spline.push_back(points[i + 1]);
    }

    return true;
  }

  bool CubicSplineInterpolation(
    const std::vector<tf::Vector3>& points,
    double delta,
    std::vector<std::vector<tf::Vector3> >& splines)
  {
    std::vector<cv::Vec2d> cv_points(points.size());
    for (size_t i = 0; i < points.size(); i++)
    {
      cv_points[i] = cv::Vec2d(points[i].x(), points[i].y());
    }

    std::vector<std::vector<cv::Vec2d> > cv_splines;
    bool result = CubicSplineInterpolation(cv_points, delta, cv_splines);

    splines.resize(cv_splines.size());
    for (size_t i = 0; i < cv_splines.size(); i++)
    {
      splines[i].resize(cv_splines[i].size());
      for (size_t j = 0; j < cv_splines[i].size(); j++)
      {
        splines[i][j] = tf::Vector3(cv_splines[i][j][0], cv_splines[i][j][1], 0);
      }
    }

    return result;
  }
}


#ifndef STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{modified\_bessel\_first\_kind}(v, z) =
   \begin{cases}
     I_v(z) & \mbox{if } -\infty\leq z \leq \infty \\[6pt]
     \textrm{error} & \mbox{if } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{modified\_bessel\_first\_kind}(v, z)}{\partial z} =
   \begin{cases}
     \frac{\partial\, I_v(z)}{\partial z} & \mbox{if } -\infty\leq z\leq \infty
 \\[6pt] \textrm{error} & \mbox{if } z = \textrm{NaN} \end{cases} \f]

   \f[
     {I_v}(z) = \left(\frac{1}{2}z\right)^v\sum_{k=0}^\infty
 \frac{\left(\frac{1}{4}z^2\right)^k}{k!\Gamma(v+k+1)} \f]

   \f[
   \frac{\partial \, I_v(z)}{\partial z} = I_{v-1}(z)-\frac{v}{z}I_v(z)
   \f]
 *
 */
template <typename T2, require_arithmetic_t<T2>* = nullptr>
inline T2 modified_bessel_first_kind(int v, const T2 z) {
  check_not_nan("modified_bessel_first_kind", "z", z);

  return boost::math::cyl_bessel_i(v, z);
}

/**
 * This function exists because when z is of type integer,
 * cyl_bessel_i(v, z) returns an integer. This
 * results in overflow when the function value is large.
 */
inline double modified_bessel_first_kind(int v, int z) {
  check_not_nan("modified_bessel_first_kind", "z", z);

  return boost::math::cyl_bessel_i(v, static_cast<double>(z));
}

/**
 * Enables the vectorised application of the modified_bessel_first_kind
 * function, when the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return modified_bessel_first_kind function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto modified_bessel_first_kind(const T1& a, const T2& b) {
  return apply_scalar_binary(a, b, [&](const auto& c, const auto& d) {
    return modified_bessel_first_kind(c, d);
  });
}

}  // namespace math
}  // namespace stan

#endif


/**
 * \Description:
 * this node is used to estimate the absoulte location(x,y) of the robot using Trilteration.
 * It subscribe to:
 * - /beacon_distances
 * It publishes to:
 * - /pose
 *
 * ALI&SERRANO_ECN_M1_2017
 */

//Cpp
#include <sstream>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
using Eigen::MatrixXd;

//ROS
#include "ros/ros.h"

// Include here the ".h" files corresponding to the topic type you use.
#include <nav_msgs/Odometry.h>
#include "tf/transform_datatypes.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include <std_msgs/Float64MultiArray.h>

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose2D.h>

//#include <geometry_msgs/PoseWithCovariance.h>
//#include <std_msgs/Float32.h>

// global variables...
std_msgs::Float64MultiArray dist;
bool  is_dist_available=false;

void msurmnt_Callback ( std_msgs::Float64MultiArray  dist_msg) {
    is_dist_available= true;
    dist = dist_msg;
}

int circle_circle_intersection(double x0, double y0, double r0,
                               double x1, double y1, double r1,
                               double &xi, double &yi,
                               double &xi_prime, double &yi_prime)
{
    double a, dx, dy, d, h, rx, ry;
    double x2, y2;

    // dx and dy are the vertical and horizontal distances between the circle centers.
    dx = x1 - x0;
    dy = y1 - y0;

    // Determine the straight-line distance between the centers.
    //d = sqrt((dy*dy) + (dx*dx));
    d = hypot(dx,dy); // Suggested by Keith Briggs

    // Check for solvability.
    if (d > (r0 + r1)) /* no solution. circles do not intersect. */
        return 0;
    if (d < fabs(r0 - r1)) /* no solution. one circle is contained in the other */
        return 0;

    /* 'point 2' is the point where the line through the circle
   * intersection points crosses the line between the circle centers.
   */

    /* Determine the distance from point 0 to point 2. */
    a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;

    /* Determine the coordinates of point 2. */
    x2 = x0 + (dx * a/d);
    y2 = y0 + (dy * a/d);

    // Determine the distance from point 2 to either of the intersection points.
    h = sqrt((r0*r0) - (a*a));

    // Now determine the offsets of the intersection points from point 2.
    rx = -dy * (h/d);
    ry = dx * (h/d);

    /* Determine the absolute intersection points. */
    xi = x2 + rx;
    xi_prime = x2 - rx;
    yi = y2 + ry;
    yi_prime = y2 - ry;

    return 1;
}

void select_point(double p1x, double p1y, double p2x, double p2y, double bx, double by, double &px, double &py, double d){
    double t1= fabs (sqrt( (bx- p1x)*(bx-p1x) + (by-p1y)*(by-p1y)) );
    double t2= fabs (sqrt( (bx-p2x)*(bx-p2x) + (by-p2y)*(by-p2y)) );
    if ( fabs(d-t1) < fabs(d-t2) )
       { px= p1x;  py=p1y; }
    else
       { px= p2x;  py=p2y; }
}

void incircle(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double &xc, double &yc){
    double a1 =fabs (sqrt( (p3x-p2x)*(p3x-p2x) + (p3y-p2y)*(p3y-p2y)) );
    double a2 =fabs (sqrt( (p1x-p3x)*(p1x-p3x) + (p1y-p3y)*(p1y-p3y)) );
    double a3 =fabs (sqrt( (p1x-p2x)*(p1x-p2x) + (p1y-p2y)*(p1y-p2y)) );
    xc= ( a1*p1x + a2*p2x + a3*p3x )/(a1+a2+a3);
    yc= ( a1*p1y + a2*p2y + a3*p3y )/(a1+a2+a3);
}

int main (int argc, char** argv){

    //ROS Initialization
    ros::init(argc, argv, "ekf_node");

    // Define your node handles
    ros::NodeHandle nh_loc("~"), nh_glob;

    // Read the node parameters if any

    // Declare your node's subscriptions and service clients
    ros::Subscriber msurmnt_sub = nh_glob.subscribe<std_msgs::Float64MultiArray>("/beacon_distances",1, msurmnt_Callback) ;

    // Declare you publishers and service servers
    ros::Publisher  pose_pub = nh_glob.advertise<geometry_msgs::Pose2D>("/pose",1) ;

    // node initilization

    //geometry_msgs::Point B1, B2, B3;
    //B1.x=0;  B1.y=10; B2.x=10;  B2.y=0; B3.x=20; B3.y=20;

    //double x1=0, x2=0, x3=10, y1=5, y2=10, y3=10, x, y, d1, d2, d3;
    double d1, d2, d3, xc, yc;
    ros::Rate rate(10);

    while (ros::ok()){

        ros::spinOnce();
                if( ! is_dist_available ){
                    ROS_INFO("Waiting for odom/dist.") ;
                    rate.sleep() ;
                    continue ;
                }

        // measurement part:

        // Actual measurement  Y
        d1= dist.data[0];
        d2= dist.data[1];
        d3= dist.data[2];

        //double d1=5, d2=5, d3=1;
        double b1x=0, b1y=0, b2x=0, b2y=190, b3x=350, b3y=0;
        double ip12x1, ip12y1, ip12x2, ip12y2,   ip13x1, ip13y1, ip13x2, ip13y2,   ip23x1, ip23y1, ip23x2, ip23y2;

        int t12= circle_circle_intersection(b1x, b1y, d1, b2x, b2y, d2, ip12x1, ip12y1, ip12x2, ip12y2); //12
        int t13= circle_circle_intersection(b1x, b1y, d1, b3x, b3y, d3, ip13x1, ip13y1, ip13x2, ip13y2); //13
        int t23= circle_circle_intersection(b2x, b2y, d2, b3x, b3y, d3, ip23x1, ip23y1, ip23x2, ip23y2); //23

        if ( t12 && t13 && t23){
            //std::cout<< "point1: (" << ip1x << ", " << ip1y << ")" << ")"<< std::endl;
            //std::cout<< "point2: (" << ip2x << ", " << ip2y << ")" << ")"<< std::endl;
            double ip1x, ip1y, ip2x, ip2y, ip3x, ip3y;

            select_point(ip12x1, ip12y1, ip12x2, ip12y2, b3x, b3y, ip1x, ip1y, d3 );
            select_point(ip13x1, ip13y1, ip13x2, ip13y2, b2x, b2y, ip2x, ip2y, d2 );
            select_point(ip23x1, ip23y1, ip23x2, ip23y2, b1x, b1y, ip3x, ip3y, d1 );

            std::cout<< "point1: (" << ip1x << ", " << ip1y << ")" << ")"<< std::endl;
            std::cout<< "point2: (" << ip2x << ", " << ip2y << ")" << ")"<< std::endl;
            std::cout<< "point3: (" << ip3x << ", " << ip3y << ")" << ")"<< std::endl;

            incircle(ip1x, ip1y, ip2x, ip2y, ip3x, ip3y, xc, yc);
            std::cout<< "center: (" << xc << ", " << yc << ")" << ")"<< std::endl;

        }
        else
            std::cout<<"no common intersection"<<std::endl;

//        float c1= (d1*d1) - (x1*x1) - (y1*y1);
//        float c2= (d2*d2) - (x2*x2) - (y2*y2);
//        float c3= (d3*d3) - (x3*x3) - (y3*y3);

//        x= ( ((c1-c2)*(y3-y1)) - ((c1-c3)*(y2-y1)) ) / ( 2*( ( (x2-x1)*(y3-y1) ) - ((x3-x1)*(y2-y1)) ));
//        y= ( c1 - c2 - (2*x*(x2-x1)) ) / (2 * (y2-y1) );
        // measurement covariance Q_gamma
        //Q_gamma(0,0)= .1;  Q_gamma(1,1)= .1;  Q_gamma(2,2)= .1;

        // Estimated_covariance_est: Pk+1/k+1 = ( I - Kk*Ck ) * Pk+1/k
        //P= ( I - K*C ) * P;

        // publish the final pose ........................................................................................................
        geometry_msgs::Pose2D pose;
        pose.x = xc;
        pose.y = yc;
        pose.theta =  0;
        pose_pub.publish(pose);

        rate.sleep();
    }

}


#include "DiffWristKinematics.h"

#include <Eigen/Dense>

using Eigen::Matrix2f;
using Eigen::Vector2f;

gearpos_t::gearpos_t(const Vector2f& vec) : left(vec(0)), right(vec(1)) {}
gearpos_t::gearpos_t(float left, float right) : left(left), right(right) {}
Vector2f gearpos_t::vec() const {
	return Vector2f(left, right);
}

jointpos_t::jointpos_t(const Vector2f& vec) : pitch(vec(0)), roll(vec(1)) {}
jointpos_t::jointpos_t(float pitch, float roll) : pitch(pitch), roll(roll) {}
Vector2f jointpos_t::vec() const {
	return Vector2f(pitch, roll);
}

static Matrix2f createTransform() {
	Matrix2f ret;
	ret <<
		0.5, 0.5,
		-0.5, 0.5;
	return ret;
}

static const Matrix2f gearToJointPosTransform = createTransform();
static const Matrix2f jointToGearPosTransform = gearToJointPosTransform.inverse();

jointpos_t DiffWristKinematics::gearPosToJointPos(const gearpos_t& gearPos) const {
	Vector2f res = gearToJointPosTransform * gearPos.vec();
	return res;
}

gearpos_t DiffWristKinematics::jointPosToGearPos(const jointpos_t &jointPos) const {
	Vector2f res = jointToGearPosTransform * jointPos.vec();
	return res;
}

jointpos_t DiffWristKinematics::gearPowerToJointPower(const gearpos_t &gearPwr) const {
	Vector2f jointPwr = gearToJointPosTransform * gearPwr.vec();
	// compute infinity norm; i.e. component with max absolute value
	float maxVal = jointPwr.lpNorm<Eigen::Infinity>();
	// if any component is absolutely greater than 1, divide by maximum component to scale
	// everything to the interval [-1,1]
	if(maxVal > 1){
		jointPwr /= maxVal;
	}
	return jointPwr;
}

gearpos_t DiffWristKinematics::jointPowerToGearPower(const jointpos_t &jointPwr) const {
    Vector2f gearPwr = jointToGearPosTransform * jointPwr.vec();
	// compute infinity norm; i.e. component with max absolute value
	float maxVal = gearPwr.lpNorm<Eigen::Infinity>();
	// if any component is absolutely greater than 1, divide by maximum component to scale
	// everything to the interval [-1,1]
	if(maxVal > 1){
		gearPwr /= maxVal;
	}
	return gearPwr;
}


#define EIGEN_NO_DEBUG

#include "ccpca.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <cmath>
#include <iostream>
#include <limits>
#include <thread>

CCPCA::CCPCA(Eigen::Index const nComponents, bool const standardize)
    : cpca_(CPCA(nComponents, standardize)) {
  featContribs_.resize(0);
  concatMat_.resize(0, 0);
}

Eigen::MatrixXf
CCPCA::fitTransform(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                    bool const autoAlphaSelection, float const alpha,
                    float const varThresRatio, bool parallel,
                    unsigned int const nAlphas, float const maxLogAlpha,
                    bool const keepReports) {
  fit(K, R, autoAlphaSelection, alpha, varThresRatio, parallel, nAlphas,
      maxLogAlpha, keepReports);
  return cpca_.transform(concatMat_);
}

void CCPCA::fit(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                bool const autoAlphaSelection, float const alpha,
                float const varThresRatio, bool parallel,
                unsigned int const nAlphas, float const maxLogAlpha,
                bool const keepReports) {
  if (autoAlphaSelection) {
    fitWithBestAlpha(K, R, varThresRatio, parallel, nAlphas, maxLogAlpha,
                     keepReports);
  } else {
    fitWithManualAlpha(K, R, alpha);
  }
}

void CCPCA::fitWithBestAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                             float const varThresRatio, bool parallel,
                             unsigned int const nAlphas,
                             float const maxLogAlpha, bool const keepReports) {
  bestAlpha(K, R, varThresRatio, parallel, nAlphas, maxLogAlpha, keepReports);
  cpca_.updateComponents(bestAlpha_);
  featContribs_ = cpca_.getLoading(0);
}

void CCPCA::fitWithManualAlpha(Eigen::MatrixXf const &K,
                               Eigen::MatrixXf const &R, float const alpha) {
  Eigen::Index nSamplesK = K.rows();
  Eigen::Index nSamplesR = R.rows();

  if (K.cols() != R.cols()) {
    std::cerr << "# of rows of K and all matrix in R must be the same."
              << std::endl;
  }

  Eigen::MatrixXf concatMat_(nSamplesK + nSamplesR, K.cols());
  concatMat_ << K, R;

  cpca_.fit(concatMat_, R, alpha);
  featContribs_ = cpca_.getLoading(0);
}

Eigen::MatrixXf CCPCA::transform(Eigen::MatrixXf const &X) {
  return cpca_.transform(X);
}

float CCPCA::bestAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                       float const varThresRatio, bool parallel,
                       unsigned int const nAlphas, float const maxLogAlpha,
                       bool const keepReports) {
  Eigen::Index nSamplesK = K.rows();
  Eigen::Index nSamplesR = R.rows();

  if (K.cols() != R.cols()) {
    std::cerr << "# of rows of K and all matrix in R must be the same."
              << std::endl;
  }

  Eigen::MatrixXf concatMat_(nSamplesK + nSamplesR, K.cols());
  concatMat_ << K, R;

  cpca_.fit(concatMat_, R, 0.0f);

  Eigen::VectorXf bestProjK = cpca_.transform(K).col(0);
  Eigen::VectorXf bestProjR = cpca_.transform(R).col(0);

  bestAlpha_ = 0.0f;
  auto baseVarK = scaledVar(bestProjK, bestProjR).first;
  auto bestDiscrepancy =
      1.0f / std::max(float(histIntersect(bestProjK, bestProjR)),
                      std::numeric_limits<float>::min());

  reports_.clear();
  if (keepReports) {
    reports_.push_back(std::make_tuple(0.0, float(bestDiscrepancy), baseVarK,
                                       bestProjK, bestProjR,
                                       cpca_.getLoading(0)));
    if (parallel) {
      parallel = false;
      std::cout << "current version keepReports only support non-parallel "
                   "running. parallel is turned off."
                << std::endl;
    }
  }

  auto alphas = cpca_.logspace(-1, maxLogAlpha, nAlphas - 1);

  if (!parallel) {
    // non-parallel ver
    for (auto const &alpha : alphas) {
      cpca_.updateComponents(alpha);

      Eigen::VectorXf tmpProjK = cpca_.transform(K).col(0);
      Eigen::VectorXf tmpProjR = cpca_.transform(R).col(0);

      auto varK = scaledVar(tmpProjK, tmpProjR).first;
      auto discrepancy =
          1.0f / std::max(float(histIntersect(tmpProjK, tmpProjR)),
                          std::numeric_limits<float>::min());

      if (varK >= baseVarK * varThresRatio && discrepancy > bestDiscrepancy) {
        bestDiscrepancy = discrepancy;
        bestAlpha_ = alpha;
      }

      if (keepReports) {
        reports_.push_back(std::make_tuple(alpha, float(bestDiscrepancy), varK,
                                           tmpProjK, tmpProjR,
                                           cpca_.getLoading(0)));
      }
    }
  } else {
    // parallel version
    auto numWorkers = std::max(std::thread::hardware_concurrency(),
                               1u); // obtain max thread num
    std::vector<std::thread> worker;
    auto n = alphas.size();
    std::vector<std::pair<float, float>> varAndDiscpSet(n, {0.0f, 0.0f});

    for (size_t i = 0; i < numWorkers; ++i) {
      worker.emplace_back(
          [&](size_t id) {
            auto r0 = n / numWorkers * id + std::min(n % numWorkers, id);
            auto r1 =
                n / numWorkers * (id + 1) + std::min(n % numWorkers, id + 1);

            for (auto j = r0; j < r1; ++j) {
              Eigen::VectorXf component;
              if (numWorkers > 4) {
                // less mtx locks but more steps
                mtx.lock();
                auto alpha = alphas[j];
                Eigen::MatrixXf diffCov = cpca_.getDiffCov(alpha);
                mtx.unlock();

                Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es(diffCov);
                component = es.eigenvectors().rightCols(1);
              } else {
                // less steps but more mtx locks
                mtx.lock();
                auto alpha = alphas[j];
                cpca_.updateComponents(alpha);
                component = cpca_.getComponent(0);
                mtx.unlock();
              }

              Eigen::VectorXf tmpProjK = K * component;
              Eigen::VectorXf tmpProjR = R * component;

              auto varK = scaledVar(tmpProjK, tmpProjR).first;
              auto discrepancy =
                  1.0f / std::max(float(histIntersect(tmpProjK, tmpProjR)),
                                  std::numeric_limits<float>::min());
              mtx.lock();
              varAndDiscpSet[j] = {varK, discrepancy};
              mtx.unlock();
            }
          },
          i);
    }
    for (auto &w : worker)
      w.join();

    for (size_t i = 0; i < n; ++i) {
      auto var = varAndDiscpSet[i].first;
      auto discrepancy = varAndDiscpSet[i].second;
      if (var >= baseVarK * varThresRatio && discrepancy > bestDiscrepancy) {
        bestDiscrepancy = discrepancy;
        bestAlpha_ = alphas[i];
      }
    }
  }

  return bestAlpha_;
}

std::pair<float, float> CCPCA::scaledVar(Eigen::VectorXf const &a,
                                         Eigen::VectorXf const &b) {
  float minVal = std::min(a.minCoeff(), b.minCoeff());
  float maxVal = std::max(a.maxCoeff(), b.maxCoeff());
  float range = std::max(maxVal - minVal, std::numeric_limits<float>::min());

  float varA = ((a.array() - a.mean()) / range).array().square().mean();
  float varB = ((b.array() - b.mean()) / range).array().square().mean();

  return {varA, varB};
}

float CCPCA::binWidthScott(Eigen::VectorXf const &vals) {
  auto n = vals.size();
  float sd = 0.0f;
  if (n > 1) {
    sd = std::sqrt((vals.array() - vals.mean()).square().sum() / float(n - 1));
  }
  float denom = std::max(std::pow(float(n), 1.0f / 3.0f),
                         std::numeric_limits<float>::min());
  float binWidth = 3.5f * sd / denom;

  return binWidth;
}

int CCPCA::histIntersect(Eigen::VectorXf const &a, Eigen::VectorXf const &b) {
  float minVal = std::min(a.minCoeff(), b.minCoeff());
  float maxVal = std::max(a.maxCoeff(), b.maxCoeff());
  float range = std::max(maxVal - minVal, std::numeric_limits<float>::min());
  auto nA = a.size();
  auto nB = b.size();

  Eigen::VectorXf tmpA = (a.array() - minVal) / range;
  Eigen::VectorXf tmpB = (b.array() - minVal) / range;
  Eigen::VectorXf tmpAB(nA + nB);
  tmpAB << tmpA, tmpB;

  float binW =
      std::max(binWidthScott(tmpAB), std::numeric_limits<float>::min());
  unsigned int nBins = static_cast<unsigned int>(1.0f / binW) + 1;

  std::vector<int> countsA(nBins, 0);
  std::vector<int> countsB(nBins, 0);

  for (Eigen::Index i = 0; i < nA; ++i) {
    int binIndex = int(tmpA(i) / binW);
    countsA[binIndex]++;
  }
  for (Eigen::Index i = 0; i < nB; ++i) {
    int binIndex = int(tmpB(i) / binW);
    countsB[binIndex]++;
  }

  int histIntersect = 0;
  for (unsigned int i = 0; i < nBins; ++i) {
    histIntersect += std::min(countsA[i], countsB[i]);
  }

  return histIntersect;
}


#pragma once

#include <boost/multiprecision/gmp.hpp>
#include <math.h>
#include <time.h>

#include "FiniteFields.hpp"
#include "FastFourierTransform.hpp"
#include "NumberTheoreticTransform.hpp"

namespace ligero {

class SecretSharingNTT {
	public:
		/* Constructors allowing some flexibility in the choice of domains */
		~SecretSharingNTT() {
			delete _smallDomain;
			delete _largeDomain;
		}
		SecretSharingNTT(size_t modulusIdx, size_t lSecretLength, size_t kDegree, size_t nShares, void (*computeSmall) (uint64_t*,uint64_t const*), void (*computeLarge) (uint64_t*,uint64_t const*)) : modulusIdx_(modulusIdx), lSecretLength_(lSecretLength), kDegree_(kDegree), nNumberShares_(nShares) {
			this->dCompositeDomainSize_ = nShares;

		    _smallDomain = new NTT(modulusIdx, this->kDegree_, computeSmall);
		    _largeDomain = new NTT(modulusIdx, this->nNumberShares_, computeLarge);
		};

		int share(uint64_t *secret);
		int reconstruct(uint64_t *eval, bool exapnded = false);

		int padMany(uint64_t *secrets, size_t lrows);
		int shareMany(uint64_t *secrets, size_t lrows);
		int reconstructMany(uint64_t *secrets, size_t lrows);

		bool degreeTest(uint64_t *eval);
		bool zeroTest(uint64_t *eval, bool expanded = false);
		bool zeroSumTest(uint64_t *eval);
		bool zeroSumTestSigmaProtocol(uint64_t *eval, std::vector<uint64_t> beta);

	protected:
		int padSecret(uint64_t *secret);
		int padPolynominal(uint64_t *secret);
		int padShares(uint64_t *secret);
		int padIntrablocRandomness(uint64_t *secret);

		size_t  modulusIdx_;
		size_t	lSecretLength_;
		size_t	kDegree_;
		size_t	nNumberShares_;
		size_t	dCompositeDomainSize_;

		NTT 	*_smallDomain;
		NTT		*_largeDomain;
};

template<typename FieldT>
class SecretSharingInterface: SecretSharingNTT {
	public:
		/* Constructors allowing some flexibility in the choice of domains */
		SecretSharingInterface(size_t modulusIdx, size_t lSecretLength, size_t kDegree, size_t nShares, void (*computeSmall) (uint64_t*,uint64_t const*), void (*computeLarge) (uint64_t*,uint64_t const*)) : SecretSharingNTT(modulusIdx, lSecretLength, kDegree, nShares, computeSmall, computeLarge) {
			this->dCompositeDomainSize_ = nShares;
		};

		using SecretSharingNTT::degreeTest;
		using SecretSharingNTT::zeroTest;
		using SecretSharingNTT::zeroSumTest;
		using SecretSharingNTT::share;
		using SecretSharingNTT::reconstruct;

		int share(FieldT *secret);
		int reconstruct(FieldT *eval, bool expanded = false);

		int padMany(FieldT *secrets, size_t lrows);
		int padIntrablocRandomness(FieldT *secret);
		int shareMany(FieldT *secrets, size_t lrows);
		int reconstructMany(FieldT *secrets, size_t lrows);

		bool degreeTest(FieldT *eval);
		bool zeroTest(FieldT *eval, bool expanded = false);
		bool zeroSumTestSigmaProtocol(FieldT *eval, std::vector<uint64_t> beta);
		bool zeroSumTest(FieldT *eval);
		FieldT sumReconstruct(FieldT *eval);

	protected:
		int padSecret(FieldT *secret);
		int padPolynominal(FieldT *secret);
		int padShares(FieldT *secret);

		// adapter
		std::vector<uint64_t> demote(FieldT *eval, size_t length);
		void promote(uint64_t *eval, size_t length, FieldT *dest);
};

// Implementation
// =============================================================================================

// Secret Sharing Interface
template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padSecret(FieldT *secret) {
	FieldT::randomVector(secret + this->lSecretLength_, this->kDegree_ - this->lSecretLength_, true);
	return 0;
}

template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padIntrablocRandomness(FieldT *secret) {
	for (size_t i = this->lSecretLength_; i < kDegree_ ;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padPolynominal(FieldT *secret) {
	for (size_t i = this->kDegree_; i < nNumberShares_;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padShares(FieldT *secret) {
	for (size_t i = this->nNumberShares_; i < dCompositeDomainSize_ ;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::padMany(FieldT *secret, size_t lrows) {

	// Add randomness for padding
	for (size_t i = 0; i < lrows; i++) {
		this->padSecret(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::share(FieldT *secret) {

	std::vector<uint64_t> secret64 = demote(secret, this->nNumberShares_);
	this->share(&secret64[0]);
	promote(&secret64[0], this->nNumberShares_, secret);

	return 0;
}

// this method is used when the resulting degree of the polynomial is larger than the domain space of the secret
template <typename FieldT>
int SecretSharingInterface<FieldT>::reconstruct(FieldT *eval, bool expandedDegree) {
	std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);
	this->reconstruct(&eval64[0], expandedDegree);
	promote(&eval64[0], this->kDegree_, eval);

	return 0;
}

template <typename FieldT>
std::vector<uint64_t> SecretSharingInterface<FieldT>::demote(FieldT *eval, size_t length) {
	std::vector<uint64_t> temp(length);
	for (size_t idx = 0; idx < length; idx++) {
		temp[idx] = static_cast<uint64_t>(eval[idx].getValue());
	}

	return temp;
}

template <typename FieldT>
void SecretSharingInterface<FieldT>::promote(uint64_t *eval, size_t length, FieldT *dest) {
	for (size_t idx = 0; idx < length; idx++) {
		dest[idx] = FieldT(eval[idx]);
	}
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::degreeTest(FieldT *eval) {

	std::vector<uint64_t> eval64 = demote(eval,this->nNumberShares_);
	return this->degreeTest(&eval64[0]);
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroTest(FieldT *eval, bool largerDegree) {

	std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
	reconstruct(&localCopy[0], largerDegree);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (!(localCopy[i] == FieldT(0))) return false;
	}

	return true;
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroSumTest(FieldT *eval) {

	std::vector<FieldT> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	FieldT sum = FieldT(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	if (sum == FieldT(0)) return true;
	else return false;
}

template <typename FieldT>
FieldT SecretSharingInterface<FieldT>::sumReconstruct(FieldT *eval) {

	std::vector<FieldT> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	FieldT sum = FieldT(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	return sum;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::shareMany(FieldT *secret, size_t lrows) {

	// std::cout << "Performing FFTs: " << lrows << std::endl;
	for (size_t i = 0; i < lrows; i++) {
		share(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::reconstructMany(FieldT *secret, size_t lrows) {

	for (size_t i = 0; i < lrows; i++) {
		reconstruct(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroSumTestSigmaProtocol(FieldT *eval, std::vector<uint64_t> beta) {
	std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);

	return true;
}

// Secret Sharing NTT

int SecretSharingNTT::padSecret(uint64_t *secret) {

	// uint64_t::randomVector(secret + this->lSecretLength_, this->kDegree_ - this->lSecretLength_, true);
	return 0;
}

int SecretSharingNTT::padIntrablocRandomness(uint64_t *secret) {

	for (size_t i = this->lSecretLength_; i < kDegree_ ;i++) {
		secret[i] = uint64_t(0);
	}

	return 0;
}

int SecretSharingNTT::padPolynominal(uint64_t *secret) {

	for (size_t i = this->kDegree_; i < nNumberShares_;i++) {
		secret[i] = uint64_t(0);
	}

	return 0;
}

int SecretSharingNTT::padShares(uint64_t *secret) {

	for (size_t i = this->nNumberShares_; i < dCompositeDomainSize_ ;i++) {
		secret[i] = uint64_t(0);
	}

	return 0;
}

int SecretSharingNTT::padMany(uint64_t *secret, size_t lrows) {

	// Add randomness for padding
	for (size_t i = 0; i < lrows; i++) {
		this->padSecret(secret + i * this->nNumberShares_);
	}

	return 0;
}

int SecretSharingNTT::share(uint64_t *data) {

	// Interpolate Poly from Secret
	_smallDomain->inv_ntt(data);

	this->padPolynominal(data);

	// Evaluate Publicly Shared Data from Poly
	_largeDomain->ntt(data);

	return 0;
}

// this method is used when the resulting degree of the polynomial is larger than the domain space of the secret
int SecretSharingNTT::reconstruct(uint64_t *eval, bool expandedDegree) {
	size_t degree = kDegree_;

	// Interpolate Poly from Publicly Shared Data
	_largeDomain->inv_ntt(eval);

	// if (expandedDegree) degree = kDegree_*2;
	// Because we evaluate on roots of unity, we can use coefs[i] + coefs[i+k]
	// on k coefficients instead of a 2k evaluation
	if (expandedDegree) {
		for (size_t i = 0; i < kDegree_; i++) {
				if (eval[i+kDegree_] > eval[i]) {
					eval[i] = eval[i] - eval[i + kDegree_] + params<uint64_t>::P[this->modulusIdx_];
				} else {
					eval[i] = eval[i] - eval[i + kDegree_];
				}
			}
	}

	// Evaluate Poly Into Secret
	_smallDomain->ntt(eval);

	// rebuild accounting for the stride implied by the larger degree
	size_t stride = degree/this->kDegree_;
	for (size_t col = 0; col < this->lSecretLength_; col++) {
		eval[col] = eval[col*stride];
	}

	return 0;
}

bool SecretSharingNTT::degreeTest(uint64_t *eval) {

	// Interpolate Poly from Publicly Shared Data
	std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
	_largeDomain->inv_ntt(&localCopy[0]);

	for (size_t i = kDegree_; i < nNumberShares_; i++) {
		if (!(localCopy[i] == uint64_t(0))) {
			DBG("val:"<< localCopy[i] << ",degree>" << i);
			return false;
		}
	}

	return true;
}

bool SecretSharingNTT::zeroTest(uint64_t *eval, bool largerDegree) {

	std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
	reconstruct(&localCopy[0], largerDegree);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (!(localCopy[i] == uint64_t(0))) return false;
	}

	return true;
}

bool SecretSharingNTT::zeroSumTest(uint64_t *eval) {

	std::vector<uint64_t> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	uint64_t sum = uint64_t(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	if (sum == uint64_t(0)) return true;
	else return false;
}

bool SecretSharingNTT::zeroSumTestSigmaProtocol(uint64_t *eval, std::vector<uint64_t> beta) {

	std::vector<uint64_t> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (localCopy[i] != beta[i]) return false;
	}

	return true;
}

int SecretSharingNTT::shareMany(uint64_t *secret, size_t lrows) {

	// std::cout << "Performing FFTs: " << lrows << std::endl;
	for (size_t i = 0; i < lrows; i++) {
		share(secret + i * this->nNumberShares_);
	}

	return 0;
}

int SecretSharingNTT::reconstructMany(uint64_t *secret, size_t lrows) {

	for (size_t i = 0; i < lrows; i++) {
		reconstruct(secret + i * this->nNumberShares_);
	}

	return 0;
}

}


#include "CEGO/CEGO.hpp"
#include <Eigen/Dense>
#if defined(PYBIND11)
#include <pybind11/embed.h>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
namespace py = pybind11;
#include <atomic>

std::atomic_size_t Ncalls(0);

inline bool ValidNumber(double x)
{
    // Idea from http://www.johndcook.com/IEEE_exceptions_in_cpp.html
    return (x <= DBL_MAX && x >= -DBL_MAX);
};

enum fitting_options{ FIT_P, FIT_RHOV, FIT_RHOL};

class RatPolyAncillary {
public:
    double m_Tt, m_Tc, m_pc, m_Dc;
    std::size_t m_Nnum, m_Nden;
    Eigen::ArrayXd m_LHS, m_THETA, m_T;
    std::string m_name;
    fitting_options to_fit = FIT_RHOV;
    RatPolyAncillary(const std::string &name, const std::size_t Nnum, const std::size_t Nden) : m_Nnum(Nnum), m_Nden(Nden), m_name(name)
    {
        py::module ct = py::module::import("ctREFPROP.ctREFPROP"); // Import cytpes wrapper of REFPROP dll
        std::string RPPREFIX(getenv("RPPREFIX"));
        auto R = ct.attr("REFPROPFunctionLibrary")(RPPREFIX + "/REFPRP64.DLL");
        auto version = R.attr("RPVersion")().cast<std::string>();
        auto setup_info = R.attr("SETUPdll")(1, name+".FLD", "HMX.BNC","DEF");
        if (setup_info.attr("ierr").cast<int>() != 0) {
            throw std::invalid_argument( setup_info.attr("herr").cast<std::string>() );
        }
        auto info = R.attr("INFOdll")(1);
        m_pc = info.attr("Pc").cast<double>();
        m_Tc = info.attr("Tc").cast<double>();
        m_Tt = info.attr("Ttrp").cast<double>();
        m_Dc = info.attr("Dc").cast<double>();

        std::size_t N = 300;
        m_LHS.resize(N);
        m_THETA.resize(N);
        m_T.resize(N);
        std::vector<double> z(1,1);
        Eigen::ArrayXd Tvec = Eigen::ArrayXd::LinSpaced(N, m_Tc*0.999, 0.5*m_Tt);
        Eigen::Index j = 0;

        for (auto i = 0; i < Tvec.size(); ++i){
            auto o = R.attr("TQFLSHdll")(Tvec[i], 1, &z, 0);
            int ierr = o.attr("ierr").cast<int>();
            if (ierr <= 100){
                //std::cout << Tvec(i) << " " << o << std::endl;
                if (to_fit == FIT_P){
                    double p = o.attr("P").cast<double>();
                    if (!ValidNumber(p)){ continue; }
                    m_LHS(j) = log(p/m_pc)*Tvec(i)/m_Tc;
                }
                else if (to_fit == FIT_RHOL) {
                    double DL = o.attr("Dl").cast<double>();
                    if (!ValidNumber(DL)) { continue; }
                    m_LHS(j) = DL/m_Dc;
                }
                else if (to_fit == FIT_RHOV) {
                    double DV = o.attr("Dv").cast<double>();
                    if (!ValidNumber(DV)) { continue; }
                    m_LHS(j) = log(DV / m_Dc);//*Tvec(i)/m_Tc;
                }
                else {
                    throw std::invalid_argument("Argument for what to fit is invalid");
                }
                m_THETA(j) = 1-(Tvec(i)/m_Tc);
                m_T(j) = Tvec(i);
                j++;
            }
        }
        m_LHS.conservativeResize(j-1);
        m_THETA.conservativeResize(j-1);
        m_T.conservativeResize(j - 1);
    }
    void plot_curve(){
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(m_THETA, m_LHS);
        plt.attr("show")();
    }
    void plot_deviation(const Eigen::ArrayXd &c) {

        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        Eigen::ArrayXd T = m_Tc*(1+m_THETA), deviation;
        std::string ylabel;
        if (to_fit == FIT_P){
            Eigen::ArrayXd pfit = eval_RHS(m_THETA, c).exp();
            Eigen::ArrayXd peos = m_LHS.exp();
            deviation = (pfit/peos-1)*100;
            ylabel = "Pressure deviation (%)";
        }
        else if (to_fit == FIT_RHOV) {
            Eigen::ArrayXd rhofit = eval_RHS(m_THETA, c).exp()*m_Dc;
            Eigen::ArrayXd rhoeos = m_LHS.exp()*m_Dc;
            deviation = (rhofit / rhoeos - 1) * 100;
            ylabel = "Density deviation (%)";
        }
        else if (to_fit == FIT_RHOL) {
            Eigen::ArrayXd rhofit = eval_RHS(m_THETA, c)*m_Dc;
            Eigen::ArrayXd rhoeos = m_LHS;
            deviation = (rhofit / rhoeos - 1) * 100;
            ylabel = "Density deviation (%)";
        }
        using namespace pybind11::literals;
        plt.attr("plot")(1 / (m_THETA + 1), deviation);
        plt.attr("ylabel")(ylabel);
        plt.attr("axvline")(m_Tt / m_Tc, "dashes"_a = std::vector<double>(2, 2));
        plt.attr("xlabel")("$T/T_c$");
        plt.attr("savefig")(m_name + ".pdf");
        plt.attr("close")();
    }
    Eigen::ArrayXd eval_RHS(const Eigen::ArrayXd& x, const Eigen::ArrayXd &c) {
        Eigen::ArrayXd num = Eigen::ArrayXd::Zero(x.size()), den = Eigen::ArrayXd::Zero(x.size());
        assert(m_Nnum + m_Nden == c.size());
        for (auto i = 0; i < m_Nnum; ++i) {
            num += c[i]*x.pow(i);
        }
        for (auto i = 0; i < m_Nden; ++i) {
            den += c[m_Nnum+i]*x.pow(i+1);
        }
        return num/(1+den);
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const std::vector<double> &c = static_cast<const CEGO::NumericalIndividual<double>*>(pind)->get_coefficients();
        return objective(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()) );
    }
    double objective(const Eigen::ArrayXd &c) {
        //Ncalls++;
        return (eval_RHS(m_THETA, c) - m_LHS).square().sum();
    }
    void plot_trace(const std::vector<double> &best_costs) {
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(best_costs);
        plt.attr("show")();
    }
};

int main()
{
    py::scoped_interpreter interp{};
    std::srand((unsigned int)time(0));

    std::size_t Nnum = 4, Nden = 4;

    // Construct the bounds
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < Nnum; ++i) {
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-10000, 10000)));
    }
    for (auto i = 0; i < Nden; ++i){
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-10000, 10000)));
    }
    RatPolyAncillary rp("PROPANE", Nnum, Nden);
    rp.plot_curve();

    auto Ncalls = 0;
    CEGO::CostFunction cost_wrapper = std::bind((double (RatPolyAncillary::*)(const CEGO::AbstractIndividual *)) &RatPolyAncillary::objective, &rp, std::placeholders::_1);
    auto Nlayers = 1;
    auto layers = CEGO::Layers<double>(cost_wrapper, bounds.size(), 40, Nlayers, 5);
    layers.parallel = true;
    layers.set_bounds(bounds);

    auto flags = layers.get_evolver_flags();
    flags["Nelite"] = 2;
    flags["Fmin"] = 0.5;
    flags["Fmax"] = 0.5;
    flags["CR"] = 0.9;
    layers.set_evolver_flags(flags);

    std::vector<double> best_costs;
    std::vector<std::vector<double> > objs;
    double VTR = 1e-6, best_cost = 999999.0;
    auto startTime = std::chrono::system_clock::now();
    for (auto counter = 0; counter < 50000; ++counter) {
        layers.do_generation();

        // Store the best objective function in each layer
        std::vector<double> oo;
        for (auto &&cost_coefficients : layers.get_best_per_layer()) {
            oo.push_back(std::get<0>(cost_coefficients));
        }
        objs.push_back(oo);

        // For the overall best result, print it, and write JSON to file
        auto best_layer = layers.get_best();
        best_cost = std::get<0>(best_layer); best_costs.push_back(best_cost);
        if (counter % 50 == 0) {
            std::cout << counter << ": best: " << best_cost << "\n ";// << CEGO::vec2string(best_coeffs) << "\n";
            //auto c = std::get<1>(best_layer);
            //rp.plot_deviation(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()));
        }
        if (best_cost < VTR){ break; }
    }
    auto best_layer = layers.get_best();
    auto c = std::get<1>(best_layer);
    rp.plot_deviation(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()));
    auto endTime = std::chrono::system_clock::now();
    double elap = std::chrono::duration<double>(endTime - startTime).count();
    std::cout << "run:" << elap << " s\n";
    std::cout << "NFE:" << Ncalls << std::endl;

}

#else
int main(){
    std::cout << "Due to missing support for pybind11, this file cannot be run\n";
}
#endif


#include <omp.h>

#include <Eigen/Dense>
#include <bitset>
#include <iostream>
#include <tuple>

#include "qpp.h"

using namespace qpp;
using uint = unsigned int;

struct SimState {
  ket state = ket::Zero(1);
  int num_qubits = 0;
  SimState() { state << 1; }
};

typedef SimState* state_t;

extern "C" state_t empty() { return new SimState; }

extern "C" void discard(state_t s) { delete s; }

extern "C" int qinit(state_t s) {
  s->state = kron(s->state, 0_ket);
  return s->num_qubits++;
}

enum Gate : int {
  X = 0,
  Y = 1,
  Z = 2,
  H = 3,
  CNOT = 4,
  CZ = 5,
  TOF = 6,
  FRED = 7,
  PHASE = 8,
  CPHASE = 9
};

extern "C" void unitary1(state_t s, Gate g, uint q) {
  cmat u;
  switch (g) {
    case X:
      u = gt.X;
      break;
    case Y:
      u = gt.Y;
      break;
    case Z:
      u = gt.Z;
      break;
    case H:
      u = gt.H;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q});
}

extern "C" void unitary2(state_t s, Gate g, uint q1, uint q2) {
  cmat u;
  switch (g) {
    case CNOT:
      u = gt.CNOT;
      break;
    case CZ:
      u = gt.CZ;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q1, q2});
}

extern "C" void unitary3(state_t s, Gate g, uint q1, uint q2, uint q3) {
  cmat u;
  switch (g) {
    case TOF:
      u = gt.TOF;
      break;
    case FRED:
      u = gt.FRED;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q1, q2, q3});
}

extern "C" void punitary1(state_t s, Gate g, uint q, double p) {
  cmat u = std::polar(1.0, M_PI * p) * gt.RZ(2 * M_PI * p);
  switch (g) {
    case PHASE:
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q});
}

extern "C" void punitary2(state_t s, Gate g, uint q1, uint q2, double p) {
  cmat u = std::polar(1.0, M_PI * p) * gt.RZ(2 * M_PI * p);
  switch (g) {
    case CPHASE:
      break;
    default:
      abort();
  }
  s->state = applyCTRL(s->state, u, {q1}, {q2});
}

extern "C" bool measure(state_t s, uint q) {
  --s->num_qubits;
  auto measured = measure_seq(s->state, {q});
  s->state = std::get<ST>(measured);
  return std::get<RES>(measured)[0];
}

inline uint swap_bits(uint x, uint p1, uint p2) {
  const uint y = ((x >> p1) & 1) ^ ((x >> p2) & 1);
  return x ^ ((y << p1) | (y << p2));
}

inline void swap(ket& state, const idx numdims, const uint* const qs, uint n) {
  using namespace Eigen;
  PermutationMatrix<Dynamic, Dynamic> perm(1UL << numdims);

#ifdef HAS_OPENMP
#pragma omp parallel for
#endif
  for (idx i = 0; i < 1UL << numdims; ++i) {
    idx j = i;
    for (uint k = 0; k < n; ++k) {
      j = swap_bits(j, numdims - qs[n - k - 1] - 1, k);
    }
    perm.indices()[i] = j;
  }

  state = perm * state;
}

extern "C" bool separable(state_t s, const uint* const qs, uint n) {
  ket k = s->state;
  swap(k, s->num_qubits, qs, n);
  const idx dim = 1UL << n;
  const idx rem = 1UL << (s->num_qubits - n);
  auto coeffs = schmidtcoeffs(k, {rem, dim});
  return std::count_if(coeffs.data(), coeffs.data() + coeffs.size(),
                       [](double coeff) { return fabs(coeff) > chop; }) == 1;
}

extern "C" void print(state_t s) {
  if (s->state.size() < 2) {
    std::cout << "(empty)" << std::endl;
  } else {
    for (idx i = 0; i < s->state.size(); ++i) {
      std::bitset<32> bs(i);
      std::cout << "|";
      for (int j = s->num_qubits - 1; j >= 0; --j) {
        std::cout << bs[j];
      }
      std::cout << "> : " << s->state[i].real() << " + " << s->state[i].imag()
                << "i" << std::endl;
    }
  }
}


#include <boost/thread.hpp>
#include <iostream>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <math.h>
#include <c2_algorithm.h>

using namespace std;

c2_algorithm::c2_algorithm()
{
    pos = 0;
    t_pos = 0;
    vel = 0;
    t_vel = 0;
    acc = 0;
    t_acc = 0;
    last_vel = 0;
    last_acc = 0;
}
c2_algorithm::~c2_algorithm()
{
}

double c2_algorithm::sign(double x)
{
    if (x > 0)
    {
        return 1;
    }
    else if (x == 0)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

double c2_algorithm::power(double x, int n)
{
    double val = 1.0;
    while (n--)
        val *= x;
    return val;
}

void c2_algorithm::prepare()
{
    ek = (pos - t_pos) / jeck;
    ek1 = (vel - t_vel) / jeck;
}

double c2_algorithm::sat(double x)
{
    if (x < -1)
    {
        return -1;
    }
    else if (x >= -1 && x <= 1)
    {
        return x;
    }
    else
    {
        return 1;
    }
}

void c2_algorithm::c2_function()
{
    double ts = (double)1 / frequency;
    double zk = 1 / ts * (ek / ts + ek1 / 2);
    double zk1 = ek1 / ts;
    double m1 = 1 + 8 * abs(zk);
    double m = floor((1 + sqrt(m1)) / 2);
    double omk = zk1 + zk / m + (m - 1) / 2 * sign(zk);
    double uk1 = vel * sign(omk) + max_vel - ts * jeck;
    double uk2 = 1 + sign(uk1);
    uk = -1 * jeck * sat(omk) * uk2 / 2;
}

void c2_algorithm::get_qk(double l_vel, double l_pos, double &vel_, double &pos_)
{
    prepare();
    c2_function();
    double ts = (double)1 / frequency;
    qk1 = l_vel + ts * uk;
    qk = l_pos + ts / 2 * (qk1 + l_vel);

    vel = qk1;
    pos = qk;

    vel_ = qk1;
    pos_ = qk;
}

void c2_algorithm::start(double c_pos, double target_pos, double c_vel, double target_vel)
{
    pos = c_pos;
    t_pos = target_pos;
    vel = c_vel;
    t_vel = target_vel;
}

void c2_algorithm::init(double min_vel_, double max_vel_, double min_acc_, double max_acc_, double jeck_, int frequency_ = 100)
{
    min_vel = min_vel_;
    max_vel = max_vel_;
    min_acc = min_acc_;
    max_acc = max_acc_;
    jeck = jeck_;
    frequency = frequency_;
}


/*
This sample illustrates how to use the Fréchet Range Queries Library from a
typical Boost::Geometry project.

two versions are given:
  one is vector_of_points, which models a trajectory as a vector of boost points
  one is linestring, which models a trajectory as a linestring feature.

*/
#include <cmath>
#include <functional>
#include <iostream>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/linestring.hpp>

#include "../include/frechetrange/frechetrange.hpp"

using std::cout;
using std::endl;
namespace bg = boost::geometry;

// Declare Geometry
typedef bg::model::point<double, 2, bg::cs::cartesian> point_type;
typedef bg::model::linestring<point_type> linestring_type;

struct get_coordinate {
    template <size_t dim>
    static double get(const point_type &p) {
        return p.get<dim>();
    }
};

struct squared_distance {
    double operator()(const point_type &p, const point_type &q) const {
        return bg::comparable_distance(p, q);
    }
};

int main(int argc, char **argv) {
    // Linestring version
    linestring_type t1, t2;

    // read from WKT
    bg::read_wkt("LINESTRING(0 0 , 0 1, 0 2)", t1);
    bg::read_wkt("LINESTRING(1 1, 2 2, 1 3)", t2);

    // instantiate some decider, this time fully specified.
    frechetrange::detail::dv::frechet_distance<2, get_coordinate,
                                               squared_distance>
        fd;

    cout << std::fixed;
    // a range scan
    for (double d = 1; d < 5; d += 0.25)
        cout << "Reachable at " << d << ":\t"
             << (fd.is_bounded_by(t1, t2, d) ? "yes" : "no") << endl;

    // estimate the distance by interval cutting
    {
        std::pair<double, double> interval = {0, 5};
        while ((interval.second - interval.first) > 0.001) {
            double m = (interval.first + interval.second) / 2;

            if (fd.is_bounded_by(t1, t2, m)) {
                interval.second = m;
            } else {
                interval.first = m;
            }
        }
        cout << "Final Interval: [" << interval.first << "," << interval.second
             << "]" << endl;
    }

    // Bringmann Baldus Case

    frechetrange::detail::bb::frechet_distance<2, get_coordinate,
                                               squared_distance>
        fd2;

    cout << std::fixed;
    // a range scan
    for (double d = 1; d < 5; d += 0.25)
        cout << "Reachable at " << d << ":\t"
             << (fd2.is_bounded_by(t1, t2, d) ? "yes" : "no") << endl;

    // estimate the distance by interval cutting
    {
        std::pair<double, double> interval = {0, 5};
        while ((interval.second - interval.first) > 0.001) {
            double m = (interval.first + interval.second) / 2;

            if (fd2.is_bounded_by(t1, t2, m)) {
                interval.second = m;
            } else {
                interval.first = m;
            }
        }
        cout << "Final Interval: [" << interval.first << "," << interval.second
             << "]" << endl;
    }

    return 0;
}


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/algorithms/detail/vincenty_inverse.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gdav2.3.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class vincenty
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline vincenty()
        : m_spheroid()
    {}

    explicit inline vincenty(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return geometry::detail::vincenty_inverse
                <
                    typename calculation_type<Point1, Point2>::type
                >(get_as_radian<0>(point1),
                  get_as_radian<1>(point1),
                  get_as_radian<0>(point2),
                  get_as_radian<1>(point2),
                  m_spheroid).distance();
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<vincenty<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<vincenty<Spheroid, CalculationType>, P1, P2>
    : vincenty<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<vincenty<Spheroid, CalculationType> >
{
    typedef vincenty<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<vincenty<Spheroid, CalculationType> >
{
    static inline vincenty<Spheroid, CalculationType> apply(vincenty<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<vincenty<Spheroid, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<vincenty<Spheroid, CalculationType>, P1, P2>::type
        apply(vincenty<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP


/*!  @author Michael Brand
*    @Excercise 10 - final
*    @date 26.07.2017
*
*   Algorithm to find Steiner tree on a graph where primes count as termainals.
*
*   I decided to leave all code in one file since its mainly consistent of two bigger algorithms
*   1. Dijkstra_mod
*   2. Analyzing Dijkstra output in main-function
*   I don't think it gets to complicated reading it from top to bottom.
*
*   Parallel computing
*   Take #n random source nodes from parameter input and start the heurisitic parallely from these sources
*/

#include <iostream>
#include <fstream>
#include <stdexcept>
#include <cstdio>
#include <ctime>
#include <chrono>
#include <string>
#include <vector>
#include <climits>
#include <utility>                          // for std::pair
#include <random>

#include <boost/graph/adjacency_list.hpp>

#ifdef _OPENMP
#include <omp.h>
#endif

using namespace std;

using vertex_ = int;
using weight_ = int;

/**Edge
 * pair of ints containing the weight and
 * the vertex pointed to. This is useful
 * for the adjacency list
 */
using Edge = pair<vertex_, weight_>;

/**Graph
 * An adjacency list representing the graph.
 * graph[i] returns a list with Edge elements.
 *
 */
using Graph = vector< vector<Edge> >;

/**struct pq_compare
 * compare structure for edges
 * allows me to compare two edges in a graph
 * this way I can sort edges for any vertex under consideration of their weight
 */
struct pq_compare {
    bool operator() (const Edge i, const Edge j) const{
    return (i.second <= j.second); }
};

/*! \fn bool isPrime(int number)
    \brief checks if number is a prime.
    \param number number to be checked.
*/
bool isPrime(int number){
    if(number < 2) return false;
    if(number == 2) return true;
    if(number % 2 == 0) return false;
    for(int i=3; (i*i)<=number; i+=2){
        if(number % i == 0 ) return false;
    }
    return true;

}

/*! \fn std::vector<int> getRequiredPrimes(int numV)
 *  \brief lists all primes <= numV in a vector.
 *  \param numV number to be checked.
*/
std::vector<int> getRequiredPrimes(int numV){
  std::vector<int> primes;
  for(int i=2; i<=numV; i++){
    if(isPrime(i)){
      primes.push_back(i);
    }
  }
  return primes;
}

/*! \fn bool edgeInSteinerAlEx(Graph &stGph, vertex_ u, vertex_ v){
*   \brief Checks if Edge (u,v) already exists in SteinerGraph
*	  \param stGph Steiner Tree Graph that is checked for existing edge
*   \param u source-vertex
*   \param v dest-vertex
*/
bool edgeInSteinerAlEx(Graph &stGph, vertex_ u, vertex_ v){
	for(vector<Edge>::const_iterator ni = stGph[u].begin(); ni != stGph[u].end(); ni++){
		if(ni->first==v)
			return true;
	}
	return false;
}
/*! \fn std::vector<vertex_> dijkstra_mod(const Graph &graph, vertex_ root, vector<int> &remPrimes)
*   \brief modified dijkstra algorithm used for steiner tree problem
*   \param &graph the graph we are working on.
*   \param &steinerGraph an empty graph of size(graph) that will be used to build steinerGraph
*   \param root root-vertex
*   \return objective Value of SteinerTree
*/
int dijkstra_mod(const Graph &graph, Graph &steinerGraph, vertex_ root) {
  int wghtLocTree = 0; /*!< return value */
  std::vector<int> remPrimes = getRequiredPrimes(graph.size());
  std::vector<weight_> dist(graph.size(), INT_MAX);
  /* A set helps insertion and insert/erase/find operations in logarithmic time.
   * This set maintains Edge(distance,vertex number) sorted on basis of distance
   */
  set< Edge , pq_compare> pq;
  set< Edge , pq_compare > ::iterator it;

  vector<vertex_> pre(graph.size(), (-1)); /*!< vevtor of predecessors */
  int u,v,wt;

  dist[root] = 0;
  pq.insert(Edge(root,0));
  while(pq.size() != 0){
  	if(!(remPrimes.size()>0)) break;
    bool pFound = false;
    it = pq.begin();
    u = it->first;
    pq.erase(it);
    if((u != root) && (isPrime(u+1))){
      for(unsigned int chk=0; chk<remPrimes.size(); chk++){
        if((remPrimes[chk])==(u+1)){
          //NEW PRIME FOUND
          pFound = true;
          //delete u from remainingPrimes
          remPrimes.erase(remPrimes.begin()+chk);
          pq.insert(Edge(u,0));
          dist[u]=0;
          //Add Path to u to SteinerTree Structure
          int lV = u;
          do{
            int preV = pre[lV];
            for(unsigned int eIndx = 0; eIndx < graph[lV].size(); eIndx++){
              if(graph[lV][eIndx].first==preV){
              	if(!(edgeInSteinerAlEx(steinerGraph, lV,preV))){
            			steinerGraph[lV].push_back(Edge(preV, graph[lV][eIndx].second));
	                steinerGraph[preV].push_back(Edge(lV, graph[lV][eIndx].second));
	                wghtLocTree += graph[lV][eIndx].second;
	                pq.insert(Edge(preV,0));
            		}
                else{
                  //abort inserting edges since they are all part of graph
                  break;
                }
              }
            }
	          lV = pre[lV];
          }while(dist[lV] != 0);
          break;
        }
      }
    }
    if(pFound){
      continue;
    }
    for(vector<Edge>::const_iterator ni = graph[u].begin(); ni != graph[u].end(); ni++){
      v  = ni->first;
      wt = ni->second;
      if(dist[v] > dist[u] + wt){
        pre[v] = u;
        if(dist[v] != INT_MAX){
        	pq.erase(Edge(v,dist[v]));
        }
        dist[v] = dist[u] + wt;
        pq.insert(Edge(v,dist[v]));
      }

    }
  }
  return wghtLocTree;
}

/*! \fn int validateStGraph(&gphToChk)
*   \brief tests if gphToChk is connected and inherits all primes
*   \param &grphToChk minimal SteinerTree found
*   \return 1 if successful, 0 if unsuccessful
*/
int validateStGraph(Graph &gphToChk){

	std::vector<int> remPrimes = getRequiredPrimes(gphToChk.size());
	int totPrimes = remPrimes.size();
	int stTreePrimeCount = 0;

	bool visited[gphToChk.size()];
	for(int i=0; i<gphToChk.size(); i++)
		visited[i]=false;

  if(!(gphToChk[1].empty())){
		set< Edge , pq_compare> pq;
		set< Edge , pq_compare > ::iterator it;
		int u,v,wt;
		pq.insert(Edge(1,0));
		while(pq.size() != 0){
	    it = pq.begin();
		  u = it->first;
		  pq.erase(it);
		  if(visited[u]){
		  	return 0;
		  }
			visited[u] = true;
			for(vector<Edge>::const_iterator ni = gphToChk[u].begin(); ni != gphToChk[u].end(); ni++){
				v  = ni->first;
	     	wt = ni->second;
	     	pq.insert(Edge(v,wt));
	     	//Delete ṕarrallel edge from minSteinerGraph
	     	for(int k=0; k<gphToChk[v].size(); k++){
	     		if(gphToChk[v][k].first == u){
	     			gphToChk[v].erase(gphToChk[v].begin()+k);
	     		}
	     	}
			}
    }

    for(int j=1; j<gphToChk.size(); j++){
    	if(isPrime(j-1)){
    		if(!(gphToChk[j-1].empty()))
    			stTreePrimeCount++;
    	}
    }

    if(stTreePrimeCount = totPrimes){
    	return 1;
    }
    else{
    	return 0;
    }
  }
  else{
		return 0;
	}
}

/*!
*
*		INPUT n_root_nodes gives an int-number that defines the number of chosen terminals
*/
int main (int argc, char* argv[]) {
  /**
  * start timers for cpu and wall time
  */
  clock_t cpu0 = clock();
  bool output = false;

  /*
  *   Check input arguments
  */
  if( argc < 3){
      fprintf(stderr, "Call the program as: %s 'filename.gph' 'n_root_nodes' (-s)\n", argv[0]);
      exit(EXIT_FAILURE);
  }
  if(std::string(argv[argc-1]) == "-s"){
  	output=true;
  }

  ifstream    file(argv[1]);
  string      line;

  if(!file){
    fprintf(stderr, "Could not open file.\n");
    return -1;
  }

  /**
  * read number of vertices and edges
  */
  getline(file, line, ' ');
  const int numV = stoi(line);
  getline(file, line, '\n');
  const int numE = stoi(line);

  /**
  *	check if no nodes doesnt exceed vertices of graph
  */
  int n_nodes = 0;

  try{
  	if((stoi(argv[2]) > 0) && (stoi(argv[2]) <= numV))
  		n_nodes = stoi(argv[2]);
  	else{
  		fprintf(stderr, "Make sure parameter n_root_nodes is positive and does not exceed graph size.\n");
      exit(EXIT_FAILURE);
  	}
  }
  catch(...){
  	fprintf(stderr, "Call the program as: %s 'filename.gph' 'n_root_nodes' (-s)\n", argv[0]);
    exit(EXIT_FAILURE);
  }

  /*!
  *   FILL UP sourceVert by random numbers in range 0<->graph.size())
  */
  vector<int> sourceVert;
	std::random_device rd;     // only used once to initialise (seed) engine
	std::mt19937 rng(rd());    // random-number engine used (Mersenne-Twister in this case)
	for(int k=0; k<n_nodes; k++){
		std::uniform_int_distribution<int> uni(0,numV-1);
		sourceVert.push_back(uni(rng));
	}

  /**
  * create graph
  */
  Graph graph(numV);

  /**
  * read ín given gph file
  */
  while( getline(file, line) ){
    stringstream linestream(line);
    string       vertex1, vertex2, weight;
    try{
      getline(linestream, vertex1, ' ');
      getline(linestream, vertex2, ' ');
      getline(linestream, weight, '\n');

      /**
      * add both directions of edge to the graph since undirected
      * index switch applies: 1 --> 0, 1 --> 2, etc.
      */
      graph[stoi(vertex1)-1].push_back(Edge(stoi(vertex2)-1, stoi(weight)));
      graph[stoi(vertex2)-1].push_back(Edge(stoi(vertex1)-1, stoi(weight)));
    }catch (...){
    	fprintf(stderr, "Corrupt data in .gph file.");
      exit(EXIT_FAILURE);
    }

  }
  file.close();

  //Input Done. Start Wall Time counter
  auto   wall0 = chrono::system_clock::now();

  int minStVal = INT_MAX;
  Graph minSteiner(numV);
  int locStTrWght[sourceVert.size()];

  //Parallel section (ex10)
  #pragma omp parallel for shared(locStTrWght, minStVal, minSteiner)

  for(unsigned int initroot=0; initroot<sourceVert.size(); initroot++){
    //Create local SteinerGraph as emty Graph
    Graph stGraph(numV);
    //Check if initroot is in Graph-range
    if(sourceVert[initroot]>=graph.size()){
      fprintf(stderr, "Invalid source node: %i. Please make sure that root node indices are lower than graph size!\n", sourceVert[initroot]+1);
      exit(EXIT_FAILURE);
    }
    //Call Dijkstra with initroot-vertex as source
    locStTrWght[initroot] = dijkstra_mod(graph, stGraph, sourceVert[initroot]);

    //If return value is new min, save SteinerGraph
    if(locStTrWght[initroot]<minStVal){
    	minSteiner = stGraph;
    	minStVal = locStTrWght[initroot];
    }
  }

  //End Wall-Timer
  chrono::duration<double> wallDur = (chrono::system_clock::now() - wall0);
  double wallTime = wallDur.count();

  /*!
  *   RUN EXTRA TEST
  *   Checks if graph is a tree + if all primes are found in the tree
  *   returns 1 if successful, 0 if unsuccessful
  */
  int validate = validateStGraph(minSteiner);

  if(validate==0) fprintf(stdout, "ERROR.\nSteiner Graph is not a tree\n");

  /*
  *   OUTPUT
  */
  int ocount=0;
  fprintf(stdout, "TLEN:\t %i\n", minStVal);

  //Output of MinSteiner-Edges (only in manual call (-s).)
  if(output){
  	fprintf(stdout, "TREE:\t");
  	for(unsigned int i=0; i<minSteiner.size(); i++){
  		if(!(minSteiner[i].empty())){
  			for(unsigned int j=0; j<minSteiner[i].size(); j++){
  				fprintf(stdout, "(%i,%i), ", i+1, minSteiner[i][j].first+1);
  				ocount++;
  				if((ocount%10)==0) fprintf(stdout, "\n\t");
  			}
  		}
  	}
  	fprintf(stdout, "\n");
  }
  double cpuTime = (clock() - cpu0) / (double) CLOCKS_PER_SEC;
  fprintf(stdout, "TIME:\t %f\n", cpuTime);
  fprintf(stdout, "WALL:\t %f\n", wallTime);
  return 0;
}


/*    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
 *      131204    S. Hirsh          File created.
 *      140120    T. Roegiers       Codecheck small changes.
 *      140410    S. Hirsh          Minor revisions.
 *      140411    T. Roegiers       Minor textual changes during codecheck.
 *
 *    References
 *
 *
 *    Notes
 *
 */

#include <cmath>

#include <Eigen/Geometry>

#include "Tudat/Mathematics/BasicMathematics/rotationAboutArbitraryAxis.h"

namespace tudat
{
namespace basic_mathematics
{

//! Compute rotation of point about arbitrary axis
Eigen::Vector3d computeRotationOfPointAboutArbitraryAxis(
        const Eigen::Vector3d& originOfRotation,
        const double angleOfRotation,
        const Eigen::Vector3d& axisOfRotation,
        const Eigen::Vector3d& initialPositionOfPoint )
{

    //Declare and initialize rotation matrix
    Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Zero( );

    // Compute rotation matrix using AngleAxis object.
    rotationMatrix = Eigen::AngleAxisd( angleOfRotation, axisOfRotation.normalized( ) );

    // Compute initial of position of point with respect to origin of rotation.
    const Eigen::Vector3d initialPositionOfPointWithRespectToOriginOfRotation =
            initialPositionOfPoint - originOfRotation;

    // Compute rotation of point about axis of rotation with respect to origin of rotation.
    const Eigen::Vector3d rotatedPositionWithRespectToOriginOfRotation =
            rotationMatrix * initialPositionOfPointWithRespectToOriginOfRotation;

    //Return position with respect to the chosen arbitrary origin after rotation about
    //arbitrary axis.
    return rotatedPositionWithRespectToOriginOfRotation + originOfRotation;

}

//! Compute rotation of vector about arbitrary axis
Eigen::Vector3d computeRotationOfVectorAboutArbitraryAxis(
        const Eigen::Vector3d& originOfRotation,
        const double angleOfRotation,
        const Eigen::Vector3d& axisOfRotation,
        const Eigen::Vector3d& initialPositionOfVectorTail,
        const Eigen::Vector3d& initialVector )
{

    // Compute rotation of the tail of vector. Resulted position is with respect to the chosen
    // arbitrary origin.
    Eigen::Vector3d rotatedPositionOfVectorTail =
            computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                      axisOfRotation, initialPositionOfVectorTail );

    // Compute rotation of the head of vector. Resulted position is with respect to the chosen
    // arbitrary origin.
    Eigen::Vector3d rotatedPositionOfVectorHead =
            computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                      axisOfRotation,
                                                      initialPositionOfVectorTail + initialVector );

    // Return rotated vector with respect to the chosen arbitrary origin
    return rotatedPositionOfVectorHead - rotatedPositionOfVectorTail;

}

} // namespace basic_mathematics
} // namespace tudat