//
// Tests the iLQR Taylor expansions of the dynamics and the cost function.
//

#include <ilqr/ilqr_taylor_expansions.hh>
#include <utils/math_utils.hh>
#include <utils/debug_utils.hh>

#include <Eigen/Dense>

void test_nonlinear_dynamics_linearization(const int state_dim, const int control_dim)
{
    std::srand(1);
    const Eigen::MatrixXd A_true = Eigen::MatrixXd::Random(state_dim, state_dim);
    const Eigen::MatrixXd B_true = Eigen::MatrixXd::Random(state_dim, control_dim);
    auto dyn = [&A_true, &B_true](const Eigen::VectorXd &x, const Eigen::VectorXd& u) -> Eigen::VectorXd
    {
        const int state_dim = x.size();
        Eigen::VectorXd x_next = A_true * x.array().cos().matrix() + B_true * u.array().sin().matrix();
        IS_EQUAL(x_next.size(), state_dim);
        return x_next;
    };
    const Eigen::VectorXd x_star = Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd u_star = Eigen::VectorXd::Random(control_dim);
    const ilqr::Dynamics dynamics = ilqr::linearize_dynamics(dyn, x_star, u_star);

    // Next linearization point under true dynamics.
    const Eigen::VectorXd xt1_star = dyn(x_star, u_star);

    Eigen::MatrixXd rep_x = (x_star.replicate(1, state_dim)).transpose();
    Eigen::MatrixXd rep_u = (u_star.replicate(1, state_dim)).transpose();
    Eigen::MatrixXd J_x = -1.*(A_true.array()*rep_x.array().sin());
    Eigen::MatrixXd J_u = B_true.array()*rep_u.array().cos();
    IS_TRUE(math::is_equal(dynamics.A, J_x, 1e-3)); // high tolerance since finite differencing is not that great.
    IS_TRUE(math::is_equal(dynamics.B, J_u, 1e-3));

    const Eigen::VectorXd xt_test = x_star + 0.01*Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd ut_test = u_star + 0.01*Eigen::VectorXd::Random(control_dim);

    Eigen::VectorXd xt_diff = xt_test- x_star;
    const Eigen::VectorXd ut_diff = ut_test - u_star;

    // Use the linearization to predict the next x_{t+1}.
    const Eigen::VectorXd xt1_diff = dynamics.A*xt_diff + dynamics.B*ut_diff;
    IS_LESS_EQUAL(((xt1_diff + xt1_star) - dyn(xt_test, ut_test)).norm(), 1e-3);
}

void test_linear_dynamics_linearization(const int state_dim, const int control_dim)
{
    IS_GREATER(state_dim, 0);
    IS_GREATER(control_dim, 0);

    std::srand(2);

    // Test with linear dynamics first. The taylor expansion should produce
    // nearly exactly the same result.
    const Eigen::MatrixXd A_true = Eigen::MatrixXd::Random(state_dim, state_dim);
    const Eigen::MatrixXd B_true = Eigen::MatrixXd::Random(state_dim, control_dim);
    auto linear_dyn = [&A_true, &B_true](const Eigen::VectorXd &x, const Eigen::VectorXd& u) -> Eigen::VectorXd
    {
        const int state_dim = x.size();
        Eigen::VectorXd x_next = A_true*x + B_true*u;
        IS_EQUAL(x_next.size(), state_dim);
        return x_next;
    };

    const Eigen::VectorXd x_star = Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd u_star = Eigen::VectorXd::Random(control_dim);
    const ilqr::Dynamics dynamics = ilqr::linearize_dynamics(linear_dyn, x_star, u_star);

    IS_TRUE(math::is_equal(dynamics.A, A_true, 1e-10));
    IS_TRUE(math::is_equal(dynamics.B, B_true, 1e-10));

    // Next linearization point under true dynamics.
    const Eigen::VectorXd xt1_star = linear_dyn(x_star, u_star);

    const Eigen::VectorXd xt_test= Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd ut_test = Eigen::VectorXd::Random(control_dim);

    Eigen::VectorXd xt_diff = xt_test- x_star;
    const Eigen::VectorXd ut_diff = ut_test - u_star;

    // Use the linearization to predict the next x_{t+1}.
    const Eigen::VectorXd xt1_diff = dynamics.A*xt_diff + dynamics.B*ut_diff;

    IS_TRUE(math::is_equal(xt1_diff + xt1_star, linear_dyn(xt_test, ut_test), 1e-7));
}

void test_quadratic_cost_taylor_expansion(const int state_dim, const int control_dim)
{
    IS_GREATER(state_dim, 0);
    IS_GREATER(control_dim, 0);

    std::srand(3);

    // Test with linear dynamics first. The taylor expansion should produce
    // nearly exactly the same result.
    Eigen::MatrixXd Q_true = Eigen::MatrixXd::Random(state_dim, state_dim);
    Eigen::MatrixXd R_true = Eigen::MatrixXd::Random(control_dim, control_dim);
    Q_true = math::project_to_psd((Q_true + Q_true.transpose()), 1e-5);
    R_true = math::project_to_psd((R_true + R_true.transpose()), 1e-5);
    auto cost_func = [&Q_true, &R_true](const Eigen::VectorXd &x, const Eigen::VectorXd& u) -> double
    {
        Eigen::VectorXd cost = 0.5*(x.transpose()*Q_true*x + u.transpose()*R_true*u);
        IS_EQUAL(cost.size(), 1);
        return cost(0);
    };
    // Compute the Taylor expansion at a nominal point.
    const Eigen::VectorXd x_star = Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd u_star = Eigen::VectorXd::Random(control_dim);
    const ilqr::Cost cost = ilqr::quadraticize_cost(cost_func, x_star, u_star);

    IS_TRUE(math::is_equal(cost.Q, Q_true, 1e-4));
    IS_TRUE(math::is_equal(cost.R, R_true, 1e-4));
    IS_TRUE(math::is_equal(cost.P, Eigen::MatrixXd::Zero(state_dim, control_dim), 1e-4));
    IS_TRUE(math::is_equal(cost.g_x,  Q_true*x_star, 1e-10));
    IS_TRUE(math::is_equal(cost.g_u,  R_true*u_star, 1e-3)); // high tolerance since numerical error from finite differencing.

    // Next linearization point under true dynamics.
    const Eigen::VectorXd xt_test= Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd ut_test = Eigen::VectorXd::Random(control_dim);

    Eigen::VectorXd xt_diff = Eigen::VectorXd::Ones(state_dim);
    xt_diff = xt_test- x_star;
    const Eigen::VectorXd ut_diff = ut_test - u_star;
    Eigen::VectorXd ct_pred = 0.5*(xt_diff.transpose() * cost.Q * xt_diff)
            + (xt_diff.transpose()*cost.P*ut_diff)
            + 0.5*(ut_diff.transpose() * cost.R * ut_diff)
            + (cost.g_u.transpose() * ut_diff)
            + (cost.g_x.transpose() * xt_diff);
    ct_pred.array() += cost.c;
    IS_EQUAL(ct_pred.size(), 1);
    IS_ALMOST_EQUAL(ct_pred(0,0), cost_func(xt_test, ut_test), 1e-5);
}

void test_exp_cost_taylor_expansion(const int state_dim, const int control_dim)
{
    IS_GREATER(state_dim, 0);
    IS_GREATER(control_dim, 0);

    std::srand(3);

    // Test with linear dynamics first. The taylor expansion should produce
    // nearly exactly the same result.
    Eigen::MatrixXd wx = Eigen::VectorXd::Random(state_dim);
    Eigen::MatrixXd wu = Eigen::VectorXd::Random(control_dim);
    auto cost_func = [&wx, &wu](const Eigen::VectorXd &x, const Eigen::VectorXd& u) -> double
    {
        const double norm_sq = x.squaredNorm();
        const Eigen::VectorXd term = wx.transpose() * x + wu.transpose() * u * 0.5*norm_sq;
        IS_EQUAL(term.size(), 1);
        const double cost = std::exp(-1.*term(0));
        return cost;
    };

    // Compute the Taylor expansion at a nominal point.
    const Eigen::VectorXd x_star = Eigen::VectorXd::Random(state_dim);
    const Eigen::VectorXd u_star = Eigen::VectorXd::Random(control_dim);
    const ilqr::Cost cost = ilqr::quadraticize_cost(cost_func, x_star, u_star);

    double ct_xu = cost_func(x_star, u_star);
    const Eigen::VectorXd g_x_true = ct_xu * (-1.*wx - (wu.transpose()*u_star)[0]*x_star);
    const Eigen::VectorXd g_u_true = ct_xu * (-0.5*wu*x_star.squaredNorm());
    const Eigen::MatrixXd Hxx_true = ct_xu * -1.0*(wu.transpose()*u_star)[0]*Eigen::MatrixXd::Identity(state_dim, state_dim).array()
        + (1.0/ct_xu) * (g_x_true * g_x_true.transpose()).array();
    const Eigen::MatrixXd Huu_true =  1.0/ct_xu * (g_u_true * g_u_true.transpose()).array();
    const Eigen::MatrixXd Hux_true = ct_xu * (-1.0*wu*x_star.transpose()).array() + (1.0/ct_xu) * (g_u_true * g_x_true.transpose()).array();

    // Since we may need to project to PSD cone, construct and project it.
    Eigen::MatrixXd Q_true = math::project_to_psd(Hxx_true, 1e-11);
    math::check_psd(Q_true, 1e-12);

    Eigen::MatrixXd R_true = math::project_to_psd(Huu_true, 1e-8);
    math::check_psd(R_true, 1e-9);

    IS_TRUE(math::is_equal(cost.Q, Q_true, 1e-2));
    IS_TRUE(math::is_equal(cost.R, R_true, 1e-2));
    IS_TRUE(math::is_equal(cost.P, Hux_true.transpose(), 1e-2));
    IS_TRUE(math::is_equal(cost.g_u,  g_u_true, 1e-2)); // high tolerance since numerical error from finite differencing.
    IS_TRUE(math::is_equal(cost.g_x,  g_x_true, 1e-2)); // high tolerance since numerical error from finite differencing.

    constexpr double deviation = 1e-2;
    const Eigen::VectorXd xt_test = x_star.array() + deviation;
    const Eigen::VectorXd ut_test = u_star.array() + deviation;

    Eigen::VectorXd xt_diff = xt_test- x_star;
    const Eigen::VectorXd ut_diff = ut_test - u_star;
    Eigen::VectorXd ct_pred = 0.5*(xt_diff.transpose() * cost.Q * xt_diff)
            + (xt_diff.transpose()*cost.P*ut_diff)
            + 0.5*(ut_diff.transpose() * cost.R * ut_diff)
            + (cost.g_u.transpose() * ut_diff)
            + (cost.g_x.transpose() * xt_diff);
    ct_pred.array() += cost.c;
    IS_EQUAL(ct_pred.size(), 1);
    IS_ALMOST_EQUAL(ct_pred(0,0), cost_func(xt_test, ut_test), 5e-2);
}

int main()
{
    // Test linear dynamics linearization.
    test_linear_dynamics_linearization(12, 12);
    test_linear_dynamics_linearization(12, 5);
    test_linear_dynamics_linearization(5, 12);
    test_linear_dynamics_linearization(1, 5);
    test_linear_dynamics_linearization(5, 1);
    test_linear_dynamics_linearization(1, 1);

    // Test nonlinear (cos & sin) dynamics linearization.
    test_nonlinear_dynamics_linearization(12, 12);
    test_nonlinear_dynamics_linearization(12, 5);
    test_nonlinear_dynamics_linearization(5, 12);
    test_nonlinear_dynamics_linearization(5, 3);
    test_nonlinear_dynamics_linearization(1, 1);
    test_nonlinear_dynamics_linearization(3, 3);

    // Test quadratic cost function 2nd order taylor expansions.
    test_quadratic_cost_taylor_expansion(12, 12);
    test_quadratic_cost_taylor_expansion(12, 5);
    test_quadratic_cost_taylor_expansion(5, 12);
    test_quadratic_cost_taylor_expansion(1, 5);
    test_quadratic_cost_taylor_expansion(5, 1);
    test_quadratic_cost_taylor_expansion(1, 1);

    // Test exponential cost function 2nd order taylor expansions.
    // Larger dimensions 3 makes the tolerances higher in the test.
    test_exp_cost_taylor_expansion(5,5);
    test_exp_cost_taylor_expansion(3,5);
    test_exp_cost_taylor_expansion(5,3);
    test_exp_cost_taylor_expansion(3,1);
    test_exp_cost_taylor_expansion(1,3);
    test_exp_cost_taylor_expansion(1,1);

    return 0;
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/minus.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::minus(hana::int_c<3>, hana::int_c<5>) == hana::int_c<-2>);
    static_assert(hana::minus(1, 2) == -1, "");
}


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <orient/axis.hpp>

#include <Eigen/Dense>

namespace orient::detail {

template<Axis axis>
Eigen::Matrix3d make_generator()
{
  Eigen::Matrix3d M = Eigen::Matrix3d::Zero();
  if constexpr(axis == Axis::x){
    M(1,2) = -1;
    M(2,1) = 1;
  }
  else if constexpr(axis == Axis::y){
    M(0,2) = 1;
    M(2,0) = -1;
  }
  else {
    M(0,1) = -1;
    M(1,0) = 1;
  }
  return M;
}

template<Axis axis>
static const auto generator = make_generator<axis>();

}


#include "discrete.hpp"

#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>

using namespace MultidimensionalArray;
using namespace ProbabilityDistributions;

TEST(DiscreteTest, Likelihood) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Discrete<5, double> dist;
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  double likelihood1 = dist.log_likelihood(samples);

  dist.MLE(samples);

  double likelihood2 = dist.log_likelihood(samples);

  EXPECT_GE(likelihood2, likelihood1);

  auto p = dist.get_p();
  double sum = 0;
  for (size_t i = 0; i < p.size(); i++) {
    EXPECT_LE(0, p[i]);
    sum += p[i];
  }
  EXPECT_DOUBLE_EQ(1, sum);
}

TEST(DiscreteTest, MLE) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Discrete<5, double> dist;
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  dist.MLE(samples);

  std::vector<double> p = dist.get_p();
  double eps = 1e-2;
  double ll = dist.log_likelihood(samples);

  for (int i = 0; i < 5; i++) {
    p[i] += eps;
    dist.set_p(p);
    EXPECT_GE(ll, dist.log_likelihood(samples));
    p[i] -= 2* eps;
    dist.set_p(p);
    EXPECT_GE(ll, dist.log_likelihood(samples));
    p[i] += eps;
  }
}

TEST(DiscreteTest, Samples) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  {
    Discrete<2, double> dist;
    Array<double> samples;
    dist.sample(samples, n_samples, rng);
    size_t count_0 = 0, count_1 = 0;
    for (size_t i = 0; i < n_samples; i++) {
      if (samples(i,0) == 1)
        count_0++;
      if (samples(i,1) == 1)
        count_1++;
    }
    EXPECT_EQ(n_samples, count_0 + count_1);
    EXPECT_LT(0, count_0);
    EXPECT_LT(0, count_1);
  }
  {
    Discrete<2, double> dist({0,1});
    Array<double> samples;
    dist.sample(samples, n_samples, rng);
    size_t count_0 = 0, count_1 = 0;
    for (size_t i = 0; i < n_samples; i++) {
      if (samples(i,0) == 1)
        count_0++;
      if (samples(i,1) == 1)
        count_1++;
    }
    EXPECT_EQ(0, count_0);
    EXPECT_EQ(n_samples, count_1);
  }
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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 <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

// This example shows how to start with a connected planar graph
// and add edges to make the graph maximal planar (triangulated.)
// Any maximal planar simple graph on n vertices has 3n - 6 edges and
// 2n - 4 faces, a consequence of Euler's formula.

using namespace boost;

// This visitor is passed to planar_face_traversal to count the
// number of faces.
struct face_counter : public planar_face_traversal_visitor
{
  face_counter() : count(0) {}
  void begin_face() { ++count; }
  int count;
};

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

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  // Create the graph - a straight line
  graph g(10);
  add_edge(0,1,g);
  add_edge(1,2,g);
  add_edge(2,3,g);
  add_edge(3,4,g);
  add_edge(4,5,g);
  add_edge(5,6,g);
  add_edge(6,7,g);
  add_edge(7,8,g);
  add_edge(8,9,g);

  std::cout << "Since the input graph is planar with " << num_vertices(g)
            << " vertices," << std::endl
            << "The output graph should be planar with "
            << 3*num_vertices(g) - 6 << " edges and "
            << 2*num_vertices(g) - 4 << " faces." << std::endl;

  //Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity; compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   )
      )
    std::cout << "Input graph is planar" << std::endl;
  else
    std::cout << "Input graph is not planar" << std::endl;

  make_biconnected_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity again; compute the planar embedding as a side-effect
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   )
      )
    std::cout << "After calling make_biconnected, the graph is still planar"
              << std::endl;
  else
    std::cout << "After calling make_biconnected, the graph is not planar"
              << std::endl;

  make_maximal_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  // Test for planarity one final time; compute the planar embedding as a
  // side-effect
  std::cout << "After calling make_maximal_planar, the final graph ";
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   )
      )
    std::cout << "is planar." << std::endl;
  else
    std::cout << "is not planar." << std::endl;

  std::cout << "The final graph has " << num_edges(g)
            << " edges." << std::endl;

  face_counter count_visitor;
  planar_face_traversal(g, &embedding[0], count_visitor);
  std::cout << "The final graph has " << count_visitor.count << " faces."
            << std::endl;

  return 0;
}


/// @file  sphere.hpp
/// @brief Declarations for methods to find sphere intersections.
///
/// The intersection of d spheres in R^d is either zero, one, or two points.
/// More than d spheres can only intersect in zero or one points.
/// The methods here provide alternative ways to compute the intersection.
///
/// sphere_intersection_gauss() is the fastest but least reliable. It requires
/// exactly d spheres, expressed as centerpoints in A and radii in r, and will
/// find an exact solution only when A is invertible. In particular, none of the
/// centerpoints can be at the origin, as this would cause a zero column in A.
///
/// sphere_intersection_gauss() and sphere_intersection_orth() are both O(d^3),
/// but sphere_intersection_orth() takes roughly twice as long. Its advantage is
/// that sphere_intersection_orth() can handle non-invertible matrices A.
///
/// The final method, sphere_intersection_opt(), can also find a point close to
/// all the spheres in the case that they do not intersect.
/// It first runs sphere_intersection_orth(), and if the spheres intersect it
/// will return that solution. If they do not intersect, it will run a
/// Newton-Rhapson method to optimize a least-squares objective to choose the
/// unique point "closest" to all the spheres, in the sense that it minimizes
/// the sum of the squared Euclidean distances to the sphere boundaries.
/// The optimization method generally converges in just a few iterations, so
/// in practice sphere_intersection_opt() is generally not much slower than
/// sphere_intersection_orth().
///
/// Ref: I. D. Coope, “Reliable computation of the points of intersection of $n$
///   spheres in $R^n$,” ANZIAM Journal, vol. 42, no. 0, pp. 461–477, Dec. 2000.

#pragma once
#ifndef OGT_EMBED_SPHERE_HPP
#define OGT_EMBED_SPHERE_HPP

#include <ogt/config.hpp>
#include <vector>
#include <Eigen/Dense>

namespace OGT_NAMESPACE {
namespace embed {

/// Find the two points where n spheres intersect in R^n by Gaussian
/// elimination.
/// center is nxn; its columns are the locations of the sphere centers.
/// radius is nx1; d_i is the radius of the i'th sphere in center.
/// 2*eps is the smallest permissible distance between intersection points to
/// consider them distinct.
/// If only one intersection point exists, the second vector will have size 0.
/// If neither intersection point exists, both vectors will have size 0.
///
/// Throws std::invalid_argument if the matrix dimensions are incorrect.
///
/// @see sphere_intersection_orth(), sphere_intersection_opt()
std::vector<Eigen::VectorXd> sphere_intersection_gauss(
	const Eigen::MatrixXd &center, const Eigen::VectorXd& radius, double eps);

/// Find the two points where n spheres intersect in R^n by an orthogonal
/// transformation. This method is more robust than sphere_intersection_gauss,
/// but takes up to twice as long.
///
/// Throws std::invalid_argument if the matrix dimensions are incorrect.
///
/// @see sphere_intersection_gauss(), sphere_intersection_opt()
std::vector<Eigen::VectorXd> sphere_intersection_orth(
	const Eigen::MatrixXd &center, const Eigen::VectorXd& radius, double eps);

/// Find points close to n or more spheres in n dimensions. If the spheres
/// intersect, the interesection points are returned. Otherwise, an optimization
/// based approach is taken to find a point minimizing the distance to each
/// sphere.
///
/// Throws std::invalid_argument if the matrix dimensions are incorrect.
/// Throws ogt::EmbedErr if the embedding fails.
///
/// @see sphere_intersection_gauss(), sphere_intersection_orth()
std::vector<Eigen::VectorXd> sphere_intersection_opt(
	const Eigen::MatrixXd &center, const Eigen::VectorXd& radius, double eps,
	double min_delta, size_t max_iter, bool verbose);

/// Find a point within a certain margin of n or more spheres in n dimensions.
/// If the spheres intersect, an interesection point is returned.
/// Otherwise, a point is found to minimize the distance to each spherical
/// shell, defined as points with distances radius +/- margin of the center.
///
/// Throws std::invalid_argument if the matrix dimensions are incorrect.
/// Throws ogt::EmbedErr if the embedding fails.
///
/// @see sphere_intersection_gauss(), sphere_intersection_orth()
std::vector<Eigen::VectorXd> sphere_intersection_margin(
	const Eigen::MatrixXd &center, const Eigen::VectorXd& radius,
	const Eigen::VectorXd& margin, double eps,
	double min_delta, size_t max_iter, bool verbose);

} // end namespace embed
} // end namespace OGT_NAMESPACE
#endif /* OGT_EMBED_SPHERE_HPP */


#ifndef SignalProcessing_hpp
#define SignalProcessing_hpp

#include <QAlgorithm.hpp>
#include <TSpectrum.h>
#include <TMath.h>
#include <alglib/fasttransforms.h>
#include <armadillo>

namespace UMF {
	class ReduceChannels : public QAlgorithm {

		Q_OBJECT

	public:
		enum channels{
			interleaved, 	// C1,C2,C3,C1,C2,C3,...
			separated		// C1,...,C1,C2,...,C2,C3,...,C3
		};
		Q_ENUM(channels)

		enum operation{
			average
		};
		Q_ENUM(operation)

		QA_INPUT(QVector<double>, Signal)
		QA_PARAMETER(int, ChannelsArrangement, interleaved)
		QA_PARAMETER(int, Operation, average)
		QA_PARAMETER(int, NumberChannels, 2)
		QA_OUTPUT(QVector<double>, Signal)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(ReduceChannels)

	public:
		void run();
	};

	class Windowing : public QAlgorithm {

		Q_OBJECT

	public:
		enum function{
			hann
		};
		Q_ENUM(function)

		QA_INPUT(QVector<double>, Signal)
		QA_PARAMETER(int, Type, hann)
		QA_PARAMETER(int, Length, 5)
		QA_OUTPUT(QVector<double>, Signal)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(Windowing)

	private:
		QVector<double> window;

	public:
		void run();

		void init();
	};

	class ArrayPad : public QAlgorithm {

		Q_OBJECT

	public:
		enum border_type {
			constant,		// iiiiii|abcdefgh|iiiiiii with some specified i
			replicate,		// aaaaaa|abcdefgh|hhhhhhh
			reflect,		// fedcba|abcdefgh|hgfedcb
			wrap,			// cdefgh|abcdefgh|abcdefg
			reflect_101		// gfedcb|abcdefgh|gfedcba
		};
		Q_ENUM(border_type)

		/** Radius of the gaussian filter. */
		QA_PARAMETER(int, Radius, 5)
		/** Out of border interpolation mode. */
		QA_PARAMETER(int, BorderType, constant)
		/** Vector to be filtered. */
		QA_INPUT(QVector<double>, Signal)
		/** Filtered vector. */
		QA_OUTPUT(QVector<double>, Signal)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(ArrayPad)

	public:
		void run();

		void init();

		/** Compute interpolation coordinates.
		 The function computes and returns the coordinate of a donor pixel corresponding
		 to the specified extrapolated pixel when using the specified extrapolation border mode.

		 If the border type is constant, then -1 is returned.

		 There is no check whether the final result is out of bounds: it may happen
		 that a pos too much out of bounds leads to negative returning value.
		 */
		int borderInterpolate(const int& pos,
							  const int& len,
							  const border_type& bd);
	};

	class GaussianFilter : public QAlgorithm {

		Q_OBJECT

		/** Radius of the gaussian filter. */
		QA_PARAMETER(int, Radius, 5)
		/** Out of border interpolation mode. */
		QA_PARAMETER(int, BorderType, ArrayPad::constant)
		/** Vector to be filtered. */
		QA_INPUT(QVector<double>, Signal)
		/** Filtered vector. */
		QA_OUTPUT(QVector<double>, Signal)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(GaussianFilter)

	public:
		void init();

		void run();

	private:
		arma::vec kernel;
	};

	class SpectrumMagnitude : public QAlgorithm {

		Q_OBJECT

		QA_INPUT(QVector<double>, Signal)
		QA_OUTPUT(QVector<double>, Signal)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(SpectrumMagnitude)

	public:
		void run();
	};

	class SpectrumRemoveBackground : public QAlgorithm {

		Q_OBJECT

	public:
		enum filterOrder {
			kBackOrder2 =0,
			kBackOrder4 =1,
			kBackOrder6 =2,
			kBackOrder8 =3
		};
		enum windowDirection {
			kBackIncreasingWindow =0,
			kBackDecreasingWindow =1
		};
		enum smoothingWindow {
			kBackSmoothing3 =3,
			kBackSmoothing5 =5,
			kBackSmoothing7 =7,
			kBackSmoothing9 =9,
			kBackSmoothing11 =11,
			kBackSmoothing13 =13,
			kBackSmoothing15 =15
		};
		Q_ENUM(filterOrder)
		Q_ENUM(windowDirection)
		Q_ENUM(smoothingWindow)

		QA_INPUT(QVector<double>, Signal)
		QA_PARAMETER(int, NumberIterations, 6)
		QA_PARAMETER(int, Direction, TSpectrum::kBackIncreasingWindow)
		QA_PARAMETER(int, FilterOrder, TSpectrum::kBackOrder2)
		QA_PARAMETER(bool, Smoothing, kFALSE)
		QA_PARAMETER(int, SmoothWindow, TSpectrum::kBackSmoothing3)
		QA_PARAMETER(bool, Compton, kFALSE)
		QA_OUTPUT(QVector<double>, Signal)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(SpectrumRemoveBackground)

	public:
		void run();

		void init();
	};
}

#endif /* SignalProcessing_hpp */


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_BARYCOORDS_HPP
#define MCL_BARYCOORDS_HPP 1

#include <Eigen/Core>

namespace mcl
{

// vertex-edge barycoords, allows negative barys
template <typename T, int DIM>
static inline Eigen::Matrix<T,2,1> point_edge_barys(
		const Eigen::Matrix<T,DIM,1> &p,
		const Eigen::Matrix<T,DIM,1> &p0,
		const Eigen::Matrix<T,DIM,1> &p1)
{
	Eigen::Matrix<T,DIM,1> v0 = p1 - p0;
	Eigen::Matrix<T,DIM,1> v2 = p - p0;
	T d00 = v0.dot(v0);
	if (d00 <= T(0))
	{
		return Eigen::Matrix<T,2,1>(-1,0);
	}
	T d20 = v2.dot(v0);
	T invDenom = 1.0 / d00;
	Eigen::Matrix<T,2,1> r;
	r[0] = (d00 - d20)*invDenom;
	r[1] = 1.0 - r[0];
	return r;
}

template <typename T, int DIM> // Compute barycentric coords for a point on a triangle
static inline Eigen::Matrix<T,3,1> point_triangle_barys(
	const Eigen::Matrix<T,DIM,1> &p,
	const Eigen::Matrix<T,DIM,1> &p0,
	const Eigen::Matrix<T,DIM,1> &p1,
	const Eigen::Matrix<T,DIM,1> &p2)
{
	Eigen::Matrix<T,DIM,1> v0 = p1 - p0, v1 = p2 - p0, v2 = p - p0;
	T d00 = v0.dot(v0);
	T d01 = v0.dot(v1);
	T d11 = v1.dot(v1);
	T d20 = v2.dot(v0);
	T d21 = v2.dot(v1);
	T invDenom = 1.0 / (d00 * d11 - d01 * d01);
	Eigen::Matrix<T,3,1> r;
	r[1] = (d11 * d20 - d01 * d21) * invDenom;
	r[2] = (d00 * d21 - d01 * d20) * invDenom;
	r[0] = 1.0 - r[1] - r[2];
	return r;
}

template <typename T> // Compute barycentric coords for a point in a tet (3D)
static inline Eigen::Matrix<T,4,1> point_tet_barys(
	const Eigen::Matrix<T,3,1> &p,
	const Eigen::Matrix<T,3,1> &a,
	const Eigen::Matrix<T,3,1> &b,
	const Eigen::Matrix<T,3,1> &c,
	const Eigen::Matrix<T,3,1> &d)
{
	auto scalar_triple_product = [](
		const Eigen::Matrix<T,3,1> &u,
		const Eigen::Matrix<T,3,1> &v,
		const Eigen::Matrix<T,3,1> &w ){
		return u.dot(v.cross(w));
	};
	Eigen::Matrix<T,3,1> vap = p - a;
	Eigen::Matrix<T,3,1> vbp = p - b;
	Eigen::Matrix<T,3,1> vab = b - a;
	Eigen::Matrix<T,3,1> vac = c - a;
	Eigen::Matrix<T,3,1> vad = d - a;
	Eigen::Matrix<T,3,1> vbc = c - b;
	Eigen::Matrix<T,3,1> vbd = d - b;
	T va6 = scalar_triple_product(vbp, vbd, vbc);
	T vb6 = scalar_triple_product(vap, vac, vad);
	T vc6 = scalar_triple_product(vap, vad, vab);
	T vd6 = scalar_triple_product(vap, vab, vac);
	T v6 = 1.0 / scalar_triple_product(vab, vac, vad);
	return Eigen::Matrix<T,4,1>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
}

} // end namespace mcl

#endif


#include <boost/math/distribution.hpp>

Real blackScholesMertonFwd(const Real& fwd, const Real& strike, const Volatility& vol,
						  const Rate& rf, const Rate& rf, const Time& tau, const Integer& phi)
{
	boost::math::normal_distribution<> d(0.0, 1.0);
	Real dp, dm, stdDev, res, domDf, forDf;

	forDf = std::exp(-rf*tau);
	domDf = std::exp(-rd*tau);
	stdDev = vol*std::sqrt(tau);

	dp = (std::log(fwd/strike) + 0.5 * stdDev * stdDev) / stdDev;
	dp = (std::log(fwd/strike) - 0.5 * stdDev * stdDev) / stdDev;

	res = phi*domDf*(fwd*cdf(d, phi*dp) - strike* cdf(d, phi*dm));

	return res;
}


#include <NTL/mat_poly_ZZ_p.h>

#include <NTL/new.h>

NTL_START_IMPL

static
void HessCharPoly(ZZ_pX& g, const ZZ_pX& a, const ZZ_pX& f)
{
   long n = deg(f);
   if (n <= 0 || deg(a) >= n)
      Error("HessCharPoly: bad args");

   mat_ZZ_p M;
   M.SetDims(n, n);

   long i, j;

   ZZ_pX t;
   t = a;

   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++)
         M[i][j] = coeff(t, j);

      if (i < n-1)
         MulByXMod(t, t, f);
   }

   CharPoly(g, M);
}

void CharPolyMod(ZZ_pX& g, const ZZ_pX& a, const ZZ_pX& ff)
{
   ZZ_pX f = ff;
   MakeMonic(f);
   long n = deg(f);

   if (n <= 0 || deg(a) >= n)
      Error("CharPoly: bad args");

   if (IsZero(a)) {
      clear(g);
      SetCoeff(g, n);
      return;
   }

   if (n > 25) {
      ZZ_pX h;
      MinPolyMod(h, a, f);
      if (deg(h) == n) {
         g = h;
         return;
      }
   }

   if (ZZ_p::modulus() < n+1) {
      HessCharPoly(g, a, f);
      return;
   }

   vec_ZZ_p u(INIT_SIZE, n+1), v(INIT_SIZE, n+1);

   ZZ_pX h, h1;
   negate(h, a);
   long i;

   for (i = 0; i <= n; i++) {
      u[i] = i;
      add(h1, h, u[i]);
      resultant(v[i], f, h1);
   }

   interpolate(g, u, v);
}

NTL_END_IMPL


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// This test is written by Jan Bos to test the convergence of complex linear systems

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
void fill(Matrix& m)
{
  double s=19, u=21, p=16, e=5, r=18, l =12;

  std::complex<double> delta(2,1);
  mtl::mat::inserter<Matrix> sm(m);
  // set diagonal
  sm(0,0) << s;
  sm(1,1) << u;
  sm(2,2) << p;
  sm(3,3) << e;
  sm(4,4) << r;
  // below diagonal
  sm(1,0) << l+delta;
  sm(4,0) << l;
  sm(2,1) << l;
  sm(4,1) << l+delta;
  // above diagonal
  sm(0,2) << u;
  sm(0,3) << u+delta;
  //sm(3,4) << u;
  sm(3,4) << u + delta;
}

int main()
{
  // For a more realistic example set size to 1000 or larger
  const int N = 5;

  typedef mtl::compressed2D<std::complex<double> > matrix_type;
  matrix_type                   A(N, N);

  itl::pc::identity<matrix_type>     P(A);
  mtl::dense_vector<std::complex<double> > b(N, std::complex<double>(1,1)), x(N);

  fill(A);
  x= 0;

  itl::cyclic_iteration<double> iter(b, N, 1.e-6, 0.0, 5);
  bicgstab(A, x, b, P, iter);

  return 0;
}


// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <fft/fft2.hpp>
#include <fft/fft2_r2c.hpp>
#include <fft/planner.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

using namespace std;

const char* fname = "test_rt_random.h5";

typedef RT<std::complex<double>, RidgeletFrame, FFTr2c<PlannerR2C> > RT_t;

typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

void dump_frc(const std::vector<rt_coeff_t>& f_rc, const RidgeletFrame& rt)
{
  const char* fname = "f_rc_random.h5";
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "Written f(lambda, t) to " << fname << "\n";
}

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

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y, nrep;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(6), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(6), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("nrep,n", po::value<unsigned int>(&nrep)->default_value(1), "timings/nrep")
      ("input,f", po::value<std::string>(), "input file")
      ("measure", "use FFTW_MEASURE");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n";

  RDTSCTimer timer;

  timer.start();
  RidgeletFrame frame(Jx, Jy, rho_x, rho_y);
  auto t_create_frame = timer.stop();
  cout << "TIMINGS::create_frame: " << t_create_frame / 1e9 << "\n";

  const unsigned int ncols = frame.Nx();  // #cols
  const unsigned int nrows = frame.Ny();  // #rows
  cout << setw(20) << "Ny"
       << ":" << nrows << "\n";
  cout << setw(20) << "Nx"
       << ":" << ncols << "\n";

  RT_t rt(frame);
  array_t F(nrows, ncols);
  array_t Fo(nrows, ncols);

  if (!vm.count("input")) {
    cout << "generating random input"
         << "\n";
    F.setRandom();
    F *= nrows * ncols;
  } else {
    std::string fname = vm["input"].as<std::string>();
    cout << "read input from file: " << fname << "\n";
    hid_t lfile = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
    eigen2hdf::load(lfile, "R", F);
    // matlab is column major ...
    F = F.transpose();
    Fo = F;
  }
  typename RT_t::fft_t fft;
  timer.start();
  if (vm.count("measure")) {
    cout << "using FFTW_MEASURE"
         << "\n";
    init_fftw(fft, FFTW_MEASURE, frame);
  } else {
    cout << "using FFTW_ESTIMATE"
         << "\n";
    init_fftw(fft, FFTW_ESTIMATE, frame);
  }

  timer.print(cout, timer.stop(), "FFTW plans");

  complex_array_t Fh(F.rows(), F.cols());
  fft.ft(Fh, F);
  std::vector<rt_coeff_t> rt_coeffs(frame.size());
  timer.start();
  for (unsigned int i = 0; i < nrep; ++i) {
    rt.rt(rt_coeffs, Fh);
  }
  auto time_rt = timer.stop() / nrep;
  timer.print(cout, time_rt, "rt.rt");
  dump_frc(rt_coeffs, frame);

  // -------------------- Inverse transform --------------------
  complex_array_t Fh2(nrows, ncols);
  timer.start();
  for (unsigned int i = 0; i < nrep; ++i) {
    rt.irt(Fh2, rt_coeffs);
  }
  auto time_irt = timer.stop() / nrep;
  timer.print(cout, time_irt, "rt.irt");
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  eigen2hdf::save(file, "Fh", Fh);
  eigen2hdf::save(file, "R", Fo);
  eigen2hdf::save(file, "Fh2", Fh2);
  H5Fclose(file);
  cout << "written results to " << fname << "\n";

  complex_array_t diff = (ftcut(Fh2, nrows / 2, ncols / 2) - ftcut(Fh, nrows / 2, ncols / 2));
  cout << "avg diff (ft): " << diff.abs().sum() / (nrows / 2 * ncols / 2) << "\n";

  array_t Fr(nrows / 2, ncols / 2);
  array_t Fr2(nrows / 2, ncols / 2);

  cout << "nrows/2 * ncols/2: " << (nrows / 2 * ncols / 2) << "\n";

  fft.ift(Fr, ftcut(Fh, nrows / 2, ncols / 2));
  fft.ift(Fr2, ftcut(Fh2, nrows / 2, ncols / 2));

  array_t diffr = Fr - Fr2;
  cout << "avg diff (real):" << diffr.abs().sum() / (nrows / 2 * ncols / 2) << "\n";

  return 0;
}


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * 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.
 *******************************************************************************/

#ifndef MathUtils_hpp
#define MathUtils_hpp

#include <cmath>
#include <boost/math/distributions/students_t.hpp>

class DirectionalStatistics{
    double mCircularMean;
    double mCircularVariance;

public:
    DirectionalStatistics(double theta, double v){
        this->mCircularMean = theta;
        this->mCircularVariance = v;
    }
    double circularMean() const{
        return mCircularMean;
    }
    double circularVariance() const{
        return mCircularVariance;
    }
};

class NormalParameter{
protected:
    double mMean;
    double mStdev;
public:
    NormalParameter() = default;
    NormalParameter(double mean, double stdev){
        this->mMean = mean;
        this->mStdev = stdev;
    }
    ~NormalParameter() = default;
    double mean() const{return mMean;}
    double stdev() const{return mStdev;}
};

using WrappedNormalParameter = NormalParameter;

class MathUtils{

public:
    static double probaNormal(double x, double mu, double sigma){
        return 1.0/(std::sqrt(2.0*M_PI)*sigma)*std::exp(-std::pow(x-mu, 2)/(2*sigma*sigma));
    }

    static double logProbaNormal(double x, double mu, double sigma){
        return -1.0/2.0*std::log(2*M_PI) -1.0/2.0*std::log(sigma*sigma)
        -std::pow(x-mu, 2)/(2.0*sigma*sigma);
    }

    static std::function<double(double,double,double)> logProbatDistFunc(double nu) {
        boost::math::students_t dist(nu);
        return [=] (double x, double mu, double sigma){
            double z = (x - mu) / sigma;
            return std::log(boost::math::pdf(dist, z));
        };
    }

    static double mahalanobisDistance(double x, double mu, double sigma){
        return std::pow(x-mu, 2)/(sigma*sigma);
    }

    static double quantileChiSquaredDistribution(int degreeOfFreedom, double cumulativeDensity);

    static double normalizeOrientaion(double orientation){
        double x = std::cos(orientation);
        double y = std::sin(orientation);
        return std::atan2(y,x);
    }

    static DirectionalStatistics computeDirectionalStatistics(std::vector<double> orientations);
    static WrappedNormalParameter computeWrappedNormalParameters(const std::vector<double>& orientations);
};
#endif /* MathUtils_hpp */


#include "fgpg/geometrics.h"

#include <iostream>
#include <Eigen/Dense>
#include <pcl/common/intersections.h>
#include <vector>

/**
 p0, u: line
 plane: plane
 @see http://geomalgorithms.com/a05-_intersect-1.html
*/
bool calcLinePlaneIntersection(
  const TrianglePlaneData& plane,
  const Eigen::Ref<const Eigen::Vector3d>&  p0,
  const Eigen::Ref<const Eigen::Vector3d>&  u, ///< norm of point
  Eigen::Ref<Eigen::Vector3d> p ///< result
  )
{
  auto n = plane.normal;
  if(n.dot(u) == 0.0)
  {
    return false;
  }
  Eigen::Vector3d w = p0 - plane.points[0];
  double s = -n.dot(w) / n.dot(u);

  // inverse direction
  if(s<0)
    return false;

  p = p0 + s*u;

  return true;
}

/**
 p0, u: line
 plane: plane
 @see http://geomalgorithms.com/a05-_intersect-1.html
*/
double calcLinePlaneDistance(
  const TrianglePlaneData& plane,
  const Eigen::Ref<const Eigen::Vector3d>&  p0,
  const Eigen::Ref<const Eigen::Vector3d>&  u ///< norm of point
  )
{
  auto n = plane.normal;
  if(n.dot(u) == 0.0)
  {
    return false;
  }
  Eigen::Vector3d w = p0 - plane.points[0];
  double s = -n.dot(w) / n.dot(u);
  return s;
}

/// @see: http://blackpawn.com/texts/pointinpoly/default.html
bool sameSide(const Eigen::Ref<const Eigen::Vector3d>&  p1,const Eigen::Ref<const Eigen::Vector3d>& p2, const Eigen::Ref<const Eigen::Vector3d>& a, const Eigen::Ref<const Eigen::Vector3d>& b)
{
  auto cp1 = (b-a).cross(p1-a);
  auto cp2 = (b-a).cross(p2-a);
  if (cp1.dot(cp2) >= 0.0) return true;

  return false;
}

bool pointInTriangle(const Eigen::Ref<const Eigen::Vector3d>& p, const TrianglePlaneData& plane)
{

  auto a = plane.points[0];
  auto b = plane.points[1];
  auto c = plane.points[2];

  if(abs(plane.normal.dot(p-a)) + abs(plane.normal.dot(p-b)) + abs(plane.normal.dot(p-c)) > 1e-6) // not on the plane
  {
    return false;
  }
  // Compute vectors
  auto v0 = c - a;
  auto v1 = b - a;
  auto v2 = p - a;

  // Compute dot products
  double dot00 = v0.dot(v0);
  double dot01 = v0.dot(v1);
  double dot02 = v0.dot(v2);
  double dot11 = v1.dot(v1);
  double dot12 = v1.dot(v2);

  // Compute barycentric coordinates
  double inv_denom = 1 / (dot00 * dot11 - dot01 * dot01);
  double u = (dot11 * dot02 - dot01 * dot12) * inv_denom;
  double v = (dot00 * dot12 - dot01 * dot02) * inv_denom;

  // Check if point is in triangle
  return (u >= 0) && (v >= 0) && (u + v < 1);
}

Eigen::Vector3d orthogonalVector3d(const Eigen::Ref<const Eigen::Vector3d>& n, const Eigen::Ref<const Eigen::Vector3d>& v0, double theta)
{
  Eigen::Vector3d v;
  v.setZero();

  v = Eigen::AngleAxisd(theta, n).matrix() * v0;
  return v;
}

Eigen::Vector3d getOrthogonalVector(const Eigen::Ref<const Eigen::Vector3d>& n)
{
  Eigen::Vector3d v;

  int max_index = 0;
  int new_index[3];
  double max = 0;
  for(int i=0; i<3; i++)
  {
    if(abs(n(i)) > max)
    {
      max = abs(n(i));
      max_index = i;
    }
  }

  int loc = 0;
  new_index[2] = max_index;
  for(int i=0; i<3; i++)
  {
    if(i == max_index) continue;

    new_index[loc] = i;
    loc++;
  }

  v(new_index[0]) = 1;
  v(new_index[1]) = 1;
  v(new_index[2]) = -(n(new_index[0])+n(new_index[1])) / n(new_index[2]);

  v = v.normalized();

  return v;
}


#define BOOST_TEST_MODULE Gpufit

#define PI 3.1415926535897f

#include "Gpufit/gpufit.h"

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

#include <array>

template<std::size_t SIZE>
void generate_gauss_2d_rotated(std::array< float, SIZE>& values)
{
    int const size_x = int(std::sqrt(SIZE));
    int const size_y = size_x;

    float const a = 10.f;
    float const x0 = (float(size_x) - 1.f) / 2.f;
    float const y0 = (float(size_y) - 1.f) / 2.f;
    float const sx = 0.4f;
    float const sy = 0.5f;
    float const b = 1.f;
    float const r = PI / 16.f;

    for (int point_index_y = 0; point_index_y < size_y; point_index_y++)
    {
        for (int point_index_x = 0; point_index_x < size_x; point_index_x++)
        {
            int const point_index = point_index_y * size_x + point_index_x;
            float const arga = ((point_index_x - x0) * cosf(r)) - ((point_index_y - y0) * sinf(r));
            float const argb = ((point_index_x - x0) * sinf(r)) + ((point_index_y - y0) * cosf(r));
            float const ex = exp((-0.5f) * (((arga / sx) * (arga / sx)) + ((argb / sy) * (argb / sy))));
            values[point_index] = a * ex + b;
        }
    }
}

BOOST_AUTO_TEST_CASE( Gauss_Fit_2D_Rotated )
{
    std::size_t const n_fits{ 1 } ;
    std::size_t const n_points{ 64 } ;
    std::array< float, n_points > data{};
    generate_gauss_2d_rotated(data);
    std::array< float, n_points > weights{};
    std::fill(weights.begin(), weights.end(), 1.f);
    std::array< float, 7 > initial_parameters{ { 8.f, 3.4f, 3.6f, 0.4f, 0.5f, 2.f, 0.f } };
    float tolerance{ 0.001f };
    int max_n_iterations{ 10 };
    std::array< int, 7 > parameters_to_fit{ { 1, 1, 1, 1, 1, 1, 1 } };
    std::array< float, 7 > output_parameters;
    int output_states;
    float output_chi_square;
    int output_n_iterations;

    int const status
            = gpufit
            (
                n_fits,
                n_points,
                data.data(),
                weights.data(),
                GAUSS_2D_ROTATED,
                initial_parameters.data(),
                tolerance,
                max_n_iterations,
                parameters_to_fit.data(),
                LSE,
                0,
                0,
                output_parameters.data(),
                &output_states,
                &output_chi_square,
                &output_n_iterations
            ) ;

    BOOST_CHECK( status == 0 ) ;
}


/*------------------------------------------------------------------------
    Universidade do Estado do Rio de Janeiro
    Autores: Anderson Zudio de Moraes
             Victor Cracel Messner
*   Teste de Primalidade AKS - NTL

*   O código apresentado faz parte do projeto de conclusão de curso
*   dos autores pelo título de Bacharel, apresentado ao
*   Intituto de Matemática e Estatística,
*   da Universidade do Estado do Rio de Janeiro.
*   Você pode modificar e distribuir este código livremente
*   através dos termos do GNU General Public como publicado
*   pelo Free Software Foundation através da versão 3 da licença,
*   ou qualquer versão subsequente por opção sua.

*   O objetivo deste código é fornecer uma implmementação
*   do AKS correta, rápida e que suporte entradas
*   maiores que a implementação simples. Este código
*   utiliza a biblioteca NTL, que pode ser vista
*   em < http://www.shoup.net/ntl/ >

-----------------------------------------------------------------------*/

#include <gmp.h>
#include <NTL/ZZ.h>
#include <NTL/RR.h>
#include <NTL/ZZ_pX.h>
/* -------------------------------------------
--Header Default do NTL inclui por default:
--<cstdlib>, <cmath> e <iostream>
------------------------------------------- */

inline void totiente_euler(long &resultado, long n);

/* Observe que a função abaixo recebe a entrada
*  em forma de STRING. Ela está esperando um string
*  que contenha somente um número inteiro natural,
*  válido para a execução do AKS. Não há validação
*  de entrada no código. */
bool aks_ntl(const std::string n){ //Evita o programdor a utilizar I/O do NTL
	//Etapa 1
    mpz_t mpz_n; // tipo utilizado pelo GMP
    mpz_init(mpz_n);
    mpz_set_str(mpz_n, n.c_str(), 10);
    //O NTL nao oferece uma função para
    //computar a etapa 1. O GMP oferece:
    if(mpz_perfect_power_p(mpz_n))
            return false;
	//Fim da Etapa 1

	//Etapas 2, 3 e 4
	NTL::ZZ ZZ_n;  // tipo utilizado pelo NTL, inteiro prec arbitrária
	ZZ_n = to_ZZ(NTL::conv<NTL::ZZ>(n.c_str())); //Não há validação de entrada
	bool teste;
    long k; //Overflow para n > 2^(32768*raiz(2))
	NTL::ZZ r(2), n_mod_r; //N muito grande pode causar overflow no r na etapa 5

	double logaritmo_n = log(ZZ_n)/log(2); //Precisa ser real, se nao for causa erros de calculo
    double logaritmo_n_2 = logaritmo_n*logaritmo_n;

    while(1){
        if(r == ZZ_n){  //Etapa 4
            return true;
        }
        if(IsOne(GCD(ZZ_n, r))){ //Verificando se n é relativamente primo com q
            teste = true;
            rem(n_mod_r, ZZ_n, r);
            for(k = 1; k <= logaritmo_n_2; ++k){
                if(IsOne(PowerMod(n_mod_r, k, r))){ // (n^k)%r==1 --> n%r passado para evitar BAD_ARGUMENTS
                    teste = false;                  // resolve um problema da biblioteca
                    break;
                }
            }
            if(teste) break; //Ordem menor que logaritmo_n_2
        }
        else //Etapa 3
            return false;
        ++r;
    }
    //Fim da Etapa 2, Etapa 3 e Etapa 4

	//Etapa 5
	//-- Cuidado: N muito grande pode causar overflow no grau do polinomio ZZ_pX --
	//-- Limitacao da biblioteca! Grau max (2^31-1) ... r vale no maximo log2(N)^5 ...
	//WARNING! Possivel Overflow para n > 2^(raiz[5](2^32 - 1)) (~23 dígitos)
    long phi; //Funcao totiente de Eulerĵ
    long r_long = NTL::conv<long>(r);
    totiente_euler(phi, r_long);
    long int constante = floor(sqrt(phi)*logaritmo_n); //piso de raiz(phi(n)) log2 (n)

    NTL::ZZ_p::init(ZZ_n);//Ativa a modularidade com n para o NTL
    NTL::ZZ_pX f(r_long, 1); f -= 1; //x^r - 1, polinomio para acelerar as contas
    const NTL::ZZ_pXModulus modulo(f);  //internas do NTL com pre-computacao
    NTL::ZZ_pX lado_direito(NTL::conv<long>(n_mod_r), 1); //p(x) = x^(n mod r)
    NTL::ZZ_pX lado_esquerdo(1, 1); //p(x) = x

    for(long a; a <= constante; ++a){
        SetCoeff(lado_esquerdo, 1); lado_esquerdo += a;//p(x) = x + a
        PowerMod(lado_esquerdo, lado_esquerdo, ZZ_n, modulo); //(lhs = (x + a)^n mod (x^r - 1, n)
        lado_esquerdo -= a;

        if(lado_esquerdo != lado_direito) return false;
        clear(lado_esquerdo);
    }
    return true;
	//Fim Etapa 5

}

//Esta função é exatamente a mesma utilizada
//no AKS simples, ela é bem rápida
inline void totiente_euler(long &resultado, long n){
    resultado = n;

    for (long i = 2; i*i<=n; ++i){
        if ((n % i) == 0){
            while ((n % i) == 0){
                n /= i;
            }
            resultado -= resultado / i;
        }
    }
    if (n > 1) {
        resultado -= resultado / n;
     }
}


//
//! Copyright © 2018
//! Brandon Kohn
//
//  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)
//
#pragma once

#include <geometrix/algebra/expression.hpp>
#include <geometrix/primitive/point_traits.hpp>
#include <boost/concept_check.hpp>

namespace geometrix {

    enum class point_circle_orientation
    {
        outside = -1
        cocircular = 0
        inside = 1
    };

    template <typename Point1, typename Point2, typename Point3, typename Point4, typename NumberComparisonPolicy>
    inline point_circle_orientation point_in_circumcircle(const Point1& a, const Point2& b, const Point3& c, const Point4& p, const NumberComparisonPolicy& cmp)
    {
		BOOST_CONCEPT_ASSERT( (PointConcept<Point1>) );
		BOOST_CONCEPT_ASSERT( (PointConcept<Point2>) );
		BOOST_CONCEPT_ASSERT( (PointConcept<Point3>) );
		BOOST_CONCEPT_ASSERT( (PointConcept<Point4>) );
		BOOST_CONCEPT_ASSERT( (NumberComparisonPolicyConcept<NumberComparisonPolicy>) );
        static_assert(dimension_of<Point1>::value == 2, "point_in_cicumcircle is 2D only.");
        static_assert(dimension_of<Point2>::value == 2, "point_in_cicumcircle is 2D only.");
        static_assert(dimension_of<Point3>::value == 2, "point_in_cicumcircle is 2D only.");
        static_assert(dimension_of<Point4>::value == 2, "point_in_cicumcircle is 2D only.");

        using length_t = typename arithmetic_type_of<Point1>::type;
        using area_t = decltype(std::declval<length_t>()*std::declval<length_t>);

        length_t apx = get<0>(a) - get<0>(p);
        length_t apy = get<1>(a) - get<1>(p);
        length_t bpx = get<0>(b) - get<0>(p);
        length_t bpy = get<1>(b) - get<1>(p);
        length_t cpx = get<0>(c) - get<0>(p);
        length_t cpy = get<1>(c) - get<1>(p);

        area_t abdet = adx * bdy - bdx * ady;
        area_t bcdet = bdx * cdy - cdx * bdy;
        area_t cadet = cdx * ady - adx * cdy;
        area_t alift = adx * adx + ady * ady;
        area_t blift = bdx * bdx + bdy * bdy;
        area_t clift = cdx * cdx + cdy * cdy;

        area_t r = alift * bcdet + blift * cadet + clift * abdet;
        if(cmp.greater_than(r, constants::zero<area_t>())
            return point_circle_orientation::inside;

        if(cmp.less_than(r, constants::zero<area_t>())
            return point_circle_orientation::outside;

        return point_circle_orientation::cocircular;
    }

}//! namespace geometrix;


// Boost.Geometry

// Copyright (c) 2017-2018, Oracle and/or its affiliates.

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

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DENSIFY_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DENSIFY_HPP

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/signed_size_type.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/strategies/densify.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace densify
{

/*!
\brief Densification of cartesian segment.
\ingroup strategies
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.densify.densify_4_with_strategy densify (with strategy)]
}
 */
template
<
    typename CalculationType = void
>
class cartesian
{
public:
    template <typename Point, typename AssignPolicy, typename T>
    static inline void apply(Point const& p0, Point const& p1, AssignPolicy & policy, T const& length_threshold)
    {
        typedef typename AssignPolicy::point_type out_point_t;
        typedef typename select_most_precise
            <
                typename coordinate_type<Point>::type,
                typename coordinate_type<out_point_t>::type,
                CalculationType
            >::type calc_t;

        typedef model::point<calc_t, geometry::dimension<Point>::value, cs::cartesian> calc_point_t;

        calc_point_t cp0, cp1;
        geometry::detail::conversion::convert_point_to_point(p0, cp0);
        geometry::detail::conversion::convert_point_to_point(p1, cp1);

        // dir01 = xy1 - xy0
        calc_point_t dir01 = cp1;
        geometry::subtract_point(dir01, cp0);
        calc_t const dot01 = geometry::dot_product(dir01, dir01);
        calc_t const len = math::sqrt(dot01);

        BOOST_GEOMETRY_ASSERT(length_threshold > T(0));

        signed_size_type n = signed_size_type(len / length_threshold);
        if (n <= 0)
        {
            return;
        }

        // NOTE: Normalization will not work for integral coordinates
        // normalize
        //geometry::divide_value(dir01, len);

        calc_t step = len / (n + 1);

        calc_t d = step;
        for (signed_size_type i = 0 ; i < n ; ++i, d += step)
        {
            // pd = xy0 + d * dir01
            calc_point_t pd = dir01;

            // without normalization
            geometry::multiply_value(pd, calc_t(i + 1));
            geometry::divide_value(pd, calc_t(n + 1));
            // with normalization
            //geometry::multiply_value(pd, d);

            geometry::add_point(pd, cp0);

            // NOTE: Only needed if types calc_point_t and out_point_t are different
            // otherwise pd could simply be passed into policy
            out_point_t p;
            assert_dimension_equal<calc_point_t, out_point_t>();
            geometry::detail::conversion::convert_point_to_point(pd, p);

            policy.apply(p);
        }
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<cartesian_tag>
{
    typedef strategy::densify::cartesian<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::densify

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DENSIFY_HPP


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

Eigen::MatrixXd StrassenMultiply(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) {
    if (A.rows() > 128) {
        return A * B;
    }

    unsigned int rows = A.rows() / 2;
    unsigned int cols = A.cols() / 2;

    Eigen::MatrixXd a = A.topLeftCorner(rows, cols);
    Eigen::MatrixXd b = A.topRightCorner(rows, cols);
    Eigen::MatrixXd c = A.bottomLeftCorner(rows, cols);
    Eigen::MatrixXd d = A.bottomRightCorner(rows, cols);
    Eigen::MatrixXd e = B.topLeftCorner(rows, cols);
    Eigen::MatrixXd f = B.topRightCorner(rows, cols);
    Eigen::MatrixXd g = B.bottomLeftCorner(rows, cols);
    Eigen::MatrixXd h = B.bottomRightCorner(rows, cols);

    Eigen::MatrixXd P1 = StrassenMultiply(a, f - h);
    Eigen::MatrixXd P2 = StrassenMultiply(a + b, h);
    Eigen::MatrixXd P3 = StrassenMultiply(c + d, e);
    Eigen::MatrixXd P4 = StrassenMultiply(d, g - e);
    Eigen::MatrixXd P5 = StrassenMultiply(a + d, e + h);
    Eigen::MatrixXd P6 = StrassenMultiply(b - d, g + h);
    Eigen::MatrixXd P7 = StrassenMultiply(a - c, e + f);

    Eigen::MatrixXd r = P5 + P4 - P2 + P6;
    Eigen::MatrixXd s = P1 + P2;
    Eigen::MatrixXd t = P3 + P4;
    Eigen::MatrixXd u = P1 + P5 - P3 - P7;

    Eigen::MatrixXd result = Eigen::MatrixXd::Zero(r.cols() + s.cols(), r.cols() + s.cols());
    result.topLeftCorner(rows, cols) = r;
    result.topRightCorner(rows, cols) = s;
    result.bottomLeftCorner(rows, cols) = t;
    result.bottomRightCorner(rows, cols) = h;

    return result;
}

int main(int argc, char const *argv[]) {
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(2, 2);
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(2, 2);

    Eigen::MatrixXd result = StrassenMultiply(A, B);
    std::cout << "Multiplication done !" << std::endl;

    return 0;
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

typedef boost::numeric::ublas::matrix<float> Matrix44;
typedef boost::numeric::ublas::identity_matrix<float> IdentityMtx44;

Matrix44 transposeMtx(const Matrix44 &matrix)
{
	Matrix44 matrix_ = IdentityMtx44(4);

	matrix_(0, 0) = matrix(0, 0);
	matrix_(0, 1) = matrix(1, 0);
	matrix_(0, 2) = matrix(2, 0);
	matrix_(0, 3) = matrix(3, 0);

	matrix_(1, 0) = matrix(0, 1);
	matrix_(1, 1) = matrix(1, 1);
	matrix_(1, 2) = matrix(2, 1);
	matrix_(1, 3) = matrix(3, 1);

	matrix_(2, 0) = matrix(0, 2);
	matrix_(2, 1) = matrix(1, 2);
	matrix_(2, 2) = matrix(2, 2);
	matrix_(2, 3) = matrix(3, 2);

	matrix_(3, 0) = matrix(0, 3);
	matrix_(3, 1) = matrix(1, 3);
	matrix_(3, 2) = matrix(2, 3);
	matrix_(3, 3) = matrix(3, 3);

	return matrix_;
}

int main()
{
	Matrix44 matrix(4, 4);

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			matrix(i, j) = 4 * i + j;
		}
	}

	std::cout << matrix << std::endl;

	matrix = transposeMtx(matrix);

	std::cout << matrix << std::endl;

	return 0;
}


/**
 * @file matode_main.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>
#include <iomanip>
#include <iostream>

#include "matode.h"

int main(int /*argc*/, char** /*argv*/) {
  /* SAM_LISTING_BEGIN_6 */
  double h = 0.01;  // stepsize
  Eigen::Vector3d norms;
  // Build M
  Eigen::Matrix3d M;
  M << 8, 1, 6, 3, 5, 7, 9, 9, 2;
  // Build A
  Eigen::Matrix3d A;
  A << 0, 1, 1, -1, 0, 1, -1, -1, 0;
  Eigen::MatrixXd I = Eigen::Matrix3d::Identity();

  auto QR = M.householderQr();

  Eigen::Matrix3d Q = QR.householderQ() * Eigen::Matrix3d::Identity();

  Eigen::Matrix3d explicit_euler, implicit_euler, implicit_midpoint;

  explicit_euler = Q;
  implicit_euler = Q;
  implicit_midpoint = Q;

	std::cout << (explicit_euler * explicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_euler * implicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_midpoint * implicit_midpoint.transpose() - I).norm() << std::endl;

	std::cout << 0 << "\t" <<(explicit_euler * explicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_euler * implicit_euler.transpose() - I).norm() << "\t"
	          <<  (implicit_midpoint * implicit_midpoint.transpose() - I).norm() << std::endl;

  for(int i = 0; i < 20; i++) {
	explicit_euler = MatODE::eeulstep(A, explicit_euler, h);
	implicit_euler = MatODE::ieulstep(A, implicit_euler, h);
	implicit_midpoint = MatODE::impstep(A, implicit_midpoint, h);

	std::cout << i + 1 << "\t" <<(explicit_euler * explicit_euler.transpose() - I).norm() << "\t"
				<<  (implicit_euler * implicit_euler.transpose() - I).norm() << "\t"
				<<  (implicit_midpoint * implicit_midpoint.transpose() - I).norm() << std::endl;

  }

  /* SAM_LISTING_END_6 */
  return 0;
}


//    boost asinh.hpp header file

//  (C) Copyright Eric Ford 2001 & Hubert Holin.
//  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 for updates, documentation, and revision history.

#ifndef BOOST_ACOSH_HPP
#define BOOST_ACOSH_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <cmath>
#include <boost/config.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

// This is the inverse of the hyperbolic cosine function.

namespace boost
{
    namespace math
    {
       namespace detail
       {
#if defined(__GNUC__) && (__GNUC__ < 3)
        // gcc 2.x ignores function scope using declarations,
        // put them in the scope of the enclosing namespace instead:

        using    ::std::abs;
        using    ::std::sqrt;
        using    ::std::log;

        using    ::std::numeric_limits;
#endif

        template<typename T, typename Policy>
        inline T    acosh_imp(const T x, const Policy& pol)
        {
            using    ::std::abs;
            using    ::std::sqrt;
            using    ::std::log;

            T const    one = static_cast<T>(1);
            T const    two = static_cast<T>(2);

            static T const    taylor_2_bound = sqrt(tools::epsilon<T>());
            static T const    taylor_n_bound = sqrt(taylor_2_bound);
            static T const    upper_taylor_2_bound = one/taylor_2_bound;

            if(x < one)
            {
               return policies::raise_domain_error<T>(
                  "boost::math::acosh<%1%>(%1%)",
                  "acosh requires x >= 1, but got x = %1%.", x, pol);
            }
            else if    (x >= taylor_n_bound)
            {
                if    (x > upper_taylor_2_bound)
                {
                    // approximation by laurent series in 1/x at 0+ order from -1 to 0
                    return( log( x*two) );
                }
                else
                {
                    return( log( x + sqrt(x*x-one) ) );
                }
            }
            else
            {
                T    y = sqrt(x-one);

                // approximation by taylor series in y at 0 up to order 2
                T    result = y;

                if    (y >= taylor_2_bound)
                {
                    T    y3 = y*y*y;

                    // approximation by taylor series in y at 0 up to order 4
                    result -= y3/static_cast<T>(12);
                }

                return(sqrt(static_cast<T>(2))*result);
            }
        }
       }

        template<typename T, typename Policy>
        inline typename tools::promote_args<T>::type acosh(const T x, const Policy& pol)
        {
           typedef typename tools::promote_args<T>::type result_type;
           return detail::acosh_imp(
              static_cast<result_type>(x), pol);
        }
        template<typename T>
        inline typename tools::promote_args<T>::type acosh(const T x)
        {
           typedef typename tools::promote_args<T>::type result_type;
           return detail::acosh_imp(
              static_cast<result_type>(x), policies::policy<>());
        }

    }
}

#endif /* BOOST_ACOSH_HPP */


#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>
#include <functional>
#include <vector>
#include <cmath>
#include "Tools.hpp"
using namespace std;

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

This program is for the first case of 1-Dimensional drag. The Euler1D
class is generalized to keep track of position and velocity. It accepts
a lambda as a generic euler step function. The only variable it depends
on is dt, generally used to change the accuracy and computation time.

The main function creates a Euler1D object called point with an initial
velocity of v_i and and an initial position (x) of 0. The main loop
increments t by dt each iteration, saving the x, v, and t in vectors and
applying the euler step. The program then plots v vs t and x vs t in
files VelTime.png and PosTime.png respectively. Then, the program uses
a few functions from Tools.hpp to find the percent error of the terminal
velocity and the rising and falling times of the object.

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

const double v_i = 5;			// Initial velocity of the object
const double g = 9.8;			// Gravitational acceleration (negative)
const double b = 1;				// Generalized drag constant
const double dt = 0.001;//0.255;		// Time step
const double SIM_TIME = 2;		// Time to run the simulation (Should be large enough to let v = v_t)

function<double(double,double)> dragStep = [](double x, double v){ return -g-b*v*abs(v); };	// This is the lambda function used as the euler step. The type isn't auto because\
	// it's passed as an argument to the Euler1D::update fuction and the compiler has to know parameter type.

class Euler1D{						// Euler1D definition
public:
	Euler1D(double x_0, double v_0);// Constructor
	void update(function<double(double,double)>& f);// Euler step and updating x
	double getX() const;	// Accessor methods
	double getV() const;
private:
	double x;	// Instance variables
	double v;
};

Euler1D::Euler1D(double x_0, double v_0){	// Constructor just initializes position and velocity
	x = x_0;
	v = v_0;
}

void Euler1D::update(function<double(double,double)>& f){	// Reference to Euler step lambda as argument for update function
	double eulerStep = f(x,v);	// Calculate the step before updating anything
	x += v*dt;					// Update x
	v += eulerStep*dt;			// Update v
}

double Euler1D::getX() const { return x; }	// Accessor methods for x and v
double Euler1D::getV() const { return v; }

void plotStuff(vector<double> x, vector<double> v, vector<double> t){	// Plots the velocity v. time and position v. time graphs
	Gnuplot gp;	// Instance of gnuplot terminal stream

	gp << setprecision(3);	// Velocity v. time graph...
	gp << "set xrange [0:" << SIM_TIME << "]\n";
	gp << "set yrange [" << v[0] << ":" << getMinVal(v)*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"t (s)\"\n";
	gp << "set ylabel \"v (m/s)\"\n";
	gp << "set title \"Velocity vs. Time with Rayleigh's Drag Equation, b = " << b << "\"\n";
	gp << "set output \"VelTime.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(t,v));

	gp << "set xrange [0:" << SIM_TIME << "]\n";	// Position v. time graph...
	gp << "set yrange [0:" << getMaxVal(x)*1.1 << "]\n";
	gp << "set xlabel \"t (s)\"\n";
	gp << "set ylabel \"x (m)\"\n";
	gp << "set title \"Position vs. Time with Rayleigh's Drag Equation, b = " << b << "\"\n";
	gp << "set output \"PosTime.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(t,x));
}

int main(){					// Main program!
	Euler1D point(0,v_i);	// Create an Euler1D object, x=0 v=v_i
	vector<double> x_n;		// Vectors for x, v, and t
	vector <double> v_n;
	vector <double> t_n;

	for(double t = 0; t < SIM_TIME; t += dt){	// Main loop
		x_n.push_back(point.getX());	// Save point's x and v before modification
		v_n.push_back(point.getV());
		t_n.push_back(t);				// Save the value of t for plotting
		point.update(dragStep);			// Update the point using kinematic equations and Euler step
	}

	plotStuff(x_n, v_n, t_n);	// Make the plots!

	double diff = pErr(g,b,v_n.back());	// Calculate the percent error between accepted and estimated terminal velocities
	cout << "Difference between calculated and final value: " << diff*100 << "%\n";	// Display the percent error as a percentage

	double rising = sgnChangeTime(v_n, t_n);	// Find the time when velocity changes sign, marking the peak of the trajectory and rising time
	double hitground = sgnChangeTime(x_n, t_n);	// Find the time when position changes time, marking when the object returns to 0
	cout << "Rising time: " << rising << "\n";	// Output rising time
	cout << "Falling time: " << hitground-rising << "\n";	// Then falling time (they should be different if b!= 0)

	return 0;	// Everything went well, return 0!
}


#pragma once

#include <Eigen/Core>
#include <boost/static_assert.hpp>

namespace kt84 {

template <class _Func, int _DimIn, int _DimOut>
struct FiniteDifferentiator {
    typedef Eigen::Matrix<double, _DimIn , 1> Point;
    typedef Eigen::Matrix<double, _DimOut, _DimIn> Gradient;
    typedef Eigen::Matrix<double, _DimIn, _DimIn> Hessian;

    Gradient gradient_fd(const Point& point, double epsilon) const {
        Gradient result;
        const _Func& f = *reinterpret_cast<const _Func*>(this);
        for (int i = 0; i < _DimIn; ++i) {
            Point delta = Point::Unit(_DimIn, i) * epsilon;
            result.col(i) = (f(point + delta) - f(point - delta)) / (2 * epsilon);
        }
        return result;
    }
    Hessian hessian_fd(const Point& point, double epsilon) const {
        BOOST_STATIC_ASSERT(_DimOut == 1);
        Hessian result;
        for (int i = 0; i < _DimIn; ++i) {
            Point delta = Point::Unit(_DimIn, i) * epsilon;
            result.row(i) = (gradient_fd(point + delta, epsilon) - gradient_fd(point - delta, epsilon)) / (2 * epsilon);
        }
        return result;
    }
};

}


#pragma once

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <armadillo>

using namespace std;
using namespace arma;

// digamma function
double digamma(double x);
// multivariate digamma function
double digamma_mult(double x,uint32_t d);
// cateorical distribution (Multionomial for one word)
double Cat(uint32_t x, Row<double> pi);
// log cateorical distribution (Multionomial for one word)
double logCat(uint32_t x, Row<double> pi);
// evaluate beta distribution at x
double Beta(double x, double alpha, double beta);
// log beta function
double betaln(double alpha, double beta);
// evaluate log beta distribution at x
double logBeta(double x, double alpha, double beta);
double logBeta(const Row<double>& x, double alpha, double beta);
double logDir(const Row<double>& x, const Row<double>& alpha);

void betaMode(Col<double>& v, const Col<double>& alpha, const Col<double>& beta);
void betaMode(Row<double>& v, const Col<double>& alpha, const Col<double>& beta);
// stick breaking proportions;  truncated stickbreaking -> stick breaks will be dim longer than proportions v
void stickBreaking(Col<double>& prop, const Col<double>& v);
void stickBreaking(Row<double>& prop, const Row<double>& v);
uint32_t multinomialMode(const Row<double>& p);
void dirMode(Row<double>& mode, const Row<double>& alpha);
void dirMode(Col<double>& mode, const Col<double>& alpha);

template <class U>
Row<uint32_t> size(Mat<U> A)
{
  Row<uint32_t> s(2);
  s(0) = A.n_rows;
  s(1) = A.n_cols;
  return s;
};

template <class U>
Row<uint32_t> size(Col<U> A)
{
  Row<uint32_t> s(2);
  s(0) = A.n_rows;
  s(1) = 1;
  return s;
};

template <class U>
Row<uint32_t> size(Row<U> A)
{
  Row<uint32_t> s(2);
  s(0) = 1;
  s(1) = A.n_cols;
  return s;
};


#include "multibrot_opencl_calculator.h"

#include <boost/format.hpp>
#include <unordered_map>

namespace {
static const char* kMainProgram = R"(
/*
Requires a definition:
- REAL_T floating point type (acceptable types are float and, if device supports, half and double),
- RESULT_T - type of result pixel component (either uchar or ushort)
- RESULT_MAX - max value accepted by RESULT_T (either CHAR_MAX or USHRT_MAX)
- POWER_FUNC - name of a function that executes power of Z and adds C
  Function must take the following parameters:
  - Z = (zreal, zimg) - input and output number
  - zlen_sqr - square of absolute value of Z (square)
  - power
  - C = (real, img) - the second addend
  In general this function should do the following mathematical operation:
  (zreal, zimg) = (zreal, zimg) ^ power + (real, img)
  Different functions are used for optimisation purposes
*/

// Preprocessor magic based on https://stackoverflow.com/a/1489985
#define PASTER_2(x,y) x ## y
#define PASTER_3(x,y,z) x ## y ## z
#define EVALUATOR_2(x,y)  PASTER_2(x,y)
#define EVALUATOR_3(x,y,z)  PASTER_3(x,y,z)
#define CONVERT EVALUATOR_3(convert_, RESULT_T, _sat)
#define REAL_T4 EVALUATOR_2(REAL_T, 4)

// Universal complex number power function, supports all real powers but the slowest
void UniversalPowerOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    // Formula for z^p is a multi-value function, by we use a basic plane only
    REAL_T multiplier = powr( (REAL_T)(zlen_sqr), (REAL_T)(0.5*power) );
    // TODO danger zone here - at first iteration atan2 gets zero as both arguments
    // Fix UB somehow
    REAL_T phi = atan2( *zimg, *zreal );
    *zreal = multiplier * cos( power * phi ) + real;
    *zimg = multiplier * sin( power * phi ) + img;
}

// Should be used when power is 1
void Power1OfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    *zreal += real;
    *zimg += img;
}

// Should be used when power is 2
void SquareOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    REAL_T zreal_new = (*zreal) * (*zreal) - (*zimg) * (*zimg) + real;
    *zimg = 2 * (*zreal) * (*zimg) + img;
    *zreal = zreal_new;
}

// Should be used when power is 3
void CubeOfComplex(
    __private REAL_T* restrict zreal,
    __private REAL_T* restrict zimg,
    const REAL_T zlen_sqr,
    const REAL_T power,
    const REAL_T real,
    const REAL_T img
)
{
    REAL_T zreal_new = pown(*zreal, 3) - 3 * (*zreal) * (*zimg) * (*zimg) + real;
    *zimg = 3 * (*zreal) * (*zreal) * (*zimg) - pown(*zimg, 3) + img;
    *zreal = zreal_new;
}

/*
    Calculate number of iterations that point after which point
    escaped the set.
    Returns raw number of iterations.
*/
REAL_T CalcPointOnMultibrotSet( REAL_T real, REAL_T img, REAL_T power, ushort max_iter_number )
{
    REAL_T iter_number = 0;
    REAL_T zreal = 0;
    REAL_T zimg = 0;
    REAL_T zlen_sqr = 0;
    while ( zlen_sqr < 2*2 && iter_number < max_iter_number )
    {
        POWER_FUNC( &zreal, &zimg, zlen_sqr, power, real, img );

        zlen_sqr = zreal*zreal + zimg*zimg;
        iter_number += 1;
    }
    return iter_number;
}

#ifdef COLOR_ENABLED
/*
    Scales iteration number and picks a color.
    May be used only when RESULT_T is uchar4 or ushort4.
    Useful for building color images.
*/
RESULT_T ProcessIterationNumber( REAL_T iter_number, ushort max_iter_number )
{
    // Calculate a color in HSV
    // This algorithm is based on https://www.codingame.com/playgrounds/2358/how-to-plot-the-mandelbrot-set/adding-some-colors
    REAL_T value = iter_number < max_iter_number ? RESULT_MAX : 0;
    REAL_T hue = ( RESULT_MAX / max_iter_number ) * iter_number;
    // Saturation is always max.
    // Convert to RGB
    REAL_T hue_section = hue / ( RESULT_MAX / 6 ); // hue_section is in range [0; 6]
    uchar i1 = convert_uchar_sat_rtz( hue_section / 2 );
    uchar i2 = convert_uchar_sat_rtz( remainder( hue_section, 2 ) );
    uchar ic = ( i1 + i2 ) % 3;
    uchar ix = ( i1 - i2 + 1 ) % 3;

    REAL_T rgb[3] = { 0 };
    const REAL_T chroma = value; // * saturation
    REAL_T second_color = chroma * ( 1 - fabs( remainder( hue_section, 2 ) - 1 ) );
    rgb[ic] = chroma;
    rgb[ix] = second_color;

    REAL_T4 result = (REAL_T4)( vload3( 0, rgb ), RESULT_MAX );
    return CONVERT( result );
}
#else
/*
    Scales iteration number so it uses all available values from 0 to max supported by a given
    number type.
    May be used only when RESULT_T is uchar or ushort.
    Useful for building grayscale images.
*/
RESULT_T ProcessIterationNumber( REAL_T iter_number, ushort max_iter_number )
{
    iter_number = ( RESULT_MAX / max_iter_number ) * iter_number;
    iter_number = RESULT_MAX - iter_number;
    return CONVERT( iter_number );
}
#endif

/*
    Build an image of Mandelbrot or Multibrot set.

    (rmin, imin) - complex value that corresponds to the left top corner of the image,
    (rmax, imax) - right bottom corner.

    Output is a pointer to buffer that will contain pixel data. Data format is defined by RESULT_T.

    Must be called with two-dimensional work assignment, where dimensions are used as image
    size (first dimension is width, second dimension is height).
    Pixels are stored in row-major order.
*/
__kernel void MultibrotSetKernel(
    REAL_T rmin, REAL_T imin,
    REAL_T rmax, REAL_T imax,
    REAL_T power,
    ushort max_iter_number,
    __global RESULT_T* restrict output
)
{
    REAL_T rstep = ( rmax - rmin ) / get_global_size(0);
    REAL_T istep = ( imax - imin ) / get_global_size(1);

    size_t row_width = get_global_size(0);
    size_t result_index = get_global_id(1) * row_width + get_global_id(0);

    REAL_T real = rmin + get_global_id(0) * rstep;
    REAL_T img = imin + get_global_id(1) * istep;
    REAL_T multibrot_val = CalcPointOnMultibrotSet( real, img, power, max_iter_number );
    RESULT_T result = ProcessIterationNumber( multibrot_val, max_iter_number );
    output[result_index] = result;
}
)";

template <typename T>
struct TempValueConstants {
    static const char* opencl_type_name;
    static const char* required_extension;
};

const char* TempValueConstants<half_float::half>::opencl_type_name = "half";
const char* TempValueConstants<half_float::half>::required_extension = "cl_khr_fp16";

const char* TempValueConstants<float>::opencl_type_name = "float";
const char* TempValueConstants<float>::required_extension = "";

const char* TempValueConstants<double>::opencl_type_name = "double";
const char* TempValueConstants<double>::required_extension = "cl_khr_fp64";

template <typename P>
struct ResultTypeConstants {
    static const char* result_type_name;
    static const char* result_max_val_macro;
    static const int result_max_val;
    static const bool color_enabled;
};

// Grayscale 8 bit
const char* ResultTypeConstants<cl_uchar>::result_type_name = "uchar";
const char* ResultTypeConstants<cl_uchar>::result_max_val_macro = "UCHAR_MAX";
const int ResultTypeConstants<cl_uchar>::result_max_val = CL_UCHAR_MAX;
const bool ResultTypeConstants<cl_uchar>::color_enabled = false;

// Grayscale 16 bit
const char* ResultTypeConstants<cl_ushort>::result_type_name = "ushort";
const char* ResultTypeConstants<cl_ushort>::result_max_val_macro = "USHRT_MAX";
const int ResultTypeConstants<cl_ushort>::result_max_val = CL_USHRT_MAX;
const bool ResultTypeConstants<cl_ushort>::color_enabled = false;

// RGB 8 bit
const char* ResultTypeConstants<cl_uchar4>::result_type_name = "uchar4";
const char* ResultTypeConstants<cl_uchar4>::result_max_val_macro = "UCHAR_MAX";
const int ResultTypeConstants<cl_uchar4>::result_max_val = CL_UCHAR_MAX;
const bool ResultTypeConstants<cl_uchar4>::color_enabled = true;

// RGB 16 bit
const char* ResultTypeConstants<cl_ushort4>::result_type_name = "ushort4";
const char* ResultTypeConstants<cl_ushort4>::result_max_val_macro = "USHRT_MAX";
const int ResultTypeConstants<cl_ushort4>::result_max_val = CL_USHRT_MAX;
const bool ResultTypeConstants<cl_ushort4>::color_enabled = true;
}  // namespace

template <typename T, typename P>
MultibrotOpenClCalculator<T, P>::MultibrotOpenClCalculator(
    const boost::compute::device& device, const boost::compute::context& context,
    size_t max_width_pix, size_t max_height_pix)
    : device_(device),
      context_(context),
      queue_(context, device, boost::compute::command_queue::enable_profiling),
      max_width_pix_(max_width_pix),
      max_height_pix_(max_height_pix),
      output_device_vector_(max_width_pix * max_height_pix, context) {
    BuildKernels();
}

template <typename T, typename P>
std::string MultibrotOpenClCalculator<T, P>::PrepareCompilerOptions(const std::string& power_func) {
    return (boost::format("-Werror -DREAL_T=%1% -DRESULT_T=%2% -DRESULT_MAX=%3% "
                          "-DPOWER_FUNC=%4% %5% ") %
            TempValueConstants<T>::opencl_type_name % ResultTypeConstants<P>::result_type_name %
            ResultTypeConstants<P>::result_max_val_macro % power_func %
            (ResultTypeConstants<P>::color_enabled ? "-DCOLOR_ENABLED" : ""))
        .str();
}

template <typename T, typename P>
void MultibrotOpenClCalculator<T, P>::BuildKernels() {
    // Collecting required extensions
    std::string required_extension = TempValueConstants<T>::required_extension;
    std::vector<std::string> extensions;
    if (!required_extension.empty()) {
        extensions.push_back(required_extension);
    }

    static const std::unordered_map<double /* power */, std::string> kFixedPowerFunctions = {
        {1.0, "Power1OfComplex"},
        {2.0, "SquareOfComplex"},
        {3.0, "CubeOfComplex"},
    };

    for (const auto& d : kFixedPowerFunctions) {
        specialized_kernels_.emplace(
            d.first, Utils::BuildKernel(
                         "MultibrotSetKernel", context_, kMainProgram,
                         PrepareCompilerOptions(d.second), extensions));
    }

    universal_kernel_ = Utils::BuildKernel(
        "MultibrotSetKernel", context_, kMainProgram,
        PrepareCompilerOptions("UniversalPowerOfComplex"), extensions);
}

template <typename T, typename P>
void MultibrotOpenClCalculator<T, P>::ExecutePrecalculateChecks(
    size_t width_pix, size_t height_pix, int max_iterations) {
    EXCEPTION_ASSERT(width_pix <= max_width_pix_);
    EXCEPTION_ASSERT(height_pix <= max_height_pix_);
    // Verify that given max iterations is valid for given pixel bit depth
    EXCEPTION_ASSERT(max_iterations <= ResultTypeConstants<P>::result_max_val);
}


//  (C) Copyright Raffi Enficiaud 2019.
//  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 example84
#include <boost/test/included/unit_test.hpp>
#include <random>
#include <cmath>

// this function does not compute properly the polynomial root estimation
// in the case of a double root.
template <class random_generator_t>
std::pair<double, double> estimate_polynomial_roots(
  random_generator_t& gen,
  std::function<double(double)> polynomial) {

  using namespace std;

  std::uniform_real_distribution<> dis(-10, 10);
  double x1 = dis(gen);
  double x2 = dis(gen);
  double fx1 = polynomial(x1);
  double fx2 = polynomial(x2);

  BOOST_TEST_INFO_SCOPE("sample1 = " << x1);
  BOOST_TEST_INFO_SCOPE("sample2 = " << x2);

  // from Vieta formula
  double minus_b = x2 + x1 - (fx2 - fx1) / (x2 - x1);
  double c = (x1 * fx2 - x2 * fx1 + x2 * x1 * x1 - x1 * x2 * x2) / (x1 - x2);

  BOOST_TEST(minus_b * minus_b >= 4*c);

  return std::make_pair(
    (minus_b - sqrt(minus_b * minus_b - 4 * c)) / 2,
    (minus_b + sqrt(minus_b * minus_b - 4 * c)) / 2);
}

BOOST_AUTO_TEST_CASE(quadratic_estimation)
{
  std::random_device rd;
  unsigned int seed = rd();
  std::mt19937 gen(seed);
  std::uniform_int_distribution<> dis(-10, 10);

  BOOST_TEST_MESSAGE("Seed = " << seed);

  for(int i = 0; i < 50; i++) {
    BOOST_TEST_INFO_SCOPE("trial " << i+1);
    int root1 = dis(gen);
    int root2 = dis(gen);
    if(root1 > root2) {
      std::swap(root1, root2);
    }
    BOOST_TEST_INFO_SCOPE("root1 = " << root1);
    BOOST_TEST_INFO_SCOPE("root2 = " << root2);

    std::pair<double, double> estimated = estimate_polynomial_roots(
      gen,
      [root1, root2](double x) -> double { return (x - root1) * (x - root2); });

    BOOST_TEST(estimated.first == double(root1), 10. % boost::test_tools::tolerance());
    BOOST_TEST(estimated.second == double(root2), 10. % boost::test_tools::tolerance());
  }
}
//]

BOOST_AUTO_TEST_CASE(making_it_fail)
{
  // cheating a bit ... but shhhh, do not show in the docs ...
  BOOST_FAIL("Making it fail always on all platforms");
}


/**
* @file: transition.hpp
* @brief: to compute the transitivity closure.
* @author: Changjiang Cai, ccai1@stevens.edu, caicj5351@gmail.com
* @version: 0.0.1
* @creation date: 17-12-2015
* @last modified: Thu 10 Mar 2016 09:32:02 AM EST
*/

#ifndef transition_hpp__
#define transition_hpp__

#include <stdio.h>
#include <vector>
#include <string>
#include <Eigen/Dense>

//read matrix from the file, and generate the transition matrix.
std::vector<std::vector<double> >
generateTransitionMatrix (const std::string &path, const int &n);

std::vector<std::vector<double> >
generateTransitionMatrix (const std::vector<std::vector<double> >& matrix);

std::vector<std::vector<double> >
generateTransitionMatrix_Eigen(const std::vector<std::vector<double>>& matrix);

#endif /* transition_hpp */


/*
    This file is part of beautiful-bullet.

    Copyright (c) 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

    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 <iostream>

#include <Eigen/Core>

#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/parsers/urdf.hpp>

int main(int argc, char const* argv[])
{
    // Model
    pinocchio::Model model;
    pinocchio::urdf::buildModel("models/iiwa_bullet/model.urdf", model);
    pinocchio::Data data(model);

    // Frame ID
    const std::string name = "lbr_iiwa_link_7";
    const int id = model.getFrameId(name);

    // Random joint position and velocity
    Eigen::Matrix<double, 7, 1> q = Eigen::VectorXd::Random(7),
                                v = Eigen::VectorXd::Random(7);

    // Forward Kinematics
    pinocchio::forwardKinematics(model, data, q, v);
    pinocchio::framesForwardKinematics(model, data, q);

    // Jacobian
    pinocchio::Data::Matrix6x J(6, model.nv);
    J.setZero();
    pinocchio::computeFrameJacobian(model, data, q, id, J);

    // Pose
    pinocchio::SE3 pose = data.oMf[id];
    Eigen::Vector3d trans = pose.translation();
    Eigen::Matrix3d rot = pose.rotation();

    // Compare
    std::cout << "Jacobian derivation" << std::endl;
    std::cout << (J * v).transpose() << std::endl;

    std::cout << "Lie Algebra derivation" << std::endl;
    std::cout << pinocchio::log6(pose).toVector().transpose() << std::endl;

    return 0;
}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/spatial_sort.h>
#include <CGAL/Spatial_sort_traits_adapter_3.h>
#include <vector>
#include <boost/iterator/counting_iterator.hpp>

typedef CGAL::Simple_cartesian<double>                  Kernel;
typedef Kernel::Point_3                                 Point_3;
typedef CGAL::Spatial_sort_traits_adapter_3<Kernel,
          CGAL::Pointer_property_map<Point_3>::type > Search_traits_3;

int main()
{
  std::vector<Point_3> points;
  points.push_back(Point_3(1,3,11));
  points.push_back(Point_3(14,34,46));
  points.push_back(Point_3(414,34,4));
  points.push_back(Point_3(4,2,56));
  points.push_back(Point_3(744,4154,43));
  points.push_back(Point_3(74,44,1));

  std::vector<std::size_t> indices;
  indices.reserve(points.size());

  std::copy(boost::counting_iterator<std::size_t>(0),
            boost::counting_iterator<std::size_t>(points.size()),
            std::back_inserter(indices));

  CGAL::spatial_sort( indices.begin(),
                      indices.end(),
                      Search_traits_3(CGAL::make_property_map(points)) );

  for (std::vector<std::size_t>::iterator it=indices.begin();it!=indices.end();++it)
    std::cout << points[*it] << "\n";

  std::cout << "done" << std::endl;

  return 0;
}