#include <boost/variant/apply_visitor.hpp>

#include <CGAL/Kernel/global_functions.h>

#include <jlcxx/module.hpp>

#include <julia.h>

#include "global_kernel_functions.hpp"
#include "kernel.hpp"

#define DO_INTERSECT_2(T) \
  cgal.method("do_intersect", &do_intersect<T, Point_2>); \
  cgal.method("do_intersect", &do_intersect<T, Line_2>); \
  cgal.method("do_intersect", &do_intersect<T, Segment_2>); \
  cgal.method("do_intersect", &do_intersect<T, Ray_2>); \
  cgal.method("do_intersect", &do_intersect<T, Triangle_2>); \
  cgal.method("do_intersect", &do_intersect<T, Iso_rectangle_2>)
#define DO_INTERSECT_2_SYM \
  DO_INTERSECT_2(Point_2); \
  DO_INTERSECT_2(Line_2); \
  DO_INTERSECT_2(Segment_2); \
  DO_INTERSECT_2(Ray_2); \
  DO_INTERSECT_2(Triangle_2); \
  DO_INTERSECT_2(Iso_rectangle_2)

#define INTERSECTION_2(T) \
  cgal.method("intersection", &intersection<T, Point_2>); \
  cgal.method("intersection", &intersection<T, Line_2>); \
  cgal.method("intersection", &intersection<T, Segment_2>); \
  cgal.method("intersection", &intersection<T, Ray_2>); \
  cgal.method("intersection", &intersection<T, Triangle_2>); \
  cgal.method("intersection", &intersection<T, Iso_rectangle_2>)
#define INTERSECTION_2_SYM \
  INTERSECTION_2(Point_2); \
  INTERSECTION_2(Line_2); \
  INTERSECTION_2(Segment_2); \
  INTERSECTION_2(Ray_2); \
  INTERSECTION_2(Triangle_2); \
  INTERSECTION_2(Iso_rectangle_2)

#define DO_INTERSECT_3(T) \
  cgal.method("do_intersect", &do_intersect<T, Point_3>); \
  cgal.method("do_intersect", &do_intersect<T, Tetrahedron_3>); \
  cgal.method("do_intersect", &do_intersect<T, Segment_3>); \
  cgal.method("do_intersect", &do_intersect<T, Line_3>); \
  cgal.method("do_intersect", &do_intersect<T, Plane_3>); \
  cgal.method("do_intersect", &do_intersect<T, Triangle_3>); \
  cgal.method("do_intersect", &do_intersect<T, Ray_3>); \
  cgal.method("do_intersect", &do_intersect<T, Iso_cuboid_3>); \
  cgal.method("do_intersect", &do_intersect<T, Sphere_3>); \
  cgal.method("do_intersect", &do_intersect<T, Bbox_3>)
#define DO_INTERSECT_3_ALL \
  DO_INTERSECT_3(Point_3); \
  DO_INTERSECT_3(Tetrahedron_3); \
  DO_INTERSECT_3(Segment_3); \
  DO_INTERSECT_3(Line_3); \
  DO_INTERSECT_3(Plane_3); \
  DO_INTERSECT_3(Triangle_3); \
  DO_INTERSECT_3(Ray_3); \
  DO_INTERSECT_3(Iso_cuboid_3); \
  DO_INTERSECT_3(Sphere_3); \
  DO_INTERSECT_3(Bbox_3)

#define INTERSECTION_3(T) \
  cgal.method("intersection", &intersection<T, Point_3>); \
  cgal.method("intersection", &intersection<T, Segment_3>); \
  cgal.method("intersection", &intersection<T, Line_3>); \
  cgal.method("intersection", &intersection<T, Plane_3>); \
  cgal.method("intersection", &intersection<T, Triangle_3>); \
  cgal.method("intersection", &intersection<T, Ray_3>)
#define INTERSECTION_3_SYM \
  INTERSECTION_3(Point_3); \
  INTERSECTION_3(Segment_3); \
  INTERSECTION_3(Line_3); \
  INTERSECTION_3(Plane_3); \
  INTERSECTION_3(Triangle_3); \
  INTERSECTION_3(Ray_3)

#define SQUARED_DISTANCE_2(T) \
  cgal.method("squared_distance", &squared_distance<T, Point_2>); \
  cgal.method("squared_distance", &squared_distance<T, Line_2>); \
  cgal.method("squared_distance", &squared_distance<T, Ray_2>); \
  cgal.method("squared_distance", &squared_distance<T, Segment_2>); \
  cgal.method("squared_distance", &squared_distance<T, Triangle_2>)
#define SQUARED_DISTANCE_2_ALL \
  SQUARED_DISTANCE_2(Point_2); \
  SQUARED_DISTANCE_2(Line_2); \
  SQUARED_DISTANCE_2(Ray_2); \
  SQUARED_DISTANCE_2(Segment_2); \
  SQUARED_DISTANCE_2(Triangle_2)

#define SQUARED_DISTANCE_3(T) \
  cgal.method("squared_distance", &squared_distance<T, Point_3>); \
  cgal.method("squared_distance", &squared_distance<T, Segment_3>); \
  cgal.method("squared_distance", &squared_distance<T, Line_3>); \
  cgal.method("squared_distance", &squared_distance<T, Plane_3>); \
  cgal.method("squared_distance", &squared_distance<T, Ray_3>)
#define SQUARED_DISTANCE_3_SYM \
  SQUARED_DISTANCE_3(Point_3); \
  SQUARED_DISTANCE_3(Segment_3); \
  SQUARED_DISTANCE_3(Line_3); \
  SQUARED_DISTANCE_3(Plane_3); \
  SQUARED_DISTANCE_3(Ray_3)

namespace jlcgal {

template<typename T1, typename T2 = T1>
inline
bool
do_intersect(const T1& t1, const T2& t2) {
  return CGAL::do_intersect(t1, t2);
}

template<typename T1, typename T2 = T1>
inline
jl_value_t*
intersection(const T1& t1, const T2& t2) {
  auto result = CGAL::intersection(t1, t2);
  return result ?
    boost::apply_visitor(Intersection_visitor(), *result) :
    jl_nothing;
}

template<typename T1, typename T2 = T1>
inline
FT
squared_distance(const T1& t1, const T2& t2) {
  return CGAL::squared_distance(t1, t2);
}

void wrap_global_lk_functions(jlcxx::Module& cgal) {
  cgal.set_override_module(jl_base_module);
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Vector_2&, const Vector_2&)>(&CGAL::angle));
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Point_2&,  const Point_2&, const Point_2&)>(&CGAL::angle));
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Point_2&,  const Point_2&, const Point_2&, const Point_2&)>(&CGAL::angle));
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Vector_3&, const Vector_3&)>(&CGAL::angle));
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Point_3&,  const Point_3&, const Point_3&)>(&CGAL::angle));
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Point_3&,  const Point_3&, const Point_3&, const Point_3&)>(&CGAL::angle));
  cgal.method("angle", static_cast<CGAL::Angle(*)(const Point_3&,  const Point_3&, const Point_3&, const Vector_3&)>(&CGAL::angle));
  cgal.unset_override_module();

  cgal.method("approximate_angle", static_cast<FT(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::approximate_angle));
  cgal.method("approximate_angle", static_cast<FT(*)(const Vector_3&, const Vector_3&)>(&CGAL::approximate_angle));

  cgal.method("approximate_dihedral_angle", static_cast<FT(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::approximate_dihedral_angle));

  cgal.method("area", static_cast<FT(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::area));

  cgal.method("are_ordered_along_line", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::are_ordered_along_line));
  cgal.method("are_ordered_along_line", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::are_ordered_along_line));

  cgal.method("are_strictly_ordered_along_line", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::are_strictly_ordered_along_line));
  cgal.method("are_strictly_ordered_along_line", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::are_strictly_ordered_along_line));

  cgal.method("barycenter", static_cast<Point_2(*)(const Point_2&, const FT&, const Point_2&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_2(*)(const Point_2&, const FT&, const Point_2&, const FT&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_2(*)(const Point_2&, const FT&, const Point_2&, const FT&, const Point_2&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_2(*)(const Point_2&, const FT&, const Point_2&, const FT&, const Point_2&, const FT&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_2(*)(const Point_2&, const FT&, const Point_2&, const FT&, const Point_2&, const FT&, const Point_2&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_2(*)(const Point_2&, const FT&, const Point_2&, const FT&, const Point_2&, const FT&, const Point_2&, const FT&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_3(*)(const Point_3&, const FT&, const Point_3&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_3(*)(const Point_3&, const FT&, const Point_3&, const FT&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_3(*)(const Point_3&, const FT&, const Point_3&, const FT&, const Point_3&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_3(*)(const Point_3&, const FT&, const Point_3&, const FT&, const Point_3&, const FT&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_3(*)(const Point_3&, const FT&, const Point_3&, const FT&, const Point_3&, const FT&, const Point_3&)>(&CGAL::barycenter));
  cgal.method("barycenter", static_cast<Point_3(*)(const Point_3&, const FT&, const Point_3&, const FT&, const Point_3&, const FT&, const Point_3&, const FT&)>(&CGAL::barycenter));

  cgal.method("bisector", static_cast<Line_2(*)(const Point_2&, const Point_2&)>(&CGAL::bisector));
  cgal.method("bisector", static_cast<Line_2(*)(const Line_2&,  const Line_2&)>(&CGAL::bisector));
  cgal.method("bisector", static_cast<Plane_3(*)(const Point_3&, const Point_3&)>(&CGAL::bisector));
  cgal.method("bisector", static_cast<Plane_3(*)(const Plane_3&,  const Plane_3&)>(&CGAL::bisector));

  cgal.method("centroid", static_cast<Point_2(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::centroid));
  cgal.method("centroid", static_cast<Point_2(*)(const Point_2&, const Point_2&, const Point_2&, const Point_2&)>(&CGAL::centroid));
  cgal.method("centroid", static_cast<Point_2(*)(const Triangle_2&)>(&CGAL::centroid));
  cgal.method("centroid", static_cast<Point_3(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::centroid));
  cgal.method("centroid", static_cast<Point_3(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::centroid));
  cgal.method("centroid", static_cast<Point_3(*)(const Triangle_3&)>(&CGAL::centroid));
  cgal.method("centroid", static_cast<Point_3(*)(const Tetrahedron_3&)>(&CGAL::centroid));

  cgal.method("circumcenter", static_cast<Point_2(*)(const Point_2&, const Point_2&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_2(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_2(*)(const Triangle_2&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_3(*)(const Point_3&, const Point_3&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_3(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_3(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_3(*)(const Triangle_3&)>(&CGAL::circumcenter));
  cgal.method("circumcenter", static_cast<Point_3(*)(const Tetrahedron_3&)>(&CGAL::circumcenter));

  cgal.method("collinear_are_ordered_along_line", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::collinear_are_ordered_along_line));
  cgal.method("collinear_are_ordered_along_line", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::collinear_are_ordered_along_line));

  cgal.method("collinear_are_strictly_ordered_along_line", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::collinear_are_strictly_ordered_along_line));
  cgal.method("collinear_are_strictly_ordered_along_line", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::collinear_are_strictly_ordered_along_line));

  cgal.method("collinear", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::collinear));
  cgal.method("collinear", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::collinear));

  cgal.method("compare_dihedral_angle", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const FT&)>(&CGAL::compare_dihedral_angle));
  cgal.method("compare_dihedral_angle", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::compare_dihedral_angle));
  cgal.method("compare_dihedral_angle", static_cast<CGAL::Comparison_result(*)(const Vector_3&, const Vector_3&, const Vector_3&, const FT&)>(&CGAL::compare_dihedral_angle));
  cgal.method("compare_dihedral_angle", static_cast<CGAL::Comparison_result(*)(const Vector_3&, const Vector_3&, const Vector_3&, const Vector_3&, const Vector_3&, const Vector_3&)>(&CGAL::compare_dihedral_angle));

  cgal.method("compare_distance_to_point", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::compare_distance_to_point));
  cgal.method("compare_distance_to_point", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::compare_distance_to_point));

  cgal.method("compare_lexicographically", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&)>(&CGAL::compare_lexicographically));
  cgal.method("compare_lexicographically", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&)>(&CGAL::compare_lexicographically));

  cgal.method("compare_signed_distance_to_line", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Point_2&, const Point_2&)>(&CGAL::compare_signed_distance_to_line));
  cgal.method("compare_signed_distance_to_line", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&, const Point_2&, const Point_2&)>(&CGAL::compare_signed_distance_to_line));

  cgal.method("compare_signed_distance_to_plane", static_cast<CGAL::Comparison_result(*)(const Plane_3&,  const Point_3&, const Point_3&)>(&CGAL::compare_signed_distance_to_plane));
  cgal.method("compare_signed_distance_to_plane", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::compare_signed_distance_to_plane));

  cgal.method("compare_slope", static_cast<CGAL::Comparison_result(*)(const Line_2&,    const Line_2&)>(&CGAL::compare_slope));
  cgal.method("compare_slope", static_cast<CGAL::Comparison_result(*)(const Segment_2&, const Segment_2&)>(&CGAL::compare_slope));
  cgal.method("compare_slope", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::compare_slope));

  cgal.method("compare_squared_distance", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&, const FT&)>(&CGAL::compare_squared_distance));
  cgal.method("compare_squared_distance", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const FT&)>(&CGAL::compare_squared_distance));

  cgal.method("compare_squared_radius", static_cast<CGAL::Comparison_result(*)(const Point_3&, const FT&)>(&CGAL::compare_squared_radius));
  cgal.method("compare_squared_radius", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const FT&)>(&CGAL::compare_squared_radius));
  cgal.method("compare_squared_radius", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&, const FT&)>(&CGAL::compare_squared_radius));
  cgal.method("compare_squared_radius", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const FT&)>(&CGAL::compare_squared_radius));

  cgal.method("compare_x", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&)>(&CGAL::compare_x));
  cgal.method("compare_x", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&)>(&CGAL::compare_x));
  cgal.method("compare_x", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Line_2&, const Line_2&)>(&CGAL::compare_x));
  cgal.method("compare_x", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&)>(&CGAL::compare_x));
  cgal.method("compare_x", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&, const Line_2&)>(&CGAL::compare_x));

  cgal.method("compare_xy", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&)>(&CGAL::compare_xy));

  cgal.method("compare_x_at_y", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Line_2&)>(&CGAL::compare_x_at_y));
  cgal.method("compare_x_at_y", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Line_2&, const Line_2&)>(&CGAL::compare_x_at_y));
  cgal.method("compare_x_at_y", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&)>(&CGAL::compare_x_at_y));
  cgal.method("compare_x_at_y", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&, const Line_2&)>(&CGAL::compare_x_at_y));

  cgal.method("compare_y_at_x", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Line_2&)>(&CGAL::compare_y_at_x));
  cgal.method("compare_y_at_x", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Line_2&, const Line_2&)>(&CGAL::compare_y_at_x));
  cgal.method("compare_y_at_x", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&)>(&CGAL::compare_y_at_x));
  cgal.method("compare_y_at_x", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&, const Line_2&)>(&CGAL::compare_y_at_x));
  cgal.method("compare_y_at_x", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Segment_2&)>(&CGAL::compare_y_at_x));
  cgal.method("compare_y_at_x", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Segment_2&, const Segment_2&)>(&CGAL::compare_y_at_x));

  cgal.method("compare_y", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&)>(&CGAL::compare_y));
  cgal.method("compare_y", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&)>(&CGAL::compare_y));
  cgal.method("compare_y", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Line_2&, const Line_2&)>(&CGAL::compare_y));
  cgal.method("compare_y", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&)>(&CGAL::compare_y));
  cgal.method("compare_y", static_cast<CGAL::Comparison_result(*)(const Line_2&,  const Line_2&, const Line_2&, const Line_2&)>(&CGAL::compare_y));

  cgal.method("compare_xyz", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&)>(&CGAL::compare_xyz));

  cgal.method("compare_z", static_cast<CGAL::Comparison_result(*)(const Point_3&, const Point_3&)>(&CGAL::compare_z));

  cgal.method("compare_yx", static_cast<CGAL::Comparison_result(*)(const Point_2&, const Point_2&)>(&CGAL::compare_yx));

  cgal.method("coplanar", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::coplanar));

  cgal.method("coplanar_orientation", static_cast<CGAL::Orientation(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::coplanar_orientation));
  cgal.method("coplanar_orientation", static_cast<CGAL::Orientation(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::coplanar_orientation));

  cgal.method("coplanar_side_of_bounded_circle", static_cast<CGAL::Bounded_side(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::coplanar_side_of_bounded_circle));

  cgal.method("cross_product", static_cast<Vector_3(*)(const Vector_3&, const Vector_3&)>(&CGAL::cross_product));

  cgal.method("determinant", static_cast<FT(*)(const Vector_2&, const Vector_2&)>(&CGAL::determinant));
  cgal.method("determinant", static_cast<FT(*)(const Vector_3&, const Vector_3&, const Vector_3&)>(&CGAL::determinant));

  DO_INTERSECT_2_SYM;
  cgal.method("do_intersect", &do_intersect<Circle_2>);
  cgal.method("do_intersect", &do_intersect<Circle_2, Point_2>);
  cgal.method("do_intersect", &do_intersect<Circle_2, Line_2>);
  cgal.method("do_intersect", &do_intersect<Circle_2, Iso_rectangle_2>);
  cgal.method("do_intersect", &do_intersect<Circle_2, Bbox_2>);
  cgal.method("do_intersect", &do_intersect<Bbox_2, Point_2>);
  cgal.method("do_intersect", &do_intersect<Bbox_2, Line_2>);
  cgal.method("do_intersect", &do_intersect<Bbox_2, Ray_2>);
  cgal.method("do_intersect", &do_intersect<Bbox_2, Circle_2>);
  cgal.method("do_intersect", &do_intersect<Point_2, Circle_2>);
  cgal.method("do_intersect", &do_intersect<Point_2, Bbox_2>);
  cgal.method("do_intersect", &do_intersect<Line_2, Circle_2>);
  cgal.method("do_intersect", &do_intersect<Line_2, Bbox_2>);
  cgal.method("do_intersect", &do_intersect<Ray_2, Bbox_2>);
  cgal.method("do_intersect", &do_intersect<Iso_rectangle_2, Circle_2>);

  DO_INTERSECT_3_ALL;

  cgal.method("has_larger_distance_to_point", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::has_larger_distance_to_point));
  cgal.method("has_larger_distance_to_point", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::has_larger_distance_to_point));

  cgal.method("has_larger_signed_distance_to_line", static_cast<bool(*)(const Line_2&,  const Point_2&, const Point_2&)>(&CGAL::has_larger_signed_distance_to_line));
  cgal.method("has_larger_signed_distance_to_line", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&, const Point_2&)>(&CGAL::has_larger_signed_distance_to_line));

  cgal.method("has_larger_signed_distance_to_plane", static_cast<bool(*)(const Plane_3&,  const Point_3&, const Point_3&)>(&CGAL::has_larger_signed_distance_to_plane));
  cgal.method("has_larger_signed_distance_to_plane", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::has_larger_signed_distance_to_plane));

  cgal.method("has_smaller_distance_to_point", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::has_smaller_distance_to_point));
  cgal.method("has_smaller_distance_to_point", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::has_smaller_distance_to_point));

  cgal.method("has_smaller_signed_distance_to_line", static_cast<bool(*)(const Line_2&,  const Point_2&, const Point_2&)>(&CGAL::has_smaller_signed_distance_to_line));
  cgal.method("has_smaller_signed_distance_to_line", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&, const Point_2&)>(&CGAL::has_smaller_signed_distance_to_line));

  cgal.method("has_smaller_signed_distance_to_plane", static_cast<bool(*)(const Plane_3&,  const Point_3&, const Point_3&)>(&CGAL::has_smaller_signed_distance_to_plane));
  cgal.method("has_smaller_signed_distance_to_plane", static_cast<bool(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::has_smaller_signed_distance_to_plane));

  INTERSECTION_2_SYM;
  cgal.method("intersection", &intersection<Point_2, Circle_2>);
  cgal.method("intersection", &intersection<Circle_2, Point_2>);
  cgal.method("intersection", &intersection<Point_2, Bbox_2>);
  cgal.method("intersection", &intersection<Bbox_2, Point_2>);

  INTERSECTION_3_SYM;
  cgal.method("intersection", &intersection<Point_3, Tetrahedron_3>);
  cgal.method("intersection", &intersection<Point_3, Iso_cuboid_3>);
  cgal.method("intersection", &intersection<Point_3, Sphere_3>);
  cgal.method("intersection", &intersection<Point_3, Bbox_3>);
  cgal.method("intersection", &intersection<Tetrahedron_3, Point_3>);
  cgal.method("intersection", &intersection<Segment_3, Iso_cuboid_3>);
  cgal.method("intersection", &intersection<Segment_3, Bbox_3>);
  cgal.method("intersection", &intersection<Line_3, Iso_cuboid_3>);
  cgal.method("intersection", &intersection<Line_3, Bbox_3>);
  cgal.method("intersection", &intersection<Plane_3, Sphere_3>);
  cgal.method("intersection", &intersection<Triangle_3, Iso_cuboid_3>);
  cgal.method("intersection", &intersection<Ray_3, Iso_cuboid_3>);
  cgal.method("intersection", &intersection<Ray_3, Bbox_3>);
  cgal.method("intersection", &intersection<Iso_cuboid_3>);
  cgal.method("intersection", &intersection<Iso_cuboid_3, Point_3>);
  cgal.method("intersection", &intersection<Iso_cuboid_3, Segment_3>);
  cgal.method("intersection", &intersection<Iso_cuboid_3, Line_3>);
  cgal.method("intersection", &intersection<Iso_cuboid_3, Triangle_3>);
  cgal.method("intersection", &intersection<Iso_cuboid_3, Ray_3>);
  cgal.method("intersection", &intersection<Sphere_3>);
  cgal.method("intersection", &intersection<Sphere_3, Point_3>);
  cgal.method("intersection", &intersection<Sphere_3, Plane_3>);
  cgal.method("intersection", &intersection<Bbox_3, Point_3>);
  cgal.method("intersection", &intersection<Bbox_3, Segment_3>);
  cgal.method("intersection", &intersection<Bbox_3, Line_3>);
  cgal.method("intersection", &intersection<Bbox_3, Ray_3>);

  cgal.method("l_infinity_distance", static_cast<FT(*)(const Point_2&, const Point_2&)>(&CGAL::l_infinity_distance));
  cgal.method("l_infinity_distance", static_cast<FT(*)(const Point_3&, const Point_3&)>(&CGAL::l_infinity_distance));

  cgal.method("left_turn", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::left_turn));

  cgal.method("lexicographically_xy_larger", static_cast<bool(*)(const Point_2&, const Point_2&)>(&CGAL::lexicographically_xy_larger));

  cgal.method("lexicographically_xy_larger_or_equal", static_cast<bool(*)(const Point_2&, const Point_2&)>(&CGAL::lexicographically_xy_larger_or_equal));

  cgal.method("lexicographically_xy_smaller", static_cast<bool(*)(const Point_2&, const Point_2&)>(&CGAL::lexicographically_xy_smaller));

  cgal.method("lexicographically_xy_smaller_or_equal", static_cast<bool(*)(const Point_2&, const Point_2&)>(&CGAL::lexicographically_xy_smaller_or_equal));

  cgal.method("lexicographically_xyz_smaller", static_cast<bool(*)(const Point_3&, const Point_3&)>(&CGAL::lexicographically_xyz_smaller));

  cgal.method("lexicographically_xyz_smaller_or_equal", static_cast<bool(*)(const Point_3&, const Point_3&)>(&CGAL::lexicographically_xyz_smaller_or_equal));

  cgal.method("max_vertex", static_cast<Point_2(*)(const Iso_rectangle_2&)>(&CGAL::max_vertex));
  cgal.method("max_vertex", static_cast<Point_3(*)(const Iso_cuboid_3&)>(&CGAL::max_vertex));

  cgal.method("midpoint", static_cast<Point_2(*)(const Point_2&, const Point_2&)>(&CGAL::midpoint));
  cgal.method("midpoint", static_cast<Point_3(*)(const Point_3&, const Point_3&)>(&CGAL::midpoint));

  cgal.method("min_vertex", static_cast<Point_2(*)(const Iso_rectangle_2&)>(&CGAL::min_vertex));
  cgal.method("min_vertex", static_cast<Point_3(*)(const Iso_cuboid_3&)>(&CGAL::min_vertex));

  cgal.method("normal", static_cast<Vector_3(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::normal));

  cgal.method("orientation", static_cast<CGAL::Orientation(*)(const Point_2&,  const Point_2&, const Point_2&)>(&CGAL::orientation));
  cgal.method("orientation", static_cast<CGAL::Orientation(*)(const Vector_2&, const Vector_2&)>(&CGAL::orientation));
  cgal.method("orientation", static_cast<CGAL::Orientation(*)(const Point_3&,  const Point_3&, const Point_3&, const Point_3&)>(&CGAL::orientation));
  cgal.method("orientation", static_cast<CGAL::Orientation(*)(const Vector_3&, const Vector_3&, const Vector_3&)>(&CGAL::orientation));

  cgal.method("orthogonal_vector", static_cast<Vector_3(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::orthogonal_vector));

  cgal.method("parallel", static_cast<bool(*)(const Line_2&, const Line_2&)>(&CGAL::parallel));
  cgal.method("parallel", static_cast<bool(*)(const Ray_2&, const Ray_2&)>(&CGAL::parallel));
  cgal.method("parallel", static_cast<bool(*)(const Segment_2&, const Segment_2&)>(&CGAL::parallel));
  cgal.method("parallel", static_cast<bool(*)(const Line_3&, const Line_3&)>(&CGAL::parallel));
  cgal.method("parallel", static_cast<bool(*)(const Plane_3&, const Plane_3&)>(&CGAL::parallel));
  cgal.method("parallel", static_cast<bool(*)(const Ray_3&, const Ray_3&)>(&CGAL::parallel));
  cgal.method("parallel", static_cast<bool(*)(const Segment_3&, const Segment_3&)>(&CGAL::parallel));

  cgal.method("radical_plane", static_cast<Plane_3(*)(const Sphere_3&, const Sphere_3&)>(&CGAL::radical_plane));

  cgal.method("radical_line", static_cast<Line_2(*)(const Circle_2&, const Circle_2&)>(&CGAL::radical_line));

  cgal.method("rational_rotation_approximation", static_cast<void(*)(const RT&, const RT&, RT&, RT&, RT&, const RT&, const RT&)>(&CGAL::rational_rotation_approximation));

  cgal.method("right_turn", static_cast<bool(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::right_turn));

  cgal.method("scalar_product", static_cast<FT(*)(const Vector_2&, const Vector_2&)>(&CGAL::scalar_product));
  cgal.method("scalar_product", static_cast<FT(*)(const Vector_3&, const Vector_3&)>(&CGAL::scalar_product));

  cgal.method("side_of_bounded_circle", static_cast<CGAL::Bounded_side(*)(const Point_2&, const Point_2&, const Point_2&, const Point_2&)>(&CGAL::side_of_bounded_circle));
  cgal.method("side_of_bounded_circle", static_cast<CGAL::Bounded_side(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::side_of_bounded_circle));

  cgal.method("side_of_bounded_sphere", static_cast<CGAL::Bounded_side(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::side_of_bounded_sphere));
  cgal.method("side_of_bounded_sphere", static_cast<CGAL::Bounded_side(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::side_of_bounded_sphere));
  cgal.method("side_of_bounded_sphere", static_cast<CGAL::Bounded_side(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::side_of_bounded_sphere));

  cgal.method("side_of_oriented_circle", static_cast<CGAL::Oriented_side(*)(const Point_2&, const Point_2&, const Point_2&, const Point_2&)>(&CGAL::side_of_oriented_circle));

  cgal.method("side_of_oriented_sphere", static_cast<CGAL::Oriented_side(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::side_of_oriented_sphere));

  cgal.method("squared_area", static_cast<FT(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::squared_area));

  SQUARED_DISTANCE_2_ALL;

  SQUARED_DISTANCE_3_SYM;
  cgal.method("squared_distance", &squared_distance<Point_3, Triangle_3>);
  cgal.method("squared_distance", &squared_distance<Triangle_3, Point_3>);

  cgal.method("squared_radius", static_cast<FT(*)(const Point_2&, const Point_2&, const Point_2&)>(&CGAL::squared_radius));
  cgal.method("squared_radius", static_cast<FT(*)(const Point_2&, const Point_2&)>(&CGAL::squared_radius));
  cgal.method("squared_radius", static_cast<FT(*)(const Point_2&)>(&CGAL::squared_radius));
  cgal.method("squared_radius", static_cast<FT(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::squared_radius));
  cgal.method("squared_radius", static_cast<FT(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::squared_radius));
  cgal.method("squared_radius", static_cast<FT(*)(const Point_3&, const Point_3&)>(&CGAL::squared_radius));
  cgal.method("squared_radius", static_cast<FT(*)(const Point_3&)>(&CGAL::squared_radius));

  cgal.method("unit_normal", static_cast<Vector_3(*)(const Point_3&, const Point_3&, const Point_3&)>(&CGAL::unit_normal));

  cgal.method("volume", static_cast<FT(*)(const Point_3&, const Point_3&, const Point_3&, const Point_3&)>(&CGAL::volume));

  cgal.method("x_equal", static_cast<bool(*)(const Point_2&, const Point_2&)>(&CGAL::x_equal));
  cgal.method("x_equal", static_cast<bool(*)(const Point_3&, const Point_3&)>(&CGAL::x_equal));
  cgal.method("y_equal", static_cast<bool(*)(const Point_2&, const Point_2&)>(&CGAL::y_equal));
  cgal.method("y_equal", static_cast<bool(*)(const Point_3&, const Point_3&)>(&CGAL::y_equal));
  cgal.method("z_equal", static_cast<bool(*)(const Point_3&, const Point_3&)>(&CGAL::z_equal));

  cgal.method("do_overlap", static_cast<bool(*)(const Bbox_2&, const Bbox_2&)>(&CGAL::do_overlap));
  cgal.method("do_overlap", static_cast<bool(*)(const Bbox_3&, const Bbox_3&)>(&CGAL::do_overlap));
}

} // jlcgal

#undef DO_INTERSECT_2
#undef DO_INTERSECT_2_SYM

#undef INTERSECTION_2
#undef INTERSECTION_2_SYM

#undef DO_INTERSECT_3
#undef DO_INTERSECT_3_ALL

#undef INTERSECTION_3
#undef INTERSECTION_3_SYM

#undef SQUARED_DISTANCE_2
#undef SQUARED_DISTANCE_2_ALL

#undef SQUARED_DISTANCE_3
#undef SQUARED_DISTANCE_3_SYM


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_DETAIL_COPY_HPP
#define AMA_TENSOR_DETAIL_COPY_HPP 1

#include <boost/mpl/bool.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/modulus.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/vector/vector0_c.hpp>

namespace ama
{
  namespace tensor_
  {

    namespace mpl = ::boost::mpl;

    /* given the order this struct create the first index */
    template <typename O>
    struct first:
        mpl::fold<
              mpl::range_c<size_t, 0, O::value>
            , mpl::vector0_c<size_t>
            , mpl::push_back< mpl::_1 , mpl::size_t<0> >
            >::type { };

     /* increment the multi-index */
     template <
           typename D /* the dimension */
         , typename I /* the multi-index */
         >
     struct increment:
         mpl::fold<
               I
             , mpl::pair<                  /* a pair with:           */
                   mpl::vector0_c<size_t>  /*  - the multi-index     */
                 , mpl::true_              /*  - an the carry of sum */
                 >
             , mpl::pair<
                   mpl::push_back<
                       mpl::first<mpl::_1>
                     , mpl::modulus<
                           mpl::if_<
                               mpl::second<mpl::_1>
                             , mpl::next<mpl::_2>
                             , mpl::_2
                             >
                         , D
                         >
                     >
                 , mpl::equal_to<
                       mpl::if_<
                           mpl::second<mpl::_1>
                         , mpl::next<mpl::_2>
                         , mpl::_2
                         >
                     , D
                     >
                 >
             > { };

     /* this struct implement the copy */
     template <
           typename D               /* the dimensione of tensor */
         , typename O               /* the order of tensor */
         , typename I = first<O>    /* the first multi-index */
         , typename R = mpl::false_ /* this type is true for the last multi-index */
         >
     struct copy
     {
       template <typename SRC, typename DST>
       static void apply(SRC const & src, DST & dst)
       {
         dst.template at<I>() = src.template at<I>();

         /* increment the multi-index */
         typedef typename increment<D,I>::type increment_type;

         /* the first is the multi-index incremented */
         typedef typename mpl::first<increment_type>::type i;
         /* the second is a boolean flag toidentify the last multi-index */
         typedef typename mpl::second<increment_type>::type r;

         /* iterative call */
         copy<D,O,i,r>::apply(src,dst);
       }
     };

     /* partial specialization, end the iterative call */
     template <typename D, typename O, typename I>
     struct copy<D,O,I,mpl::true_>
     {
       template <typename SRC, typename DST>
       static void apply(SRC const &, DST &) { }
     };

  }
}

#endif /* AMA_TENSOR_DETAIL_COPY_HPP */


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifndef UUID_5265FC7CA1C011DE9EBDFFA956D89593
#define UUID_5265FC7CA1C011DE9EBDFFA956D89593

#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/assert.hpp>
#include <boost/qvm/enable_if.hpp>

namespace
boost
    {
    namespace
    qvm
        {
        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int Col,class OriginalMatrix>
            class
            col_
                {
                col_( col_ const & );
                col_ & operator=( col_ const & );
                ~col_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                col_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <int Col,class OriginalMatrix>
        struct
        vec_traits< qvm_detail::col_<Col,OriginalMatrix> >
            {
            typedef qvm_detail::col_<Col,OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=mat_traits<OriginalMatrix>::rows;
            BOOST_QVM_STATIC_ASSERT(Col>=0);
            BOOST_QVM_STATIC_ASSERT(Col<mat_traits<OriginalMatrix>::cols);

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<I,Col>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<I,Col>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(i,Col,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(i,Col,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <int Col,class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::col_<Col,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Col,class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::col_<Col,OriginalMatrix>,qvm_detail::col_<Col,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Col,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::col_<Col,A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        col( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::col_<Col,A> const &>(a);
            }

        template <int Col,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::col_<Col,A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        col( A & a )
            {
            return reinterpret_cast<typename qvm_detail::col_<Col,A> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int Row,class OriginalMatrix>
            class
            row_
                {
                row_( row_ const & );
                row_ & operator=( row_ const & );
                ~row_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                row_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <int Row,class OriginalMatrix>
        struct
        vec_traits< qvm_detail::row_<Row,OriginalMatrix> >
            {
            typedef qvm_detail::row_<Row,OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=mat_traits<OriginalMatrix>::cols;
            BOOST_QVM_STATIC_ASSERT(Row>=0);
            BOOST_QVM_STATIC_ASSERT(Row<mat_traits<OriginalMatrix>::rows);

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<Row,I>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<Row,I>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(Row,i,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(Row,i,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <int Row,class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::row_<Row,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Row,class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::row_<Row,OriginalMatrix>,qvm_detail::row_<Row,OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <int Row,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::row_<Row,A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        row( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::row_<Row,A> const &>(a);
            }

        template <int Row,class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::row_<Row,A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        row( A & a )
            {
            return reinterpret_cast<typename qvm_detail::row_<Row,A> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class OriginalMatrix>
            class
            diag_
                {
                diag_( diag_ const & );
                diag_ & operator=( diag_ const & );
                ~diag_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                diag_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };

            template <int X,int Y,bool Which>
            struct diag_bool_dispatch;

            template <int X,int Y>
            struct
            diag_bool_dispatch<X,Y,true>
                {
                static int const value=X;
                };

            template <int X,int Y>
            struct
            diag_bool_dispatch<X,Y,false>
                {
                static int const value=Y;
                };
            }

        template <class OriginalMatrix>
        struct
        vec_traits< qvm_detail::diag_<OriginalMatrix> >
            {
            typedef qvm_detail::diag_<OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=qvm_detail::diag_bool_dispatch<
                    mat_traits<OriginalMatrix>::rows,
                    mat_traits<OriginalMatrix>::cols,
                    mat_traits<OriginalMatrix>::rows<=mat_traits<OriginalMatrix>::cols>::value;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<I,I>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<I,I>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(i,i,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(i,i,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::diag_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::diag_<OriginalMatrix>,qvm_detail::diag_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::diag_<A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        diag( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::diag_<A> const &>(a);
            }

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value,
            qvm_detail::diag_<A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        diag( A & a )
            {
            return reinterpret_cast<typename qvm_detail::diag_<A> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class OriginalMatrix>
            class
            translation_
                {
                translation_( translation_ const & );
                ~translation_();

                public:

                translation_ &
                operator=( translation_ const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                translation_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class OriginalMatrix>
        struct
        vec_traits< qvm_detail::translation_<OriginalMatrix> >
            {
            typedef qvm_detail::translation_<OriginalMatrix> this_vector;
            typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
            static int const dim=mat_traits<OriginalMatrix>::rows-1;
            BOOST_QVM_STATIC_ASSERT(mat_traits<OriginalMatrix>::rows==mat_traits<OriginalMatrix>::cols);
            BOOST_QVM_STATIC_ASSERT(mat_traits<OriginalMatrix>::rows>=3);

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template read_element<I,dim>(reinterpret_cast<OriginalMatrix const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return mat_traits<OriginalMatrix>::template write_element<I,dim>(reinterpret_cast<OriginalMatrix &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::read_element_idx(i,dim,reinterpret_cast<OriginalMatrix const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return mat_traits<OriginalMatrix>::write_element_idx(i,dim,reinterpret_cast<OriginalMatrix &>(x));
                }
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec<qvm_detail::translation_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class OriginalMatrix,int D>
        struct
        deduce_vec2<qvm_detail::translation_<OriginalMatrix>,qvm_detail::translation_<OriginalMatrix>,D>
            {
            typedef vec<typename mat_traits<OriginalMatrix>::scalar_type,D> type;
            };

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value && mat_traits<A>::rows==mat_traits<A>::cols && mat_traits<A>::rows>=3,
            qvm_detail::translation_<A> const &>::type
        BOOST_QVM_INLINE_TRIVIAL
        translation( A const & a )
            {
            return reinterpret_cast<typename qvm_detail::translation_<A> const &>(a);
            }

        template <class A>
        typename boost::enable_if_c<
            is_mat<A>::value && mat_traits<A>::rows==mat_traits<A>::cols && mat_traits<A>::rows>=3,
            qvm_detail::translation_<A> &>::type
        BOOST_QVM_INLINE_TRIVIAL
        translation( A & a )
            {
            return reinterpret_cast<typename qvm_detail::translation_<A> &>(a);
            }

        ////////////////////////////////////////////////
        }
    }

#endif


 /*
 *  Copyright 2007-2015 The OpenMx Project
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *       http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include "omxExpectation.h"
#include "omxFitFunction.h"
#include "omxDefines.h"
#include "omxGREMLExpectation.h"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Dense>

void omxInitGREMLExpectation(omxExpectation* ox){

  SEXP rObj = ox->rObj;
  SEXP Rmtx, casesToDrop, yXcolnames;
  int i=0;
  omxState* currentState = ox->currentState;

  if(OMX_DEBUG) { mxLog("Initializing GREML expectation."); }

  //omxGREMLExpectation *oge = (omxGREMLExpectation*) R_alloc(1, sizeof(omxGREMLExpectation));
  omxGREMLExpectation *oge = new omxGREMLExpectation;

  /* Set Expectation Calls and Structures */
  ox->computeFun = omxComputeGREMLExpectation;
	ox->destructFun = omxDestroyGREMLExpectation;
	ox->componentFun = omxGetGREMLExpectationComponent;
	ox->populateAttrFun = omxPopulateGREMLAttributes;
	ox->argStruct = (void*) oge;

    /* Set up expectation structures */
  //y:
  oge->y = new omxData();
  {ScopedProtect p1(Rmtx, R_do_slot(rObj, Rf_install("y")));
	  oge->y->newDataStatic(currentState, Rmtx);
  }
  //V:
	if(OMX_DEBUG) { mxLog("Processing V."); }
	oge->cov = omxNewMatrixFromSlot(rObj, currentState, "V");
  if( oge->cov->rows != oge->cov->cols ){
    Rf_error("'V' matrix is not square");
  }
  //X:
	if(OMX_DEBUG) { mxLog("Processing X."); }
  {ScopedProtect p1(Rmtx, R_do_slot(rObj, Rf_install("X")));
	oge->X = omxNewMatrixFromRPrimitive(Rmtx, currentState, 0, 0);
  }
  //Eigy (local) will have however many rows and 1 column:
  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(oge->y->dataMat), oge->y->dataMat->cols, 1);
  if(oge->X->rows != Eigy.rows()){Rf_error("'X' and 'y' matrices have different numbers of rows");}
  //means:
  oge->means = omxInitMatrix(Eigy.rows(), 1, 1, currentState);
  //logdetV_om:
  oge->logdetV_om = omxInitMatrix(1, 1, 1, currentState);
  oge->logdetV_om->data[0] = 0;
  //cholV_fail_om:
  oge->cholV_fail_om = omxInitMatrix(1, 1, 1, currentState);
  oge->cholV_fail_om->data[0] = 0;
  //quadXinv:
  oge->quadXinv.setZero(oge->X->cols, oge->X->cols);

  //Deal with missing data:
  int* casesToDrop_intptr;
  oge->numcases2drop = 0;
  {
  ScopedProtect p1(casesToDrop, R_do_slot(rObj, Rf_install("casesToDrop")));
  if(Rf_length(casesToDrop)){
    if(OMX_DEBUG) { mxLog("Preparing GREML expectation to handle missing data."); }
    oge->numcases2drop = Rf_length(casesToDrop);
    casesToDrop_intptr = INTEGER(casesToDrop);
    oge->dropcase.assign(oge->cov->rows,0);
    for(i=0; i < Rf_length(casesToDrop); i++){
      if(casesToDrop_intptr[i] >= oge->cov->rows){
        Rf_warning("casesToDrop vector in GREML expectation contains indices greater than the number of datapoints");
        oge->numcases2drop--;
      }
      //Need to subtract 1 from the index because R begins array indexing with 1, not 0:
      else{oge->dropcase[casesToDrop_intptr[i]-1] = 1;}
  }}
  }
  if(Eigy.rows() != oge->cov->rows - oge->numcases2drop){
    Rf_error("y and V matrices do not have equal numbers of rows");
  }

  //column names of y and X:
  {
  ScopedProtect p1(yXcolnames, R_do_slot(rObj, Rf_install("yXcolnames")));
  oge->yXcolnames.resize(Rf_length(yXcolnames));
  for(i=0; i < Rf_length(yXcolnames); i++){
    SEXP elem;
    {ScopedProtect p2(elem, STRING_ELT(yXcolnames, i));
  	oge->yXcolnames[i] = CHAR(elem);}
  }
  }

  //Initially compute everything involved in computing means:
  oge->alwaysComputeMeans = 1;
  oge->cholquadX_fail = 0;
  EigenMatrixAdaptor EigX(oge->X);
  Eigen::Map< Eigen::MatrixXd > yhat(omxMatrixDataColumnMajor(oge->means), oge->means->rows, oge->means->cols);
  Eigen::MatrixXd EigV(Eigy.rows(), Eigy.rows());
  Eigen::MatrixXd quadX(oge->X->cols, oge->X->cols);
  //Apparently you need to initialize a matrix's elements before you try to write to its lower triangle:
  quadX.setZero(oge->X->cols, oge->X->cols);
  Eigen::LLT< Eigen::MatrixXd > cholV(Eigy.rows());
  Eigen::LLT< Eigen::MatrixXd > cholquadX(oge->X->cols);
  if( oge->numcases2drop ){
    dropCasesAndEigenize(oge->cov, EigV, oge->numcases2drop, oge->dropcase, 1);
  }
  else{EigV = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(oge->cov), oge->cov->rows, oge->cov->cols);}
  //invcov:
  oge->invcov = omxInitMatrix(EigV.rows(), EigV.cols(), 1, currentState);
  Eigen::Map< Eigen::MatrixXd > Vinv(omxMatrixDataColumnMajor(oge->invcov), EigV.rows(), EigV.cols());
  cholV.compute(EigV.selfadjointView<Eigen::Lower>());
  if(cholV.info() != Eigen::Success){
    Rf_error("Expected covariance matrix is non-positive-definite at initial values");
  }
  oge->cholV_vectorD = (( Eigen::MatrixXd )(cholV.matrixL())).diagonal();
  for(i=0; i < oge->X->rows; i++){
    oge->logdetV_om->data[0] += log(oge->cholV_vectorD[i]);
  }
  oge->logdetV_om->data[0] *= 2;
  Vinv = cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )); //<-- V inverse
  oge->XtVinv = EigX.transpose() * Vinv;
  quadX.triangularView<Eigen::Lower>() = oge->XtVinv * EigX;
  cholquadX.compute(quadX.selfadjointView<Eigen::Lower>());
  if(cholquadX.info() != Eigen::Success){
    Rf_error("Cholesky factorization failed at initial values; possibly, the matrix of covariates is rank-deficient");
  }
  oge->cholquadX_vectorD = (( Eigen::MatrixXd )(cholquadX.matrixL())).diagonal();
  oge->quadXinv = ( cholquadX.solve(Eigen::MatrixXd::Identity(oge->X->cols, oge->X->cols)) ).triangularView<Eigen::Lower>();
  yhat = EigX * oge->quadXinv.selfadjointView<Eigen::Lower>() * oge->XtVinv * Eigy;

  /*Prepare y as the data that the FIML fitfunction will use:*/
  oge->data2 = ox->data;
  ox->data = oge->y;
  if (oge->data2->hasDefinitionVariables()) {
	  Rf_error("definition variables are incompatible (and unnecessary) with GREML expectation");
  }
}

void omxComputeGREMLExpectation(omxExpectation* ox, const char *, const char *) {
  omxGREMLExpectation* oge = (omxGREMLExpectation*) (ox->argStruct);
	omxRecompute(oge->cov, NULL);
  int i=0;
  oge->cholV_fail_om->data[0] = 0;
  oge->cholquadX_fail = 0;
  oge->logdetV_om->data[0] = 0;

  EigenMatrixAdaptor EigX(oge->X);
  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(oge->y->dataMat), oge->y->dataMat->cols, 1);
  Eigen::Map< Eigen::MatrixXd > yhat(omxMatrixDataColumnMajor(oge->means), oge->means->rows, oge->means->cols);
  Eigen::MatrixXd EigV(Eigy.rows(), Eigy.rows());
  Eigen::Map< Eigen::MatrixXd > Vinv(omxMatrixDataColumnMajor(oge->invcov), oge->invcov->rows, oge->invcov->cols);
  Eigen::MatrixXd quadX(oge->X->cols, oge->X->cols);
  quadX.setZero(oge->X->cols, oge->X->cols);
  Eigen::LLT< Eigen::MatrixXd > cholV(oge->y->dataMat->rows);
  Eigen::LLT< Eigen::MatrixXd > cholquadX(oge->X->cols);
  if( oge->numcases2drop ){
    dropCasesAndEigenize(oge->cov, EigV, oge->numcases2drop, oge->dropcase, 1);
  }
  else{EigV = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(oge->cov), oge->cov->rows, oge->cov->cols);}
  cholV.compute(EigV.selfadjointView<Eigen::Lower>());
  if(cholV.info() != Eigen::Success){
    oge->cholV_fail_om->data[0] = 1;
    return;
  }
  oge->cholV_vectorD = (( Eigen::MatrixXd )(cholV.matrixL())).diagonal();
  for(i=0; i < oge->X->rows; i++){
    oge->logdetV_om->data[0] += log(oge->cholV_vectorD[i]);
  }
  oge->logdetV_om->data[0] *= 2;
  if(oge->alwaysComputeMeans){
  	Vinv = cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )); //<-- V inverse
  	oge->XtVinv = EigX.transpose() * Vinv;
  }
  /*alwaysComputeMeans is initialized as true, and the only way it can be set to false is by the GREML
  fitfunction.  If its false, that means that the GREML fitfunction is being used, and it knows how to handle
  a "half-full" Vinv.*/
  else{
  	//V inverse:
  	Vinv.triangularView<Eigen::Lower>() = ( cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )) ).triangularView<Eigen::Lower>();
  	oge->XtVinv = EigX.transpose() * Vinv.selfadjointView<Eigen::Lower>();
  }
  quadX.triangularView<Eigen::Lower>() = oge->XtVinv * EigX;
  cholquadX.compute(quadX.selfadjointView<Eigen::Lower>());
  if(cholquadX.info() != Eigen::Success){
    oge->cholquadX_fail = 1;
    return;
  }
  oge->cholquadX_vectorD = (( Eigen::MatrixXd )(cholquadX.matrixL())).diagonal();
  oge->quadXinv = ( cholquadX.solve(Eigen::MatrixXd::Identity(oge->X->cols, oge->X->cols)) ).triangularView<Eigen::Lower>();
  if(oge->alwaysComputeMeans){
    yhat = EigX * oge->quadXinv.selfadjointView<Eigen::Lower>() * oge->XtVinv * Eigy;
  }
}

void omxDestroyGREMLExpectation(omxExpectation* ox) {
	if(OMX_DEBUG) { mxLog("Destroying GREML Expectation."); }
  omxGREMLExpectation* argStruct = (omxGREMLExpectation*)(ox->argStruct);
  ox->data = argStruct->data2;
  omxFreeMatrix(argStruct->means);
  omxFreeMatrix(argStruct->invcov);
  omxFreeMatrix(argStruct->logdetV_om);
  omxFreeMatrix(argStruct->cholV_fail_om);
}

/*Possible TODO: it will require some additional computation, but it is probably best to calculate the final
regression coefficients using QR, which is more numerically stable*/
void omxPopulateGREMLAttributes(omxExpectation *ox, SEXP algebra) {
  if(OMX_DEBUG) { mxLog("Populating GREML expectation attributes."); }

  omxGREMLExpectation* oge = (omxGREMLExpectation*) (ox->argStruct);

  Rf_setAttrib(algebra, Rf_install("numStats"), Rf_ScalarReal(oge->y->dataMat->cols));
  Rf_setAttrib(algebra, Rf_install("numFixEff"), Rf_ScalarInteger(oge->X->cols));

  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(oge->y->dataMat), oge->y->dataMat->cols, 1);
  SEXP b_ext, bcov_ext, yXcolnames;
  oge->quadXinv = oge->quadXinv.selfadjointView<Eigen::Lower>();
  Eigen::MatrixXd GREML_b = oge->quadXinv * oge->XtVinv * Eigy;

  {
  ScopedProtect p1(b_ext, Rf_allocMatrix(REALSXP, GREML_b.rows(), 1));
  for(int row = 0; row < GREML_b.rows(); row++){
    REAL(b_ext)[0 * GREML_b.rows() + row] = GREML_b(row,0);
  }
  Rf_setAttrib(algebra, Rf_install("b"), b_ext);
  }

  {
  ScopedProtect p1(bcov_ext, Rf_allocMatrix(REALSXP, oge->quadXinv.rows(),
  	oge->quadXinv.cols()));
  for(int row = 0; row < oge->quadXinv.rows(); row++){
    for(int col = 0; col < oge->quadXinv.cols(); col++){
      REAL(bcov_ext)[col * oge->quadXinv.rows() + row] = oge->quadXinv(row,col);
  }}
  Rf_setAttrib(algebra, Rf_install("bcov"), bcov_ext);
  }

  //yXcolnames:
  {
  ScopedProtect p1(yXcolnames, Rf_allocVector(STRSXP, oge->yXcolnames.size()));
  for(int i=0; i < (int)(oge->yXcolnames.size()); i++){
    SET_STRING_ELT(yXcolnames, i, Rf_mkChar(oge->yXcolnames[i]));
  }
  Rf_setAttrib(algebra, Rf_install("yXcolnames"), yXcolnames);
  }

}

omxMatrix* omxGetGREMLExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component){
/* Return appropriate parts of Expectation to the Fit Function */
  if(OMX_DEBUG) { mxLog("GREML expectation: %s requested--", component); }

	omxGREMLExpectation* oge = (omxGREMLExpectation*)(ox->argStruct);
	omxMatrix* retval = NULL;

  if(strEQ("y", component)) {
    retval = oge->y->dataMat;
	}
  else if(strEQ("invcov", component)) {
    retval = oge->invcov;
  }
  else if(strEQ("means", component)) {
  	retval = oge->means;
  }
  else if(strEQ("cholV_fail_om", component)){
    retval = oge->cholV_fail_om;
  }
  else if(strEQ("logdetV_om", component)){
    retval = oge->logdetV_om;
  }
  else if(strEQ("cov", component)) {
		retval = oge->cov;
	}
  else if(strEQ("X", component)) {
		retval = oge->X;
	}

	if (retval) omxRecompute(retval, NULL);

	return retval;
}

static double omxAliasedMatrixElement(omxMatrix *om, int row, int col)
{
  int index = 0;
  if(row >= om->originalRows || col >= om->originalCols) {
  	char *errstr = (char*) calloc(250, sizeof(char));
		sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.",
			row + 1, col + 1, om->originalRows, om->originalCols);
		Rf_error(errstr);
		free(errstr);  // TODO not reached
        return (NA_REAL);
	}
	if(om->colMajor) {
		index = col * om->originalRows + row;
	} else {
		index = row * om->originalCols + col;
	}
	return om->data[index];
}

void dropCasesAndEigenize(omxMatrix* om, Eigen::MatrixXd &em, int num2drop, std::vector< int > todrop,
	int symmetric){

  if(OMX_DEBUG) { mxLog("Trimming out cases with missing data..."); }

  if(num2drop < 1){ return; }

  omxEnsureColumnMajor(om);

  if(om->algebra == NULL){ //i.e., if omxMatrix is from a frontend MxMatrix

    em.setZero(om->rows - num2drop, om->cols - num2drop);

    int nextCol = 0;
    int nextRow = 0;

    for(int j = 0; j < om->cols; j++) {
  	  if(todrop[j]) continue;
  		nextRow = (symmetric ? nextCol : 0);
  		for(int k = (symmetric ? j : 0); k < om->rows; k++) {
  			if(todrop[k]) continue;
  			em(nextRow,nextCol) = omxAliasedMatrixElement(om, k, j);
  			nextRow++;
  		}
  		nextCol++;
  	}
  }
  else{ /*If the omxMatrix is from an algebra, then copying is not necessary; it can be resized directly
  and Eigen-mapped, since the algebra will be recalculated back to its original dimensions anyhow.*/
    if(om->originalRows == 0 || om->originalCols == 0) Rf_error("Not allocated");
    if (om->rows != om->originalRows || om->cols != om->originalCols) {
      // Feasible, but the code is currently not robust to this case
      Rf_error("Can only omxRemoveRowsAndColumns once");
    }

    int oldRows = om->originalRows;
    int oldCols = om->originalCols;

    int nextCol = 0;
    int nextRow = 0;

    om->rows = oldRows - num2drop;
    om->cols = oldCols - num2drop;

    for(int j = 0; j < oldCols; j++) {
      if(todrop[j]) continue;
      nextRow = (symmetric ? nextCol : 0);
      for(int k = (symmetric ? j : 0); k < oldRows; k++) {
        if(todrop[k]) continue;
        omxSetMatrixElement(om, nextRow, nextCol, omxAliasedMatrixElement(om, k, j));
        nextRow++;
      }
      nextCol++;
    }
    em = Eigen::Map< Eigen::MatrixXd >(om->data, om->rows, om->cols);
    omxMarkDirty(om); //<--Need to mark it dirty so that it gets recalculated back to original dimensions.
  }
  if(OMX_DEBUG) { mxLog("Finished trimming out cases with missing data..."); }
}


#include <fstream>
#include <Eigen/Core>
#include <igl/point_mesh_squared_distance.h>
#include <queue>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <algorithm>
#include <omp.h>
#include <glm/glm.hpp>

Eigen::MatrixXf V;
Eigen::MatrixXi F;
Eigen::MatrixXf FN;
Eigen::MatrixXf FV;

int* E2E;
glm::vec3 *pts1, *pts2;
std::pair<glm::vec3, glm::vec3> *pframes;
int *face_indices1, *face_indices2, *group_indices;
glm::vec2* group_tex;

int* find_queue;
glm::vec3 *ux_queue, *uy_queue;
glm::vec2 *coord_queue;
int* neighbor_hash;
glm::vec3 *ux_hash, *uy_hash;
glm::vec2 *coord_hash;

glm::vec3 *cudaV;
glm::ivec3 *cudaF;
glm::vec3 *cudaFV;
glm::vec3 *cudaFN;
int *cudaE2E;
int num_v, num_f;

glm::vec3 *p8192, *p1024, *p256, *p64, *p16;
glm::ivec3 *i1024, *i256, *i64, *i16;

FILE* fps;

extern "C" {

double GetTickCount(void)
{
  struct timespec now;
  if (clock_gettime(CLOCK_MONOTONIC, &now))
    return 0;
  return now.tv_sec * 1000.0 + now.tv_nsec / 1000000.0;
}

void ReadArray(void* indatav, const char* filename, int size, int stride) {
	FILE* fp = fopen(filename, "rb");
	fread(indatav, stride, size, fp);
	fclose(fp);
}

void InitializeE2E(void* vertices, void* faces, int num_v, int num_f, void* pE2E) {
	int* iE2E = (int*)pE2E;
	V = Eigen::Map<Eigen::MatrixXf>((float*)vertices, 3, num_v);
	F = Eigen::Map<Eigen::MatrixXi>((int*)faces, 3, num_f);
	//E2E = Eigen::VectorXi(num_f * 3);
	std::map<std::pair<int, int>, int > dedgeid;
	for (int i = 0; i < num_f; ++i) {
		for (int j = 0; j < 3; ++j) {
			iE2E[i * 3 + j] = -1;
			int x = F(j, i);
			int y = F((j + 1) % 3, i);
			auto key = std::make_pair(x, y);
			auto reverse_key = std::make_pair(y, x);
			if (dedgeid.count(reverse_key)) {
				int deid = dedgeid[reverse_key];
				iE2E[i * 3 + j] = deid;
				iE2E[deid] = i * 3 + j;
				dedgeid.erase(reverse_key);
			} else {
				dedgeid[key] = i * 3 + j;
			}
		}
	}
}

void InitializeMesh(void* vertices, void* faces, void* faceV, void* faceN, void* iE2E, int _num_v, int _num_f) {
	num_v = _num_v;
	num_f = _num_f;

	V = Eigen::Map<Eigen::MatrixXf>((float*)vertices, 3, num_v);
	F = Eigen::Map<Eigen::MatrixXi>((int*)faces, 3, num_f);
	FV = Eigen::Map<Eigen::MatrixXf>((float*)faceV, 3, num_f);
	FN = Eigen::Map<Eigen::MatrixXf>((float*)faceN, 3, num_f);
	E2E = (int*)iE2E;
}
void ReadBaryCentry(void* bary_coords, void* bary_indices, const char* filename) {
	FILE* fp = fopen(filename, "rb");
	int num_C, num_I;
	fread(&num_C, sizeof(int), 1, fp);
	fread(&num_I, sizeof(int), 1, fp);
	fread(bary_coords, sizeof(float), 3 * num_C, fp);
	fread(bary_indices, sizeof(int), num_I, fp);
	fclose(fp);
}

void FurthestSampling(void* pointcloud, void* inds, int num_input, int num_indices) {
	int* indices = (int*)inds;
	float* pts = (float*)pointcloud;
	std::vector<double> distance(num_input, 1e30);
	for (int i = 0; i < num_indices; ++i) {
		if (i == 0) {
			indices[i] = rand() % num_input;
			continue;
		}
		int offset_p = indices[i - 1] * 3;
		float px = pts[offset_p];
		float py = pts[offset_p + 1];
		float pz = pts[offset_p + 2];
		for (int j = 0; j < num_input; ++j) {
			int offset_j = j * 3;
			float x = pts[offset_j] - px;
			float y = pts[offset_j + 1] - py;
			float z = pts[offset_j + 2] - pz;
			float dis = x * x + y * y + z * z;
			if (dis < distance[j]) {
				distance[j] = dis;
			}
		}

		int max_index = 0;
		for(int j = 0; j < num_input; ++j)
		{
			if(distance[j] > distance[max_index])
			{
				max_index = j;
			}
		}
		indices[i] = max_index;
	}
}

void ComputeFacesInfo(void* fv, void* fn, void* v, void* f, int num_v, int num_f) {
	V = Eigen::Map<Eigen::MatrixXf>((float*)v, 3, num_v);
	F = Eigen::Map<Eigen::MatrixXi>((int*)f, 3, num_f);
#pragma omp parallel for
	for (int i = 0; i < num_f; ++i) {
		float* fv_f = ((float*)fv) + 3 * i;
		float* fn_f = ((float*)fn) + 3 * i;
		Eigen::Vector3f FV = (V.col(F(0, i)) + V.col(F(1, i)) + V.col(F(2, i))) / 3.0f;
		Eigen::Vector3f FN = (Eigen::Vector3f(V.col(F(1, i)) - V.col(F(0, i))).cross(Eigen::Vector3f(V.col(F(2, i)) - V.col(F(0, i))))).normalized();
		fv_f[0] = FV[0];
		fv_f[1] = FV[1];
		fv_f[2] = FV[2];
		fn_f[0] = FN[0];
		fn_f[1] = FN[1];
		fn_f[2] = FN[2];
	}
}

inline Eigen::Vector3d rotate_vector_into_plane(Eigen::Vector3d q, const Eigen::Vector3d &source_normal,
                                         const Eigen::Vector3d &target_normal) {
    const double cosTheta = source_normal.dot(target_normal);
    if (cosTheta < 0.9999f) {
        if (cosTheta < -0.9999f) return -q;
        Eigen::Vector3d axis = source_normal.cross(target_normal);
        q = q * cosTheta + axis.cross(q) +
            axis * (axis.dot(q) * (1.0 - cosTheta) / axis.dot(axis));
    }
    return q;
}

inline Eigen::Vector3d Travel(Eigen::Vector3d p, const Eigen::Vector3d& dir, double len, int f, int* E2E, const Eigen::MatrixXf& V, const Eigen::MatrixXi& F, const Eigen::MatrixXf& NF, const double* triangle_space) {
	Eigen::Vector3f Nf = NF.col(f);
	Eigen::Vector3d N(Nf[0], Nf[1], Nf[2]);
	Eigen::Vector3d pt = (dir - dir.dot(N) * N).normalized();

	int prev_id = -1;
	int count = 0;

	while (len > 0) {
		count += 1;
		//printf("%d %f\n", count, len);
		Eigen::Vector3f t0f = V.col(F(0, f));
		Eigen::Vector3f t1f = V.col(F(1, f));
		Eigen::Vector3f t2f = V.col(F(2, f));
		Eigen::Vector3d t0(t0f[0], t0f[1], t0f[2]);
		Eigen::Vector3d t1(t1f[0], t1f[1], t1f[2]);
		Eigen::Vector3d t2(t2f[0], t2f[1], t2f[2]);
		t1 -= t0;
		t2 -= t0;

		int edge_id = f * 3;
		double max_len = 1e30;
		bool found = false;
		int next_id, next_f;
		Eigen::Vector3d next_q;

		double* triangle_space_f = ((double*)triangle_space) + f * 6;
		Eigen::Matrix<double, 2, 3> T = Eigen::Map<Eigen::Matrix<double, 2, 3> >(triangle_space_f);

		//const Eigen::MatrixXd& T = triangle_space[f];
		Eigen::VectorXd coord = T * (p - t0);
		Eigen::VectorXd dirs = (T * pt);

		double lens[3];
		lens[0] = -coord.y() / dirs.y();
		lens[1] = (1 - coord.x() - coord.y()) / (dirs.x() + dirs.y());
		lens[2] = -coord.x() / dirs.x();

		for (int fid = 0; fid < 3; ++fid) {
			if (fid + edge_id == prev_id)
				continue;

			if (lens[fid] >= 0 && lens[fid] < max_len) {
				max_len = lens[fid];
				next_id = E2E[edge_id + fid];
				next_f = next_id;
				if (next_f != -1)
					next_f /= 3;
				found = true;
			}
		}

		Eigen::Vector3f Nf_f = NF.col(f);
//		printf("status: %f %f %d\n", len, max_len, f);
		if (max_len >= len) {
			p = p + len * pt;
			len = 0;
			break;
		}
		p = t0 + t1 * (coord.x() + dirs.x() * max_len) + t2 * (coord.y() + dirs.y() * max_len);
		if (!found) {
			break;
		}
		len -= max_len;
		if (next_f == -1) {
			break;
		}

		Eigen::Vector3f Nf_nf = NF.col(next_f);
		pt = rotate_vector_into_plane(pt, Eigen::Vector3d(Nf_f[0], Nf_f[1], Nf_f[2]), Eigen::Vector3d(Nf_nf[0], Nf_nf[1], Nf_nf[2]));
		f = next_f;
		prev_id = next_id;
	}
	return p;
}

void InitializeTangentSpace(void* vertices, void* faces, void* nf, int num_v, int num_f, void* tangent_space) {
	V = Eigen::Map<Eigen::MatrixXf>((float*)vertices, 3, num_v);
	F = Eigen::Map<Eigen::MatrixXi>((int*)faces, 3, num_f);
	FN = Eigen::Map<Eigen::MatrixXf>((float*)nf, 3, num_f);
	for (int i = 0; i < num_f; ++i) {
		Eigen::Matrix3d p, q;
		Eigen::Vector3f v1 = V.col(F(1, i)) - V.col(F(0, i));
		Eigen::Vector3f v2 = V.col(F(2, i)) - V.col(F(0, i));
		Eigen::Vector3f v3 = FN.col(i);
		p.col(0) = Eigen::Vector3d(v1[0], v1[1], v1[2]);
		p.col(1) = Eigen::Vector3d(v2[0], v2[1], v2[2]);
		p.col(2) = Eigen::Vector3d(v3[0], v3[1], v3[2]);
		q = p.inverse();
		double* triangle_space_f = ((double*)tangent_space) + 6 * i;
		for (int j = 0; j < 2; ++j) {
			for (int k = 0; k < 3; ++k) {
				triangle_space_f[k * 2 + j] = q(j, k);
			}
		}
	}

}

void GetTangentNeighbors(float radius, void* positions, void* frames, void* finds, void* triangle_space, int num_v, void* out_p) {
#pragma omp parallel for
	for (int v = 0; v < num_v; ++v) {
		float* p_pos = ((float*)positions) + v * 3;
		float* p_frame = ((float*)frames) + v * 6;
		float* p_normal = p_frame + 3;
		float* out = ((float*)out_p) + v * 27;
		Eigen::Vector3d p(p_pos[0], p_pos[1], p_pos[2]);
		Eigen::Vector3d orient(p_frame[0], p_frame[1], p_frame[2]);
		Eigen::Vector3d normal(p_normal[0], p_normal[1], p_normal[2]);
		Eigen::Vector3d orient_y = normal.cross(orient);
		for (int i = -1; i <= 1; ++i) {
			for (int j = -1; j <= 1; ++j) {
				Eigen::Vector3d dir = orient * i + orient_y * j;
				double len = dir.norm();
				dir = dir / len;
				Eigen::Vector3d np = Travel(p, dir, len * radius, ((int*)finds)[v], E2E, V, F, FN, (double*)triangle_space);
				float* outp = out + ((i + 1) * 3 + (j + 1)) * 3;
				outp[0] = np[0];
				outp[1] = np[1];
				outp[2] = np[2];
			}
		}
	}
}

struct Coordinate {
	Coordinate()
	{}
	Coordinate(int f, const float* end, const float* from, const float* tex, const float* _ux) {
		float* n = ((float*)FN.data()) + f * 3;
		uy[0] = n[1] * _ux[2] - n[2] * _ux[1];
		uy[1] = n[2] * _ux[0] - n[0] * _ux[2];
		uy[2] = n[0] * _ux[1] - n[1] * _ux[0];
		float len = 1.0 / sqrt(uy[0] * uy[0] + uy[1] * uy[1] + uy[2] * uy[2]);
		uy[0] *= len;
		uy[1] *= len;
		uy[2] *= len;

		ux[0] = uy[1] * n[2] - uy[2] * n[1];
		ux[1] = uy[2] * n[0] - uy[0] * n[2];
		ux[2] = uy[0] * n[1] - uy[1] * n[0];

		float dx = ux[0] * (end[0] - from[0]) + ux[1] * (end[1] - from[1]) + ux[2] * (end[2] - from[2]);
		float dy = uy[0] * (end[0] - from[0]) + uy[1] * (end[1] - from[1]) + uy[2] * (end[2] - from[2]);
		coord[0] = tex[0] + dx;
		coord[1] = tex[1] + dy;
		find = f;
	}
	int find;
	float ux[3];
	float uy[3];
	float coord[2];
};
void GetNeighborhood(float radius,
	void* positions, void* frames, void* finds, int num_group_pts,
	void* pointcloud, void* pointcloud_finds, int num_pts,
	void* neighbor_inds, void* neighbor_tex, int num_indices)
{
	float radius2 = radius * radius;
	std::vector<int> indices(num_pts);
	std::iota(indices.begin(), indices.end(), 0);
	std::random_shuffle(indices.begin(), indices.end());
#pragma omp parallel for
	for (int pt_ind = 0; pt_ind < num_group_pts; ++pt_ind) {
		Eigen::MatrixXf pts = Eigen::Map<Eigen::MatrixXf>((float*)pointcloud, 3, num_pts);
		Eigen::Vector3f p = Eigen::Map<Eigen::Vector3f>(((float*)positions) + 3 * pt_ind);
		int find = ((int*)finds)[pt_ind];

		std::unordered_map<int, int> neighbor_faces;
		std::vector<Coordinate> q;
		q.reserve(1024);
		int front = 0;
		float* fv = (float*)FV.data();
		//q.push(std::make_pair(find, c));
		float tex[] = {0, 0};
		q.emplace_back(Coordinate(find, fv + find * 3, (float*)&p, tex, (float*)frames + 6 * pt_ind));
		neighbor_faces[find] = q.size() - 1;
		while (front < q.size()) {
			auto& info = q[front];
			int f = info.find;
			int* nf = E2E + f * 3;
			for (int i = 0; i < 3; ++i) {
				int next_f = *nf++;
				if (next_f == -1)
					continue;
				next_f /= 3;
				bool flag = false;
				for (int j = 0; j < 3; ++j) {
					auto diff = V.col(F(j, next_f)) - p;
					if (diff.squaredNorm() <= radius2 * 2) {
						flag = true;
						break;
					}
				}
				if (flag && !neighbor_faces.count(next_f)) {
					q.emplace_back(Coordinate(next_f, fv + next_f * 3, fv + f * 3, info.coord, info.ux));
					neighbor_faces[next_f] = q.size() - 1;
				}
			}
			front += 1;
		}
		int offset_i = rand() % indices.size();
		int offset = 0;
		int* neighbor_ind = ((int*)neighbor_inds) + num_indices * pt_ind;
		float* neighbor_texs = ((float*)neighbor_tex) + num_indices * pt_ind * 2;
		auto it = indices.begin() + offset_i;
		bool found = false;
		for (int i = 0; i < num_pts; ++i) {
			int seed = *it++;
			if (it == indices.end())
				it = indices.begin();
			Eigen::Vector3f qs = pts.col(seed) - p;
			if (qs.squaredNorm() <= radius2 * 2) {
				int find = ((int*)pointcloud_finds)[seed];
				auto it = neighbor_faces.find(find);
				if (it != neighbor_faces.end()) {
					auto diff = pts.col(seed) - FV.col(find);
					auto& coord = q[it->second];
					Eigen::Vector3f ux(coord.ux[0],coord.ux[1],coord.ux[2]);
					Eigen::Vector3f uy(coord.uy[0],coord.uy[1],coord.uy[2]);
					float x = coord.coord[0] + diff.dot(ux);
					float y = coord.coord[1] + diff.dot(uy);
					if (std::abs(x) <= radius && std::abs(y) <= radius) {
						if (offset == num_indices) {
							if (x < 1e-5 && y < 1e-5) {
								neighbor_texs[0] = x;
								neighbor_texs[1] = y;
								neighbor_ind[0] = seed;
								break;
							}
						}
						else {
							if (x < 1e-5 && y < 1e-5) {
								found = true;
							}
							neighbor_texs[offset * 2] = x;
							neighbor_texs[offset * 2 + 1] = y;
							neighbor_ind[offset++] = seed;
							if (offset == num_indices && found)
								break;
						}
					}
				}
			}
		}
		for (; offset < num_indices; ++offset) {
			neighbor_ind[offset] = neighbor_ind[0];
			neighbor_texs[offset * 2] = 1e30;
			neighbor_texs[offset * 2 + 1] = 1e30;
		}
	}
}

void ComputeMask(void* maskp, void* indicesp, int num) {
	std::unordered_set<int> uset;
	int* mask = (int*)maskp;
	int* indices = (int*)indicesp;
	for (int i = 0; i < num; ++i) {
		int d = indices[i];
		if (uset.count(d)) {
			mask[i] = 0;
		} else {
			mask[i] = 1;
			uset.insert(d);
		}
	}
}
}


#pragma once
#include <boost/pending/disjoint_sets.hpp>
#include "xtensor/xtensor.hpp"

namespace mutex_watershed {

    // the datastructure to hold the mutex edges for a single cluster and all clusters
    typedef std::unordered_set<uint32_t> MutexSet;
    typedef std::vector<MutexSet> MutexStorage;

    template<class UFD>
    inline bool check_mutex(const uint32_t u, const uint32_t rv,
                            UFD & ufd, const MutexStorage & mutexes) {
        // the mutex storages are symmetric, so we only need to check one of them
        const auto & mutex_u = mutexes[u];
        bool have_mutex = false;
        // we check for all representatives of mutex edges if
        // they are the same as the reperesentative of v
        for(const auto mu : mutex_u) {
            if(ufd.find_set(mu) == rv) {
                have_mutex = true;
                break;
            }
        }
        return have_mutex;
    }

    template<class UFD>
    inline void insert_mutex(const uint32_t u, const uint32_t v, const uint32_t rv,
                             UFD & ufd, MutexStorage & mutexes) {

        auto & mutex_u = mutexes[u];
        // if we don't have a mutex yet, insert it
        if(mutex_u.size() == 0) {
            mutex_u.insert(v);
        }

        // otherwise check if v is already in the mutexes
        // and filter the mutexes in the process
        else {

            bool have_mutex = false;
            std::unordered_set<uint32_t> mutex_representatives;

            // iterate over all current mutexes
            auto mutex_it = mutex_u.begin();
            while(mutex_it != mutex_u.end()) {
                const uint32_t rm = ufd.find_set(*mutex_it);

                // check if this mutex is already present in the list
                // if it is not, insert it, otherwise delete this mutex
                if(mutex_representatives.find(rm) == mutex_representatives.end()) {
                    mutex_representatives.insert(rm);
                    ++mutex_it;  // we don't erase, so we need to increase by hand
                } else {
                    mutex_it = mutex_u.erase(mutex_it);
                }

                // if we have not already found v as mutex, check for it
                if(!have_mutex) {
                    have_mutex = rv == rm;
                }
            }

            // insert the v mutex if it is not present
            if(!have_mutex) {
                // std::cout << "Inserting mutex " << u << " " << v << std::endl;
                mutex_u.insert(v);
            }
        }
    }

    template<class UFD>
    inline void merge_mutexes(const uint32_t u, const uint32_t v,
                              UFD & ufd, MutexStorage & mutexes) {
        auto & mutex_u = mutexes[u];
        auto & mutex_v = mutexes[v];

        // extract all representatives (which should be unique here)
        std::unordered_map<uint32_t, uint32_t> mutex_reps_u;
        for(const auto mu : mutex_u) {
            mutex_reps_u[ufd.find_set(mu)] = mu;
        }

        std::unordered_map<uint32_t, uint32_t> mutex_reps_v;
        for(const auto mv : mutex_v) {
            mutex_reps_v[ufd.find_set(mv)] = mv;
        }

        // merge u into v
        for(const auto mu : mutex_reps_u) {
            if(mutex_reps_v.find(mu.first) == mutex_reps_v.end()) {
                mutex_v.insert(mu.second);
            }
        }

        // merge v into u
        for(const auto mv : mutex_reps_v) {
            if(mutex_reps_u.find(mv.first) == mutex_reps_u.end()) {
                mutex_u.insert(mv.second);
            }
        }

    }

    template<class EDGE_ARRAY, class WEIGHT_ARRAY, class NODE_ARRAY>
    void compute_mws_clustering(const size_t number_of_labels,
                                const xt::xexpression<EDGE_ARRAY> & uvs_exp,
                                const xt::xexpression<EDGE_ARRAY> & mutex_uvs_exp,
                                const xt::xexpression<WEIGHT_ARRAY> & weights_exp,
                                const xt::xexpression<WEIGHT_ARRAY> & mutex_weights_exp,
                                xt::xexpression<NODE_ARRAY> & node_labeling_exp) {

        // casts
        const auto & uvs = uvs_exp.derived_cast();
        const auto & mutex_uvs = mutex_uvs_exp.derived_cast();
        const auto & weights = weights_exp.derived_cast();
        const auto & mutex_weights = mutex_weights_exp.derived_cast();
        auto & node_labeling = node_labeling_exp.derived_cast();

        // make ufd
        std::vector<uint64_t> ranks(number_of_labels);
        std::vector<uint64_t> parents(number_of_labels);
        boost::disjoint_sets<uint64_t*, uint64_t*> ufd(&ranks[0], &parents[0]);
        for(uint64_t label = 0; label < number_of_labels; ++label) {
            ufd.make_set(label);
        }

        // determine number of edge types
        const size_t num_edges = uvs.shape()[0];
        const size_t num_mutex = mutex_uvs.shape()[0];

        // argsort ALL edges
        // we sort in ascending order
        std::vector<size_t> indices(num_edges + num_mutex);
        std::iota(indices.begin(), indices.end(), 0);
        std::sort(indices.begin(), indices.end(), [&](const size_t a, const size_t b){
            const double val_a = (a < num_edges) ? weights(a) : mutex_weights(a - num_edges);
            const double val_b = (b < num_edges) ? weights(b) : mutex_weights(b - num_edges);
            return val_a < val_b;
        });

        MutexStorage mutexes(number_of_labels);

        // iterate over all edges
        for(const size_t edge_id : indices) {

            // check whether this edge is mutex via the edge offset
            const bool is_mutex = edge_id >= num_edges;

            if(is_mutex) {
                // find the mutex id and the connected nodes
                const size_t mutex_id = edge_id - num_edges;
                const uint32_t u = mutex_uvs(mutex_id, 0);
                const uint32_t v = mutex_uvs(mutex_id, 1);

                // find the current representatives
                const uint32_t ru = ufd.find_set(u);
                const uint32_t rv = ufd.find_set(v);

                // if the nodes are already connected, do nothing
                if(ru == rv) {
                    continue;
                }

                // otherwise, insert the mutex
                insert_mutex(u, v, rv, ufd, mutexes);
                insert_mutex(v, u, ru, ufd, mutexes);

            } else {

                // find the connected nodes
                const uint32_t u = uvs(edge_id, 0);
                const uint32_t v = uvs(edge_id, 1);

                // find the current representatives
                const uint32_t ru = ufd.find_set(u);
                const uint32_t rv = ufd.find_set(v);

                // if the nodes are already connected, do nothing
                if(ru == rv) {
                    continue;
                }

                // otherwise, check if we have an active constraint / mutex edge
                const bool have_mutex = check_mutex(u, rv, ufd, mutexes) || check_mutex(v, ru, ufd, mutexes);
                //const bool have_mutex = check_mutex_edge(u, v, mutexes);

                // only merge if we don't have a mutex
                if(!have_mutex) {
                    ufd.link(u, v);
                    merge_mutexes(u, v, ufd, mutexes);
                }

            }
        }

        // get node labeling into output
        for(size_t label = 0; label < number_of_labels; ++label) {
            node_labeling[label] = ufd.find_set(label);
        }
    }
}