#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Simple_cartesian.h>

// Graphs
#include <CGAL/Polyhedron_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Regular_triangulation_2.h>
#include <CGAL/boost/graph/properties_Regular_triangulation_2.h>

#include <CGAL/Polygon_mesh_processing/locate.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Bbox_3.h>
#include <CGAL/boost/graph/io.h>
#include <CGAL/boost/graph/named_params_helper.h>
#include <CGAL/boost/graph/generators.h>
#include <CGAL/boost/graph/helpers.h>
#include <CGAL/Dimension.h>
#include <CGAL/Kernel_traits.h>
#include <CGAL/Origin.h>
#include <CGAL/property_map.h>
#include <CGAL/Random.h>
#include <CGAL/Unique_hash_map.h>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/optional.hpp>

#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <set>
#include <type_traits>
#include <utility>
#include <vector>

namespace PMP = CGAL::Polygon_mesh_processing;

typedef CGAL::Exact_predicates_inexact_constructions_kernel    EPICK;
typedef CGAL::Exact_predicates_exact_constructions_kernel      EPECK;

typedef CGAL::Simple_cartesian<typename CGAL::internal::Exact_field_selector<double>::Type> Exact_kernel;

template<typename AABB_tree>
typename CGAL::Kernel_traits<typename AABB_tree::AABB_traits::Point_3>::type::Ray_2
random_2D_ray(const AABB_tree& aabb_tree, CGAL::Random& rnd)
{
  typedef typename AABB_tree::AABB_traits::Point_3           Point_3;
  typedef typename CGAL::Kernel_traits<Point_3>::type        Kernel;
  typedef typename Kernel::FT                                FT;
  typedef typename Kernel::Point_2                           Point_2;
  typedef typename Kernel::Ray_2                             Ray_2;

  const CGAL::Bbox_3& bbox = aabb_tree.bbox();

  FT px = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT py = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());

  FT qx = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT qy = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());

  return Ray_2(Point_2(px, py), Point_2(qx, qy));
}

template<typename AABB_tree>
typename CGAL::Kernel_traits<typename AABB_tree::AABB_traits::Point_3>::type::Ray_3
random_3D_ray(const AABB_tree& aabb_tree, CGAL::Random& rnd)
{
  typedef typename AABB_tree::AABB_traits::Point_3           Point_3;
  typedef typename CGAL::Kernel_traits<Point_3>::type        Kernel;
  typedef typename Kernel::FT                                FT;
  typedef typename Kernel::Ray_3                             Ray_3;

  const CGAL::Bbox_3& bbox = aabb_tree.bbox();

  FT px = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT py = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());
  FT pz = (bbox.zmin() == bbox.zmax()) ? bbox.zmin() : rnd.get_double(bbox.zmin(), bbox.zmax());

  FT qx = (bbox.xmin() == bbox.xmax()) ? bbox.xmin() : rnd.get_double(bbox.xmin(), bbox.xmax());
  FT qy = (bbox.ymin() == bbox.ymax()) ? bbox.ymin() : rnd.get_double(bbox.ymin(), bbox.ymax());
  FT qz = (bbox.zmin() == bbox.zmax()) ? bbox.zmin() : rnd.get_double(bbox.zmin(), bbox.zmax());

  return Ray_3(Point_3(px, py, pz), Point_3(qx, qy, qz));
}

template<typename FT>
bool is_equal(const FT& a, const FT& b)
{
  if(std::is_floating_point<FT>::value)
    return (CGAL::abs(a - b) <= 1e-7); // numeric_limits' epsilon is too restrictive...
  else
    return (a == b);
}

template<typename K, typename G>
void test_snappers(const G& g)
{
  std::cout << "  test snappers..." << std::endl;

  typedef typename K::FT                                              FT;

  PMP::Barycentric_coordinates<FT> coords = CGAL::make_array(FT(1e-6), FT(0.9999999999999999999), FT(1e-7));
  PMP::Face_location<G, FT> loc = std::make_pair(*(faces(g).first), coords);

  // ---------------------------------------------------------------------------
  PMP::internal::snap_coordinates_to_border<FT>(coords); // uses numeric_limits' epsilon()
  assert(coords[0] == FT(1e-6) && coords[1] == FT(1) && coords[2] == FT(1e-7));

  PMP::internal::snap_coordinates_to_border(coords, FT(1e-5));
  assert(coords[0] == FT(0) && coords[1] == FT(1) && coords[2] == FT(0));

  // ---------------------------------------------------------------------------
  PMP::internal::snap_location_to_border<FT>(loc, g); // uses numeric_limits' epsilon()
  assert(!PMP::is_on_face_border(loc, g));

  PMP::internal::snap_location_to_border(loc, g, FT(1e-7));
  assert(PMP::is_on_face_border(loc, g));
}

template <typename K, int>
struct Point_to_bare_point
{
  typedef typename K::Point_2 type;
};

template <typename K>
struct Point_to_bare_point<K, 3>
{
  typedef typename K::Point_3 type;
};

template<typename K, typename G, typename VPM>
void test_constructions(const G& g,
                        const VPM vpm,
                        CGAL::Random& rnd)
{
  std::cout << "  test constructions..." << std::endl;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;
  typedef typename PMP::descriptor_variant<G>                                descriptor_variant;

  typedef typename boost::property_traits<VPM>::value_type                        Point;
  typedef typename boost::property_traits<VPM>::reference                         Point_reference;
  typedef typename K::FT                                                          FT;
  typedef typename Point_to_bare_point<K, Point::Ambient_dimension::value>::type  Bare_point;

  typedef typename PMP::Barycentric_coordinates<FT>                          Barycentric_coordinates;
  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  halfedge_descriptor h = halfedge(f, g);
  vertex_descriptor v = source(h, g);

  Point_reference p = get(vpm, v);
  Point_reference q = get(vpm, target(h, g));
  Point_reference r = get(vpm, target(next(h, g), g));

  const Bare_point bp(p);
  const Bare_point bq(q);
  const Bare_point br(r);

  Barycentric_coordinates bar;
  Face_location loc;
  loc.first = f;

  // ---------------------------------------------------------------------------
  bar = PMP::barycentric_coordinates(p, q, r, p, K());
  assert(is_equal(bar[0], FT(1)) && is_equal(bar[1], FT(0)) && is_equal(bar[2], FT(0)));
  bar = PMP::barycentric_coordinates(p, q, r, q, K());
  assert(is_equal(bar[0], FT(0)) && is_equal(bar[1], FT(1)) && is_equal(bar[2], FT(0)));
  bar = PMP::barycentric_coordinates(p, q, r, r, K());
  assert(is_equal(bar[0], FT(0)) && is_equal(bar[1], FT(0)) && is_equal(bar[2], FT(1)));

  Point mp = Point(CGAL::midpoint(bp, bq));
  bar = PMP::barycentric_coordinates(p, q, r, mp);
  assert(is_equal(bar[0], FT(0.5)) && is_equal(bar[1], FT(0.5)) && is_equal(bar[2], FT(0)));

  int n = 100;
  while(n --> 0) // :)
  {
    const FT a = rnd.get_double(-1., 1.);
    const FT b = rnd.get_double(-1., 1.);
    const FT c = 1. - a - b;

    // Point to location and inversely
    Bare_point barycentric_pt = CGAL::barycenter(bp, a, bq, b, br, c);
    bar = PMP::barycentric_coordinates(p, q, r, Point(barycentric_pt));
    assert(is_equal(bar[0], a) && is_equal(bar[1], b) && is_equal(bar[2], c));

    loc.second = bar;
    const Bare_point barycentric_pt_2 =
      Bare_point(PMP::construct_point(loc, g,
                                      CGAL::parameters::vertex_point_map(vpm)
                                                       .geom_traits(K())));

    const FT sq_dist = CGAL::squared_distance(barycentric_pt, barycentric_pt_2);
    assert(is_equal(sq_dist, FT(0)));
  }

  // ---------------------------------------------------------------------------
  loc = std::make_pair(f, CGAL::make_array(FT(0.3), FT(0.4), FT(0.3)));
  descriptor_variant dv = PMP::get_descriptor_from_location(loc, g);
  const face_descriptor* fd = boost::get<face_descriptor>(&dv);
  assert(fd);

  loc = std::make_pair(f, CGAL::make_array(FT(0.5), FT(0.5), FT(0)));
  dv = PMP::get_descriptor_from_location(loc, g);
  const halfedge_descriptor* hd = boost::get<halfedge_descriptor>(&dv);
  assert(hd);

  loc = std::make_pair(f, CGAL::make_array(FT(1), FT(0), FT(0)));
  assert(PMP::is_on_vertex(loc, source(halfedge(f, g), g), g));
  dv = PMP::get_descriptor_from_location(loc, g);
  if(const vertex_descriptor* v = boost::get<vertex_descriptor>(&dv)) { } else { assert(false); }

  // ---------------------------------------------------------------------------
  // just to check the API
  PMP::construct_point(loc, g);
  PMP::construct_point(loc, g, CGAL::parameters::default_values());
}

template<typename K, typename G>
void test_random_entities(const G& g, CGAL::Random& rnd)
{
  std::cout << "  test random entities..." << std::endl;

  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename K::FT                                                     FT;
  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  // ---------------------------------------------------------------------------
  Face_location loc;

  halfedge_descriptor h = CGAL::internal::random_halfedge_in_mesh(g, rnd);
  if(is_border(h, g))
    h = opposite(h, g);
  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);

  int nn = 100;
  while(nn --> 0) // the infamous 'go to zero' operator
  {
    loc = PMP::random_location_on_mesh<FT>(g, rnd);
    assert(loc.first != boost::graph_traits<G>::null_face());
    assert(loc.second[0] >= FT(0) && loc.second[0] <= FT(1) &&
           loc.second[1] >= FT(0) && loc.second[1] <= FT(1) &&
           loc.second[2] >= FT(0) && loc.second[2] <= FT(1));

    loc = PMP::random_location_on_face<FT>(f, g, rnd);
    assert(loc.first == f);
    assert(loc.second[0] >= FT(0) && loc.second[0] <= FT(1) &&
           loc.second[1] >= FT(0) && loc.second[1] <= FT(1) &&
           loc.second[2] >= FT(0) && loc.second[2] <= FT(1));

    loc = PMP::random_location_on_halfedge<FT>(h, g, rnd);
    assert(loc.first == face(h, g));
    assert(loc.second[0] >= FT(0) && loc.second[0] <= FT(1) &&
           loc.second[1] >= FT(0) && loc.second[1] <= FT(1) &&
           loc.second[2] >= FT(0) && loc.second[2] <= FT(1));
    int h_id = CGAL::halfedge_index_in_face(h, g);
    assert(loc.second[(h_id+2)%3] == FT(0));
  }
}

template<typename K, typename G>
void test_helpers(const G& g, CGAL::Random& rnd)
{
  std::cout << "  test helpers..." << std::endl;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename K::FT                                                     FT;
  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  halfedge_descriptor h = halfedge(f, g);
  vertex_descriptor v = source(h, g);

  // ---------------------------------------------------------------------------
  // Local index
  int pos = CGAL::vertex_index_in_face(v, f, g);
  assert(pos == 0);
  pos = CGAL::vertex_index_in_face(target(h, g), f, g);
  assert(pos == 1);
  pos = CGAL::vertex_index_in_face(target(next(h, g), g), f, g);
  assert(pos == 2);

  pos = CGAL::halfedge_index_in_face(h, g);
  assert(pos == 0);
  pos = CGAL::halfedge_index_in_face(next(h, g), g);
  assert(pos == 1);
  pos = CGAL::halfedge_index_in_face(prev(h, g), g);
  assert(pos == 2);

  // ---------------------------------------------------------------------------
  // Incident faces
  Face_location loc = PMP::random_location_on_face<FT>(f, g, rnd);
  std::set<face_descriptor> s;
  PMP::internal::incident_faces(loc, g, std::inserter(s, s.begin()));
  assert(PMP::is_on_face_border(loc, g) || s.size() == 1);

  loc = PMP::random_location_on_halfedge<FT>(h, g, rnd);
  std::vector<face_descriptor> vec;
  PMP::internal::incident_faces(loc, g, std::back_inserter(vec));
  assert(PMP::is_on_vertex(loc, source(h, g), g) ||
         PMP::is_on_vertex(loc, target(h, g), g) ||
         vec.size() == 2);
}

template<typename K, typename G>
void test_predicates(const G& g, CGAL::Random& rnd)
{
  std::cout << "  test predicates..." << std::endl;

  typedef typename K::FT                                                     FT;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  halfedge_descriptor h = halfedge(f, g);
  vertex_descriptor v = source(h, g);

  // ---------------------------------------------------------------------------
  Face_location loc(f, CGAL::make_array(FT(1), FT(0), FT(0)));
  assert(PMP::is_on_vertex<FT>(loc, v, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(1), FT(0)));
  assert(PMP::is_on_vertex<FT>(loc, target(h, g), g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0), FT(1)));
  assert(PMP::is_on_vertex<FT>(loc, target(next(h, g), g), g));
  loc = Face_location(f, CGAL::make_array(FT(-1), FT(1), FT(1)));
  assert(!PMP::is_on_vertex<FT>(loc, target(next(h, g), g), g));

  // ---------------------------------------------------------------------------
  loc = Face_location(f, CGAL::make_array(FT(0.5), FT(0.5), FT(0)));
  assert(PMP::is_on_halfedge<FT>(loc, h, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0.5), FT(0.5)));
  assert(PMP::is_on_halfedge<FT>(loc, next(h, g), g));
  loc = Face_location(f, CGAL::make_array(FT(-0.5), FT(1.5), FT(0)));
  assert(!PMP::is_on_halfedge<FT>(loc, h, g));
  loc = Face_location(f, CGAL::make_array(FT(0.1), FT(-0.6), FT(1.5)));
  assert(!PMP::is_on_halfedge<FT>(loc, h, g));

  // ---------------------------------------------------------------------------
  loc = Face_location(f, CGAL::make_array(FT(0.3), FT(0.3), FT(0.4)));
  assert(PMP::is_in_face<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0), FT(1)));
  assert(PMP::is_in_face<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(2), FT(-1)));
  assert(!PMP::is_in_face<FT>(loc, g));

  // ---------------------------------------------------------------------------
  loc = Face_location(f, CGAL::make_array(FT(0.3), FT(0.3), FT(0.4)));
  assert(!PMP::is_on_face_border<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0.6), FT(0.4)));
  assert(PMP::is_on_face_border<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(0), FT(0), FT(1)));
  assert(PMP::is_on_face_border<FT>(loc, g));
  loc = Face_location(f, CGAL::make_array(FT(-0.2), FT(0), FT(1.2)));
  assert(!PMP::is_on_face_border(loc, g));

  // ---------------------------------------------------------------------------
  int max = 1000, counter = 0;
  typename boost::graph_traits<G>::halfedge_iterator hit, hend;
  boost::tie(hit, hend) = halfedges(g);
  for(; hit!=hend; ++hit)
  {
    const halfedge_descriptor h = *hit;
    if(face(h, g) == boost::graph_traits<G>::null_face())
      continue;

    const int id_of_h = CGAL::halfedge_index_in_face(h, g);
    const face_descriptor f = face(h, g);
    loc.first = f;

    loc.second[id_of_h] = FT(1);
    loc.second[(id_of_h+1)%3] = FT(0);
    loc.second[(id_of_h+2)%3] = FT(0);
    boost::optional<halfedge_descriptor> opt_hd = CGAL::is_border(source(h, g), g);
    assert(PMP::is_on_mesh_border<FT>(loc, g) == (opt_hd != boost::none));

    loc.second[id_of_h] = FT(0.5);
    loc.second[(id_of_h+1)%3] = FT(0.5);
    assert(PMP::is_on_mesh_border<FT>(loc, g) == CGAL::is_border(edge(h, g), g));

    // Even if the point does lie on the border of the mesh, 'false' is returned because
    // another face descriptor should be used.
    loc.second[id_of_h] = -0.5;
    loc.second[(id_of_h+1)%3] = 1.5;
    assert(!PMP::is_on_mesh_border<FT>(loc, g));

    if(++counter > max)
      break;
  }
}

template<typename K, typename G, typename VPM>
void test_locate_in_face(const G& g,
                         const VPM vpm,
                         CGAL::Random& rnd)
{
  std::cout << "  test locate_in_face()..." << std::endl;

  typedef typename boost::property_traits<VPM>::reference                    Point_reference;
  typedef typename K::FT                                                     FT;

  typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
  typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
  typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

  typedef typename PMP::Face_location<G, FT>                                 Face_location;

  const face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
  const halfedge_descriptor h = halfedge(f, g);
  const vertex_descriptor v = target(h, g);

  Face_location loc;
  FT a = 0.1;
  Point_reference p = get(vpm, v);

  loc = PMP::locate_vertex<FT>(v, g);
  assert(is_equal(loc.second[CGAL::vertex_index_in_face(v, loc.first, g)], FT(1)));
  assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+1)%3], FT(0)));
  assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+2)%3], FT(0)));

  loc = PMP::locate_vertex<FT>(v, f, g);
  assert(loc.first == f);
  assert(is_equal(loc.second[0], FT(0)) && is_equal(loc.second[1], FT(1)) && is_equal(loc.second[2], FT(0)));

  loc = PMP::locate_on_halfedge<FT>(h, a, g);
  const int h_id = CGAL::halfedge_index_in_face(h, g);
  assert(loc.first == f && is_equal(loc.second[(h_id+2)%3], FT(0)));

  loc = PMP::locate_in_face(p, f, g, CGAL::parameters::vertex_point_map(vpm).geom_traits(K()));
  int v_id = CGAL::vertex_index_in_face(v, f, g);
  assert(loc.first == f && is_equal(loc.second[v_id], FT(1)));

  // Internal vertex point pmap
  typedef typename boost::property_map_value<G, CGAL::vertex_point_t>::type     Point;

  Point p2 = get(CGAL::vertex_point, g, v);
  PMP::locate_in_face(p2, f, g);
  assert(loc.first == f && is_equal(loc.second[v_id], FT(1)));

  // ---------------------------------------------------------------------------
  loc.second[0] = FT(0.2);
  loc.second[1] = FT(0.8);
  loc.second[2] = FT(0);

  halfedge_descriptor neigh_hd = opposite(halfedge(f, g), g);
  face_descriptor neigh_f = face(neigh_hd, g);

  // Want to check good correspondence seen from one side and the other. If unfortunately
  // we have selected a border face, can't do anything!
  if(neigh_f != boost::graph_traits<G>::null_face())
  {
    int neigh_hd_id = CGAL::halfedge_index_in_face(neigh_hd, g);
    Face_location neigh_loc;
    neigh_loc.first = neigh_f;
    neigh_loc.second[neigh_hd_id] = FT(0.3);
    neigh_loc.second[(neigh_hd_id+1)%3] = FT(0.7);
    neigh_loc.second[(neigh_hd_id+2)%3] = FT(0);

    PMP::locate_in_adjacent_face(loc, neigh_f, g);

    assert(PMP::locate_in_common_face<FT>(loc, neigh_loc, g));

    assert(PMP::locate_in_common_face<FT>(loc, p, neigh_loc, g, CGAL::parameters::vertex_point_map(vpm).geom_traits(K())));
    assert(PMP::locate_in_common_face<FT>(loc, p, neigh_loc, g, CGAL::parameters::vertex_point_map(vpm).geom_traits(K()), 1e-7));
  }
}

template <typename K, typename VPM,
          int dim = CGAL::Ambient_dimension<typename boost::property_traits<VPM>::value_type>::value>
struct Locate_with_AABB_tree_Tester // 2D case
{
  template <typename G>
  void test(const G& g, const VPM vpm, CGAL::Random& rnd) const
  {
    std::cout << "  test locate_with_AABB_tree (2D)..." << std::endl;

    typedef typename boost::property_traits<VPM>::reference                    Point_reference;

    typedef typename K::FT                                                     FT;
    typedef typename K::Ray_2                                                  Ray_2;
    typedef typename K::Ray_3                                                  Ray_3;
    typedef typename K::Point_3                                                Point_3;

    typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
    typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
    typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

    typedef typename PMP::Face_location<G, FT>                                 Face_location;

    face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
    halfedge_descriptor h = halfedge(f, g);
    vertex_descriptor v = target(h, g);

    // ---------------------------------------------------------------------------
    typedef typename boost::property_traits<VPM>::value_type                   Intrinsic_point;
    typedef PMP::internal::Point_to_Point_3<G, Intrinsic_point>                Intrinsic_point_to_Point_3;
    typedef PMP::internal::Point_to_Point_3_VPM<G, VPM>                        WrappedVPM;
    typedef CGAL::AABB_face_graph_triangle_primitive<G, WrappedVPM>            AABB_face_graph_primitive;
    typedef CGAL::AABB_traits<K, AABB_face_graph_primitive>                    AABB_face_graph_traits;

    CGAL_static_assertion((std::is_same<typename AABB_face_graph_traits::Point_3, Point_3>::value));

    Intrinsic_point_to_Point_3 to_p3;

    CGAL::AABB_tree<AABB_face_graph_traits> tree_a;
    Point_reference p_a = get(vpm, v);
    const Point_3& p3_a = to_p3(p_a);

    CGAL::AABB_tree<AABB_face_graph_traits> tree_b;
    WrappedVPM vpm_b(vpm);
    // ---------------------------------------------------------------------------

    PMP::build_AABB_tree(g, tree_a, CGAL::parameters::vertex_point_map(vpm));
    PMP::build_AABB_tree(g, tree_b, CGAL::parameters::vertex_point_map(vpm_b));
    assert(tree_b.size() == num_faces(g));

    Face_location loc = PMP::locate_with_AABB_tree(p_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));

    // sanitize otherwise some test platforms fail
    PMP::internal::snap_location_to_border(loc, g, FT(1e-7));

    assert(PMP::is_on_vertex(loc, v, g)); // might fail du to precision issues...
    assert(is_equal(loc.second[CGAL::vertex_index_in_face(v, loc.first, g)], FT(1)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+1)%3], FT(0)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+2)%3], FT(0)));
    assert(is_equal(CGAL::squared_distance(to_p3(
      PMP::construct_point<FT>(loc, g, CGAL::parameters::vertex_point_map(vpm))), p3_a), FT(0)));

    loc = PMP::locate_with_AABB_tree(p_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(CGAL::squared_distance(to_p3(
      PMP::construct_point<FT>(loc, g, CGAL::parameters::vertex_point_map(vpm))), p3_a), FT(0)));

    // ---------------------------------------------------------------------------
    loc = PMP::locate(p_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(CGAL::squared_distance(to_p3(
      PMP::construct_point(loc, g, CGAL::parameters::vertex_point_map(vpm))), p3_a), FT(0)));
    assert(PMP::is_in_face(loc, g));

    loc = PMP::locate_with_AABB_tree(CGAL::ORIGIN, tree_b, g, CGAL::parameters::vertex_point_map(vpm_b));
    assert(PMP::is_in_face(loc, g));

    loc = PMP::locate(CGAL::ORIGIN, g, CGAL::parameters::vertex_point_map(vpm_b));
    assert(PMP::is_in_face(loc, g));

    // ---------------------------------------------------------------------------
    Ray_2 r2 = random_2D_ray<CGAL::AABB_tree<AABB_face_graph_traits> >(tree_a, rnd);
    loc = PMP::locate_with_AABB_tree(r2, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    if(loc.first != boost::graph_traits<G>::null_face())
      assert(PMP::is_in_face(loc, g));

    Ray_3 r3 = random_3D_ray<CGAL::AABB_tree<AABB_face_graph_traits> >(tree_b, rnd);
    loc = PMP::locate_with_AABB_tree(r3, tree_b, g, CGAL::parameters::vertex_point_map(vpm_b)
                                                                     .geom_traits(K()));
  }
};

template <typename K>
struct My_3D_Point
{
  typedef typename K::FT FT;

  typedef K R; // so that we can use Kernel_traits
  typedef CGAL::Dimension_tag<3>  Ambient_dimension;
  typedef CGAL::Dimension_tag<0>  Feature_dimension;

  My_3D_Point() { }
  My_3D_Point(const CGAL::Origin& /*o*/) : cx(0), cy(0), cz(0) { }
  My_3D_Point(const FT x, const FT y, const FT z) : cx(x), cy(y), cz(z) { }

  FT x() const { return cx; }
  FT y() const { return cy; }
  FT z() const { return cz; }

private:
  FT cx, cy, cz;
};

template <typename K, typename VPM>
struct Locate_with_AABB_tree_Tester<K, VPM, 3> // 3D
{
  template <typename G>
  void test(const G& g, const VPM vpm, CGAL::Random& rnd) const
  {
    std::cout << "  test locate_with_AABB_tree (3D)..." << std::endl;

    typedef typename boost::property_traits<VPM>::reference                    Point_reference;

    typedef typename K::FT                                                     FT;
    typedef typename K::Ray_3                                                  Ray_3;

    typedef typename boost::graph_traits<G>::vertex_descriptor                 vertex_descriptor;
    typedef typename boost::graph_traits<G>::halfedge_descriptor               halfedge_descriptor;
    typedef typename boost::graph_traits<G>::face_descriptor                   face_descriptor;

    typedef typename PMP::Face_location<G, FT>                                 Face_location;

    face_descriptor f = CGAL::internal::random_face_in_mesh(g, rnd);
    halfedge_descriptor h = halfedge(f, g);
    vertex_descriptor v = target(h, g);

    // ---------------------------------------------------------------------------
    typedef CGAL::AABB_face_graph_triangle_primitive<G, VPM>                   AABB_face_graph_primitive;
    typedef CGAL::AABB_traits<K, AABB_face_graph_primitive>                    AABB_face_graph_traits;

    typedef typename K::Point_3                            Point_3;
    CGAL_static_assertion((std::is_same<typename AABB_face_graph_traits::Point_3, Point_3>::value));

    CGAL::AABB_tree<AABB_face_graph_traits> tree_a;
    Point_reference p3_a = get(vpm, v);

    // below tests the case where the value type of the VPM is not Kernel::Point_3
    typedef My_3D_Point<K>                                                     Custom_point;
    typedef std::map<vertex_descriptor, Custom_point>                          Custom_map;
    typedef boost::associative_property_map<Custom_map>                        Custom_VPM;
    typedef PMP::internal::Point_to_Point_3_VPM<G, Custom_VPM>                 WrappedVPM;
    typedef CGAL::AABB_face_graph_triangle_primitive<G, WrappedVPM>            AABB_face_graph_primitive_with_WVPM;
    typedef CGAL::AABB_traits<K, AABB_face_graph_primitive_with_WVPM>          AABB_face_graph_traits_with_WVPM;

    CGAL::AABB_tree<AABB_face_graph_traits_with_WVPM> tree_b;
    Custom_map custom_map;
    for(vertex_descriptor vd : vertices(g))
    {
      const Point_reference p = get(vpm, vd);
      custom_map[vd] = Custom_point(p.x(), p.y(), p.z());
    }

    Custom_VPM custom_vpm(custom_map);
    WrappedVPM custom_vpm_3D(custom_vpm);
    // ---------------------------------------------------------------------------

    PMP::build_AABB_tree(g, tree_a); // just for the API
    assert(tree_a.size() == num_faces(g));

    PMP::build_AABB_tree(g, tree_a, CGAL::parameters::vertex_point_map(vpm));
    PMP::build_AABB_tree(g, tree_b, CGAL::parameters::vertex_point_map(custom_vpm_3D));
    assert(tree_b.size() == num_faces(g));

    Face_location loc = PMP::locate_with_AABB_tree(p3_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(loc.second[CGAL::vertex_index_in_face(v, loc.first, g)], FT(1)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+1)%3], FT(0)));
    assert(is_equal(loc.second[(CGAL::vertex_index_in_face(v, loc.first, g)+2)%3], FT(0)));
    assert(is_equal(CGAL::squared_distance(PMP::construct_point(loc, g), p3_a), FT(0)));

    loc = PMP::locate_with_AABB_tree(p3_a, tree_a, g, CGAL::parameters::vertex_point_map(vpm));
    assert(is_equal(CGAL::squared_distance(PMP::construct_point(loc, g), p3_a), FT(0)));

    // ---------------------------------------------------------------------------
    loc = PMP::locate(p3_a, g, CGAL::parameters::snapping_tolerance(1e-7));
    assert(is_equal(CGAL::squared_distance(PMP::construct_point(loc, g), p3_a), FT(0)));
    assert(PMP::is_in_face(loc, g));

    loc = PMP::locate_with_AABB_tree(CGAL::ORIGIN, tree_b, g, CGAL::parameters::vertex_point_map(custom_vpm_3D));
    assert(PMP::is_in_face(loc, g));

    // Doesn't necessarily have to wrap with a P_to_P3: it can be done automatically internally
    loc = PMP::locate(CGAL::ORIGIN, g, CGAL::parameters::vertex_point_map(custom_vpm));
    assert(PMP::is_in_face(loc, g));

    // ---------------------------------------------------------------------------
    Ray_3 r3 = random_3D_ray<CGAL::AABB_tree<AABB_face_graph_traits_with_WVPM> >(tree_b, rnd);
    loc = PMP::locate_with_AABB_tree(r3, tree_b, g, CGAL::parameters::vertex_point_map(custom_vpm_3D));
  }
};

template<typename K, typename G, typename VPM>
void test_locate(const G& g,
                 const VPM vpm,
                 CGAL::Random& rnd)
{
  assert(num_vertices(g) != 0 && num_faces(g) != 0);

  test_snappers<K>(g);
  test_constructions<K>(g, vpm, rnd);
  test_random_entities<K>(g, rnd);
  test_helpers<K>(g, rnd);
  test_predicates<K>(g, rnd);
  test_locate_in_face<K>(g, vpm, rnd);

  // This test has slight syntax changes between 2D and 3D (e.g. testing ray_2 in 3D makes no sense)
  Locate_with_AABB_tree_Tester<K, VPM> AABB_tester;
  AABB_tester.test(g, vpm, rnd);
}

template<typename K, typename G>
void test_locate(const G& g, CGAL::Random& rnd)
{
  return test_locate<K>(g, CGAL::get_const_property_map(boost::vertex_point, g), rnd);
}

template<typename K>
void test_2D_triangulation(const std::string fname, CGAL::Random& rnd)
{
  typedef CGAL::Regular_triangulation_2<K>                    RT;

  std::cout << "Testing Regular_triangulation_2 " << fname;

  std::ifstream in(fname);
  assert(in.good());

  RT tr;
  double x, y;
  while(in >> x >> y)
    tr.insert(typename RT::Point(x, y));

  std::ofstream out("triangulation.off");
  out << "OFF\n";
  out << tr.number_of_vertices() << " " << std::distance(tr.finite_faces_begin(), tr.finite_faces_end()) << " 0\n";

  std::size_t counter = 0;
  std::map<typename RT::Point_2, std::size_t> ids;
  for(const auto& v : CGAL::make_range(tr.finite_vertices_begin(), tr.finite_vertices_end()))
  {
    out << v.point().point() << " 0\n";
    if(ids.insert(std::make_pair(v.point().point(), counter)).second)
      ++counter;
  }

  for(const auto& fd : CGAL::make_range(tr.finite_faces_begin(), tr.finite_faces_end()))
  {
    out << "3 " << ids[fd.vertex(0)->point().point()] << " " << ids[fd.vertex(1)->point().point()] << " " << ids[fd.vertex(2)->point().point()] << "\n";
  }

  out.close();

  std::cout << " (" << tr.number_of_vertices() << " vertices)..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  test_locate<K>(tr, rnd);
}

template<typename K>
void test_2D_surface_mesh(const std::string fname, CGAL::Random& rnd)
{
  typedef typename K::Point_2                                 Point;
  typedef CGAL::Surface_mesh<Point>                           Mesh;

  std::cout << "Testing Surface_mesh " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  assert(input.good());

  Mesh tm;
  if(!input || !(input >> tm))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  test_locate<K>(tm, rnd);
}

template<typename K>
void test_surface_mesh_3D(const std::string fname, CGAL::Random& rnd)
{
  typedef typename K::Point_3                                 Point;
  typedef CGAL::Surface_mesh<Point>                           Mesh;

  std::cout << "Testing (3D) Surface_mesh " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  Mesh tm;
  if(!input || !(input >> tm))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  typedef typename boost::property_map<Mesh, CGAL::vertex_point_t>::const_type  VertexPointMap;
  VertexPointMap vpm = CGAL::get_const_property_map(boost::vertex_point, tm);

  test_locate<K>(tm, vpm, rnd);
}

template<typename K>
void test_surface_mesh_projection(const std::string fname, CGAL::Random& rnd)
{
  typedef typename K::Point_3                                       Point;
  typedef CGAL::Surface_mesh<Point>                                 Mesh;
  typedef typename K::Point_2                                       Projected_point;

  typedef typename boost::graph_traits<Mesh>::vertex_descriptor     vertex_descriptor;

  std::cout << "Testing Projected Surface_mesh " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  Mesh tm;
  if(!input || !(input >> tm))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  const auto& proj_vpm = tm.template add_property_map<typename Mesh::Vertex_index,
                                                      Projected_point>("P2", Projected_point()).first;

  for(vertex_descriptor v : vertices(tm))
  {
    const Point& p = tm.point(v);
    put(proj_vpm, v, Projected_point(p.x(), p.y()));
  }

  test_locate<K>(tm, proj_vpm, rnd);
}

template<typename K>
void test_polyhedron(const std::string fname, CGAL::Random& rnd)
{
  typedef CGAL::Polyhedron_3<K>                               Polyhedron;

  std::cout << "Testing Polyhedron_3 " << fname << "..." << std::endl;
  std::cout << "Kernel: " << typeid(K()).name() << std::endl;

  std::ifstream input(fname);
  Polyhedron poly;
  if(!input || !(input >> poly))
  {
    std::cerr << "Error: cannot read file.";
    return;
  }

  test_locate<K>(poly, rnd);
}

template <typename K>
void test(CGAL::Random& rnd)
{
  test_2D_triangulation<K>("data/stair.xy", rnd);
//  test_2D_surface_mesh<K>("data/blobby_2D.off", rnd); // temporarily disabled, until Surface_mesh's IO is "fixed"
  test_surface_mesh_3D<K>(CGAL::data_file_path("meshes/mech-holes-shark.off"), rnd);
  test_surface_mesh_projection<K>("data/unit-grid.off", rnd);
  test_polyhedron<K>("data-coref/elephant_split_2.off", rnd);
}

int main()
{
  CGAL::Set_ieee_double_precision pfr;

  std::cout.precision(17);
  std::cout << std::fixed;

//  CGAL::Random rnd(1557332474); // if needed to debug with a fixed seed
  CGAL::Random rnd(CGAL::get_default_random());

  std::cout << "The seed is " << rnd.get_seed() << std::endl;

  test<EPICK>(rnd);
  test<Exact_kernel>(rnd);

  return EXIT_SUCCESS;
}


#include <iostream>
#include <map>
#include <vector>
#include <utility>
#include <string>
#include <algorithm>
#include <fstream>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <illust_image_similarity.hpp>

using namespace illust_image_similarity::feature;

std::vector<std::string> getFileNames(std::string pathStr) {
	std::vector<std::string> res;
	namespace fs = boost::filesystem;
	const fs::path path(pathStr);

	BOOST_FOREACH(const fs::path& p, std::make_pair(fs::directory_iterator(path), fs::directory_iterator())) {
		if (!fs::is_directory(p))
			res.push_back(p.filename().string());
	}
	return res;
}

int main(){
	cv::Mat mask = cv::imread("tentee_patch/mask/mask.png", 0);
	// vvv For testing vvv //
	// b1, b3, y1 -> double edge
	// b2, p1, p2, p3 -> single edge
	// p3 -> no edge
	/*std::map<std::string, cv::Mat> images = {
		{ "blue1", cv::imread("../tentee_patch/dream/0001.png") },
		{ "blue2", cv::imread("../tentee_patch/dream/0087.png") },
		{ "blue3", cv::imread("../tentee_patch/dream/0006.png") },
		{ "blue4", cv::imread("../tentee_patch/dream/0255.png") },
		{ "yell1", cv::imread("../tentee_patch/dream/0005.png") },
		{ "pink1", cv::imread("../tentee_patch/dream/0002.png") },
		{ "pink2", cv::imread("../tentee_patch/dream/0132.png") },
		{ "pink3", cv::imread("../tentee_patch/dream/0138.png") },
		{ "pink4", cv::imread("../tentee_patch/dream/0004.png") }
	};*/
	// memo: 1と12は近い, 2は遠い
	// memo: 255と2はどちらも縞
	// ^^^ For testing ^^^ //
	std::map<std::string, cv::Mat> images;

	auto fileList = getFileNames("../tentee_patch/dream/");
	{
		int cnt = 0;
		for(auto&& fName : fileList){
			cv::Mat img = cv::imread("../tentee_patch/dream/" + fName);
			if(++cnt % 10 == 0)
				std::cout << "loading: "  << cnt << " images..." << std::endl;
			if(img.data)
				images[fName] = img;
		}
	}

	std::cout << "start" << std::endl;

	std::map<std::string, cv::MatND> hists;
	for(auto&& [name, img] : images)
		hists[name] = img | hueHistgramAlgorithm(mask);

	std::map<std::string, cv::MatND> placements;
	for(auto&& [name, img] : images)
		placements[name] = img | placementAlgorithm;

	std::map<std::string, std::vector<double>> directions;
	for(auto&& [name, img] : images)
		directions[name] = img | directionPreprocess | directionVec(mask);

	int cnt = 0;
	auto cdot = [](const std::vector<double>& lhs, const std::vector<double>& rhs) -> double {
		int dim = std::min(lhs.size(), rhs.size());
		double res = 0;
		for(int i=0; i<dim; ++i)
			res += lhs[i] * rhs[i];
		return res;
	};
	// from, type [hue, edge, dir], to, index (0-), score (0-1)
	const int maxIndex = 5;
	std::ofstream data("recommend_data.csv");
	for(auto&& [name, img] : images){
		if(++cnt % 10 == 0)
			std::cout << cnt << " / " << images.size() << " ... " << std::endl;

		std::vector<std::pair<double, std::string>> list_hue;
		std::vector<std::pair<double, std::string>> list_edge;
		std::vector<std::pair<double, std::string>> list_dir;
		auto&& pls  = placements.at(name);
		auto&& hist = hists.at(name);
		auto&& dir  = directions.at(name);
		// target
		for(auto&& [tName, tImg] : images){
			if(name == tName) continue; // 自身とは比較しない (edge,dir,hueのスコアは常に1)
			auto&& tPls  = placements.at(tName);
			auto&& tHist = hists.at(tName);
			auto&& tDir  = directions.at(tName);
			list_hue.emplace_back(cv::compareHist(hist, tHist, cv::HISTCMP_CORREL), tName);

			cv::Mat tmp;
			// Normalized Cross-Correlation
			//list_edge.emplace_back(cv::norm(img, tImg, cv::NORM_L1), tName);
			cv::matchTemplate(pls, tPls, tmp, CV_TM_CCOEFF_NORMED);
			list_edge.emplace_back(tmp.at<float>(0,0), tName);

			// dot product
			list_dir.emplace_back(cdot(dir, tDir), tName);
		}

		std::sort(list_edge.begin(), list_edge.end());
		std::reverse(list_edge.begin(), list_edge.end());

		std::sort(list_dir.begin(), list_dir.end());
		std::reverse(list_dir.begin(), list_dir.end());

		std::sort(list_hue.begin(), list_hue.end());
		std::reverse(list_hue.begin(), list_hue.end());

		for(int i=0; i<maxIndex && i < list_hue.size(); ++i){
			auto [tHist, tName] = list_hue[i];
			data << name << "," << "hue" << "," << tName << "," << i << "," << tHist << "\n";
		}

		for(int i=0; i<maxIndex && i < list_edge.size(); ++i){
			auto [tHist, tName] = list_edge[i];
			data << name << "," << "edge" << "," << tName << "," << i << "," << tHist << "\n";
		}

		for(int i=0; i<maxIndex && i < list_dir.size(); ++i){
			auto [tHist, tName] = list_dir[i];
			data << name << "," << "dir" << "," << tName << "," << i << "," << tHist << "\n";
		}
		data << std::flush;

	}

	std::cout << "end" << std::endl;
}


/*
 * EigenvalueCovariance.cpp
 *
 *  Created on: May 6, 2015
 *      Author: dbazazian
 *
 *  Modified by: Milos Prokop, August 2019
 */
#include <iostream>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <vector>
#include <math.h>
#include <cmath>
#include <fstream>
#include <string>
#include <vector>
//#include <cstdlib>
#include <pcl/io/io.h>
//#include <pcl/io/pcd_io.h>
//#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
//#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/shot_omp.h>
#include "pcl/features/fpfh.h"
#include <pcl/io/ply_io.h>
#include <pcl/search/impl/kdtree.hpp>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

#include <pcl/filters/statistical_outlier_removal.h>

#include "Difference_Eigenvalues.h"

#define KNN 20

using namespace std;
using namespace Eigen;

//Sigma threshold
//float threshold = 0.05;

//int main (int argc, char*argv[]){eeeeeeeeeeee
pcl::PointCloud<pcl::PointXYZ>::Ptr extract_edges(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const std::string& outputname, bool writeFile ){

    pcl::PointCloud<pcl::PointXYZ>::Ptr Normals (new pcl::PointCloud<pcl::PointXYZ>);
    Normals->resize(cloud->size());

    // K nearest neighbor search
    int KNumbersNeighbor = 10; // numbers of neighbors 7 , 120
    std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
    std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

    int* NumbersNeighbor = new  int [cloud ->points.size ()];
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud (cloud);
    pcl::PointXYZ searchPoint;

    double* SmallestEigen = new  double [cloud->points.size() ];
    double* MiddleEigen = new  double [cloud->points.size() ];
    double* LargestEigen = new  double [cloud->points.size() ];

    //double* DLS = new  double [cloud->points.size() ];
    //double* DLM = new  double [cloud->points.size() ];
    //double* DMS = new  double [cloud->points.size() ];
    double* Sigma = new  double [cloud->points.size() ];

    // std::vector<double> SmallestEigen;
    // std::vector<double> MiddleEigen;
    // std::vector<double> LargestEigen;
    //
    // std::vector<double> DLS;
    // std::vector<double> DML;
    // std::vector<double> DMS;

    //  ************ All the Points of the cloud *******************
    for (size_t i = 0; i < cloud ->points.size (); ++i) {

        searchPoint.x =   cloud->points[i].x;
        searchPoint.y =   cloud->points[i].y;
        searchPoint.z =   cloud->points[i].z;

        if ( kdtree.nearestKSearch (searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0 ) {
                 NumbersNeighbor[i]= NeighborsKNSearch.size (); }
            else { NumbersNeighbor[i] = 0; }

        float Xmean; float Ymean; float Zmean;
        float sum= 0.00;

        // Computing Covariance Matrix
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += cloud->points[ NeighborsKNSearch[ii] ].x;

        Xmean = sum / NumbersNeighbor[i];

        sum= 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += cloud->points[NeighborsKNSearch[ii] ].y;
        Ymean = sum / NumbersNeighbor[i];

        sum= 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += cloud->points[NeighborsKNSearch[ii] ].z;
        Zmean = sum / NumbersNeighbor[i];

        float CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].x - Xmean )  );
        CovXX = sum / ( NumbersNeighbor[i]-1);

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
        sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );
        CovXY = sum / ( NumbersNeighbor[i]-1);
        CovYX = CovXY ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
        sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );
        CovXZ= sum / ( NumbersNeighbor[i]-1);
        CovZX = CovXZ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );
        CovYY = sum / ( NumbersNeighbor[i]-1);

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );
        CovYZ = sum / ( NumbersNeighbor[i]-1);
        CovZY = CovYZ;

        sum = 0.00 ;
        for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii)
            sum += ( (cloud->points[NeighborsKNSearch[ii] ].z - Zmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );
        CovZZ = sum / ( NumbersNeighbor[i]-1);

        // Computing Eigenvalue and EigenVector
        Matrix3f Cov;
        Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

        SelfAdjointEigenSolver<Matrix3f> eigensolver(Cov);
        if (eigensolver.info() != Success) abort();

        double EigenValue1 = eigensolver.eigenvalues()[0];
        double EigenValue2 = eigensolver.eigenvalues()[1];
        double EigenValue3 = eigensolver.eigenvalues()[2];

        double Smallest = 0.00; double Middle = 0.00; double Largest= 0.00;

        if (EigenValue1 < EigenValue2)
            Smallest =  EigenValue1;
        else
            Smallest = EigenValue2;

        if (EigenValue3 < Smallest)
            Smallest =  EigenValue3;

        if(EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3) {
              Smallest = EigenValue1;
        if(EigenValue2 <= EigenValue3) {Middle = EigenValue2; Largest = EigenValue3;}
        else {Middle = EigenValue3; Largest = EigenValue2;}
        }

        if(EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
        {
              Largest = EigenValue1;
        if(EigenValue2 <= EigenValue3) { Smallest = EigenValue2; Middle = EigenValue3; }
        else {Smallest = EigenValue3; Middle = EigenValue2;}
        }

        if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
        {
              Middle = EigenValue1;
        if(EigenValue2 >= EigenValue3){Largest = EigenValue2; Smallest = EigenValue3;}
        else{Largest = EigenValue3; Smallest = EigenValue2;}
        }

        SmallestEigen[i]= Smallest ;
        MiddleEigen[i]= Middle;
        LargestEigen[i]= Largest;

        //DLS[i] = std::abs ( SmallestEigen[i] / LargestEigen[i]) ;          // std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
        //DLM[i] = std::abs ( MiddleEigen[i] /  LargestEigen[i]) ;             // std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
        //DMS[i] = std::abs ( SmallestEigen[i] / MiddleEigen[i]) ;       // std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;

        Sigma[i] = (SmallestEigen[i] ) / ( SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i] ) ;
    }

    //std::cout<< " Computing Sigma is Done! " << std::endl;
    // Color Map For the difference of the eigen values

    double MaxD = 0.00;
    double MinD = std::numeric_limits<double>::max();

    int Ncolors = 256;

    for (size_t i = 0; i < cloud ->points.size (); ++i) {

          if (Sigma [i] < MinD)
              MinD= Sigma [i];

          if (Sigma[i] > MaxD)
              MaxD = Sigma [i];
    }

    //std::cout<< " Minimum is :" << MinD<< std::endl;
    //std::cout<< " Maximum  is :" << MaxD << std::endl;

	/*
	  // computing the standard deviation
	 double ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += Sigma [i] ;}
	  double avg = ss / cloud ->points.size () ;
	  ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += (Sigma [i] -  avg ) * (  Sigma [i] -  avg ) ;}
	  double stddvtion =   sqrt (  ss  /  ( cloud ->points.size () - 1 )  ) ;

	  std::cout<< " Standard Deviation is :" << stddvtion << std::endl;

	   MaxD = ( 2 )* stddvtion;
	  //MaxD = 10* stddvtion;

	  // Color table
		double line;
		double code[Ncolors][3];
	   ifstream colorcode ( "/Path/TO/ArtificialPointClouds/JetColorDensity/ColorCodes256.txt" );
	   //store color codes in array
	    int i=0,j=0;
	    while( colorcode>> line ) {
	    code[i][j]=line;
	    j++;
	    if (j == 3)
	    i++;
	}
	    code[1][0] = 0;
	    code[1][1] = 0;
	    code[1][2] = 135.468;

	    // jet color map

	int level = 0;
	float step = ( ( MaxD -  MinD) / Ncolors ) ;
	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
    if (  SmallestEigen [i] <= MaxD ) {
                level = floor( (SmallestEigen [i] - MinD ) /  step ) ;

                cloud->points[i].r = code[ level ][0];
                cloud->points[i].g =  code[ level ][1];
                cloud->points[i].b =  code[ level ][2];
    } // if sigma less than Max
                }
    */

    int ePointsCount = 0;

    int level = 0;
    float step = ( ( MaxD -  MinD) / Ncolors ) ;
    //  level = floor( (Sigma [i] - MinD ) /  step ) ;
    float threshold = ( MinD + ( 6 * step) );
    //float threshold=0.1;
    //std::cout << ( MinD + ( 6 * step) ) << "\n";

    pcl::PointCloud<pcl::PointXYZ>::Ptr edgePoints (new pcl::PointCloud<pcl::PointXYZ>);

    for (size_t i = 0; i < cloud -> points.size(); ++i) {
        if ( Sigma [i] > threshold ) {

            edgePoints->points.push_back( cloud->points[i] );
            ePointsCount++;

        }
    }
    if ( writeFile ){

        pcl::PointCloud<pcl::PointXYZRGB> cloudRGB;
        pcl::copyPointCloud(*edgePoints, cloudRGB);

        for (size_t i = 0; i < cloudRGB.points.size(); ++i) {
            cloudRGB.points[i].r = 255;
            cloudRGB.points[i].g = 0;
            cloudRGB.points[i].b = 0;
        }

        pcl::PLYWriter writePLY;
        writePLY.write(outputname, cloudRGB,  true, false);
        std::cout << "File " << outputname << " written.\n";

    }

/*
   //DELETE
   pcl::PLYWriter writePLY;
   pcl::PointCloud<pcl::PointXYZRGB> cloudRGB;
   pcl::copyPointCloud(*edgePoints,cloudRGB);

   for (size_t i = 0; i < cloudRGB.points.size(); ++i) {
                cloudRGB.points[i].r = 255;
                cloudRGB.points[i].g = 0;
                cloudRGB.points[i].b = 0;

   }

  writePLY.write(outputname, cloudRGB,  true, false);

  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (edgePoints);
  sor.setMeanK (50);
  sor.setStddevMulThresh (0.05);
  sor.filter (*filtered);

  pcl::copyPointCloud(*filtered,cloudRGB);
  writePLY.write(outputname+"_filtered.ply", cloudRGB,  true, false);

*/

    // writing the Sigma on the disk
    //	    	   		std::ofstream ofsSigma;
    //	    	   		ofsSigma.open("/Path/TO/SigmaDragon.txt");
    //	    	            for (size_t i = 0; i < cloud ->points.size (); ++i) {
    //	    	            	ofsSigma << Sigma [i]<< ","<< std::endl ;
    //	    	                    }
    //  	pcl::PLYWriter writePLY;
    //  	writePLY.write (OUTPUT_FILE, *cloud,  false);

    //return 0;

  return edgePoints;
}


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


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/InputOutput/basicInputOutput.h"

#include "Tudat/Astrodynamics/EarthOrientation/shortPeriodEarthOrientationCorrectionCalculator.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h"
#include "Tudat/External/SofaInterface/sofaTimeConversions.h"

namespace tudat
{

namespace unit_tests
{

using namespace basic_astrodynamics;
using namespace sofa_interface;
using namespace earth_orientation;
using namespace input_output;
using namespace unit_conversions;

//! NOTE on tolerances: 1 mm position error on Earth's surface due to single angle error in Earth orientation corresponds to
//! 0.15 nrad angle uncertainty. This corresponds to 32 microarcseconds. For Earth rotation, this corresponds to 2.2 microseconds
//! in UT1. The applied tolerances (1 microarc seconds and 50 ns) correspond to about 20-30 microns difference at Earth's surface.
BOOST_AUTO_TEST_SUITE( test_short_period_eop_corrections )

//! Test short-periodic ut1-utc variations by comparing to test output of UTLIBR.f file
BOOST_AUTO_TEST_CASE( testShortPeriodLibrationalPolarMotion)
{
    // Create polar motion correction libration corrections
    ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > polarMotionCalculator(
               convertArcSecondsToRadians< double >( 1.0E-6 ), 0.0,
    { getEarthOrientationDataFilesPath( ) + "polarMotionLibrationAmplitudesQuasiDiurnalOnly.txt" },
    { getEarthOrientationDataFilesPath( ) + "polarMotionLibrationFundamentalArgumentMultipliersQuasiDiurnalOnly.txt" } );

    //  Define test time
    double testMjd = 54335.0;
    double testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    double testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    double testEphemerisTime = convertUTCtoTT( testUtc );

    // Compute polar motion correction
    double microAsToRadians =  mathematical_constants::PI / ( 180.0 * 1.0E6 * 3600.0 );
    Eigen::Vector2d polarMotionCorrections = polarMotionCalculator.getCorrections( testEphemerisTime ) / microAsToRadians;

    // Compare against IERS reference code. Difference between IERS code and this code occurs since the reference uses a slightly
    // different implementation for computation. Difference (1 micro arc seconds) is well below observable threshold
    BOOST_CHECK_SMALL( polarMotionCorrections( 0 ) - 24.65518398386097942, 1.0 );
    BOOST_CHECK_SMALL( polarMotionCorrections( 1 ) + 14.11070254891893327, 1.0 );

}

//! Test short-periodic polar motion libration variations by comparing to test output of ORTHO_EOP.f file
BOOST_AUTO_TEST_CASE( testShortPeriodOceanTidePolarMotion)
{
    // Create polar motion correction ocean tide corrections
    ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > polarMotionCalculator(
               convertArcSecondsToRadians< double >( 1.0E-6 ), 0.0,
    { getEarthOrientationDataFilesPath( ) + "polarMotionOceanTidesAmplitudes.txt", },
    { getEarthOrientationDataFilesPath( ) + "polarMotionOceanTidesFundamentalArgumentMultipliers.txt" } );

    //  Define test time
    double testMjd = 47100.0;
    double testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    double testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    double testEphemerisTime = convertUTCtoTT( testUtc );

    // Compute polar motion correction
    double microAsToRadians =  mathematical_constants::PI / ( 180.0 * 1.0E6 * 3600.0 );
    Eigen::Vector2d polarMotionCorrections = polarMotionCalculator.getCorrections( testEphemerisTime ) / microAsToRadians;

    // Compare against IERS reference code. Difference between IERS code and this code occurs since the reference uses a
    // different algorothm for calculation (ortho-weights vs. Delaunay arguments). Difference (1 micro arc seconds) is well below
    // observable threshold
    BOOST_CHECK_SMALL( polarMotionCorrections.x( ) + 162.8386373279636530, 1.0 );
    BOOST_CHECK_SMALL( polarMotionCorrections.y( ) - 117.7907525842668974, 1.0 );

}

//! Test short-periodic polar motion variations by checking if multiple contributions are properly combined
BOOST_AUTO_TEST_CASE( testShortPeriodPolarMotionCombinedCorrections )
{
    // Create polar motion correction ocean tide corrections
    ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > polarMotionOceanTideCorrectionCalculator(
               convertArcSecondsToRadians< double >( 1.0E-6 ), 0.0,
    { getEarthOrientationDataFilesPath( ) + "polarMotionOceanTidesAmplitudes.txt" },
    { getEarthOrientationDataFilesPath( ) + "polarMotionOceanTidesFundamentalArgumentMultipliers.txt" } );

    // Create polar motion correction libration corrections
    ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > polarMotionLibrationCorrectionCalculator(
               convertArcSecondsToRadians< double >( 1.0E-6 ), 0.0,
    { getEarthOrientationDataFilesPath( ) + "polarMotionLibrationAmplitudesQuasiDiurnalOnly.txt" },
    { getEarthOrientationDataFilesPath( ) + "polarMotionLibrationFundamentalArgumentMultipliersQuasiDiurnalOnly.txt" } );

    // Create polar motion correction combined corrections
    ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > polarMotionTotalCorrectionCalculator(
               convertArcSecondsToRadians< double >( 1.0E-6 ), 0.0,
    { getEarthOrientationDataFilesPath( ) + "polarMotionOceanTidesAmplitudes.txt",
      getEarthOrientationDataFilesPath( ) + "polarMotionLibrationAmplitudesQuasiDiurnalOnly.txt" },
    { getEarthOrientationDataFilesPath( ) + "polarMotionOceanTidesFundamentalArgumentMultipliers.txt",
      getEarthOrientationDataFilesPath( ) + "polarMotionLibrationFundamentalArgumentMultipliersQuasiDiurnalOnly.txt" } );

    //  Define test time
    double testMjd = 47100.0;
    double testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    double testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    double testEphemerisTime = convertUTCtoTT( testUtc );

    // Compute corrections and check validity of combination
    Eigen::Vector2d polarMotionCorrectionOceanTides = polarMotionOceanTideCorrectionCalculator.getCorrections(
                testEphemerisTime );
    Eigen::Vector2d polarMotionCorrectionLibration = polarMotionLibrationCorrectionCalculator.getCorrections(
                testEphemerisTime );
    Eigen::Vector2d polarMotionCorrectionTotal = polarMotionTotalCorrectionCalculator.getCorrections( testEphemerisTime );

    BOOST_CHECK_SMALL( std::fabs( polarMotionCorrectionTotal( 0 ) -
                                  ( polarMotionCorrectionLibration( 0 ) + polarMotionCorrectionOceanTides( 0 ) ) ), 1.0E-24 );
    BOOST_CHECK_SMALL( std::fabs( polarMotionCorrectionTotal( 1 ) -
                                  ( polarMotionCorrectionLibration( 1 ) + polarMotionCorrectionOceanTides( 1 ) ) ), 1.0E-24 );
}

//! Test short-periodic ut1-utc variations by comparing to test output of UTLIBR.f file
BOOST_AUTO_TEST_CASE( testShortPeriodLibrationalUt1)
{
    // Create UT1 correction libration corrections
    ShortPeriodEarthOrientationCorrectionCalculator < double > ut1CorrectionCalculator(
                1.0E-6, 0.0,
    { getEarthOrientationDataFilesPath( ) + "utcLibrationAmplitudes.txt" },
    { getEarthOrientationDataFilesPath( ) + "utcLibrationFundamentalArgumentMultipliers.txt" } );

    //  Define test time
    double testMjd = 44239.1 ;
    double testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    double testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    double testEphemerisTime = convertUTCtoTT( testUtc );

    // Compute UT1 correction
    double ut1Correction = ut1CorrectionCalculator.getCorrections( testEphemerisTime );

    // Compare against IERS reference code. Difference between IERS code and this code occurs since the reference uses a slightly
    // different implementation for computation. Difference (10 ns) is well below observable threshold for Earth rotation
    BOOST_CHECK_SMALL( ut1Correction - 2.441143834386761746E-6, 1.0E-8 );

    //  Define second test time
    testMjd = 55227.4 ;
    testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    testEphemerisTime = convertUTCtoTT( testUtc );

    // Compute UT1 correction
    ut1Correction = ut1CorrectionCalculator.getCorrections( testEphemerisTime );

    // Compare against IERS reference code. Difference between IERS code and this code occurs since the reference uses a slightly
    // different implementation for computation. Difference (10 ns) is well below observable threshold for Earth rotation
    BOOST_CHECK_SMALL( ut1Correction + 2.655705844335680244E-6, 5.0E-8 );

}

//! Test short-periodic ut1-utc libration variations by comparing to test output of ORTHO_EOP.f file
BOOST_AUTO_TEST_CASE( testShortPeriodOceanTideUt1 )
{
    // Create UT1 correction ocean tide corrections
    ShortPeriodEarthOrientationCorrectionCalculator < double > ut1CorrectionCalculator =
            ShortPeriodEarthOrientationCorrectionCalculator < double >(
                1.0E-6, 0.0,
    { getEarthOrientationDataFilesPath( ) + "utcOceanTidesAmplitudes.txt", },
    { getEarthOrientationDataFilesPath( ) + "utcOceanTidesFundamentalArgumentMultipliers.txt" } );

    //  Define test time
    double testMjd = 47100.0;
    double testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    double testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    double testEphemerisTime = convertUTCtoTT( testUtc );
    double ut1Correction = ut1CorrectionCalculator.getCorrections( testEphemerisTime );

    // Compare against IERS reference code. Difference between IERS code and this code occurs since the reference uses a
    // different algorothm for calculation (ortho-weights vs. Delaunay arguments). Difference (50 ns) is well below observable
    // threshold for Earth rotation
    BOOST_CHECK_SMALL( ut1Correction + 23.39092370609808214E-6, 5.0E-8 );

}

//! Test short-periodic ut1-utc variations by checking if multiple contributions are properly combined
BOOST_AUTO_TEST_CASE( testShortPeriodUt1CombinedCorrections )
{
    // Create UT1 correction ocean tide corrections
    ShortPeriodEarthOrientationCorrectionCalculator < double > ut1OceanTideCorrectionCalculator =
            ShortPeriodEarthOrientationCorrectionCalculator < double >(
                1.0E-6, 0.0,
    { getEarthOrientationDataFilesPath( ) + "utcOceanTidesAmplitudes.txt" },
    { getEarthOrientationDataFilesPath( ) + "utcOceanTidesFundamentalArgumentMultipliers.txt" } );

    // Create UT1 correction libration corrections
    ShortPeriodEarthOrientationCorrectionCalculator < double > ut1LibrationCorrectionCalculator(
                1.0E-6, 0.0,
    { getEarthOrientationDataFilesPath( ) + "utcLibrationAmplitudes.txt" },
    { getEarthOrientationDataFilesPath( ) + "utcLibrationFundamentalArgumentMultipliers.txt" } );

    // Create UT1 correction combined corrections
    ShortPeriodEarthOrientationCorrectionCalculator < double > ut1TotalCorrectionCalculator(
                1.0E-6, 0.0,
    { getEarthOrientationDataFilesPath( ) + "utcLibrationAmplitudes.txt",
                getEarthOrientationDataFilesPath( ) + "utcOceanTidesAmplitudes.txt" },
    { getEarthOrientationDataFilesPath( ) + "utcLibrationFundamentalArgumentMultipliers.txt",
                getEarthOrientationDataFilesPath( ) + "utcOceanTidesFundamentalArgumentMultipliers.txt" } );

    //  Define test time
    double testMjd = 47100.0;
    double testJulianDay = testMjd + JULIAN_DAY_AT_0_MJD;
    double testUtc = convertJulianDayToSecondsSinceEpoch( testJulianDay, JULIAN_DAY_ON_J2000 );
    double testEphemerisTime = convertUTCtoTT( testUtc );

    // Compute corrections and check validity of combination
    double ut1CorrectionOceanTides = ut1OceanTideCorrectionCalculator.getCorrections( testEphemerisTime );
    double ut1CorrectionLibration = ut1LibrationCorrectionCalculator.getCorrections( testEphemerisTime );
    double ut1CorrectionTotal = ut1TotalCorrectionCalculator.getCorrections( testEphemerisTime );

    BOOST_CHECK_SMALL( std::fabs( ut1CorrectionTotal - ( ut1CorrectionLibration + ut1CorrectionOceanTides ) ), 1.0E-20 );
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include <boost/math/distributions/triangular.hpp>


#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

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

#include "Map/2d/RayToPixelMask.h"
#include "Map/2d/MapLimits.h"

namespace {

bool isEqual(const Eigen::Array2i & lhs, const Eigen::Array2i & rhs) {
    return ((lhs - rhs).matrix().lpNorm<1>() == 0);
}

}

BOOST_AUTO_TEST_CASE(EQUAL) {
    BOOST_CHECK(isEqual(Eigen::Array2i(1,3), Eigen::Array2i(1,3)) == true);
}

BOOST_AUTO_TEST_CASE(SingleCell) {
    const Eigen::Array2i & begin = {1, 1};
    const Eigen::Array2i & end = {1, 1};
    const int subPixelScale = 1;

    std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_CHECK(isEqual(ray[0], begin) == true);
}

BOOST_AUTO_TEST_CASE(AxisAlginedX) {
    const Eigen::Array2i & begin = {1, 1};
    const Eigen::Array2i & end = {3, 1};
    const int subPixelScale = 1;
    std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], Eigen::Array2i({(1+i), 1})));
    }
    ray.clear();
    ray = VISFS::Map::rayToPixelMask(end, begin, subPixelScale);
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], Eigen::Array2i({(1+i), 1})));
    }
}

BOOST_AUTO_TEST_CASE(AxisAlginedY) {
    const Eigen::Array2i & begin = {1, 1};
    const Eigen::Array2i & end = {1, 3};
    const int subPixelScale = 1;
    std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], Eigen::Array2i({1, 1+i})));
    }
    ray.clear();
    ray = VISFS::Map::rayToPixelMask(end, begin, subPixelScale);
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], Eigen::Array2i({1, 1+i})));
    }
}

BOOST_AUTO_TEST_CASE(Digonal) {
    Eigen::Array2i begin = {1, 1};
    Eigen::Array2i end = {3, 3};
    const int subPixelScale = 1;
    std::vector<Eigen::Array2i> rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})};
    std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
    ray.clear();
    ray = VISFS::Map::rayToPixelMask(end, begin, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }

    begin = Eigen::Array2i({1, 3});
    end = Eigen::Array2i({3, 1});
    rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({1, 3}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 1})};
    ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
    ray = VISFS::Map::rayToPixelMask(end, begin, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
}

BOOST_AUTO_TEST_CASE(SteepLine) {
    Eigen::Array2i begin = {1, 1};
    Eigen::Array2i end = {2, 5};
    const int subPixelScale = 1;
    std::vector<Eigen::Array2i> rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}),
        Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})};
    std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
    begin = {1, 1};
    end = {2, 4};
    rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}),
        Eigen::Array2i({2, 4})};
    ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
}

BOOST_AUTO_TEST_CASE(FlatLine) {
    Eigen::Array2i begin = {1, 1};
    Eigen::Array2i end = {5, 2};
    const int subPixelScale = 1;
    std::vector<Eigen::Array2i> rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({1, 1}), Eigen::Array2i({2, 1}), Eigen::Array2i({3, 1}),
        Eigen::Array2i({3, 2}), Eigen::Array2i({4, 2}), Eigen::Array2i({5, 2})};
    std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
    begin = {1, 1};
    end = {4, 2};
    rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({1, 1}), Eigen::Array2i({2, 1}), Eigen::Array2i({3, 2}),
        Eigen::Array2i({4, 2})};
    ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
    BOOST_REQUIRE(ray.size() == rayReference.size());
    for (int i = 0; i < ray.size(); ++i) {
        BOOST_CHECK(isEqual(ray[i], rayReference[i]));
    }
}

BOOST_AUTO_TEST_CASE(MultiScaleAxisAlignedX) {
    int subPixelScale;
    const int numCellsX = 10;
    const int numCellsY = 10;
    double resolution = 0.1;
    Eigen::Vector2d max = {1.0, 1.0};
    std::vector<Eigen::Array2i> rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({9, 6}), Eigen::Array2i({9, 7}), Eigen::Array2i({9, 8}), Eigen::Array2i({9, 9})};
    for (subPixelScale = 1; subPixelScale < 10000; subPixelScale *= 2) {
        double superscaledResolution  = resolution / subPixelScale;
        VISFS::Map::MapLimits superScaledLimits(superscaledResolution, max,
            VISFS::Map::CellLimits(numCellsX * subPixelScale, numCellsY * subPixelScale));
        Eigen::Array2i begin = superScaledLimits.getCellIndex(Eigen::Vector2d({0.05, 0.05}));
        Eigen::Array2i end = superScaledLimits.getCellIndex(Eigen::Vector2d({0.35, 0.05}));
        std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
        BOOST_REQUIRE(ray.size() == rayReference.size());
        for (int i = 0; i < ray.size(); ++i) {
            BOOST_CHECK(isEqual(ray[i], rayReference[i]));
            // Eigen::Vector2d center = superScaledLimits.getCellCenter(ray[i]);
            // std::cout << "center: " << center.transpose() << std::endl;
        }
    }
}

BOOST_AUTO_TEST_CASE(MultiScaleSkewedLine) {
    int subPixelScale;
    const int numCellsX = 1;
    const int numCellsY = 1;
    double resolution = 0.1;
    Eigen::Vector2d max = {1.0, 1.0};
    std::vector<Eigen::Array2i> rayReference = std::vector<Eigen::Array2i>{
        Eigen::Array2i({8, 7}), Eigen::Array2i({8, 8}), Eigen::Array2i({9, 8}), Eigen::Array2i({9, 9})};
    for (subPixelScale = 1; subPixelScale < 2; subPixelScale *= 2) {
        double superscaledResolution  = resolution / subPixelScale;
        VISFS::Map::MapLimits superScaledLimits(superscaledResolution, max,
            VISFS::Map::CellLimits(numCellsX * subPixelScale, numCellsY * subPixelScale));
        Eigen::Array2i begin = superScaledLimits.getCellIndex(Eigen::Vector2d({0.01, 0.09}));
        Eigen::Array2i end = superScaledLimits.getCellIndex(Eigen::Vector2d({0.21, 0.19}));
        std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
        BOOST_REQUIRE(ray.size() == rayReference.size());
        for (int i = 0; i < ray.size(); ++i) {
            BOOST_CHECK(isEqual(ray[i], rayReference[i]));
        }
    }

    std::vector<Eigen::Array2i> rayReference2 = std::vector<Eigen::Array2i>{
        Eigen::Array2i({8, 7}), Eigen::Array2i({8, 8}), Eigen::Array2i({8, 9}), Eigen::Array2i({9, 9})};
    for (subPixelScale = 20; subPixelScale < 1000; subPixelScale *= 2) {
        double superscaledResolution  = resolution / subPixelScale;
        VISFS::Map::MapLimits superScaledLimits(superscaledResolution, max,
            VISFS::Map::CellLimits(numCellsX * subPixelScale, numCellsY * subPixelScale));
        Eigen::Array2i begin = superScaledLimits.getCellIndex(Eigen::Vector2d({0.01, 0.09}));
        Eigen::Array2i end = superScaledLimits.getCellIndex(Eigen::Vector2d({0.21, 0.19}));
        std::vector<Eigen::Array2i> ray = VISFS::Map::rayToPixelMask(begin, end, subPixelScale);
        BOOST_REQUIRE(ray.size() == rayReference2.size());
        for (int i = 0; i < ray.size(); ++i) {
            BOOST_CHECK(isEqual(ray[i], rayReference2[i]));
        }
    }
}


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

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

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointTranslation)

BOOST_AUTO_TEST_CASE(spatial)
{
  typedef TransformTranslationTpl<double,0> TransformTranslation;
  typedef SE3::Vector3 Vector3;

  const Vector3 displacement(Vector3::Random());
  SE3 Mplain, Mrand(SE3::Random());

  TransformTranslation Mtrans(displacement);
  Mplain = Mtrans;
  BOOST_CHECK(Mplain.translation().isApprox(displacement));
  BOOST_CHECK(Mplain.rotation().isIdentity());
  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mtrans));

  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionTranslation mp(MotionTranslation::Vector3(1.,2.,3.));
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelTranslation, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelTranslation,JointModelTranslation(),0,SE3::Identity(),"translation",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);

  Data dataTranslation(modelTranslation);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelTranslation.nq);
  VectorFF qff; qff << 1, 1, 1, 0, 0, 0, 1 ;
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelTranslation.nv);
  Vector6 vff; vff << 1, 1, 1, 0, 0, 0;
  Eigen::VectorXd tauTranslation = Eigen::VectorXd::Ones(modelTranslation.nv);
  Eigen::VectorXd tauff(6); tauff << 1, 1, 1, 0, 0, 0;
  Eigen::VectorXd aTranslation = Eigen::VectorXd::Ones(modelTranslation.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelTranslation, dataTranslation, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelTranslation, dataTranslation, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataTranslation.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataTranslation.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataTranslation.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataTranslation.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
                                                         dataFreeFlyer.nle[1],
                                                         dataFreeFlyer.nle[2]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataTranslation.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataTranslation.com[0]));

  // InverseDynamics == rnea
  tauTranslation = rnea(modelTranslation, dataTranslation, q, v, aTranslation);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(2);
  BOOST_CHECK(tauTranslation.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaTranslation = aba(modelTranslation,dataTranslation, q, v, tauTranslation);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
                                    aAbaFreeFlyer[1],
                                    aAbaFreeFlyer[2]
                                    ;
  BOOST_CHECK(aAbaTranslation.isApprox(a_expected));

  // crba
  crba(modelTranslation, dataTranslation,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.topLeftCorner<3,3>());

  BOOST_CHECK(dataTranslation.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_translation;jacobian_translation.resize(6,3); jacobian_translation.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelTranslation, dataTranslation, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelTranslation, dataTranslation, 1, LOCAL, jacobian_translation);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
                                                                      jacobian_ff.col(1),
                                                                      jacobian_ff.col(2)
                                                                      ;
  BOOST_CHECK(jacobian_translation.isApprox(jacobian_expected));
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright John Maddock 2006.
//  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)

#ifndef BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP
#define BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/tr1/tuple.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T>
T find_inverse_s(T p, T q)
{
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   // See equation 32.
   //
   BOOST_MATH_STD_USING
   T t;
   if(p < 0.5)
   {
      t = sqrt(-2 * log(p));
   }
   else
   {
      t = sqrt(-2 * log(q));
   }
   static const double a[4] = { 3.31125922108741, 11.6616720288968, 4.28342155967104, 0.213623493715853 };
   static const double b[5] = { 1, 6.61053765625462, 6.40691597760039, 1.27364489782223, 0.3611708101884203e-1 };
   T s = t - tools::evaluate_polynomial(a, t) / tools::evaluate_polynomial(b, t);
   if(p < 0.5)
      s = -s;
   return s;
}

template <class T>
T didonato_SN(T a, T x, unsigned N, T tolerance = 0)
{
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   // See equation 34.
   //
   T sum = 1;
   if(N >= 1)
   {
      T partial = x / (a + 1);
      sum += partial;
      for(unsigned i = 2; i <= N; ++i)
      {
         partial *= x / (a + i);
         sum += partial;
         if(partial < tolerance)
            break;
      }
   }
   return sum;
}

template <class T, class Policy>
inline T didonato_FN(T p, T a, T x, unsigned N, T tolerance, const Policy& pol)
{
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   // See equation 34.
   //
   BOOST_MATH_STD_USING
   T u = log(p) + boost::math::lgamma(a + 1, pol);
   return exp((u + x - log(didonato_SN(a, x, N, tolerance))) / a);
}

template <class T, class Policy>
T find_inverse_gamma(T a, T p, T q, const Policy& pol, bool* p_has_10_digits)
{
   //
   // In order to understand what's going on here, you will
   // need to refer to:
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   BOOST_MATH_STD_USING

   T result;
   *p_has_10_digits = false;

   if(a == 1)
   {
      result = -log(q);
      BOOST_MATH_INSTRUMENT_VARIABLE(result);
   }
   else if(a < 1)
   {
      T g = boost::math::tgamma(a, pol);
      T b = q * g;
      BOOST_MATH_INSTRUMENT_VARIABLE(g);
      BOOST_MATH_INSTRUMENT_VARIABLE(b);
      if((b > 0.6) || ((b >= 0.45) && (a >= 0.3)))
      {
         // DiDonato & Morris Eq 21:
         //
         // There is a slight variation from DiDonato and Morris here:
         // the first form given here is unstable when p is close to 1,
         // making it impossible to compute the inverse of Q(a,x) for small
         // q.  Fortunately the second form works perfectly well in this case.
         //
         T u;
         if((b * q > 1e-8) && (q > 1e-5))
         {
            u = pow(p * g * a, 1 / a);
            BOOST_MATH_INSTRUMENT_VARIABLE(u);
         }
         else
         {
            u = exp((-q / a) - constants::euler<T>());
            BOOST_MATH_INSTRUMENT_VARIABLE(u);
         }
         result = u / (1 - (u / (a + 1)));
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if((a < 0.3) && (b >= 0.35))
      {
         // DiDonato & Morris Eq 22:
         T t = exp(-constants::euler<T>() - b);
         T u = t * exp(t);
         result = t * exp(u);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if((b > 0.15) || (a >= 0.3))
      {
         // DiDonato & Morris Eq 23:
         T y = -log(b);
         T u = y - (1 - a) * log(y);
         result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u));
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if (b > 0.1)
      {
         // DiDonato & Morris Eq 24:
         T y = -log(b);
         T u = y - (1 - a) * log(y);
         result = y - (1 - a) * log(u) - log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a)) / (u * u + (5 - a) * u + 2));
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else
      {
         // DiDonato & Morris Eq 25:
         T y = -log(b);
         T c1 = (a - 1) * log(y);
         T c1_2 = c1 * c1;
         T c1_3 = c1_2 * c1;
         T c1_4 = c1_2 * c1_2;
         T a_2 = a * a;
         T a_3 = a_2 * a;

         T c2 = (a - 1) * (1 + c1);
         T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2);
         T c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6);
         T c5 = (a - 1) * (-(c1_4 / 4)
                           + (11 * a - 17) * c1_3 / 6
                           + (-3 * a_2 + 13 * a -13) * c1_2
                           + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2
                           + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12);

         T y_2 = y * y;
         T y_3 = y_2 * y;
         T y_4 = y_2 * y_2;
         result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
         if(b < 1e-28f)
            *p_has_10_digits = true;
      }
   }
   else
   {
      // DiDonato and Morris Eq 31:
      T s = find_inverse_s(p, q);

      BOOST_MATH_INSTRUMENT_VARIABLE(s);

      T s_2 = s * s;
      T s_3 = s_2 * s;
      T s_4 = s_2 * s_2;
      T s_5 = s_4 * s;
      T ra = sqrt(a);

      BOOST_MATH_INSTRUMENT_VARIABLE(ra);

      T w = a + s * ra + (s * s -1) / 3;
      w += (s_3 - 7 * s) / (36 * ra);
      w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a);
      w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra);

      BOOST_MATH_INSTRUMENT_VARIABLE(w);

      if((a >= 500) && (fabs(1 - w / a) < 1e-6))
      {
         result = w;
         *p_has_10_digits = true;
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if (p > 0.5)
      {
         if(w < 3 * a)
         {
            result = w;
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            T D = (std::max)(T(2), T(a * (a - 1)));
            T lg = boost::math::lgamma(a, pol);
            T lb = log(q) + lg;
            if(lb < -D * 2.3)
            {
               // DiDonato and Morris Eq 25:
               T y = -lb;
               T c1 = (a - 1) * log(y);
               T c1_2 = c1 * c1;
               T c1_3 = c1_2 * c1;
               T c1_4 = c1_2 * c1_2;
               T a_2 = a * a;
               T a_3 = a_2 * a;

               T c2 = (a - 1) * (1 + c1);
               T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2);
               T c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6);
               T c5 = (a - 1) * (-(c1_4 / 4)
                                 + (11 * a - 17) * c1_3 / 6
                                 + (-3 * a_2 + 13 * a -13) * c1_2
                                 + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2
                                 + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12);

               T y_2 = y * y;
               T y_3 = y_2 * y;
               T y_4 = y_2 * y_2;
               result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4);
               BOOST_MATH_INSTRUMENT_VARIABLE(result);
            }
            else
            {
               // DiDonato and Morris Eq 33:
               T u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w));
               result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u));
               BOOST_MATH_INSTRUMENT_VARIABLE(result);
            }
         }
      }
      else
      {
         T z = w;
         T ap1 = a + 1;
         if(w < 0.15f * ap1)
         {
            // DiDonato and Morris Eq 35:
            T v = log(p) + boost::math::lgamma(ap1, pol);
            T s = 1;
            z = exp((v + w) / a);
            s = boost::math::log1p(z / ap1 * (1 + z / (a + 2)));
            z = exp((v + z - s) / a);
            z = exp((v + z - s) / a);
            s = boost::math::log1p(z / ap1 * (1 + z / (a + 2) * (1 + z / (a + 3))));
            z = exp((v + z - s) / a);
            BOOST_MATH_INSTRUMENT_VARIABLE(z);
         }

         if((z <= 0.01 * ap1) || (z > 0.7 * ap1))
         {
            result = z;
            if(z <= 0.002 * ap1)
               *p_has_10_digits = true;
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            // DiDonato and Morris Eq 36:
            T ls = log(didonato_SN(a, z, 100, T(1e-4)));
            T v = log(p) + boost::math::lgamma(ap1, pol);
            z = exp((v + z - ls) / a);
            result = z * (1 - (a * log(z) - z - v + ls) / (a - z));

            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
      }
   }
   return result;
}

template <class T, class Policy>
struct gamma_p_inverse_func
{
   gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv)
   {
      //
      // If p is too near 1 then P(x) - p suffers from cancellation
      // errors causing our root-finding algorithms to "thrash", better
      // to invert in this case and calculate Q(x) - (1-p) instead.
      //
      // Of course if p is *very* close to 1, then the answer we get will
      // be inaccurate anyway (because there's not enough information in p)
      // but at least we will converge on the (inaccurate) answer quickly.
      //
      if(p > 0.9)
      {
         p = 1 - p;
         invert = !invert;
      }
   }

   std::tr1::tuple<T, T, T> operator()(const T& x)const
   {
      BOOST_FPU_EXCEPTION_GUARD
      //
      // Calculate P(x) - p and the first two derivates, or if the invert
      // flag is set, then Q(x) - q and it's derivatives.
      //
      typedef typename policies::evaluation<T, Policy>::type value_type;
      typedef typename lanczos::lanczos<T, Policy>::type evaluation_type;
      typedef typename policies::normalise<
         Policy,
         policies::promote_float<false>,
         policies::promote_double<false>,
         policies::discrete_quantile<>,
         policies::assert_undefined<> >::type forwarding_policy;

      BOOST_MATH_STD_USING  // For ADL of std functions.

      T f, f1;
      value_type ft;
      f = static_cast<T>(boost::math::detail::gamma_incomplete_imp(
               static_cast<value_type>(a),
               static_cast<value_type>(x),
               true, invert,
               forwarding_policy(), &ft));
      f1 = static_cast<T>(ft);
      T f2;
      T div = (a - x - 1) / x;
      f2 = f1;
      if((fabs(div) > 1) && (tools::max_value<T>() / fabs(div) < f2))
      {
         // overflow:
         f2 = -tools::max_value<T>() / 2;
      }
      else
      {
         f2 *= div;
      }

      if(invert)
      {
         f1 = -f1;
         f2 = -f2;
      }

      return std::tr1::make_tuple(f - p, f1, f2);
   }
private:
   T a, p;
   bool invert;
};

template <class T, class Policy>
T gamma_p_inv_imp(T a, T p, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std functions.

   static const char* function = "boost::math::gamma_p_inv<%1%>(%1%, %1%)";

   BOOST_MATH_INSTRUMENT_VARIABLE(a);
   BOOST_MATH_INSTRUMENT_VARIABLE(p);

   if(a <= 0)
      policies::raise_domain_error<T>(function, "Argument a in the incomplete gamma function inverse must be >= 0 (got a=%1%).", a, pol);
   if((p < 0) || (p > 1))
      policies::raise_domain_error<T>(function, "Probabilty must be in the range [0,1] in the incomplete gamma function inverse (got p=%1%).", p, pol);
   if(p == 1)
      return tools::max_value<T>();
   if(p == 0)
      return 0;
   bool has_10_digits;
   T guess = detail::find_inverse_gamma<T>(a, p, 1 - p, pol, &has_10_digits);
   if((policies::digits<T, Policy>() <= 36) && has_10_digits)
      return guess;
   T lower = tools::min_value<T>();
   if(guess <= lower)
      guess = tools::min_value<T>();
   BOOST_MATH_INSTRUMENT_VARIABLE(guess);
   //
   // Work out how many digits to converge to, normally this is
   // 2/3 of the digits in T, but if the first derivative is very
   // large convergence is slow, so we'll bump it up to full
   // precision to prevent premature termination of the root-finding routine.
   //
   unsigned digits = policies::digits<T, Policy>();
   if(digits < 30)
   {
      digits *= 2;
      digits /= 3;
   }
   else
   {
      digits /= 2;
      digits -= 1;
   }
   if((a < 0.125) && (fabs(gamma_p_derivative(a, guess, pol)) > 1 / sqrt(tools::epsilon<T>())))
      digits = policies::digits<T, Policy>() - 2;
   //
   // Go ahead and iterate:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   guess = tools::halley_iterate(
      detail::gamma_p_inverse_func<T, Policy>(a, p, false),
      guess,
      lower,
      tools::max_value<T>(),
      digits,
      max_iter);
   policies::check_root_iterations(function, max_iter, pol);
   BOOST_MATH_INSTRUMENT_VARIABLE(guess);
   if(guess == lower)
      guess = policies::raise_underflow_error<T>(function, "Expected result known to be non-zero, but is smaller than the smallest available number.", pol);
   return guess;
}

template <class T, class Policy>
T gamma_q_inv_imp(T a, T q, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std functions.

   static const char* function = "boost::math::gamma_q_inv<%1%>(%1%, %1%)";

   if(a <= 0)
      policies::raise_domain_error<T>(function, "Argument a in the incomplete gamma function inverse must be >= 0 (got a=%1%).", a, pol);
   if((q < 0) || (q > 1))
      policies::raise_domain_error<T>(function, "Probabilty must be in the range [0,1] in the incomplete gamma function inverse (got q=%1%).", q, pol);
   if(q == 0)
      return tools::max_value<T>();
   if(q == 1)
      return 0;
   bool has_10_digits;
   T guess = detail::find_inverse_gamma<T>(a, 1 - q, q, pol, &has_10_digits);
   if((policies::digits<T, Policy>() <= 36) && has_10_digits)
      return guess;
   T lower = tools::min_value<T>();
   if(guess <= lower)
      guess = tools::min_value<T>();
   //
   // Work out how many digits to converge to, normally this is
   // 2/3 of the digits in T, but if the first derivative is very
   // large convergence is slow, so we'll bump it up to full
   // precision to prevent premature termination of the root-finding routine.
   //
   unsigned digits = policies::digits<T, Policy>();
   if(digits < 30)
   {
      digits *= 2;
      digits /= 3;
   }
   else
   {
      digits /= 2;
      digits -= 1;
   }
   if((a < 0.125) && (fabs(gamma_p_derivative(a, guess, pol)) > 1 / sqrt(tools::epsilon<T>())))
      digits = policies::digits<T, Policy>();
   //
   // Go ahead and iterate:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   guess = tools::halley_iterate(
      detail::gamma_p_inverse_func<T, Policy>(a, q, true),
      guess,
      lower,
      tools::max_value<T>(),
      digits,
      max_iter);
   policies::check_root_iterations(function, max_iter, pol);
   if(guess == lower)
      guess = policies::raise_underflow_error<T>(function, "Expected result known to be non-zero, but is smaller than the smallest available number.", pol);
   return guess;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   gamma_p_inv(T1 a, T2 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::gamma_p_inv_imp(
      static_cast<result_type>(a),
      static_cast<result_type>(p), pol);
}

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   gamma_q_inv(T1 a, T2 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::gamma_q_inv_imp(
      static_cast<result_type>(a),
      static_cast<result_type>(p), pol);
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   gamma_p_inv(T1 a, T2 p)
{
   return gamma_p_inv(a, p, policies::policy<>());
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   gamma_q_inv(T1 a, T2 p)
{
   return gamma_q_inv(a, p, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/make_int_range_count.hpp>
#include <fcppt/tag_type.hpp>
#include <fcppt/use.hpp>
#include <fcppt/algorithm/loop.hpp>
#include <fcppt/algorithm/loop_break_mpl.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	algorithm_loop_mpl
)
{
FCPPT_PP_POP_WARNING

	int value{
		0
	};

	fcppt::algorithm::loop(
		boost::mpl::range_c<
			int,
			0,
			5
		>{},
		[
			&value
		](
			auto const _index
		)
		{
			FCPPT_USE(
				_index
			);

			typedef
			fcppt::tag_type<
				decltype(
					_index
				)
			>
			index;

			static_assert(
				index::value
				<
				5,
				""
			);

			value +=
				index::value;
		}
	);

	BOOST_CHECK_EQUAL(
		value,
		10
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	algorithm_loop_range
)
{
FCPPT_PP_POP_WARNING

	int value{
		0
	};

	fcppt::algorithm::loop(
		fcppt::make_int_range_count(
			5
		),
		[
			&value
		](
			int const _value
		)
		{
			value +=
				_value;
		}
	);

	BOOST_CHECK_EQUAL(
		value,
		10
	);
}