// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_INTERVAL_ERROR_BOUND_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_INTERVAL_ERROR_BOUND_HPP

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expression_tree.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template <typename Expression, std::size_t max_argn>
struct interval_min_impl
{
    using type = Expression;
};

template <typename Expression, std::size_t max_argn>
struct interval_max_impl
{
    using type = Expression;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_min_impl<difference<Left, Right>, max_argn>
{
private:
    using min_left = typename interval_min_impl<Left, max_argn>::type;
    using max_right = typename interval_max_impl<Right, max_argn>::type;
public:
    using type = difference<min_left, max_right>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_min_impl<sum<Left, Right>, max_argn>
{
private:
    using min_left = typename interval_min_impl<Left, max_argn>::type;
    using min_right = typename interval_min_impl<Right, max_argn>::type;
public:
    using type = sum<min_left, min_right>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_max_impl<difference<Left, Right>, max_argn>
{
private:
    using max_left = typename interval_max_impl<Left, max_argn>::type;
    using min_right = typename interval_min_impl<Right, max_argn>::type;
public:
    using type = difference<max_left, min_right>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_max_impl<sum<Left, Right>, max_argn>
{
private:
    using max_left = typename interval_max_impl<Left, max_argn>::type;
    using max_right = typename interval_max_impl<Right, max_argn>::type;
public:
    using type = sum<max_left, max_right>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_min_impl<product<Left, Right>, max_argn>
{
private:
    using min_left = typename interval_min_impl<Left, max_argn>::type;
    using max_left = typename interval_max_impl<Left, max_argn>::type;
    using min_right = typename interval_min_impl<Right, max_argn>::type;
    using max_right = typename interval_max_impl<Right, max_argn>::type;
public:
    using type = min
        <
            min
                <
                    product<min_left, min_right>,
                    product<min_left, max_right>
                >,
            min
                <
                    product<max_left, min_right>,
                    product<max_left, max_right>
                >
        >;
};

template <typename Child, std::size_t max_argn>
struct interval_min_impl<product<Child, Child>, max_argn>
{
private:
    using min_child = typename interval_min_impl<Child, max_argn>::type;
    using max_child = typename interval_max_impl<Child, max_argn>::type;
public:
    using type = min
        <
            product<min_child, min_child>,
            product<max_child, max_child>
        >;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_max_impl<product<Left, Right>, max_argn>
{
private:
    using min_left = typename interval_min_impl<Left, max_argn>::type;
    using max_left = typename interval_max_impl<Left, max_argn>::type;
    using min_right = typename interval_min_impl<Right, max_argn>::type;
    using max_right = typename interval_max_impl<Right, max_argn>::type;
public:
    using type = max
        <
            max
                <
                    product<min_left, min_right>,
                    product<min_left, max_right>
                >,
            max
                <
                    product<max_left, min_right>,
                    product<max_left, max_right>
                >
        >;
};

template <typename Child, std::size_t max_argn>
struct interval_max_impl<product<Child, Child>, max_argn>
{
private:
    using min_child = typename interval_min_impl<Child, max_argn>::type;
    using max_child = typename interval_max_impl<Child, max_argn>::type;
public:
    using type = max
        <
            product<min_child, min_child>,
            product<max_child, max_child>
        >;
};

template <typename Child, std::size_t max_argn>
struct interval_min_impl<abs<Child>, max_argn>
{
private:
    using min_child = typename interval_min_impl<Child, max_argn>::type;
    using max_child = typename interval_max_impl<Child, max_argn>::type;
public:
    using type = min<abs<min_child>, abs<max_child>>;
};

template <typename Child, std::size_t max_argn>
struct interval_max_impl<abs<Child>, max_argn>
{
private:
    using min_child = typename interval_min_impl<Child, max_argn>::type;
    using max_child = typename interval_max_impl<Child, max_argn>::type;
public:
    using type = max<abs<min_child>, abs<max_child>>;
};

template <std::size_t argn, std::size_t max_argn>
struct interval_min_impl<argument<argn>, max_argn>
{
    using type = argument<argn + max_argn>;
};

template <std::size_t argn, std::size_t max_argn>
struct interval_max_impl<argument<argn>, max_argn>
{
    using type = argument<argn>;
};

template <typename Expression, std::size_t max_argn>
struct interval_impl
{
    using type = Expression;
};

template <typename Child, std::size_t max_argn>
struct interval_impl<abs<Child>, max_argn>
{
private:
    using min_child = typename interval_min_impl<Child, max_argn>::type;
    using max_child = typename interval_max_impl<Child, max_argn>::type;
public:
    using type = max<abs<min_child>, abs<max_child>>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_impl<difference<Left, Right>, max_argn>
{
private:
    using left = typename interval_impl<Left, max_argn>::type;
    using right = typename interval_impl<Right, max_argn>::type;
public:
    using type = difference<left, right>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_impl<sum<Left, Right>, max_argn>
{
private:
    using left = typename interval_impl<Left, max_argn>::type;
    using right = typename interval_impl<Right, max_argn>::type;
public:
    using type = sum<left, right>;
};

template <typename Left, typename Right, std::size_t max_argn>
struct interval_impl<product<Left, Right>, max_argn>
{
private:
    using left = typename interval_impl<Left, max_argn>::type;
    using right = typename interval_impl<Right, max_argn>::type;
public:
    using type = product<left, right>;
};

template <typename Expression, std::size_t max_argn>
using interval_min = typename interval_min_impl<Expression, max_argn>::type;

template <typename Expression, std::size_t max_argn>
using interval_max = typename interval_max_impl<Expression, max_argn>::type;

template <typename Expression>
using interval =
    typename interval_impl<Expression, max_argn<Expression>::value>::type;

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_INTERVAL_ERROR_BOUND_HPP


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


#pragma once

#include <algorithm>
#include <cassert>
#include <vector>

#include <Eigen/Core>

#include "cho_util/core/random.hpp"

namespace cho {
namespace core {

void GenerateConvexPolygon(
    const int n, std::vector<Eigen::Vector2f>* const points,
    cho::core::RNG& rng = cho::core::RNG::GetInstance()) {
  assert(n > 2);

  std::vector<float> xv, yv;
  for (auto& v_out : {&xv, &yv}) {
    float v0{1}, v1{1};
    v_out->clear();
    v_out->reserve(n);
    for (int i = 0; i < n - 2; ++i) {
      const bool sel = rng.Uniform(0, 1) < 0.5f;
      const float v = rng.Uniform(0, 1);
      if (sel) {
        v_out->emplace_back(v - v0);
        v0 = v;
      } else {
        v_out->emplace_back(v1 - v);
        v1 = v;
      }
    }
    v_out->emplace_back(v1 - 0);
    v_out->emplace_back(0 - v0);
  }
  std::shuffle(yv.begin(), yv.end(), rng.generator);

  // Compute angles of each cumulative vector.
  std::vector<float> hv(n);
  for (int i = 0; i < n; ++i) {
    hv[i] = std::atan2(yv[i], xv[i]);
  }

  // Argsort by angle.
  std::vector<int> indices(n);
  std::iota(indices.begin(), indices.end(), 0);
  std::sort(indices.begin(), indices.end(),
            [&hv](const int i0, const int i1) { return hv[i0] < hv[i1]; });

  // Accumulative vector sum into output.
  Eigen::Vector2f point{0, 0};
  points->clear();
  points->reserve(n);
  for (int i = 0; i < xv.size(); ++i) {
    point.x() += xv[indices[i]];
    point.y() += yv[indices[i]];
    points->emplace_back(point);
  }
}

}  // namespace core
}  // namespace cho


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2019 Prashant K. Jha
//
//  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)
////////////////////////////////////////////////////////////////////////////////

#ifndef UTILS_H
#define UTILS_H

#include "utilCompare.hpp"
#include "utilIO.hpp"
#include "utilLibs.hpp"
#include <boost/random.hpp>

//#include "../../external/gmm/gmm.h"

#define X_POSITIVE_BOUNDARY 0
#define X_NEGATIVE_BOUNDARY 1
#define Y_POSITIVE_BOUNDARY 2
#define Y_NEGATIVE_BOUNDARY 3
#define Z_POSITIVE_BOUNDARY 4
#define Z_NEGATIVE_BOUNDARY 5

#define OUTER_BOUNDARY_NODE 0
#define INNER_BOUNDARY_NODE 1
#define INNER_NETWORK_NODE 2

namespace util {

/*!
 * @brief Return the Square of numbers and ignores values lower than 0 and
 * higher than 1
 *
 * @param x  Number
 * @return value Value between 0 and 1
 */
double square(double x);

/*!
 * @brief Ignores values lower than 0 and higher than 1
 *
 * @param x  Number
 * @return value Value between 0 and 1
 */
double linear(double x);

/*!
 * @brief Regularized Heaviside function
 *
 * @param x  Number
 * @return value Heaveside function
 */
double heaviside(double x);

/*!
 * @brief Modulus of a vector (norm)
 *
 * @param xg X coordinate
 * @param yg Y coordinate
 * @return value Norm
 */
double modulus(double xg, double yg);

/*!
 * @brief Determinant of matrix
 *
 * @param M Matrix
 * @return value Determinant of M
 */
double determinant(const std::vector<std::vector<double>> &M);

/*!
 * @brief Inverse of matrix
 *
 * @param M Matrix
 * @return Minv Inverse of matrix M
 */
std::vector<std::vector<double>> inverse(const std::vector<std::vector<double>> &M);

/*!
 * @brief Transpose of matrix
 *
 * @param M Matrix
 * @return Mt Transpose of matrix M
 */
std::vector<std::vector<double>> transpose(const std::vector<std::vector<double>> &M);

/*!
 * @brief Add element to list
 *
 * @param i Element to be added
 * @param data List
 * @return true True if i is added to list
 */
bool addToList(unsigned int i, std::vector<unsigned int> *data);

/*!
 * @brief compute the mass of the qoi
 *
 * @param es Equation system
 * @param system_name Name of system
 * @param var_name Name of variable
 * @param value_mass Value of mass
 */
void computeMass(EquationSystems &es, const std::string &system_name,
                 const std::string &var_name, double &value_mass);

/*! @brief Position index of given point in 2d
   * @param x X coordinate of point
   * @param y Y coordinate of point
   * @param Nelx Number of elements in along x-axis (and y-axis)
   * @return Index Unique integer corresponding to the point
 */
int positionIndex(double x, double y, int Nelx);

/*!
 * @brief Projects concentration to physical range i.e. [0,1]
 *
 * @param x Number
 * @return value Value between 0 and 1
 */
double project_concentration(double x);

/*!
 * @brief Find node closer to given point
 *
 * @param x Point
 * @param mesh Mesh
 * @return id Id of node
 */
unsigned int locate_node(const Point &x, const MeshBase &mesh);

/*!
 * @brief Returns point in line formed by points p1 and p2
 *
 * @param p1 Point 1
 * @param p2 Point 2
 * @param s Parametric coordinate
 * @return p Point
 */
Point point_on_line(const Point &p1, const Point &p2, const double &s);

/*!
 * @brief Returns direction from point 1 to 2
 *
 * @param p1 Point 1
 * @param p2 Point 2
 * @return p Unit vector pointing at point 2 from point 1
 */
Point get_direction(const Point &p1, const Point &p2);

/*!
 * @brief Returns true if point is inside the box
 *
 * @param p Point
 * @param box Box
 * @return True If point inside box otherwise false
 */
bool is_inside_box(const Point &p, const std::pair<Point, Point> &box, double tol = 0.);
bool is_inside_box(const Point &p, const double &box_size, double tol = 0.);

/*!
 * @brief Returns true if point is inside the cylinder
 *
 * @param p Point
 * @param length Length of cylinder
 * @param radius Radius of cylinder
 * @param axis Axis of cylinder
 * @return True If point inside cylinder otherwise false
 */
bool is_inside_cylinder(const Point &p, const double &length, const double &radius, const Point &axis);

/*!
 * @brief Returns true if point is inside the cylinder
 *
 * @param p Point
 * @param R Radius of cylinder
 * @param x1 Point at the center of cross-section at s=0
 * @param x2 Point at the center of cross-section at s=L
 * @param
 * @return True If point inside cylinder otherwise false
 */
bool is_inside_cylinder(const Point &p, const double &radius, const Point &x1,
                        const Point &x2);

/*!
 * @brief Returns true if point is inside the ellipsoid
 *
 * @param p Point
 * @param center Center of ellipse
 * @param radius_vec Vector of radius describing ellipse
 * @param dim Dimension
 * @param d
 * @return True If point inside otherwise false
 */
bool is_inside_ellipse(const Point &p, const Point &center, const std::vector<double> &radius_vec, unsigned int dim);

/*!
 * @brief Returns true if point is inside the ellipsoid
 *
 * Also computes
 * d = x^2 / r1^2 + y^2 / r2^2 + z^2 / r3^2
 *
 * @param p Point
 * @param center Center of ellipse
 * @param radius_vec Vector of radius describing ellipse
 * @param dim Dimension
 * @param d
 * @return True If point inside otherwise false
 */
bool is_inside_ellipse(const Point &p, const Point &center, const std::vector<double> &radius_vec, unsigned int dim, double &d);

/*!
 * @brief Transforms point in ellipse to point in ball of given radius
 *
 * @param p Point
 * @param center Center of ellipse
 * @param radius_vec Vector of radius describing ellipse
 * @param dim Dimension
 * @param ball_r Radius of ball
 * @return Point Point in ball
 */
Point ellipse_to_ball(const Point &p, const Point &center, const std::vector<double> &radius_vec, unsigned int dim, const double &ball_r);

/*!
 * @brief Returns the vector after rotating by desired angle
 *
 * @param p Vector
 * @param theta Angle of rotation
 * @param axis Axis of rotation
 */
Point rotate(const Point &p, const double &theta, const Point &axis);

/*!
 * @brief Returns the unit vector corresponding to cross product
 *
 * Note: Better to use inbuilt function in Point - p1.cross(p2)
 *
 * @param p1 Vector 1
 * @param p2 Vector 2
 * @return vector Unit vector along the cross product
 */
Point cross_product(const Point &p1, const Point &p2);

/*!
 * @brief Computes angle between two vectors
 * @param a Vector 1
 * @param b Vector 2
 */
double angle(Point a, Point b);

/*!
 * @brief Computes angle between two vectors
 * @param a Vector 1
 * @param b Vector 2
 * @param axis Axis of rotation
 * @param is_axis If true then axis is the axis of orientation, otherwise
 * axis specifies the +ve side of the plane in which a and b are
 */
double angle(Point a, Point b, Point axis, bool is_axis = true);

/*!
 * @brief Computes following function
 *
 * exp[1 - 1 / (1 - r^a)]
 *
 * @param r Argument of function
 * @param exponent Value of power a in above function
 */
double exp_decay_function(double r, double exponent = 4.);

std::vector<Point> discretize_ball_surface(const unsigned int
                                             &disc_num,
                                           const double
                                             &ball_r,
                                           unsigned int dim);

std::vector<Point> discretize_cube(const unsigned int
                                     &disc_num,
                                   const double
                                     &cube_size,
                                   unsigned int dim);

/*!
 * @brief Do lines intersect
 *
 * @param line_1 Line 1
 * @param line_2 Line 2
 * @return True If lines intersect, else false
 */
bool lines_intersect(const std::pair<Point, Point> &line_1, const std::pair<Point,
                                                                            Point> &line_2);

/*!
 * @brief Compute distance between lines
 *
 * @param line_1 Line 1
 * @param line_2 Line 2
 * @return Value Distance
 */
double distance_between_lines(const std::pair<Point, Point> &line_1,
                              const std::pair<Point, Point> &line_2);
double distance_between_segments(const std::pair<Point, Point> &line_1,
                                 const std::pair<Point, Point> &line_2);

/*!
 * @brief Compute distance between planes
 *
 * @param plane_1 Plane 1 given by pair of normal and one point which it
 * contains
 * @param plane_2 Plane 2 given by pair of normal and one point which it
 * contains
 * @return Value Distance
 */
double distance_between_planes(const std::pair<Point, Point> &plane_1,
                               const std::pair<Point, Point> &plane_2);

/*!
 * @brief Compute distance between point and line
 *
 * @param p Point
 * @param line Line
 * @return Value Distance
 */
double point_distance_line(const Point &p,
                           const std::pair<Point, Point> &line);
double point_distance_segment(const Point &p,
                              const std::pair<Point, Point> &line);

/*!
 * @brief Compute distance between point and plane
 *
 * @param p Point
 * @param plane Plane given by pair of normal and one point which it
 * contains
 * @return Value Distance
 */
double point_distance_plane(const Point &p,
                            const std::pair<Point, Point> &plane);

/*!
 * @brief Get list of elements which have this node as vertex
 *
 * @param node_id Node id
 * @param mesh Mesh
 * @return List
 */
std::vector<unsigned int> find_elems(const unsigned int node_id,
                                     const MeshBase &mesh);

/*!
 * @brief Get list of elements which have this node as vertex
 *
 * @param node_id Node id
 * @param mesh Mesh
 * @return List
 */
void add_unique(unsigned int dof, Real val,
                std::vector<unsigned int> &list, std::vector<Real> &list_val);

/*!
 * @brief Get the major axis which is parallel to given vector within some
 * tolerance
 *
 * @param vec Given vector
 * @return axis Parallel to vec
 */
std::string get_vec_major_axis(Point vec);

/*!
 * @brief Get the length between two points
 *
 * @param p1
 * @param p2
 * @return distance
 */

double dist_between_points(const std::vector<double> &p1, const std::vector<double> &p2);

void get_unit_vector(std::vector<double> &p1);

unsigned int get_elem_id(const std::vector<double> &p, const double &mesh_size,
                         const unsigned int &num_elems, const unsigned int &dim);
unsigned int get_elem_id(const Point &p, const double &mesh_size,
                         const unsigned int &num_elems, const unsigned int &dim);

Point to_point(const std::vector<double> &p);

std::vector<double> cross_prod(std::vector<double> &p1, std::vector<double> &p2);

std::vector<double> rotate(std::vector<double> &p, double theta, std::vector<double> &axis);

Point determineRotator(const Point &dir);
std::vector<double> determineRotator(const std::vector<double> &dir);

inline float time_diff(std::chrono::steady_clock::time_point begin,
                       std::chrono::steady_clock::time_point end) {

  return std::chrono::duration_cast<std::chrono::microseconds>(end -
                                                               begin)
    .count();
}

template<class T>
inline int locate_in_set(const T &key, const std::vector<T> &set) {

  for (int i = 0; i < set.size(); i++)
    if (set[i] == key)
      return i;

  return -1;
}

inline void clear_oss(std::ostringstream &oss) {
  oss.str("");
  oss.clear();
}

template<class T>
inline T get_avg(const std::vector<T> &list) {

  if (list.size() == 0)
    return T(0);

  T avg = 0.;
  for (const auto &l : list)
    avg += l;
  avg = avg / (double(list.size()));

  return avg;
}

template<class T>
inline T get_std_dev(const std::vector<T> &list, const T mean) {

  if (list.size() == 0)
    return T(0);

  T dev = 0.;
  for (const auto &l : list)
    dev += (l - mean) * (l - mean);
  dev = dev / (T(list.size()));

  return std::sqrt(dev);
}

template<class T>
inline T get_std_dev(const std::vector<T> &list) {

  if (list.size() == 0)
    return T(0);

  // get mean
  auto mean = get_avg(list);

  T dev = 0.;
  for (const auto &l : list)
    dev += (l - mean) * (l - mean);
  dev = dev / (T(list.size()));

  return std::sqrt(dev);
}

inline std::mt19937 get_random_generator(int seed) {
  if (seed < 0) {
    std::random_device rd;
    return std::mt19937(rd());
  } else {
    return std::mt19937(seed);
  }
}

inline boost::mt19937 get_random_generator_boost(int seed) {

  if (seed < 0) {
    auto rd = boost::mt19937();
    return rd;
  } else {
    auto rd = boost::mt19937(seed);
    return rd;
  }
}

inline double proj(double x) {
  auto y = 1. < x ? 1. : x;
  return 0. > y ? 0. : y;
}

} // namespace util

#endif // UTILS_H


#include <aikido/constraint/NewtonsMethodProjectable.hpp>
#include <aikido/constraint/Satisfied.hpp>
#include <aikido/constraint/dart/TSR.hpp>
#include "PolynomialConstraint.hpp"

#include <aikido/statespace/Rn.hpp>

#include <Eigen/Dense>
#include <gtest/gtest.h>

using aikido::constraint::NewtonsMethodProjectable;
using aikido::constraint::Satisfied;
using aikido::constraint::dart::TSR;
using aikido::statespace::R1;
using aikido::statespace::R3;

TEST(NewtonsMethodProjectableTest, ConstructorThrowsOnNullDifferentiable)
{
  EXPECT_THROW(
      NewtonsMethodProjectable(nullptr, std::vector<double>{}, 1, 1),
      std::invalid_argument);
}

TEST(NewtonsMethodProjectableTest, ConstructorThrowsOnBadToleranceDimension)
{
  auto ss = std::make_shared<R3>();
  auto constraint = std::make_shared<Satisfied>(ss); // dimension = 0
  EXPECT_THROW(
      NewtonsMethodProjectable(constraint, std::vector<double>({0.1}), 1, 1e-4),
      std::invalid_argument);
}

TEST(NewtonsMethodProjectableTest, ConstructorThrowsOnNegativeTolerance)
{
  auto constraint
      = std::make_shared<PolynomialConstraint<1>>(Eigen::Vector3d(1, 2, 3));
  EXPECT_THROW(
      NewtonsMethodProjectable(
          constraint, std::vector<double>({-0.1}), 1, 1e-4),
      std::invalid_argument);
}

TEST(NewtonsMethodProjectableTest, ConstructorThrowsOnNegativeIteration)
{
  auto ss = std::make_shared<R3>();
  auto constraint = std::make_shared<Satisfied>(ss); // dimension = 0
  EXPECT_THROW(
      NewtonsMethodProjectable(constraint, std::vector<double>(), 0, 1e-4),
      std::invalid_argument);
  EXPECT_THROW(
      NewtonsMethodProjectable(constraint, std::vector<double>(), -1, 1e-4),
      std::invalid_argument);
}

TEST(NewtonsMethodProjectableTest, ConstructorThrowsOnNegativeStepsize)
{
  auto ss = std::make_shared<R3>();
  auto constraint = std::make_shared<Satisfied>(ss); // dimension = 0
  EXPECT_THROW(
      NewtonsMethodProjectable(constraint, std::vector<double>(), 1, 0),
      std::invalid_argument);
  EXPECT_THROW(
      NewtonsMethodProjectable(constraint, std::vector<double>(), 1, -0.1),
      std::invalid_argument);
}

TEST(NewtonsMethodProjectable, Constructor)
{
  // Constraint: x^2 - 1 = 0.
  NewtonsMethodProjectable projector(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector3d(-1, 0, 1)),
      std::vector<double>({0.1}),
      10,
      1e-4);
}

TEST(NewtonsMethodProjectable, ProjectPolynomialFirstOrder)
{
  NewtonsMethodProjectable projector(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector2d(1, 2)),
      std::vector<double>({0.1}),
      1,
      1e-4);

  Eigen::VectorXd v(1);
  v(0) = -2;

  R1 rvss;
  auto s1 = rvss.createState();
  s1.setValue(v);

  auto out = rvss.createState();
  ASSERT_TRUE(projector.project(s1, out));
  Eigen::VectorXd projected = rvss.getValue(out);

  Eigen::VectorXd expected(1);
  expected(0) = -0.5;

  EXPECT_TRUE(expected.isApprox(projected));
}

TEST(NewtonsMethodProjectable, ProjectPolynomialSecondOrder)
{
  // Constraint: x^2 - 1 = 0.
  NewtonsMethodProjectable projector(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector3d(-1, 0, 1)),
      std::vector<double>({1e-6}),
      10,
      1e-8);

  // Project x = -2. Should get -1 as projected solution.
  Eigen::VectorXd v(1);
  v(0) = -2;

  R1 rvss;
  auto seedState = rvss.createState();
  seedState.setValue(v);

  auto out = rvss.createState();
  EXPECT_TRUE(projector.project(seedState, out));
  Eigen::VectorXd projected = rvss.getValue(out);

  Eigen::VectorXd expected(1);
  expected(0) = -1;

  EXPECT_TRUE(expected.isApprox(projected, 1e-5));

  // Project x = 1.5. Should get 1 as projected solution.
  v(0) = 1.5;
  seedState.setValue(v);

  EXPECT_TRUE(projector.project(seedState, out));
  projected = rvss.getValue(out);

  expected(0) = 1;

  EXPECT_TRUE(expected.isApprox(projected, 1e-5));

  // Project x = 1. Should get 1 as projected solution.
  v(0) = 1;
  seedState.setValue(v);

  EXPECT_TRUE(projector.project(seedState, out));
  projected = rvss.getValue(out);

  expected(0) = 1;

  EXPECT_TRUE(expected.isApprox(projected, 1e-5));
}

TEST(NewtonsMethodProjectable, ProjectTSRTranslation)
{
  std::shared_ptr<TSR> tsr = std::make_shared<TSR>();

  // non-trivial translation bounds
  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = 1;
  Bw(0, 1) = 2;

  tsr->mBw = Bw;

  auto space = tsr->getSE3();

  auto seedState = space->createState();

  Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity();
  isometry.translation() = Eigen::Vector3d(-1, 0, 1);
  seedState.setIsometry(isometry);

  NewtonsMethodProjectable projector(
      tsr, std::vector<double>(6, 1e-4), 1000, 1e-8);

  auto out = space->createState();
  EXPECT_TRUE(projector.project(seedState, out));
  auto projected = space->getIsometry(out);

  Eigen::Isometry3d expected = Eigen::Isometry3d::Identity();
  expected.translation() = Eigen::Vector3d(1, 0, 0);

  EXPECT_TRUE(expected.isApprox(projected, 5e-4));
}

TEST(NewtonsMethodProjectable, ProjectTSRRotation)
{
  std::shared_ptr<TSR> tsr = std::make_shared<TSR>();

  // non-trivial rotation bounds
  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(3, 0) = M_PI_4;
  Bw(3, 1) = M_PI_2;

  tsr->mBw = Bw;

  auto space = tsr->getSE3();
  auto seedState = space->createState();

  NewtonsMethodProjectable projector(
      tsr, std::vector<double>(6, 1e-4), 1000, 1e-8);

  auto out = space->createState();
  EXPECT_TRUE(projector.project(seedState, out));
  auto projected = space->getIsometry(out);

  Eigen::Isometry3d expected = Eigen::Isometry3d::Identity();

  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitX());
  expected.linear() = rotation;

  EXPECT_TRUE(expected.isApprox(projected, 5e-4));
}


#include "testsuite.h"
#include <blitz/array.h>
#include <blitz/array/stencil-et.h>

BZ_USING_NAMESPACE(blitz)

typedef blitz::Array<double,1> array_1;
typedef blitz::Array<double,2> array_2;
typedef blitz::Array<double,3> array_3;

/* Test slicing functionality for expressions. */

#define test_expr(d1,d2) BZTEST(all(abs(d1-d2)<1e-5));

// test with functors
class doubler {
public:
  double operator()(double x) const {return 2.0*x;}
  BZ_DECLARE_FUNCTOR(doubler);
};

class multiplier {
public:
  double operator()(double a, double b) const {return a*b;}
  BZ_DECLARE_FUNCTOR2(multiplier);
};

int main()
{
  array_2 a(5);
  a=tensor::i+10.*tensor::j;

  test_expr((2.*a)(1,Range::all()), array_2(2.*a)(1,Range::all()));
  test_expr(sin(a)(1,Range::all()), array_2(sin(a))(1,Range::all()));
  test_expr(where(a<5.,1./a,a)(1,Range::all()),
	    array_2(where(a<5.,1./a,a))(1,Range::all()));
  // stencils can't be sliced down in rank, so integers are converted to unit ranges
  test_expr(Laplacian2D(sin(a))(1,Range::all()),
	    array_2(Laplacian2D(sin(a)))(Range(1,1),Range::all()));
  doubler d;
  multiplier m;
  test_expr(d(a)(1,Range::all()), array_2(d(a))(1,Range::all()));
  test_expr(m(a,a)(1,Range::all()), array_2(m(a,a))(1,Range::all()));

  //complete slicing to scalar
  BZTEST((2.*a)(1,1)==array_2(2.*a)(1,1));
  BZTEST((2.*a(1,Range::all())(1))==(array_1(2.*a(1,Range::all()))(1)));

  //more dims
  Array<double,5> big(2,3,4,5,6);
  big=tensor::i+10.*tensor::j+100*tensor::k+1000.*tensor::l+10000.*tensor::m;
  BZTEST(((2.*big)(1,2,3,4,5))==2.*big(1,2,3,4,5));

  test_expr((2.*big)(0,Range(0,1),1,Range::all(), Range(2,4,2)),
	    (Array<double,3>(2.*big(0,Range(0,1),1,Range::all(), Range(2,4,2)))));

  // slicing reductions, index remappings and index placeholder
  // expressions does not work.

  return 0;
}


/*
 * Copyright 2009-2018 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE bfgs_test
#include <boost/test/unit_test.hpp>
#include <votca/xtp/optimiser_costfunction.h>
#include <votca/xtp/bfgs-trm.h>
#include <votca/ctp/logger.h>
#include <iostream>
#include <boost/format.hpp>
using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(bfgs_test)

BOOST_AUTO_TEST_CASE(parabola_test) {
  class parabola : public Optimiser_costfunction{

     double EvaluateCost(const Eigen::VectorXd& parameters) {
      Eigen::VectorXd value=parameters;
      value(0)-=2;
      double cost = value.cwiseAbs2().sum();
      return cost;
    }

     Eigen::VectorXd EvaluateGradient(const Eigen::VectorXd& parameters){
       Eigen::VectorXd gradient=2*parameters;
       gradient(0)-=4;
       return gradient;
     }

    bool Converged(const Eigen::VectorXd& delta_parameters,
            double delta_cost, const Eigen::VectorXd& gradient){
      if (gradient.cwiseAbs().maxCoeff() < 1e-9)return true;
      else return false;
    }

    int NumParameters()const {
      return 5;
    }

  };

  parabola p5;
  BFGSTRM bfgstrm(p5);
  bfgstrm.setNumofIterations(100);
  bfgstrm.setTrustRadius(0.1);
  bfgstrm.Optimize(5*Eigen::VectorXd::Ones(5));

  Eigen::VectorXd ref=Eigen::VectorXd::Zero(5);
  ref(0)=2;
 bool equal= bfgstrm.getParameters().isApprox(ref,0.00001);
 if(!equal){
   cout<<"minimum found:"<<endl;
   cout<<bfgstrm.getParameters()<<endl;
   cout<<"minimum ref:"<<endl;
   cout<<ref<<endl;
 }else{
   cout<<bfgstrm.getIteration()<<endl;
 }
  BOOST_CHECK_EQUAL(equal, 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <boost/chrono.hpp>
//#include <chrono>
#include <ctime>

long fibonacci(unsigned n)
{
    if (n < 2) return n;
    return fibonacci(n-1) + fibonacci(n-2);
}

int main()
{
    boost::chrono::time_point<boost::chrono::system_clock> start, end;
    start = boost::chrono::system_clock::now();
    std::cout << "f(42) = " << fibonacci(42) << '\n';
    end = boost::chrono::system_clock::now();

    boost::chrono::duration<double> elapsed_seconds = end-start;
    std::time_t end_time = boost::chrono::system_clock::to_time_t(end);

    std::cout << "finished computation at " << std::ctime(&end_time)
              << "elapsed time: " << elapsed_seconds.count() << "s\n";
}


#include <iostream>
#include <boost/numeric/interval.hpp>

int main()
{
	boost::numeric::interval<int> range1(0, 100);
	std::cout << boost::numeric::median(range1) << std::endl;
	std::cout << boost::numeric::width(range1) << std::endl;

	boost::numeric::interval<int> range2(6, 7);
	boost::numeric::interval<int> range3(4, 8);

	auto range_min = boost::numeric::min(range2, range3);

	std::cout << range_min.lower() << " ~ "
		<< range_min.upper() << std::endl;

	auto range_max = boost::numeric::max(range2, range3);

	std::cout << range_max.lower() << " ~ "
		<< range_max.upper() << std::endl;

	boost::numeric::interval<int> range4(-4, 8);

	auto range_abs = boost::numeric::abs(range4);

	std::cout << range_abs.lower() << " ~ "
		<< range_abs.upper() << std::endl;

	return 0;
}


// Boost.Graph library isomorphism test

// Copyright (C) 2001 Douglas Gregor (gregod@cs.rpi.edu)
//
// 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)

// For more information, see http://www.boost.org
//
// Revision History:
//
// 29 Nov 2001    Jeremy Siek
//      Changed to use Boost.Random.
// 29 Nov 2001    Doug Gregor
//      Initial checkin.

#define BOOST_INCLUDE_MAIN
#include <boost/test/test_tools.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/isomorphism.hpp>
//#include "isomorphism-v3.hpp"
#include <boost/property_map/property_map.hpp>
#include <iostream>
#include <fstream>
#include <map>
#include <algorithm>
#include <cstdlib>
#include <ctime>

using namespace boost;

enum
{
    a,
    b,
    c,
    d,
    e,
    f,
    g,
    h
};
enum
{
    _1,
    _2,
    _3,
    _4,
    _5,
    _6,
    _7,
    _8
};

void test_isomorphism()
{
    typedef adjacency_list< vecS, vecS, bidirectionalS > GraphA;
    typedef adjacency_list< vecS, vecS, bidirectionalS > GraphB;

    char a_names[] = "abcdefgh";
    char b_names[] = "12345678";

    GraphA Ga(8);
    add_edge(a, d, Ga);
    add_edge(a, h, Ga);
    add_edge(b, c, Ga);
    add_edge(b, e, Ga);
    add_edge(c, f, Ga);
    add_edge(d, a, Ga);
    add_edge(d, h, Ga);
    add_edge(e, b, Ga);
    add_edge(f, b, Ga);
    add_edge(f, e, Ga);
    add_edge(g, d, Ga);
    add_edge(g, f, Ga);
    add_edge(h, c, Ga);
    add_edge(h, g, Ga);

    GraphB Gb(8);
    add_edge(_1, _6, Gb);
    add_edge(_2, _1, Gb);
    add_edge(_2, _5, Gb);
    add_edge(_3, _2, Gb);
    add_edge(_3, _4, Gb);
    add_edge(_4, _2, Gb);
    add_edge(_4, _3, Gb);
    add_edge(_5, _4, Gb);
    add_edge(_5, _6, Gb);
    add_edge(_6, _7, Gb);
    add_edge(_6, _8, Gb);
    add_edge(_7, _8, Gb);
    add_edge(_8, _1, Gb);
    add_edge(_8, _7, Gb);

    std::vector< std::size_t > in_degree_A(num_vertices(Ga));
    boost::detail::compute_in_degree(Ga, &in_degree_A[0]);

    std::vector< std::size_t > in_degree_B(num_vertices(Gb));
    boost::detail::compute_in_degree(Gb, &in_degree_B[0]);

    degree_vertex_invariant< std::size_t*, GraphA > invariantA(
        &in_degree_A[0], Ga);
    degree_vertex_invariant< std::size_t*, GraphB > invariantB(
        &in_degree_B[0], Gb);

    std::vector< graph_traits< GraphB >::vertex_descriptor > f(
        num_vertices(Ga));

    bool ret = isomorphism(Ga, Gb, &f[0], invariantA, invariantB,
        (invariantB.max)(), get(vertex_index, Ga), get(vertex_index, Gb));
    assert(ret == true);

    for (std::size_t i = 0; i < num_vertices(Ga); ++i)
        std::cout << "f(" << a_names[i] << ")=" << b_names[f[i]] << std::endl;

    BOOST_TEST(verify_isomorphism(Ga, Gb, &f[0]));
}

int test_main(int, char*[])
{
    test_isomorphism();
    return boost::report_errors();
}


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_math_matrix_hpp__
#define __pinocchio_math_matrix_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/math/fwd.hpp"

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

namespace pinocchio
{

  template<typename Derived>
  inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
  {
    return !((m.derived().array()==m.derived().array()).all());
  }

  template<typename M1, typename M2>
  struct MatrixMatrixProduct
  {
#if EIGEN_VERSION_AT_LEAST(3,2,90)
    typedef typename Eigen::Product<M1,M2> type;
#else
    typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
#endif
  };

  template<typename Scalar, typename Matrix>
  struct ScalarMatrixProduct
  {
#if EIGEN_VERSION_AT_LEAST(3,3,0)
    typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<Scalar,typename Eigen::internal::traits<Matrix>::Scalar>,
    const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type, const Matrix> type;
#elif EIGEN_VERSION_AT_LEAST(3,2,90)
    typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#else
    typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#endif
  };

  template<typename Matrix, typename Scalar>
  struct MatrixScalarProduct
  {
#if EIGEN_VERSION_AT_LEAST(3,3,0)
    typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<typename Eigen::internal::traits<Matrix>::Scalar,Scalar>,
    const Matrix, const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type> type;
#elif EIGEN_VERSION_AT_LEAST(3,2,90)
    typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#else
    typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#endif
  };

  namespace internal
  {
    template<typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
    struct isUnitaryAlgo
    {
      typedef typename MatrixLike::Scalar Scalar;
      typedef typename MatrixLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
                      const RealScalar & prec =
                      Eigen::NumTraits< Scalar >::dummy_precision())
      {
        return mat.isUnitary(prec);
      }
    };

    template<typename MatrixLike>
    struct isUnitaryAlgo<MatrixLike,false>
    {
      typedef typename MatrixLike::Scalar Scalar;
      typedef typename MatrixLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
                      const RealScalar & prec =
                      Eigen::NumTraits< Scalar >::dummy_precision())
      {
        PINOCCHIO_UNUSED_VARIABLE(prec);
        return true;
      }
    };
  }

  ///
  /// \brief Check whether the input matrix is Unitary within the given precision.
  ///
  /// \param[in] mat Input matrix
  /// \param[in] prec Required precision
  ///
  /// \returns true if mat is unitary within the precision prec
  ///
  template<typename MatrixLike>
  inline bool isUnitary(const Eigen::MatrixBase<MatrixLike> & mat,
                        const typename MatrixLike::RealScalar & prec =
                        Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
  {
    return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
  }

  namespace internal
  {
    template<typename VectorLike, bool value = boost::is_floating_point<typename VectorLike::Scalar>::value>
    struct isNormalizedAlgo
    {
      typedef typename VectorLike::Scalar Scalar;
      typedef typename VectorLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<VectorLike> & vec,
                      const RealScalar & prec =
                      Eigen::NumTraits<RealScalar>::dummy_precision())
      {
        return math::fabs(vec.norm() - RealScalar(1)) <= prec;
      }
    };

    template<typename VectorLike>
    struct isNormalizedAlgo<VectorLike,false>
    {
      typedef typename VectorLike::Scalar Scalar;
      typedef typename VectorLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<VectorLike> & /*vec*/,
                      const RealScalar & prec =
                      Eigen::NumTraits<RealScalar>::dummy_precision())
      {
        PINOCCHIO_UNUSED_VARIABLE(prec);
        return true;
      }
    };
  }

  ///
  /// \brief Check whether the input vector is Normalized within the given precision.
  ///
  /// \param[in] vec Input vector
  /// \param[in] prec Required precision
  ///
  /// \returns true if vec is normalized within the precision prec.
  ///
  template<typename VectorLike>
  inline bool isNormalized(const Eigen::MatrixBase<VectorLike> & vec,
                           const typename VectorLike::RealScalar & prec =
                           Eigen::NumTraits< typename VectorLike::Scalar >::dummy_precision())
  {
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
    return internal::isNormalizedAlgo<VectorLike>::run(vec,prec);
  }

  namespace internal
  {
    template<typename Scalar>
    struct CallCorrectMatrixInverseAccordingToScalar
    {
      template<typename MatrixIn, typename MatrixOut>
      static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
                      const Eigen::MatrixBase<MatrixOut> & dest)
      {
        MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
        dest_.noalias() = m_in.inverse();
      }
    };

  }

  template<typename MatrixIn, typename MatrixOut>
  inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
                      const Eigen::MatrixBase<MatrixOut> & dest)
  {
    MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
    internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
  }

}

#endif //#ifndef __pinocchio_math_matrix_hpp__


#pragma once
#include <Eigen/Core>
#include "RealType.hxx"
#include "MyTriDiagonalMatrix.hxx"
#include "my_tbb_parallel_for.hxx"
#include "TBBSSESolverOperator.hxx"
#include "TBBSSESolverOperatorBis.hxx"

class SIMDSolve{

  static const int blockSize=4;

  typedef Legolas::MultiVector<RealType,1> V1D;

  typedef Eigen::Array<RealType, blockSize, 1> BlockArray;
  typedef Eigen::Map<BlockArray,Eigen::Aligned>  BlockArrayView;
  typedef Eigen::Map<const BlockArray,Eigen::Aligned>  ConstBlockArrayView;

  typedef Legolas::MultiVector<RealType,2>  V2D;

  V2D  XI_;
  V2D  BI_;
  V2D  SI_;

  V2D  DI_;
  V2D  LI_;
  V2D  UI_;

  Legolas::MultiVector<RealType,1>  S_;

  int s1_,s2_;

public:
  SIMDSolve(const MyDiagonalBlockMatrix & A):XI_(),BI_(),DI_(),LI_(),UI_(),S_(),s1_(),s2_(){

    s1_=A.nrows();
    const MyTriDiagonalMatrix<RealType> & A00=static_cast<const  MyTriDiagonalMatrix<RealType> & >(A.diagonalGetElement(0));
    s2_=A00.nrows();

    Legolas::MultiVector<RealType,2>::Shape  shape(s1_/blockSize,s2_*blockSize);

    XI_.reshape(shape);
    BI_.reshape(shape);
    SI_.reshape(shape);

    DI_.reshape(shape);
    LI_.reshape(shape);
    UI_.reshape(shape);

    S_.reshape(s2_*blockSize);

    for (int i=0 ; i<s1_/blockSize ; i++){
      for (int j=0 ; j<s2_ ; j++){
	for (int k=0 ; k<blockSize ; k++){

	  const int newI=i;
	  const int newJ=blockSize*j+k;
	  const int oldI=blockSize*i+k;
	  const int oldJ=j;

	  const MyTriDiagonalMatrix<RealType> & Aii=static_cast<const  MyTriDiagonalMatrix<RealType> & >(A.diagonalGetElement(oldI));

	  DI_[newI][newJ]=Aii.diagonalGetElement(oldJ);
	  LI_[newI][newJ]=Aii.lowerDiagonalGetElement(oldJ);
	  UI_[newI][newJ]=Aii.upperDiagonalGetElement(oldJ);

	}
      }
    }
  }

  void updateB(const V2D & B){

    for (int i=0 ; i<s1_/blockSize ; i++){
      for (int j=0 ; j<s2_ ; j++){
	for (int k=0 ; k<blockSize ; k++){

	  const int newI=i;
	  const int newJ=blockSize*j+k;
	  const int oldI=blockSize*i+k;
	  const int oldJ=j;

	  BI_[newI][newJ]=B[oldI][oldJ];
	}
      }
    }
  }

  void updateB(RealType value){
    BI_=value;
  }

  void updateX(V2D & X) const {

    for (int i=0 ; i<s1_/blockSize ; i++){
      for (int j=0 ; j<s2_ ; j++){
	for (int k=0 ; k<blockSize ; k++){

	  const int newI=i;
	  const int newJ=blockSize*j+k;
	  const int oldI=blockSize*i+k;
	  const int oldJ=j;

	  X[oldI][oldJ]=XI_[newI][newJ];
	}
      }
    }
  }

  void solve( void ){

    for (int b=0 ; b<s1_/blockSize ; b++){

      const V1D & D=DI_[b];
      const V1D & U=UI_[b];
      const V1D & L=LI_[b];

      const V1D & B=BI_[b];

      V1D & X=XI_[b];

      ConstBlockArrayView D0(&D[0]);
      BlockArray s(D0);

      BlockArrayView X0(&X[0]);
      ConstBlockArrayView B0(&B[0]);

      //	  X0=B0/s;
      X0=B0;
      X0/=s;

      for (int i=1 ; i < s2_ ; i++ ){

	const int I=blockSize*i;

	//S_[i]=A.upperDiagonalGetElement(i-1)/s;
	BlockArrayView Si(&S_[I]);
	Si=ConstBlockArrayView(&U[I-blockSize]);
	Si/=s;

	//	    Si=ConstBlockArrayView(&U[I])/s;

	ConstBlockArrayView Li(&L[I]);

	s=ConstBlockArrayView(&D[I])-Li*Si;

	BlockArrayView Xi(&X[I]);
	ConstBlockArrayView Xim1(&X[I-blockSize]);
	ConstBlockArrayView Bi(&B[I]);
	Xi=Bi;
	Xi-=Li*Xim1;
	Xi/=s;

      }

      for (int i=(s2_-2) ; i >= 0 ; i-- ){

	const int I=blockSize*i;
	//X[i]-=S_[i+1]*X[i+1];
	BlockArrayView Xi(&X[I]);
	ConstBlockArrayView Sip1(&S_[I+blockSize]);
	ConstBlockArrayView Xip1(&X[I+blockSize]);
	Xi-=Sip1*Xip1;
      }
    }

  }

  void parallelSolve( void ){
    my_tbb::parallel_for(my_tbb::blocked_range<int>(0,s1_/blockSize),TBBSSESolverOperator(DI_,LI_,UI_,BI_,XI_));
    //    my_tbb::parallel_for(my_tbb::blocked_range<int>(0,s1_/blockSize),TBBSSESolverOperatorBis(DI_,LI_,UI_,BI_,SI_,XI_));
  }

};


#include <boost/random.hpp>
#include <iostream>

int main()
{
	boost::mt19937 rng1(time(0));
	boost::random::random_number_generator<boost::mt19937, int> rand(rng1);

	for (int i = 0; i < 7; ++i)
	{
		std::cout << rand(30) << std::endl;
	}

	for (int i = 0; i < 7; ++i)
	{
		std::cout << rand(10) << std::endl;
	}
}


#include <stdio.h>
#include <stdlib.h>
#include <GL/glu.h>
#include <GLFW/glfw3.h>
#include <armadillo>
#include <Class_Object.cpp>
#include "Transform.hpp"

void DibujaPlanetas(vector<Vertex> vl);
vector <Vertex>  TranformaPlanetas(vector <Vertex> PlanetaVertices,arma::fmat transf);

int main( void )
{
    arma::frowvec eye = {0.0, 0.0, 10.0};
    arma::frowvec camera = {0.0, 0.0, 0.0};

    GLFWwindow* window;

       Object sol = Object();
    sol.init("roca.obj");

    Object mercurio = Object();
    mercurio.init("roca.obj");

    Object venus = Object();
    venus.init("roca.obj");

    Object tierra = Object();
    tierra.init("roca.obj");

    Object luna = Object();
    luna.init("roca.obj");

    Object marte = Object();
    marte.init("roca.obj");

    Object jupiter = Object();
    jupiter.init("roca.obj");

    Object saturno = Object();
    saturno.init("roca.obj");

    Object urano = Object();
    urano.init("roca.obj");

    Object neptuno = Object();
    neptuno.init("roca.obj");

    if( !glfwInit() )
    {
        fprintf( stderr, "Fallo al inicializar GLFW\n" );
        getchar();
        return -1;
    }

    window = glfwCreateWindow(1024, 768, "Sistema Solar", NULL, NULL);
    if( window == NULL ) {
        fprintf( stderr, "Fallo al abrir la ventana de GLFW.\n" );
        getchar();
        glfwTerminate();
        return -1;
    }
    glfwMakeContextCurrent(window);
    glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);

    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

//  Proyecciones
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    int width, height;
    glfwGetFramebufferSize(window, &width, &height);

    float ar = width / height;

//  Proyección en paralelo
    glViewport(0, 0, width, height);
    glOrtho(-ar, ar, -1.0, 1.0, -20.0, 20.0);

//  Proyección en perspectiva
//    glFrustum(-ar, ar, -ar, ar, 2.0, 4.0);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    Transform Tr = Transform();
	float m_angle = 0.0f;
	float v_angle = 40.0f;
	float t_angle = 80.0f;
	float l_angle = 120.0f;
	float mt_angle = 160.0f;
	float j_angle = 200.0f;
	float s_angle = 240.0f;
	float u_angle = 280.0f;
	float n_angle = 360.0f;

    do {
        glClear( GL_COLOR_BUFFER_BIT  | GL_DEPTH_BUFFER_BIT );

        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(eye[0], eye[1], eye[2],
                camera[0], camera[1], camera[2],
                0.0, 1.0, 0.0);

        // Dibujar el Sol
        arma::fmat transf = Tr.S(0.3, 0.3, 0.3);
        std::vector< Vertex > sol_vertices = TranformaPlanetas(sol.get_faces_verts(),transf);
        glColor3f(1.0, 1.0, 0.0);
        DibujaPlanetas(sol_vertices);
        // Fin dibujado del Sol

	 // Dibujar la Mercurio
	arma::fmat transf_mercurio = Tr.S(0.06, 0.06, 0.06);
        m_angle = (t_angle < 360.0f) ? t_angle+1.0 : 0.0f;

        transf_mercurio = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.20, 0.0, 0.0) * Tr.S(0.06, 0.06, 0.06) * transf;

       std::vector< Vertex > Mercurio_vertices = TranformaPlanetas(venus.get_faces_verts(),transf_mercurio);

        glColor3f(1.0, 0.5, 0.0);
      	 DibujaPlanetas(Mercurio_vertices);
        // Fin dibujado de Mercurio

	// Dibujar la Venus
	arma::fmat transf_Venus = Tr.S(0.08, 0.08, 0.08);
        v_angle = (t_angle < 360.0f) ? t_angle+1.6: 0.0f;

        transf_Venus = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.30, 0.0, 0.0) * Tr.S(0.08, 0.08, 0.08) * transf;

       std::vector< Vertex > venus_vertices = TranformaPlanetas(venus.get_faces_verts(),transf_Venus);

        glColor3f(0.5, 0.5, 0.0);
      	 DibujaPlanetas(venus_vertices);
        // Fin dibujado de venus

	 // Dibujar la Tierra
	arma::fmat transf_Tierra = Tr.S(0.1, 0.1, 0.1);
        t_angle = (t_angle < 360.0f) ? t_angle+2.0 : 0.0f;

        transf_Tierra = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.40, 0.0, 0.0) * Tr.S(0.1, 0.1, 0.1) * transf;

       std::vector< Vertex > tierra_vertices = TranformaPlanetas(tierra.get_faces_verts(),transf_Tierra);

        glColor3f(0.0, 0.0, 1.0);
      	 DibujaPlanetas(tierra_vertices);

        // Fin dibujado de la Tierra

	//Dibuja Luna
	    l_angle = (l_angle < 360.0f) ? l_angle + 12.8 : 0.0f;
           arma::fmat transf_luna = transf_Tierra* Tr.R(0.0, 1.0, 0.0, l_angle) * Tr.T(-0.6, 0.0, 0.0) * Tr.S(0.2, 0.2, 0.2);

        std::vector< Vertex >p_vertices = luna.get_faces_verts();
        std::vector< Vertex > luna_vertices;
        for (unsigned int i = 0; i < p_vertices.size(); i++) {
            arma::fcolvec v = p_vertices[i].getHomg();
            arma::fcolvec vp = transf_luna * v;
            Vertex rv =Vertex();
            rv.set_value2(arma::trans(vp));
            luna_vertices.push_back(rv);
        }
		glColor3f(1.0, 1.0, 1.0);
        glBegin(GL_TRIANGLES);
        for (unsigned int i = 0; i < luna_vertices.size(); i++) {
            arma::frowvec vert = luna_vertices[i].get_value();
            glVertex3f(vert[0], vert[1], vert[2]);
        }
        glEnd();

	//fin dibuja luna

        // Dibujar la Marte
	arma::fmat transf_Marte = Tr.S(0.08, 0.08, 0.08);
        mt_angle = (t_angle < 360.0f) ? t_angle+0.5: 0.0f;

        transf_Marte = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.55, 0.0, 0.0) * Tr.S(0.08, 0.08, 0.08) * transf;

       std::vector< Vertex > marte_vertices = TranformaPlanetas(marte.get_faces_verts(),transf_Marte);

        glColor3f(1.0, 0.0, 0.0);
      	 DibujaPlanetas(marte_vertices);
        // Fin dibujado de la Marte

	 // Dibujar la Jupiter
	arma::fmat transf_Jupiter = Tr.S(0.5, 0.5, 0.5);
        j_angle = (t_angle < 360.0f) ? t_angle+0.082 : 0.0f;

        transf_Jupiter = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.68, 0.0, 0.0) * Tr.S(0.5, 0.5, 0.5) * transf;

       std::vector< Vertex > jupiter_vertices = TranformaPlanetas(jupiter.get_faces_verts(),transf_Jupiter);

        glColor3f(1.0, 1.0, 0.5);
      	 DibujaPlanetas(jupiter_vertices);
        // Fin dibujado de la Jupiter

	 // Dibujar la Saturno
	arma::fmat transf_Saturno = Tr.S(0.30, 0.30, 0.30);
        s_angle = (t_angle < 360.0f) ? t_angle+0.0344 : 0.0f;

        transf_Saturno = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.83, 0.0, 0.0) * Tr.S(0.35, 0.35, 0.35) * transf;

       std::vector< Vertex > Saturno_vertices = TranformaPlanetas(saturno.get_faces_verts(),transf_Saturno);

        glColor3f(0.2, 0.5, 0.2);
      	 DibujaPlanetas(Saturno_vertices);
        // Fin dibujado de la Saturno

	 // Dibujar la Urano
	arma::fmat transf_Urano = Tr.S(0.16, 0.16, 0.16);
        u_angle = (t_angle < 360.0f) ? t_angle+0.02 : 0.0f;

        transf_Urano = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.90, 0.0, 0.0) * Tr.S(0.16, 0.16, 0.16) * transf;

       std::vector< Vertex > Urano_vertices = TranformaPlanetas(urano.get_faces_verts(),transf_Urano);

        glColor3f(1.0, 0.0, 0.5);
      	 DibujaPlanetas(Urano_vertices);
        // Fin dibujado de la urano

	 // Dibujar la Neptuno
	arma::fmat transf_Neptuno = Tr.S(0.16, 0.16, 0.16);
        n_angle = (t_angle < 360.0f) ? t_angle+0.01: 0.0f;

        transf = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.95, 0.0, 0.0) * Tr.S(0.16, 0.16, 0.16) * transf;

       std::vector< Vertex > Neptuno_vertices = TranformaPlanetas(neptuno.get_faces_verts(),transf);

        glColor3f(0.0, 0.0, 0.8);
      	 DibujaPlanetas(Neptuno_vertices);
        // Fin dibujado de la Neptuno

        glfwSwapBuffers(window);
        glfwPollEvents();

    } while( glfwGetKey(window, GLFW_KEY_ESCAPE ) != GLFW_PRESS &&
           glfwWindowShouldClose(window) == 0 );

    glfwTerminate();

    return 0;
}
void DibujaPlanetas(vector <Vertex> vl){
   glBegin(GL_TRIANGLES);
        for ( unsigned int i=0; i<vl.size(); i++ ) {
            arma::frowvec vert = vl[i].get_value();
            glVertex3f(vert[0], vert[1], vert[2]);
        }
        glEnd();

}

	vector <Vertex> TranformaPlanetas(vector <Vertex> PlanetaVertices,arma::fmat transf){

 	std::vector< Vertex >planeta_vertices = PlanetaVertices;
        std::vector< Vertex > planeta_vertices_trans;
        for ( unsigned int i=0; i<planeta_vertices.size(); i++ ) {
            arma::fcolvec v = planeta_vertices[i].getHomg();
            arma::fcolvec vp = transf * v;
            Vertex rv = Vertex();
           //rv.set_value(vp[0],vp[1],vp[2]);
		rv.set_value2(arma::trans(vp));
            planeta_vertices_trans.push_back(rv);
        }

return planeta_vertices_trans;
}


#pragma once
#include "mtao/types.hpp"
#include <Eigen/Sparse>

namespace mtao::geometry {
    //A collection of simplicies, indices into simplices, and related barycentric coordinates
    template <typename SimplexType, typename BType, typename SIType>
        Eigen::SparseMatrix<typename BType::Scalar>
        barycentric_matrix(int num_vertices, const Eigen::MatrixBase<SimplexType>& S, const Eigen::MatrixBase<SIType>& SI, const Eigen::MatrixBase<BType>& B) {
            assert(SI.size() == B.cols());

            using Scalar = typename BType::Scalar;
            Eigen::SparseMatrix<Scalar> A(B.cols(), num_vertices);

            std::vector<Eigen::Triplet<Scalar>> trips;
            trips.reserve(S.rows()*SI.size());

            for(int i = 0; i < B.cols(); ++i)
            {
                auto s = S.col(SI(i));
                auto b = B.col(i);
                for(int j = 0; j < b.rows(); ++j)
                {
                    trips.emplace_back(i,s(j),b(j));
                }
            }
            A.setFromTriplets(trips.begin(),trips.end());
            return A;
        }
    //A collection of simplicies, indices into simplices, and related barycentric coordinates
    //piecewise constant per-face values
    template <typename Scalar, typename SIType>
        Eigen::SparseMatrix<Scalar>
        barycentric_matrix_face(int num_faces, const Eigen::MatrixBase<SIType>& SI) {

            Eigen::SparseMatrix<Scalar> A(SI.rows(), num_faces);

            std::vector<Eigen::Triplet<Scalar>> trips;
            trips.reserve(SI.size());

            for(int i = 0; i < SI.cols(); ++i)
            {
                trips.emplace_back(i,SI(i),1);
            }
            A.setFromTriplets(trips.begin(),trips.end());
            return A;
        }
}


// third party includes
#include <dlib/dnn.h>
#include <dlib/global_optimization.h>
#include <dlib/matrix.h>
#include <dlib/svm.h>
#include <dlib/svm_threaded.h>
#include <plot.h>

// stl includes
#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <memory>
#include <regex>
#include <streambuf>

// application includes
#include "../ioutils.h"
#include "../utils.h"

// Namespace and type aliases
namespace fs = std::experimental::filesystem;
using DType = double;  // DLib internally uses double - so in many cases it is
                       // impossible to use float
using Matrix = dlib::matrix<DType>;
using DataSet = std::pair<std::vector<Matrix>, std::vector<DType>>;
//  SVM types
using svm_kernel_type = dlib::radial_basis_kernel<Matrix>;
using svm_ova_trainer = dlib::one_vs_all_trainer<dlib::any_trainer<Matrix>>;
using svm_dec_funct_type = dlib::one_vs_all_decision_function<svm_ova_trainer>;
// using svm_normalizer_type = dlib::vector_normalizer_pca<Matrix>;
using svm_normalizer_type = dlib::vector_normalizer<Matrix>;
using svm_funct_type =
    dlib::normalized_function<svm_dec_funct_type, svm_normalizer_type>;

static const std::string train_data_url =
    "https://raw.githubusercontent.com/pandas-dev/pandas/master/pandas/tests/"
    "data/iris.csv";

DataSet LoadData() {
  // ----------- Download the data
  const std::string data_path{"iris.csv"};
  if (!fs::exists(data_path)) {
    if (!utils::DownloadFile(train_data_url, data_path)) {
      std::cerr << "Unable to download the file " << train_data_url
                << std::endl;
      return {};
    }
  }
  // ----------- Load data to string
  std::ifstream data_file(data_path);
  std::string train_data_str((std::istreambuf_iterator<char>(data_file)),
                             std::istreambuf_iterator<char>());

  // ----------- Remove first line - columns labels
  train_data_str.erase(0, train_data_str.find_first_of("\n") + 1);

  // ----------- Replace string labels with ints
  train_data_str =
      std::regex_replace(train_data_str, std::regex("Iris-setosa"), "0");
  train_data_str =
      std::regex_replace(train_data_str, std::regex("Iris-versicolor"), "1");
  train_data_str =
      std::regex_replace(train_data_str, std::regex("Iris-virginica"), "2");

  // ----------- Load data to matrix
  Matrix train_data(150, 5);
  std::stringstream ss(train_data_str);
  ss >> train_data;
  // ----------- Extract labels
  Matrix labels = dlib::colm(train_data, 4);
  // std::cout << dlib::csv << labels << std::endl;
  // ----------- Extract and transpose samples
  Matrix samples = dlib::subm_clipped(train_data, 0, 0, train_data.nr(),
                                      train_data.nc() - 1);
  // std::cout << dlib::csv << samples << std::endl;
  // convert to matrices to vectors, because algorithms require such data types
  DataSet ds;
  for (long row = 0; row < samples.nr(); ++row) {
    ds.first.push_back(dlib::reshape_to_column_vector(
        dlib::subm_clipped(samples, row, 0, 1, samples.nc())));
  }
  ds.second.assign(labels.begin(), labels.end());
  return ds;
}

svm_funct_type TrainSVMClassifier(DataSet dataset) {
  // based on http://dlib.net/model_selection_ex.cpp.html

  auto& [samples, labels] = dataset;
  // ----------- Pre-process data
  // Here we normalize all the samples by subtracting their mean and
  // dividing by their standard deviation.
  svm_normalizer_type normalizer;
  // Let the normalizer learn the mean and standard deviation of the samples
  // normalizer.train(samples, 0.9); // configure how much dimensions will be
  // left after PCA
  normalizer.train(samples);
  std::cout << "Dimension of sample vector = " << normalizer.out_vector_size()
            << " after PCA" << std::endl;
  // now normalize each sample
  for (size_t i = 0; i < samples.size(); ++i) {
    samples[i] = normalizer(samples[i]);
  }

  // ----------- Select best parameters for svm model

  //  Here we define a function, that will do the cross-validation and return
  //  a number indicating how good a particular setting of gamma, c1, and c2
  //  is.
  auto cross_validation_score = [&](const DType gamma, const DType c1,
                                    const DType c2) {
    // Make a RBF SVM trainer and tell it what the parameters are supposed to
    // be.
    dlib::svm_c_trainer<svm_kernel_type> svm_trainer;
    svm_trainer.set_kernel(svm_kernel_type(gamma));
    svm_trainer.set_c_class1(c1);
    svm_trainer.set_c_class2(c2);

    svm_ova_trainer trainer;
    trainer.set_num_threads(4);
    trainer.set_trainer(svm_trainer);

    // Perform 10-fold cross validation and then print and return the
    // results - confusion matrix.
    Matrix result =
        dlib::cross_validate_multiclass_trainer(trainer, samples, labels, 10);
    auto accuracy = sum(diag(result)) / sum(result);
    std::cout << "gamma: " << gamma << "  c1: " << c1 << "  c2: " << c2
              << "\ncross validation accuracy: " << accuracy
              << "\nconfusion matrix:\n"
              << dlib::csv << result << std::endl;

    // Now return a number indicating how good the parameters are.  Bigger is
    // better in this example.
    return accuracy;
  };

  // Call this global optimizer that will search for the best
  // parameters. It will call cross_validation_score() 50 times with different
  // settings and return the best parameter setting it finds.
  auto result = dlib::find_max_global(
      cross_validation_score,
      {1e-5, 1e-5,
       1e-5},  // lower bound constraints on gamma, c1, and c2, respectively
      {100, 1e6,
       1e6},  // upper bound constraints on gamma, c1, and c2, respectively
      dlib::max_function_calls(50));

  double best_gamma = result.x(0);
  double best_c1 = result.x(1);
  double best_c2 = result.x(2);

  std::cout << " best cross-validation score: " << result.y << std::endl;
  std::cout << " best gamma: " << best_gamma << "   best c1: " << best_c1
            << "    best c2: " << best_c2 << std::endl;

  // ---------- Create final SVM model

  dlib::svm_c_trainer<svm_kernel_type> svm_trainer;
  svm_trainer.set_kernel(svm_kernel_type(best_gamma));
  svm_trainer.set_c_class1(best_c1);
  svm_trainer.set_c_class2(best_c2);
  svm_ova_trainer trainer;
  trainer.set_num_threads(4);
  trainer.set_trainer(svm_trainer);

  svm_funct_type learned_function;
  learned_function.normalizer = normalizer;  // save normalization information
  learned_function.function = trainer.train(
      samples,
      labels);  // perform the actual SVM training and save the results
  return learned_function;
}

auto TrainNNClassifier(DataSet dataset) {
  // based on http://dlib.net/dnn_introduction_ex.cpp.html
  using namespace dlib;

  auto& [samples, real_labels] = dataset;
  std::vector<unsigned long> labels;
  labels.assign(real_labels.begin(), real_labels.end());
  // ----------- Pre-process data
  // Here we normalize all the samples by subtracting their mean and
  // dividing by their standard deviation.
  vector_normalizer<Matrix> normalizer;
  // let the normalizer learn the mean and standard deviation of the samples
  normalizer.train(samples);
  // now normalize each sample
  for (size_t i = 0; i < samples.size(); ++i) {
    samples[i] = normalizer(samples[i]);
  }

  using net_type = loss_multiclass_log<
      fc<3, relu<fc<10, relu<fc<5, input<matrix<DType>>>>>>>>;
  net_type net;
  dnn_trainer<net_type> trainer(net);
  trainer.set_learning_rate(0.01);
  trainer.set_min_learning_rate(0.00001);
  trainer.set_mini_batch_size(8);
  trainer.be_verbose();
  trainer.train(samples, labels);
  net.clean();
  return std::make_pair(net, normalizer);
}

// ---------- Evaluate model on samples
template <typename Classfier>
void TestSVMClassfier(const std::string& name,
                      const Classfier& classifier,
                      DataSet dataset) {
  auto& [test_data, test_labels] = dataset;
  DType matches_num = 0;
  for (size_t i = 0; i < test_data.size(); ++i) {
    auto predicted_class = classifier(test_data[i]);
    auto true_class = test_labels[i];
    if (predicted_class == true_class) {
      ++matches_num;
    }
  }
  std::cout << name << " test accuracy = " << matches_num / test_data.size()
            << std::endl;
}

template <typename Classfier>
void TestNNClassfier(const std::string& name,
                     Classfier& classifier,
                     DataSet dataset) {
  auto& [test_data, test_labels] = dataset;
  for (auto& ts : test_data)
    ts = classifier.second(ts);  // normalize

  auto predicted_labels = classifier.first(test_data);
  DType matches_num = 0;
  for (size_t i = 0; i < test_data.size(); ++i) {
    auto predicted_class = predicted_labels[i];
    auto true_class = test_labels[i];
    if (predicted_class == true_class) {
      ++matches_num;
    }
  }
  std::cout << name << " test accuracy = " << matches_num / test_data.size()
            << std::endl;
}

int main(int, char* []) {
  using namespace std::string_literals;
  try {
    auto [samples, labels] = LoadData();

    // ----------- Pre-process data
    // It have sense to compile DLib in debug mode with DLIB_ENABLE_ASSERTS
    // CMake option enabled, to see inconvinience in data types

    dlib::randomize_samples(samples, labels);

    // exctract some samples as test data
    std::vector<Matrix> test_data;
    std::ptrdiff_t split_point = 135;
    test_data.assign(samples.begin() + split_point, samples.end());
    samples.erase(samples.begin() + split_point, samples.end());
    std::vector<double> test_labels;
    test_labels.assign(labels.begin() + split_point, labels.end());
    labels.erase(labels.begin() + split_point, labels.end());

    // ----------- SVM
    auto svm_classifier = TrainSVMClassifier({samples, labels});
    TestSVMClassfier("SVM"s, svm_classifier, {test_data, test_labels});

    // Decision trees and Random Forest algorithms are missed in DLib

    // ----------- Neural Net
    auto nn_classifier = TrainNNClassifier({samples, labels});
    TestNNClassfier("NN"s, nn_classifier, {test_data, test_labels});

  } catch (const std::exception& err) {
    std::cout << "Program crashed : " << err.what() << std::endl;
  }
  return 0;
}


#include "problem_generator.h"

#include "macros.h"

#include <iostream>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace std;
using namespace boost;

vector<problem::edge_descriptor> edge_order_by_occurrences(const vector<commodity>& commodities, const problem::graph_type& graph)
{
	int V = num_vertices(graph);
	int K = commodities.size();

	// Occurences of each edge in the shortest path of each commodity
	map<problem::edge_descriptor, int> occurrences;

	LOOP(k, K) {
		// Find the shortest path
		vector<int> parents(V);
		auto index_map = get(vertex_index, graph);

		boost::dijkstra_shortest_paths(graph, commodities[k].origin,
									   predecessor_map(make_iterator_property_map(parents.begin(), index_map)));

		// Count the occurences
		int current = commodities[k].destination;
		while (current != commodities[k].origin) {
			int prev = parents[current];
			auto edge = boost::edge(prev, current, graph).first;

			++occurrences[edge];

			current = prev;
		}
	}

	// Sort the edges
	auto edge_it = edges(graph);
	vector<problem::edge_descriptor> edge_order(edge_it.first, edge_it.second);

	sort(edge_order.begin(), edge_order.end(),
		 [&](auto& a, auto& b) { return occurrences[a] < occurrences[b]; });

	return edge_order;
}

vector<problem::edge_descriptor> edge_symmetric_order_by_occurrences(const vector<commodity>& commodities, const problem::graph_type& graph)
{
	int V = num_vertices(graph);
	int K = commodities.size();

	// Occurences of each edge in the shortest path of each commodity
	map<problem::edge_descriptor, int> occurrences;

	LOOP(k, K) {
		// Find the shortest path
		vector<int> parents(V);
		auto index_map = get(vertex_index, graph);

		boost::dijkstra_shortest_paths(graph, commodities[k].origin,
									   predecessor_map(make_iterator_property_map(parents.begin(), index_map)));

		// Count the occurences
		int current = commodities[k].destination;
		while (current != commodities[k].origin) {
			int prev = parents[current];
			int a = min(prev, current);
			int b = max(prev, current);
			auto edge = boost::edge(a, b, graph).first;

			++occurrences[edge];

			current = prev;
		}
	}

	// Sort the edges
	auto edge_it = edges(graph);
	vector<problem::edge_descriptor> edge_order(edge_it.first, edge_it.second);

	// Remove symmetric edges
	edge_order.erase(remove_if(edge_order.begin(), edge_order.end(),
							   [&](auto& e) {
								   return source(e, graph) > target(e, graph);
							   }),
					 edge_order.end());

	sort(edge_order.begin(), edge_order.end(),
		 [&](auto& a, auto& b) { return occurrences[a] < occurrences[b]; });

	return edge_order;
}

bool is_removable(problem::graph_type& graph, problem::graph_type& toll_free_graph, problem::edge_descriptor& edge_desc, const vector<commodity>& commodities)
{
	int src = source(edge_desc, graph);
	int dst = target(edge_desc, graph);

	// Try to remove edge and test for reachability
	remove_edge(src, dst, toll_free_graph);

	bool ok_to_remove = all_of(commodities.begin(), commodities.end(), [&toll_free_graph](const commodity& c) {
		return is_reachable(c.origin, c.destination, toll_free_graph);
							   });

	// Otherwise, re-add the removed edge
	if (!ok_to_remove) {
		add_edge(src, dst, toll_free_graph);
	}

	return ok_to_remove;
}

bool is_symmetrically_removable(problem::graph_type& graph, problem::graph_type& toll_free_graph, problem::edge_descriptor& edge_desc, const vector<commodity>& commodities)
{
	int src = source(edge_desc, graph);
	int dst = target(edge_desc, graph);

	// Try to remove edge and test for reachability
	remove_edge(src, dst, toll_free_graph);
	remove_edge(dst, src, toll_free_graph);

	bool ok_to_remove = all_of(commodities.begin(), commodities.end(), [&toll_free_graph](const commodity& c) {
		return is_reachable(c.origin, c.destination, toll_free_graph);
							   });

	// Otherwise, re-add the removed edge
	if (!ok_to_remove) {
		add_edge(src, dst, toll_free_graph);
		add_edge(dst, src, toll_free_graph);
	}

	return ok_to_remove;
}

boost::adjacency_list<> grid_graph(int width, int height) {
	using graph_type = boost::adjacency_list<>;

	graph_type graph(width * height);

	for (int i = 0; i < height; ++i) {
		for (int j = 0; j < width; ++j) {
			// Horizontal edges
			if (j + 1 < width) {
				boost::add_edge(i * width + j, i * width + j + 1, graph);
				boost::add_edge(i * width + j + 1, i * width + j, graph);
			}
			// Vertical edges
			if (i + 1 < height) {
				boost::add_edge(i * width + j, (i + 1) * width + j, graph);
				boost::add_edge((i + 1) * width + j, i * width + j, graph);
			}
		}
	}

	return graph;
}

bool pointSortPredicate(const Shx& a, const Shx& b)
{
	if (a.r < b.r)
		return true;
	else if (a.r > b.r)
		return false;
	else if (a.c < b.c)
		return true;
	else
		return false;
};


#include <iostream>

#include <Eigen/Core>

void printRandomMatrix() {
    std::cout << Eigen::MatrixXd::Random(4, 4);
}


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

#include <boost/random.hpp>

#include <Eigen/Dense>

#include "Kernel.h"
#include "GaussianProcess.h"
#include "SparseGaussianProcess.h"

#include "Likelihood.h"
#include "Prior.h"
#include "SparseLikelihood.h"

#include "GaussianProcessInference.h"

// setup sparse Gaussian process
typedef gpr::SparseGaussianProcess<double> SparseGaussianProcessType;
typedef gpr::GaussianProcess<double> GaussianProcessType;
typedef SparseGaussianProcessType::VectorType VectorType;
typedef SparseGaussianProcessType::MatrixType MatrixType;
typedef SparseGaussianProcessType::DiagMatrixType DiagMatrixType;
typedef SparseGaussianProcessType::VectorListType VectorListType;

typedef gpr::GaussianKernel<double>             GaussianKernelType;
typedef std::shared_ptr<GaussianKernelType>     GaussianKernelTypePointer;

// test the efficient inversion resp. determinant
void Test1(){
    std::cout << "Test 1.1 efficient inversion: ... " << std::flush;

    // generate a cool ground truth function
    auto f = [](double x)->double { return (0.5*std::sin(x+10*x) + std::sin(4*x))*x*x; };
    double noise = 0.1;
    double jitter = 0.5;

    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, noise);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    // setup kernel
    double sigma = 0.23;
    double scale = 10;
    GaussianKernelTypePointer gk(new GaussianKernelType(sigma, scale));

    // setup sparse gaussian process
    SparseGaussianProcessType::Pointer sgp(new SparseGaussianProcessType(gk));
    //sgp->DebugOn();
    sgp->SetSigma(noise);
    sgp->SetJitter(jitter);

    // large index set
    unsigned n = 1000;
    double start = -2;
    double stop = 5;
    VectorType Xn = VectorType::Zero(n);
    for(unsigned i=0; i<n; i++){
        Xn[i] = start + i*(stop-start)/n;
    }
    // fill up to sgp
    std::vector<double> dense_y;
    std::vector<double> sparse_y;
    for(unsigned i=0; i<n; i++){
        double v = f(Xn[i])+r();
        dense_y.push_back(v);
        sgp->AddSample(VectorType::Constant(1,Xn[i]), VectorType::Constant(1,v));
    }

    // small index set
    unsigned m = 25;
    VectorType Xm = VectorType::Zero(m);
    for(unsigned i=0; i<m; i++){
        Xm[i] = start + i*(stop-start)/m;
    }
    // fill up to sgp
    for(unsigned i=0; i<m; i++){
        double v = f(Xm[i])+r();
        sparse_y.push_back(v);
        sgp->AddInducingSample(VectorType::Constant(1,Xm[i]), VectorType::Constant(1,v));
    }

    // construct Gaussian log likelihood
    typedef gpr::SparseGaussianLogLikelihood<double> SparseGaussianLogLikelihoodType;
    typedef typename SparseGaussianLogLikelihoodType::Pointer SparseGaussianLogLikelihoodTypePointer;
    SparseGaussianLogLikelihoodTypePointer sgl(new SparseGaussianLogLikelihoodType());

    // get all the important matrices
    MatrixType K;
    MatrixType K_inv;
    MatrixType Knm;
    DiagMatrixType I_sigma;

    sgl->GetCoreMatrices(sgp, K, K_inv, Knm, I_sigma);

    //---------------------------------------------------------------------------
    // inversion test
    MatrixType D;
    sgl->EfficientInversion(sgp, D, I_sigma, K_inv, K, Knm);

    MatrixType T = (MatrixType(I_sigma)+Knm*K_inv*Knm.adjoint()).inverse();
    double inv_error = (D-T).norm();

    if(inv_error < 1e-7){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<inv_error; throw ss.str();
    }

    std::cout << "Test 1.2 efficient determinant: ... " << std::flush;
    //---------------------------------------------------------------------------
    // determinant test
    double determinant = sgl->EfficientDeterminant(I_sigma, K_inv, K, Knm);
    double det_err = std::fabs(determinant-(MatrixType(I_sigma)+Knm*K_inv*Knm.adjoint()).determinant());

    if(det_err < 1e-10){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<det_err; throw ss.str();
    }

}

void Test2(double jitter){
    std::cout << "Test 2.1 core matrix test (jitter=" << jitter << "): ... " << std::flush;

    // generate a cool ground truth function
    auto f = [](double x)->double { return (0.5*std::sin(x+10*x) + std::sin(4*x))*x*x; };
    double noise = 0.01;

    // setup kernel
    double sigma = 0.23;
    double scale = 10;
    GaussianKernelTypePointer gk(new GaussianKernelType(sigma, scale));

    // setup sparse gaussian process
    SparseGaussianProcessType::Pointer sgp(new SparseGaussianProcessType(gk));
    //sgp->DebugOn();
    sgp->SetSigma(noise);
    sgp->SetJitter(jitter);

    // large index set
    unsigned n = 10;
    double start = -2;
    double stop = 5;
    VectorType Xn = VectorType::Zero(n);
    for(unsigned i=0; i<n; i++){
        Xn[i] = start + i*(stop-start)/n;
    }
    // fill up to sgp
    std::vector<double> dense_y;
    std::vector<double> sparse_y;
    for(unsigned i=0; i<n; i++){
        double v = f(Xn[i]);
        dense_y.push_back(v);
        sgp->AddSample(VectorType::Constant(1,Xn[i]), VectorType::Constant(1,v));
    }

    // small index set
    unsigned m = 10;
    VectorType Xm = VectorType::Zero(m);
    for(unsigned i=0; i<m; i++){
        Xm[i] = start + i*(stop-start)/m;
    }
    // fill up to sgp
    for(unsigned i=0; i<m; i++){
        double v = f(Xm[i]);
        sparse_y.push_back(v);
        sgp->AddInducingSample(VectorType::Constant(1,Xm[i]), VectorType::Constant(1,v));
    }

    // construct Gaussian log likelihood
    typedef gpr::SparseGaussianLogLikelihood<double> SparseGaussianLogLikelihoodType;
    typedef typename SparseGaussianLogLikelihoodType::Pointer SparseGaussianLogLikelihoodTypePointer;
    SparseGaussianLogLikelihoodTypePointer sgl(new SparseGaussianLogLikelihoodType());

    // get all the important matrices
    MatrixType K;
    MatrixType K_inv;
    MatrixType Kmn;
    DiagMatrixType I_sigma;

    sgl->GetCoreMatrices(sgp, K, K_inv, Kmn, I_sigma);

    //---------------------------------------------------------------------------
    // core matrix test
    MatrixType C; //
    sgl->GetCoreMatrix(sgp, C, K_inv, Kmn);

    MatrixType M;
    sgp->ComputeDenseKernelMatrix(M);

    double err = (C-M).norm();

    if((err < 1e-2 && jitter > 0) || (err < 2000 && jitter == 0)){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err; throw ss.str();
    }

    std::cout << "Test 2.2 core matrix trace test (jitter=" << jitter << "): ... " << std::flush;
    if(M.trace() == sgl->GetKernelMatrixTrace(sgp)){
        std::cout << "[passed]" << std::endl;
    }
    else{
        throw std::string("error in calculating kernel matrix trace.");
    }
}

void Test3(){
    std::cout << "Test 3: sparse likelihood gradient test... " << std::flush;

    // generate a cool ground truth function
    auto f = [](double x)->double { return (0.5*std::sin(x+10*x) + std::sin(4*x))*x*x; };
    double noise = 0.2;
    double jitter = 0.01;

    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, noise);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    double h = 0.0001;
    bool passed = true;

    for(unsigned iter=0; iter<10; iter++){

        // setup kernel
        double scale = 10;
        double sigma = 0.1 + iter*0.02;

        GaussianKernelTypePointer gk(new GaussianKernelType(sigma, scale));

        // setup sparse gaussian process
        SparseGaussianProcessType::Pointer sgp(new SparseGaussianProcessType(gk));
        //sgp->DebugOn();
        sgp->SetSigma(noise);
        sgp->SetJitter(jitter);

        // large index set
        //unsigned n = 100;
        unsigned n = 50 + iter;
        double start = -2;
        double stop = 5;
        VectorType Xn = VectorType::Zero(n);
        for(unsigned i=0; i<n; i++){
            Xn[i] = start + i*(stop-start)/n;
        }
        // fill up to sgp
        std::vector<double> dense_y;
        std::vector<double> sparse_y;
        for(unsigned i=0; i<n; i++){
            double v = f(Xn[i])+r();
            dense_y.push_back(v);
            sgp->AddSample(VectorType::Constant(1,Xn[i]), VectorType::Constant(1,v));
        }

        // small index set
        //unsigned m = 20;
        unsigned m = 5 + iter;
        VectorType Xm = VectorType::Zero(m);
        for(unsigned i=0; i<m; i++){
            Xm[i] = start + i*(stop-start)/m;
        }
        // fill up to sgp
        for(unsigned i=0; i<m; i++){
            double v = f(Xm[i])+r();
            sparse_y.push_back(v);
            sgp->AddInducingSample(VectorType::Constant(1,Xm[i]), VectorType::Constant(1,v));
        }

        // construct Gaussian log likelihood
        typedef gpr::SparseGaussianLogLikelihood<double> SparseGaussianLogLikelihoodType;
        typedef typename SparseGaussianLogLikelihoodType::Pointer SparseGaussianLogLikelihoodTypePointer;
        SparseGaussianLogLikelihoodTypePointer sgl(new SparseGaussianLogLikelihoodType());

        //sgl->DebugOn();

        VectorType D = sgl->GetParameterDerivatives(sgp);

        {
            // central differences sigma
            sgp->SetKernel(GaussianKernelTypePointer(new GaussianKernelType(sigma+h/2, scale)));
            double plus = (*sgl)(sgp)[0];
            sgp->SetKernel(GaussianKernelTypePointer(new GaussianKernelType(sigma-h/2, scale)));
            double minus = (*sgl)(sgp)[0];

            double d = (plus - minus)/h;

            if(std::fabs(D[0] - d) > 1){
                passed = false;
                std::cout <<  "central difference (sigma): " << d << ", but is " << D[0] <<   std::endl;
            }
        }
        {
            // central differences scale
            sgp->SetKernel(GaussianKernelTypePointer(new GaussianKernelType(sigma, scale+h/2)));
            double plus = (*sgl)(sgp)[0];
            sgp->SetKernel(GaussianKernelTypePointer(new GaussianKernelType(sigma, scale-h/2)));
            double minus = (*sgl)(sgp)[0];

            double d = (plus - minus)/h;

            if(std::fabs(D[1] - d) > 0.1){
                passed = false;
                std::cout <<  "central difference (scale): " << d << ", but is " << D[1] <<   std::endl;
            }
        }
    }

    if(passed){
        std::cout << "[passed]" << std::endl;
    }
    else{
        throw std::string("errors in central differences calculation");
    }

    return;
}

void Test4(){
    //std::cout << "Test 4: sparse maximum gaussian log likelihood with gradient descent test ..." << std::flush;
    std::cout.precision(8);

    typedef gpr::GaussianExpKernel<double>           GaussianExpKernelType;
    typedef GaussianExpKernelType::Pointer           GaussianExpKernelTypePointer;
    typedef gpr::GaussianKernel<double>              GaussianKernelType;
    typedef GaussianKernelType::Pointer              GaussianKernelTypePointer;
    typedef gpr::SparseGaussianLogLikelihood<double>       LikelihoodType;
    typedef LikelihoodType::Pointer                  LikelihoodTypePointer;
    typedef gpr::GaussianProcessInference<double>    GaussianProcessInferenceType;
    typedef GaussianProcessInferenceType::Pointer    GaussianProcessInferenceTypePointer;

    typedef gpr::GaussianProcess<double> GaussianProcessType;
    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;
    typedef GaussianProcessType::DiagMatrixType DiagMatrixType;
    typedef GaussianProcessType::VectorListType VectorListType;

    // global parameters
    unsigned n = 300;
    unsigned m = 50;
    double noise = 0.1;

    // construct training data
    auto f = [](double x)->double { return (0.5*std::sin(x+10*x) + std::sin(4*x))*x*x; };

    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, noise);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    double start = -5;
    double stop = 10;
    VectorType Xn = VectorType::Zero(n);
    VectorType Yn = VectorType::Zero(n);
    for(unsigned i=0; i<n; i++){
        Xn[i] = start + i*(stop-start)/n;
        Yn[i] = f(Xn[i])+r();
    }

    GaussianExpKernelTypePointer gk(new GaussianExpKernelType(1, 1));
    SparseGaussianProcessType::Pointer gp(new SparseGaussianProcessType(gk, 0.11));
    //gp->DebugOn();
    gp->SetSigma(noise);
    for(unsigned i=0; i<n; i++){
        gp->AddSample(VectorType::Constant(1,Xn[i]), VectorType::Constant(1,Yn[i]));
    }

    std::vector<unsigned> indices;
    for(unsigned i=0; i<n; i++){
        indices.push_back(i);
    }
    std::random_shuffle(indices.begin(), indices.end());

    for(unsigned i=0; i<m; i++){
        gp->AddInducingSample(VectorType::Constant(1,Xn[indices[i]]), VectorType::Constant(1,Yn[indices[i]]));
    }

    // setup likelihood
    double step = 1e-1;
    unsigned iterations = 100;

    LikelihoodTypePointer lh(new LikelihoodType());
    GaussianProcessInferenceTypePointer gpi(new GaussianProcessInferenceType(lh, gp, step, iterations));

    bool exp_output = true;
    gpi->Optimize(true, exp_output);

    std::cout << "print \"Parameters are: ";
    GaussianProcessInferenceType::ParameterVectorType parameters = gpi->GetParameters();
    for(unsigned i=0; i<parameters.size(); i++){
        parameters[i] = std::exp(parameters[i]);
        std::cout << parameters[i] << ", ";
    }
    std::cout << "\"" << std::endl;

    GaussianKernelTypePointer k(new GaussianKernelType(1,1));
    k->SetParameters(parameters);
    gp->SetKernel(k);

    // evaluate error
    double error = 0;
    unsigned gt_n = 1000;
    for(unsigned i=0; i<gt_n; i++){
        double x = start + i*(stop-start)/gt_n;
        double p = gp->Predict(VectorType::Constant(1,x))[0];
        error += std::fabs(p-f(x));
    }

//    if(error/gt_n > 2){
//        std::cout << "[failed] with an avg error of " << error/gt_n << std::endl;
//    }
//    else{
//        std::cout << "[passed]" << std::endl;
//    }
return;

    std::cout << "import numpy as np" << std::endl;
    std::cout << "import pylab as plt" << std::endl;

    std::cout << "x = np.array([" << std::endl;
    for(unsigned i=0; i<gt_n; i++){
        std::cout << start + i*(stop-start)/gt_n << ", ";
    }
    std::cout << "])" << std::endl;

    std::cout << "y = np.array([" << std::endl;
    for(unsigned i=0; i<gt_n; i++){
        std::cout << f(start + i*(stop-start)/gt_n) << ", ";
    }
    std::cout << "])" << std::endl;
    std::cout << "plt.plot(x,y)" << std::endl;

    std::cout << "xp = np.array([" << std::endl;
    for(unsigned i=0; i<m; i++){
        std::cout << Xn[indices[i]] << ", ";
    }
    std::cout << "])" << std::endl;

    std::cout << "yp = np.array([" << std::endl;
    for(unsigned i=0; i<m; i++){
        std::cout << Yn[indices[i]] << ", ";
    }
    std::cout << "])" << std::endl;
    std::cout << "plt.plot(xp,yp, '.k')" << std::endl;

    std::cout << "Y = np.array([" << std::endl;
    for(unsigned i=0; i<gt_n; i++){
        std::cout << gp->Predict(VectorType::Constant(1,(start + i*(stop-start)/gt_n)))[0] << ", ";
    }
    std::cout << "])" << std::endl;
    std::cout << "plt.plot(x,Y, '-r')" << std::endl;

    std::cout << "plt.show()" << std::endl;
}

int main (int argc, char *argv[]){
    //std::cout << "Sparse Gaussian Process test: " << std::endl;
    try{
//        Test1();
//        Test2(0); // jitter 0
//        Test2(0.001); // jitter 0.001
//        Test3();
        Test4();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


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

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/hypot.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/sqrt.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
template <typename T>
constexpr void test()
{
    // Error Handling
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::isnan(boost::math::ccmath::hypot(std::numeric_limits<T>::quiet_NaN(), T(1))), "If x is NaN, NaN is returned");
        static_assert(boost::math::ccmath::isnan(boost::math::ccmath::hypot(T(1), std::numeric_limits<T>::quiet_NaN())), "If y is NaN, NaN is returned");
    }

    static_assert(boost::math::ccmath::isinf(boost::math::ccmath::hypot(std::numeric_limits<T>::infinity(), T(1))));
    static_assert(boost::math::ccmath::isinf(boost::math::ccmath::hypot(-std::numeric_limits<T>::infinity(), T(1))));
    static_assert(boost::math::ccmath::isinf(boost::math::ccmath::hypot(T(1), std::numeric_limits<T>::infinity())));
    static_assert(boost::math::ccmath::isinf(boost::math::ccmath::hypot(T(1), -std::numeric_limits<T>::infinity())));

    // Correct promoted types
    if constexpr (!std::is_same_v<T, float>)
    {
        constexpr auto test_type = boost::math::ccmath::hypot(T(1), 1.0f);
        static_assert(std::is_same_v<T, std::remove_cv_t<decltype(test_type)>>);
    }
    else
    {
        constexpr auto test_type = boost::math::ccmath::hypot(1.0f, 1);
        static_assert(std::is_same_v<double, std::remove_cv_t<decltype(test_type)>>);
    }

    // Functionality
    static_assert(boost::math::ccmath::hypot(T(1), T(1)) == boost::math::ccmath::sqrt(T(2)));
    static_assert(boost::math::ccmath::hypot(T(-1), T(1)) == boost::math::ccmath::sqrt(T(2)));
    static_assert(boost::math::ccmath::hypot(T(-1), T(-1)) == boost::math::ccmath::sqrt(T(2)));
    static_assert(boost::math::ccmath::hypot(T(1), T(-1)) == boost::math::ccmath::sqrt(T(2)));
    static_assert(boost::math::ccmath::hypot(T(1), T(2)) == boost::math::ccmath::sqrt(T(5)));
    static_assert(boost::math::ccmath::hypot(T(2), T(2)) == boost::math::ccmath::sqrt(T(8)));
}

int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #ifdef BOOST_HAS_FLOAT128
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


// Boost.Geometry

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

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

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

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

#ifndef BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP
#define BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_direct.hpp>

#ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
#endif

namespace boost { namespace geometry { namespace formula
{

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

*/
template <
    typename CT,
    bool EnableCoordinates = true,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
class vincenty_direct
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_direct<CT> result_type;

    template <typename T, typename Dist, typename Azi, typename Spheroid>
    static inline result_type apply(T const& lo1,
                                    T const& la1,
                                    Dist const& distance,
                                    Azi const& azimuth12,
                                    Spheroid const& spheroid)
    {
        result_type result;

        CT const lon1 = lo1;
        CT const lat1 = la1;

        if ( math::equals(distance, Dist(0)) || distance < Dist(0) )
        {
            result.lon2 = lon1;
            result.lat2 = lat1;
            return result;
        }

        CT const radius_a = CT(get_radius<0>(spheroid));
        CT const radius_b = CT(get_radius<2>(spheroid));
        CT const flattening = formula::flattening<CT>(spheroid);

        CT const sin_azimuth12 = sin(azimuth12);
        CT const cos_azimuth12 = cos(azimuth12);

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        CT const one_min_f = CT(1) - flattening;
        CT const tan_U1 = one_min_f * tan(lat1);
        CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1)

        // may be calculated from tan using 1 sqrt()
        CT const U1 = atan(tan_U1);
        CT const sin_U1 = sin(U1);
        CT const cos_U1 = cos(U1);

        CT const sin_alpha = cos_U1 * sin_azimuth12; // (2)
        CT const sin_alpha_sqr = math::sqr(sin_alpha);
        CT const cos_alpha_sqr = CT(1) - sin_alpha_sqr;

        CT const b_sqr = radius_b * radius_b;
        CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr;
        CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3)
        CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4)

        CT s_div_bA = distance / (radius_b * A);
        CT sigma = s_div_bA; // (7)

        CT previous_sigma;
        CT sin_sigma;
        CT cos_sigma;
        CT cos_2sigma_m;
        CT cos_2sigma_m_sqr;

        int counter = 0; // robustness

        do
        {
            previous_sigma = sigma;

            CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5)

            sin_sigma = sin(sigma);
            cos_sigma = cos(sigma);
            CT const sin_sigma_sqr = math::sqr(sin_sigma);
            cos_2sigma_m = cos(two_sigma_m);
            cos_2sigma_m_sqr = math::sqr(cos_2sigma_m);

            CT const delta_sigma = B * sin_sigma * (cos_2sigma_m
                                        + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr)
                                            - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6)

            sigma = s_div_bA + delta_sigma; // (7)

            ++counter; // robustness

        } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12)
               //&& geometry::math::abs(sigma) < pi
               && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness

        if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
        {
            result.lat2
                = atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12,
                         one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8)

            CT const lambda = atan2( sin_sigma * sin_azimuth12,
                                     cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_azimuth12); // (9)
            CT const C = (flattening/CT(16)) * cos_alpha_sqr * ( CT(4) + flattening * ( CT(4) - CT(3) * cos_alpha_sqr ) ); // (10)
            CT const L = lambda - (CT(1) - C) * flattening * sin_alpha
                            * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( CT(-1) + CT(2) * cos_2sigma_m_sqr ) ) ); // (11)

            result.lon2 = lon1 + L;
        }

        if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
        {
            result.reverse_azimuth
                = atan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_azimuth12); // (12)
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, result.lon2, result.lat2,
                              azimuth12, result.reverse_azimuth,
                              radius_b, flattening,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }

};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP