#include "RBGL.hpp"

#include <stdlib.h>

#include <boost/graph/simple_point.hpp>

extern "C"
{

#include <Rdefines.h>
#include <R_ext/Random.h>
#include <Rmath.h>

    using namespace std;
    using namespace boost;

    static void delta_and_tau
     (const Graph_ud& g, vector<int>& v_delta, vector<int>& v_tau)
    {
        Graph_ud::vertex_iterator vi, v_end;
        Graph_ud::adjacency_iterator ui, u_end, wi, w_end;

        int dv = 0, tv = 0;

        v_delta.clear();
        v_tau.clear();

        for ( tie(vi, v_end) = vertices(g); vi != v_end; ++vi )
        {
            // delta(v)
            dv = 0;
            for ( tie(ui, u_end) = adjacent_vertices(*vi, g);
                    ui != u_end; ++ui )
            {
                wi = ui;
                for ( ++wi; wi != u_end; ++wi )
                    if ( edge(*ui, *wi, g).second ) dv++;
            }
            v_delta.push_back(dv);

            // tau(v)
            dv = degree(*vi, g);
            tv = dv * ( dv - 1 ) / 2;
            v_tau.push_back(tv);
        }
    }

    SEXP clusteringCoef(
        SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in,
        SEXP weighted, SEXP R_v_weights_in)
    {
        int i;

        int NV = INTEGER(num_verts_in)[0];
        vector<double> v_weight(NV, 1);

        if ( INTEGER(weighted)[0] )
        {
            double* weights = REAL(R_v_weights_in);
            for ( i = 0; i < NV; i++ ) v_weight[i] = weights[i];
        }

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);
        vector<int> v_delta, v_tau;
        delta_and_tau(g, v_delta, v_tau);

        double nn = 0;		// count nodes w/ deg(v) >= 2
        double cG = 0;
	Graph_ud::vertex_descriptor v;
        for ( i = 0; i < NV; i++ )
        {
	    v = vertex(i, g);
            if ( out_degree(v, g) >= 2 && v_tau[i] > 0 )
            {
                cG += v_weight[i] * v_delta[i] / v_tau[i];
                nn += v_weight[i];
            }
        }

        if ( nn ) cG /= nn;

        SEXP ccoef;
        PROTECT(ccoef = NEW_NUMERIC(1));
        REAL(ccoef)[0] = cG;
        UNPROTECT(1);
        return(ccoef);
    }

    SEXP transitivity( SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in)
    {
        int NV = INTEGER(num_verts_in)[0];

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);
        vector<int> v_delta, v_tau;
        delta_and_tau(g, v_delta, v_tau);

	double tG = 0;
        double sum_dv = 0, sum_tv = 0;
        for ( int i = 0; i < NV; i++ )
        {
            sum_dv += v_delta[i];
            sum_tv += v_tau[i];
        }
        if ( sum_tv ) tG = sum_dv / sum_tv;

#if DEBUG
        cout << " sum_dv = " << sum_dv
        << " sum_tv = " << sum_tv
        << " v_delta.size() = " << v_delta.size()
        << " v_tau.size() = " << v_tau.size()
	<< " tG = " << tG
        << endl;
#endif

        SEXP tcoef;
        PROTECT(tcoef = NEW_NUMERIC(1));
        REAL(tcoef)[0] = tG;
        UNPROTECT(1);
        return(tcoef);
    }

    // uniformly sample in [1, n]
    static inline int uniformRandomNumber(const int n)
    {
	int j = (int)(n * unif_rand()) + 1;   // unif_rand in [0, 1)
	return j;
    }

    static inline int findIndex(const int r, const vector<int>& W)
    {
	unsigned int i;
	for ( i = 1; i < W.size(); i++ ) if ( r <= W[i] ) break;
	return i;
    }

    // input: a node in graph g
    // output: one neighbor of node n chosen uniformly randomly
    static inline void uniformRandomAdjacentNode
	(const Graph_ud::vertex_descriptor& v, const Graph_ud& g,
	 Graph_ud::vertex_descriptor& u,
	 Graph_ud::vertex_descriptor& w)
    {
	int nc = out_degree(v, g);

	Graph_ud::adjacency_iterator vi, v_end;
	tie(vi, v_end) = adjacent_vertices(v, g);

	switch (nc)
	{
	case 0:
	case 1: u = w = *vi;
		break;
	case 2:
		u = *vi; vi++;
		w = *vi;
		break;
	default:
		{
		int r1 = uniformRandomNumber(nc);
		int r2 = uniformRandomNumber(nc);

		while ( r1 == r2 ) r2 = uniformRandomNumber(nc);

		for ( int i = 0; vi != v_end; vi++, i++ )
		{
		    if ( i == r1 ) u = *vi;
		    if ( i == r2 ) w = *vi;
		}

		break;
		}
	}
#if DEBUG
	cout << " uniformRandomAdjacentNode: " << endl;
	cout << " n = " << n << endl;
	cout << " nc = " << nc << endl;
	cout << " *vi = " << *vi << endl;
	cout << " u = " << u << endl;
	cout << " w = " << w << endl;
#endif
    }

    static inline void uniformRandomAdjacentNode_i
	(const int n, const Graph_ud& g,
	 Graph_ud::vertex_descriptor& u,
	 Graph_ud::vertex_descriptor& w)
    {
	Graph_ud::vertex_descriptor v = vertex(n, g);
	uniformRandomAdjacentNode(v, g, u, w);
    }

    // Approximating Cw
    //    Outline of the algorithm:
    //    Input: integer k;
    //           array A[1..|V'|] of nodes V' = {v in V: d(v) >= 2}
    //           node weights w: V' -> N>0;
    //           adjacentcy array for each node
    //    Output: approximation of Cw
    //    Data: node variables: u, w;
    //           integer variables: r, l, j, W[0..|V'|]
    //    Algorithm:
    //    W[0] = 0
    //    for i = (1, ..., |V'|) do
    //       W[i] = W[i-1] + w(A[i])
    //    l = 0
    //    for i in (1, ..., k) do
    //    {
    //       r = UniformRandomNumber( {1,...,W[|V'|]} )
    //       j = FindIndex( j: W[j-1] < r <= W[j] )
    //       u = UniformRandomAdjacentNode(A[j])
    //       repeat
    //         w = UniformRandomAdjacentNode(A[j])
    //       until u != w
    //       if ( EdgeExists(u, w) then
    //          l = l + 1
    //    }
    //    return l/k

    SEXP clusteringCoefAppr(SEXP k_in,
        SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in,
        SEXP weighted, SEXP R_v_weights_in)
    {
	// prepare for later unif_rand call
	GetRNGstate();

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

        int i, l, r, j;

        int k = INTEGER(k_in)[0];
        int NV = INTEGER(num_verts_in)[0];
        vector<int> v_weight(NV, 1);
        vector<int> W(NV+1, 0);

#if DEBUG
	cout << " inside clusteringCoefAppr "
		<< " k = " << k
		<< " NV = " << NV
		<< endl;
#endif

        if ( INTEGER(weighted)[0] )
        {
            double* weights = REAL(R_v_weights_in);
            for ( i = 0; i < NV; i++ ) v_weight[i] = (int) weights[i];
        }

        Graph_ud::vertex_descriptor u=Graph_ud::null_vertex(), w=Graph_ud::null_vertex();

	W[0] = 0;
	for ( i = 1; i < NV+1; i++ ) W[i] = W[i-1] + v_weight[i-1];

	// TODO: limit nodes to those w/ degree >= 2
	//       pick a number within range uniformaly
	//	 pick an adjacent node uniformaly randomly
	for ( l = 0, i = 0; i < k; i++ )
	{
	   r = uniformRandomNumber(W[NV]);
	   j = findIndex(r, W);
	   uniformRandomAdjacentNode_i(j-1, g, u, w);

	   if ( edge(u, w, g).second ) l++;

#if DEBUG
	   cout << " i = " << i;
	   cout << " r = " << r;
	   cout << " j = " << j;
	   cout << " l = " << l << endl;
#endif
	}

	double cG = double(l) / double(k);

        SEXP ccoef;
        PROTECT(ccoef = NEW_NUMERIC(1));
        REAL(ccoef)[0] = cG;
        UNPROTECT(1);
        return(ccoef);

    }

    inline bool prob_cmp(const simple_point<int>& p1,
			 const simple_point<int>& p2)
	{ return p1.y > p2.y; }

    //  To find a random node w/ probability d(u) / sum(d(V))
    //  The following closely mirrors the codes on
    //  Unequal probability sampling; without-replacement case
    //
    //    /* Record element identities */
    //    for (i = 0; i < n; i++)
    //        perm[i] = i + 1;
    //
    //    //* Sort probabilities into descending order */
    //    //* Order element identities in parallel */
    //    revsort(p, perm, n);
    //
    //    //* Compute the sample */
    //    totalmass = 1;
    //    for (i = 0, n1 = n-1; i < nans; i++, n1--) {
    //        rT = totalmass * unif_rand();
    //        mass = 0;
    //        for (j = 0; j < n1; j++) {
    //            mass += p[j];
    //            if (rT <= mass)
    //                break;
    //        }
    //        ans[i] = perm[j];
    //        totalmass -= p[j];
    //        for(k = j; k < n1; k++) {
    //            p[k] = p[k + 1];
    //            perm[k] = perm[k + 1];
    //        }
    //    }
    static void ProbRandomNode
	(const Graph_ud::vertex_descriptor& v, const Graph_ud& g,
	 Graph_ud::vertex_descriptor& u)
    {

	typedef graph_traits<Graph_ud>::vertex_iterator vertex_iterator;

	vertex_iterator vi, v_end;

	int NV = num_vertices(g);
	std::vector < simple_point<int> > pp(num_vertices(g));

	int i = 0, totalmass = 0;
	for ( tie(vi, v_end) = vertices(g); vi != v_end; vi++, i++ )
	{
	   pp[i].x = i+1;
	   pp[i].y = out_degree(*vi, g);
	   totalmass += pp[i].y;
	}

	std::stable_sort(pp.begin(), pp.end(), prob_cmp);

	int j, k, n1, rT, mass;
	for ( i = 0, n1 = NV-1; i < NV; i++, n1-- )
	{
            rT = (int)(totalmass * unif_rand());
            mass = 0;
            for (j = 0; j < n1; j++) {
                mass += pp[j].y;
                if (rT <= mass) break;
            }
	    u = vertex(i, g);
	    if ( !edge(v, u, g).second ) break;

	    totalmass -= pp[j].y;
	    for ( k = j; k < n1; k++ ) pp[k] = pp[k+1];
	}
    }

    // Graph Generator:
    //    Outline of algorithm:
    //    Input: initial graph G: two connected nodes
    //           integer: n >= 3, d >= 2, o
    //    Output: graph G
    //    Algorithm:
    //    for ( i = (3, ..., n) do
    //    {
    //       v = NewNode()
    //       for 1, ..., Min(i-1, d) do
    //       {
    //          repeat
    //            u = RandomNode( with prob du / sum(d(v) )
    //          until node EdgeExists(v, u)
    //          AddEdge(v, u)
    //       }
    //       for 1, ..., o do
    //       {
    //          u = RandomAdjacentNode(v)
    //          repeat
    //             w = RandomAdjacentNode(v)
    //          until w != u
    //          if ( node EdgeExists(u, w) then
    //             AddEdge(u, w)
    //       }
    //    }

    SEXP graphGenerator(SEXP n_in, SEXP d_in, SEXP o_in)
    {
	int i, j;
        int n = INTEGER(n_in)[0];
        int d = INTEGER(d_in)[0];
        int o = INTEGER(o_in)[0];

	GetRNGstate();	// get random number generator ready

	// initial graph with 2 connected nodes
	Graph_ud g(2);
	boost::add_edge(0, 1, g);

        Graph_ud::vertex_descriptor v, u, w=Graph_ud::null_vertex();

	for ( i = 3; i <= n; i++ )
	{
	   // generate a new node
	   v = boost::add_vertex(g);

	   for ( j = 1; j <= min(i-1, d); j++ )
	   {
		ProbRandomNode(v, g, u);
		boost::add_edge(v, u, g);
	   }

	   for ( j = 1; j <= o; j++ )
	   {
	        uniformRandomAdjacentNode(v, g, u, w);

		if ( !edge(u, w, g).second )
		   boost::add_edge(u, w, g);
	   }
	}

#if DEBUG
	typedef graph_traits<Graph_ud>::vertex_iterator vertex_iterator;
	vertex_iterator vi, v_end;
	cout << " no. of vertices: " << num_vertices(g)
	     << " no. of edges:    " << num_edges(g)
	     << endl;

	for ( tie(vi, v_end) = vertices(g); vi != v_end; vi++, i++ )
	{
	   cout << " vertex: " << *vi
		<< " has degree: " << out_degree(*vi, g)
		<< endl;
	}
#endif

        int NE = num_edges(g);
        SEXP anslst, ncnt, ecnt, enlst;
        PROTECT(anslst = allocVector(VECSXP, 3));
        PROTECT(ncnt = NEW_INTEGER(1));
        PROTECT(ecnt = NEW_INTEGER(1));
        PROTECT(enlst = allocMatrix(INTSXP, 2, NE));

        INTEGER(ncnt)[0] = num_vertices(g);
        INTEGER(ecnt)[0] = NE;

	typedef graph_traits<Graph_ud>::edge_iterator edge_iterator;
	edge_iterator ei, e_end;
	for ( i = 0, tie(ei, e_end) = edges(g); ei != e_end ; ei++ )
        {
            INTEGER(enlst)[i++] = source(*ei, g);
            INTEGER(enlst)[i++] = target(*ei, g);
        }

	SET_VECTOR_ELT(anslst,0,ncnt);
	SET_VECTOR_ELT(anslst,1,ecnt);
        SET_VECTOR_ELT(anslst,2,enlst);
        UNPROTECT(4);
        return(anslst);
    }

}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "alpha_kernel_d"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epick_d.h>
#include <CGAL/Epeck_d.h>
#include <CGAL/NT_converter.h>

#include <iostream>
#include <vector>
#include <utility>  // for std::pair

#include <gudhi/Alpha_complex/Alpha_kernel_d.h>
#include <gudhi/Unitary_tests_utils.h>

// Use dynamic_dimension_tag for the user to be able to set dimension
typedef CGAL::Epeck_d< CGAL::Dynamic_dimension_tag > Exact_kernel_d;
// Use static dimension_tag for the user not to be able to set dimension
typedef CGAL::Epeck_d< CGAL::Dimension_tag<4> > Exact_kernel_s;
// Use dynamic_dimension_tag for the user to be able to set dimension
typedef CGAL::Epick_d< CGAL::Dynamic_dimension_tag > Inexact_kernel_d;
// Use static dimension_tag for the user not to be able to set dimension
typedef CGAL::Epick_d< CGAL::Dimension_tag<4> > Inexact_kernel_s;
// The triangulation uses the default instantiation of the TriangulationDataStructure template parameter

typedef boost::mpl::list<Exact_kernel_d, Exact_kernel_s, Inexact_kernel_d, Inexact_kernel_s> list_of_kernel_variants;

BOOST_AUTO_TEST_CASE_TEMPLATE(Alpha_kernel_d_dimension, TestedKernel, list_of_kernel_variants) {
  // Test for a point (weighted or not) in 4d, that the dimension is 4.

  Gudhi::alpha_complex::Alpha_kernel_d<TestedKernel, false> kernel;
  std::vector<double> p0 {0., 1., 2., 3.};
  typename TestedKernel::Point_d p0_d(p0.begin(), p0.end());

  std::clog << "Dimension is " << kernel.get_dimension(p0_d) << std::endl;
  BOOST_CHECK(kernel.get_dimension(p0_d) == 4);

  Gudhi::alpha_complex::Alpha_kernel_d<TestedKernel, true> w_kernel;
  typename TestedKernel::Weighted_point_d w_p0_d(p0_d, 10.);

  std::clog << "Dimension is " << w_kernel.get_dimension(w_p0_d) << std::endl;
  BOOST_CHECK(w_kernel.get_dimension(w_p0_d) == 4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Alpha_kernel_d_sphere, TestedKernel, list_of_kernel_variants) {
  // Test with 5 points on a 3-sphere, that get_sphere returns the same center and squared radius
  // for dD unweighted and for dD weighted with all weights at 0.

  using Unweighted_kernel = Gudhi::alpha_complex::Alpha_kernel_d<TestedKernel, false>;
  // Sphere: (x-1)² + (y-1)² + z² + t² = 1
  // At least 5 points for a 3-sphere
  std::vector<double> p0 {1., 0., 0., 0.};
  std::vector<double> p1 {0., 1., 0., 0.};
  std::vector<double> p2 {1., 1., 1., 0.};
  std::vector<double> p3 {1., 1., 0., 1.};
  std::vector<double> p4 {1., 1., -1., 0.};

  using Point_d = typename Unweighted_kernel::Point_d;
  std::vector<Point_d> unw_pts;
  unw_pts.emplace_back(p0.begin(), p0.end());
  unw_pts.emplace_back(p1.begin(), p1.end());
  unw_pts.emplace_back(p2.begin(), p2.end());
  unw_pts.emplace_back(p3.begin(), p3.end());
  unw_pts.emplace_back(p4.begin(), p4.end());

  Unweighted_kernel kernel;
  auto unw_sphere = kernel.get_sphere(unw_pts.cbegin(), unw_pts.cend());

  std::clog << "Center is " << unw_sphere.first << " - squared radius is " << unw_sphere.second << std::endl;

  using Weighted_kernel = Gudhi::alpha_complex::Alpha_kernel_d<TestedKernel, true>;

  using Weighted_point_d = typename Weighted_kernel::Weighted_point_d;
  using Bare_point_d = typename Weighted_kernel::Bare_point_d;
  std::vector<Weighted_point_d> w_pts;
  w_pts.emplace_back(Bare_point_d(p0.begin(), p0.end()), 0.);
  w_pts.emplace_back(Bare_point_d(p1.begin(), p1.end()), 0.);
  w_pts.emplace_back(Bare_point_d(p2.begin(), p2.end()), 0.);
  w_pts.emplace_back(Bare_point_d(p3.begin(), p3.end()), 0.);
  w_pts.emplace_back(Bare_point_d(p4.begin(), p4.end()), 0.);

  Weighted_kernel w_kernel;
  auto w_sphere = w_kernel.get_sphere(w_pts.cbegin(), w_pts.cend());

  std::clog << "Center is " << w_sphere.point() << " - squared radius is " << w_sphere.weight() << std::endl;

  CGAL::NT_converter<typename Weighted_kernel::FT, double> cast_to_double;
  // The results shall be the same with weights = 0.
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(cast_to_double(unw_sphere.second), cast_to_double(w_sphere.weight()));
  BOOST_CHECK(unw_sphere.first == w_sphere.point());

  auto unw_sq_rd = kernel.get_squared_radius(unw_pts.cbegin(), unw_pts.cend());
  std::clog << "Squared radius is " << unw_sq_rd << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(cast_to_double(unw_sphere.second), cast_to_double(unw_sq_rd));
  auto w_sq_rd = w_kernel.get_squared_radius(w_pts.cbegin(), w_pts.cend());
  std::clog << "Squared radius is " << w_sq_rd << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(cast_to_double(w_sphere.weight()), cast_to_double(w_sq_rd));
}


#pragma once
#include "Optimizer.hpp"

#include <Eigen/Dense>
#include <array>
#include <nlohmann/json.hpp>

namespace yavque
{
class Adam : public Optimizer
{
private:
	const double alpha_;
	const double beta1_;
	const double beta2_;
	const double eps_;

	int t_ = 0;
	Eigen::VectorXd m_;
	Eigen::VectorXd v_;

public:
	static constexpr std::array<double, 4> DEFAULT_PARAMS = {1e-3, 0.9, 0.999, 1e-8};

	explicit Adam(double alpha = DEFAULT_PARAMS[0], double beta1 = DEFAULT_PARAMS[1],
	              double beta2 = DEFAULT_PARAMS[2], double eps = DEFAULT_PARAMS[3])
		: alpha_(alpha), beta1_(beta1), beta2_(beta2), eps_(eps)
	{
	}

	explicit Adam(const nlohmann::json& params)
		: alpha_(params.value("alpha", DEFAULT_PARAMS[0])),
		  beta1_(params.value("beta1", DEFAULT_PARAMS[1])),
		  beta2_(params.value("beta2", DEFAULT_PARAMS[2])),
		  eps_(params.value("eps", DEFAULT_PARAMS[3]))
	{
	}

	static nlohmann::json defaultParams()
	{
		return nlohmann::json{
			{"name", "Adam"},
			{"alhpa", DEFAULT_PARAMS[0]},
			{"beta1", DEFAULT_PARAMS[1]},
			{"beta2", DEFAULT_PARAMS[2]},
			{"eps", DEFAULT_PARAMS[3]},
		};
	}

	[[nodiscard]] nlohmann::json desc() const override
	{
		return nlohmann::json{
			{"name", "Adam"},  {"alhpa", alpha_}, {"beta1", beta1_},
			{"beta2", beta2_}, {"eps", eps_},
		};
	}

	Eigen::VectorXd getUpdate(const Eigen::VectorXd& grad) override
	{
		if(t_ == 0)
		{
			m_ = Eigen::VectorXd::Zero(grad.rows());
			v_ = Eigen::VectorXd::Zero(grad.rows());
		}
		++t_;

		m_ *= beta1_;
		m_ += (1 - beta1_) * grad;

		Eigen::VectorXd g2 = grad.array().square();
		v_ *= beta2_;
		v_ += (1 - beta2_) * g2;

		double epsnorm = eps_ * sqrt(1.0 - pow(beta2_, t_));
		Eigen::VectorXd denom
			= v_.unaryExpr([epsnorm](double x) { return sqrt(x) + epsnorm; });

		double alphat = alpha_ * sqrt(1.0 - pow(beta2_, t_)) / (1.0 - pow(beta1_, t_));

		return -alphat * m_.cwiseQuotient(denom);
	}
};
} // namespace yavque


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "../include/core/pardiso.hpp"

using namespace std;

TEST(Pardiso, unsymmSolver)
{
    // CSR sparse matrix
    int sizeA = 8;
    int rowPtrA[9] = { 0, 4, 7, 9, 11, 12, 15, 17, 20 };
    int colIdA[20] = { 0,    2,       5, 6,
                          1, 2,    4,
                             2,             7,
                                   3,       6,
                          1,
                             2,       5,    7,
                          1,             6,
                             2,          6, 7 };
    double dataA[20] = { 7.0,      1.0,           2.0, 7.0,
                             -4.0, 8.0,      2.0,
                                   1.0,                     5.0,
                                        7.0,           9.0,
                             -4.0,
                                   7.0,           3.0,      8.0,
                              1.0,                    11.0,
                                  -3.0,                2.0, 5.0 };

    // create an Eigen Sparse matrix from rowPtrA, colIdA, dataA
    Eigen::SparseMatrix<double> matA(sizeA, sizeA);
    matA.reserve(20);
    for (int i=0; i<sizeA; i++) {
        for (int j=rowPtrA[i]; j<rowPtrA[i+1]; j++) {
            matA.insert(i,colIdA[j]) = dataA[j];
        }
    }
    matA.makeCompressed();

    // rhs
    Eigen::VectorXd b(sizeA);
    //b = Eigen::VectorXd::Constant(sizeA, 1);
    b = Eigen::VectorXd::Random(sizeA);

    // solve with Eigen
    Eigen::VectorXd true_x(sizeA);
    Eigen::SparseLU sparselu(matA);
    true_x = sparselu.solve(b);

    // solve with Pardiso
    int mtype = 11;
    Eigen::VectorXd x(sizeA);
    PardisoSolver pardisoSolver(mtype);
    pardisoSolver.initialize(sizeA, rowPtrA, colIdA, dataA);
    pardisoSolver.factorize();
    pardisoSolver.solve(b.data(), x.data());
    pardisoSolver.finalize();

    double TOL = 1E-8;
    ASSERT_LE((true_x - x).norm(), TOL);
}


/**
 * \file ex7.cxx
 * \author Ray Chew
 * \date 7 July 2017
 * \brief Dijkstra graph algorithm for .gph graphs
 */

/**
 * \mainpage Ex7
 *
 * \section Description
 *
 * Shortest-longest path distance calculator using the Dijkstra algorithm for `.gph` graphs.

 * Reads `.gph` graph file and prints the node number and distance of the node corresponding to the end of longest shortest path.

 * Flag `-m1` uses the Boost Graph Library Dijkstra Algorithm.

 * Flag `-m2` uses a self-implemented Dijkstra Algorithm for undirected graphs.

 *
 * * compile: `g++ -std=c++14 -O3 ex7.cxx -o ex7 -lboost_timer -Wall`

 * * run: `./ex7 filepath/graph.gph [-m1/-m2]`

 * * flags: `-m1` for boost algorithm, `-m2` for self-implemented algorithm.
 */

/* -- Includes -- */
/* C++ includes. */
#include <iostream> /* for std::cout, std::ofstream */
#include <fstream> /* for fstream::app */
#include <utility> /* for std::pair */
#include <vector> /* for std::vector */

/* Boost Dijkstra Algorithm includes. */
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

/* Boost qi parser and timer includes. */
#include <boost/spirit/include/qi.hpp>
#include <boost/timer/timer.hpp>

/* Boost Bimap Container includes for own algorithm. */
#include <boost/bimap.hpp>
#include <boost/bimap/multiset_of.hpp>
#include <boost/bimap/support/lambda.hpp> /* for bimap::_data and bimap::_key */
//#include <boost/bind.hpp> /* for boost::bind in sorting of vector<pair<int,int>> */

namespace bm = boost::bimaps;
using namespace boost::spirit;
using qi::int_;
using qi::double_;
using qi::parse;

/**
 *  \brief A method that calculates the longest-shortest path from the source node with name `1` using the boost library dijkstra algorithm.
 *  \param n number of edges as int.
 *  \param file pointer to graph file opened.
 *  \return MaxVertex and MaxDistance, the node name and distance of the longest-shortest path as `std::pair<int,int>`.
 */
std::pair<int,int> m1 (int& n, std::ifstream& file){

  /* start get list of edges and weights */
  using Edge = std::pair<int, int>;
  std::vector<Edge> Edges; // vector to store std::pair of edges.
  std::vector<int> Weights; // vector to weights as integers.
  std::string str; // string to store line of graph file.

  while (getline(file,str)){ /// get graph line-by-line.
    int Vert1;
    int Vert2;
    int Weight;

    auto it = str.begin(); /// initializes iterator for qi::parse.

    parse(it, str.end(), int_[([&Vert1](int i){Vert1 = i;})] >> qi::space >> int_[([&Vert2](int i){Vert2 = i;})] >> qi::space >> double_[([&Weight](int i){Weight = i;})]);

    Edge edge = std::make_pair(Vert1, Vert2);  /// make edge-pair out of vertices.
    Edges.push_back(edge);
    Weights.push_back(Weight);
  }
  /* end get list of edges and weights */

  /* start building graph */
  /// initialize type to store weights on edges.
  typedef boost::property<boost::edge_weight_t, int> EdgeWeightProperty;
  // adjacency_list<out-edges, vertex_set, directedness, vertex properties, edge properties>
  /// create graph.
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProperty> Graph;

  Graph g(Edges.begin(),Edges.end(), Weights.begin(), n);   /// populate graph.

  /* end building graph */

  /* start finding shortest path from source node. */
  typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  vertex_descriptor source = boost::vertex(1, g);   /// define source vertex as vertex with index == 1.
  /// initialize vectors for predecessor and distances.
  std::vector<vertex_descriptor> parents(boost::num_vertices(g));
  std::vector<int> distances(boost::num_vertices(g));

  boost::dijkstra_shortest_paths(g, source, boost::predecessor_map(&parents[0]).distance_map(&distances[0]));

  /* end finding shortest path of from source node. */

  /* start finding longest-shortest path from source node. */
  signed int maxDistance = 0;
  unsigned int maxVertex = 0;

  /// create iterator over vertices.
  // vertexPair.first is the iterated element, and .second is the end-index of all vertices.
  typedef boost::graph_traits <Graph>::vertex_iterator vertex_iter;
  std::pair<vertex_iter, vertex_iter> vertexPair;

  // vertexPair = boost::vertices loops over all vertices in g.
  for (vertexPair = boost::vertices(g); vertexPair.first != vertexPair.second; ++vertexPair.first){
    /// replace maxDistance if a greater distance is found, and maxDistance must be less than "infinity" (of 32-bit signed integer).
    if ((distances[*vertexPair.first] > maxDistance) && (distances[*vertexPair.first] < std::numeric_limits<int>::max())){
      maxDistance = distances[*vertexPair.first];
      maxVertex = *vertexPair.first;
    }
    /// if distance == maxDistance, check if vertex index is smaller.
    if ((distances[*vertexPair.first] == maxDistance) && (*vertexPair.first < maxVertex)){
      maxDistance = distances[*vertexPair.first];
    }
  }
  /* end finding longest-shortest path from source node. */

  std::pair<int,int> Final = std::make_pair(maxVertex,maxDistance);
  return Final;
}

/**
 *  \brief A method that calculates the longest-shortest path from the source node with name `1` using a self-implemented dijkstra algorithm.
 *  \param n number of edges as int.
 *  \param file pointer to graph file opened.
 *  \return MaxVertex and MaxDistance, the node name and distance of the longest-shortest path as `std::pair<int,int>`.
 */
std::pair<int,int> m2 (int& n, std::ifstream& file){

  /* start populating adjacency list */
  using vertex =  std::pair<int, int>;
  std::vector<std::vector<vertex>> adjList(n); // empty adjacency list.

  std::cout << "pass initialize adjList." << std::endl;
  std::string str; // string to store line of graph.

  while (getline(file,str)){
    int Vert1;
    int Vert2;
    int Weight;

    auto it = str.begin(); /// initialize iterator for qi::parse.

    parse(it, str.end(), int_[([&Vert1](int i){Vert1 = i;})] >> qi::space >> int_[([&Vert2](int i){Vert2 = i;})] >> qi::space >> double_[([&Weight](double i){Weight = i;})]);

    vertex VertexWeight1(Vert2,Weight);
    adjList[Vert1].push_back(VertexWeight1);

    vertex VertexWeight2(Vert1,Weight);
    adjList[Vert2].push_back(VertexWeight2);
  }
  std::cout << "pass build adjList." << std::endl;
  /* end populating adjacency list */

  using bimap = bm::bimap<int, boost::bimaps::multiset_of<int,std::less<int>>>;
  bimap Unvisited; /// define unvisited set as a boost::bimap container.
  std::vector<vertex> finalWeights(n); // vector to store all calculated weights/distances.

  /* start initializing unvisited set */
  for(int i=1; i<n; i++){
    Unvisited.left.insert(bimap::left_value_type(i,std::numeric_limits<int>::max()));
    finalWeights[i] = std::make_pair(i, std::numeric_limits<int>::max());
  }
  bimap::right_iterator itr = Unvisited.right.begin();
  Unvisited.right.replace_key(itr, 0);
  finalWeights[1].second = 0;

  std::cout << "pass initialize unvisited set." << std::endl;
  /* end initializing unvisited set */

  /* start calculating and updating distances */
  while(Unvisited.size()>0){
    auto minPair = Unvisited.right.begin();
    int minIdx = minPair->second;
    int minDist = minPair->first;

    signed int adjListSize = adjList[minIdx].size();

    for(int j=0; j<adjListSize; j++){
      int neighbour = adjList[minIdx][j].first;
      int dist = adjList[minIdx][j].second;
      int newDist = minDist + dist;
      int nPWeight = finalWeights[neighbour].second;

      if (newDist < nPWeight){
	auto toBeReplaced = Unvisited.left.find(neighbour);
	Unvisited.left.modify_data(toBeReplaced, bm::_data=newDist);
	finalWeights[neighbour].second = newDist;
      }
    }
    Unvisited.left.erase(minIdx);
  }
  std::cout << "pass distance calculation." << std::endl;
  /* end calculating and updating distances */

  /* start find node of the longest-shortest path */
  //std::sort(finalWeights.begin(), finalWeights.end(), [](auto &left, auto &right) {
  //    return left.second < right.second;});

  int maxDistance = 0;
  int maxVertex = 0;

  for(auto ita = finalWeights.begin(); ita != finalWeights.end(); ita++){
    /// replace maxDistance if a greater distance is found, and maxDistance must be less than "infinity" (of 32-bit signed integer).
    if ((ita->second > maxDistance) && (ita->second < std::numeric_limits<int>::max())){
      maxDistance = ita->second;
      maxVertex = ita->first;
    }
    /// if distance == maxDistance, check if vertex index is smaller.
    if ((ita->second == maxDistance) && (ita->first < maxVertex)){
      maxVertex = ita->first;
    }
  }
  /* end find node of the longest-shortest path */

  vertex Final = std::make_pair(maxVertex,maxDistance);
  return Final;
}

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

  if (argc < 3){ // must have filename of graph and a flag of some sort...
    std::cerr << "No file or flag (-m1 or -m2)!!!" << std::endl;
    return -1;
  }

  std::ifstream file(argv[1]);  // read graph file.
  std::string str; /// read graph file line by line.

  /* start get number of edges */
  getline(file, str);
  int n; /// store n as int for number of edges.

  auto it = str.begin();
  parse(it, str.end(), int_[([&n](int i){n = i;})] >> int_);
  n = n + 1; // No?
  /* end get number of edges */

  /* start dijkstra algorithm according to flag */
  std::pair<int,int> f =std::make_pair(0,0); /// initialize f as (int,int) pair for final node number and distance.
  boost::timer::cpu_timer timer;
  for(int i = 0; i < argc; i++){
    if (std::string(argv[i]) == "-m1"){
      f = m1(n,file);
    }
    else if(std::string(argv[i]) == "-m2"){
      f = m2(n,file);
    }
  }
  boost::timer::cpu_times times = timer.elapsed();
  /* end dijkstra algorithm according to flag */

  /// output vertex and distance of the longest-shortest path.
  std::cout << "RESULT VERTEX " << f.first << std::endl;
  std::cout << "RESULT DIST " << f.second <<  std::endl;

  /// print CPU- and Wall-Time.
  // boost::timer::cpu_times returns tuple of wall, system and user times in nanoseconds.
  std::cout << std::endl;
  std::cout << "WALL-CLOCK " << times.wall / 1e9 << "s" << std::endl;
  std::cout << "USER TIME " << times.user / 1e9 << "s" << std::endl;

  file.close();
  return 0;
}


/*
 *  lengthdistribution.cpp
 *  MetaQuant
 *
 *  Kevin McLoughlin
 *  Based on code from eXpress, created by Adam Roberts in 2013.
 *  Copyright 2014 Kevin McLoughlin, Adam Roberts. All rights reserved.
 */

#include "lengthdistribution.h"
#include "main.h"
#include <numeric>
#include <boost/assign.hpp>
#include <iostream>
#include <fstream>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp>

using namespace std;

LengthDistribution::LengthDistribution(double alpha, size_t max_val,
                                       size_t prior_mu, size_t prior_sigma,
                                       size_t kernel_n, double kernel_p,
                                       size_t bin_size)
    : _hist(max_val/bin_size+1),
      _tot_mass(LOG_0),
      _sum(LOG_0),
      _min(max_val/bin_size),
      _bin_size(bin_size) {

  max_val = max_val/bin_size;
  kernel_n = kernel_n/bin_size;
  assert(kernel_n % 2 == 0);

  double tot = log(alpha);

  // Set to prior distribution
  if (prior_mu) {
    boost::math::normal norm(prior_mu/bin_size,
                             prior_sigma/(bin_size*bin_size));

    for (size_t i = 0; i <= max_val; ++i) {
      double norm_mass = boost::math::cdf(norm, i+0.5) -
                         boost::math::cdf(norm, i-0.5);
      double mass = LOG_EPSILON;
      if (norm_mass != 0) {
        mass = tot + log(norm_mass);
      }
      _hist[i] = mass;
      _sum = log_add(_sum, log((double)i)+mass);
      _tot_mass = log_add(_tot_mass, mass);
    }
  } else {
    _hist = vector<double>(max_val + 1, tot - log((double)max_val));
    _hist[0] = LOG_0;
    _sum = _hist[1] + log((double)(max_val * (max_val + 1))) - log(2.);
    _tot_mass = tot;
  }

  // Define kernel
  boost::math::binomial_distribution<double> binom(kernel_n, kernel_p);
  _kernel = vector<double>(kernel_n + 1);
  for (size_t i = 0; i <= kernel_n; i++) {
    _kernel[i] = log(boost::math::pdf(binom, i));
  }
}

LengthDistribution::LengthDistribution(string param_file_name,
                                       string length_type) :
    _bin_size(1) {
  ifstream infile (param_file_name.c_str());
  const size_t BUFF_SIZE = 99999;
  char line_buff[BUFF_SIZE];

  if (!infile.is_open()) {
    logger.severe("Unable to open paramater file '%s'.",
                  param_file_name.c_str());
  }

  do {
    infile.getline (line_buff, BUFF_SIZE, '\n');
  } while (strncmp(line_buff + 1, length_type.c_str(), length_type.size()));

  infile.getline (line_buff, BUFF_SIZE, '\n');
  char *p = strtok(line_buff, "\t");
  size_t i = 0;

  _tot_mass = 0;
  _sum = 0;
  do {
    double val = strtod(p,NULL);
    _hist.push_back(log(val));
    _tot_mass += val;
    _sum += i*val;
    i++;
    p = strtok(NULL, "\t");
  } while (p);

  _tot_mass = log(_tot_mass);
  _sum = log(_sum);
  _min = max_val();;
}

size_t LengthDistribution::max_val() const {
  return (_hist.size()-1) * _bin_size;
}

size_t LengthDistribution::min_val() const {
  if (_min == _hist.size() - 1) {
    return 1;
  }
  return _min;
}

void LengthDistribution::add_val(size_t len, double mass) {
  assert(!isnan(mass));
  assert(_kernel.size());

  len /= _bin_size;

  if (len > max_val()) {
      len = max_val();
  }
  if (len < _min) {
    _min = len;
  }

  size_t offset = len - _kernel.size()/2;

  for (size_t i = 0; i < _kernel.size(); i++) {
    if (offset > 0 && offset < _hist.size()) {
      double k_mass = mass + _kernel[i];
      _hist[offset] = log_add(_hist[offset], k_mass);
      _sum = log_add(_sum, log((double)offset)+k_mass);
      _tot_mass = log_add(_tot_mass, k_mass);
    }
    offset++;
  }
}

double LengthDistribution::pmf(size_t len) const {
  len /= _bin_size;
  if (len > max_val()) {
    len = max_val();
  }
  return _hist[len]-_tot_mass;
}

double LengthDistribution::cmf(size_t len) const {
  double cum = LOG_0;
  vector<double> cdf(_hist.size());
  for (size_t i = 0; i < _hist.size(); ++i) {
    cum = log_add(cum, _hist[i]);

  }
  return cum - _tot_mass;
}

vector<double> LengthDistribution::cmf() const {
  double cum = LOG_0;
  vector<double> cdf(_hist.size());
  for (size_t i = 0; i < _hist.size(); ++i) {
    cum = log_add(cum, _hist[i]);
    cdf[i] = cum - _tot_mass;
  }
  assert(approx_eq(cum, _tot_mass));

  return cdf;
}

double LengthDistribution::tot_mass() const {
  return _tot_mass;
}

double LengthDistribution::mean() const {
  return _sum - tot_mass();
}

string LengthDistribution::to_string() const {
  string s = "";
  char buffer[50];
  for(size_t i = 0; i < _hist.size(); i++) {
    sprintf(buffer, "%e\t",sexp(pmf(i*_bin_size)));
    s += buffer;
  }
  s.erase(s.length()-1,1);
  return s;
}

void LengthDistribution::append_output(ofstream& outfile,
                                       string length_type) const {
  outfile << ">" << length_type << " Length Distribution (0-" << max_val()*_bin_size;
  outfile << ")\n" << to_string() << endl;
}


// Copyright (c) 2018-2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/estimator/KalmanFilter.h>
#include <frc/logging/CSVLogFile.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <units/length.h>
#include <units/velocity.h>

#include "Constants.hpp"

namespace frc3512 {

class ElevatorController {
public:
    // State tolerances in meters and meters/sec respectively.
    static constexpr double kPositionTolerance = 0.05;
    static constexpr double kVelocityTolerance = 2.0;

    ElevatorController();

    ElevatorController(const ElevatorController&) = delete;
    ElevatorController& operator=(const ElevatorController&) = delete;

    void Enable();
    void Disable();
    bool IsEnabled() const;

    void SetScoringIndex();
    void SetClimbingIndex();

    void SetGoal(double goal);

    /**
     * Sets the references.
     *
     * @param position Position of the carriage in meters.
     * @param velocity Velocity of the carriage in meters per second.
     */
    void SetReferences(units::meter_t position,
                       units::meters_per_second_t velocity);

    bool AtReferences() const;

    bool AtGoal() const;

    /**
     * Sets the current encoder measurement.
     *
     * @param measuredPosition Position of the carriage in meters.
     */
    void SetMeasuredPosition(double measuredPosition);

    /**
     * Returns the control loop calculated voltage.
     */
    double ControllerVoltage() const;

    /**
     * Returns the estimated position.
     */
    double EstimatedPosition() const;

    /**
     * Returns the estimated velocity.
     */
    double EstimatedVelocity() const;

    /**
     * Returns the error between the position reference and the position
     * estimate.
     */
    double PositionError() const;

    /**
     * Returns the error between the velocity reference and the velocity
     * estimate.
     */
    double VelocityError() const;

    /**
     * Returns the current position reference set by the profile.
     */
    double PositionReference();

    /**
     * Returns the current velocity reference set by the profile.
     */
    double VelocityReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update();

    /**
     * Resets any internal state.
     */
    void Reset();

private:
    // The current sensor measurement.
    Eigen::Matrix<double, 1, 1> m_y;
    frc::TrapezoidProfile<units::meters>::State m_goal;

    frc::TrapezoidProfile<units::meters>::Constraints scoringConstraints{
        Constants::Elevator::kMaxV, Constants::Elevator::kMaxA};
    frc::TrapezoidProfile<units::meters>::Constraints climbingConstraints{
        Constants::Elevator::kClimbMaxV, Constants::Elevator::kClimbMaxA};
    frc::TrapezoidProfile<units::meters>::Constraints m_activeConstraints =
        scoringConstraints;

    frc::TrapezoidProfile<units::meters>::State m_profiledReference;

    frc::LinearSystem<2, 1, 1> m_scorePlant = [=] {
        constexpr auto motor = frc::DCMotor::NEO();

        // Carriage mass
        constexpr auto m = 9.785262_kg;

        // Radius of pulley
        constexpr auto r = 0.0181864_m;

        // Gear ratio
        constexpr double G = 8.0;

        return frc::LinearSystemId::ElevatorSystem(motor, m, r, G);
    }();

    frc::LinearSystem<2, 1, 1> m_climbPlant = [=] {
        auto motor = frc::DCMotor::NEO();

        // Carriage mass
        constexpr auto m = 8.381376_kg;

        // Radius of pulley
        constexpr auto r = 0.0181864_m;

        // Gear ratio
        constexpr double G = 12.5;

        return frc::LinearSystemId::ElevatorSystem(motor, m, r, G);
    }();

    frc::LinearQuadraticRegulator<2, 1> m_scoreController{
        m_scorePlant, {0.3, 3.0}, {12.0}, Constants::kDt};
    frc::LinearQuadraticRegulator<2, 1> m_climbController{
        m_climbPlant, {0.3, 3.0}, {12.0}, Constants::kDt};

    frc::KalmanFilter<2, 1, 1> m_scoreObserver{
        m_scorePlant, {0.05, 100.0}, {0.0001}, Constants::kDt};
    frc::KalmanFilter<2, 1, 1> m_climbObserver{
        m_climbPlant, {0.05, 100.0}, {0.0001}, Constants::kDt};

    bool m_isEnabled = false;
    bool m_climbing = false;

    Eigen::Matrix<double, 2, 1> m_nextR;
    Eigen::Matrix<double, 1, 1> m_u;

    bool m_atReferences = false;

    frc::CSVLogFile elevatorLogger{"Elevator",   "EstPos (m)",  "EstVel (m/s)",
                                   "RefPos (m)", "Voltage (V)", "RefVel (m/s)"};
};

}  // namespace frc3512


#ifndef MATHTOOLBOX_GRADIENT_DESCENT_HPP
#define MATHTOOLBOX_GRADIENT_DESCENT_HPP

#include <Eigen/Core>

namespace mathtoolbox
{
    namespace optimization
    {
        /// \brief Run a simple gradient descent to find a local minimizer of the specified function
        ///
        /// \details This algorithm uses the Backtracking Line Search algorithm to determine an appropriate step size.
        ///
        /// \param lower_bound The lower bound values. If this is a zero-length empty vector, the algorithm just ignores
        /// the lower bound condition.
        ///
        /// \param upper_bound The upper bound values. If this is a zero-length empty vector, the algorithm just ignores
        /// the upper bound condition.
        ///
        /// \param default_alpha The default step size that the algorithm first tries.
        void RunGradientDescent(const Eigen::VectorXd&                                        x_init,
                                const std::function<double(const Eigen::VectorXd&)>&          f,
                                const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& g,
                                const Eigen::VectorXd&                                        lower_bound,
                                const Eigen::VectorXd&                                        upper_bound,
                                const double                                                  epsilon,
                                const double                                                  default_alpha,
                                const unsigned int                                            max_num_iters,
                                Eigen::VectorXd&                                              x_star,
                                unsigned int&                                                 num_iters);
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_GRADIENT_DESCENT_HPP


#include <gtest/gtest.h>

#include "scheme/numeric/euler_angles.hh"

#include <Eigen/Geometry>

#include <random>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>

#include "scheme/util/Timer.hh"

namespace scheme { namespace numeric { namespace test {

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

TEST(euler_angles,test){
	using namespace Eigen;
	std::mt19937 rng((unsigned int)time(0));
	std::normal_distribution<> gauss;
	std::uniform_real_distribution<> uniform;

	for(int i = 0; i < 10000; ++i){
		Quaterniond q( fabs(gauss(rng)), gauss(rng), gauss(rng), gauss(rng) );
		q.normalize();
		Matrix3d m = q.matrix();
		Vector3d euler;
		euler_angles(m,euler);
		// cout << euler.transpose() << endl;
		// cout << m << endl;
		// cout << endl;
		Matrix3d m2;
		from_euler_angles(euler,m2);
		double thresh = 0.0000001;
		if( euler[2] < 0.000001 || euler[2] > M_PI-0.000001 ) thresh = 0.0002;
		if( !m2.isApprox(m,thresh) ){
			cout << euler << endl;
			cout << m << endl;
			cout << m2 << endl;
		}
		ASSERT_TRUE( m2.isApprox(m,thresh) );

	}
}

TEST( euler_angles, performance ){
	using namespace Eigen;
	std::mt19937 rng((unsigned int)time(0));
	std::normal_distribution<> gauss;
	std::uniform_real_distribution<> uniform;

	int NSAMP = 1*1000*1000;

	std::vector<Matrix3d> samp(NSAMP);
	std::vector<Vector3d> euler(NSAMP);

	for(int i = 0; i < NSAMP; ++i){
		Quaterniond q( fabs(gauss(rng)), gauss(rng), gauss(rng), gauss(rng) );
		q.normalize();
		Matrix3d m = q.matrix();
		samp[i] = m;
	}

	util::Timer<> t;
	for(int i = 0; i < NSAMP; ++i){
		euler_angles(samp[i],euler[i]);
	}
	cout << "rate " << NSAMP/(double)t.elapsed() << endl;

	for(int i = 0; i < NSAMP; ++i){
		// cout << euler.transpose() << endl;
		// cout << m << endl;
		// cout << endl;
		Matrix3d m2;
		from_euler_angles(euler[i],m2);
		double thresh = 0.0000001;
		if( euler[i][2] < 0.000001 || euler[i][2] > M_PI-0.000001 ) thresh = 0.0002;
		if( !m2.isApprox(samp[i],thresh) ){
			cout << euler[i] << endl;
			cout << samp[i] << endl;
			cout << m2 << endl;
		}
		ASSERT_TRUE( m2.isApprox(samp[i],thresh) );

	}
}

}}}


#include "compi.hpp"

#include <complex>

#include <boost/math/quadrature/tanh_sinh.hpp>

#include "integration_routines_template.hpp"
#include "IntegrandFunctionWrapper.hpp"

extern "C" {
    #include "integration_routines.h"
}

struct TanhSinhParameters: public RoutineParametersBase {
    Real x_min;
    Real x_max;

    TanhSinhParameters(PyObject* routine_args, PyObject* routine_kwargs){
        constexpr auto keywords = generate_keyword_list<IntegralRange::finite>();

        if(!PyArg_ParseTupleAndKeywords(routine_args,routine_kwargs,"Odd|OO$pId",const_cast<char**>(keywords.data()),
                &integrand,&x_min,&x_max,
                &args,&kw,
                &full_output, &max_levels,&tolerance)){
            throw could_not_parse_arguments("Unable to parse python arguments to C variables");
        }
    }

    struct result_type:public RoutineParametersBase::result_type {
        size_t levels;
    };
};

TanhSinhParameters::result_type run_integration_routine(const compi_internal::IntegrandFunctionWrapper& f,const TanhSinhParameters& parameters){
    auto integrator = boost::math::quadrature::tanh_sinh<Real>(static_cast<size_t>(parameters.max_levels));
    TanhSinhParameters::result_type result;

    result.result =  integrator.integrate(f,parameters.x_min,parameters.x_max,parameters.tolerance,&(result.err),&(result.l1),&(result.levels));

    return result;
}

extern "C" PyObject* tanh_sinh(PyObject* self, PyObject* args, PyObject* kwargs){
    return integration_routine<TanhSinhParameters>(args,kwargs);
}


#include <gtest/gtest.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>

#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/macroscopic_quantities.hpp"
#include "spectral/polar_to_nodal.hpp"
#include "spectral/quadrature/qhermitew.hpp"

using namespace boltzmann;

TEST(spectral, mqEval)
{
  typedef SpectralBasisFactoryKS::basis_type basis_t;

  double PI = boost::math::constants::pi<double>();

  basis_t basis;
  int K = 30;
  SpectralBasisFactoryKS::create(basis, K);

  Polar2Nodal<> p2n(basis);

  MQEval mq(basis);

  QHermiteW quad(1.0, K);

  double o = 0.1;
  auto f = [PI, o](double x, double y) {
    double cx = (x - o);
    double cy = (y - o);
    return 1. / 2 / PI * std::exp(-cx * cx / 2 - cy * cy / 2);
  };

  Eigen::MatrixXd Nd(K, K);

  for (int i = 0; i < K; ++i) {
    double xi = quad.pts(i);
    double wi = quad.wts(i);
    for (int j = 0; j < K; ++j) {
      double xj = quad.pts(j);
      double wj = quad.wts(j);
      Nd(i, j) = f(xi, xj) * std::sqrt(wi * wj);
    }
  }

  Eigen::VectorXd v(basis.size());

  p2n.to_polar(v, Nd);

  auto evaluator = mq.evaluator();
  evaluator(v);
  const double tol = 1e-12;

  EXPECT_TRUE(std::abs(evaluator.m - 1.0) < tol) << evaluator.m;
  EXPECT_TRUE(std::abs(evaluator.v[0] - o) < tol) << evaluator.v[0];
  EXPECT_TRUE(std::abs(evaluator.v[1] - o) < tol) << evaluator.v[1];

  // std::cout << v << "\n";
}


//  Copyright (c) 2015 Boost.Test team
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE tolerance_01
#include <boost/test/included/unit_test.hpp>
namespace utf = boost::unit_test;

BOOST_AUTO_TEST_CASE(test1, * utf::tolerance(0.00001))
{
  double x = 10.0000000;
  double y = 10.0000001;
  double z = 10.001;
  BOOST_TEST(x == y); // irrelevant difference
  BOOST_TEST(x == z); // relevant difference
}
//]


#include <benchmark.hpp>
#include <pi_helpers.hpp>

#include <boost/mpi.hpp>
#include <gmpxx.h>

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

namespace mpi = boost::mpi;

namespace
{

static constexpr int ROOT_ID = 0;
static constexpr int TAG = 0;

void send_mpf_pi_part(const mpf_class& pi_part, const mpi::communicator& world)
{
    mpf_srcptr pi_part_raw = pi_part.get_mpf_t();

    world.send(ROOT_ID, TAG, pi_part_raw->_mp_prec);
    world.send(ROOT_ID, TAG, pi_part_raw->_mp_size);
    world.send(ROOT_ID, TAG, pi_part_raw->_mp_exp);
    world.send(ROOT_ID, TAG, pi_part_raw->_mp_d, std::abs(pi_part_raw->_mp_size));
}

void recv_and_add_mpf_pi_part(mpf_class& pi, int rank, const mpi::communicator& world)
{
    mpf_t another_pi_part;

    world.recv(rank, TAG, another_pi_part->_mp_prec);
    world.recv(rank, TAG, another_pi_part->_mp_size);
    world.recv(rank, TAG, another_pi_part->_mp_exp);

    auto mp_d = std::make_unique<mp_limb_t[]>(another_pi_part->_mp_prec + 1);
    another_pi_part->_mp_d = mp_d.get();
    world.recv(rank, TAG, another_pi_part->_mp_d, std::abs(another_pi_part->_mp_size));

    mpf_add(pi.get_mpf_t(), pi.get_mpf_t(), another_pi_part);
}

void pi_sum_reduce(const mpf_class& pi_part, mpf_class& pi, const mpi::communicator& world)
{
    // TODO: create a user-defined type for GMP float
    if (world.rank() == ROOT_ID)
    {
        pi = std::move(pi_part);
        std::size_t process_count = world.size();
        for (std::size_t rank = 1; rank < process_count; ++rank)
        {
            recv_and_add_mpf_pi_part(pi, rank, world);
        }
    }
    else
    {
        send_mpf_pi_part(pi_part, world);
    }
}

mpf_class pi_leibniz_mpi(std::size_t summand_count, mp_bitcnt_t precision,
                         const mpi::communicator& world)
{
    mpi::broadcast(world, summand_count, ROOT_ID);

    mpf_class pi_part = my::pi::pi_part_leibniz_mpi(summand_count, precision, world.rank(), world.size());
    world.barrier();

    mpf_class pi(0.0, precision);
    pi_sum_reduce(pi_part, pi, world);

    return 4 * pi;
}

mpf_class pi_bellard_mpi(std::size_t summand_count, mp_bitcnt_t precision,
                         const mpi::communicator& world)
{
    mpi::broadcast(world, summand_count, ROOT_ID);

    mpf_class pi_part = my::pi::pi_part_bellard_mpi(summand_count, precision, world.rank(), world.size());
    world.barrier();

    mpf_class pi(0.0, precision);
    pi_sum_reduce(pi_part, pi, world);

    pi /= (1 << 6);
    return pi;
}

template <typename RegularPiCalculationFunction,
          typename MPIPiCalculationFunction>
void benchmark(std::size_t summand_count,
               mp_bitcnt_t precision,
               const mpi::communicator& world,
               RegularPiCalculationFunction pi_regular,
               MPIPiCalculationFunction pi_mpi)
{
    static constexpr std::size_t ITERATIONS_COUNT = 100;

    if (summand_count < static_cast<std::size_t>(world.size()))
    {
        throw std::runtime_error("Summand count is less than processor count, please decrease number of processors.");
    }

    if (world.rank() == ROOT_ID)
    {
        auto pi_regular_wrapper = [summand_count, precision, pi_regular]()
        {
            return pi_regular(summand_count, precision);
        };
        double pi_regular_result = my::benchmark_function(pi_regular_wrapper, ITERATIONS_COUNT);
        my::print_result("Regular time: ", pi_regular_result);
    }

    double pi_mpi_result;
    {
        auto pi_mpi_wrapper = [summand_count, precision, pi_mpi, world]()
        {
            return pi_mpi(summand_count, precision, world);
        };
        pi_mpi_result = my::benchmark_function(pi_mpi_wrapper, ITERATIONS_COUNT);
    }
    if (world.rank() == ROOT_ID)
    {
        my::print_result("    MPI time: ", pi_mpi_result);
    }
}

template <typename MPIPiCalculationFunction>
void calculate(std::size_t summand_count,
               mp_bitcnt_t precision,
               const mpi::communicator& world,
               MPIPiCalculationFunction pi_mpi)
{
    if (summand_count < static_cast<std::size_t>(world.size()))
    {
        throw std::runtime_error("Summand count is less than processor count, please decrease number of processors.");
    }

    mpf_class pi_mpi_result = pi_mpi(summand_count, precision, world);

    if (world.rank() == ROOT_ID)
    {
        mp_exp_t exp;
        std::string pi_string = pi_mpi_result.get_str(exp);
        assert(exp == 1);
        std::cout << std::string_view(pi_string.data(), 1);
        std::cout << '.';
        std::cout << std::string_view(pi_string.data() + 1, pi_string.size()) << std::endl;
    }
}

}  // namespace

int main(int argc, char* argv[]) try
{
    mpi::environment env(argc, argv);
    mpi::communicator world;

    struct AlgorithmInfo
    {
        const std::function<mpf_class(std::size_t summand_count, mp_bitcnt_t precision)> pi_regular;
        const std::function<mpf_class(std::size_t summand_count, mp_bitcnt_t precision, const mpi::communicator& world)> pi_mpi;
        my::pi::AlgorithmParams params;
    };

    std::unordered_map<my::pi::AlgorithmType, AlgorithmInfo>
    algorithm_info_map =
    {
        {
            my::pi::AlgorithmType::BELLARD,
            {
                .pi_regular = my::pi::pi_bellard_regular,
                .pi_mpi = pi_bellard_mpi,
                .params =
                {
                    .precision = (1 << 26),
                    .benchmark_summand_count = std::size_t{1} << 8,
                    .calculation_summand_count = std::size_t{1} << 22
                }
            }
        },
        {
            my::pi::AlgorithmType::LEIBNIZ,
            {
                .pi_regular = my::pi::pi_leibniz_regular,
                .pi_mpi = pi_leibniz_mpi,
                .params =
                {
                    .precision = (1 << 7),
                    .benchmark_summand_count = std::size_t{1} << 26,
                    .calculation_summand_count = std::size_t{1} << 45
                }
            }
        },
    };

    bool do_benchmark = true;
    auto algorithm = my::pi::AlgorithmType::LEIBNIZ;

    const AlgorithmInfo& algorithm_info = algorithm_info_map.at(algorithm);

    if (do_benchmark)
    {
        benchmark(algorithm_info.params.benchmark_summand_count,
                  algorithm_info.params.precision,
                  world,
                  algorithm_info.pi_regular,
                  algorithm_info.pi_mpi);
    }
    else
    {
        calculate(algorithm_info.params.calculation_summand_count,
                  algorithm_info.params.precision,
                  world,
                  algorithm_info.pi_mpi);
    }

    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    std::cerr << "Exception caught: " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (...)
{
    std::cerr << "An unknown exception caught" << std::endl;
    return EXIT_FAILURE;
}


#include <gtest/gtest.h>

#include "scheme/numeric/bcc_lattice.hh"
#include "scheme/io/dump_pdb_atom.hh"
#include "scheme/util/Timer.hh"
#include <fstream>
#include <random>
#include <boost/foreach.hpp>
#include <iterator>     // std::back_inserter
#include <boost/format.hpp>
#include <sparsehash/dense_hash_set>

#include <Eigen/Geometry>

namespace scheme { namespace numeric { namespace test {

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

// TEST(TEMPORARY,gm_20140905){
// 	int NSAMP = 1000000;
// 	std::mt19937 r((unsigned int)time(0));
// 	std::uniform_real_distribution<> u;
// 	Eigen::Matrix<double,6,6> dis; dis.fill(0);
// 	for(int idata = 1; idata <= 6; ++idata ){
// 		for(int irep  = 1; irep <= idata; ++irep ){
// 			int totbin = 16<<(idata-1)*2;
// 			int nbins = std::pow( totbin, 1.0/irep );
// 			cout << idata << " " << irep << " " << totbin << " " << std::pow(nbins,irep) << endl;
// 			for(int iter = 0; iter < NSAMP; ++iter){
// 				util::SimpleArray<6,double> samp(0.5);
// 				for(int i=0; i<idata; ++i) samp[i] = u(r);
// 				util::SimpleArray<6,double> rep(0.5);
// 				for(int i=0; i<irep; ++i){
// 					rep[i] = ((int)(samp[i]*nbins)+u(r))/nbins;
// 				}
// 				// cout << idata << " " << irep << " " << samp << endl;
// 				// cout << idata << " " << irep << " " << rep << endl;
// 				// cout << endl;
// 				// std::exit(-1);
// 				dis(irep-1,idata-1) += (samp-rep).norm();
// 			}
// 		}
// 	}
// 	dis = dis / (double)NSAMP * 100;
// 	cout << dis << endl;
// }

TEST(bcc_lattice,centers_map){
	typedef util::SimpleArray<3,double> V;
	typedef util::SimpleArray<3,uint64_t> I;

	// {
	// 	BCC<3,double> bcc(I(2,2,2),V(0,0,0),V(1,1,1));
	// 	for(int i = 0; i < bcc.size(); ++i)
	// 		cout << i << "\t" << bcc[i] << endl;

	// }

	BCC<3,double> bcc(I(3,5,7),V(0,0,0),V(6,10,14));

	// std::ofstream out("test.pdb");
	for(size_t i = 0; i < bcc.size(); i+=2){
		ASSERT_EQ( i, bcc[bcc[i]] );
		// printf("%6lu %10.6f %10.6f %10.6f\n",i,bcc[i][0],bcc[i][1],bcc[i][2]);
		// io::dump_pdb_atom(out,i,bcc[i]*3.0);
		// cout << i << " " << bcc[i] << endl;
	}
	// out.close();

}

	// double const R3approx = 0.558099;
	// double const R4approx = 0.701687;
	// double const R5approx = 0.742306;
	// double const R6approx = 0.845359;
	// double const R7approx = 0.882879;
template<int N, class F, class S>
F
test_bcc_performance(
	size_t NSAMP,
	S const Nside,
	F const Width
){
	typedef util::SimpleArray<N,F> V;
	typedef util::SimpleArray<N,S> I;
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> runif;
	BCC<N,F,S> bcc(I(Nside),V(-Width/2),V(Width/2));

	std::vector<V> samples(NSAMP);
	for(int i = 0; i < NSAMP; ++i)
		for(int j = 0; j < N; ++j)
			// samples[i][j] = runif(rng)*9.4+0.3;
			samples[i][j] = runif(rng)*9.0-4.5;

	std::vector<size_t> indices(NSAMP);
	util::Timer<> lookup_time;
	for(int i = 0; i < NSAMP; ++i)
		indices[i] = bcc[samples[i]];
	cout << "BCC DIM " << N << " lookup rate: " << (double)NSAMP / lookup_time.elapsed() << " sec / ";

	std::vector<V> centers(NSAMP);
	util::Timer<> getval_time;
	for(int i = 0; i < NSAMP; ++i)
		centers[i] = bcc[indices[i]];
	cout << " getval rate: " << (double)NSAMP / getval_time.elapsed() << " sec" << endl;

	F maxdiff = 0;
	for(int i = 0; i < NSAMP; ++i)
		maxdiff = std::max( maxdiff, (samples[i]-centers[i]).squaredNorm() );

	maxdiff = sqrt(maxdiff);
	F frac = bcc.width_[0] * sqrt(N)/2.0 / maxdiff;
	F improvement = 1.0; for(int i = 0; i < N; ++i) improvement *= frac;
	cout << "     improvement over cubic: " << improvement / 2.0 << " cov: " << maxdiff / Width * Nside
	     << " vs. " << sqrt(N)/2.0 << endl; // 2 x num samp as cubic

	return maxdiff;
}

TEST(bcc_lattice,performance){
	size_t NITER = 50*1000;
	#ifdef SCHEME_BENCHMARK
	NITER *= 50;
	#endif
	size_t Nside = 100;
	double Width = 10.0;
	double const R3test = test_bcc_performance<3,double,uint64_t>( NITER, Nside, Width );
	                      test_bcc_performance<4,double,uint64_t>( NITER, Nside, Width );
	                      test_bcc_performance<5,double,uint64_t>( NITER, Nside, Width );
	                      test_bcc_performance<6,double,uint64_t>( NITER, Nside, Width );
	                      test_bcc_performance<7,double,uint64_t>( NITER, Nside, Width );
	double const R3 = std::pow(2.0,-5.0/3.0)*sqrt(5) / std::pow(2.0,1.0/3.0) * Width / Nside;
	ASSERT_LE( R3test     , R3 );
	ASSERT_GT( R3test*1.1 , R3 );
}

template<int N, class F, class S>
F
test_bcc_inradius(){
	typedef util::SimpleArray<N,F> V;
	typedef util::SimpleArray<N,S> I;
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	S const Nside = 5;
	BCC<N,F,S> bcc(I(5),V(-(F)Nside/2.0),V((F)Nside/2.0));
	BOOST_VERIFY( bcc[bcc[V(0.0)]] == V(0.0) );
	BOOST_VERIFY( bcc.width_ == V(1.0) );
	S const i0 = bcc[V(0)];

	double const RNapprox[5] = { 0.558099, 0.701687, 0.742306, 0.845359, 0.882879 };
	double const Rapprox = RNapprox[N-3];

	int NSAMP = 50*1000;

	double min_inrad = Rapprox;
	for(int i = 0; i < NSAMP; ++i){
		V samp;
		for(int j = 0; j < N; ++j) samp[j] = rnorm(rng);
		samp.normalize();
		double const radius = runif(rng)*min_inrad*0.1 + 0.9*min_inrad;
		samp *= radius;
		if( bcc[samp] != i0 ){
			min_inrad = radius;
		}

	}
	return min_inrad;
}

TEST(bcc_lattice,inradius){
	ASSERT_NEAR( (test_bcc_inradius<3,double,uint64_t>()), 0.433015, 0.03 );
	ASSERT_NEAR( (test_bcc_inradius<4,double,uint64_t>()), 0.500000, 0.03 );
	ASSERT_NEAR( (test_bcc_inradius<5,double,uint64_t>()), 0.500000, 0.03 );
	ASSERT_NEAR( (test_bcc_inradius<6,double,uint64_t>()), 0.500000, 0.03 );
	ASSERT_NEAR( (test_bcc_inradius<7,double,uint64_t>()), 0.500000, 0.03 );
}

template<int N, class F, class S>
F
test_bcc_neighbors( size_t NSAMP ){
	typedef util::SimpleArray<N,F> V;
	typedef util::SimpleArray<N,S> I;
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	S const Nside = 5;
	BCC<N,F,S> bcc(I(5),V(-(F)Nside/2.0),V((F)Nside/2.0));
	BOOST_VERIFY( bcc[bcc[V(0.0)]] == V(0.0) );
	BOOST_VERIFY( bcc.width_ == V(1.0) );
	S const i0 = bcc[V(0)];
	std::vector<size_t> nbrs,nbrs_we;
	bcc.neighbors( i0, std::back_inserter(nbrs   ), false );
	bcc.neighbors( i0, std::back_inserter(nbrs_we), true );

	Cubic<N,F,S> cubic(I(5),V(-(F)Nside/2.0),V((F)Nside/2.0));
	BOOST_VERIFY( cubic[cubic[V(0.0)]] == V(0.0) );
	BOOST_VERIFY( cubic.width_ == V(1.0) );
	S const i0cubic = cubic[V(0)];
	std::vector<size_t> nbrs_cubic;
	cubic.neighbors( i0cubic, std::back_inserter(nbrs_cubic) );

	/////////////////////////////////////////////////////
	// test neighbor coverage
	///////////////////////////////////////////////////////////////

	F maxrad_99 = 0;
	// F const RNapprox[5] = { 0.558099, 0.701687, 0.742306, 0.845359, 0.882879 };
	// F const Rapprox = RNapprox[N-3];
	F inrad = 0.5;
	if(N==3) inrad = 0.433015;
	// F const radius = 1.9*inrad;
	for(F radius = 0; radius < 3.0*inrad; radius += inrad/10.0){
	// F radius = 2.1*inrad; {

		int sum_in_nbrs=0, sum_in_nbrs_we=0;
		for(int i = 0; i < NSAMP; ++i){

			// pick random point in i0 cell
			V cen; for(int j = 0; j < N; ++j) cen[j] = (runif(rng)-0.5)*2.0*inrad;
			if( bcc[cen] != i0 ){ --i; continue; }

			V samp;
			for(int j = 0; j < N; ++j) samp[j] = rnorm(rng);
			samp *= (radius/samp.norm());
			S index = bcc[samp+cen];
			// if( index == i0 ){ --i; continue; }
			if( std::find( nbrs_we.begin(), nbrs_we.end(), index ) != nbrs_we.end() ){
				++sum_in_nbrs_we;
				if( std::find( nbrs.begin(), nbrs.end(), index ) != nbrs.end() ){
					++sum_in_nbrs;
				}
			}
		}
		if( (F)sum_in_nbrs_we/NSAMP > 0.99 ) maxrad_99 = radius;

		// uncomment for print report
		if( fabs( radius - 2.0*inrad ) > 0.001 ) continue;
		int sum_in_nbrs_cubic=0;
		for(int i = 0; i < NSAMP; ++i){
			// pick random point in i0 cell
			V cen; for(int j = 0; j < N; ++j) cen[j] = (runif(rng)-0.5);
			BOOST_VERIFY( cubic[cen] == i0cubic );

			V samp;
			for(int j = 0; j < N; ++j) samp[j] = rnorm(rng);
			samp *= (radius/inrad*0.5/samp.norm());
			S index = cubic[samp+cen];
			// if( index == i0 ){ --i; continue; }
			if( std::find( nbrs_cubic.begin(), nbrs_cubic.end(), index ) != nbrs_cubic.end() ){
				++sum_in_nbrs_cubic;
			}
		}
		printf("%i %7.3f %9.7f %9.7f %9.7f\n",
			N,
			radius/inrad,
			(F)sum_in_nbrs/NSAMP,
			(F)sum_in_nbrs_we/NSAMP,
			(F)sum_in_nbrs_cubic/NSAMP
		);

	}

	// ///////////////////////////////////////////////////////////////////
	// // // dump neighbors
	// //////////////////////////////////////////////////////////////////
	// if(N != 3) return;
	// #define LAT bcc
	// std::ofstream out_bcc("bcc.pdb");
	// for(int i = 0; i < LAT.size(); ++i) io::dump_pdb_atom(out_bcc,i,LAT[i]*10.0);
	// out_bcc.close();
	// for(int i = 0; i < LAT.size(); ++i){
	// 	std::vector<size_t> nbrs;
	// 	LAT.neighbors( i, std::back_inserter(nbrs), true );
	// 	// BOOST_FOREACH(size_t nbr, nbrs) cout << nbr << " " << LAT[nbr] << endl;
	// 	std::string s = boost::str(boost::format("%4i") % i);
	// 	std::ofstream out("test_"+s+".pdb");
	// 	BOOST_FOREACH(size_t nbr, nbrs) io::dump_pdb_atom(out,nbr,LAT[nbr]*10.0);
	// 	out.close();
	// }

	return maxrad_99;
}

TEST(bcc_lattice,neighbors){
	size_t NITER = 1000;
	#ifdef SCHEME_BENCHMARK
	NITER *= 50;
	#endif
	// for(int i = 3; i < 8; ++i){
	// 	int nc = std::pow(3,i);
	// 	int nbccFC = 1+2*i+std::pow(2,i);
	// 	int nbccFCE = nbccFC + i*(i-1)/2 * 4;
	// 	cout << "Nnbrs: " << i <<" cubic "<< nc << " bccFC " << nbccFC << " bccFCE " << nbccFCE << endl;
	// }
	// Nnbrs: 3 cubic   27 bccFC  15 bccFCE  27
	// Nnbrs: 4 cubic   81 bccFC  25 bccFCE  49
	// Nnbrs: 5 cubic  243 bccFC  43 bccFCE  83
	// Nnbrs: 6 cubic  729 bccFC  77 bccFCE 137
	// Nnbrs: 7 cubic 2187 bccFC 143 bccFCE 227
	double v1 = test_bcc_neighbors<3,double,uint64_t>(NITER);
	double v2 = test_bcc_neighbors<4,double,uint64_t>(NITER);
	double v3 = test_bcc_neighbors<5,double,uint64_t>(NITER);
	double v4 = test_bcc_neighbors<6,double,uint64_t>(NITER);
	double v5 = test_bcc_neighbors<7,double,uint64_t>(NITER);
	EXPECT_LE( 0.73, v1 ); // these are total approximations
	EXPECT_LE( 0.69, v2 ); // these are total approximations
	EXPECT_LE( 0.64, v3 ); // these are total approximations
	EXPECT_LE( 0.57, v4 ); // these are total approximations
	EXPECT_LE( 0.51, v5 ); // these are total approximations
	// cout << test_bcc_neighbors<3,double,size_t>(1000000) << endl; // 0.779427
	// cout << test_bcc_neighbors<4,double,size_t>(1000000) << endl; // 0.75
	// cout << test_bcc_neighbors<5,double,size_t>(1000000) << endl; // 0.7
	// cout << test_bcc_neighbors<6,double,size_t>(1000000) << endl; // 0.65
	// cout << test_bcc_neighbors<7,double,size_t>(1000000) << endl; // 0.6
}

// TEST(bcc_lattice,coverage_transform_7d){
// 	using namespace Eigen;
// 	typedef Transform<double,3,AffineCompact> Xform;
// 	typedef util::SimpleArray<7,double> V;
// 	typedef util::SimpleArray<7,size_t> I;
// 	typedef Matrix<double,7,1> Vector7d;
// 	std::mt19937 mt((unsigned int)time(0));
// 	std::normal_distribution<> rnorm;
// 	size_t Nside = 64;
// 	V bounds = V(1.5,1.5,1.5,1.5,10,10,10);
// 	BCC<7,double> bcc(I(Nside),-bounds,bounds);

// 	Matrix<double,3,6> pts0;
// 	pts0 <<  1, 0, 0,-2, 0, 0,
// 	         0, 1, 0, 0,-1, 0,
// 	         0, 0, 1, 0, 0,-1;
// 	pts0.colwise() -= pts0.rowwise().sum()/pts0.cols();

// 	Xform X( AngleAxisd(2,Vector3d(1,2,3)) );

// 	// cout << pts0 << endl;
// 	// cout << X*pts0 << endl;

// 	int const NSAMP = 3;
// 	for(int i = 0; i < NSAMP; ++i){
// 		Vector4d quat( rnorm(mt), rnorm(mt), rnorm(mt), rnorm(mt) ); quat.normalize();
// 		Vector3d trans( rnorm(mt), rnorm(mt), rnorm(mt) );
// 		Vector7d samp;
// 		samp.block(0,0,4,1) = quat;
// 		samp.block(4,0,3,1) = trans;
// 		// cout << samp.transpose() << endl;
// 		V tmp = bcc[ bcc[ samp ] ];
// 		Vector7d cen;
// 		for(int i = 0; i < 7; ++i) cen[i] = tmp[i];

// 		cout << samp.transpose() << endl;
// 		cout << cen.transpose() << endl;
// 		cout << endl;

// // 	std::vector<size_t> indices(NSAMP);
// // 	util::Timer<> lookup_time;
// // 	for(int i = 0; i < NSAMP; ++i)
// // 		indices[i] = bcc[samples[i]];
// // 	cout << N << " lookup: " << (double)NSAMP / lookup_time.elapsed() << " sec" << endl;

// // 	std::vector<V> centers(NSAMP);
// // 	util::Timer<> getval_time;
// // 	for(int i = 0; i < NSAMP; ++i)
// // 		centers[i] = bcc[indices[i]];

// 	}

// }

template<int N, class F, class S>
F
test_bcc_children( size_t NSAMP ){
	typedef util::SimpleArray<N,F> V;
	typedef util::SimpleArray<N,S> I;
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	S const Nside = 5;
	BCC<N,F,S> bcc_parent(I(5),V(-(F)Nside),V((F)Nside));
	BCC<N,F,S> bcc(I(5),V(-(F)Nside/2.0),V((F)Nside/2.0));
	BOOST_VERIFY( bcc[bcc[V(0.0)]] == V(0.0) );
	BOOST_VERIFY( bcc.width_ == V(1.0) );
	S const i0 = bcc[V(0)];
	std::vector<size_t> nbrs,nbrs_we;
	bcc.neighbors( i0, std::back_inserter(nbrs   ), false );
	bcc.neighbors( i0, std::back_inserter(nbrs_we), true );

	/////////////////////////////////////////////////////
	// test neighbor coverage
	///////////////////////////////////////////////////////////////

	// F const RNapprox[5] = { 0.558099, 0.701687, 0.742306, 0.845359, 0.882879 };
	// F const Rapprox = RNapprox[N-3];
	F inrad = 0.5;
	if(N==3) inrad = 0.433015;

	int sum_in_nbrs=0, sum_in_nbrs_we=0;
	for(int i = 0; i < NSAMP; ++i){

		// pick random point in i0 parent cell
		V samp; for(int j = 0; j < N; ++j)
			samp[j] = (runif(rng)-0.5)*4.0*inrad;
		if( bcc_parent[samp] != i0 ){ --i; continue; }

		S index = bcc[samp];
		// if( index == i0 ){ --i; continue; }
		if( std::find( nbrs_we.begin(), nbrs_we.end(), index ) != nbrs_we.end() ){
			++sum_in_nbrs_we;
			if( std::find( nbrs.begin(), nbrs.end(), index ) != nbrs.end() ){
				++sum_in_nbrs;
			}
		}
	}

	//int nc = std::pow(3,N);
	int nbccFC = 1+2*N+std::pow(2,N);
	int nbccFCE = nbccFC + N*(N-1)/2 * 4;
	printf("BCC child coverage: %i %3i %9.7f   %3i %9.7f\n",
		N,
		nbccFC, (F)sum_in_nbrs/NSAMP,
		nbccFCE, (F)sum_in_nbrs_we/NSAMP
	);
	return 0;
}

TEST(bcc_lattice,children){
	int NITER = 5*1000;
	#ifdef SCHEME_BENCHMARK
	NITER *= 30;
	#endif

	cout << "BCC               DIM Nfc   frac_fc  Nfce  frac_fce" << std::endl;
	test_bcc_children<3,double,uint64_t>( NITER );
	test_bcc_children<4,double,uint64_t>( NITER );
	test_bcc_children<5,double,uint64_t>( NITER );
	test_bcc_children<6,double,uint64_t>( NITER );
	test_bcc_children<7,double,uint64_t>( NITER );
}

}}}


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

#pragma once

#include <Eigen/Dense>

namespace vinecopulib {

namespace tools_interpolation {
//! A class for cubic spline interpolation of bivariate copulas
//!
//! The class is used for implementing kernel estimators. It makes storing the
//! observations obsolete and allows for fast numerical integration.
class InterpolationGrid
{
public:
  InterpolationGrid() {}

  InterpolationGrid(const Eigen::VectorXd& grid_points,
                    const Eigen::MatrixXd& values,
                    int norm_times = 3);

  Eigen::MatrixXd get_values() const;

  void set_values(const Eigen::MatrixXd& values, int norm_times = 3);

  void flip();

  void normalize_margins(int times);

  Eigen::VectorXd interpolate(const Eigen::MatrixXd& x);

  Eigen::VectorXd integrate_1d(const Eigen::MatrixXd& u, size_t cond_var);

  Eigen::VectorXd integrate_2d(const Eigen::MatrixXd& u);

private:
  Eigen::Matrix<ptrdiff_t, 1, 2> get_indices(double x0, double x1);
  double bilinear_interpolation(double z11,
                                double z12,
                                double z21,
                                double z22,
                                double x1,
                                double x2,
                                double y1,
                                double y2,
                                double x,
                                double y);
  double int_on_grid(const double& upr,
                     const Eigen::VectorXd& vals,
                     const Eigen::VectorXd& grid);

  Eigen::VectorXd grid_points_;
  Eigen::MatrixXd values_;
};
}
}

#include <vinecopulib/misc/implementation/tools_interpolation.ipp>


#include <boost/test/unit_test.hpp>

#include <scorum/utils/fraction.hpp>

#include <scorum/protocol/asset.hpp>

#include "defines.hpp"

#include <limits>

namespace fraction_tests {
using namespace scorum;
using namespace scorum::protocol;

BOOST_AUTO_TEST_SUITE(fraction_tests)

BOOST_AUTO_TEST_CASE(fraction_creation_check)
{
    BOOST_CHECK_THROW(utils::make_fraction(ASSET_SP(10).amount, 0);, fc::assert_exception);

    {
        auto f = utils::make_fraction(1, 2);
        BOOST_CHECK_EQUAL(f.numerator, 1);
        BOOST_CHECK_EQUAL(f.denominator, 2);
    }
    {
        auto f = utils::make_fraction(ASSET_SP(10).amount, 2);
        BOOST_CHECK_EQUAL(f.numerator, 10);
        BOOST_CHECK_EQUAL(f.denominator, 2);
    }
    {
        auto f = utils::make_fraction(1, ASSET_SP(10).amount);
        BOOST_CHECK_EQUAL(f.numerator, 1);
        BOOST_CHECK_EQUAL(f.denominator, 10);
    }
    {
        auto f = utils::make_fraction(ASSET_SP(10).amount, ASSET_SCR(20).amount);
        BOOST_CHECK_EQUAL(f.numerator, 10);
        BOOST_CHECK_EQUAL(f.denominator, 20);
    }
}

BOOST_AUTO_TEST_CASE(multiply_by_fractional_negative_check)
{
    // negative value
    BOOST_CHECK_THROW(utils::multiply_by_fractional(-10, 1, 2), fc::assert_exception);
    BOOST_CHECK_THROW(utils::multiply_by_fractional(10, -1, 2), fc::assert_exception);
    BOOST_CHECK_THROW(utils::multiply_by_fractional(10, 1, -2), fc::assert_exception);
    BOOST_CHECK_THROW(utils::multiply_by_fractional(-10, -1, -2), fc::assert_exception);
    // zero denominator
    BOOST_CHECK_THROW(utils::multiply_by_fractional(10, 1, 0), fc::assert_exception);
}

BOOST_AUTO_TEST_CASE(multiply_by_fractional_positive_check)
{
    // claculate half off int32 max
    BOOST_CHECK_EQUAL(utils::multiply_by_fractional(std::numeric_limits<int32_t>::max(), 50, 100),
                      std::numeric_limits<int32_t>::max() / 2);

    // claculate half off int64 max
    BOOST_CHECK_EQUAL(utils::multiply_by_fractional(std::numeric_limits<int64_t>::max(), 50, 100),
                      std::numeric_limits<int64_t>::max() / 2);

    // claculate half off asset_symbol_type max
    BOOST_CHECK_EQUAL(utils::multiply_by_fractional(std::numeric_limits<asset_symbol_type>::max(), 50, 100),
                      std::numeric_limits<asset_symbol_type>::max() / 2);
}

BOOST_AUTO_TEST_CASE(asset_with_fractional_operations_check)
{
    // calculate 20 % of 100 SP
    {
        auto value = ASSET_SP(100e+9);
        value *= utils::make_fraction(20, 100);
        BOOST_CHECK_EQUAL(value, ASSET_SP(20e+9));
    }

    // calculate 20 % of 100 SP
    {
        auto value = ASSET_SP(100e+9) * utils::make_fraction(20, 100);
        BOOST_CHECK_EQUAL(value, ASSET_SP(20e+9));
    }

    // calculate 20 % of maximum SP
    {
        auto value = asset::maximum(SP_SYMBOL) * utils::make_fraction(20, 100);
        BOOST_CHECK_EQUAL(value, asset::maximum(SP_SYMBOL) / 5);
    }
}

BOOST_AUTO_TEST_CASE(fraction_simplify_check)
{
    BOOST_CHECK(utils::make_fraction(20'000, 100'000).simplify() == utils::make_fraction(1, 5));

    BOOST_CHECK(utils::make_fraction(8, 12).simplify() == utils::make_fraction(2, 3));
    BOOST_CHECK(utils::make_fraction(-8, 12).simplify() == utils::make_fraction(-2, 3));
    BOOST_CHECK(utils::make_fraction(8, -12).simplify() == utils::make_fraction(2, -3));
    BOOST_CHECK(utils::make_fraction(-8, -12).simplify() == utils::make_fraction(-2, -3));

    BOOST_CHECK(utils::make_fraction(2, 3).simplify() == utils::make_fraction(2, 3));
    BOOST_CHECK(utils::make_fraction(-2, 3).simplify() == utils::make_fraction(-2, 3));
    BOOST_CHECK(utils::make_fraction(2, -3).simplify() == utils::make_fraction(2, -3));
    BOOST_CHECK(utils::make_fraction(-2, -3).simplify() == utils::make_fraction(-2, -3));
}

BOOST_AUTO_TEST_CASE(fraction_invert_check)
{
    BOOST_CHECK(utils::make_fraction(2, 3).invert() == utils::make_fraction(1, 3));

    BOOST_CHECK(utils::make_fraction(12, 20).invert() == utils::make_fraction(8, 20));

    BOOST_CHECK(utils::make_fraction(-2, 20).invert() == utils::make_fraction(22, 20));

    BOOST_CHECK(utils::make_fraction(-12, -20).invert() == utils::make_fraction(8, 20));
}

BOOST_AUTO_TEST_CASE(fraction_coup_check)
{
    BOOST_CHECK(utils::make_fraction(2, 3).coup() == utils::make_fraction(3, 2));

    BOOST_CHECK_THROW(utils::make_fraction(0, 2).coup(), fc::assert_exception);
}

BOOST_AUTO_TEST_SUITE_END()
}


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

#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <ceres-error-terms/position-error-term.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

class PosegraphErrorTerms : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    prior_position_ << 1, 2, 3;
    prior_orientation_.coeffs() << sqrt(2) / 2, 0, 0, sqrt(2) / 2;

    current_pose_ << prior_orientation_.coeffs(), prior_position_;

    covariance_matrix_.setIdentity();
  }

  void addResidual();
  void solve();

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  Eigen::Vector3d prior_position_;
  Eigen::Quaterniond prior_orientation_;

  Eigen::Matrix<double, 7, 1> current_pose_;
  Eigen::Matrix<double, 3, 3> covariance_matrix_;
};

void PosegraphErrorTerms::addResidual() {
  ceres::CostFunction* cost_function = new ceres_error_terms::PositionErrorTerm(
      prior_position_, covariance_matrix_);

  problem_.AddResidualBlock(cost_function, NULL, current_pose_.data());
  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;
  problem_.SetParameterization(current_pose_.data(), pose_parameterization);
}

void PosegraphErrorTerms::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.parameter_tolerance = 1e-20;
  options.gradient_tolerance = 1e-20;
  options.function_tolerance = 1e-20;
  options.max_num_iterations = 1e3;
  ceres::Solve(options, &problem_, &summary_);

  LOG(INFO) << summary_.BriefReport() << std::endl;
  LOG(INFO) << summary_.message << std::endl;
}

TEST_F(PosegraphErrorTerms, TestPositionPriorErrorTermZeroCost) {
  addResidual();
  solve();

  EXPECT_EQ(summary_.initial_cost, 0.0);
  EXPECT_EQ(summary_.final_cost, 0.0);
  EXPECT_EQ(summary_.iterations.size(), 1u);
}

TEST_F(PosegraphErrorTerms, TestPositionPriorErrorTermPositionOptimization) {
  current_pose_.tail(3) << 4, 7, 4;
  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(current_pose_.tail(3), prior_position_, 1e-10);
  EXPECT_LT(summary_.final_cost, 1e-10);
}

TEST_F(PosegraphErrorTerms, TestPositionPriorErrorTermPositionOptimization2) {
  current_pose_.tail(3) << 2.484e8, -1.264e9, -8.4567e24;
  addResidual();
  solve();

  EXPECT_NEAR_EIGEN(current_pose_.tail(3), prior_position_, 1e-10);
  EXPECT_LT(summary_.final_cost, 1e-10);
}

MAPLAB_UNITTEST_ENTRYPOINT


#define BOOST_TEST_MODULE "test_vector"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <mill/math/Vector.hpp>

#include <random>
constexpr static unsigned int seed = 32479327;
constexpr static std::size_t N = 10000;

BOOST_AUTO_TEST_CASE(add_vector_3d)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const mill::Vector<double, 3> lhs(uni(mt), uni(mt), uni(mt));
        const mill::Vector<double, 3> rhs(uni(mt), uni(mt), uni(mt));

        mill::Vector<double, 3> vec = lhs + rhs;
        BOOST_CHECK_EQUAL(vec[0], lhs[0] + rhs[0]);
        BOOST_CHECK_EQUAL(vec[1], lhs[1] + rhs[1]);
        BOOST_CHECK_EQUAL(vec[2], lhs[2] + rhs[2]);
    }
}

BOOST_AUTO_TEST_CASE(sub_vector_3d)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const mill::Vector<double, 3> lhs(uni(mt), uni(mt), uni(mt));
        const mill::Vector<double, 3> rhs(uni(mt), uni(mt), uni(mt));

        mill::Vector<double, 3> vec = lhs - rhs;
        BOOST_CHECK_EQUAL(vec[0], lhs[0] - rhs[0]);
        BOOST_CHECK_EQUAL(vec[1], lhs[1] - rhs[1]);
        BOOST_CHECK_EQUAL(vec[2], lhs[2] - rhs[2]);
    }
}

BOOST_AUTO_TEST_CASE(mul_vector_3d)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const mill::Vector<double, 3> lhs(uni(mt), uni(mt), uni(mt));
        const double scl = uni(mt);

        mill::Vector<double, 3> vec = lhs * scl;
        BOOST_CHECK_EQUAL(vec[0], lhs[0] * scl);
        BOOST_CHECK_EQUAL(vec[1], lhs[1] * scl);
        BOOST_CHECK_EQUAL(vec[2], lhs[2] * scl);
    }
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_MODIFIED_BESSEL_SECOND_KIND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_MODIFIED_BESSEL_SECOND_KIND_HPP

#include <boost/math/special_functions/bessel.hpp>

namespace stan {
  namespace math {

    /**
     *
       \f[
       \mbox{modified\_bessel\_second\_kind}(v, z) =
       \begin{cases}
         \textrm{error} & \mbox{if } z \leq 0 \\
         K_v(z) & \mbox{if } z > 0 \\[6pt]
         \textrm{NaN} & \mbox{if } z = \textrm{NaN}
       \end{cases}
       \f]

       \f[
       \frac{\partial\, \mbox{modified\_bessel\_second\_kind}(v, z)}{\partial z} =
       \begin{cases}
         \textrm{error} & \mbox{if } z \leq 0 \\
         \frac{\partial\, K_v(z)}{\partial z} & \mbox{if } z > 0 \\[6pt]
         \textrm{NaN} & \mbox{if } z = \textrm{NaN}
       \end{cases}
       \f]

       \f[
       {K_v}(z)
       =
       \frac{\pi}{2}\cdot\frac{I_{-v}(z) - I_{v}(z)}{\sin(v\pi)}
       \f]

       \f[
       \frac{\partial \, K_v(z)}{\partial z} = -\frac{v}{z}K_v(z)-K_{v-1}(z)
       \f]
     *
     */
    template<typename T2>
    inline T2
    modified_bessel_second_kind(int v, const T2 z) {
      return boost::math::cyl_bessel_k(v, z);
    }

  }
}

#endif


#include "stratton-chu/distorted-surface.hpp"

#include <boost/math/special_functions/legendre.hpp>

using namespace boost::math;

double legendre(int num, double arg)
{
    if (arg > 1.0)
        return legendre_p(num, 1.0);
    if (arg < -1.0)
        return legendre_p(num, -1.0);
    return legendre_p(num, arg);
}

double legendre_derivative(int num, double arg)
{
    if (arg > 1.0)
        return 0.0;
    if (arg < -1.0)
        return 0.0;
    return legendre_p_prime(num, arg);
}

SurfaceDistortionHarmonic::SurfaceDistortionHarmonic(
        const ISurface& pure_surface,
        const Vector& v,
        const std::vector<DistortionHarmonic>& harmonics) :  // v - unit vector
    m_pure_surface(pure_surface), m_harmonics(harmonics), m_v(v)
{}

Position SurfaceDistortionHarmonic::point(const Vector2D& pos) const
{
    Position result = m_pure_surface.point(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * cos(m_harmonics[i].kx * pos[0] + m_harmonics[i].ky * pos[1]);
        Vector delta = m_v * shift;
        result += delta;
    }

    return result;
}

Vector SurfaceDistortionHarmonic::tau1(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau1(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = - m_harmonics[i].ampl * m_harmonics[i].kx * sin(m_harmonics[i].kx * pos[0] + m_harmonics[i].ky * pos[1]);
        Vector delta =  m_v * shift;
        result += delta;
    }
    return result;
}

Vector SurfaceDistortionHarmonic::tau2(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau2(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = - m_harmonics[i].ampl * m_harmonics[i].ky * sin(m_harmonics[i].kx * pos[0] + m_harmonics[i].ky * pos[1]);
        Vector delta = m_v * shift;
        result += delta;
    }
    return result;
}

SurfaceDistortionLegendre::DistortionPolinom::DistortionPolinom(double ampl, double alpha, int number) :
    ampl(ampl), direction(cos(alpha), sin(alpha)), number(number)
{
}

SurfaceDistortionLegendre::SurfaceDistortionLegendre(
        const ISurface& pure_surface,
        const Vector& v,
        double radius,
        Vector2D center,
        const std::vector<DistortionPolinom>& harmonics) :  // v - unit vector
    m_pure_surface(pure_surface), m_harmonics(harmonics), m_v(v), m_center(center), m_radius(radius)
{
}

Position SurfaceDistortionLegendre::point(const Vector2D& pos) const
{
    Position result = m_pure_surface.point(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * legendre(m_harmonics[i].number, m_harmonics[i].direction * (pos - m_center) / m_radius);
        Vector delta = m_v * shift;
        result += delta;
    }

    return result;
}

Vector SurfaceDistortionLegendre::tau1(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau1(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * m_harmonics[i].direction[0] / m_radius * legendre_derivative(m_harmonics[i].number, m_harmonics[i].direction * (pos - m_center) / m_radius);
        Vector delta =  m_v * shift;
        result += delta;
    }
    return result;
}

Vector SurfaceDistortionLegendre::tau2(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau2(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * m_harmonics[i].direction[1] / m_radius * legendre_derivative(m_harmonics[i].number, m_harmonics[i].direction * (pos - m_center) / m_radius);
        Vector delta =  m_v * shift;
        result += delta;
    }
    return result;
}


/*
 * Advent of Code 2016
 * Day 3 (part 1)
 *
 * Command: clang++ --std=c++14 -I/usr/local/include -L/usr/local/lib day03a.cpp
 *
 */

#include <cmath>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>

const int MAX_LINE_LENGTH = 2000;

class Triangle {
	public:
		Triangle(int, int, int);
		Triangle(std::string, std::string, std::string);
		int a;
		int b;
		int c;
		bool valid();
};

Triangle::Triangle(int a, int b, int c) {
	this->a = a;
	this->b = b;
	this->c = c;
}

Triangle::Triangle(std::string a, std::string b, std::string c) {
	this->a = atoi(a.c_str());
	this->b = atoi(b.c_str());
	this->c = atoi(c.c_str());
}

bool Triangle::valid() {
	return ((this->a + this->b > this->c) && (this->a + this->c > this->b) && (this->b + this->c > this->a));
}

void split(const std::string &s, char delim, std::vector<std::string> &elems) {
	std::stringstream ss;
	ss.str(s);
	std::string item;
	while (std::getline(ss, item, delim)) {
		elems.push_back(item);
	}
}

std::vector<std::string> split(const std::string &s, char delim) {
	std::vector<std::string> elems;
	split(s, delim, elems);
	return elems;
}

int main(void) {
	// Open the input file
	std::ifstream fin("input03.txt");
	if (!fin) {
		std::cerr << "Error reading input file input03.txt" << std::endl;
		return -1;
	}

	std::vector<Triangle> candidates;

	// Read the input
	std::vector<std::string> input;
	char cInput[MAX_LINE_LENGTH];
	while (fin.getline(cInput, MAX_LINE_LENGTH)) {
		using namespace std;
		using namespace boost;

		vector<string> bits;
		string input(cInput);
		tokenizer<> tokenizer(input);

		for (auto token : tokenizer) {
			bits.push_back(token);
		}

		Triangle t(bits.at(0), bits.at(1), bits.at(2));
		candidates.push_back(t);
	}
	fin.close();

	// Solve the problem
	int triangles = 0; int other = 0;
	for (auto t : candidates) {
		if (t.valid()) {
			triangles++;
		} else {
			other++;
		}
	}

	std::cout << "Triangles: " << triangles << std::endl << "Non-triangles: " << other << std::endl;

	return 0;
}


/*
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
 */

#ifndef __eigenpy_geometry_conversion_hpp__
#define __eigenpy_geometry_conversion_hpp__

#include "eigenpy/fwd.hpp"
#include <Eigen/Geometry>

namespace eigenpy
{

  namespace bp = boost::python;

  template<typename Scalar,int Options=0>
  struct EulerAnglesConvertor
  {

    typedef typename Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef typename Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef typename Vector3::Index Index;

    typedef typename Eigen::AngleAxis<Scalar> AngleAxis;

    static void expose()
    {
      bp::def("toEulerAngles",&EulerAnglesConvertor::toEulerAngles,
              bp::args("mat (dim 3x3)","a0","a1","a2"),
              "It returns the Euler-angles of the rotation matrix mat using the convention defined by the triplet (a0,a1,a2).");

      bp::def("fromEulerAngles",&EulerAnglesConvertor::fromEulerAngles,
              bp::args("ea (vector of Euler angles)","a0","a1","a2"),
              "It returns the rotation matrix associated to the Euler angles using the convention defined by the triplet (a0,a1,a2).");
    }

    static Vector3 toEulerAngles(const Matrix3 & mat,
                                 Index a0,
                                 Index a1,
                                 Index a2)
    {
      return mat.eulerAngles(a0,a1,a2);
    }

    static Matrix3 fromEulerAngles(const Vector3 & ea,
                                   Index a0,
                                   Index a1,
                                   Index a2)
    {
      Matrix3 mat;
      mat = AngleAxis(ea[0], Vector3::Unit(a0))
      * AngleAxis(ea[1], Vector3::Unit(a1))
      * AngleAxis(ea[2], Vector3::Unit(a2));
      return mat;
    }
  };

} // namespace eigenpy

#endif // define __eigenpy_geometry_conversion_hpp__