/*******************************************************************************
 *
 * File Name: H01indexedCSVdatabaseAlgorithms.hpp
 * Author: Richard Bruce Baxter - Copyright (c) 2021 Baxter AI (baxterai.com)
 * License: MIT License
 * Project: H01LocalConnectome
 * Requirements: see H01indexedCSVdatabase.hpp
 * Compilation: see H01indexedCSVdatabase.hpp
 * Usage: see H01indexedCSVdatabase.hpp
 * Description: H01 indexed CSV database algorithms
 * Comments:
 * /
 *******************************************************************************/

#ifndef HEADER_H01indexedCSVdatabaseAlgorithms
#define HEADER_H01indexedCSVdatabaseAlgorithms

#include "H01indexedCSVdatabase.hpp"

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

#ifdef INDEXED_CSV_DATABASE_ALGORITHMS

//https://gist.github.com/chrisengelsma/108f7ab0a746323beaaf7d6634cf4add
/**
 * PURPOSE:
 *
 *	Polynomial Regression aims to fit a non-linear relationship to a set of
 *	points. It approximates this by solving a series of linear equations using
 *	a least-squares approach.
 *
 *	We can model the expected value y as an nth degree polynomial, yielding
 *	the general polynomial regression model:
 *
 *	y = a0 + a1 * x + a2 * x^2 + ... + an * x^n
 *
 * LICENSE:
 *
 * MIT License
 *
 * Copyright (c) 2020 Chris Engelsma
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * @author Chris Engelsma
 */
#include <vector>
#include <stdlib.h>
#include <cmath>
#include <stdexcept>
#include "SHAREDvars.hpp"

class H01indexedCSVdatabaseAlgorithmsFit
{
public:

	H01indexedCSVdatabaseAlgorithmsFit(void);
	~H01indexedCSVdatabaseAlgorithmsFit(void);

	#ifdef INDEXED_CSV_DATABASE_QUERY_PERFORM_INCOMING_AXON_MAPPING_2D_POLY_REGRESSION
	double calculatePoly(int xx);
	#endif

	long connectionNeuronID;
	int estSynapseType;
	#ifdef INDEXED_CSV_DATABASE_QUERY_PERFORM_INCOMING_AXON_MAPPING_2D_POLY_REGRESSION
	double a;
	double b;
	double c;
	#endif
	#ifdef INDEXED_CSV_DATABASE_QUERY_PERFORM_INCOMING_AXON_MAPPING_3D_LINEAR_REGRESSION
	vec origin;
	vec axis;
	#endif
};

template <class TYPE>
class PolynomialRegression {
	public:

		PolynomialRegression();
		virtual ~PolynomialRegression(){};

		bool fitIt(
			const std::vector<TYPE> & x,
			const std::vector<TYPE> & y,
			const int & order,
			std::vector<TYPE> &	coeffs);
};

template <class TYPE>
PolynomialRegression<TYPE>::PolynomialRegression() {};

template <class TYPE>
bool PolynomialRegression<TYPE>::fitIt(
	const std::vector<TYPE> & x,
	const std::vector<TYPE> & y,
	const int & order,
	std::vector<TYPE> & coeffs)
{
	// The size of xValues and yValues should be same
	if (x.size() != y.size()) {
		throw std::runtime_error( "The size of x & y arrays are different" );
		return false;
	}
	// The size of xValues and yValues cannot be 0, should not happen
	if (x.size() == 0 || y.size() == 0) {
		throw std::runtime_error( "The size of x or y arrays is 0" );
		return false;
	}

	size_t N = x.size();
	int n = order;
	int np1 = n + 1;
	int np2 = n + 2;
	int tnp1 = 2 * n + 1;
	TYPE tmp;

	// X = vector that stores values of sigma(xi^2n)
	std::vector<TYPE> X(tnp1);
	for (int i = 0; i < tnp1; ++i) {
		X[i] = 0;
		for (int j = 0; j < N; ++j)
			X[i] += (TYPE)pow(x[j], i);
	}

	// a = vector to store final coefficients.
	std::vector<TYPE> a(np1);

	// B = normal augmented matrix that stores the equations.
	std::vector<std::vector<TYPE> > B(np1, std::vector<TYPE> (np2, 0));

	for (int i = 0; i <= n; ++i)
		for (int j = 0; j <= n; ++j)
			B[i][j] = X[i + j];

	// Y = vector to store values of sigma(xi^n * yi)
	std::vector<TYPE> Y(np1);
	for (int i = 0; i < np1; ++i) {
		Y[i] = (TYPE)0;
		for (int j = 0; j < N; ++j) {
			Y[i] += (TYPE)pow(x[j], i)*y[j];
		}
	}

	// Load values of Y as last column of B
	for (int i = 0; i <= n; ++i)
		B[i][np1] = Y[i];

	n += 1;
	int nm1 = n-1;

	// Pivotisation of the B matrix.
	for (int i = 0; i < n; ++i)
		for (int k = i+1; k < n; ++k)
			if (B[i][i] < B[k][i])
				for (int j = 0; j <= n; ++j) {
					tmp = B[i][j];
					B[i][j] = B[k][j];
					B[k][j] = tmp;
				}

	// Performs the Gaussian elimination.
	// (1) Make all elements below the pivot equals to zero
	//		 or eliminate the variable.
	for (int i=0; i<nm1; ++i)
		for (int k =i+1; k<n; ++k) {
			TYPE t = B[k][i] / B[i][i];
			for (int j=0; j<=n; ++j)
				B[k][j] -= t*B[i][j];				 // (1)
		}

	// Back substitution.
	// (1) Set the variable as the rhs of last equation
	// (2) Subtract all lhs values except the target coefficient.
	// (3) Divide rhs by coefficient of variable being calculated.
	for (int i=nm1; i >= 0; --i) {
		a[i] = B[i][n];									 // (1)
		for (int j = 0; j<n; ++j)
			if (j != i)
				a[i] -= B[i][j] * a[j];			 // (2)
		a[i] /= B[i][i];									// (3)
	}

	coeffs.resize(a.size());
	for (size_t i = 0; i < a.size(); ++i)
		coeffs[i] = a[i];

	return true;
}

//https://gist.github.com/ialhashim/0a2554076a6cf32831ca (ialhashim/fitting3D.cpp)

template<class Vector3>
std::pair < Vector3, Vector3 > best_line_from_points(const std::vector<Vector3> & c)
{
	//copy coordinates to matrix in Eigen format
	size_t num_atoms = c.size();
	Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3);
	for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i];

	Vector3 origin = centers.colwise().mean();
	Eigen::MatrixXd centered = centers.rowwise() - origin.transpose();
	Eigen::MatrixXd cov = centered.adjoint() * centered;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);
	Vector3 axis = eig.eigenvectors().col(2).normalized();

	return std::make_pair(origin, axis);
}

#endif

#endif


/*
	1/2^n hyperspace.
*/

#ifndef __NVAR_HALFSPACE__
#define __NVAR_HALFSPACE__

#include <vector>
#include <cmath>
#include <stdexcept>
#include <Eigen/Dense>

namespace sisl {
	template<int N, class I=double>
	class halfspace{
	public:
		halfspace(const Eigen::Matrix<I, Dynamic, Dynamic> &M) {
			this->T = M;
			this->Tinv = M.inverse();
		}

		bool in_halfspace(std::vector<I> x) {
			Eigen::Matrix<I, Dynamic, 1> v(x.size());
			for(int i=0; i < x.size(); i++) v(i,0) = x[i];

			v = Tinv * v;
			for(int i=0; i < x.size(); i++)
				if(v(i) < (I)0)
					return false;
			return true;

		}
	private:
		Eigen::Matrix<I, Dynamic, Dynamic> T, Tinv;
	};
}

#endif //__NVAR_HALFSPACE__


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * 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.
 */
/*!
 * \file
 * \brief Example of tikhonov class with blurred sine test problem.
 */
#include <iomanip>
#include <iostream>
#include <random>
#include <unordered_map>
#include <vector>

#include <Eigen/Core>
#include <pybind11/embed.h>
#include <pybind11/stl.h>

#include "num_collect/regularization/explicit_l_curve.h"
#include "num_collect/regularization/tikhonov.h"
#include "num_prob_collect/regularization/blur_sine.h"

auto main() -> int {
    constexpr int prec = 15;
    std::cout << std::setprecision(prec);

    using coeff_type = Eigen::MatrixXd;
    using data_type = Eigen::VectorXd;
    static constexpr num_collect::index_type solution_size = 60;
    static constexpr num_collect::index_type data_size = 60;
    constexpr double error_rate = 0.01;

    // create test problem
    const auto prob =
        num_prob_collect::regularization::blur_sine(data_size, solution_size);
    std::mt19937 engine;  // NOLINT
    std::normal_distribution<double> dist{0.0,
        std::sqrt(prob.data().squaredNorm() /
            static_cast<double>(prob.data().size()) * error_rate)};
    auto data_with_error = prob.data();
    for (num_collect::index_type i = 0; i < data_with_error.size(); ++i) {
        data_with_error(i) += dist(engine);
    }

    // solve
    using solver_type =
        num_collect::regularization::tikhonov<coeff_type, data_type>;
    solver_type tikhonov;
    tikhonov.compute(prob.coeff(), data_with_error);

    using searcher_type =
        num_collect::regularization::explicit_l_curve<solver_type>;
    searcher_type searcher{tikhonov};
    searcher.search();
    Eigen::VectorXd solution;
    searcher.solve(solution);
    std::cout << "Optimal parameter: " << searcher.opt_param() << std::endl;
    std::cout << "Error rate: "
              << (solution - prob.solution()).squaredNorm() /
            prob.solution().squaredNorm()
              << std::endl;

    // plot graphs
    std::vector<double> param_list;
    std::vector<std::string> type_list;
    std::vector<double> value_list;
    const auto [min_param, max_param] = tikhonov.param_search_region();
    constexpr std::size_t num_samples = 101;
    for (std::size_t i = 0; i < num_samples; ++i) {
        const double rate =
            static_cast<double>(i) / static_cast<double>(num_samples - 1);
        const double param = min_param * std::pow(max_param / min_param, rate);

        const double residual_norm = tikhonov.residual_norm(param);
        param_list.push_back(param);
        type_list.emplace_back("residual norm");
        value_list.push_back(residual_norm);

        const double regularization_term = tikhonov.regularization_term(param);
        param_list.push_back(param);
        type_list.emplace_back("regularization term");
        value_list.push_back(regularization_term);

        const double curvature_value = tikhonov.l_curve_curvature(param);
        param_list.push_back(param);
        type_list.emplace_back("curvature");
        value_list.push_back(curvature_value);

        tikhonov.solve(param, solution);
        const double error = (solution - prob.solution()).squaredNorm() /
            prob.solution().squaredNorm();
        param_list.push_back(param);
        type_list.emplace_back("error rate");
        value_list.push_back(error);
    }

    pybind11::scoped_interpreter interpreter;
    auto pd = pybind11::module::import("pandas");
    auto px = pybind11::module::import("plotly.express");

    std::unordered_map<std::string, pybind11::object> data;
    data.try_emplace("param", pybind11::cast(param_list));
    data.try_emplace("type", pybind11::cast(type_list));
    data.try_emplace("value", pybind11::cast(value_list));

    auto fig = px.attr("line")(pybind11::arg("data_frame") = data,
        pybind11::arg("x") = "param", pybind11::arg("y") = "value",
        pybind11::arg("color") = "type", pybind11::arg("log_x") = true,
        pybind11::arg("log_y") = true);

    fig.attr("write_html")("blur_sine_tikhonov_norms.html");
    fig.attr("write_image")("blur_sine_tikhonov_norms.png");
}


// faces_angles.cpp
/*
 * Calculates all angles between crystal grids
 * (C) 2006 olegabr. All rights reserved.
*/

#include <iostream>
using std::cout; using std::endl;

#include <cmath>
using std::sqrt;

#include <vector>
using std::vector;

#include <map>
using std::multimap;

#include <utility>
using std::pair; using std::make_pair;

#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>

#include <point3d.h>
typedef point3d face_t;

typedef boost::array<face_t, 19> hkl_array;

struct face_pair
{
	face_t face1;
	face_t face2;
	friend bool operator == (face_pair const& lh, face_pair const& rh) {
		return
			lh.face1 == rh.face1 &&
			lh.face2 == rh.face2;
	}
	friend bool operator != (face_pair const& lh, face_pair const& rh) {
		return !(lh == rh);
	}
};

struct face_triple
{
	face_t face1;
	face_t face2;
	face_t face3;
};

namespace {
	double calc_angle(face_t const& face1, face_t const& face2)
	{
		double dCosAngle = face1 * face2 / (norm(face1) * norm(face2));
		double dAngleRadians = std::acos(dCosAngle);
		double dAngleDegrees = 180.0 - 180.0 * dAngleRadians / 3.14159265358979323F;
		return dAngleDegrees;
	}

	void print_faces(hkl_array const& arrHKL)
	{
		hkl_array::const_iterator iHKL = arrHKL.begin();
		hkl_array::const_iterator hklEnd = arrHKL.end();
		for (; iHKL != hklEnd; ++iHKL) {
			hkl_array::const_iterator jHKL = iHKL;
			for (++jHKL; jHKL != hklEnd; ++jHKL) {
				cout
					<< iHKL->x << iHKL->y << iHKL->z
					<< '\t'
					<< jHKL->x << jHKL->y << jHKL->z
					<< '\t'
					<< calc_angle(*iHKL, *jHKL) << "\n";
			}
		}
	}

	vector< face_pair >
	calc_faces(hkl_array const& arrHKL, double dAngle12, double dAngleErrorMin, double dAngleErrorMax)
	{
		multimap< double/*angle error*/, face_pair > mapFaces;

		hkl_array::const_iterator iHKL = arrHKL.begin();
		hkl_array::const_iterator hklEnd = arrHKL.end();
		for (; iHKL != hklEnd; ++iHKL) {
			hkl_array::const_iterator jHKL = iHKL;
			for (++jHKL; jHKL != hklEnd; ++jHKL) {
				double dAngle = calc_angle(*iHKL, *jHKL);
				double dAngleDelta = fabs(dAngle - dAngle12);
				if (dAngleDelta >= dAngleErrorMin && dAngleDelta <= dAngleErrorMax) {
					face_pair pr;
					pr.face1 = *iHKL;
					pr.face2 = *jHKL;
					mapFaces.insert(make_pair(dAngleDelta, pr));
				}
			}
		}

		vector< face_pair > vecFaces;
		vecFaces.reserve(mapFaces.size());

		multimap< double, face_pair >::const_iterator iterMap = mapFaces.begin();
		multimap< double, face_pair >::const_iterator iterMapEnd = mapFaces.end();
		for(; iterMap != iterMapEnd; ++iterMap) {
			vecFaces.push_back(iterMap->second);
		}

		return vecFaces;
	}

	void print_faces_pairs(vector< face_pair > const& vecFaces, double dAngle12)
	{
		vector< face_pair >::const_iterator iHKL = vecFaces.begin();
		vector< face_pair >::const_iterator hklEnd = vecFaces.end();
		for (; iHKL != hklEnd; ++iHKL) {
			face_t f1 = iHKL->face1;
			face_t f2 = iHKL->face2;
			double dAngle = calc_angle(f1, f2);
			double dErr = fabs(dAngle - dAngle12);
			dErr = dErr >= 0.01 ? dErr : 0.0;
			cout
				<< f1.x << f1.y << f1.z
				<< '\t'
				<< f2.x << f2.y << f2.z
				<< '\t'
				<< dAngle
				<< '\t'
				<< dErr
				<< "\n";
		}
	}

	void print_faces_within_error_bounds(hkl_array const& arrHKL, double dAngle12, double dAngleErrorMin, double dAngleErrorMax)
	{
		vector< face_pair > vecFaces;
		vecFaces = calc_faces(arrHKL, dAngle12, dAngleErrorMin, dAngleErrorMax);
		if (!vecFaces.empty()) {
			cout << "Angle error is in range [" << dAngleErrorMin << ", " << dAngleErrorMax << "]\n";
			print_faces_pairs(vecFaces, dAngle12);
			cout << "\n";
		}
	}

	void print_faces_triples(vector< face_triple > const& vecFaces, double dAngle12, double dAngle23)
	{
		vector< face_triple >::const_iterator iHKL = vecFaces.begin();
		vector< face_triple >::const_iterator hklEnd = vecFaces.end();
		for (; iHKL != hklEnd; ++iHKL) {
			face_t f1 = iHKL->face1;
			face_t f2 = iHKL->face2;
			face_t f3 = iHKL->face3;
			double dAngle1 = calc_angle(f1, f2);
			double dAngle2 = calc_angle(f2, f3);
			double dAngle3 = calc_angle(f3, f1);

			double dErr1 = fabs(dAngle1 - dAngle12);
			dErr1 = dErr1 >= 0.01 ? dErr1 : 0.0;

			double dErr2 = fabs(dAngle2 - dAngle23);
			dErr2 = dErr2 >= 0.01 ? dErr2 : 0.0;

			cout
				<< f1.x << f1.y << f1.z
				<< '\t'
				<< f2.x << f2.y << f2.z
				<< '\t'
				<< f3.x << f3.y << f3.z
				<< '\t'
				<< dAngle1
				<< '\t'
				<< dErr1
				<< '\t'
				<< dAngle2
				<< '\t'
				<< dErr2
				<< '\t'
				<< dAngle3
				<< "\n";
		}
	}

	void print_faces_within_error_bounds(
		hkl_array const& arrHKL,
		double dAngle12,
		double dAngle23,
		double dAngleErrorMin, double dAngleErrorMax
	)
	{
		vector< face_pair > vecFaces12;
		vecFaces12 = calc_faces(arrHKL, dAngle12, dAngleErrorMin, dAngleErrorMax);

		vector< face_pair > vecFaces23;
		vecFaces23 = calc_faces(arrHKL, dAngle23, dAngleErrorMin, dAngleErrorMax);

		if (!vecFaces12.empty() && !vecFaces23.empty()) {
			vector<face_triple> vecFaceTriples;

			vector< face_pair >::const_iterator iterFacePair12 = vecFaces12.begin();
			vector< face_pair >::const_iterator iterFacePair12End = vecFaces12.end();
			for (; iterFacePair12 != iterFacePair12End; ++iterFacePair12) {
				vector< face_pair >::const_iterator iterFacePair23 = vecFaces23.begin();
				vector< face_pair >::const_iterator iterFacePair23End = vecFaces23.end();
				for (; iterFacePair23 != iterFacePair23End; ++iterFacePair23) {
					if (*iterFacePair12 != *iterFacePair23) {
						if (iterFacePair12->face1 == iterFacePair23->face1) {
							face_triple triple;
							triple.face1 = iterFacePair12->face2;
							triple.face2 = iterFacePair12->face1;
							triple.face3 = iterFacePair23->face2;
							vecFaceTriples.push_back(triple);
						} else if (iterFacePair12->face1 == iterFacePair23->face2) {
							face_triple triple;
							triple.face1 = iterFacePair12->face2;
							triple.face2 = iterFacePair12->face1;
							triple.face3 = iterFacePair23->face1;
							vecFaceTriples.push_back(triple);
						} else if (iterFacePair12->face2 == iterFacePair23->face1) {
							face_triple triple;
							triple.face1 = iterFacePair12->face1;
							triple.face2 = iterFacePair12->face2;
							triple.face3 = iterFacePair23->face2;
							vecFaceTriples.push_back(triple);
						} else if (iterFacePair12->face2 == iterFacePair23->face2) {
							face_triple triple;
							triple.face1 = iterFacePair12->face1;
							triple.face2 = iterFacePair12->face2;
							triple.face3 = iterFacePair23->face1;
							vecFaceTriples.push_back(triple);
						}
					}
				}
			}

			cout << "Angle error is in range [" << dAngleErrorMin << ", " << dAngleErrorMax << "]\n";
			print_faces_triples(vecFaceTriples, dAngle12, dAngle23);
			cout << "\n";
		}
	}

	void print_faces_triples(vector< face_triple > const& vecFaces, double dAngle12, double dAngle23, double dAngle31)
	{
		vector< face_triple >::const_iterator iHKL = vecFaces.begin();
		vector< face_triple >::const_iterator hklEnd = vecFaces.end();
		for (; iHKL != hklEnd; ++iHKL) {
			face_t f1 = iHKL->face1;
			face_t f2 = iHKL->face2;
			face_t f3 = iHKL->face3;
			double dAngle1 = calc_angle(f1, f2);
			double dAngle2 = calc_angle(f2, f3);
			double dAngle3 = calc_angle(f3, f1);

			double dErr1 = fabs(dAngle1 - dAngle12);
			dErr1 = dErr1 >= 0.01 ? dErr1 : 0.0;

			double dErr2 = fabs(dAngle2 - dAngle23);
			dErr2 = dErr2 >= 0.01 ? dErr2 : 0.0;

			double dErr3 = fabs(dAngle3 - dAngle31);
			dErr3 = dErr3 >= 0.01 ? dErr3 : 0.0;

			cout
				<< f1.x << f1.y << f1.z
				<< '\t'
				<< f2.x << f2.y << f2.z
				<< '\t'
				<< f3.x << f3.y << f3.z
				<< '\t'
				<< dAngle1
				<< '\t'
				<< dErr1
				<< '\t'
				<< dAngle2
				<< '\t'
				<< dErr2
				<< '\t'
				<< dAngle3
				<< '\t'
				<< dErr3
				<< "\n";
		}
	}

	void print_faces_within_error_bounds(
		hkl_array const& arrHKL,
		double dAngle12,
		double dAngle23,
		double dAngle31,
		double dAngleErrorMin, double dAngleErrorMax
	)
	{
		vector< face_pair > vecFaces12;
		vecFaces12 = calc_faces(arrHKL, dAngle12, dAngleErrorMin, dAngleErrorMax);

		vector< face_pair > vecFaces23;
		vecFaces23 = calc_faces(arrHKL, dAngle23, dAngleErrorMin, dAngleErrorMax);

		if (!vecFaces12.empty() && !vecFaces23.empty()) {
			vector<face_triple> vecFaceTriples;

			vector< face_pair >::const_iterator iterFacePair12 = vecFaces12.begin();
			vector< face_pair >::const_iterator iterFacePair12End = vecFaces12.end();
			for (; iterFacePair12 != iterFacePair12End; ++iterFacePair12) {
				vector< face_pair >::const_iterator iterFacePair23 = vecFaces23.begin();
				vector< face_pair >::const_iterator iterFacePair23End = vecFaces23.end();
				for (; iterFacePair23 != iterFacePair23End; ++iterFacePair23) {
					if (*iterFacePair12 != *iterFacePair23) {
						if (iterFacePair12->face1 == iterFacePair23->face1) {
							double dAngle = calc_angle(iterFacePair12->face2, iterFacePair23->face2);
							double dErr = fabs(dAngle - dAngle31);
							dErr = dErr >= 0.01 ? dErr : 0;
							if (dErr >= dAngleErrorMin && dErr <= dAngleErrorMax) {
								face_triple triple;
								triple.face1 = iterFacePair12->face2;
								triple.face2 = iterFacePair12->face1;
								triple.face3 = iterFacePair23->face2;
								vecFaceTriples.push_back(triple);
							}
						} else if (iterFacePair12->face1 == iterFacePair23->face2) {
							double dAngle = calc_angle(iterFacePair12->face2, iterFacePair23->face1);
							double dErr = fabs(dAngle - dAngle31);
							dErr = dErr >= 0.01 ? dErr : 0;
							if (dErr >= dAngleErrorMin && dErr <= dAngleErrorMax) {
								face_triple triple;
								triple.face1 = iterFacePair12->face2;
								triple.face2 = iterFacePair12->face1;
								triple.face3 = iterFacePair23->face1;
								vecFaceTriples.push_back(triple);
							}
						} else if (iterFacePair12->face2 == iterFacePair23->face1) {
							double dAngle = calc_angle(iterFacePair12->face1, iterFacePair23->face2);
							double dErr = fabs(dAngle - dAngle31);
							dErr = dErr >= 0.01 ? dErr : 0;
							if (dErr >= dAngleErrorMin && dErr <= dAngleErrorMax) {
								face_triple triple;
								triple.face1 = iterFacePair12->face1;
								triple.face2 = iterFacePair12->face2;
								triple.face3 = iterFacePair23->face2;
								vecFaceTriples.push_back(triple);
							}
						} else if (iterFacePair12->face2 == iterFacePair23->face2) {
							double dAngle = calc_angle(iterFacePair12->face1, iterFacePair23->face1);
							double dErr = fabs(dAngle - dAngle31);
							dErr = dErr >= 0.01 ? dErr : 0;
							if (dErr >= dAngleErrorMin && dErr <= dAngleErrorMax) {
								face_triple triple;
								triple.face1 = iterFacePair12->face1;
								triple.face2 = iterFacePair12->face2;
								triple.face3 = iterFacePair23->face1;
								vecFaceTriples.push_back(triple);
							}
						}
					}
				}
			}

			if (!vecFaceTriples.empty()) {
				cout << "Angle error is in range [" << dAngleErrorMin << ", " << dAngleErrorMax << "]\n";
				print_faces_triples(vecFaceTriples, dAngle12, dAngle23, dAngle31);
				cout << "\n";
			}
		}
	}

	void usage()
	{
		cout
			<< "up to 3 angles between 3 faces (in degrees):\n"
			   "f1 f2 f3 => angle12 [angle23 [angle31]]"
			<< endl;
	}
}

using std::fixed;

int main(int argc, char* argv[])
try {
	hkl_array arrHKL =
	{
		face_t(1.0, 1.0, 1.0),

		face_t(1.0, 1.0, 0.0),
		face_t(1.0, 0.0, 1.0),
		face_t(0.0, 1.0, 1.0),

		face_t(1.0, 0.0, 0.0),
		face_t(0.0, 0.0, 1.0),
		face_t(0.0, 1.0, 0.0),

		face_t(1.0, 1.0, 2.0),
		face_t(2.0, 1.0, 1.0),
		face_t(1.0, 2.0, 1.0),

		face_t(2.0, 1.0, 0.0),
		face_t(1.0, 2.0, 0.0),
		face_t(1.0, 0.0, 2.0),
		face_t(2.0, 0.0, 1.0),
		face_t(0.0, 2.0, 1.0),
		face_t(0.0, 1.0, 2.0),

		face_t(1.0, 2.0, 2.0),
		face_t(2.0, 1.0, 2.0),
		face_t(2.0, 2.0, 1.0)
	};

	//cout.precision(2);
	//cout << fixed;

	switch(argc) {
		case 1 :
			print_faces(arrHKL);
			break;
		case 2 : // angle12 in degrees
			{
				double dAngle12 = boost::lexical_cast<double>(argv[1]);
				print_faces_within_error_bounds(arrHKL, dAngle12, 0.0, 1.0);
				print_faces_within_error_bounds(arrHKL, dAngle12, 1.0, 2.0);
				print_faces_within_error_bounds(arrHKL, dAngle12, 2.0, 3.0);
				print_faces_within_error_bounds(arrHKL, dAngle12, 3.0, 4.0);
				print_faces_within_error_bounds(arrHKL, dAngle12, 4.0, 5.0);
				print_faces_within_error_bounds(arrHKL, dAngle12, 5.0, 6.0);
			}
			break;
		case 3 : // angle12 angle23
			{
				double dAngle12 = boost::lexical_cast<double>(argv[1]);
				double dAngle23 = boost::lexical_cast<double>(argv[2]);
				print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 0.0, 5.0);
				////print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 0.0, 1.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 1.0, 2.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 2.0, 3.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 3.0, 4.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 4.0, 5.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, 5.0, 6.0);
			}
			break;
		case 4 : // angle12 angle23 angle31
			{
				double dAngle12 = boost::lexical_cast<double>(argv[1]);
				double dAngle23 = boost::lexical_cast<double>(argv[2]);
				double dAngle31 = boost::lexical_cast<double>(argv[3]);
				print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 0.0, 5.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 0.0, 1.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 1.0, 2.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 2.0, 3.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 3.0, 4.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 4.0, 5.0);
				//print_faces_within_error_bounds(arrHKL, dAngle12, dAngle23, dAngle31, 5.0, 6.0);
			}
			break;
		default :
			usage();
			break;
	}

	return 0;
}
catch (std::exception &e) {
	cout << e.what() << endl;
	return 1;
}
catch (...) {
	cout << "Unknown exception occured." << endl;
	return 1;
}


#pragma once

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

class HomographyModel {
 public:
  HomographyModel(const Eigen::Matrix3d& matrix);
  HomographyModel();
  cv::Point2f operator()(const cv::Point2f& p) const;
  void print() const;
  void operator=(const HomographyModel& o);
  HomographyModel inverse();
  cv::Mat toMat();
 private:
  Eigen::Matrix3d _matrix;
};


// The MIT License
// (c) 2019 Daniel Williams

#include <memory>
#include <string>
#include <iostream>

#include <Eigen/Geometry>

#include "geometry/cell_value.hpp"
#include "geometry/sample.hpp"

using std::cerr;
using std::endl;
using std::make_unique;
using std::string;
using std::unique_ptr;

using Eigen::Vector2d;

namespace terrain::geometry {

cell_value_t interpolate_linear(const Eigen::Vector2d& to, const Sample& s1, const Sample& s2){
    if(s1.at.isApprox(s2.at)){
        return s1.is;
    }

    // distances from query point to each interpolation point
    const double dist1 = (s1.at - to).norm();
    const double dist2 = (s2.at - to).norm();

    // If the point is farther than from a point than the distance between the two interpolation points,
    //     return the value at one of the end-points.
    // This is not perfect, but it's a reasonable heuristic.
    // ... in particular, it will return odd values at large distances
    // ... arguably, this should return a NAN value instead -- for not-applicable
    const double dist12 = (s1.at - s2.at).norm();
    if(dist12 < dist1){
        return s2.is;
    }else if( dist12 < dist2){
        return s1.is;
    }

    const double combined_distance = dist1 + dist2;
    const double normdist1 = 1 - dist1 / combined_distance;
    const double normdist2 = 1 - dist2 / combined_distance;
    const double interp_value = (normdist1*s1.is + normdist2*s2.is);

    return round(interp_value);
}

cell_value_t interpolate_bilinear(  const Eigen::Vector2d& to,
                                    const Sample& ne,
                                    const Sample& nw,
                                    const Sample& sw,
                                    const Sample& se)
{
    // cerr << "    ==>>  to:    @" << to[0] << ", " << to[1] << endl;
    // cerr << "        - NE:     " << ne.at[0] << ", " << ne.at[1] << " = " << (int)ne.is << endl;
    // cerr << "        - NW:     " << nw.at[0] << ", " << nw.at[1] << " = " << (int)nw.is << endl;
    // cerr << "        - SW:     " << sw.at[0] << ", " << sw.at[1] << " = " << (int)sw.is << endl;
    // cerr << "        - SE:     " << se.at[0] << ", " << se.at[1] << " = " << (int)se.is << endl;

    // not necessary ?
    // // test for degenerate cases:
    // if( &sx == &sd ){
    //     // top or bottom border
    //     return interpolate_linear(to, {to[0], xn.at[1]}, xn);
    // }else if( &sy  == &sd ){
    //     // left or right border
    //     return interpolate_linear({yn[0], to[1]}, yn);
    // }

    // calculate full bilinear interpolation:
    const Eigen::Vector2d upper_point = {to[0], (nw.at[1] + ne.at[1])/2};
    const Sample upper_sample = { upper_point,
                                  interpolate_linear(upper_point, nw, ne)};

    const Eigen::Vector2d lower_point = {to[0], (sw.at[1] + se.at[1])/2};
    const Sample lower_sample = { lower_point,
                                interpolate_linear(lower_point, sw, se) };

    // cerr << "        ::Upper:    " << upper_point[0] << ", " << upper_point[1] << " = " << (int)upper_sample.is << endl;
    // cerr << "        ::Lower:    " << lower_point[0] << ", " << lower_point[1] << " = " << (int)lower_sample.is << endl;

    return interpolate_linear(to, upper_sample, lower_sample);
}

} // namespace terrain::geometry


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "../src/utils.h"

// Use either Eigen or Diospyros RQ routine.
#ifdef DIOS
#include "dios_rq_decomposition.h"
static const char *BACKEND = "Dios";
#else
#include "rq_decomposition.h"
static const char *BACKEND = "Eigen";
#endif

using namespace theia;
using Eigen::Matrix3f;
using Eigen::Vector3f;

// Copied from `util.cc` in Theia! --AS
// Projects a 3x3 matrix to the rotation matrix in SO3 space with the closest
// Frobenius norm. For a matrix with an SVD decomposition M = USV, the nearest
// rotation matrix is R = UV'.
Matrix3f ProjectToRotationMatrix(const Matrix3f& matrix) {
  Eigen::JacobiSVD<Matrix3f> svd(matrix,
                                 Eigen::ComputeFullU | Eigen::ComputeFullV);
  Matrix3f rotation_mat = svd.matrixU() * (svd.matrixV().transpose());

  // The above projection will give a matrix with a determinant +1 or -1. Valid
  // rotation matrices have a determinant of +1.
  if (rotation_mat.determinant() < 0) {
    rotation_mat *= -1.0;
  }

  return rotation_mat;
}

// Used as the projection matrix type.
typedef Eigen::Matrix<float, 3, 4> Matrix3x4f;

// Generate a calibration matrix from some parameters.
void IntrinsicsToCalibrationMatrix(const float focal_length,
                                   const float skew,
                                   const float aspect_ratio,
                                   const float principal_point_x,
                                   const float principal_point_y,
                                   Matrix3f* calibration_matrix) {
  *calibration_matrix <<
      focal_length, skew, principal_point_x,
      0, focal_length * aspect_ratio, principal_point_y,
      0, 0, 1.0;
}

// Generate a projection matrix from a rotation and position.
bool ComposeProjectionMatrix(const Matrix3f& calibration_matrix,
                             const Vector3f& rotation,
                             const Vector3f& position,
                             Matrix3x4f* pmatrix) {
  const double rotation_angle = rotation.norm();
  if (rotation_angle == 0) {
    pmatrix->block<3, 3>(0, 0) = Matrix3f::Identity();
  } else {
    pmatrix->block<3, 3>(0, 0) = Eigen::AngleAxisf(
        rotation_angle, rotation / rotation_angle).toRotationMatrix();
  }

  pmatrix->col(3) = - (pmatrix->block<3, 3>(0, 0) *  position);
  *pmatrix = calibration_matrix * (*pmatrix);
  return true;
}

bool DecomposeProjectionMatrix(const Matrix3x4f pmatrix,
                               Eigen::Matrix3f* calibration_matrix,
                               Eigen::Vector3f* rotation,
                               Eigen::Vector3f* position) {

#ifdef DIOS
  RQDecomposition rq(pmatrix.block<3, 3>(0, 0));
#else
  RQDecomposition<Eigen::Matrix3f> rq(pmatrix.block<3, 3>(0, 0));
#endif

  Eigen::Matrix3f rotation_matrix = ProjectToRotationMatrix(rq.matrixQ());

  const float k_det = rq.matrixR().determinant();
  if (k_det == 0) {
    return false;
  }

  Eigen::Matrix3f& kmatrix = *calibration_matrix;
  if (k_det > 0) {
    kmatrix = rq.matrixR();
  } else {
    kmatrix = -rq.matrixR();
  }

  // Fix the matrix such that all internal parameters are greater than 0.
  for (int i = 0; i < 3; ++i) {
    if (kmatrix(i, i) < 0) {
      kmatrix.col(i) *= -1.0;
      rotation_matrix.row(i) *= -1.0;
    }
  }

  // Solve for t.
  const Eigen::Vector3f t =
      kmatrix.triangularView<Eigen::Upper>().solve(pmatrix.col(3));

  // c = - R' * t, and flip the sign according to k_det;
  if (k_det > 0) {
    *position = - rotation_matrix.transpose() * t;
  } else {
    *position = rotation_matrix.transpose() * t;
  }

  const Eigen::AngleAxisf rotation_aa(rotation_matrix);
  *rotation = rotation_aa.angle() * rotation_aa.axis();

  return true;
}

int main() {
  int time = 0;

  // Create a projection matrix from "raw materials."
  Matrix3f in_calibration;
  IntrinsicsToCalibrationMatrix(0.3, 0.2, 0.4, 0.5, 0.6, &in_calibration);
  Vector3f in_rotation;
  in_rotation << 0.3, 0.7, 0.2;
  Vector3f in_position;
  in_position << 0.9, 0.1, 0.6;
  Matrix3x4f random_pmatrix;
  ComposeProjectionMatrix(in_calibration, in_rotation, in_position, &random_pmatrix);

  Matrix3f calibration_matrix;
  Vector3f rotation, position;

  start_cycle_timing;
  bool success = DecomposeProjectionMatrix(random_pmatrix,
                                           &calibration_matrix,
                                           &rotation,
                                           &position);
  stop_cycle_timing;
  time = get_time();
  printf("DecomposeProjectionMatrix - %s: %d cycles\n", BACKEND, time);

  std::cout << "Input:" << std::endl << random_pmatrix << std::endl;
  std::cout << "Rotation:" << std::endl << rotation << std::endl;
  std::cout << "Position:" << std::endl << position << std::endl;
  return 0;
}


#pragma once
#include <iostream>
#include <string>
#include <cmath>
#include <limits>
#include <unordered_map>
#include <boost/algorithm/string.hpp>
#include <xtensor/xarray.hpp>
#include <xtensor/xmath.hpp>

using namespace std;

double compute_probability_from_distance(const double distance, const bool left_tail) {
	double prob;
	if (left_tail) {
		prob = (double) 1.0 - distance;
	} else {
		prob = distance;
	}
	if (isnan(prob)) {
		throw runtime_error("compute_probability_from_distance: NaN values encountered");
	}
	return prob;
}

xt::xarray<double> compute_probabilities_from_distance(const vector<double>& distances, const bool left_tail) {
	xt::xarray<double> probabilities = xt::zeros<double>({distances.size()});
	for (int i = 0; i < distances.size(); ++i) {
		probabilities[i] = compute_probability_from_distance(distances[i], left_tail);
	}
	return probabilities;
}

xt::xarray<double> compute_baseline_probabilities(const xt::xarray<double>& probabilities) {
	xt::xarray<double> baseline = xt::zeros<double>({probabilities.size()});
	double mean = xt::mean(probabilities)();
	for (int j = 0; j < probabilities.size(); ++j) {
		baseline[j] = mean;
	}
	return baseline;
}

class GeneProbabilies {
	vector<string> keys;
	unordered_map<string, size_t> lookup;
	xt::xarray<double> probabilities;
	xt::xarray<double> random_probabilities;

	public:
		GeneProbabilies(istream& is, const string& tail) :keys{}, lookup{} {
			if (tail != "left" && tail != "right") {
				throw runtime_error(
					"GeneProbabilies: tail argument must be one of left, right "
					"but got: " + tail
				);
			}
			string line;
			int i = 0;
			vector<double> distances;
			while (getline(is, line)) {
				if (i == 0 || line.empty()) {
					++i;
					continue;
				}
				vector<string> elements;
				boost::split(elements, line, boost::is_any_of(","));
				if (elements.size() != 2) {
					throw runtime_error(
						"GeneProbabilies: 2 columns expected "
						"but got: " + to_string(elements.size())
					);
				}
				string key = elements[0];
				keys.push_back(key);
				lookup[key] = i-1;

				double distance = stod(elements[1]);
				distances.push_back(distance);
				++i;
			}

			if (distances.empty()) {
				throw runtime_error("GeneProbabilies: no data");
			}

			probabilities = compute_probabilities_from_distance(distances, tail == "left");
			random_probabilities = compute_baseline_probabilities(probabilities);
		}

		double operator[](const string& id) {
			return probabilities[lookup[id]];
		}

		double Get(const string& id, const bool random = false) {
			if (!random) {
				return probabilities[lookup[id]];
			} else {
				return random_probabilities[lookup[id]];
			}
		}

		size_t IndexOf(const string& id) {
			return lookup[id];
		}

		vector<string>& Keys() {
			return keys;
		}

		xt::xarray<double>& Values() {
			return probabilities;
		}

		xt::xarray<double>& RandomValues() {
			return random_probabilities;
		}

		size_t size() {
			return lookup.size();
		}
};

xt::xarray<double> make_uniform_prior(const size_t n_elements) {
	return xt::ones<double>({n_elements}) / n_elements;
}

xt::xarray<double> make_uniform_prior(const size_t n_rows, const size_t n_cols) {
	return xt::ones<double>({n_rows, n_cols}) / n_cols;
}

double normalize_probability(const double& probability, const size_t& n_elements) {
	return exp(log(probability) / n_elements);
}


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
#include <vector>
#include <cmath>
#include <iostream>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

//return f s.t. q^f<M<=q^(f+1)
unsigned long get_f(unsigned long q, unsigned long M);
vector<unsigned long> generate_primes(unsigned long MAX);

string PminusOneFactorizer_step1_cppfunc(string s, unsigned long M){
    Bint n(s);
    Bint A("2");
    if(n%A==0){
        return A.str();
    }
    for(unsigned long p : generate_primes(M)){
        unsigned long f = get_f(p, M);
        Bint Mp = pow(Bint(p), f);
        A = powm(A, Mp, n);
    }
    Bint retval = gcd(A-1, n);
    return retval.str();
}

string PminusOneFactorizer_step2_cppfunc(string s, unsigned long M, unsigned long L){
    Bint n(s);
    Bint A("2");
    if(n%A==0){
        return A.str();
    }
    for(unsigned long p : generate_primes(L)){
        if(p<M){
            unsigned long f = get_f(p, M);
            Bint Mp = pow(Bint(p), f);
            A = powm(A, Mp, n);
        }
        else{
            Bint AK = powm(A, p, n);
            Bint candidate = gcd(AK-1, n);
            if(1<candidate and candidate<n){
                return candidate.str();
            }
        }
    }
    return "1";
}

unsigned long get_f(unsigned long q, unsigned long M){
    unsigned long f = log(M)/log(q);
    if(pow(q,f)==M){
        return f-1;
    }
    else{
        assert(pow(q,f)<M and M<=pow(q,f+1));
        return f;
    }
}

vector<unsigned long> generate_primes(unsigned long MAX){ // we have to make this more smart...
    vector<bool> v(MAX+1, true);
    v.at(0) = false;
    v.at(1) = false;
    vector<unsigned long> retval;
    retval.clear();
    unsigned long sqrt_MAX = (unsigned long)sqrt(MAX);
    for(unsigned long i=2;i<=MAX;i++){
        if(!v.at(i)){
            continue;
        }
        retval.push_back(i);
        if(i>sqrt_MAX+1){continue;}
        for(unsigned long k=2;k*i<=MAX;k++){
            v.at(k*i) = false;
        }
    }
    return retval;
}


//==============================================================================
//         Copyright 2011-2014     Karsten Ahnert
//         Copyright 2011-2014     Mario Mulansky
//         Copyright 2014          LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2014          NumScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <iostream>
#include <utility>

#include <boost/numeric/odeint.hpp>

#ifndef M_PI //not there on windows
#define M_PI 3.141592653589793 //...
#endif

#include <boost/random.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#include <nt2/include/functions/cos.hpp>
#include <nt2/include/functions/sin.hpp>
#include <nt2/include/functions/atan2.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/sum.hpp>
#include <nt2/include/functions/mean.hpp>
#include <nt2/arithmetic/include/functions/hypot.hpp>
#include <nt2/include/functions/tie.hpp>

#include <boost/numeric/odeint/external/nt2/nt2_algebra_dispatcher.hpp>

using namespace std;
using namespace boost::numeric::odeint;

template <typename container_type, typename T>
pair< T, T > calc_mean_field( const container_type &x )

{
  T cos_sum = 0.0 , sin_sum = 0.0;

  nt2::tie(cos_sum,sin_sum) = nt2::tie(nt2::mean( nt2::cos(x) ), nt2::mean( nt2::sin(x) ));

  T K = nt2::hypot(sin_sum,cos_sum);
  T Theta = nt2::atan2( sin_sum , cos_sum );

  return make_pair( K , Theta );
}

template <typename container_type, typename T>
struct phase_ensemble
{
  typedef typename boost::dispatch::meta::as_integer<T,unsigned>::type int_type;
  container_type m_omega;
  T m_epsilon;

  phase_ensemble( const int_type n , T g = 1.0 , T epsilon = 1.0 )
  : m_epsilon( epsilon )
  {
    m_omega = nt2::zeros(nt2::of_size(n), nt2::meta::as_<T>());
    create_frequencies( g );
  }

  void create_frequencies( T g )
  {
    boost::mt19937 rng;
    boost::cauchy_distribution<> cauchy( 0.0 , g );
    boost::variate_generator< boost::mt19937&, boost::cauchy_distribution<> > gen( rng , cauchy );
    generate( m_omega.begin() , m_omega.end() , gen );
}

  void set_epsilon( T epsilon ) { m_epsilon = epsilon; }

  T get_epsilon( void ) const { return m_epsilon; }

  void operator()( const container_type &x , container_type &dxdt , T ) const
  {
    pair< T, T > mean = calc_mean_field<container_type,T>( x );
    dxdt = m_omega +  m_epsilon * mean.first * nt2::sin( mean.second - x );
  }
};

template<typename T>
struct statistics_observer
{
    typedef typename boost::dispatch::meta::as_integer<T,unsigned>::type int_type;
    T m_K_mean;
    int_type m_count;

    statistics_observer( void )
    : m_K_mean( 0.0 ) , m_count( 0 ) { }

    template< class State >
    void operator()( const State &x , T t )
    {
        pair< T, T > mean = calc_mean_field<State,T>( x );
        m_K_mean += mean.first;
        ++m_count;
    }

    T get_K_mean( void ) const { return ( m_count != 0 ) ? m_K_mean / T( m_count ) : 0.0 ; }

    void reset( void ) { m_K_mean = 0.0; m_count = 0; }
};

template<typename T>
struct test_ode_table
{
  typedef nt2::table<T> array_type;
  typedef void experiment_is_immutable;

  typedef typename boost::dispatch::meta::as_integer<T,unsigned>::type int_type;

  test_ode_table ( )
                 : size_(16384), ensemble( size_ , 1.0 ), unif( 0.0 , 2.0 * M_PI ), gen( rng , unif ), obs()
  {
    x.resize(nt2::of_size(size_));
  }

  void operator()()
  {
    for( T epsilon = 0.0 ; epsilon < 5.0 ; epsilon += 0.1 )
    {
      ensemble.set_epsilon( epsilon );
      obs.reset();

      // start with random initial conditions
      generate( x.begin() , x.end() , gen );
      // calculate some transients steps
      integrate_const( runge_kutta4< array_type, T >() , boost::ref( ensemble ) , x , T(0.0) , T(10.0) , dt );

      // integrate and compute the statistics
      integrate_const( runge_kutta4< array_type, T >() , boost::ref( ensemble ) , x , T(0.0) , T(100.0) , dt , boost::ref( obs ) );
      cout << epsilon << "\t" << obs.get_K_mean() << endl;
    }
  }

  friend std::ostream& operator<<(std::ostream& os, test_ode_table<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }

  private:
    std::size_t size_;
    phase_ensemble<array_type,T> ensemble;
    boost::uniform_real<> unif;
    array_type x;
    boost::mt19937 rng;
    boost::variate_generator< boost::mt19937&, boost::uniform_real<> > gen;
    statistics_observer<T> obs;

    static const T dt = 0.1;
};

int main()
{
  std::cout<< " With T = [double] \n";
  test_ode_table<double> test_double;
  test_double();

  std::cout<< " With T = [float] \n";
  test_ode_table<float> test_float;
  test_float();
}


//
// SpecialFunctions.hpp
//
// Copyright (c) 2020 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//
#include<iostream>
#include<Eigen/Dense>
#include<Eigen/Core>
#include <unsupported/Eigen/SpecialFunctions>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include"utils.hpp"

double normalLogPdf(const Eigen::VectorXd &y, const Eigen::VectorXd &mu, double sigma2);

double normalLogPdf(const Eigen::VectorXd &y, const Eigen::VectorXd &mu, const Eigen::MatrixXd &covMatrix);

double normalPdf(const Eigen::VectorXd &y, const Eigen::VectorXd &mu, double sigma2);

double normalPdf(const Eigen::VectorXd &y, const Eigen::VectorXd &mu, const Eigen::MatrixXd &covMatrix);


#include <iostream>

#include <Eigen/Core>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

using namespace fd;

TEST_CASE("Test finite difference jacobian of linear", "[jacobian]")
{
    int n = GENERATE(1, 2, 4, 10, 100);

    // f(x) = Ax
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(n, n);

    const auto f = [&](const Eigen::VectorXd x) -> Eigen::VectorXd {
        return A * x;
    };

    Eigen::VectorXd x = Eigen::VectorXd::Random(n);

    Eigen::MatrixXd jac = A;

    AccuracyOrder accuracy = GENERATE(SECOND, FOURTH, SIXTH, EIGHTH);

    Eigen::MatrixXd fjac;
    finite_jacobian(x, f, fjac, accuracy);

    CHECK(compare_jacobian(jac, fjac));
}

TEST_CASE("Test finite difference jacobian of trig", "[jacobian]")
{
    int n = GENERATE(1, 2, 4, 10, 100);

    const auto f = [&](const Eigen::VectorXd x) -> Eigen::VectorXd {
        return x.array().sin();
    };

    Eigen::VectorXd x = Eigen::VectorXd::Random(n);

    Eigen::MatrixXd jac = x.array().cos().matrix().asDiagonal();

    AccuracyOrder accuracy = GENERATE(SECOND, FOURTH, SIXTH, EIGHTH);

    Eigen::MatrixXd fjac;
    finite_jacobian(x, f, fjac, accuracy);

    CHECK(compare_jacobian(jac, fjac));
}


// Boost.Geometry

// Copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// 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_FORMULAS_SPHERICAL_HPP
#define BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP

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

//#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry {

namespace formula {

template <typename T>
struct result_spherical
{
    result_spherical()
        : azimuth(0)
        , reverse_azimuth(0)
    {}

    T azimuth;
    T reverse_azimuth;
};

template <typename T>
static inline void sph_to_cart3d(T const& lon, T const& lat, T & x, T & y, T & z)
{
    T const cos_lat = cos(lat);
    x = cos_lat * cos(lon);
    y = cos_lat * sin(lon);
    z = sin(lat);
}

template <typename Point3d, typename PointSph>
static inline Point3d sph_to_cart3d(PointSph const& point_sph)
{
    typedef typename coordinate_type<Point3d>::type calc_t;

    calc_t const lon = get_as_radian<0>(point_sph);
    calc_t const lat = get_as_radian<1>(point_sph);
    calc_t x, y, z;
    sph_to_cart3d(lon, lat, x, y, z);

    Point3d res;
    set<0>(res, x);
    set<1>(res, y);
    set<2>(res, z);

    return res;
}

template <typename T>
static inline void cart3d_to_sph(T const& x, T const& y, T const& z, T & lon, T & lat)
{
    lon = atan2(y, x);
    lat = asin(z);
}

template <typename PointSph, typename Point3d>
static inline PointSph cart3d_to_sph(Point3d const& point_3d)
{
    typedef typename coordinate_type<PointSph>::type coord_t;
    typedef typename coordinate_type<Point3d>::type calc_t;

    calc_t const x = get<0>(point_3d);
    calc_t const y = get<1>(point_3d);
    calc_t const z = get<2>(point_3d);
    calc_t lonr, latr;
    cart3d_to_sph(x, y, z, lonr, latr);

    PointSph res;
    set_from_radian<0>(res, lonr);
    set_from_radian<1>(res, latr);

    coord_t lon = get<0>(res);
    coord_t lat = get<1>(res);

    math::normalize_spheroidal_coordinates
        <
            typename coordinate_system<PointSph>::type::units,
            coord_t
        >(lon, lat);

    set<0>(res, lon);
    set<1>(res, lat);

    return res;
}

// -1 right
// 1 left
// 0 on
template <typename Point3d1, typename Point3d2>
static inline int sph_side_value(Point3d1 const& norm, Point3d2 const& pt)
{
    typedef typename select_coordinate_type<Point3d1, Point3d2>::type calc_t;
    calc_t c0 = 0;
    calc_t d = dot_product(norm, pt);
    return math::equals(d, c0) ? 0
        : d > c0 ? 1
        : -1; // d < 0
}

template <typename CT, bool ReverseAzimuth, typename T1, typename T2>
static inline result_spherical<CT> spherical_azimuth(T1 const& lon1,
                                                     T1 const& lat1,
                                                     T2 const& lon2,
                                                     T2 const& lat2)
{
    typedef result_spherical<CT> result_type;
    result_type result;

    // http://williams.best.vwh.net/avform.htm#Crs
    // https://en.wikipedia.org/wiki/Great-circle_navigation
    CT dlon = lon2 - lon1;

    // An optimization which should kick in often for Boxes
    //if ( math::equals(dlon, ReturnType(0)) )
    //if ( get<0>(p1) == get<0>(p2) )
    //{
    //    return - sin(get_as_radian<1>(p1)) * cos_p2lat);
    //}

    CT const cos_dlon = cos(dlon);
    CT const sin_dlon = sin(dlon);
    CT const cos_lat1 = cos(lat1);
    CT const cos_lat2 = cos(lat2);
    CT const sin_lat1 = sin(lat1);
    CT const sin_lat2 = sin(lat2);

    {
        // "An alternative formula, not requiring the pre-computation of d"
        // In the formula below dlon is used as "d"
        CT const y = sin_dlon * cos_lat2;
        CT const x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
        result.azimuth = atan2(y, x);
    }

    if (ReverseAzimuth)
    {
        CT const y = sin_dlon * cos_lat1;
        CT const x = sin_lat2 * cos_lat1 * cos_dlon - cos_lat2 * sin_lat1;
        result.reverse_azimuth = atan2(y, x);
    }

    return result;
}

template <typename ReturnType, typename T1, typename T2>
inline ReturnType spherical_azimuth(T1 const& lon1, T1 const& lat1,
                                    T2 const& lon2, T2 const& lat2)
{
    return spherical_azimuth<ReturnType, false>(lon1, lat1, lon2, lat2).azimuth;
}

template <typename T>
inline T spherical_azimuth(T const& lon1, T const& lat1, T const& lon2, T const& lat2)
{
    return spherical_azimuth<T, false>(lon1, lat1, lon2, lat2).azimuth;
}

template <typename T>
inline int azimuth_side_value(T const& azi_a1_p, T const& azi_a1_a2)
{
    T const pi = math::pi<T>();
    T const two_pi = math::two_pi<T>();

    // instead of the formula from XTD
    //calc_t a_diff = asin(sin(azi_a1_p - azi_a1_a2));

    T a_diff = azi_a1_p - azi_a1_a2;
    // normalize, angle in [-pi, pi]
    while (a_diff > pi)
        a_diff -= two_pi;
    while (a_diff < -pi)
        a_diff += two_pi;

    // NOTE: in general it shouldn't be required to support the pi/-pi case
    // because in non-cartesian systems it makes sense to check the side
    // only "between" the endpoints.
    // However currently the winding strategy calls the side strategy
    // for vertical segments to check if the point is "between the endpoints.
    // This could be avoided since the side strategy is not required for that
    // because meridian is the shortest path. So a difference of
    // longitudes would be sufficient (of course normalized to [-pi, pi]).

    // NOTE: with the above said, the pi/-pi check is temporary
    // however in case if this was required
    // the geodesics on ellipsoid aren't "symmetrical"
    // therefore instead of comparing a_diff to pi and -pi
    // one should probably use inverse azimuths and compare
    // the difference to 0 as well

    // positive azimuth is on the right side
    return math::equals(a_diff, 0)
        || math::equals(a_diff, pi)
        || math::equals(a_diff, -pi) ? 0
        : a_diff > 0 ? -1 // right
        : 1; // left
}

} // namespace formula

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2018 - 2021.                 //
//  Distributed under the Boost Software License,                //
//  Version 1.0. (See accompanying file LICENSE_1_0.txt          //
//  or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////

// This Miller-Rabin primality test is loosely based on
// an adaptation of some code from Boost.Multiprecision.
// The Boost.Multiprecision code can be found here:
// https://www.boost.org/doc/libs/1_76_0/libs/multiprecision/doc/html/boost_multiprecision/tut/primetest.html

#include <ctime>
#include <random>

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#if (defined(__clang__) && (__clang_major__ > 9)) && !defined(__APPLE__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif

#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/multiprecision/uintwide_t_backend.hpp>

#include <examples/example_uintwide_t.h>

bool math::wide_integer::example008a_miller_rabin_prime()
{
  using wide_integer_type = boost::multiprecision::number<boost::multiprecision::uintwide_t_backend<512U>,
                                                          boost::multiprecision::et_off>;

  boost::random::mt11213b base_gen(static_cast<typename boost::random::mt11213b::result_type>(std::clock()));

  boost::random::independent_bits_engine<boost::random::mt11213b,
                                         std::numeric_limits<wide_integer_type>::digits,
                                         wide_integer_type>
  gen(base_gen);

  using random_engine2_type =
    std::linear_congruential_engine<std::uint32_t, UINT32_C(48271), UINT32_C(0), UINT32_C(2147483647)>;

  random_engine2_type gen2(static_cast<typename random_engine2_type::result_type>(std::clock()));

  wide_integer_type p0;
  wide_integer_type p1;

  for(;;)
  {
    p0 = gen();

    const bool miller_rabin_result = boost::multiprecision::miller_rabin_test(p0, 25U, gen2);

    if(miller_rabin_result)
    {
      break;
    }
  }

  for(;;)
  {
    p1 = gen();

    const bool miller_rabin_result = boost::multiprecision::miller_rabin_test(p1, 25U, gen2);

    if(miller_rabin_result)
    {
      break;
    }
  }

  const wide_integer_type gd = gcd(p0, p1);

  const bool result_is_ok = (   (p0 != 0U)
                             && (p1 != 0U)
                             && (p0 != p1)
                             && (gd == 1U));

  return result_is_ok;
}

// Enable this if you would like to activate this main() as a standalone example.
#if 0

#include <iomanip>
#include <iostream>

int main()
{
  const bool result_is_ok = wide_integer::example008a_miller_rabin_prime();

  std::cout << "result_is_ok: " << std::boolalpha << result_is_ok << std::endl;
}

#endif

#if (defined(__clang__) && (__clang_major__ > 9)) && !defined(__APPLE__)
#pragma GCC diagnostic pop
#endif

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#endif


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include <sstream>
#include <unordered_map>
#include <map>
#include <limits>
using namespace std;

typedef boost::multiprecision::checked_cpp_int bigint;

namespace std {
  template<> struct hash<bigint> {
    size_t operator()(const bigint& x) const {
      auto mask = bigint(numeric_limits<size_t>::max());
      return size_t(bigint(x & mask));
    }
  };
} // namespace std

// Solve h = g^x [mod p], using a meet-in-the-middle attack
int main(int argc, char** argv) {
  if (argc <= 4) {
    cerr << "Not enough arguments, usage: ./middle p g h xmax\n"
         << "computes (x) s.t. h = g^x [mod p] | 0 < x < xmax" << endl;
  }

  // read in the parameters of the problem
  auto p = bigint(), g = bigint(), h = bigint();
  auto xmax = uint64_t();
  istringstream(argv[1]) >> p;
  istringstream(argv[2]) >> g;
  istringstream(argv[3]) >> h;
  istringstream(argv[4]) >> xmax;
  cerr << "# p=" << p << "\n# g=" << g << "\n# h=" << h << "\n# xmax=" << xmax << endl;

  // solve it
  const auto b = uint64_t(sqrt(xmax));
  const auto gb = powm(g, b, p);
  cerr << "# b=" << b << "\n# gb=" << gb << endl;

  // // build a table of the lefts
  auto lefts = unordered_map<bigint, uint64_t>();
  for (uint64_t x1 = 0; x1 <= b; ++x1) {
    auto gx1 = bigint(powm(g, x1, p));
    auto lhs = (h * bigint(powm(gx1, p-2, p))) % p;
    // cerr << lhs << endl;
    lefts.insert(make_pair(lhs, x1));
  }
  // // search for a matching right
  for (uint64_t x0 = 0; x0 <= b; ++x0) {
    auto rhs = powm(gb, x0, p);
    auto it = lefts.find(rhs);
    if (it != lefts.end()) {
      auto x1 = it->second;
      auto x = x0 * b + x1;
      cout << x << endl;
      cerr << "# Check: " << (powm(g, x, p) == h ? "OK" : "Not OK") << endl;
      return 0;
    }
  }
  cerr << "# No solution found" << endl;
  return 1;
}


//	Objective: To implement the option class that is defined in the header file: CashOrNothingOption.hpp
//
// (c) Sudhansh Dua

#include "CashOrNothingOption.hpp"
#include <string>
#include <boost/math/distributions.hpp>
#include <cmath>

using namespace std;
using namespace boost::math;

double CashOrNothingOption::CallPrice() const
{
	return ::CashOrNothingCallPrice(S, K, cr, T, r, sig, b, type);
}

double CashOrNothingOption::PutPrice() const
{
	return ::CashOrNothingPutPrice(S, K, cr, T, r, sig, b, type);
}

void CashOrNothingOption::init()					// Initialising all the default values
{
	//	Default values
	T = 1;
	cr = 10;
	r = 0.08;
	sig = 0.25;
	K = 100;
	S = 100;			//	Default stock price
	b = r;				//	Black - Scholes(1973) stock option model : b = r

	type = "C";			//	Call option as the default

}

void CashOrNothingOption::copy(const CashOrNothingOption& option)
{
	cr = option.cr;
	T = option.T;
	r = option.r;
	sig = option.sig;
	K = option.K;
	b = option.b;
	type = option.type;
	S = option.S;
}

//	Constructors and destructor
//	Default Constructor
CashOrNothingOption::CashOrNothingOption() : Option()
{
	init();
}

//	Copy constructor
CashOrNothingOption::CashOrNothingOption(const CashOrNothingOption& option) : Option(option)
{
	copy(option);
}

//	Constructor that accepts values
CashOrNothingOption::CashOrNothingOption(const double& S1, const double& K1, const double& cr1, const double& T1, const double& r1, const double& sig1,
	const double& b1, const string type1) : Option(), S(S1), K(K1), cr(cr1), T(T1), r(r1), sig(sig1), b(b1), type(type1) {}

//	Destructor
CashOrNothingOption::~CashOrNothingOption() {}

//	Assignment Operator
CashOrNothingOption& CashOrNothingOption::operator = (const CashOrNothingOption& option)
{
	if (this == &option)
	{
		return *this;		//	Self-assignment check!
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

// Functions that calculate the option price
double CashOrNothingOption::Price() const
{
	if (type == "C")
	{
		return CallPrice();
	}
	else
	{
		return PutPrice();
	}
}

// Modifier functions
void CashOrNothingOption::toggle()								//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

// Global Functions
double CashOrNothingCallPrice(const double S, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type)
{
	double d = (log(S / K) + (b - (sig * sig * 0.5)) * T) / (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return (cr * exp(-r * T) * cdf(standard_normal, d));
}

double CashOrNothingPutPrice(const double S, const double K, const double cr, const double T, const double r, const double sig, const double b, const string type)
{
	double d = (log(S / K) + (b - (sig * sig * 0.5)) * T) / (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return (cr * exp(-r * T) * cdf(standard_normal, -d));
}


#include <iostream>
#include <armadillo>
#include <numeric>

#include "LinearCodeGenerator.h"
#include "Math.h"
#include <gurobi_c++.h>

template<class T, class I1, class I2>
T hamming_distance(uint32_t size, I1 b1, I2 b2)
{
	return std::inner_product(b1, b1 + size, b2, T{},
		std::plus<T>(), std::not_equal_to<T>());
}

int getMinHammingDistance(const int q, const int k, const arma::s32_mat &G)
{
	auto language = Math::generateLanguage(q, k);
	language.erase(language.begin());

	std::vector<arma::s32_vec> codes;
	codes.reserve(language.size());
	for (const auto& word : language)
	{
		codes.push_back(Math::encode(word, G, q));
	}

	auto minDistance = std::numeric_limits<int>::max();
	for (auto& codeword : codes)
	{
		for (auto& codeword2 : codes)
		{
			const auto distance = hamming_distance<int>(codeword.size(), codeword.begin(), codeword2.begin());
			if (distance > 0 && distance < minDistance)
			{
				minDistance = distance;
			}
		}
	}

	return minDistance;
}

int main()
{
	/* Parameters */
	constexpr int q = 7;
	constexpr int k = 3;
	constexpr int b = 3;

	std::cout << "###### Linear Code Creator ######" << std::endl << std::endl;
	std::cout << "q = " << q << std::endl << "k = " << k << std::endl << "b = " << b << std::endl;

	const LinearCodeGenerator linCodeGenerator(q, k, b);

	try
	{
		const auto linCode = linCodeGenerator.generateCode();

		std::cout << "We have now following code" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		std::cout << "[n,k,d]_q" << std::endl;
		std::cout << "[" << linCode.n << "," << linCode.k << "," << linCode.d << "]_" << linCode.q << "" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl << std::endl;
		std::cout << "Generatormatrix G" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		linCode.G.raw_print();
		std::cout << "----------------------------------------------------" << std::endl << std::endl;

		std::cout << "Proof Hamming Distance" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl;
		const int minDistance = getMinHammingDistance(q, k, linCode.G);
		std::cout << minDistance << " = calculated distance" << std::endl;
		std::cout << linCode.d << " = should be the distance" << std::endl;
		std::cout << "----------------------------------------------------" << std::endl << std::endl;
	}
	catch (const GRBException& e) {
		std::cout << "Error code = " << e.getErrorCode() << std::endl;
		std::cout << e.getMessage() << std::endl;
	}
	catch (...) {
		std::cout << "Exception during optimization" << std::endl;
	}

	std::cout << "Press any key to exit...";
	std::cin.get();

	return 0;
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Geomlib_BestFitPlane.h"

#include <assert.h>

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

namespace {

Eigen::Vector3f ComputeCentroid(const Eigen::MatrixXf& V)
{
	assert(V.rows() == 3);

	Eigen::Vector3f c(0.0f, 0.0f, 0.0f);

	for (int i = 0; i < V.cols(); ++i) {
		c += V.col(i);
	}

	if (V.cols() != 0) {
		float sc = (1.0f / V.cols());
		c *= sc;
	}

	return c;
}

} // namespace

void Geomlib::BestFitPlane(
	const Urho3D::Vector<Urho3D::Vector3>& point_cloud,
	Urho3D::Vector3& point,
	Urho3D::Vector3& normal
)
{
	using Urho3D::Vector3;

	int num_vertices = (int)point_cloud.Size();
	if (num_vertices <= 0) {
		return;
	}

	Eigen::MatrixXf V(3, num_vertices);
	for (int i = 0; i < num_vertices; ++i) {
		Vector3 vec = point_cloud[i];
		V.col(i) = Eigen::Vector3f(vec.x_, vec.y_, vec.z_);
	}

	Eigen::Vector3f c = ComputeCentroid(V);

	Eigen::MatrixXf NV = V;
	for (int i = 0; i < NV.cols(); ++i) {
		NV.col(i) -= c;
	}

	Eigen::JacobiSVD<Eigen::MatrixXf> svd(NV, Eigen::DecompositionOptions::ComputeThinU);

	Eigen::MatrixXf U = svd.matrixU();
	assert(U.rows() == 3 && U.cols() == 3);

	normal = Vector3(U(0, 2), U(1, 2), U(2, 2));
	point = Vector3(c(0), c(1), c(2));
}

void Geomlib::BestFitPlane(
	const Urho3D::VariantVector& vertex_list,
	Urho3D::Vector3& point,
	Urho3D::Vector3& normal
)
{
	using Urho3D::Vector;
	using Urho3D::Vector3;

	Vector<Vector3> point_cloud;
	for (int i = 0; i < vertex_list.Size(); ++i) {
		point_cloud.Push(vertex_list[i].GetVector3());
	}

	BestFitPlane(point_cloud, point, normal);
}


/*
 * generate_data.cpp
 * Date: 2016-02-26
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include "generate_data.hpp"

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>

#include <iostream>
#include <cmath>
#include <cassert>

namespace dynsys {

using stepper_type = boost::numeric::odeint::runge_kutta4< state_type > ;

data_type generate_data( void )
{
    data_type data;
    state_type x {{ 10.0 , 10.0 , 10.0 }};
    state_type dxdt;
    double dt = 0.1;
    stepper_type stepper;
    for( double t = 0.0 ; t<100.0 ; t+= dt )
    {
        lorenz( x , dxdt , t );
        data.first.push_back( x );
        data.second.push_back( dxdt );
        stepper.do_step( lorenz , x , dxdt , t , dt );
    }
    return data;
}

void plot_data( data_type const& data , std::ostream& out )
{
    for( size_t i=0 ; i<data.first.size() ; ++i )
    {
        state_type const& x = data.first[i];
        state_type const& dxdt = data.second[i];
        out << x[0] << " " << x[1] << " " << x[2] << " " << dxdt[0] << " " << dxdt[1] << " " << dxdt[2] << "\n";
    }
}

norm_type normalize_data( state_container& data )
{
    norm_type means;
    for( size_t j=0 ; j<dim ; ++j )
    {
        means[j].first = means[j].second = 0.0;
    }

    for( size_t i=0 ; i<data.size() ; ++i )
    {
        assert( data[i].size() == dim );
        for( size_t j=0 ; j<dim ; ++j )
        {
            means[j].first += data[i][j];
            means[j].second += data[i][j] * data[i][j];
        }
    }
    double len = double( data.size() );
    for( size_t j=0 ; j<dim ; ++j )
    {
        double mean = means[j].first / len;
        double sqmean = means[j].second / len;
        means[j].first = mean;
        means[j].second = std::sqrt( sqmean - mean * mean );
    }
    for( size_t i=0 ; i<data.size() ; ++i )
    {
        for( size_t j=0 ; j<dim ; ++j )
        {
            data[i][j] = ( data[i][j] - means[j].first ) / means[j].second;
        }
    }

    return means;
}

} // namespace dynsys


//c
#include <ctime>
//c++
#include <iostream>
#include <stdexcept>
//eigen
#include <Eigen/Dense>

double test_eigensolver_values(int b, int e, int s){
	typedef Eigen::MatrixXf MatType;
	double sum=0;
	for(int i=b; i<=e; i+=s){
		clock_t start_=std::clock();
		MatType mat=MatType::Random(i,i);
		Eigen::EigenSolver<MatType> solver(mat);
		sum+=solver.eigenvalues()[0].real();
		clock_t stop_=std::clock();
		const double time=((double)(stop_-start_))/CLOCKS_PER_SEC;
		std::cout<<"time["<<i<<"] = "<<time<<"\n";
	}
	return sum;
}

double test_selfadjointeigensolver_values(int b, int e, int s){
	typedef Eigen::MatrixXf MatType;
	double sum=0;
	for(int i=b; i<=e; i+=s){
		clock_t start_=std::clock();
		MatType mat=MatType::Random(i,i);
		Eigen::SelfAdjointEigenSolver<MatType> solver(mat);
		sum+=solver.eigenvalues()[0];
		clock_t stop_=std::clock();
		const double time=((double)(stop_-start_))/CLOCKS_PER_SEC;
		std::cout<<"time["<<i<<"] = "<<time<<"\n";
	}
	return sum;
}

double test_selfadjointeigensolver_rank(int b, int e, int s){
	typedef Eigen::MatrixXf MatType;
	double sum=0;
	for(int i=b; i<=e; i+=s){
		clock_t start_=std::clock();
		MatType mat=MatType::Random(i,i);
		//Eigen::FullPivLU<MatType> solver(mat);
		Eigen::FullPivHouseholderQR<MatType> solver(mat);
		sum+=solver.rank();
		clock_t stop_=std::clock();
		const double time=((double)(stop_-start_))/CLOCKS_PER_SEC;
		std::cout<<"time["<<i<<"] = "<<time<<"\n";
	}
	return sum;
}

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

	test_eigensolver_values(1,1001,100);
	test_selfadjointeigensolver_values(1,1001,100);
	//test_selfadjointeigensolver_rank(1,1001,100);

	return 0;
}


#ifndef newton_raphson_hpp
#define newton_raphson_hpp 3141592

#include "constants.hpp"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>

using namespace boost::numeric::ublas;

template<class T>
bool InvertMatrix (const matrix<T>& input, matrix<T>& inverse) {
  typedef permutation_matrix<std::size_t> pmatrix;
  matrix<T> A(input);
  pmatrix pm(A.size1());
  int res = lu_factorize(A,pm);
  if( res != 0 ) return false;
  inverse.assign(identity_matrix<T>(A.size1()));
  lu_substitute(A, pm, inverse);
  return true;
}

template<typename U, typename V>void newton_raphson(U & F,V & J,vector<double>& x0,double tol){
  double res = norm_1(F(x0));
  //std::cout << "F(x0):" << F(x0) << std::endl;
  int size = x0.size();
  matrix<double> A;
  matrix<double> inverse(size,size);
  inverse = set_zero(inverse);
  std::cout << "Residu:" << res << std::endl;
  while(res > tol){
    A = J(x0);
    std::cout<< "A: " << A << std::endl;
    InvertMatrix(A, inverse);
    // //std::cout << "Inverse size " << inverse.size1() << " " << inverse.size2() << std::endl;
    // x0 = x0 - prod(inverse,F(x0));
    // res = norm_1(F(x0));
    // std::cout << "Residu: "<< res << std::endl;
    // std::cout << "Solution: "<< x0 << std::endl;
  }

}

#endif


#pragma once

#include <iostream>
#include <random>

#include <Eigen/Cholesky>
#include <Eigen/SVD>

#include "km_utils/types.hpp"

namespace km
{
namespace math
{
template <typename Scalar_t, int vector_size>
constexpr Scalar_t multivariateNormalProb(
    types::Vec<Scalar_t, vector_size> const &x,
    types::Vec<Scalar_t, vector_size> const &mean,
    types::SquareMat<Scalar_t, vector_size> const &cov_inverse,
    Scalar_t const normal_pdf_constant)
{
  types::Vec<Scalar_t, vector_size> const dx = x - mean;
  Scalar_t const normal_pdf_exponent =
      Scalar_t{-0.5} * (dx.transpose() * cov_inverse * dx).value();
  return normal_pdf_constant * std::exp(normal_pdf_exponent);
}

template <typename Scalar_t, int vector_size>
constexpr Scalar_t multivariateNormalProb(
    types::Vec<Scalar_t, vector_size> const &dx,
    types::SquareMat<Scalar_t, vector_size> const &cov_inverse,
    Scalar_t const normal_pdf_constant)
{
  Scalar_t const normal_pdf_exponent =
      Scalar_t{-0.5} * (dx.transpose() * cov_inverse * dx).value();
  return normal_pdf_constant * std::exp(normal_pdf_exponent);
}

template <typename Scalar_t, int vector_size>
constexpr Scalar_t multivariateNormalProb(
    types::Vec<Scalar_t, vector_size> const &x,
    types::Vec<Scalar_t, vector_size> const &mean,
    types::SquareMat<Scalar_t, vector_size> const &cov_inverse)
{
  Scalar_t const normal_pdf_constant =
      std::sqrt((cov_inverse / Scalar_t{2 * M_PI}).determinant());
  return multivariateNormalProb(x, mean, cov_inverse, normal_pdf_constant);
}

template <typename Derived>
constexpr auto getNormalPDFConstant(
    Eigen::MatrixBase<Derived> const &cov_inverse)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  return std::sqrt((cov_inverse / Scalar{2 * M_PI}).determinant());
}

template <typename Scalar_t, int mat_size>
constexpr std::vector<Scalar_t> getNormalPDFConstant(
    types::vector_aligned<types::SquareMat<Scalar_t, mat_size>> const
        &vec_cov_inverse)
{
  std::vector<Scalar_t> ret;
  ret.reserve(vec_cov_inverse.size());
  for(auto const &M : vec_cov_inverse)
    ret.push_back(getNormalPDFConstant(M));
  return ret;
}

/**
 * Calculate the matrix square root of a positive (semi-)definite matrix.
 *
 * @param mat The input matrix.
 * @param semidefinite_mat Indicates whether the input is a semidefinite matrix.
 *
 * @return The matrix square root of the input mat.
 */
template <typename Derived>
constexpr typename Derived::PlainObject matrixSquareRoot(
    Eigen::MatrixBase<Derived> const &mat, bool semidefinite_mat = false)
{
  constexpr auto M = Derived::RowsAtCompileTime;
  constexpr auto N = Derived::ColsAtCompileTime;
  static_assert(M == N, "Only valid for a square matrix");
  if constexpr(M == 0 || N == 0)
    return Derived::PlainObject::Zero(mat.rows(), mat.cols());
  else
  {
    if(!semidefinite_mat)
    {
      Eigen::LLT<typename Derived::PlainObject> cov_chol{mat};
      if(cov_chol.info() == Eigen::Success)
        return cov_chol.matrixL();
    }
    Eigen::LDLT<typename Derived::PlainObject> cov_chol{mat};
    if(cov_chol.info() == Eigen::Success)
    {
      typename Derived::PlainObject const L = cov_chol.matrixL();
      auto const P = cov_chol.transpositionsP();
      auto const D_sqrt =
          cov_chol.vectorD().array().sqrt().matrix().asDiagonal();
      return P.transpose() * L * D_sqrt;
    }
    return Derived::PlainObject::Zero(mat.rows(), mat.cols());
  }
}

template <typename Vec, typename Cov>
types::vector_aligned<Vec> randomSamplesGaussian(Vec const &mean,
                                                 Cov const &covariance_sqrt,
                                                 unsigned int n,
                                                 std::random_device &rd)
{
  std::mt19937 rng{rd()};
  std::normal_distribution<> dist;

  types::vector_aligned<Vec> ret;
  ret.reserve(n);
  for(unsigned int i = 0; i < n; ++i)
  {
    Vec z = Vec::Zero(mean.size());
    for(unsigned int j = 0; j < mean.size(); ++j)
      z(j) = dist(rng);
    ret.push_back(mean + covariance_sqrt * z);
  }
  return ret;
}

template <typename Derived>
constexpr typename Derived::PlainObject pseudoInverse(
    Eigen::MatrixBase<Derived> const &m)
{
  // JacobiSVD: thin U and V are only available when your matrix has a dynamic
  // number of columns.
  constexpr auto flags = (Derived::ColsAtCompileTime == Eigen::Dynamic) ?
                             (Eigen::ComputeThinU | Eigen::ComputeThinV) :
                             (Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::JacobiSVD<typename Derived::PlainObject> m_svd(m, flags);
  // std::cout << "singular values: " << m_svd.singularValues().transpose()
  //           << "\n";
  return m_svd.solve(Derived::PlainObject::Identity(m.rows(), m.rows()));
}

template <unsigned int num_repeat, typename Derived>
constexpr auto blockDiagonalMatrix(Eigen::EigenBase<Derived> const &X)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  constexpr auto M = Eigen::internal::traits<Derived>::RowsAtCompileTime;
  constexpr auto N = Eigen::internal::traits<Derived>::ColsAtCompileTime;

  auto const in_rows = (M == Eigen::Dynamic) ? X.rows() : M;
  auto const in_cols = (N == Eigen::Dynamic) ? X.cols() : N;
  auto const out_rows = num_repeat * in_rows;
  auto const out_cols = num_repeat * in_cols;
  constexpr auto out_template_rows =
      (M == Eigen::Dynamic) ? Eigen::Dynamic : out_rows;
  constexpr auto out_template_cols =
      (N == Eigen::Dynamic) ? Eigen::Dynamic : out_cols;

  auto ret = Eigen::Matrix<Scalar, out_template_rows, out_template_cols>::Zero(
                 out_rows, out_cols)
                 .eval();

  if constexpr(M != Eigen::Dynamic && N != Eigen::Dynamic)
  {
    for(unsigned int i = 0; i < num_repeat; ++i)
      ret.template block<M, N>(i * M, i * N) = X;
  }
  else
  {
    for(unsigned int i = 0; i < num_repeat; ++i)
      ret.block(i * in_rows, i * in_cols, in_rows, in_cols) = X;
  }
  return ret;
}

template <typename Derived>
constexpr auto blockDiagonalMatrix(Eigen::EigenBase<Derived> const &X,
                                   unsigned int const num_repeat)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  constexpr auto M = Eigen::internal::traits<Derived>::RowsAtCompileTime;
  constexpr auto N = Eigen::internal::traits<Derived>::ColsAtCompileTime;

  auto const in_rows = X.rows();
  auto const in_cols = X.cols();

  auto ret = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
                 num_repeat * in_rows, num_repeat * in_cols)
                 .eval();

  if constexpr(M != Eigen::Dynamic && N != Eigen::Dynamic)
  {
    for(unsigned int i = 0; i < num_repeat; ++i)
      ret.template block<M, N>(i * M, i * N) = X;
  }
  else
  {
    for(unsigned int i = 0; i < num_repeat; ++i)
      ret.block(i * in_rows, i * in_cols, in_rows, in_cols) = X;
  }
  return ret;
}

template <typename T>
constexpr T square(T p)
{
  return p * p;
}

// From https://stackoverflow.com/a/33454406
template <typename T>
constexpr T powInt(T x, unsigned int n)
{
  if(n == 0)
    return T{1};
  else if(n == 1)
    return x;
  else if(n == 2)
    return x * x;

  auto y = x * x;
  n -= 2;
  while(n > 1)
  {
    if(n % 2 == 1)
      y *= x;
    x *= x;
    n /= 2;
  }
  return x * y;
}

/**
 * @brief Normalize the given angle in radians to (-PI, PI]
 *
 * @param[in] x angle in radians
 *
 * @return normalized angle in the range (-PI, PI]
 */
template <typename Scalar_t>
constexpr Scalar_t normalize_angle(Scalar_t x)
{
  constexpr Scalar_t pi = M_PI, two_pi = 2 * M_PI;
  return x - (std::ceil((x + pi) / two_pi) - Scalar_t{1}) * two_pi;
}
} // namespace math
} // namespace km


//
// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/strong_components.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

using namespace std;

int dijkstra(const weighted_graph &G, int s, int k) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));
  int min_dist = dist_map[0];
  for(int i = 1; i < k; i++) {
    min_dist = min(min_dist, dist_map[i]);
  }
  return min_dist;
}

void testcase() {
  int n; cin >> n;
  int m; cin >> m;
  int k; cin >> k;
  int T; cin >> T;
  vector<int> tele_nodes(T);
  for(int i = 0; i < T; i++) {
    cin >> tele_nodes[i];
  }
  weighted_graph G(n);

  weight_map weights = boost::get(boost::edge_weight, G);
  edge_desc e;
  for(int i = 0; i < m; i++) {
    int u,v; cin >> u; cin >> v;
    int c; cin >> c;
    e = boost::add_edge(v, u, G).first; weights[e] = c; // flipped edges!
  }
  // scc_map[i]: index of SCC containing i-th vertex
  vector<int> scc_map(n); // exterior property map
  // nscc: total number of SCCs
  int nscc = boost::strong_components(G,
      boost::make_iterator_property_map(scc_map.begin(),
      boost::get(boost::vertex_index, G)));

  vector<int> size_scc(nscc);
  for(int i = 0; i < T; i++) {
    size_scc[scc_map[tele_nodes[i]]]++;
  }

  for(int i = 0; i < T; i++) {
      int u = tele_nodes[i];
      int scc = scc_map[u];
      int size_this_scc = size_scc[scc] - 1;
      e = boost::add_edge(u, n + scc, G).first;
      weights[e] = size_this_scc;
      e = boost::add_edge(n + scc, u, G).first;
      weights[e] = 0;
  }

  int min_dist_to_warehouse = dijkstra(G, n - 1, k);
  if(min_dist_to_warehouse <= 1e6) {
    cout << min_dist_to_warehouse << endl;
  } else {
    cout << "no" << endl;
  }

}
int main()
{
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  for(int i = 0; i < t; i++)
    testcase();
  return 0;
}