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

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

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

#ifndef BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP
#define BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP

#include <algorithm>
#include <string>

#include <boost/concept_check.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/util/promote_integral.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/math.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace policies { namespace relate
{

/*!
\brief Policy calculating the intersection points themselves
 */
template
<
    typename ReturnType
>
struct segments_intersection_points
{
    typedef ReturnType return_type;

    template
    <
        typename Point,
        typename Segment,
        typename SegmentRatio,
        typename T
    >
    static inline void assign(Point& point,
                Segment const& segment,
                SegmentRatio const& ratio,
                T const& dx, T const& dy)
    {
        typedef typename geometry::coordinate_type<Point>::type coordinate_type;

        // Calculate the intersection point based on segment_ratio
        // Up to now, division was postponed. Here we divide using numerator/
        // denominator. In case of integer this results in an integer
        // division.
        BOOST_GEOMETRY_ASSERT(ratio.denominator() != 0);

        typedef typename promote_integral<coordinate_type>::type promoted_type;

        promoted_type const numerator
            = geofeatures_boost::numeric_cast<promoted_type>(ratio.numerator());
        promoted_type const denominator
            = geofeatures_boost::numeric_cast<promoted_type>(ratio.denominator());
        promoted_type const dx_promoted = geofeatures_boost::numeric_cast<promoted_type>(dx);
        promoted_type const dy_promoted = geofeatures_boost::numeric_cast<promoted_type>(dy);

        set<0>(point, get<0, 0>(segment) + geofeatures_boost::numeric_cast
            <
                coordinate_type
            >(numerator * dx_promoted / denominator));
        set<1>(point, get<0, 1>(segment) + geofeatures_boost::numeric_cast
            <
                coordinate_type
            >(numerator * dy_promoted / denominator));
    }

    template
    <
        typename Segment1,
        typename Segment2,
        typename SegmentIntersectionInfo
    >
    static inline return_type segments_crosses(side_info const&,
                    SegmentIntersectionInfo const& sinfo,
                    Segment1 const& s1, Segment2 const& s2)
    {
        return_type result;
        result.count = 1;

        bool use_a = true;

        // Prefer one segment if one is on or near an endpoint
        bool const a_near_end = sinfo.robust_ra.near_end();
        bool const b_near_end = sinfo.robust_rb.near_end();
        if (a_near_end && ! b_near_end)
        {
            use_a = true;
        }
        else if (b_near_end && ! a_near_end)
        {
            use_a = false;
        }
        else
        {
            // Prefer shorter segment
            typedef typename SegmentIntersectionInfo::promoted_type ptype;
            ptype const len_a = sinfo.dx_a * sinfo.dx_a + sinfo.dy_a * sinfo.dy_a;
            ptype const len_b = sinfo.dx_b * sinfo.dx_b + sinfo.dy_b * sinfo.dy_b;
            if (len_b < len_a)
            {
                use_a = false;
            }
            // else use_a is true but was already assigned like that
        }

        if (use_a)
        {
            assign(result.intersections[0], s1, sinfo.robust_ra,
                sinfo.dx_a, sinfo.dy_a);
        }
        else
        {
            assign(result.intersections[0], s2, sinfo.robust_rb,
                sinfo.dx_b, sinfo.dy_b);
        }

        result.fractions[0].assign(sinfo);

        return result;
    }

    template <typename Segment1, typename Segment2, typename Ratio>
    static inline return_type segments_collinear(
        Segment1 const& a, Segment2 const& b, bool /*opposite*/,
        int a1_wrt_b, int a2_wrt_b, int b1_wrt_a, int b2_wrt_a,
        Ratio const& ra_from_wrt_b, Ratio const& ra_to_wrt_b,
        Ratio const& rb_from_wrt_a, Ratio const& rb_to_wrt_a)
    {
        return_type result;
        unsigned int index = 0, count_a = 0, count_b = 0;
        Ratio on_a[2];

        // The conditions "index < 2" are necessary for non-robust handling,
        // if index would be 2 this indicate an (currently uncatched) error

        // IMPORTANT: the order of conditions is different as in direction.hpp
        if (a1_wrt_b >= 1 && a1_wrt_b <= 3 // ra_from_wrt_b.on_segment()
            && index < 2)
        {
            //     a1--------->a2
            // b1----->b2
            //
            // ra1 (relative to b) is between 0/1:
            // -> First point of A is intersection point
            detail::assign_point_from_index<0>(a, result.intersections[index]);
            result.fractions[index].assign(Ratio::zero(), ra_from_wrt_b);
            on_a[index] = Ratio::zero();
            index++;
            count_a++;
        }
        if (b1_wrt_a == 2 //rb_from_wrt_a.in_segment()
            && index < 2)
        {
            // We take the first intersection point of B
            // a1--------->a2
            //         b1----->b2
            // But only if it is not located on A
            // a1--------->a2
            // b1----->b2      rb_from_wrt_a == 0/1 -> a already taken

            detail::assign_point_from_index<0>(b, result.intersections[index]);
            result.fractions[index].assign(rb_from_wrt_a, Ratio::zero());
            on_a[index] = rb_from_wrt_a;
            index++;
            count_b++;
        }

        if (a2_wrt_b >= 1 && a2_wrt_b <= 3 //ra_to_wrt_b.on_segment()
            && index < 2)
        {
            // Similarly, second IP (here a2)
            // a1--------->a2
            //         b1----->b2
            detail::assign_point_from_index<1>(a, result.intersections[index]);
            result.fractions[index].assign(Ratio::one(), ra_to_wrt_b);
            on_a[index] = Ratio::one();
            index++;
            count_a++;
        }
        if (b2_wrt_a == 2 // rb_to_wrt_a.in_segment()
            && index < 2)
        {
            detail::assign_point_from_index<1>(b, result.intersections[index]);
            result.fractions[index].assign(rb_to_wrt_a, Ratio::one());
            on_a[index] = rb_to_wrt_a;
            index++;
            count_b++;
        }

        // TEMPORARY
        // If both are from b, and b is reversed w.r.t. a, we swap IP's
        // to align them w.r.t. a
        // get_turn_info still relies on some order (in some collinear cases)
        if (index == 2 && on_a[1] < on_a[0])
        {
            std::swap(result.fractions[0], result.fractions[1]);
            std::swap(result.intersections[0], result.intersections[1]);
        }

        result.count = index;

        return result;
    }

    static inline return_type disjoint()
    {
        return return_type();
    }
    static inline return_type error(std::string const&)
    {
        return return_type();
    }

    // Both degenerate
    template <typename Segment>
    static inline return_type degenerate(Segment const& segment, bool)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(segment));
        set<1>(result.intersections[0], get<0, 1>(segment));
        return result;
    }

    // One degenerate
    template <typename Segment, typename Ratio>
    static inline return_type one_degenerate(Segment const& degenerate_segment,
            Ratio const& ratio, bool a_degenerate)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(degenerate_segment));
        set<1>(result.intersections[0], get<0, 1>(degenerate_segment));
        if (a_degenerate)
        {
            // IP lies on ratio w.r.t. segment b
            result.fractions[0].assign(Ratio::zero(), ratio);
        }
        else
        {
            result.fractions[0].assign(ratio, Ratio::zero());
        }
        return result;
    }
};

}} // namespace policies::relate

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP


#ifndef NFP_HPP_
#define NFP_HPP_

#include <iostream>
#include <list>
#include <string>
#include <fstream>
#include <streambuf>
#include <vector>
#include <set>
#include <exception>
#include <random>
#include <limits>

#ifdef LIBNFP_USE_RATIONAL
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/number.hpp>
#endif
#include <boost/geometry.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/geometries/register/point.hpp>

#ifdef LIBNFP_USE_RATIONAL
namespace bm = boost::multiprecision;
#endif
namespace bg = boost::geometry;
namespace trans = boost::geometry::strategy::transform;

namespace libnfp {
#ifdef NFP_DEBUG
#define DEBUG_VAL(x) std::cerr << x << std::endl;
#define DEBUG_MSG(title, value) std::cerr << title << ":" << value << std::endl;
#else
#define DEBUG_VAL(x)
#define DEBUG_MSG(title, value)
#endif

using std::string;

static constexpr long double NFP_EPSILON=0.00000001;

class LongDouble {
private:
	long double val_;
public:
	LongDouble() : val_(0) {
	}

	LongDouble(const long double& val) : val_(val) {
	}

	void setVal(const long double& v) {
		val_ = v;
	}

	long double val() const {
		return val_;
	}

	LongDouble operator/(const LongDouble& other) const {
		return this->val_ / other.val_;
	}

	LongDouble operator*(const LongDouble& other) const {
		return this->val_ * other.val_;
	}

	LongDouble operator-(const LongDouble& other) const {
		return this->val_ - other.val_;
	}

	LongDouble operator-() const {
		return this->val_ * -1;
	}

	LongDouble operator+(const LongDouble& other) const {
		return this->val_ + other.val_;
	}

	void operator/=(const LongDouble& other) {
		this->val_ = this->val_ / other.val_;
	}

	void operator*=(const LongDouble& other) {
		this->val_ = this->val_ * other.val_;
	}

	void operator-=(const LongDouble& other) {
		this->val_ = this->val_ - other.val_;
	}

	void operator+=(const LongDouble& other) {
		this->val_ = this->val_ + other.val_;
	}

	bool operator==(const int& other) const {
		return this->operator ==(static_cast<long double>(other));
	}

	bool operator==(const LongDouble& other) const {
		return this->operator ==(other.val());
	}

	bool operator==(const long double& other) const {
		return this->val() == other;
	}

	bool operator!=(const int& other) const {
		return !this->operator ==(other);
	}

	bool operator!=(const LongDouble& other) const {
		return !this->operator ==(other);
	}

	bool operator!=(const long double& other) const {
		return !this->operator ==(other);
	}

	bool operator<(const int& other) const {
		return this->operator <(static_cast<long double>(other));
	}

	bool operator<(const LongDouble& other) const {
		return this->operator <(other.val());
	}

	bool operator<(const long double& other) const {
		return this->val() < other;
	}

	bool operator>(const int& other) const {
		return this->operator >(static_cast<long double>(other));
	}

	bool operator>(const LongDouble& other) const {
		return this->operator >(other.val());
	}

	bool operator>(const long double& other) const {
		return this->val() > other;
	}

	bool operator>=(const int& other) const {
		return this->operator >=(static_cast<long double>(other));
	}

	bool operator>=(const LongDouble& other) const {
		return this->operator >=(other.val());
	}

	bool operator>=(const long double& other) const {
		return this->val() >= other;
	}

	bool operator<=(const int& other) const {
		return this->operator <=(static_cast<long double>(other));
	}

	bool operator<=(const LongDouble& other) const {
		return this->operator <=(other.val());
	}

	bool operator<=(const long double& other) const {
		return this->val() <= other;
	}
};
}

namespace std {
template<>
   struct numeric_limits<libnfp::LongDouble>
   {
     static constexpr bool is_specialized = true;

     static constexpr long double
     min() noexcept { return std::numeric_limits<long double>::min(); }

     static constexpr long double
     max() noexcept { return std::numeric_limits<long double>::max(); }

#if __cplusplus >= 201103L
     static constexpr long double
     lowest() noexcept { return -std::numeric_limits<long double>::lowest(); }
#endif

	 static constexpr int digits = std::numeric_limits<long double>::digits;
     static constexpr int digits10 = std::numeric_limits<long double>::digits10;
#if __cplusplus >= 201103L
     static constexpr int max_digits10
	 = std::numeric_limits<long double>::max_digits10;
#endif
     static constexpr bool is_signed = true;
     static constexpr bool is_integer = false;
     static constexpr bool is_exact = false;
     static constexpr int radix = std::numeric_limits<long double>::radix;

     static constexpr long double
     epsilon() noexcept { return libnfp::NFP_EPSILON; }

     static constexpr long double
     round_error() noexcept { return 0.5L; }

     static constexpr int min_exponent = std::numeric_limits<long double>::min_exponent;
     static constexpr int min_exponent10 = std::numeric_limits<long double>::min_exponent10;
     static constexpr int max_exponent = std::numeric_limits<long double>::max_exponent;
     static constexpr int max_exponent10 = std::numeric_limits<long double>::max_exponent10;

     static constexpr bool has_infinity = std::numeric_limits<long double>::has_infinity;
     static constexpr bool has_quiet_NaN = std::numeric_limits<long double>::has_quiet_NaN;
     static constexpr bool has_signaling_NaN = has_quiet_NaN;
	 static constexpr float_denorm_style has_denorm
		 = std::numeric_limits<long double>::has_denorm;
     static constexpr bool has_denorm_loss
	     = std::numeric_limits<long double>::has_denorm_loss;

     static constexpr long double
		infinity() noexcept { return std::numeric_limits<long double>::infinity(); }

     static constexpr long double
		 quiet_NaN() noexcept { return std::numeric_limits<long double>::quiet_NaN(); }

     static constexpr long double
		 signaling_NaN() noexcept { return std::numeric_limits<long double>::signaling_NaN(); }

     static constexpr long double
		 denorm_min() noexcept { return std::numeric_limits<long double>::denorm_min(); }

     static constexpr bool is_iec559
	 = has_infinity && has_quiet_NaN && has_denorm == denorm_present;

     static constexpr bool is_bounded = true;
     static constexpr bool is_modulo = false;

     static constexpr bool traps = std::numeric_limits<long double>::traps;
     static constexpr bool tinyness_before =
    		 std::numeric_limits<long double>::tinyness_before;
     static constexpr float_round_style round_style =
						      round_to_nearest;
   };
}

namespace boost {
namespace numeric {
	template<>
	struct raw_converter<boost::numeric::conversion_traits<double, libnfp::LongDouble>>
	{
		typedef typename boost::numeric::conversion_traits<double, libnfp::LongDouble>::result_type   result_type   ;
		typedef typename boost::numeric::conversion_traits<double, libnfp::LongDouble>::argument_type argument_type ;

		static result_type low_level_convert ( argument_type s ) { return s.val() ; }
	} ;
}
}

namespace libnfp {

#ifndef LIBNFP_USE_RATIONAL
typedef LongDouble coord_t;
#else
typedef bm::number<bm::gmp_rational, bm::et_off> rational_t;
typedef rational_t coord_t;
#endif

bool equals(const LongDouble& lhs, const LongDouble& rhs);
#ifdef LIBNFP_USE_RATIONAL
bool equals(const rational_t& lhs, const rational_t& rhs);
#endif
bool equals(const long double& lhs, const long double& rhs);

const coord_t MAX_COORD = 999999999999999999;
const coord_t MIN_COORD = std::numeric_limits<coord_t>::min();

class point_t {
public:
	point_t() : x_(0), y_(0) {
	}
	point_t(coord_t x, coord_t y) : x_(x), y_(y) {
	}
	bool marked_ = false;
	coord_t x_;
	coord_t y_;

	point_t operator-(const point_t& other) const {
		point_t result = *this;
		bg::subtract_point(result, other);
		return result;
	}

	point_t operator+(const point_t& other) const {
		point_t result = *this;
		bg::add_point(result, other);
		return result;
	}

	bool operator==(const point_t&  other) const {
			return bg::equals(this, other);
  }

  bool operator!=(const point_t&  other) const {
      return !this->operator ==(other);
  }

	bool operator<(const point_t&  other) const {
      return  boost::geometry::math::smaller(this->x_, other.x_) || (equals(this->x_, other.x_) && boost::geometry::math::smaller(this->y_, other.y_));
  }
};

inline long double toLongDouble(const LongDouble& c) {
	return c.val();
}

#ifdef LIBNFP_USE_RATIONAL
inline long double toLongDouble(const rational_t& c) {
	return bm::numerator(c).convert_to<long double>() / bm::denominator(c).convert_to<long double>();
}
#endif

std::ostream& operator<<(std::ostream& os, const coord_t& p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	os << toLongDouble(p);
	return os;
}
#endif

std::istream& operator>>(std::istream& is, LongDouble& c)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	long double val;
	is >> val;
	c.setVal(val);
	return is;
}
#endif

std::ostream& operator<<(std::ostream& os, const point_t& p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	os << "{" << toLongDouble(p.x_) << "," << toLongDouble(p.y_) << "}";
	return os;
}
#endif

const point_t INVALID_POINT = {MAX_COORD, MAX_COORD};

typedef bg::model::segment<point_t> segment_t;

}

#ifdef LIBNFP_USE_RATIONAL
inline long double acos(const libnfp::rational_t& r) {
	return acos(libnfp::toLongDouble(r));
}
#endif

inline long double acos(const libnfp::LongDouble& ld) {
	return acos(libnfp::toLongDouble(ld));
}

#ifdef LIBNFP_USE_RATIONAL
inline long double sqrt(const libnfp::rational_t& r) {
	return sqrt(libnfp::toLongDouble(r));
}
#endif

inline long double sqrt(const libnfp::LongDouble& ld) {
	return sqrt(libnfp::toLongDouble(ld));
}

namespace libnfp {
#ifdef LIBNFP_USE_RATIONAL
inline long double cos(const rational_t& r) {
  return std::cos(toLongDouble(r));
}
#endif

inline long double cos(const LongDouble& ld) {
  return std::cos(libnfp::toLongDouble(ld));
}

#ifdef LIBNFP_USE_RATIONAL
inline long double sin(const rational_t& r) {
  return std::sin(toLongDouble(r));
}
#endif

inline long double sin(const LongDouble& ld) {
  return std::sin(ld.val());
}

}

BOOST_GEOMETRY_REGISTER_POINT_2D(libnfp::point_t, libnfp::coord_t, cs::cartesian, x_, y_)

namespace boost {
namespace geometry {
namespace math {
namespace detail {

template <>
struct square_root<libnfp::LongDouble>
{
  typedef libnfp::LongDouble return_type;

	static inline libnfp::LongDouble apply(libnfp::LongDouble const& a)
  {
        return std::sqrt(a.val());
  }
};

#ifdef LIBNFP_USE_RATIONAL
template <>
struct square_root<libnfp::rational_t>
{
  typedef libnfp::rational_t return_type;

	static inline libnfp::rational_t apply(libnfp::rational_t const& a)
  {
        return std::sqrt(libnfp::toLongDouble(a));
  }
};
#endif

template<>
struct abs<libnfp::LongDouble>
	{
	static libnfp::LongDouble apply(libnfp::LongDouble const& value)
			{
				libnfp::LongDouble const zero = libnfp::LongDouble();
					return value.val() < zero.val() ? -value.val() : value.val();
			}
	};

template <>
struct equals<libnfp::LongDouble, false>
{
	template<typename Policy>
	static inline bool apply(libnfp::LongDouble const& lhs, libnfp::LongDouble const& rhs, Policy const& policy)
  {
		if(lhs.val() == rhs.val())
			return true;

	  return bg::math::detail::abs<libnfp::LongDouble>::apply(lhs.val() - rhs.val()) <=  policy.apply(lhs.val(), rhs.val()) * libnfp::NFP_EPSILON;
  }
};

template <>
struct smaller<libnfp::LongDouble>
{
	static inline bool apply(libnfp::LongDouble const& lhs, libnfp::LongDouble const& rhs)
  {
		if(lhs.val() == rhs.val() || bg::math::detail::abs<libnfp::LongDouble>::apply(lhs.val() - rhs.val()) <=  libnfp::NFP_EPSILON * std::max(lhs.val(), rhs.val()))
			return false;

	  return lhs < rhs;
  }
};
}
}
}
}

namespace libnfp {
inline bool smaller(const LongDouble& lhs, const LongDouble& rhs) {
	return boost::geometry::math::detail::smaller<LongDouble>::apply(lhs, rhs);
}

inline bool larger(const LongDouble& lhs, const LongDouble& rhs) {
  return smaller(rhs, lhs);
}

bool equals(const LongDouble& lhs, const LongDouble& rhs)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
	;
#else
{
	if(lhs.val() == rhs.val())
		return true;

  return bg::math::detail::abs<libnfp::LongDouble>::apply(lhs.val() - rhs.val()) <=  libnfp::NFP_EPSILON * std::max(lhs.val(), rhs.val());
}
#endif

#ifdef LIBNFP_USE_RATIONAL
inline bool smaller(const rational_t& lhs, const rational_t& rhs) {
	return lhs < rhs;
}

inline bool larger(const rational_t& lhs, const rational_t& rhs) {
  return smaller(rhs, lhs);
}

bool equals(const rational_t& lhs, const rational_t& rhs) {
	return lhs == rhs;
}
#endif

inline bool smaller(const long double& lhs, const long double& rhs) {
	return lhs < rhs;
}

inline bool larger(const long double& lhs, const long double& rhs) {
  return smaller(rhs, lhs);
}

bool equals(const long double& lhs, const long double& rhs)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	return lhs == rhs;
}
#endif

typedef bg::model::polygon<point_t, false, true> polygon_t;
typedef std::vector<polygon_t::ring_type> nfp_t;
typedef bg::model::linestring<point_t> linestring_t;

typedef typename polygon_t::ring_type::size_type psize_t;

typedef bg::model::d2::point_xy<long double> pointf_t;
typedef bg::model::segment<pointf_t> segmentf_t;
typedef bg::model::polygon<pointf_t, false, true> polygonf_t;

polygonf_t::ring_type convert(const polygon_t::ring_type& r)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	polygonf_t::ring_type rf;
	for(const auto& pt : r) {
		rf.push_back(pointf_t(toLongDouble(pt.x_), toLongDouble(pt.y_)));
	}
	return rf;
}
#endif

polygonf_t convert(polygon_t p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	polygonf_t pf;
	pf.outer() = convert(p.outer());

	for(const auto& r : p.inners()) {
		pf.inners().push_back(convert(r));
	}

	return pf;
}
#endif

polygon_t nfpRingsToNfpPoly(const nfp_t& nfp)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	polygon_t nfppoly;
	for (const auto& pt : nfp.front()) {
		nfppoly.outer().push_back(pt);
	}

	for (size_t i = 1; i < nfp.size(); ++i) {
		nfppoly.inners().push_back({});
		for (const auto& pt : nfp[i]) {
			nfppoly.inners().back().push_back(pt);
		}
	}

	return nfppoly;
}
#endif

void write_svg(std::string const& filename,const std::vector<segment_t>& segments)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
    std::ofstream svg(filename.c_str());

    boost::geometry::svg_mapper<pointf_t> mapper(svg, 100, 100, "width=\"200mm\" height=\"200mm\" viewBox=\"-250 -250 500 500\"");
    for(const auto& seg : segments) {
    	segmentf_t segf({toLongDouble(seg.first.x_), toLongDouble(seg.first.y_)}, {toLongDouble(seg.second.x_), toLongDouble(seg.second.y_)});
    	mapper.add(segf);
    	mapper.map(segf, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2");
    }
}
#endif

void write_svg(std::string const& filename,	const polygon_t& p, const polygon_t::ring_type& ring)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	std::ofstream svg(filename.c_str());

	boost::geometry::svg_mapper<pointf_t> mapper(svg, 100, 100,	"width=\"200mm\" height=\"200mm\" viewBox=\"-250 -250 500 500\"");
	auto pf = convert(p);
	auto rf = convert(ring);

	mapper.add(pf);
	mapper.map(pf, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2");
	mapper.add(rf);
	mapper.map(rf, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2");
}
#endif

void write_svg(std::string const& filename,	typename std::vector<polygon_t> const& polygons)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	std::ofstream svg(filename.c_str());

	boost::geometry::svg_mapper<pointf_t> mapper(svg, 100, 100, "width=\"200mm\" height=\"200mm\" viewBox=\"-250 -250 500 500\"");
	for (auto p : polygons) {
		auto pf  = convert(p);
		mapper.add(pf);
		mapper.map(pf, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2");
	}
}
#endif

void write_svg(std::string const& filename,	typename std::vector<polygon_t> const& polygons, const nfp_t& nfp)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	polygon_t nfppoly;
	for (const auto& pt : nfp.front()) {
		nfppoly.outer().push_back(pt);
	}

	for (size_t i = 1; i < nfp.size(); ++i) {
		nfppoly.inners().push_back({});
		for (const auto& pt : nfp[i]) {
			nfppoly.inners().back().push_back(pt);
		}
	}
	std::ofstream svg(filename.c_str());

	boost::geometry::svg_mapper<pointf_t> mapper(svg, 100, 100,	"width=\"200mm\" height=\"200mm\" viewBox=\"-250 -250 500 500\"");
	for (auto p : polygons) {
		auto pf  = convert(p);
		mapper.add(pf);
		mapper.map(pf, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2");
	}
	bg::correct(nfppoly);
	auto nfpf = convert(nfppoly);
	mapper.add(nfpf);
	mapper.map(nfpf, "fill-opacity:0.5;fill:rgb(204,153,0);stroke:rgb(204,153,0);stroke-width:2");

	for(auto& r: nfpf.inners()) {
		if(r.size() == 1) {
			mapper.add(r.front());
			mapper.map(r.front(), "fill-opacity:0.5;fill:rgb(204,153,0);stroke:rgb(204,153,0);stroke-width:2");
		} else if(r.size() == 2) {
			segmentf_t seg(r.front(), *(r.begin()+1));
			mapper.add(seg);
			mapper.map(seg, "fill-opacity:0.5;fill:rgb(204,153,0);stroke:rgb(204,153,0);stroke-width:2");
		}
	}
}
#endif

std::ostream& operator<<(std::ostream& os, const segment_t& seg)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	os << "{" << seg.first << "," << seg.second << "}";
	return os;
}
#endif

inline bool operator<(const segment_t& lhs, const segment_t& rhs) {
	return lhs.first < rhs.first || ((lhs.first == rhs.first) && (lhs.second < rhs.second));
}

inline bool operator==(const segment_t& lhs, const segment_t& rhs) {
	return (lhs.first == rhs.first && lhs.second == rhs.second) || (lhs.first == rhs.second && lhs.second == rhs.first);
}

inline bool operator!=(const segment_t& lhs, const segment_t& rhs) {
	return !operator==(lhs,rhs);
}

enum Alignment {
	LEFT,
	RIGHT,
	ON
};

point_t normalize(const point_t& pt)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	point_t norm = pt;
	coord_t len = bg::length(segment_t{{0,0},pt});

	if(len == 0.0L)
		return {0,0};

	norm.x_ /= len;
	norm.y_ /= len;

	return norm;
}
#endif

Alignment get_alignment(const segment_t& seg, const point_t& pt)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	coord_t res = ((seg.second.x_ - seg.first.x_)*(pt.y_ - seg.first.y_)
			- (seg.second.y_ - seg.first.y_)*(pt.x_ - seg.first.x_));

	if(equals(res, 0)) {
		return ON;
	} else	if(larger(res,0)) {
		return LEFT;
	} else {
		return RIGHT;
	}
}
#endif

long double get_inner_angle(const point_t& joint, const point_t& end1, const point_t& end2)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	coord_t dx21 = end1.x_-joint.x_;
	coord_t dx31 = end2.x_-joint.x_;
	coord_t dy21 = end1.y_-joint.y_;
	coord_t dy31 = end2.y_-joint.y_;
	coord_t m12 = sqrt((dx21*dx21 + dy21*dy21));
	coord_t m13 = sqrt((dx31*dx31 + dy31*dy31));
	if(m12 == 0.0L || m13 == 0.0L)
		return 0;
	return acos( (dx21*dx31 + dy21*dy31) / (m12 * m13) );
}
#endif

struct TouchingPoint {
	enum Type {
		VERTEX,
		A_ON_B,
		B_ON_A
	};
	Type type_;
	psize_t A_;
	psize_t B_;
};

struct TranslationVector {
	point_t vector_;
	segment_t edge_;
	bool fromA_;
	string name_;

	bool operator<(const TranslationVector& other) const {
		return this->vector_ < other.vector_ || ((this->vector_ == other.vector_) && (this->edge_ < other.edge_));
	}
};

std::ostream& operator<<(std::ostream& os, const TranslationVector& tv)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	os << "{" << tv.edge_ << " -> " << tv.vector_ << "} = " << tv.name_;
	return os;
}
#endif

void read_wkt_polygon(const string& filename, polygon_t& p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	std::ifstream t(filename);

	std::string str;
	t.seekg(0, std::ios::end);
	str.reserve(t.tellg());
	t.seekg(0, std::ios::beg);

	str.assign((std::istreambuf_iterator<char>(t)),
							std::istreambuf_iterator<char>());

	str.pop_back();
	bg::read_wkt(str, p);
	bg::correct(p);
}
#endif

std::vector<psize_t> find_minimum_y(const polygon_t& p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	std::vector<psize_t> result;
	coord_t min = MAX_COORD;
	auto& po = p.outer();
	for(psize_t i = 0; i < p.outer().size() - 1; ++i) {
		if(smaller(po[i].y_, min)) {
			result.clear();
			min = po[i].y_;
			result.push_back(i);
		} else if (equals(po[i].y_, min)) {
			result.push_back(i);
		}
	}
	return result;
}
#endif

std::vector<psize_t> find_maximum_y(const polygon_t& p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	std::vector<psize_t> result;
	coord_t max = MIN_COORD;
	auto& po = p.outer();
	for(psize_t i = 0; i < p.outer().size() - 1; ++i) {
		if(larger(po[i].y_, max)) {
			result.clear();
			max = po[i].y_;
			result.push_back(i);
		} else if (equals(po[i].y_, max)) {
			result.push_back(i);
		}
	}
	return result;
}
#endif

psize_t find_point(const polygon_t::ring_type& ring, const point_t& pt)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	for(psize_t i = 0; i < ring.size(); ++i) {
		if(ring[i] == pt)
			return i;
	}
	return std::numeric_limits<psize_t>::max();
}
#endif

std::vector<TouchingPoint> findTouchingPoints(const polygon_t::ring_type& ringA, const polygon_t::ring_type& ringB)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	std::vector<TouchingPoint> touchers;
	for(psize_t i = 0; i < ringA.size() - 1; i++) {
		psize_t nextI = i+1;
		for(psize_t j = 0; j < ringB.size() - 1; j++) {
			psize_t nextJ = j+1;
			if(ringA[i] == ringB[j]) {
				touchers.push_back({TouchingPoint::VERTEX, i, j});
			} else if (ringA[nextI] != ringB[j] && bg::intersects(segment_t(ringA[i],ringA[nextI]), ringB[j])) {
				touchers.push_back({TouchingPoint::B_ON_A, nextI, j});
			} else if (ringB[nextJ] != ringA[i] && bg::intersects(segment_t(ringB[j],ringB[nextJ]), ringA[i])) {
				touchers.push_back({TouchingPoint::A_ON_B, i, nextJ});
			}
		}
	}
	return touchers;
}
#endif

//TODO deduplicate code
TranslationVector trimVector(const polygon_t::ring_type& rA, const polygon_t::ring_type& rB, const TranslationVector& tv)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	coord_t shortest = bg::length(tv.edge_);
	TranslationVector trimmed = tv;
	for(const auto& ptA : rA) {
		point_t translated;
		//for polygon A we invert the translation
		trans::translate_transformer<coord_t, 2, 2> translate(-tv.vector_.x_, -tv.vector_.y_);
		boost::geometry::transform(ptA, translated, translate);
		linestring_t projection;
		segment_t segproj(ptA, translated);
		projection.push_back(ptA);
		projection.push_back(translated);
		std::vector<point_t> intersections;
		bg::intersection(rB, projection, intersections);
		if(bg::touches(projection, rB) && intersections.size() < 2) {
			continue;
		}

		//find shortest intersection
		coord_t len;
		segment_t segi;
		for(const auto& pti : intersections) {
			segi = segment_t(ptA,pti);
			len = bg::length(segi);
			if(smaller(len, shortest)) {
				trimmed.vector_ = ptA - pti;
				trimmed.edge_ = segi;
				shortest = len;
			}
		}
	}

	for(const auto& ptB : rB) {
		point_t translated;

		trans::translate_transformer<coord_t, 2, 2> translate(tv.vector_.x_, tv.vector_.y_);
		boost::geometry::transform(ptB, translated, translate);
		linestring_t projection;
		segment_t segproj(ptB, translated);
		projection.push_back(ptB);
		projection.push_back(translated);
		std::vector<point_t> intersections;
		bg::intersection(rA, projection, intersections);
		if(bg::touches(projection, rA) && intersections.size() < 2) {
			continue;
		}

		//find shortest intersection
		coord_t len;
		segment_t segi;
		for(const auto& pti : intersections) {

			segi = segment_t(ptB,pti);
			len = bg::length(segi);
			if(smaller(len, shortest)) {
				trimmed.vector_ = pti - ptB;
				trimmed.edge_ = segi;
				shortest = len;
			}
		}
	}
 	return trimmed;
}
#endif

std::vector<TranslationVector> findFeasibleTranslationVectors(polygon_t::ring_type& ringA, polygon_t::ring_type& ringB, const std::vector<TouchingPoint>& touchers)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	//use a set to automatically filter duplicate vectors
	std::vector<TranslationVector> potentialVectors;
	std::vector<std::pair<segment_t,segment_t>> touchEdges;

	for (psize_t i = 0; i < touchers.size(); i++) {
		point_t& vertexA = ringA[touchers[i].A_];
		vertexA.marked_ = true;

		// adjacent A vertices
		signed long prevAindex = touchers[i].A_ - 1;
		signed long nextAindex = touchers[i].A_ + 1;

		prevAindex = (prevAindex < 0) ? ringA.size() - 2 : prevAindex; // loop
		nextAindex = (static_cast<psize_t>(nextAindex) >= ringA.size()) ? 1 : nextAindex; // loop

		point_t& prevA = ringA[prevAindex];
		point_t& nextA = ringA[nextAindex];

		// adjacent B vertices
		point_t& vertexB = ringB[touchers[i].B_];

		signed long prevBindex = touchers[i].B_ - 1;
		signed long nextBindex = touchers[i].B_ + 1;

		prevBindex = (prevBindex < 0) ? ringB.size() - 2 : prevBindex; // loop
		nextBindex = (static_cast<psize_t>(nextBindex) >= ringB.size()) ? 1 : nextBindex; // loop

		point_t& prevB = ringB[prevBindex];
		point_t& nextB = ringB[nextBindex];

		if (touchers[i].type_ == TouchingPoint::VERTEX) {
			segment_t a1 = { vertexA, nextA };
			segment_t a2 = { vertexA, prevA };
			segment_t b1 = { vertexB, nextB };
			segment_t b2 = { vertexB, prevB };

			//swap the segment elements so that always the first point is the touching point
			//also make the second segment always a segment of ringB
			touchEdges.push_back({a1, b1});
			touchEdges.push_back({a1, b2});
			touchEdges.push_back({a2, b1});
			touchEdges.push_back({a2, b2});
#ifdef NFP_DEBUG
			write_svg("touchersV" + std::to_string(i) + ".svg", {a1,a2,b1,b2});
#endif

			//TODO test parallel edges for floating point stability
			Alignment al;
			//a1 and b1 meet at start vertex
			al = get_alignment(a1, b1.second);
			if(al == LEFT) {
				potentialVectors.push_back({b1.first - b1.second, b1, false, "vertex1"});
			} else if(al == RIGHT) {
				potentialVectors.push_back({a1.second - a1.first, a1, true, "vertex2"});
			} else {
				potentialVectors.push_back({a1.second - a1.first, a1, true, "vertex3"});
			}

			//a1 and b2 meet at start and end
			al = get_alignment(a1, b2.second);
			if(al == LEFT) {
				//no feasible translation
			} else if(al == RIGHT) {
				potentialVectors.push_back({a1.second - a1.first, a1, true, "vertex4"});
			} else {
				potentialVectors.push_back({a1.second - a1.first, a1, true, "vertex5"});
			}

			//a2 and b1 meet at end and start
			al = get_alignment(a2, b1.second);
			if(al == LEFT) {
				//no feasible translation
			} else if(al == RIGHT) {
				potentialVectors.push_back({b1.first - b1.second, b1, false, "vertex6"});
			} else {
				potentialVectors.push_back({b1.first - b1.second, b1, false, "vertex7"});
			}
		} else if (touchers[i].type_ == TouchingPoint::B_ON_A) {
			segment_t a1 = {vertexB, vertexA};
			segment_t a2 = {vertexB, prevA};
			segment_t b1 = {vertexB, prevB};
			segment_t b2 = {vertexB, nextB};

			touchEdges.push_back({a1, b1});
			touchEdges.push_back({a1, b2});
			touchEdges.push_back({a2, b1});
			touchEdges.push_back({a2, b2});
#ifdef NFP_DEBUG
			write_svg("touchersB" + std::to_string(i) + ".svg", {a1,a2,b1,b2});
#endif
			potentialVectors.push_back({vertexA - vertexB, {vertexB, vertexA}, true, "bona"});
		} else if (touchers[i].type_ == TouchingPoint::A_ON_B) {
			//TODO testme
			segment_t a1 = {vertexA, prevA};
			segment_t a2 = {vertexA, nextA};
			segment_t b1 = {vertexA, vertexB};
			segment_t b2 = {vertexA, prevB};
#ifdef NFP_DEBUG
			write_svg("touchersA" + std::to_string(i) + ".svg", {a1,a2,b1,b2});
#endif
			touchEdges.push_back({a1, b1});
			touchEdges.push_back({a2, b1});
			touchEdges.push_back({a1, b2});
			touchEdges.push_back({a2, b2});
			potentialVectors.push_back({vertexA - vertexB, {vertexA, vertexB}, false, "aonb"});
		}
	}

	//discard immediately intersecting translations
	std::vector<TranslationVector> vectors;
	for(const auto& v : potentialVectors) {
		bool discarded = false;
		for(const auto& sp : touchEdges) {
			point_t normEdge = normalize(v.edge_.second - v.edge_.first);
			point_t normFirst = normalize(sp.first.second - sp.first.first);
			point_t normSecond = normalize(sp.second.second - sp.second.first);

			Alignment a1 = get_alignment({{0,0},normEdge}, normFirst);
			Alignment a2 = get_alignment({{0,0},normEdge}, normSecond);

			if(a1 == a2 && a1 != ON) {
				long double df = get_inner_angle({0,0},normEdge, normFirst);
				long double ds = get_inner_angle({0,0},normEdge, normSecond);

				point_t normIn = normalize(v.edge_.second - v.edge_.first);
				if (equals(df, ds)) {
					TranslationVector trimmed = trimVector(ringA,ringB, v);
					polygon_t::ring_type translated;
					trans::translate_transformer<coord_t, 2, 2> translate(trimmed.vector_.x_,	trimmed.vector_.y_);
					boost::geometry::transform(ringB, translated, translate);
					if (!(bg::intersects(translated, ringA) && !bg::overlaps(translated, ringA) && !bg::covered_by(translated, ringA) && !bg::covered_by(ringA, translated))) {
						discarded = true;
						std::cerr << "discarded" << std::endl;
						break;
					}
				} else {

					if (normIn == normalize(v.vector_)) {
						if (larger(ds, df)) {
							discarded = true;
							break;
						}
					} else {
						if (smaller(ds, df)) {
							discarded = true;
							break;
						}
					}
				}
			}
		}
		if(!discarded)
			vectors.push_back(v);
	}
	return vectors;
}
#endif

bool find(const std::vector<TranslationVector>& h, const TranslationVector& tv)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	for(const auto& htv : h) {
		if(htv.edge_ == tv.edge_)
			return true;
	}
	return false;
}
#endif

TranslationVector getLongest(const std::vector<TranslationVector>& tvs)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	coord_t len;
	coord_t maxLen = MIN_COORD;
	TranslationVector longest;
	longest.vector_ = INVALID_POINT;

	for(auto& tv : tvs) {
		len = bg::length(segment_t{{0,0},tv.vector_});
		if(larger(len, maxLen)) {
			maxLen = len;
			longest = tv;
		}
	}
	return longest;
}
#endif

TranslationVector selectNextTranslationVector(const polygon_t& pA, const polygon_t::ring_type& rA,	const polygon_t::ring_type& rB, const std::vector<TranslationVector>& tvs, const std::vector<TranslationVector>& history)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	if(!history.empty()) {
		TranslationVector last = history.back();
		std::vector<TranslationVector> historyCopy = history;
		if(historyCopy.size() >= 2) {
			historyCopy.erase(historyCopy.end() - 1);
			historyCopy.erase(historyCopy.end() - 1);
			if(historyCopy.size() > 4) {
				historyCopy.erase(historyCopy.begin(), historyCopy.end() - 4);
			}

		} else {
			historyCopy.clear();
		}
		DEBUG_MSG("last", last);

		psize_t laterI = std::numeric_limits<psize_t>::max();
		point_t previous = rA[0];
		point_t next;

		if(last.fromA_) {
			for (psize_t i = 1; i < rA.size() + 1; ++i) {
				if (i >= rA.size())
					next = rA[i % rA.size() + 1];
				else
					next = rA[i];

				segment_t candidate( previous, next );
				if(candidate == last.edge_) {
					laterI = i;
					break;
				}
				previous = next;
			}

			if (laterI == std::numeric_limits<psize_t>::max()) {
				point_t later;
				if (last.vector_ == (last.edge_.second - last.edge_.first)) {
					later = last.edge_.second;
				} else {
					later = last.edge_.first;
				}

				laterI = find_point(rA, later);
			}
		} else {
			point_t later;
			if (last.vector_ == (last.edge_.second - last.edge_.first)) {
				later = last.edge_.second;
			} else {
				later = last.edge_.first;
			}

			laterI = find_point(rA, later);
		}

		if (laterI == std::numeric_limits<psize_t>::max()) {
			throw std::runtime_error(
					"Internal error: Can't find later point of last edge");
		}

		std::vector<segment_t> viableEdges;
		previous = rA[laterI];
		for(psize_t i = laterI + 1; i < rA.size() + laterI + 1; ++i) {
			if(i >= rA.size())
				next = rA[i % rA.size() + 1];
			else
				next = rA[i];

			viableEdges.push_back({previous, next});
			previous = next;
		}

//		auto rng = std::default_random_engine {};
//		std::shuffle(std::begin(viableEdges), std::end(viableEdges), rng);

		//search with consulting the history to prevent oscillation
		std::vector<TranslationVector> viableTrans;
		for(const auto& ve: viableEdges) {
			for(const auto& tv : tvs) {
				if((tv.fromA_ && (normalize(tv.vector_) == normalize(ve.second - ve.first))) && (tv.edge_.first != last.edge_.first || tv.edge_.second != last.edge_.second) && !find(historyCopy, tv)) {
					viableTrans.push_back(tv);
				}
			}
			for (const auto& tv : tvs) {
				if (!tv.fromA_) {
					point_t later;
					if (tv.vector_ == (tv.edge_.second - tv.edge_.first) && (tv.edge_.first != last.edge_.first || tv.edge_.second != last.edge_.second)) {
						later = tv.edge_.second;
					} else if (tv.vector_ == (tv.edge_.first - tv.edge_.second)) {
						later = tv.edge_.first;
					} else
						continue;

					if (later == ve.first || later == ve.second) {
						viableTrans.push_back(tv);
					}
				}
			}
		}

		if(!viableTrans.empty())
			return getLongest(viableTrans);

		//search again without the history
		for(const auto& ve: viableEdges) {
			for(const auto& tv : tvs) {
				if((tv.fromA_ && (normalize(tv.vector_) == normalize(ve.second - ve.first))) && (tv.edge_.first != last.edge_.first || tv.edge_.second != last.edge_.second)) {
					viableTrans.push_back(tv);
				}
			}
			for (const auto& tv : tvs) {
				if (!tv.fromA_) {
					point_t later;
					if (tv.vector_ == (tv.edge_.second - tv.edge_.first) && (tv.edge_.first != last.edge_.first || tv.edge_.second != last.edge_.second)) {
						later = tv.edge_.second;
					} else if (tv.vector_ == (tv.edge_.first - tv.edge_.second)) {
						later = tv.edge_.first;
					} else
						continue;

					if (later == ve.first || later == ve.second) {
						viableTrans.push_back(tv);
					}
				}
			}
		}
		if(!viableTrans.empty())
			return getLongest(viableTrans);

		/*
		//search again without the history and without checking last edge
		for(const auto& ve: viableEdges) {
			for(const auto& tv : tvs) {
				if((tv.fromA_ && (normalize(tv.vector_) == normalize(ve.second - ve.first)))) {
					return tv;
				}
			}
			for (const auto& tv : tvs) {
				if (!tv.fromA_) {
					point_t later;
					if (tv.vector_ == (tv.edge_.second - tv.edge_.first)) {
						later = tv.edge_.second;
					} else if (tv.vector_ == (tv.edge_.first - tv.edge_.second)) {
						later = tv.edge_.first;
					} else
						continue;

					if (later == ve.first || later == ve.second) {
						return tv;
					}
				}
			}
		}*/

		if(tvs.size() == 1)
			return *tvs.begin();

		TranslationVector tv;
		tv.vector_ = INVALID_POINT;
		return tv;
	} else {
		return getLongest(tvs);
	}
}
#endif

bool inNfp(const point_t& pt, const nfp_t& nfp)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	for(const auto& r : nfp) {
		if(bg::touches(pt, r))
			return true;
	}

	return false;
}
#endif

enum SearchStartResult {
	FIT,
	FOUND,
	NOT_FOUND
};

SearchStartResult searchStartTranslation(polygon_t::ring_type& rA, const polygon_t::ring_type& rB, const nfp_t& nfp,const bool& inside, point_t& result)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	for(psize_t i = 0; i < rA.size() - 1; i++) {
		psize_t index;
		if (i >= rA.size())
			index = i % rA.size() + 1;
		else
			index = i;

		auto& ptA = rA[index];

		if(ptA.marked_)
			continue;

		ptA.marked_ = true;

		for(const auto& ptB: rB) {
			point_t testTranslation = ptA - ptB;
			polygon_t::ring_type translated;
			boost::geometry::transform(rB, translated, trans::translate_transformer<coord_t, 2, 2>(testTranslation.x_, testTranslation.y_));

			//check if the translated rB is identical to rA
			bool identical = false;
			for(const auto& ptT: translated) {
				identical = false;
				for(const auto& ptA: rA) {
					if(ptT == ptA) {
						identical = true;
						break;
					}
				}
				if(!identical)
					break;
			}

			if(identical) {
				result = testTranslation;
				return FIT;
			}

			bool bInside = false;
			for(const auto& ptT: translated) {
				if(bg::within(ptT, rA)) {
					bInside = true;
					break;
				} else if(!bg::touches(ptT, rA)) {
					bInside = false;
					break;
				}
			}

			if(((bInside && inside) || (!bInside && !inside)) && (!bg::overlaps(translated, rA) && !bg::covered_by(translated, rA) && !bg::covered_by(rA, translated)) && !inNfp(translated.front(), nfp)){
				result = testTranslation;
				return FOUND;
			}

			point_t nextPtA = rA[index + 1];
			TranslationVector slideVector;
			slideVector.vector_ = nextPtA - ptA;
			slideVector.edge_ = {ptA, nextPtA};
			slideVector.fromA_ = true;
			TranslationVector trimmed = trimVector(rA, translated, slideVector);
			polygon_t::ring_type translated2;
			trans::translate_transformer<coord_t, 2, 2> trans(trimmed.vector_.x_, trimmed.vector_.y_);
			boost::geometry::transform(translated, translated2, trans);

			//check if the translated rB is identical to rA
			identical = false;
			for(const auto& ptT: translated) {
				identical = false;
				for(const auto& ptA: rA) {
					if(ptT == ptA) {
						identical = true;
						break;
					}
				}
				if(!identical)
					break;
			}

			if(identical) {
				result = trimmed.vector_ + testTranslation;
				return FIT;
			}

			bInside = false;
			for(const auto& ptT: translated2) {
				if(bg::within(ptT, rA)) {
					bInside = true;
					break;
				} else if(!bg::touches(ptT, rA)) {
					bInside = false;
					break;
				}
			}

			if(((bInside && inside) || (!bInside && !inside)) && (!bg::overlaps(translated2, rA) && !bg::covered_by(translated2, rA) && !bg::covered_by(rA, translated2)) && !inNfp(translated2.front(), nfp)){
				result = trimmed.vector_ + testTranslation;
				return FOUND;
			}
		}
	}
	return NOT_FOUND;
}
#endif

enum SlideResult {
	LOOP,
	NO_LOOP,
	NO_TRANSLATION
};

SlideResult slide(polygon_t& pA, polygon_t::ring_type& rA, polygon_t::ring_type& rB, nfp_t& nfp, const point_t& transB, bool inside)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	polygon_t::ring_type rifsB;
	boost::geometry::transform(rB, rifsB, trans::translate_transformer<coord_t, 2, 2>(transB.x_, transB.y_));
	rB = std::move(rifsB);

#ifdef NFP_DEBUG
	write_svg("ifs.svg", pA, rB);
#endif

	bool startAvailable = true;
	psize_t cnt = 0;
	point_t referenceStart = rB.front();
	std::vector<TranslationVector> history;

	//generate the nfp for the ring
	while(startAvailable) {
		DEBUG_VAL(cnt);
		//use first point of rB as reference
		nfp.back().push_back(rB.front());
		if(cnt == 15)
			std::cerr << "";

		std::vector<TouchingPoint> touchers = findTouchingPoints(rA, rB);

#ifdef NFP_DEBUG
		DEBUG_MSG("touchers", touchers.size());
		for(auto t : touchers) {
			DEBUG_VAL(t.type_);
		}
#endif
		if(touchers.empty()) {
			throw std::runtime_error("Internal error: No touching points found");
		}
		std::vector<TranslationVector> transVectors = findFeasibleTranslationVectors(rA, rB, touchers);

#ifdef NFP_DEBUG
		DEBUG_MSG("collected vectors", transVectors.size());
		for(auto pt : transVectors) {
			DEBUG_VAL(pt);
		}
#endif

		if(transVectors.empty()) {
			return NO_LOOP;
		}

		TranslationVector next = selectNextTranslationVector(pA, rA, rB, transVectors, history);

		if(next.vector_ == INVALID_POINT)
			return NO_TRANSLATION;

		DEBUG_MSG("next", next);

		TranslationVector trimmed = trimVector(rA, rB, next);
		DEBUG_MSG("trimmed", trimmed);

		history.push_back(next);

		polygon_t::ring_type nextRB;
		boost::geometry::transform(rB, nextRB, trans::translate_transformer<coord_t, 2, 2>(trimmed.vector_.x_, trimmed.vector_.y_));
		rB = std::move(nextRB);

#ifdef NFP_DEBUG
		write_svg("next" + std::to_string(cnt) + ".svg", pA,rB);
#endif

		++cnt;
		if(referenceStart == rB.front() || (inside && bg::touches(rB.front(), nfp.front()))) {
			startAvailable = false;
		}
	}
	return LOOP;
}
#endif

void removeCoLinear(polygon_t::ring_type& r)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	assert(r.size() > 2);
	psize_t nextI;
	psize_t prevI = 0;
	segment_t segment(r[r.size() - 2], r[0]);
	polygon_t::ring_type newR;

	for (psize_t i = 1; i < r.size() + 1; ++i) {
		if (i >= r.size())
			nextI = i % r.size() + 1;
		else
			nextI = i;

		if (get_alignment(segment, r[nextI]) != ON) {
			newR.push_back(r[prevI]);
		}
		segment = {segment.second, r[nextI]};
		prevI = nextI;
	}

	r = newR;
}
#endif

void removeCoLinear(polygon_t& p)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	removeCoLinear(p.outer());
	for (auto& r : p.inners())
		removeCoLinear(r);
}
#endif

nfp_t generateNFP(polygon_t& pA, polygon_t& pB, const bool checkValidity = true)
#ifndef LIBNFP_PROTOTYPES_IMPLEMENTATION
;
#else
{
	removeCoLinear(pA);
	removeCoLinear(pB);

	if(checkValidity)  {
		std::string reason;
		if(!bg::is_valid(pA, reason))
			throw std::runtime_error("Polygon A is invalid: " + reason);

		if(!bg::is_valid(pB, reason))
			throw std::runtime_error("Polygon B is invalid: " + reason);
	}

	nfp_t nfp;

#ifdef NFP_DEBUG
	write_svg("start.svg", {pA, pB});
#endif

	DEBUG_VAL(bg::wkt(pA))
	DEBUG_VAL(bg::wkt(pB));

	//prevent double vertex connections at start because we might come back the same way we go which would end the nfp prematurely
	std::vector<psize_t> ptyaminI = find_minimum_y(pA);
	std::vector<psize_t> ptybmaxI = find_maximum_y(pB);

	point_t pAstart;
	point_t pBstart;

	if(ptyaminI.size() > 1 || ptybmaxI.size() > 1) {
		//find right-most of A and left-most of B to prevent double connection at start
		coord_t maxX = MIN_COORD;
		psize_t iRightMost = 0;
		for(psize_t& ia : ptyaminI) {
			const point_t& candidateA = pA.outer()[ia];
			if(larger(candidateA.x_, maxX)) {
				maxX = candidateA.x_;
				iRightMost = ia;
			}
		}

		coord_t minX = MAX_COORD;
		psize_t iLeftMost = 0;
		for(psize_t& ib : ptybmaxI) {
			const point_t& candidateB = pB.outer()[ib];
			if(smaller(candidateB.x_, minX)) {
				minX = candidateB.x_;
				iLeftMost = ib;
			}
		}
		pAstart = pA.outer()[iRightMost];
		pBstart = pB.outer()[iLeftMost];
	} else {
		pAstart = pA.outer()[ptyaminI.front()];
		pBstart = pB.outer()[ptybmaxI.front()];
	}

	nfp.push_back({});
	point_t transB = {pAstart - pBstart};

	if(slide(pA, pA.outer(), pB.outer(), nfp, transB, false) != LOOP) {
			throw std::runtime_error("Unable to complete outer nfp loop");
	}

	DEBUG_VAL("##### outer #####");
	point_t startTrans;
  while(true) {
  	SearchStartResult res = searchStartTranslation(pA.outer(), pB.outer(), nfp, false, startTrans);
  	if(res == FOUND) {
			nfp.push_back({});
			DEBUG_VAL("##### interlock start #####")
			polygon_t::ring_type rifsB;
			boost::geometry::transform(pB.outer(), rifsB, trans::translate_transformer<coord_t, 2, 2>(startTrans.x_, startTrans.y_));
			if(inNfp(rifsB.front(), nfp)) {
				continue;
			}
			SlideResult sres = slide(pA, pA.outer(), pB.outer(), nfp, startTrans, true);
			if(sres != LOOP) {
				if(sres == NO_TRANSLATION) {
					//no initial slide found -> jiggsaw
					if(!inNfp(pB.outer().front(),nfp)) {
						nfp.push_back({});
						nfp.back().push_back(pB.outer().front());
					}
				}
			}
			DEBUG_VAL("##### interlock end #####");
  	} else if(res == FIT) {
  		point_t reference = pB.outer().front();
  		point_t translated;
			trans::translate_transformer<coord_t, 2, 2> translate(startTrans.x_, startTrans.y_);
			boost::geometry::transform(reference, translated, translate);
			if(!inNfp(translated,nfp)) {
				nfp.push_back({});
				nfp.back().push_back(translated);
			}
			break;
  	} else
  		break;
	}

	for(auto& rA : pA.inners()) {
		while(true) {
			SearchStartResult res = searchStartTranslation(rA, pB.outer(), nfp, true, startTrans);
			if(res == FOUND) {
				nfp.push_back({});
				DEBUG_VAL("##### hole start #####");
				slide(pA, rA, pB.outer(), nfp, startTrans, true);
				DEBUG_VAL("##### hole end #####");
			} else if(res == FIT) {
	  		point_t reference = pB.outer().front();
	  		point_t translated;
				trans::translate_transformer<coord_t, 2, 2> translate(startTrans.x_, startTrans.y_);
				boost::geometry::transform(reference, translated, translate);
				if(!inNfp(translated,nfp)) {
					nfp.push_back({});
					nfp.back().push_back(translated);
				}
				break;
			} else
				break;
		}
  }

#ifdef NFP_DEBUG
  write_svg("nfp.svg", {pA,pB}, nfp);
#endif

  return nfp;
}
#endif

}

#endif


// Boost.Geometry Index
//
// R-tree R*-tree split algorithm implementation
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP

#include <boost/geometry/index/detail/algorithms/intersection_content.hpp>
#include <boost/geometry/index/detail/algorithms/union_content.hpp>
#include <boost/geometry/index/detail/algorithms/margin.hpp>

#include <boost/geometry/index/detail/rtree/node/node.hpp>
#include <boost/geometry/index/detail/rtree/visitors/insert.hpp>
#include <boost/geometry/index/detail/rtree/visitors/is_leaf.hpp>

namespace boost { namespace geometry { namespace index {

namespace detail { namespace rtree {

namespace rstar {

template <typename Element, typename Translator, typename Tag, size_t Corner, size_t AxisIndex>
class element_axis_corner_less
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_TAG, (Tag));
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, box_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<Corner, AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<Corner, AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, point_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Parameters, typename Box, size_t Corner, size_t AxisIndex>
struct choose_split_axis_and_index_for_corner
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename Elements::value_type element_type;
        typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
        typedef typename tag<indexable_type>::type indexable_tag;

        BOOST_GEOMETRY_INDEX_ASSERT(elements.size() == parameters.get_max_elements() + 1, "wrong number of elements");

        // copy elements
        Elements elements_copy(elements);                                                                       // MAY THROW, STRONG (alloc, copy)

        // sort elements
        element_axis_corner_less<element_type, Translator, indexable_tag, Corner, AxisIndex> elements_less(translator);
        std::sort(elements_copy.begin(), elements_copy.end(), elements_less);                                   // MAY THROW, BASIC (copy)

        // init outputs
        choosen_index = parameters.get_min_elements();
        sum_of_margins = 0;
        smallest_overlap = (std::numeric_limits<content_type>::max)();
        smallest_content = (std::numeric_limits<content_type>::max)();

        // calculate sum of margins for all distributions
        size_t index_last = parameters.get_max_elements() - parameters.get_min_elements() + 2;
        for ( size_t i = parameters.get_min_elements() ; i < index_last ; ++i )
        {
            // TODO - awulkiew: may be optimized - box of group 1 may be initialized with
            // box of min_elems number of elements and expanded for each iteration by another element

            Box box1 = rtree::elements_box<Box>(elements_copy.begin(), elements_copy.begin() + i, translator);
            Box box2 = rtree::elements_box<Box>(elements_copy.begin() + i, elements_copy.end(), translator);

            sum_of_margins += index::detail::comparable_margin(box1) + index::detail::comparable_margin(box2);

            content_type ovl = index::detail::intersection_content(box1, box2);
            content_type con = index::detail::content(box1) + index::detail::content(box2);

            // TODO - shouldn't here be < instead of <= ?
            if ( ovl < smallest_overlap || (ovl == smallest_overlap && con <= smallest_content) )
            {
                choosen_index = i;
                smallest_overlap = ovl;
                smallest_content = con;
            }
        }

        ::boost::ignore_unused_variable_warning(parameters);
    }
};

template <typename Parameters, typename Box, size_t AxisIndex, typename ElementIndexableTag>
struct choose_split_axis_and_index_for_axis
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_TAG, (ElementIndexableTag));
};

template <typename Parameters, typename Box, size_t AxisIndex>
struct choose_split_axis_and_index_for_axis<Parameters, Box, AxisIndex, box_tag>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        size_t index1 = 0;
        margin_type som1 = 0;
        content_type ovl1 = (std::numeric_limits<content_type>::max)();
        content_type con1 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Parameters, Box, min_corner, AxisIndex>::
            apply(elements, index1,
                  som1, ovl1, con1,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        size_t index2 = 0;
        margin_type som2 = 0;
        content_type ovl2 = (std::numeric_limits<content_type>::max)();
        content_type con2 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Parameters, Box, max_corner, AxisIndex>::
            apply(elements, index2,
                  som2, ovl2, con2,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        sum_of_margins = som1 + som2;

        if ( ovl1 < ovl2 || (ovl1 == ovl2 && con1 <= con2) )
        {
            choosen_corner = min_corner;
            choosen_index = index1;
            smallest_overlap = ovl1;
            smallest_content = con1;
        }
        else
        {
            choosen_corner = max_corner;
            choosen_index = index2;
            smallest_overlap = ovl2;
            smallest_content = con2;
        }
    }
};

template <typename Parameters, typename Box, size_t AxisIndex>
struct choose_split_axis_and_index_for_axis<Parameters, Box, AxisIndex, point_tag>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        choose_split_axis_and_index_for_corner<Parameters, Box, min_corner, AxisIndex>::
            apply(elements, choosen_index,
                  sum_of_margins, smallest_overlap, smallest_content,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        choosen_corner = min_corner;
    }
};

template <typename Parameters, typename Box, size_t Dimension>
struct choose_split_axis_and_index
{
    BOOST_STATIC_ASSERT(0 < Dimension);

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choose_split_axis_and_index<Parameters, Box, Dimension - 1>::
            apply(elements, choosen_axis, choosen_corner, choosen_index,
                  smallest_sum_of_margins, smallest_overlap, smallest_content,
                  parameters, translator);                                                                  // MAY THROW, STRONG

        margin_type sum_of_margins = 0;

        size_t corner = min_corner;
        size_t index = 0;

        content_type overlap_val = (std::numeric_limits<content_type>::max)();
        content_type content_val = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_axis<
            Parameters,
            Box,
            Dimension - 1,
            typename tag<element_indexable_type>::type
        >::apply(elements, corner, index, sum_of_margins, overlap_val, content_val, parameters, translator); // MAY THROW, STRONG

        if ( sum_of_margins < smallest_sum_of_margins )
        {
            choosen_axis = Dimension - 1;
            choosen_corner = corner;
            choosen_index = index;
            smallest_sum_of_margins = sum_of_margins;
            smallest_overlap = overlap_val;
            smallest_content = content_val;
        }
    }
};

template <typename Parameters, typename Box>
struct choose_split_axis_and_index<Parameters, Box, 1>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choosen_axis = 0;

        choose_split_axis_and_index_for_axis<
            Parameters,
            Box,
            0,
            typename tag<element_indexable_type>::type
        >::apply(elements, choosen_corner, choosen_index, smallest_sum_of_margins, smallest_overlap, smallest_content, parameters, translator); // MAY THROW
    }
};

template <size_t Corner, size_t Dimension>
struct partial_sort
{
    BOOST_STATIC_ASSERT(0 < Dimension);

    template <typename Elements, typename Translator>
    static inline void apply(Elements & elements, const size_t axis, const size_t index, Translator const& tr)
    {
        if ( axis < Dimension - 1 )
        {
            partial_sort<Corner, Dimension - 1>::apply(elements, axis, index, tr);                          // MAY THROW, BASIC (copy)
        }
        else
        {
            BOOST_GEOMETRY_INDEX_ASSERT(axis == Dimension - 1, "unexpected axis value");

            typedef typename Elements::value_type element_type;
            typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
            typedef typename tag<indexable_type>::type indexable_tag;

            element_axis_corner_less<element_type, Translator, indexable_tag, Corner, Dimension - 1> less(tr);
            std::partial_sort(elements.begin(), elements.begin() + index, elements.end(), less);            // MAY THROW, BASIC (copy)
        }
    }
};

template <size_t Corner>
struct partial_sort<Corner, 1>
{
    template <typename Elements, typename Translator>
    static inline void apply(Elements & elements,
                             const size_t BOOST_GEOMETRY_INDEX_ASSERT_UNUSED_PARAM(axis),
                             const size_t index,
                             Translator const& tr)
    {
        BOOST_GEOMETRY_INDEX_ASSERT(axis == 0, "unexpected axis value");

        typedef typename Elements::value_type element_type;
        typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
        typedef typename tag<indexable_type>::type indexable_tag;

        element_axis_corner_less<element_type, Translator, indexable_tag, Corner, 0> less(tr);
        std::partial_sort(elements.begin(), elements.begin() + index, elements.end(), less);                // MAY THROW, BASIC (copy)
    }
};

} // namespace rstar

template <typename Value, typename Options, typename Translator, typename Box, typename Allocators>
struct redistribute_elements<Value, Options, Translator, Box, Allocators, rstar_tag>
{
    typedef typename rtree::node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type node;
    typedef typename rtree::internal_node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type internal_node;
    typedef typename rtree::leaf<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type leaf;

    typedef typename Options::parameters_type parameters_type;

    static const size_t dimension = geometry::dimension<Box>::value;

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Node>
    static inline void apply(
        Node & n,
        Node & second_node,
        Box & box1,
        Box & box2,
        parameters_type const& parameters,
        Translator const& translator,
        Allocators & allocators)
    {
        typedef typename rtree::elements_type<Node>::type elements_type;

        elements_type & elements1 = rtree::elements(n);
        elements_type & elements2 = rtree::elements(second_node);

        size_t split_axis = 0;
        size_t split_corner = 0;
        size_t split_index = parameters.get_min_elements();
        margin_type smallest_sum_of_margins = (std::numeric_limits<margin_type>::max)();
        content_type smallest_overlap = (std::numeric_limits<content_type>::max)();
        content_type smallest_content = (std::numeric_limits<content_type>::max)();

        rstar::choose_split_axis_and_index<
            typename Options::parameters_type,
            Box,
            dimension
        >::apply(elements1,
                 split_axis, split_corner, split_index,
                 smallest_sum_of_margins, smallest_overlap, smallest_content,
                 parameters, translator);                                                               // MAY THROW, STRONG

        // TODO: awulkiew - get rid of following static_casts?

        BOOST_GEOMETRY_INDEX_ASSERT(split_axis < dimension, "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(split_corner == static_cast<size_t>(min_corner) || split_corner == static_cast<size_t>(max_corner), "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(parameters.get_min_elements() <= split_index && split_index <= parameters.get_max_elements() - parameters.get_min_elements() + 1, "unexpected value");

        // copy original elements
        elements_type elements_copy(elements1);                                                         // MAY THROW, STRONG
        elements_type elements_backup(elements1);                                                       // MAY THROW, STRONG

        // TODO: awulkiew - check if std::partial_sort produces the same result as std::sort
        if ( split_corner == static_cast<size_t>(min_corner) )
            rstar::partial_sort<min_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)
        else
            rstar::partial_sort<max_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)

        BOOST_TRY
        {
            // copy elements to nodes
            elements1.assign(elements_copy.begin(), elements_copy.begin() + split_index);               // MAY THROW, BASIC
            elements2.assign(elements_copy.begin() + split_index, elements_copy.end());                 // MAY THROW, BASIC

            // calculate boxes
            box1 = rtree::elements_box<Box>(elements1.begin(), elements1.end(), translator);
            box2 = rtree::elements_box<Box>(elements2.begin(), elements2.end(), translator);
        }
        BOOST_CATCH(...)
        {
            //elements_copy.clear();
            elements1.clear();
            elements2.clear();

            rtree::destroy_elements<Value, Options, Translator, Box, Allocators>::apply(elements_backup, allocators);
            //elements_backup.clear();

            BOOST_RETHROW                                                                                // RETHROW, BASIC
        }
        BOOST_CATCH_END
    }
};

}} // namespace detail::rtree

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP


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

  Non-trivial node types (class lables) for MRF models.

  Copyright (c) 2012, 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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 NODE_TYPES_HPP_6CF0A0E1_8AE9_4951_A785_9339B90FB976
#define NODE_TYPES_HPP_6CF0A0E1_8AE9_4951_A785_9339B90FB976

#include <cmath>
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/operators.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace bo {
namespace mrf {

template <typename P>
class ParametricNodeType: boost::equality_comparable1<ParametricNodeType<P> >
{
public:
    typedef boost::shared_ptr<P> ClassParamsPtr;

    ParametricNodeType(ClassParamsPtr class_params): class_params_(class_params)
    { }

    //  Generated copy c-tor and assignment operator are fine.

    virtual ~ParametricNodeType()
    { }

    // Classes are equal when their parameters are equal.
    virtual bool operator==(const ParametricNodeType<P>& other) const
    { return (class_params_ == other.class_params_); }

protected:
    ClassParamsPtr class_params_;
};

// A class representing a value (class label) with associated Gauss distribution
// parameters. Parameters structure is represented as a tuple with 4 elements:
// class label, mean, standard deviation, and additional item a = ln(sigma * sqrt(2 pi)).
template <typename RealType>
class GaussDistrClasses: public ParametricNodeType<
        boost::tuples::tuple<int, RealType, RealType, RealType> >
{
public:
    typedef GaussDistrClasses<RealType> SelfType;

    typedef boost::tuples::tuple<int, RealType, RealType, RealType> ClassParams;
    typedef ParametricNodeType<ClassParams> BaseType;
    typedef typename BaseType::ClassParamsPtr ClassParamsPtr;

public:
    GaussDistrClasses(ClassParamsPtr class_params): BaseType(class_params)
    { }

    static GaussDistrClasses CreateInstance(int idx, RealType mean, RealType sigma)
    { return (SelfType(boost::make_shared<ClassParams>(idx, mean, sigma, compute_a(sigma)))); }

    // Accessors for class label and class parameters.
    int label() const
    { return this->class_params_->template get<0>(); }

    RealType mean() const
    { return this->class_params_->template get<1>(); }

    RealType sigma() const
    { return this->class_params_->template get<2>(); }

    RealType a() const
    { return this->class_params_->template get<3>(); }

    // 2.5 is precomputed sqrt(2 pi).
    static RealType compute_a(RealType sigma)
    { return (std::log(sigma * RealType(2.5))); }
};

// A class representing a value (class label) with associated Gamma distribution
// parameters. Parameters structure is represented as a tuple with 4 elements:
// class label and a set of Gamma distribution parameters (k, theta, a), where
// a = ln(G(k)) + k ln(theta) and G(t) is the Gamma function.
template <typename RealType>
class GammaDistrClasses: public ParametricNodeType<
        boost::tuples::tuple<int, RealType, RealType, RealType> >
{
public:
    typedef GammaDistrClasses<RealType> SelfType;

    typedef boost::tuples::tuple<int, RealType, RealType, RealType> ClassParams;
    typedef ParametricNodeType<ClassParams> BaseType;
    typedef typename BaseType::ClassParamsPtr ClassParamsPtr;

public:
    GammaDistrClasses(ClassParamsPtr class_params): BaseType(class_params)
    { }

    static GammaDistrClasses CreateInstance(int idx, RealType k, RealType theta)
    { return (SelfType(boost::make_shared<ClassParams>(idx, k, theta, compute_a(k, theta)))); }

    // Accessors for class label and class parameters.
    int label() const
    { return this->class_params_->template get<0>(); }

    RealType k() const
    { return this->class_params_->template get<1>(); }

    RealType theta() const
    { return this->class_params_->template get<2>(); }

    RealType a() const
    { return this->class_params_->template get<3>(); }

    RealType mean() const
    { return (k() * theta()); }

    static RealType compute_a(RealType k, RealType theta)
    { return (boost::math::lgamma(k) + k * std::log(theta)); }
};

template <typename T>
std::ostream& operator<<(std::ostream& os, const GaussDistrClasses<T>& obj)
{
    os << obj.label() << " (mean: " << obj.mean() << ", sigma: " << obj.sigma() << ")";
    return os;
}

template <typename T>
std::ostream& operator<<(std::ostream& os, const GammaDistrClasses<T>& obj)
{
    os << obj.label() << " (k: " << obj.k() << ", theta: " << obj.theta() << ")";
    return os;
}

} // namespace mrf
} // namespace bo

#endif // NODE_TYPES_HPP_6CF0A0E1_8AE9_4951_A785_9339B90FB976