#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) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. 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.
 *
 *    3. Neither the name of the copyright holder 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.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// 12D quadrotor dynamics.
//
///////////////////////////////////////////////////////////////////////////////

#include <quads/tf_parser.h>
#include <quads/types.h>
#include <crazyflie_utils/angles.h>

#include <geometry_msgs/TransformStamped.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Dense>
#include <string>

namespace quads {

void TfParser::GetXYZRPY(double* x, double* y, double* z, double* phi,
                         double* theta, double* psi) const {
  geometry_msgs::TransformStamped msg;
  try {
    msg = tf_buffer_.lookupTransform(world_frame_, quad_frame_, ros::Time(0));
  } catch (tf2::TransformException& ex) {
    ROS_WARN("%s", ex.what());
  }

  *x = msg.transform.translation.x;
  *y = msg.transform.translation.y;
  *z = msg.transform.translation.z;

  // Get roll, pitch, and yaw from quaternion.
  const Eigen::Quaterniond quat(msg.transform.rotation.w,
                                msg.transform.rotation.x,
                                msg.transform.rotation.y,
                                msg.transform.rotation.z);

  // Multiply by sign of x component to ensure quaternion giving the preferred
  // Euler transformation (here we're exploiting the fact that rot(q)=rot(-q) ).
  Eigen::Matrix3d R = quat.toRotationMatrix();
  Vector3d euler = crazyflie_utils::angles::Matrix2RPY(R);
  *phi = crazyflie_utils::angles::WrapAngleRadians(euler(0));
  *theta = crazyflie_utils::angles::WrapAngleRadians(euler(1));
  *psi = crazyflie_utils::angles::WrapAngleRadians(euler(2));

  // Maybe add noise?
  // NOTE: only for debugging!!!
  constexpr bool kAddNoise = false;
  constexpr double kNoiseStddev = 0.0005;
  if (kAddNoise) {
    std::normal_distribution<double> normal(0.0, 1.0);
    *x += normal(rng_) * kNoiseStddev;
    *y += normal(rng_) * kNoiseStddev;
    *z += normal(rng_) * kNoiseStddev;
    *phi += normal(rng_) * kNoiseStddev;
    *theta += normal(rng_) * kNoiseStddev;
    *psi += normal(rng_) * kNoiseStddev;
  }

  // Catch nans.
  if (std::isnan(*x) || std::isnan(*y) || std::isnan(*z) || std::isnan(*phi) ||
      std::isnan(*theta) || std::isnan(*psi)) {
    *x = 0.0;
    *y = 0.0;
    *z = 0.0;
    *phi = 0.0;
    *theta = 0.0;
    *psi = 0.0;
  }
}

bool TfParser::Initialize(const ros::NodeHandle& n) {
  name_ = ros::names::append(n.getNamespace(), "tf_parser");

  if (!LoadParameters(n)) {
    ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
    return false;
  }

  return true;
}

bool TfParser::LoadParameters(const ros::NodeHandle& n) {
  ros::NodeHandle nl(n);

  // Frames of reference.
  if (!nl.getParam("frames/world", world_frame_)) return false;
  if (!nl.getParam("frames/quad", quad_frame_)) return false;

  return true;
}

}  // namespace quads


#pragma once

#include <iostream>

#include <Eigen/Dense>
#include <vector>

#include "PnC/RobotSystem/CentroidModel.hpp"
#include "Utils/IO/IOUtilities.hpp"

class FootstepSequenceGenerator {
   public:
    FootstepSequenceGenerator(){};
    virtual ~FootstepSequenceGenerator(){};

    // Parameters Setting
    void SetRightFootStepWidth(double v) { r_ft_spread_ = v; }
    void SetLeftFootStepWidth(double v) { l_ft_spread_ = v; }
    void SetFootStepLength(double v) { ft_length_ = v; }
    void SetFootStepOrientation(const Eigen::VectorXd &v) {
        ft_ori_.w() = v[0];
        ft_ori_.x() = v[1];
        ft_ori_.y() = v[2];
        ft_ori_.z() = v[3];
    }
    // header description

    // Update Function
    void Update(const CentroidModel::EEfID &curr_stance,
                const Eigen::Isometry3d &curr_rf_iso,
                const Eigen::Isometry3d &curr_lf_iso) {
        curr_rf_iso_ = curr_rf_iso;
        curr_lf_iso_ = curr_lf_iso;
        curr_stance_ = curr_stance;
    }

    // Get Function
    void GetNextFootStep(CentroidModel::EEfID &next_stance,
                         Eigen::Isometry3d &next_foot_iso);
    void GetFinalFootStep(CentroidModel::EEfID &next_stance,
                          Eigen::Isometry3d &next_foot_iso);

   private:
    double r_ft_spread_;
    double l_ft_spread_;
    double ft_length_;
    Eigen::Quaternion<double> ft_ori_;

    // Current State
    Eigen::Isometry3d curr_rf_iso_;
    Eigen::Isometry3d curr_lf_iso_;
    CentroidModel::EEfID curr_stance_;
};


//
//  Currencies.hpp
//  MyMonero
//
//  Copyright (c) 2014-2019, MyMonero.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:
//
//  1. Redistributions of source code must retain the above copyright notice, this list of
//	conditions and the following disclaimer.
//
//  2. 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.
//
//  3. Neither the name of the copyright holder 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 Currencies_hpp
#define Currencies_hpp

#include <iostream>
#include <unordered_map>
#include <locale>
#include <boost/optional/optional.hpp>
#include <boost/signals2.hpp>
#include <boost/algorithm/string.hpp>
#include "misc_log_ex.h"
#include "cryptonote_format_utils.h"
//
namespace MoneroConstants
{
	static size_t currency_unitPlaces = 12;
}
//
namespace Currencies
{
	using namespace std;
	//
	typedef uint64_t TwelveDecimalMoneyAmount; // aka 12-decimal (-atomic-unit) money-amount
	typedef uint64_t TwoDecimalMoneyAmount;
	//
	inline double doubleFrom(TwelveDecimalMoneyAmount moneroAmount)
	{
		return stod(cryptonote::print_money(moneroAmount));
	}
	//
	inline char money_decimal_punctuation_char()
	{
		return '.'; // explicitly no support for locale-specific formatting to avoid bad edge-cases and bugs.. sending XXX.0 XMR instead of 0.XXX XMR
	}
	class twodecimal_comma_moneypunct : public std::moneypunct<char>
	{
	public:
		static void imbue(std::ostream &os)
		{
			os.imbue(std::locale(os.getloc(), new twodecimal_comma_moneypunct));
		}
	protected:
		virtual char_type do_decimal_point() const
		{
			return money_decimal_punctuation_char();
		}
		std::string do_grouping() const
		{
			return "\0"; // the zero by itself means do no grouping
		}
	};
	inline string localizedTwoDecimalDoubleFormattedString(double val)
	{
		stringstream ss;
		twodecimal_comma_moneypunct::imbue(ss);
		ss << val;
		//
		return ss.str();
	}
}
namespace Currencies
{
	using namespace std;
	//
	struct MoneroAmount
	{
		TwelveDecimalMoneyAmount _magnitude;
		bool _is_negative;
		//
		MoneroAmount(const string &str)
		{
			if (str.empty()) {
				BOOST_THROW_EXCEPTION(logic_error("Invalid input to MoneroAmount"));
				return;
			}
			static size_t lengthOfNegChar = 1;
			if (str.substr(0, lengthOfNegChar) == "-") {
				if (str.size() == lengthOfNegChar) {
					BOOST_THROW_EXCEPTION(logic_error("String with only '-' is invalid input to MoneroAmount"));
					return;
				}
				_is_negative = true;
				//
				std::istringstream iss(str.substr(lengthOfNegChar, str.size() - lengthOfNegChar));
				iss >> _magnitude;
			} else {
				std::istringstream iss(str);
				iss >> _magnitude;
			}
		}
		MoneroAmount(TwelveDecimalMoneyAmount magnitude, bool is_negative)
		{
			_magnitude = magnitude;
			_is_negative = is_negative;
		}
		//
		MoneroAmount(const MoneroAmount &m2)
		{
			_magnitude = m2._magnitude;
			_is_negative = m2._is_negative;
		}
		MoneroAmount operator=(MoneroAmount m2)
		{
			_magnitude = m2._magnitude;
			_is_negative = m2._is_negative;
			//
			return m2;
		}
		bool operator==(MoneroAmount m2)
		{
			if (m2._is_negative == _is_negative && m2._magnitude == _magnitude) {
				return true;
			} else {
				return false;
			}
		}
		// TODO: implement operator+ and operator- ....
	};
	static inline ostream &operator<<(ostream &out, const MoneroAmount &m)
	{
		out << m._is_negative << m._magnitude; // opting for bigint representation here rather than BigDecimal
		//
		return out;
	}
	static inline MoneroAmount MoneroAmountFromDoubleString(const string &str)
	{
		if (str.empty()) {
			BOOST_THROW_EXCEPTION(logic_error("Invalid input to MoneroAmountFromDoubleString"));
		}
		bool is_negative = false;
		static size_t lengthOfNegChar = 1;
		string final_str;
		if (str.substr(0, lengthOfNegChar) == "-") {
			if (str.size() == lengthOfNegChar) {
				BOOST_THROW_EXCEPTION(logic_error("String with only '-' is invalid input to MoneroAmountFromDoubleString"));
			}
			is_negative = true;
			final_str = str.substr(lengthOfNegChar, str.size() - lengthOfNegChar);
		} else {
			final_str = str;
		}
		TwelveDecimalMoneyAmount magnitude;
		bool r = cryptonote::parse_amount(magnitude, final_str);
		if (!r) {
			BOOST_THROW_EXCEPTION(logic_error("Unable to parse input to MoneroAmountFromDoubleString"));
		}
		//
		return MoneroAmount(magnitude, is_negative);
	}
	static inline string DoubleFormattedString(const MoneroAmount &m)
	{
		string m_str = cryptonote::print_money(m._magnitude); // as double string
		m_str.erase(m_str.find_last_not_of('0') + 1, std::string::npos);
		if (m._is_negative) {
			stringstream ss;
			ss << "-" << m_str;
			//
			return ss.str();
		}
		return m_str;
	}
	static inline double DoubleFromMoneroAmount(const MoneroAmount &m)
	{
		double abs_double = doubleFrom(m._magnitude);
		if (m._is_negative) {
			return -1 * abs_double;
		}
		return abs_double;
	}
	static inline MoneroAmount MoneroAmountFromDouble(double d)
	{
		stringstream ss;
		ss << d;
		//
		return MoneroAmountFromDoubleString(ss.str());
	}
}
namespace Currencies
{
	using namespace std;
	using namespace boost;
	//
	typedef double CcyConversion_Rate;
	typedef string CurrencySymbol;
	typedef string CurrencyUID;
	//
	enum Currency
	{
		none,
		XMR,
		USD,
		AUD,
		BRL,
		CAD,
		CHF,
		CNY,
		EUR,
		GBP,
		HKD,
		INR,
		JPY,
		KRW,
		MXN,
		NOK,
		NZD,
		SEK,
		SGD,
		TRY,
		RUB,
		ZAR
	};
	inline CurrencySymbol CurrencySymbolFrom(Currency ccy)
	{
		switch (ccy) {
			case Currency::none:
				return "";
			case XMR:
				return "TOKL";
			case USD:
				return "USD";
			case AUD:
				return "AUD";
			case BRL:
				return "BRL";
			case CAD:
				return "CAD";
			case CHF:
				return "CHF";
			case CNY:
				return "CNY";
			case EUR:
				return "EUR";
			case GBP:
				return "GBP";
			case HKD:
				return "HKD";
			case INR:
				return "INR";
			case JPY:
				return "JPY";
			case KRW:
				return "KRW";
			case MXN:
				return "MXN";
			case NOK:
				return "NOK";
			case NZD:
				return "NZD";
			case SEK:
				return "SEK";
			case SGD:
				return "SGD";
			case TRY:
				return "TRY";
			case RUB:
				return "RUB";
			case ZAR:
				return "ZAR";
			BOOST_THROW_EXCEPTION(logic_error("Unrecognized Currency"));
			return "";
		}
	}
	inline CurrencyUID CurrencyUIDFrom(Currency ccy)
	{
		return CurrencySymbolFrom(ccy); // same thing since they're unique
	}
	inline Currency CurrencyFrom(CurrencySymbol symbol)
	{
		if (symbol == "") {
			return Currency::none;
		} else if (symbol == CurrencySymbolFrom(XMR)) {
			return XMR;
		} else if (symbol == CurrencySymbolFrom(USD)) {
			return USD;
		} else if (symbol == CurrencySymbolFrom(AUD)) {
			return AUD;
		} else if (symbol == CurrencySymbolFrom(BRL)) {
			return BRL;
		} else if (symbol == CurrencySymbolFrom(CAD)) {
			return CAD;
		} else if (symbol == CurrencySymbolFrom(CHF)) {
			return CHF;
		} else if (symbol == CurrencySymbolFrom(CNY)) {
			return CNY;
		} else if (symbol == CurrencySymbolFrom(EUR)) {
			return EUR;
		} else if (symbol == CurrencySymbolFrom(GBP)) {
			return GBP;
		} else if (symbol == CurrencySymbolFrom(HKD)) {
			return HKD;
		} else if (symbol == CurrencySymbolFrom(INR)) {
			return INR;
		} else if (symbol == CurrencySymbolFrom(JPY)) {
			return JPY;
		} else if (symbol == CurrencySymbolFrom(KRW)) {
			return KRW;
		} else if (symbol == CurrencySymbolFrom(MXN)) {
			return MXN;
		} else if (symbol == CurrencySymbolFrom(NOK)) {
			return NOK;
		} else if (symbol == CurrencySymbolFrom(NZD)) {
			return NZD;
		} else if (symbol == CurrencySymbolFrom(SEK)) {
			return SEK;
		} else if (symbol == CurrencySymbolFrom(SGD)) {
			return SGD;
		} else if (symbol == CurrencySymbolFrom(TRY)) {
			return TRY;
		} else if (symbol == CurrencySymbolFrom(RUB)) {
			return RUB;
		} else if (symbol == CurrencySymbolFrom(ZAR)) {
			return ZAR;
		}
		BOOST_THROW_EXCEPTION(logic_error("Unrecognized CurrencySymbol"));
		return Currency::none;
	}
	inline size_t unitsForDisplay(Currency ccy)
	{
		if (ccy == Currency::XMR) {
			return MoneroConstants::currency_unitPlaces;
		}
		return 2;
	}
}
namespace Currencies
{
	class ConversionRatesController
	{
	public:
		ConversionRatesController() {}
		~ConversionRatesController() {
			cout << "Destructed ~ConversionRatesController" << endl;
		}
		ConversionRatesController(const ConversionRatesController&) = delete; // disable copy constructor to prevent inadvertent temporary in pointer
		ConversionRatesController& operator=(const ConversionRatesController&) = delete;
		//
		// Lifecycle
		void setup() {}
		//
		// Signals
		boost::signals2::signal<void()> didUpdateAvailabilityOfRates_signal;
		//
		// Accessors
		bool isRateReady(Currency currency) const
		{
			if (currency == Currency::none || currency == Currency::XMR)
			{
				BOOST_THROW_EXCEPTION(logic_error("Invalid 'currency' argument value"));
			}
			auto it = xmrToCurrencyRatesByCurrencyUID.find(CurrencyUIDFrom(currency));
			//
			return it != xmrToCurrencyRatesByCurrencyUID.end();
		}
		optional<CcyConversion_Rate> rateFromXMR_orNoneIfNotReady(Currency toCurrency) const
		{
			if (toCurrency == Currency::none || toCurrency == Currency::XMR) {
				BOOST_THROW_EXCEPTION(logic_error("Invalid 'currency' argument value"));
			}
			auto it = xmrToCurrencyRatesByCurrencyUID.find(CurrencyUIDFrom(toCurrency));
			if (it == xmrToCurrencyRatesByCurrencyUID.end()) {
				return none;
			}
			return it->second;
		}
		//
		// Imperatives
		bool set(
			CcyConversion_Rate XMRToCurrencyRate,
			Currency forCurrency,
			bool isPartOfBatch = false
		) {
			bool doNotNotify = isPartOfBatch;
			string ccyUID = CurrencyUIDFrom(forCurrency);
			bool wasSetValueDifferent = XMRToCurrencyRate != xmrToCurrencyRatesByCurrencyUID[ccyUID];
			xmrToCurrencyRatesByCurrencyUID[ccyUID] = XMRToCurrencyRate;
			if (doNotNotify != true) {
				_notifyOf_updateTo_XMRToCurrencyRate();
			}
			return wasSetValueDifferent;
		}
		void ifBatched_notifyOf_set_XMRToCurrencyRate()
		{
			MDEBUG("CcyConversionRates: Received updates: $xmrToCurrencyRatesByCurrencyUID");
			_notifyOf_updateTo_XMRToCurrencyRate();
		}
		void set_xmrToCcyRatesByCcy(std::unordered_map<Currency, double> xmrToCcyRatesByCcy)
		{
			bool wasAnyRateChanged = false;
			std::unordered_map<Currency, double>::iterator it = xmrToCcyRatesByCcy.begin();
			while (it != xmrToCcyRatesByCcy.end()) {
				Currency currency = it->first;
				double rate = it->second;
				bool wasSetValueDifferent = set(
					rate,
					currency,
					true // isPartOfBatch
				);
				if (wasSetValueDifferent) {
					wasAnyRateChanged = true;
				}
				it++;
			}
			if (wasAnyRateChanged) {
				ifBatched_notifyOf_set_XMRToCurrencyRate();
			}
		}
	private:
		//
		// Properties
		std::unordered_map<CurrencyUID, CcyConversion_Rate> xmrToCurrencyRatesByCurrencyUID;
		//
		// Imperatives
		void _notifyOf_updateTo_XMRToCurrencyRate()
		{
			didUpdateAvailabilityOfRates_signal();
		}
	};
}
namespace Currencies
{
	inline vector<string> split(char *phrase, string delimiter)
	{
		vector<string> list;
		string s = string(phrase);
		size_t pos = 0;
		string token;
		while ((pos = s.find(delimiter)) != string::npos) {
			token = s.substr(0, pos);
			list.push_back(token);
			s.erase(0, pos + delimiter.length());
		}
		list.push_back(s);
		return list;
	}
	//
	inline string nonAtomicCurrency_localized_formattedString( // is nonAtomic-unit'd currency a good enough way to categorize these?
		double final_amountDouble,
		Currencies::Currency inCcy
	) {
		if (inCcy == Currencies::XMR) {
			BOOST_THROW_EXCEPTION(logic_error("nonAtomicCurrency_localized_formattedString should not be called with ccy of TOKL"));
			return "";
		}
		string decimalSeparator; decimalSeparator.push_back(money_decimal_punctuation_char());
		if (final_amountDouble == 0) {
			return "0"; // not 0.0 / 0,0 / ...
		}
		string naiveLocalizedString = localizedTwoDecimalDoubleFormattedString(final_amountDouble);
		vector<string> components;
		boost::split(components, naiveLocalizedString, boost::is_any_of(decimalSeparator));
		size_t components_count = components.size();
		if (components_count == 0) {
			BOOST_THROW_EXCEPTION(logic_error("Unexpected 0 components while formatting nonatomic currency"));
			return "";
		}
		if (components_count == 1) { // meaning there's no '.'
			if (boost::algorithm::contains(naiveLocalizedString, decimalSeparator)) {
				BOOST_THROW_EXCEPTION(logic_error("Expected naiveLocalizedString not to contain decimalSeparator"));
				return "";
			}
			stringstream oss;
			oss << naiveLocalizedString << decimalSeparator << "00";
			//
			return oss.str();
		}
		if (components_count != 2) {
			BOOST_THROW_EXCEPTION(logic_error("Expected naiveLocalizedString components_count to be 2"));
			return "";
		}
		string component_1 = components[0];
		string component_2 = components[1];
		size_t component_2_characters_count = component_2.size();
		if (component_2_characters_count > unitsForDisplay(inCcy)) {
			BOOST_THROW_EXCEPTION(logic_error("Expected component_2_characters_count <= unitsForDisplay(inCcy)"));
			return "";
		}
		size_t requiredNumberOfZeroes = unitsForDisplay(inCcy) - component_2_characters_count;
		stringstream rightSidePaddingZeroes;
		if (requiredNumberOfZeroes > 0) {
			for (int i = 0 ; i < requiredNumberOfZeroes ; i++) {
				rightSidePaddingZeroes << "0"; // TODO: less verbose way to do this?
			}
		}
		stringstream return_ss;
		return_ss << component_1 << decimalSeparator << component_2 << rightSidePaddingZeroes.str(); // pad
		//
		return return_ss.str();
	}
	//
	//
	inline optional<double> displayUnitsRounded_amountInCurrency( // NOTE: __DISPLAY_ units
		TwelveDecimalMoneyAmount moneroAmount,
		Currency inCcy,
		const ConversionRatesController &controller
	) {
		if (inCcy == Currency::none) {
			BOOST_THROW_EXCEPTION(logic_error("Selected currency unexpectedly ::none"));
			return none;
		}
		double moneroAmountDouble = doubleFrom(moneroAmount);
		if (inCcy == Currencies::XMR) {
			return moneroAmountDouble; // no conversion necessary
		}
		optional<double> xmrToCurrencyRate = controller.rateFromXMR_orNoneIfNotReady(inCcy);
		if (xmrToCurrencyRate == boost::none) {
			return none; // ccyConversion rate unavailable - consumers will try again
		}
		double raw_ccyConversionRateApplied_amount = moneroAmountDouble * xmrToCurrencyRate.get();
		double roundingMultiplier = pow((double)10, (double)unitsForDisplay(inCcy));
		double truncated_amount = round(roundingMultiplier * raw_ccyConversionRateApplied_amount) / roundingMultiplier;
		//
		return truncated_amount;
	}
	//
	static int ccyConversionRateCalculated_moneroAmountDouble_roundingPlaces = 4; // 4 rather than, say, 2, b/c it's relatively more unlikely that fiat amts will be over 10-100 xmr - and b/c some currencies require it for xmr value not to be 0 - and 5 places is a bit excessive
	inline optional<double> rounded_ccyConversionRateCalculated_moneroAmountDouble(
		double userInputAmountDouble,
		Currency selectedCurrency,
		const ConversionRatesController &controller
	) { // may return nil if ccyConversion rate unavailable - consumers will try again on 'didUpdateAvailabilityOfRates'
		if (selectedCurrency == Currency::none) {
			BOOST_THROW_EXCEPTION(logic_error("Selected currency unexpectedly none"));
			return none;
		}
		optional<CcyConversion_Rate> xmrToCurrencyRate = controller.rateFromXMR_orNoneIfNotReady(selectedCurrency);
		if (xmrToCurrencyRate == boost::none) {
			return none; // ccyConversion rate unavailable - consumers will try again on 'didUpdateAvailabilityOfRates'
		}
		// conversion:
		// currencyAmt = xmrAmt * xmrToCurrencyRate;
		// xmrAmt = currencyAmt / xmrToCurrencyRate.
		// I figure it's better to apply the rounding here rather than only at the display level so that what is actually sent corresponds to what the user saw, even if greater ccyConversion precision /could/ be accomplished..
		double raw_ccyConversionRateApplied_amount = userInputAmountDouble * (1 / (*xmrToCurrencyRate));
		double roundingMultiplier = (double)pow(10, ccyConversionRateCalculated_moneroAmountDouble_roundingPlaces);
		double truncated_amount = (double)round(roundingMultiplier * raw_ccyConversionRateApplied_amount) / roundingMultiplier; // must be truncated for display purposes
		//
		return truncated_amount;
	}
//	inline func amountConverted_displayStringComponents(
//														from amount: MoneroAmount,
//														ccy: CcyConversionRates.Currency,
//														chopNPlaces: UInt = 0
//														) -> (
//															  formattedAmount: String,
//															  final_ccy: CcyConversionRates.Currency
//															  ) {
//		var formattedAmount: String
//		var final_input_amount: MoneroAmount!
//		if chopNPlaces != 0 {
//			let power = MoneroAmount("10")!.power(Int(chopNPlaces)) // this *should* be ok for amount, even if it has no decimal places, because those places would be filled with 0s in such a number
//			final_input_amount = (amount / power) * power
//		} else {
//			final_input_amount = amount
//		}
//			var mutable_ccy = ccy
//			if ccy == .XMR {
//				formattedAmount = final_input_amount!.localized_formattedString
//			} else {
//				let convertedAmount = ccy.displayUnitsRounded_amountInCurrency(fromMoneroAmount: final_input_amount!)
//				if convertedAmount != nil {
//					formattedAmount = MoneroAmount.shared_localized_twoDecimalPlaceDoubleFormatter().string(for: convertedAmount)!
//				} else {
//					formattedAmount = final_input_amount!.localized_formattedString
//					mutable_ccy = .XMR // display XMR until rate is ready? or maybe just show 'LOADING…'?
//				}
//			}
//			return (formattedAmount, mutable_ccy)
//			}
}

#endif /* Currencies_hpp */


#include "clstm.h"
#include <assert.h>
#include <iostream>
#include <vector>
#include <memory>
#include <math.h>
#include <Eigen/Dense>
#include <string>

#include "multidim.h"
#include "h5multi.h"
#include "pymulti.h"
#include "extras.h"

using std_string = std::string;
#define string std_string
using std::vector;
using std::shared_ptr;
using std::unique_ptr;
using std::to_string;
using std::make_pair;
using std::cout;
using std::stoi;
using namespace Eigen;
using namespace ocropus;
using namespace pymulti;

// OCR dataset access, including datasets that are normalized
// on the fly

using namespace h5multi;

struct IOcrDataset {
  virtual ~IOcrDataset() {}
  virtual void image(mdarray<float> &a, int index) = 0;
  virtual void transcript(mdarray<int> &a, int index) = 0;
  virtual void seq(mdarray<float> &a, int index, string name) {
    THROW("unimplemented");
  }
  virtual string to_string(mdarray<int> &transcript) = 0;
  virtual string to_string(vector<int> &transcript) = 0;
  virtual void getCodec(vector<int> &codec) = 0;
  virtual int samples() = 0;
  virtual int dim() = 0;
  virtual int classes() = 0;
};

IOcrDataset *make_HDF5Dataset(const string &fname, bool varsize = false);
IOcrDataset *make_NormalizedDataset(shared_ptr<IOcrDataset> &dataset,
                                    shared_ptr<INormalizer> &normalizer);
IOcrDataset *make_Dataset(const string &fname);

struct HDF5Dataset : IOcrDataset {
  string iname = "images";
  string oname = "transcripts";
  HDF5 h5;
  mdarray<int> codec;

 protected:
  int nsamples = -1;
  int ndims = -1;
  int nclasses = -1;

 public:
  bool varsize = false;
  bool normalize = true;

  int samples() { return nsamples; }
  int dim() { return ndims; }
  int classes() { return nclasses; }
  void getCodec(vector<int> &result) {
    result.resize(codec.size());
    for (int i = 0; i < codec.size(); i++) result[i] = codec[i];
  }

  HDF5Dataset(const char *h5file, bool varsize = false) {
    this->varsize = varsize;
    H5::Exception::dontPrint();
    h5.open(h5file);
    mdarray<int> idims, odims;
    h5.shape(idims, iname.c_str());
    h5.shape(odims, oname.c_str());
    assert(idims(0) == odims(0));
    nsamples = idims(0);
    bool verbose = getienv("verbose", 0);
    if (verbose) cerr << "# lines " << nsamples << endl;
    h5.get(codec, "codec");
    if (verbose) cerr << "# codec " << codec.size() << endl;
    nclasses = codec.size();
    mdarray<float> a;
    h5.getdrow(a, 0, iname.c_str());
    assert(a.rank() == 2);
    if (!varsize) ndims = a.dim(1);
    h5.getarow(a, 0, oname.c_str());
    assert(a.rank() == 1);
  }
  void seq(mdarray<float> &a, int index, string name) {
    h5.getdrow(a, index, name.c_str());
    assert(a.rank() == 2);
  }
  void image(mdarray<float> &a, int index) {
    seq(a, index, iname);
    if (!varsize) assert(a.dim(1) == ndims);
    if (normalize) {
      float m = amax(a);
      for (int i = 0; i < a.size(); i++) a[i] /= m;
    }
  }
  void transcript(mdarray<int> &a, int index) {
    h5.getarow(a, index, oname.c_str());
    assert(a.rank() == 1);
  }
  string to_string(mdarray<int> &transcript) {
    string result;
    for (int i = 0; i < transcript.size(); i++) {
      int label = transcript(i);
      int codepoint = codec(label);
      char chr = char(min(255, codepoint));
      result.push_back(chr);
    }
    return result;
  }
  string to_string(vector<int> &transcript) {
    mdarray<int> transcript_(int(transcript.size()));
    for (int i = 0; i < transcript.size(); i++) transcript_[i] = transcript[i];
    return to_string(transcript_);
  }
};

struct NormalizedDataset : IOcrDataset {
  shared_ptr<IOcrDataset> dataset;
  shared_ptr<INormalizer> normalizer;
  NormalizedDataset() {}
  NormalizedDataset(shared_ptr<IOcrDataset> dataset,
                    shared_ptr<INormalizer> normalizer)
      : dataset(dataset), normalizer(normalizer) {}

  int dim() { return normalizer->target_height; }
  int samples() { return dataset->samples(); }
  int classes() { return dataset->classes(); }
  void image(mdarray<float> &a, int index) {
    mdarray<float> temp;
    dataset->image(temp, index);
    normalizer->measure(temp);
    normalizer->normalize(a, temp);
  }
  void transcript(mdarray<int> &a, int index) { dataset->transcript(a, index); }
  string to_string(mdarray<int> &transcript) {
    return dataset->to_string(transcript);
  }
  string to_string(vector<int> &transcript) {
    return dataset->to_string(transcript);
  }
  void getCodec(vector<int> &result) { dataset->getCodec(result); }
};

IOcrDataset *make_HDF5Dataset(const string &fname, bool varsize) {
  return new HDF5Dataset(fname.c_str(), varsize);
}

IOcrDataset *make_NormalizedDataset(shared_ptr<IOcrDataset> &dataset,
                                    shared_ptr<INormalizer> &normalizer) {
  return new NormalizedDataset(dataset, normalizer);
}

IOcrDataset *make_Dataset(const string &fname) {
  string normalizer_name = getsenv("dewarp", "none");
  if (normalizer_name == "none") return make_HDF5Dataset(fname);
  shared_ptr<IOcrDataset> dataset(make_HDF5Dataset(fname, true));
  shared_ptr<INormalizer> normalizer(make_Normalizer(normalizer_name));
  normalizer->getparams(true);
  return make_NormalizedDataset(dataset, normalizer);
}

double error_rate(Network net, const string &testset) {
  int maxeval = getienv("maxeval", 1000000000);
  shared_ptr<IOcrDataset> dataset(make_Dataset(testset));

  mdarray<float> image;
  mdarray<int> transcript;
  Classes classes;

  double total = 0;
  double errs = 0;

  int N = min(dataset->samples(), maxeval);

  for (int sample = 0; sample < N; sample++) {
    dataset->image(image, sample);
    dataset->transcript(transcript, sample);
    assign(net->inputs, image);
    net->forward();
    Classes output_classes;
    trivial_decode(output_classes, net->outputs);
    string gt = dataset->to_string(transcript);
    ;
    string out = dataset->to_string(output_classes);
    total += gt.size();
    double err = levenshtein(gt, out);
    errs += err;
  }
  return errs / total;
}

int main_ocr(int argc, char **argv) {
  srandomize();

  const char *h5file = argc > 1 ? argv[1] : "uw3-dew.h5";
  string load_name = getsenv("load", "");

  int save_every = getienv("save_every", 0);
  string save_name = getsenv("save_name", "");
  if (save_every >= 0 && save_name == "") THROW("must give save_name=");
  if (save_every > 0 && save_name.find('%') == string::npos)
    save_name += "-%08d.clstm";
  else
    save_name += ".clstm";
  string after_save = getsenv("after_save", "");
  string after_start = getsenv("after_start", "");

  int ntrain = getienv("ntrain", 1000000);
  double lrate = getrenv("lrate", 1e-4);
  int nhidden = getrenv("nhidden", getrenv("hidden", 100));
  int nhidden2 = getrenv("nhidden2", getrenv("hidden2", -1));
  int pseudo_batch = getrenv("pseudo_batch", 1);
  double momentum = getuenv("momentum", 0.9);
  int display_every = getienv("display_every", 0);
  int report_every = getienv("report_every", 1);
  bool randomize = getienv("randomize", 1);
  string lrnorm = getoneof("lrnorm", "batch");
  string dewarp = getoneof("dewarp", "none");
  string net_type = getoneof("lstm", "BIDILSTM");
  string lstm_type = getoneof("lstm_type", "LSTM");
  string output_type = getoneof("output_type", "SoftmaxLayer");
  string target_name = getoneof("target_name", "ctc");

  string testset = getsenv("testset", "");
  int test_every = getienv("test_every", -1);
  string after_test = getsenv("after_test", "");

  print("params", "hg_version", HGVERSION, "lrate", lrate, "nhidden", nhidden,
        "nhidden2", nhidden2, "pseudo_batch", pseudo_batch, "momentum",
        momentum, "type", net_type, lstm_type, output_type);

  if (getienv("params_only", 0)) return 0;

  unique_ptr<PyServer> py;
  if (display_every > 0) {
    py.reset(new PyServer());
    if (display_every > 0) py->open();
    py->eval("ion()");
    py->eval("matplotlib.rc('xtick',labelsize=7)");
    py->eval("matplotlib.rc('ytick',labelsize=7)");
    py->eval("matplotlib.rcParams.update({'font.size':7})");
  }

  shared_ptr<IOcrDataset> dataset;
  dataset.reset(make_Dataset(h5file));
  print("dataset", dataset->samples(), dataset->dim(), dewarp);

  Network net;
  int nclasses = -1;
  int dim = dataset->dim();
  if (load_name != "") {
    net = load_net(load_name);
    nclasses = net->codec.size();
  } else {
    vector<int> codec;
    dataset->getCodec(codec);
    nclasses = codec.size();
    net = make_net(net_type, {{"ninput", dim},
                              {"noutput", nclasses},
                              {"nhidden", nhidden},
                              {"nhidden2", nhidden2}});
    net->initialize();
  }
  net->setLearningRate(lrate, momentum);
  if (getienv("info", 0)) net->info("");
  dataset->getCodec(net->codec);
  // if (load_name != "") net->load(load_name.c_str());
  INetwork::Normalization norm = INetwork::NORM_DFLT;
  if (lrnorm == "len") norm = INetwork::NORM_LEN;
  if (lrnorm == "none") norm = INetwork::NORM_NONE;
  if (norm != INetwork::NORM_DFLT) print("nonstandard lrnorm: ", lrnorm);
  net->networks("",
                [norm](string s, INetwork *net) { net->normalization = norm; });

  mdarray<float> raw_image, image, outputs, aligned;
  mdarray<int> transcript;
  Sequence targets;
  Sequence saligned;
  Classes classes;

  double start_time = now();
  double best_erate = 1e38;

  int start =
      stoi(getdef(net->attributes, "trial", getsenv("start", "-1"))) + 1;
  if (start > 0) print("start", start);
  if (after_start != "") system(after_start.c_str());
  for (int trial = start; trial < ntrain; trial++) {
    bool report = (report_every > 0) && (trial % report_every == 0);
    int sample = trial % dataset->samples();
    if (randomize) sample = irandom() % dataset->samples();
    if (trial > 0 && save_every > 0 && trial % save_every == 0) {
      char fname[4096];
      sprintf(fname, save_name.c_str(), trial);
      print("saving", fname);
      net->attributes["trial"] = to_string(trial);
      save_net(fname, net);
      if (after_save != "") system(after_save.c_str());
    }
    if (trial > 0 && test_every > 0 && trial % test_every == 0 &&
        testset != "") {
      double erate = error_rate(net, testset);
      net->attributes["trial"] = to_string(trial);
      net->attributes["last_err"] = to_string(best_erate);
#ifdef OLD_TESTERR
      // old output format
      print("TESTERR", now() - start_time, save_name, trial, erate, "lrate",
            lrate, "hidden", nhidden, nhidden2, "pseudo_batch", pseudo_batch,
            "momentum", momentum);
#else
      print("TESTERR", erate, "@", trial, ":", now() - start_time);
#endif
      if (save_every == 0 && erate < best_erate) {
        best_erate = erate;
        print("saving", save_name, "at", erate);
        save_net(save_name, net);
        if (after_save != "") system(after_save.c_str());
      }
      if (after_test != "") system(after_test.c_str());
    }
    dataset->image(image, sample);
    dataset->transcript(transcript, sample);
    if (report) {
      print(trial, sample, "dim", image.dim(0), image.dim(1), "time",
            now() - start_time, "lrate", lrate, "hidden", nhidden, nhidden2);
      print("TRU:", "'" + dataset->to_string(transcript) + "'");
    }
    assign(net->inputs, image);
    net->forward();
    assign(outputs, net->outputs);
    if (target_name == "ctc") {
      assign(classes, transcript);
      mktargets(targets, classes, dataset->classes());
      ctc_align_targets(saligned, net->outputs, targets);
      assign(aligned, saligned);
    } else {
      // Use a pre-aligned sequence; this is intended for testing
      // CTC vs non-CTC performance. For general sequence training,
      // use clstmseq
      dataset->seq(aligned, sample, target_name);
      assign(saligned, aligned);
    }
    if (anynan(outputs) || anynan(aligned)) {
      print("got nan");
      break;
    }
    assert(saligned.size() == net->outputs.size());
    for (int t = 0; t < saligned.size(); t++)
      net->outputs[t].d = saligned[t] - net->outputs[t];
    net->backward();
    if (trial % pseudo_batch == 0) net->update();
    Classes output_classes, aligned_classes;
    trivial_decode(output_classes, net->outputs);
    trivial_decode(aligned_classes, saligned);
    string gt = dataset->to_string(transcript);
    ;
    string out = dataset->to_string(output_classes);
    string aln = dataset->to_string(aligned_classes);
    if (report) {
      print("OUT:", "'" + out + "'");
      print("ALN:", "'" + aln + "'");
      print(levenshtein(gt, out));
    }

    if (display_every > 0 && trial % display_every == 0) {
      py->eval("clf()");
      py->subplot(4, 1, 1);
      py->evalf("title('%s')", gt.c_str());
      py->imshowT(image, "cmap=cm.gray,interpolation='bilinear'");
      py->subplot(4, 1, 2);
      py->evalf("title('%s')", out.c_str());
      py->imshowT(outputs, "cmap=cm.hot,interpolation='bilinear'");
      py->subplot(4, 1, 3);
      py->evalf("title('%s')", aln.c_str());
      py->imshowT(aligned, "cmap=cm.hot,interpolation='bilinear'");
      py->subplot(4, 1, 4);
      mdarray<float> v;
      v.resize(outputs.dim(0));
      for (int t = 0; t < outputs.dim(0); t++) v(t) = outputs(t, 0);
      py->plot(v, "color='b'");
      int sp = 1;
      for (int t = 0; t < outputs.dim(0); t++) v(t) = outputs(t, sp);
      py->plot(v, "color='g'");
      int nclass = net->outputs[0].size();
      for (int t = 0; t < outputs.dim(0); t++)
        v(t) = net->outputs[t].col(0).segment(2, nclass - 2).maxCoeff();
      py->evalf("xlim(0,%d)", outputs.dim(0));
      py->plot(v, "color='r'");
      py->eval("ginput(1,1e-3)");
    }
  }
  return 0;
}

int main_eval(int argc, char **argv) {
  const char *h5file = argc > 1 ? argv[1] : "uw3-dew-test.h5";
  string mode = getsenv("mode", "errs");
  string load_name = getsenv("load", "");
  shared_ptr<IOcrDataset> dataset(make_Dataset(h5file));
  Network net;
  if (load_name == "") THROW("must give load=");
  net = load_net(load_name);

  mdarray<float> image;
  mdarray<int> transcript;
  Classes classes;

  double total = 0;
  double errs = 0;

  for (int sample = 0; sample < dataset->samples(); sample++) {
    dataset->image(image, sample);
    dataset->transcript(transcript, sample);
    assign(net->inputs, image);
    net->forward();
    Classes output_classes;
    trivial_decode(output_classes, net->outputs);
    string gt = dataset->to_string(transcript);
    ;
    string out = dataset->to_string(output_classes);
    total += gt.size();
    double err = levenshtein(gt, out);
    errs += err;
    if (mode == "quiet") {
      // do nothing
    } else if (mode == "errs") {
      print(to_string(int(err)) + "\t" + out);
    } else if (mode == "text") {
      print(to_string(sample) + "\t" + out);
    } else if (mode == "full") {
      cout << int(err) << "\t";
      cout << int(sample) << "\t";
      cout << out << "\t";
      cout << gt << "\n";
    }
    cout.flush();
  }
  print("errs", errs, "total", total, "rate", errs * 100.0 / total, "%");
  cout.flush();
  return 0;
}

int main_dump(int argc, char **argv) {
  const char *h5file = argc > 1 ? argv[1] : "uw3-dew-test.h5";
  shared_ptr<IOcrDataset> dataset(make_Dataset(h5file));
  for (int sample = 0; sample < dataset->samples(); sample++) {
    mdarray<int> transcript;
    dataset->transcript(transcript, sample);
    string gt = dataset->to_string(transcript);
    ;
    print(to_string(sample) + "\t" + gt);
    cout.flush();
  }
  return 0;
}

int main_proto(int argc, char **argv) {
  Network net;
  net = load_net(argv[1]);
  bool weights = getienv("weights", 0);
  Network net2 = proto_clone_net(net.get());
  if (getienv("n", 0) == 0)
    debug_as_proto(net.get(), weights);
  else
    debug_as_proto(net2.get(), weights);
  return 0;
}

int main_testdewarp(int argc, char **argv) {
  srandomize();
  if (argc != 2) THROW("usage: ... image.png");
  mdarray<unsigned char> raw;
  mdarray<float> image, dewarped;
  read_png(raw, argv[1]);
  print("raw", raw.dim(0), raw.dim(1));
  image.resize(raw.dim(0), raw.dim(1));
  for (int i = 0; i < raw.dim(0); i++) {
    for (int j = 0; j < raw.dim(1); j++) {
      int jj = raw.dim(1) - j - 1;
      if (raw.rank() == 2)
        image(i, jj) = 1.0 - raw(i, j) / 255.0;
      else
        image(i, jj) = 1.0 - raw(i, j, 0) / 255.0;
    }
  }
  PyServer *py = new PyServer();
  py->open();
  unique_ptr<INormalizer> normalizer;
  normalizer.reset(make_CenterNormalizer());
  normalizer->target_height = int(getrenv("target_height", 48));
  normalizer->getparams(true);
  // normalizer->setPyServer(py);
  py->eval("ion()");
  py->eval("clf()");
  normalizer->measure(image);
  py->eval("subplot(211)");
  py->imshowT(image, "cmap=cm.gray,interpolation='nearest'");
  py->eval("subplot(212)");
  normalizer->normalize(dewarped, image);
  py->imshowT(dewarped, "cmap=cm.gray,interpolation='nearest'");
  return 0;
}

const char *usage =
    "data.h5\n\n"
    "data.h5 is an HDF5 file containing:\n"
    "float images(N,*): text line images (or sequences of vectors)\n"
    "int images_dims(N,2): shape of the images\n"
    "int transcripts(N,*): corresponding transcripts\n";

int main(int argc, char **argv) {
  if (argc < 2) {
    print(string(argv[0]) + " " + usage);
    exit(1);
  }
  TRY {
    string mode = getsenv("mode", "train");
    if (getienv("eval", 0)) {  // for old scripts
      return main_eval(argc, argv);
    }
    if (mode == "dump") {
      return main_dump(argc, argv);
    } else if (mode == "proto") {
      return main_proto(argc, argv);
    } else if (mode == "train") {
      return main_ocr(argc, argv);
    } else if (mode == "testdewarp") {
      return main_testdewarp(argc, argv);
    } else {
      return main_eval(argc, argv);
    }
  } CATCH (const char *msg) {
    print("EXCEPTION", msg);
  } CATCH (...) {
    print("UNKNOWN EXCEPTION");
  }
}


#include <miopen/scgemm/gemm.hpp>
#include <miopen/logger.hpp>
#include <boost/algorithm/string.hpp>
#include <cstring>
#include <vector>
#include <unordered_map>

namespace miopen {
namespace scgemm {

bool is_fgemm(const std::string& kernel_name)
{
    return kernel_name.find("sgemmcc_") != std::string::npos;
}

scgemm_gemm_routine_t get_fgemm_routine(const std::string& kernel_name)
{
    std::vector<std::string> ss;
    boost::split(ss, kernel_name, [](char c) { return c == '_'; });
    static const std::unordered_map<std::string, scgemm_gemm_routine_t> data{
        {"6x4", sgemm_064x016},
        {"6x5", sgemm_064x032},
        {"6x6", sgemm_064x064},
        {"7x6", sgemm_128x064},
        {"7x7", sgemm_128x128},
        {"8x5", sgemm_256x032},
        {"8x6", sgemm_256x064},
    };
    auto result = data.find(ss[1]);
    if(result != data.end())
    {
        return result->second;
    }
    else
    {
        MIOPEN_LOG_E("Gemm kernel: " << kernel_name << ", cannot find associate routine.");
        return sgemm_064x016;
    }
}

std::string get_gemm_kernel_tile_string(scgemm_gemm_routine_t routine)
{
    switch(routine)
    {
    case sgemm_064x016: return "6x4";
    case sgemm_064x032: return "6x5";
    case sgemm_064x064: return "6x6";
    case sgemm_128x064: return "7x6";
    case sgemm_128x128: return "7x7";
    case sgemm_256x032: return "8x5";
    case sgemm_256x064: return "8x6";
    }
    MIOPEN_LOG_E("Gemm routine " << routine << " tile not found.");
    return "6x4";
}

static void
gen_fgemm_kernel_prop(kernel_prop_t* prop, scgemm_gemm_routine_t routine, bool is_relu, int m)
{
    prop->name = "sgemmcc_";
    prop->name.append(get_gemm_kernel_tile_string(routine));

    // prop->name.append((m & 3) == 0 ? "_qm" : (((m & 1) == 0) ? "_dm" : "_om"));
    // force to use _om kernels.
    prop->name.append("_om");

    if(is_relu)
    {
        prop->name.append("_relu");
    }

    switch(routine)
    {
    case sgemm_064x016:
        prop->tile_x     = 6;
        prop->tile_y     = 4;
        prop->block_size = 64;
        break;
    case sgemm_064x032:
        prop->tile_x     = 6;
        prop->tile_y     = 5;
        prop->block_size = 64;
        break;
    case sgemm_064x064:
        prop->tile_x     = 6;
        prop->tile_y     = 6;
        prop->block_size = 64;
        break;
    case sgemm_128x064:
        prop->tile_x     = 7;
        prop->tile_y     = 6;
        prop->block_size = 128;
        break;
    case sgemm_128x128:
        prop->tile_x     = 7;
        prop->tile_y     = 7;
        prop->block_size = 256;
        break;
    case sgemm_256x032:
        prop->tile_x     = 8;
        prop->tile_y     = 5;
        prop->block_size = 256;
        break;
    case sgemm_256x064:
        prop->tile_x     = 8;
        prop->tile_y     = 6;
        prop->block_size = 256;
        break;
    }

    (void)m; // Suppress warning
}

scgemm_fgemm_params create_fgemm_params(std::string& kernel_name,
                                        std::vector<uint32_t>& grids,
                                        std::vector<uint32_t>& blocks,
                                        scgemm_gemm_routine_t routine,
                                        const group_prop_t* gprop,
                                        uint32_t mask,
                                        uint32_t ng)
{
    kernel_prop_t kprop{};
    uint32_t bs, m, n, k, dimx, ntidx, gdx, gdy;

    bs   = gprop->bat;
    m    = gprop->pnx * gprop->pny * gprop->pnz;
    n    = gprop->qnc;
    k    = gprop->pnc;
    dimx = m * bs;
    gen_fgemm_kernel_prop(&kprop, routine, static_cast<bool>(mask & 1u), m);
    ntidx = PSIZE(dimx, (1 << kprop.tile_x));
    gdx   = ntidx >> kprop.tile_x;
    gdy   = (n + (1u << kprop.tile_y) - 1) >> kprop.tile_y;

    kernel_name = kprop.name;

    grids[0]  = routine == 0 ? gdx : gdy;
    grids[1]  = routine == 0 ? gdy : gdx;
    grids[2]  = ng;
    blocks[0] = kprop.block_size;
    blocks[1] = 1;
    blocks[2] = 1;

    scgemm_fgemm_params params{ntidx, dimx, bs, m, n, k};
    return params;
}

/// This fucintion is used to present how to fill in the kernel arguments for a
/// Static Compiled GEMM - GEMM Kernel.
/// Left the code here for reference.
/*
scgemm_kernel_args_t compiled_fgemm_args(size_t& args_size,
                                         const void* src,
                                         const void* wei,
                                         const void* bias,
                                         void* dst,
                                         void* auxbuf,
                                         scgemm_fgemm_params* params,
                                         float alpha)
{
    (void)bias; // suppress warning

    args_size = 60;
    scgemm_kernel_args_t args(new char[args_size]);
    auto _args                                      = reinterpret_cast<char*>(args.get());
    *(reinterpret_cast<const void**>(&_args[0x00])) = src;
    *(reinterpret_cast<const void**>(&_args[0x08])) = wei;
    *(reinterpret_cast<uint32_t*>(&_args[0x10]))    = params->dimx;
    *(reinterpret_cast<uint32_t*>(&_args[0x14]))    = params->m;
    *(reinterpret_cast<uint32_t*>(&_args[0x18]))    = params->n;
    *(reinterpret_cast<uint32_t*>(&_args[0x1c]))    = params->k;
    *(reinterpret_cast<void**>(&_args[0x20]))       = auxbuf;
    *(reinterpret_cast<void**>(&_args[0x28])) =
        reinterpret_cast<void*>(reinterpret_cast<char*>(auxbuf) + (params->ntidx << 2));
    *(reinterpret_cast<void**>(&_args[0x30])) = dst;
    memcpy(&_args[0x38], &alpha, sizeof(float));
    // *(reinterpret_cast<float*>(&_args[0x38])) = alpha;
    return args;
}
*/
} // namespace scgemm
} // namespace miopen


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/copy.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check unary simd::copy", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+1), ref(2*N+3), out(2*N+3);
  std::iota(values.begin(), values.end(),T(1));

  std::copy(values.begin(), values.end(), ref.begin());

  // verify we stopped where we should
  STF_EQUAL ( (boost::simd::copy(values.data(), values.data()+values.size(), out.data()))
            , out.data()+values.size()
            );

  // verify values
  STF_EQUAL( out, ref );
}


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SCALAR_FAST_IROUND2EVEN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_FAST_IROUND2EVEN_HPP_INCLUDED

#include <boost/simd/function/definition/fast_iround2even.hpp>
#include <boost/simd/arch/common/scalar/function/fast_iround2even.hpp>

#endif


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef MINMAX_HH
#define MINMAX_HH

#include <vector>
#include <boost/mpl/bool.hpp>
#include <iostream>
using namespace  std;

template <class Value, class Compare = std::less<Value> >
class double_priority_queue
{
public:
    double_priority_queue() {}
    double_priority_queue(const Compare& cmp): _cmp(cmp) {}

    size_t size() { return _queue.size(); }
    bool empty() { return _queue.empty(); }

    void push(const Value& v)
    {
        _queue.push_back(v);
        bubble_up(_queue.size()-1);
    }

    void pop_top()
    {
        // for (size_t i = 0; i < _queue.size(); ++i)
        //     if (_cmp(top(), _queue[i]))
        //     {
        //         cout << size_t(log2(i+1)) << endl;
        //         cout << "ops top" << endl;
        //         abort();
        //     }
        size_t i = &top() - &_queue[0];
        swap(_queue[i], _queue.back());
        _queue.pop_back();
        if (!_queue.empty())
            trickle_down(i);
    }

    void pop_bottom()
    {
        swap(_queue.front(), _queue.back());
        _queue.pop_back();
        if (!_queue.empty())
            trickle_down(0);
    }

    const Value& top()
    {
        if (_queue.size() < 2)
            return _queue[0];
        if (_queue.size() < 3)
            return _queue[1];
        if (_cmp(_queue[1], _queue[2]))
            return _queue[2];
        else
            return _queue[1];
    }

    const Value& bottom()
    {
        // for (size_t i = 0; i < _queue.size(); ++i)
        //     if (_cmp(_queue[i], _queue.front()))
        //         cout << "ops bottom" << endl;
        return _queue.front();
    }

    // general comparison: if IsMin is mpl::true_, then a < b, otherwise a > b
    // is computed
    template <class IsMin>
    bool cmp(size_t a, size_t b, IsMin)
    {
        if (IsMin::value)
            return _cmp(_queue[a], _queue[b]);
        else
            return _cmp(_queue[b], _queue[a]);
    }

    void trickle_down(size_t i)
    {
        if ((size_t(log2(i+1)) % 2) == 0)
            trickle_down(i, boost::mpl::true_());
        else
            trickle_down(i, boost::mpl::false_());
    }

    template <class IsMin>
    void trickle_down(size_t pos, IsMin is_min)
    {
        size_t i = pos;
        while (true)
        {
            size_t m = 2*i+1;
            if (m >= _queue.size())
                break;

            for (size_t j = 2*i+1; j < 2*i+3; ++j)
            {
                if (j >= _queue.size())
                    break;
                if (cmp(j, m, is_min))
                    m = j;
                for (size_t l = 2*j+1; l < 2*j+3; ++l)
                {
                    if (l >= _queue.size())
                        break;
                    if (cmp(l, m, is_min))
                        m = l;
                }
            }

            if (m > 2*i+2) // is grandchild
            {
                if (cmp(m, i, is_min))
                {
                    swap(_queue[m], _queue[i]);
                    if (!cmp(m, (m-1)/2, is_min))
                        swap(_queue[m], _queue[(m-1)/2]);
                    i = m;
                }
                else
                {
                    break;
                }
            }
            else
            {
                if (cmp(m, i, is_min))
                    swap(_queue[m], _queue[i]);
                break;
            }
        }
    }

    void bubble_up(size_t i)
    {
        if ((size_t(log2(i+1)) % 2) == 0)
        {
            if ((i > 0) && cmp(i, (i-1)/2, boost::mpl::false_()))
            {
                swap(_queue[i], _queue[(i-1)/2]);
                bubble_up((i-1)/2,  boost::mpl::false_());
            }
            else
            {
                bubble_up(i,  boost::mpl::true_());
            }
        }
        else
        {
            if ((i > 0) && cmp(i, (i-1)/2, boost::mpl::true_()))
            {
                swap(_queue[i], _queue[(i-1)/2]);
                bubble_up((i-1)/2, boost::mpl::true_());
            }
            else
            {
                bubble_up(i, boost::mpl::false_());
            }
        }
    }

    template <class IsMin>
    void bubble_up(size_t pos, IsMin is_min)
    {
        size_t i = pos;
        while (true)
        {
            if (i > 2) // has grandparent
            {
                size_t m = ((i-1)/2 - 1)/2;
                if (cmp(i, m, is_min))
                {
                    swap(_queue[m], _queue[i]);
                    i = m;
                }
                else
                {
                    break;
                }
            }
            else
            {
                break;
            }
        }
    }

private:
    std::vector<Value> _queue;
    Compare _cmp;
};

#endif // MINMAX_HH


#pragma once

#include "robot_mode.hpp"
#include "gripper_mode.hpp"
#include "error.hpp"
#include <o80/command_types.hpp>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <string>
#include <array>

namespace franka_o80
{
///Actuator state. Some dynamic typization is done within the class, so it may contain real number, quaternion, `franka_o80::RobotMode`, `franka_o80::GripperMode` or `franka_o80::Error`
class State
{
friend bool operator==(const State &a, const State &b);
friend bool operator!=(const State &a, const State &b);

public:
    ///Type of state
    enum class Type
    {
        real,           ///< Real number
        quaternion,     ///< Quaternion
        robot_mode,     ///< `franka_o80::RobotMode`
        gripper_mode,   ///< `franka_o80::GripperMode`
        error           ///< `franka_o80::Error`
    };

private:
    Type typ_;
    union
    {
        double real;
        std::array<double, 4> quaternion;
        RobotMode robot_mode;
        GripperMode gripper_mode;
        Error error;
    } value_;

public:
    ///Creates state with zero real value
    State();
    ///Creates state with given real value
    State(double value);
    ///Creates state with given quaternion value
    State(const Eigen::Quaterniond &value);
    ///Creates state with given quaternion value given as wxyz
    State(const Eigen::Matrix<double, 4, 1> &value);
    ///Creates state with given quaternion value given as Euler angles
    State(const Eigen::Matrix<double, 3, 1> &value);
    ///Creates state with given robot mode value
    State(RobotMode value);
    ///Creates state with given gripper mode value
    State(GripperMode value);
    ///Creates state with given error value
    State(Error value);
    ///Copies state
    State(const State &state);

    ///Sets state's value to real value
    void set_real(double value);
    ///Sets state's value to quaternion value
    void set_quaternion(const Eigen::Quaterniond &value);
    ///Sets state's value to quaternion value given as wxyz
    void set_wxyz(const Eigen::Matrix<double, 4, 1> &value);
    ///Sets state's value to quaternion value given as Euler angles
    void set_euler(const Eigen::Matrix<double, 3, 1> &value);
    ///Sets state's value to robot mode value
    void set_robot_mode(RobotMode value);
    ///Sets state's value to gripper mode value
    void set_gripper_mode(GripperMode value);
    ///Sets state's value to error value
    void set_error(Error value);

    ///Returns state's type
    Type get_type() const;
    ///Returns real value
    double get_real() const;
    ///Returns quaternion value
    Eigen::Quaterniond get_quaternion() const;
    ///Returns quaternion value given as wxyz
    Eigen::Matrix<double, 4, 1> get_wxyz() const;
    ///Returns quaternion value given as Euler angles
    Eigen::Matrix<double, 3, 1> get_euler() const;
    ///Returns robot mode value
    RobotMode get_robot_mode() const;
    ///Returns gripper mode value
    GripperMode get_gripper_mode() const;
    ///Returns error value
    Error get_error() const;

    ///Returns string representation of state
    std::string to_string() const;

    ///Returns if target state is reached
    static bool finished(const o80::TimePoint &start,
                  const o80::TimePoint &now,
                  const State &start_state,
                  const State &current_state,
                  const State &previous_desired_state,
                  const State &target_state,
                  const o80::Speed &speed);
    ///Returns intermediate state between two states
    static State intermediate_state(const o80::TimePoint &start,
                           const o80::TimePoint &now,
                           const State &start_state,
                           const State &current_state,
                           const State &previous_desired_state,
                           const State &target_state,
                           const o80::Speed &speed);
    ///Returns intermediate state between two states
    static State intermediate_state(const o80::TimePoint &start,
                           const o80::TimePoint &now,
                           const State &start_state,
                           const State &current_state,
                           const State &previous_desired_state,
                           const State &target_state,
                           const o80::Duration_us &duration);
    ///Returns intermediate state between two states
    static State intermediate_state(long int start_iteration,
                           long int current_iteration,
                           const State &start_state,
                           const State &current_state,
                           const State &previous_desired_state,
                           const State &target_state,
                           const o80::Iteration &iteration);
    ///Serializes state
    template <class Archive> void serialize(Archive &archive)
    {
        archive(typ_);
        archive(value_.quaternion);
        //Most probably o80 initializes shared memory with size at first serialization. Because states are uninitialized, this state is smaller than will be in future
        //So we write largest value. Not safe and not perfect
    }
};

bool operator==(const State &a, const State &b);  ///<Compares states
bool operator!=(const State &a, const State &b);  ///<Compares states

}  // namespace franka_o80


#ifndef PERIOR_TREE_INDEXABLE_HPP
#define PERIOR_TREE_INDEXABLE_HPP
#include "rectangle.hpp"
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/preprocessor/enum_params.hpp>
#include <boost/preprocessor/repeat_from_to.hpp>
#include <utility>

namespace perior
{

template<typename pointT>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<pointT>, rectangle<pointT> >::type const&
make_aabb(rectangle<pointT> const& r) BOOST_NOEXCEPT_OR_NOTHROW
{
    return r;
}

template<typename pointT>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<pointT>, rectangle<pointT> >::type
make_aabb(pointT const& p) BOOST_NOEXCEPT_OR_NOTHROW
{
    return rectangle<pointT>(p, traits::zero_vector<pointT>());
}

namespace traits
{
template<typename T>
struct is_indexable: boost::integral_constant<bool, is_point<T>::value>{};

template<typename pointT>
struct is_indexable<rectangle<pointT> >: boost::true_type{};
}// traits

template<typename T>
struct indexable_getter
{
    BOOST_STATIC_ASSERT(traits::is_indexable<T>::value);
    typedef T indexable_type;

    BOOST_FORCEINLINE
    T const& operator()(const T& value) const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return value;
    }
};

template<typename T0, typename T1>
struct indexable_getter<std::pair<T0, T1> >
{
    BOOST_STATIC_ASSERT(traits::is_indexable<T0>::value);
    typedef T0 indexable_type;

    BOOST_FORCEINLINE T0 const&
    operator()(const std::pair<T0, T1>& value) const BOOST_NOEXCEPT_OR_NOTHROW
    {
        return value.first;
    }
};

#define PERIOR_TREE_GENERATE_INDEXABLE_GETTER(z, n, data)\
template<BOOST_PP_ENUM_PARAMS(n, typename T)>\
struct indexable_getter<boost::tuple<BOOST_PP_ENUM_PARAMS(n, T)> >\
{\
    BOOST_STATIC_ASSERT(traits::is_indexable<T0>::value);\
    typedef T0 indexable_type;\
    \
    BOOST_FORCEINLINE T0 const&\
    operator()(const boost::tuple<BOOST_PP_ENUM_PARAMS(n, T)>& value) const\
    BOOST_NOEXCEPT_OR_NOTHROW\
    {\
        return boost::get<0>(value);\
    }\
};\
/**/

BOOST_PP_REPEAT_FROM_TO(1, 10, PERIOR_TREE_GENERATE_INDEXABLE_GETTER, DUMMY)
#undef PERIOR_TREE_GENERATE_FN_GET_INDEXABLE

}// perior

#if __cplusplus >= 201103L
#include <tuple>
namespace perior
{

template<typename ...Ts>
struct indexable_getter<std::tuple<Ts...>>
{
    static_assert(traits::is_indexable<std::tuple<Ts...>>::value,
            "rtree value_type must have indexalbe type for its first type");

    BOOST_FORCEINLINE
    typename std::tuple_element<0, std::tuple<Ts...>>::type const&
    operator()(const std::tuple<Ts...>& value) const noexcept
    {
        return std::get<0>(value);
    }
};

}// perior
#endif // cpp11
#endif// PERIOR_TREE_INDEXABLE_HPP