//
//! Copyright © 2008-2011
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_UTILITIES_HPP
#define GEOMETRIX_UTILITIES_HPP
#pragma once

#include <geometrix/numeric/constants.hpp>
#include <geometrix/arithmetic/arithmetic.hpp>
#include <geometrix/tensor/numeric_sequence_compare.hpp>
#include <geometrix/primitive/segment_traits.hpp>
#include <geometrix/arithmetic/vector.hpp>
#include <geometrix/algebra/expression.hpp>
#include <geometrix/algebra/algebra.hpp>
#include <geometrix/algebra/exterior_product.hpp>
#include <boost/concept_check.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/fusion/include/all.hpp>
#include <geometrix/algorithm/orientation/point_segment_orientation.hpp>

namespace geometrix {

    namespace result_of {

        template <typename A, typename B>
        struct angle_from_a_to_b
        {
        private:
            typedef decltype(typename type_at<A, 0>::type() - typename type_at<B, 0>::type()) xtype;
            typedef decltype(typename type_at<A, 1>::type() - typename type_at<B, 1>::type()) ytype;
        public:
            typedef decltype(atan2(std::declval<ytype>(), std::declval<xtype>())) type;
        };

    }//! namespace result_of;

    //! Function to get the angle from an origin to a target point in the 2D XY plane.
    template <typename CoordinateSequenceA, typename CoordinateSequenceB>
    inline typename result_of::angle_from_a_to_b<CoordinateSequenceA, CoordinateSequenceB>::type
        angle_from_a_to_b( const CoordinateSequenceA& A,
                           const CoordinateSequenceB& B,
                       typename boost::enable_if_c
                       <
                           geometric_traits<CoordinateSequenceA>::dimension_type::value == 2 &&
                           geometric_traits<CoordinateSequenceB>::dimension_type::value == 2
                       > ::type* = 0 )
    {
        using std::atan2;
        return atan2( get<1>( B ) - get<1>( A ), get<0>( B ) - get<0>( A ) );
    }

    namespace result_of {

        template <typename Vector>
        struct vector_angle
        {
        private:
            typedef decltype(typename type_at<Vector, 0>::type()) xtype;
            typedef decltype(typename type_at<Vector, 1>::type()) ytype;
        public:
            typedef decltype(atan2(std::declval<ytype>(), std::declval<xtype>())) type;
        };

    }//! namespace result_of;

    //! Return the angle in which the specified vector points.
    template <typename Vector>
    inline typename result_of::vector_angle<Vector>::type
        vector_angle(const Vector& v)
    {
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector>));
        using std::atan2;
        return atan2(get<1>(v), get<0>(v));
    }

    //! Function to normalize an angle to within the interval [0,2*PI]
    template <typename CoordinateType>
    inline void normalize_angle_0_2pi(CoordinateType& angle)
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi
        CoordinateType twoPI = constants::two_pi<CoordinateType>();
        if (angle > twoPI || angle < constants::zero<CoordinateType>())
        {
            using std::floor;
            auto n = floor(angle / twoPI);
            if (n != constants::zero<decltype(n)>())
                angle -= twoPI * n;
            if (angle > twoPI)
                angle -= twoPI;
            else if (angle < constants::zero<CoordinateType>())
                angle += twoPI;
        }
    }

    //! Function to normalize a copy of a given angle to within the interval [0,2*PI] and return the normalized value.
    template <typename CoordinateType>
    inline CoordinateType normalize_angle_0_2pi_copy(CoordinateType angle)
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi
        normalize_angle_0_2pi(angle);
        return angle;
    }

    //! Function to normalize an angle to within the interval [-PI,PI]
    template <typename CoordinateType>
    inline void normalize_angle_minus_pi_to_pi( CoordinateType& angle )
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi first and then into the -pi,pi range.
        normalize_angle_0_2pi(angle);

        CoordinateType pi = constants::pi<CoordinateType>();
        CoordinateType twoPI = constants::two_pi<CoordinateType>();
        if( angle > pi )
            angle -= twoPI;
        else if( angle <= geometrix::get(-pi) )
            angle += twoPI;
    }

    //! Function to normalize a copy of a given angle to within the interval [-PI,PI] and return the normalized value.
    template <typename CoordinateType>
    inline CoordinateType normalize_angle_minus_pi_to_pi_copy( CoordinateType angle )
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi
        normalize_angle_minus_pi_to_pi(angle);
        return angle;
    }

    //! Function to determine if 3 points are collinear in the 2D XY plane.
    //! From Computational Geometry in C by J. O'Rourke.
    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_collinear( const PointA& A,
                       const PointB& B,
                       const PointC& C,
                       const NumberComparisonPolicy& compare,
                       typename boost::enable_if_c
                       <
                           geometric_traits<PointA>::dimension_type::value == 2 &&
                           geometric_traits<PointB>::dimension_type::value == 2 &&
                           geometric_traits<PointC>::dimension_type::value == 2
                       > ::type* = 0 )
    {
        auto det = exterior_product_area( B-A, C-A );
        return compare.equals( det, constants::zero<decltype(det)>() );//Absolute tolerance checks are fine for Zero checks.
    }

    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_collinear( const PointA& A,
                       const PointB& B,
                       const PointC& C,
                       const NumberComparisonPolicy& compare,
                       typename boost::enable_if_c
                       <
                           geometric_traits<PointA>::dimension_type::value == 3 &&
                           geometric_traits<PointB>::dimension_type::value == 3 &&
                           geometric_traits<PointC>::dimension_type::value == 3
                       > ::type* = 0 )
    {
        using length_t = typename arithmetic_type_of<PointA>::type;
        using vector_t = vector<length_t, dimension_of<PointA>::value>;
        using cross_result_t = typename result_of::cross_product<vector_t, vector_t>::type;
		auto d = cross_result_t(( B - A ) ^ ( C - A ));
		return compare.equals( get<0>( d ), constants::zero<typename type_at<cross_result_t, 0>::type>() )
			&& compare.equals( get<1>( d ), constants::zero<typename type_at<cross_result_t, 0>::type>() )
			&& compare.equals( get<2>( d ), constants::zero<typename type_at<cross_result_t, 0>::type>() );
    }

    //! Function to determine if Point C is between points A-B
    //! From Computational Geometry in C by J. O'Rourke.
    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_between( const PointA& A,
                     const PointB& B,
                     const PointC& C,
                     bool includeBounds,
                     const NumberComparisonPolicy& compare,
                     typename boost::enable_if_c
                     <
                        geometric_traits<PointA>::dimension_type::value == 2 &&
                        geometric_traits<PointB>::dimension_type::value == 2 &&
                        geometric_traits<PointC>::dimension_type::value == 2
                     >::type* = 0 )
    {
        return is_collinear( A, B, C, compare ) && is_collinear_point_between( A, B, C, includeBounds, compare );
    }

    //! Function to determine if Point C is between points A-B where C is already determined to be collinear to A-B.
    //! From Computational Geometry in C by J. O'Rourke.
    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_collinear_point_between( const PointA& A,
        const PointB& B,
        const PointC& C,
        bool includeBounds,
        const NumberComparisonPolicy& compare,
        typename boost::enable_if_c
        <
        geometric_traits<PointA>::dimension_type::value == 2 &&
        geometric_traits<PointB>::dimension_type::value == 2 &&
        geometric_traits<PointC>::dimension_type::value == 2
        > ::type* = 0 )
    {
        GEOMETRIX_ASSERT( is_collinear( A, B, C, compare ) );

        //If AB not vertical, check between on x; else on y.
        bool ABVertical = compare.equals( get<0>( A ), get<0>( B ) );
        if( !ABVertical )
        {
            if( includeBounds )
            {
                return ((compare.less_than_or_equal( get<0>( A ), get<0>( C ) ) && compare.less_than_or_equal( get<0>( C ), get<0>( B ) )) ||
                    (compare.greater_than_or_equal( get<0>( A ), get<0>( C ) ) && compare.greater_than_or_equal( get<0>( C ), get<0>( B ) )));
            }
            else
            {
                return ((compare.less_than( get<0>( A ), get<0>( C ) ) && compare.less_than( get<0>( C ), get<0>( B ) )) ||
                    (compare.greater_than( get<0>( A ), get<0>( C ) ) && compare.greater_than( get<0>( C ), get<0>( B ) )));
            }
        }
        else
        {
            if( includeBounds )
            {
                return ((compare.less_than_or_equal( get<1>( A ), get<1>( C ) ) && compare.less_than_or_equal( get<1>( C ), get<1>( B ) )) ||
                    (compare.greater_than_or_equal( get<1>( A ), get<1>( C ) ) && compare.greater_than_or_equal( get<1>( C ), get<1>( B ) )));
            }
            else
            {
                return ((compare.less_than( get<1>( A ), get<1>( C ) ) && compare.less_than( get<1>( C ), get<1>( B ) )) ||
                    (compare.greater_than( get<1>( A ), get<1>( C ) ) && compare.greater_than( get<1>( C ), get<1>( B ) )));
            }
        }
    }

    //! \brief Function to determine if vector c falls in between vectors a and b.
    //! This can be used to check angle ranges without using atan2.
    template <typename Vector1, typename Vector2, typename Vector3, typename NumberComparisonPolicy>
    inline bool is_vector_between(const Vector1& a, const Vector2& b, const Vector3& c, bool includeBounds, const NumberComparisonPolicy& cmp)
    {
        using namespace geometrix;
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector1>));
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector2>));
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector3>));
        BOOST_CONCEPT_ASSERT((NumberComparisonPolicyConcept<NumberComparisonPolicy, double>));

        BOOST_AUTO(const detcb, exterior_product_area(c, b));

        //! If b is along c bounds included it's between.
        if (cmp.equals(detcb, constants::zero<decltype(detcb)>()) && cmp.greater_than_or_equal(dot_product(b, c), constants::zero<decltype(dot_product(b,c))>()))
            return includeBounds;

        BOOST_AUTO(const detac, exterior_product_area(a, c));

        //! If a is along c and includeBounds it's between.
        if (cmp.equals(detac, constants::zero<decltype(detac)>()) && cmp.greater_than_or_equal(dot_product(a, c), constants::zero<decltype(dot_product(a,c))>()))
            return includeBounds;

        BOOST_AUTO(const detab, exterior_product_area(a, b));

        //! If b is along a, c can only be between if it is along a and included and that's handled above.
        if (cmp.equals(detab, constants::zero<decltype(detab)>()) && cmp.greater_than_or_equal(dot_product(b, a), constants::zero<decltype(dot_product(b,a))>()))
            return false;

        //! If detab and detac have the same sign, then b and c are on the same side of a and can be compared directly.
        if (cmp.greater_than_or_equal(detac * detab, constants::zero<decltype(detac*detab)>()))
        {
            //! Both are on same side of a; compare to each other.
            return cmp.greater_than(detcb, constants::zero<decltype(detcb)>());
        }

        //! At this point b and c straddle a. A negative determinant means a large angle WRT a.
        //! If c's is positive it must be between a and b, else the opposite must be true.
        return cmp.greater_than(detac, constants::zero<decltype(detac)>());
    }

    template <typename Point, typename NumberComparisonPolicy>
    inline bool is_vertical( const Point& start,
                      const Point& end,
                      const NumberComparisonPolicy& compare )
    {
        return compare.equals( get<0>( start ), get<0>( end ) );
    }

    template <typename Segment, typename NumberComparisonPolicy>
    inline bool is_vertical( const Segment& s,
                      const NumberComparisonPolicy& compare )
    {
        return is_vertical( get_start( s ),
                            get_end( s ),
                            compare );
    }

    template <typename Point, typename NumberComparisonPolicy>
    inline bool is_horizontal( const Point& start, const Point& end, const NumberComparisonPolicy& compare )
    {
        return compare.equals( get<1>( start ),
                               get<1>( end ) );
    }

    template <typename Segment, typename NumberComparisonPolicy>
    inline bool is_horizontal( const Segment& s,const NumberComparisonPolicy& compare )
    {
        return is_horizontal( get_start( s ),
                              get_end( s ),
                              compare );
    }

    //! function to get the slope defined by two points
    template <typename Point>
    inline typename geometric_traits< Point >::arithmetic_type
        get_slope( const Point& s_start, const Point& s_end )
    {
        return arithmetic_promote(get<1>( s_end ) - get<1>( s_start )) / (get<0>( s_end )-get<0>( s_start ));
    }

    //! function to get the slope defined by a segment.
    template <typename Segment>
    inline typename geometric_traits< typename geometric_traits< Segment >::point_type >::arithmetic_type get_slope( const Segment& s )
    {
        return get_slope( get_start( s ), get_end ( s ) );
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType y_of_x( const Point& s_start,
                           const Point& s_end,
                           CoordinateType x,
                           CoordinateType& slope )
    {
        BOOST_AUTO( x0, get<0>( s_start ) );
        BOOST_AUTO( x1, get<0>( s_end ) );
        BOOST_AUTO( y0, get<1>( s_start ) );
        BOOST_AUTO( y1, get<1>( s_end ) );
        slope = arithmetic_promote( (y1-y0) )/(x1-x0);
        return (x - x0) * slope + y0;
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType y_of_x( const Point& s_start,
                           const Point& s_end,
                           CoordinateType x )
    {
        CoordinateType slope;
        return y_of_x( s_start, s_end, x, slope );
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType x_of_y( const Point& s_start,
                           const Point& s_end,
                           CoordinateType y,
                           CoordinateType& slope )
    {
        CoordinateType y0, y1, x0, x1;

        x0 = get<0>( s_start );
        x1 = get<0>( s_end );
        y0 = get<1>( s_start );
        y1 = get<1>( s_end );

        slope = (y1-y0)/(x1-x0);

        CoordinateType x = (y - y0)/slope + x0;
        return x;
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType x_of_y( const Point& s_start,
                           const Point& s_end,
                           CoordinateType y )
    {
        CoordinateType slope;
        return x_of_y( s_start, s_end, y, slope );
    }

    template <typename T1, typename T2>
    inline bool lexicographical_compare(const T1& t1, const T2& t2)
    {
        return t1 < t2;
    }

    template <typename T1, typename T2, typename U, typename ...Params>
    inline bool lexicographical_compare(const T1& t1, const T2& t2, const U& u, const Params&... p)
    {
        return !(t2 < t1) && (t1 < t2 || lexicographical_compare(u, p...));
    }

    namespace detail
    {
        template <std::size_t D>
        struct lexicographical
        {
            template <typename NumericSequence, typename NumberComparisonPolicy>
            static bool compare( const NumericSequence& lhs, const NumericSequence& rhs, const NumberComparisonPolicy& nCompare )
            {
                if( nCompare.less_than( get<dimension_of<NumericSequence>::value - D>( lhs ), get<dimension_of<NumericSequence>::value - D>( rhs ) ) )
                    return true;
                else if( nCompare.equals( get<dimension_of<NumericSequence>::value - D>( lhs ), get<dimension_of<NumericSequence>::value - D>( rhs ) ) )
                    return lexicographical<D-1>::compare( lhs, rhs, nCompare );
                else
                    return false;
            }
        };

        template <>
        struct lexicographical<0>
        {
            template <typename NumericSequence, typename NumberComparisonPolicy>
            static bool compare( const NumericSequence&, const NumericSequence&, const NumberComparisonPolicy& )
            {
                return false;//all were equal.
            }
        };
    }

    //! Lexicographical compare functor for Cartesian points. Sorts first in X and then in Y (then Z).
    template <typename NumberComparisonPolicy>
    class lexicographical_comparer
    {
        template <typename NumericSequence1, typename NumericSequence2>
        struct comparer
        {
            static bool compare( const NumericSequence1& lhs, const NumericSequence2& rhs, const NumberComparisonPolicy& nCompare )
            {
                return detail::lexicographical<dimension_of<NumericSequence1>::value>::compare( lhs, rhs, nCompare );
            }
        };

    public:

        lexicographical_comparer(){}
        lexicographical_comparer( const NumberComparisonPolicy& compare )
            : m_compare( compare )
        {}

        template <typename NumericSequence1, typename NumericSequence2>
        bool operator()( const NumericSequence1& p1, const NumericSequence2& p2 ) const
        {
            return comparer<NumericSequence1, NumericSequence2>::compare( p1, p2, m_compare );
        }

    private:

        NumberComparisonPolicy m_compare;

    };

    //! Test if lhs sequence is lexicographically less than rhs sequence.
    template <typename NumericSequence1, typename NumericSequence2, typename NumberComparisonPolicy>
    inline bool lexicographically_less_than(const NumericSequence1& lhs, const NumericSequence2& rhs, const NumberComparisonPolicy& cmp)
    {
        return lexicographical_comparer<NumberComparisonPolicy>(cmp)(lhs, rhs);
    }

    //! Lexicographical compare functor for points - reversed to sort in (Z), then Y then X.
    template <typename NumberComparisonPolicy>
    struct reverse_lexicographical_point_compare
    {
        reverse_lexicographical_point_compare(){}
        reverse_lexicographical_point_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
        {}

        //! older compilers require disambiguation
        template <int> struct disambiguation_tag { disambiguation_tag(int) {} };

        template <typename Point>
        bool operator()( const Point& p1, const Point& p2, typename boost::enable_if< boost::is_same< typename geometric_traits<Point>::dimension_type, dimension<2> > >::type* = 0, disambiguation_tag<0> = 0 ) const
        {
            return ( m_compare.less_than( get<1>( p1 ), get<1>( p2 ) ) ) ||
                   ( m_compare.equals( get<1>( p1 ), get<1>( p2 ) ) && m_compare.less_than( get<0>( p1 ), get<0>( p2 ) ) );
        }

        template <typename Point>
        bool operator()( const Point& p1, const Point& p2, typename boost::disable_if< boost::is_same< typename geometric_traits<Point>::dimension_type, dimension<2> > >::type* = 0, disambiguation_tag<1> = 0 ) const
        {
            return ( m_compare.less_than( get<2>( p1 ), get<2>( p2 ) ) )||
                   ( m_compare.equals( get<2>( p1 ), get<2>( p2 ) ) && m_compare.less_than( get<1>( p1 ), get<1>( p2 ) ) ) ||
                   ( m_compare.equals( get<2>( p1 ), get<2>( p2 ) ) && m_compare.equals( get<1>( p1 ), get<1>( p2 ) ) && m_compare.less_than( get<0>( p1 ), get<0>( p2 ) ) );
        }

        NumberComparisonPolicy m_compare;

    };

    //! Lexicographical compare functor for Cartesian points. Sorts first in X and then in Y (then Z).
    template <typename NumberComparisonPolicy>
    struct lexicographical_segment_compare
    {
        lexicographical_segment_compare(){}
        lexicographical_segment_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
            , m_pointCompare( compare )
        {}

        template <typename Segment>
        bool operator()( const Segment& s1, const Segment& s2 ) const
        {
            typedef typename geometric_traits< Segment >::point_type point_type;

            const point_type& start1 = get_start( s1 );
            const point_type& end1   = get_end( s2 );
            const point_type& start2 = get_start( s2 );
            const point_type& end2   = get_end( s2 );

            //Order the segments lexicographically first.
            const point_type* lower1;
            const point_type* upper1;
            const point_type* lower2;
            const point_type* upper2;

            if( m_pointCompare( start1, end1 ) )
            {
                lower1 = &start1;
                upper1 = &end1;
            }
            else
            {
                lower1 = &end1;
                upper1 = &start1;
            }

            if( m_pointCompare( start2, end2 ) )
            {
                lower2 = &start2;
                upper2 = &end2;
            }
            else
            {
                lower2 = &end2;
                upper2 = &start2;
            }

            return m_pointCompare( *lower1, *lower2 ) || ( numeric_sequence_equals( *lower1, *lower2, m_compare ) && m_pointCompare( *upper1, *upper2 ) );
        }

        NumberComparisonPolicy                            m_compare;
        lexicographical_comparer< NumberComparisonPolicy > m_pointCompare;

    };

    //! Functor to compare segments lexicographically as intervals A-C !< B-C if [A-B-C].
    template <typename NumberComparisonPolicy>
    struct segment_interval_compare
    {
        typedef lexicographical_comparer< NumberComparisonPolicy > lex_point_compare;

        segment_interval_compare(){}
        segment_interval_compare( const NumberComparisonPolicy& compare )
            : m_lexCompare( compare ){}

        template <typename Segment>
        bool operator()( const Segment& lhs, const Segment& rhs ) const
        {
            typedef typename geometric_traits<Segment>::point_type       point_type;

            const point_type* lhs_end;
            if( m_lexCompare( get_start( lhs ), get_end( lhs ) ) )
                lhs_end   = &get_end( lhs );
            else
                lhs_end   = &get_start( lhs );

            const point_type* rhs_start;
            if( m_lexCompare( get_start( rhs ), get_end( rhs ) ) )
                rhs_start = &get_start( rhs );
            else
                rhs_start = &get_end( rhs );

            //Now we have the segments in lexi order... we can compare the intervals.
            return m_lexCompare( *lhs_end, *rhs_start );
        }

        lex_point_compare m_lexCompare;

    };

    //! Given a set of segments take the geometrix difference of the set and the specified segments.
    //! precondition segments must all be collinear.
    template <typename Segment, typename SegmentIntervalSet, typename NumberComparisonPolicy>
    inline void collinear_segment_difference( SegmentIntervalSet& segments, const Segment& segment, const NumberComparisonPolicy& compare )
    {
        typedef lexicographical_comparer< NumberComparisonPolicy > lex_point_compare;
        typedef typename geometric_traits<Segment>::point_type    point_type;
        lex_point_compare lexCompare( compare );

        const point_type& C = get_start( segment );
        const point_type& D = get_end( segment );
        if (numeric_sequence_equals(C, D, compare))
            return;

        typename SegmentIntervalSet::iterator lb,ub;
        boost::tie( lb, ub ) = segments.equal_range( segment );

        std::vector< Segment > toInsert;
        while( lb != ub )
        {
            const Segment& overlappedSegment = *lb;

            const point_type& A = get_start( overlappedSegment );
            const point_type& B = get_end( overlappedSegment );

            //! must be collinear
            GEOMETRIX_ASSERT( is_collinear( C, D, A, compare ) && is_collinear( C, D, B, compare ) );

            bool CAD = is_between( C, D, A, true, compare );
            bool CBD = is_between( C, D, B, true, compare );

            if( CAD && CBD )
            {
                //remove this one.
                segments.erase( lb++ );
                continue;
            }

            bool AEqualC = numeric_sequence_equals( A, C, compare );
            bool AEqualD = numeric_sequence_equals( A, D, compare );
            bool BEqualC = numeric_sequence_equals( B, C, compare );
            bool BEqualD = numeric_sequence_equals( B, D, compare );

            bool ACB = is_between( A, B, C, true, compare );
            bool ADB = is_between( A, B, D, true, compare );
            if( ACB && ADB )
            {
                bool CDB = is_between( C, B, D, true, compare );
                if( CDB && !BEqualC )
                {
                    if( !AEqualC )
                        toInsert.push_back( construct< Segment >( A, C ) );
                    if( !BEqualD )
                        toInsert.push_back( construct< Segment >( D, B ) );
                    segments.erase( lb++ );
                    continue;
                }

                bool ADC = is_between(A, C, D, true, compare);
                if( ADC && !AEqualC )
                {
                    if( !AEqualD )
                        toInsert.push_back( construct< Segment >( A, D ) );
                    if( !BEqualC)
                        toInsert.push_back( construct< Segment >( C, B ) );
                    segments.erase( lb++ );
                    continue;
                }

                GEOMETRIX_ASSERT( false );
            }

            if( CAD && !(AEqualC || AEqualD) )
            {
                if( ADB && !BEqualD )
                {
                    toInsert.push_back( construct< Segment >( D, B ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
                else if( ACB && !BEqualC )
                {
                    toInsert.push_back( construct< Segment >( C, B ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
            }
            else if( CBD && !(BEqualC || BEqualD) )
            {
                if( ADB && !AEqualD)
                {
                    toInsert.push_back( construct< Segment >( A, D ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
                else if( ACB && !AEqualC )
                {
                    toInsert.push_back( construct< Segment >( A, C ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
            }

            ++lb;
        }

        segments.insert( toInsert.begin(), toInsert.end() );
    }

    //! Given a set of segments take the geometrix union of the set and the specified segments.
    //! precondition segments must all be collinear.
    template <typename Segment, typename SegmentIntervalSet, typename NumberComparisonPolicy>
    inline void collinear_segment_union( SegmentIntervalSet& segments, const Segment& segment, const NumberComparisonPolicy& compare )
    {
        typedef lexicographical_comparer< NumberComparisonPolicy > lex_point_compare;
        typedef typename geometric_traits<Segment>::point_type    point_type;
        lex_point_compare lexCompare( compare );

        typename SegmentIntervalSet::iterator lb,ub;
        boost::tie( lb, ub ) = segments.equal_range( segment );

        Segment unionSegment = construct<Segment>( get_start( segment ), get_end( segment ) );

        while( lb != ub )
        {
            const Segment& overlappedSegment = *lb;

            const point_type& A = get_start( overlappedSegment );
            const point_type& B = get_end( overlappedSegment );
            const point_type& C = get_start( unionSegment );
            const point_type& D = get_end( unionSegment );

            //! must be collinear
            GEOMETRIX_ASSERT( is_collinear( C, D, A, compare ) && is_collinear( C, D, B, compare ) );

            bool CAD = is_between( C, D, A, true, compare );
            bool CBD = is_between( C, D, B, true, compare );

            //Already contained in the union.
            if( CAD && CBD )
            {
                //remove this one.
                segments.erase( lb++ );
                continue;
            }

            bool ACB = is_between( A, B, C, true, compare );
            bool ADB = is_between( A, B, D, true, compare );

            //New segment contains existing union.. replace.
            if( ACB && ADB )
            {
                unionSegment = construct<Segment>( A, B );
                segments.erase( lb++ );
                continue;
            }

            if( CAD )
            {
                if( ADB )
                {
                    unionSegment = construct<Segment>( C, B );
                    segments.erase( lb++ );
                    continue;
                }
                if( ACB )
                {
                    unionSegment = construct<Segment>( D, B );
                    segments.erase( lb++ );
                    continue;
                }
            }
            else if( CBD )
            {
                if( ADB )
                {
                    unionSegment = construct<Segment>( A, C );
                    segments.erase( lb++ );
                    continue;
                }
                if( ACB )
                {
                    unionSegment = construct<Segment>( A, D );
                    segments.erase( lb++ );
                    continue;
                }
            }

            ++lb;
        }

        segments.insert( unionSegment );
    }

    //! sorting compare functor to sort coordinates by less than order.
    template <typename CoordinateType, typename NumberComparisonPolicy>
    struct coordinate_less_compare
    {
        coordinate_less_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
        {}

        bool operator() ( const CoordinateType& lhs, const CoordinateType& rhs ) const
        {
            return m_compare.less_than( rhs, lhs );
        }

        NumberComparisonPolicy m_compare;
    };

    template <typename Compare>
    struct pair_first_compare
    {
        pair_first_compare( const Compare& compare )
            : m_compare( compare )
        {}

        template <typename T1, typename T2>
        bool operator()(const std::pair<T1,T2>& p1, const std::pair<T1,T2>& p2) const
        {
            return m_compare(p1.first, p2.first);
        }

        Compare m_compare;

    };

    template <typename Compare>
    struct pair_second_compare
    {
        pair_second_compare( const Compare& compare )
            : m_compare( compare )
        {}

        template <typename T1, typename T2>
        bool operator()(const std::pair<T1,T2>& p1, const std::pair<T1,T2>& p2) const
        {
            return m_compare(p1.second, p2.second);
        }

        Compare m_compare;

    };

    //! \struct dimension_compare
    //! \brief A predicate to compare two numeric sequences by the value at a specified dimension.
    template <std::size_t D, typename NumberComparisonPolicy>
    struct dimension_compare
    {
        dimension_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
            , m_lexicographicalCompare( compare )
        {}

        template <typename NumericSequence>
        bool operator()( const NumericSequence& lhs, const NumericSequence& rhs ) const
        {
            //! Sequences are compared by dimension specified. In the case where coordinates at D are equal, the sequences are
            //! given a total order lexicographically.
            if( m_compare.less_than( get<D>( lhs ), get<D>( rhs ) ) )
                return true;
            else if( m_compare.equals( get<D>( lhs ), get<D>( rhs ) ) )
                return m_lexicographicalCompare( lhs, rhs );
            else
                return false;
        }

        NumberComparisonPolicy                          m_compare;
        lexicographical_comparer<NumberComparisonPolicy> m_lexicographicalCompare;
    };

    template <typename T>
    inline T min_copy(const T& a, const T& b)
    {
        return a < b ? a : b;
    }

    template <typename T>
    inline T max_copy(const T& a, const T& b)
    {
        return a > b ? a : b;
    }

    // clamp n to lie within the range [min, max]
    template <typename T>
    inline T clamp(T n, T min, T max)
    {
        if (n < min) return min;
        if (n > max) return max;
        return n;
    }

    template <typename T>
    inline int sign(const T& value)
    {
        return value >= constants::zero<T>() ? 1 : -1;
    }

}//namespace geometrix;

#endif //GEOMETRIX_UTILITIES_HPP


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

    @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_PIO2_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric

    This function object computes the remainder modulo \f$\pi/2\f$,
     and the angle quadrant between 0 and 3.

     This is a rather slow version,  but accurate in the full floating range.

    @par Header <boost/simd/function/rem_pio2.hpp>

    @par Notes

     - This algorithm is accurate over  the full floating range,  but also is over
      costly and implies the knowledge  of a few hundred \f$\pi\f$ decimals

     - Some simpler algorithms  @ref rem_pio2_medium, @ref rem_pio2_cephes or @ref rem_pio2_straight
      can often be used, but the precision is only insured on smaller intervals.

      - Using  `std::tie(n, r) = rem_pio2(x);` is similar to ` n = div(inearbyint, x, Pio_2<T>())`
      and `r =  remainder(x, Pio_2<T>())`

    @see rem_pio2_medium, rem_pio2_straight, rem_2pi,  rem_pio2_cephes,

    @par Example:

      @snippet rem_pio2.cpp rem_pio2

    @par Possible output:

      @snippet rem_pio2.txt rem_pio2

  **/
  std::pair<IEEEValue, IEEEValue> rem_pio2(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/rem_pio2.hpp>
#include <boost/simd/function/simd/rem_pio2.hpp>

#endif


#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/lps/parse.h"
#include "mcrl2/lps/linearise.h"
#include "mcrl2/lps/detail/test_input.h"
#include "mcrl2/pbes/bisimulation.h"
#include "mcrl2/pbes/detail/pbes2bool.h"

using namespace mcrl2;
using namespace mcrl2::lps;
using namespace mcrl2::pbes_system;
using namespace mcrl2::log;

void test_bisimulation(const std::string& s1, const std::string& s2,
                       bool strongly_bisimilar,
                       bool branching_bisimilar,
                       bool branching_similar,
                       bool weakly_bisimilar,
                       bool linearize = false)
{
  specification spec1;
  specification spec2;
  if (linearize)
  {
    spec1=remove_stochastic_operators(linearise(s1));
    spec2=remove_stochastic_operators(linearise(s2));
  }
  else
  {
    spec1 = parse_linear_process_specification(s1);
    spec2 = parse_linear_process_specification(s2);
  }

  std::clog << "Testing strong bisimulation" << std::endl;
  pbes sb  = strong_bisimulation(spec1, spec2);
  BOOST_CHECK(sb.is_well_typed());
  bool sb_solution = pbes_system::detail::pbes2bool(sb);
  BOOST_CHECK(sb_solution == strongly_bisimilar);

  std::clog << "Testing branching bisimulation" << std::endl;
  pbes bb  = branching_bisimulation(spec1, spec2);
  bool bb_solution = pbes_system::detail::pbes2bool(bb);
  BOOST_CHECK(bb.is_well_typed());
  BOOST_CHECK(bb_solution == branching_bisimilar);

  std::clog << "Testing branching simulation" << std::endl;
  pbes bs = branching_simulation_equivalence(spec1, spec2);
  bool bs_solution = pbes_system::detail::pbes2bool(bs);
  BOOST_CHECK(bs.is_well_typed());
  BOOST_CHECK(bs_solution == branching_similar);

  std::clog << "Testing weak bisimulation" << std::endl;
  pbes wb  = weak_bisimulation(spec1, spec2);
  bool wb_solution = pbes_system::detail::pbes2bool(wb);
  BOOST_CHECK(wb.is_well_typed());
  BOOST_CHECK(wb_solution == weakly_bisimilar);
}

BOOST_AUTO_TEST_CASE(ABP)
{
  test_bisimulation(lps::detail::LINEAR_ABP_SPECIFICATION(), lps::detail::LINEAR_ABP_SPECIFICATION(), true, true, true, true);
}

BOOST_AUTO_TEST_CASE(SMALLSPEC)
{
  const std::string SMALLSPEC =
    "act a,b;                 \n"
    "proc X(s: Pos) =         \n"
    "  (s == 1) -> a . X(2)   \n"
    "+ (s == 2) -> tau . X(3) \n"
    "+ (s == 3) -> tau . X(4) \n"
    "+ (s == 4) -> b . X(1);  \n"
    "init X(1);               \n"
    ;
  test_bisimulation(SMALLSPEC, SMALLSPEC, true, true, true, true);
}

BOOST_AUTO_TEST_CASE(small_different_specs)
{
  const std::string s1 =
    "act a,b;                 \n"
    "proc X(s: Pos) =         \n"
    "  (s == 1) ->  a . X(2)  \n"
    "+ (s == 2) -> b . X(1);  \n"
    "init X(1);               \n"
    ;
  const std::string s2 =
    "act a,b,c;               \n"
    "proc X(s: Pos) =     \n"
    "  (s == 1) ->  a . X(2)  \n"
    "+ (s == 1) ->  c . X(1)  \n"
    "+ (s == 2) -> b . X(1);  \n"
    "init X(1);               \n"
    ;
    ;
  test_bisimulation(s1, s2, false, false, false, false);
  test_bisimulation(s2, s1, false, false, false, false);
}

BOOST_AUTO_TEST_CASE(buffers_silent_lose)
{
  const std::string buffer =
    "sort D = struct d1 | d2;\n"
    "map  n: Pos;\n"
    "eqn  n  =  2;\n"
    "act  r,s: D;\n"
    "proc P(b_Buffer: List(D)) =\n"
    "       !(b_Buffer == []) ->\n"
    "         s(rhead(b_Buffer)) .\n"
    "         P(b_Buffer = rtail(b_Buffer))\n"
    "     + sum d_Buffer: D.\n"
    "         (#b_Buffer < 2) ->\n"
    "         r(d_Buffer) .\n"
    "         P(b_Buffer = d_Buffer |> b_Buffer)\n"
    "     + delta;\n"
    "init P([]);\n";

  const std::string lossy_buffer =
    "sort D = struct d1 | d2;\n"
    "map  n: Pos;\n"
    "eqn  n  =  2;\n"
    "act  r,s: D;\n"
    "proc P(s3_Buffer: Pos, d_Buffer: D, b_Buffer: List(D)) =\n"
    "       sum e_Buffer: Bool.\n"
    "         (s3_Buffer == 2) ->\n"
    "         tau .\n"
    "         P(s3_Buffer = 1, d_Buffer = d1, b_Buffer = if(e_Buffer, d_Buffer |> b_Buffer, b_Buffer))\n"
    "     + (s3_Buffer == 1 && !(b_Buffer == [])) ->\n"
    "         s(rhead(b_Buffer)) .\n"
    "         P(s3_Buffer = 1, d_Buffer = d1, b_Buffer = rtail(b_Buffer))\n"
    "     + sum d0_Buffer: D.\n"
    "         (s3_Buffer == 1 && #b_Buffer < 2) ->\n"
    "         r(d0_Buffer) .\n"
    "         P(s3_Buffer = 2, d_Buffer = d0_Buffer)\n"
    "     + delta;\n"
    "init P(1, d1, []);\n";

  test_bisimulation(buffer, lossy_buffer, false, false, false, false);
  test_bisimulation(lossy_buffer, buffer, false, false, false, false);
}

BOOST_AUTO_TEST_CASE(buffers_explicit_lose)
{
  const std::string buffer =
    "sort D = struct d1 | d2;\n"
    "map  n: Pos;\n"
    "eqn  n  =  2;\n"
    "act  r,s: D;\n"
    "proc P(b_Buffer: List(D)) =\n"
    "       !(b_Buffer == []) ->\n"
    "         s(rhead(b_Buffer)) .\n"
    "         P(b_Buffer = rtail(b_Buffer))\n"
    "     + sum d_Buffer: D.\n"
    "         (#b_Buffer < 2) ->\n"
    "         r(d_Buffer) .\n"
    "         P(b_Buffer = d_Buffer |> b_Buffer)\n"
    "     + delta;\n"
    "init P([]);\n";

  const std::string lossy_buffer =
      "sort D = struct d1 | d2;\n"
      "map  n: Pos;\n"
      "eqn  n  =  2;\n"
      "act  r,s: D;\n"
      "     lose;\n"
      "proc P(s3_Buffer: Pos, d_Buffer: D, b_Buffer: List(D)) =\n"
      "       sum d0_Buffer: D.\n"
      "         (s3_Buffer == 1 && #b_Buffer < 2) ->\n"
      "         r(d0_Buffer) .\n"
      "         P(s3_Buffer = 2, d_Buffer = d0_Buffer)\n"
      "     + (s3_Buffer == 1 && !(b_Buffer == [])) ->\n"
      "         s(rhead(b_Buffer)) .\n"
      "         P(s3_Buffer = 1, d_Buffer = d1, b_Buffer = rtail(b_Buffer))\n"
      "     + (s3_Buffer == 2) ->\n"
      "         tau .\n"
      "         P(s3_Buffer = 3, d_Buffer = d1)\n"
      "     + (s3_Buffer == 2) ->\n"
      "         tau .\n"
      "         P(s3_Buffer = 1, d_Buffer = d1, b_Buffer = d_Buffer |> b_Buffer)\n"
      "     + (s3_Buffer == 3) ->\n"
      "         lose .\n"
      "         P(s3_Buffer = 1, d_Buffer = d1)\n"
      "     + delta;\n"
      "init P(1, d1, []);\n";

  test_bisimulation(buffer, lossy_buffer, false, false, false, false);
  test_bisimulation(lossy_buffer, buffer, false, false, false, false);
}

BOOST_AUTO_TEST_CASE(test_fresh_variables)
{
  data::variable_list w = { data::variable("d", data::basic_sort("D")), data::variable("e", data::basic_sort("E")), data::variable("f", data::basic_sort("F")) };
  std::set<std::string> context;
  context.insert("e");
  context.insert("f_00");
  data::variable_list w1 = pbes_system::detail::fresh_variables(w, context);
  std::cout << "w1 = " << data::pp(w1) << std::endl;

  context.clear();
  context.insert("e3_Sx0");
  context.insert("e_Sx0");
  context.insert("n0_Sx0");
  context.insert("n_S");
  context.insert("s3_S");
  std::cout << "\n" << core::detail::print_set(context, "context") << std::endl;
  data::variable_list yi = { data::variable("e3_S", data::basic_sort("A")) };
  std::cout << "\nyi " << data::pp(yi) << std::endl;
  data::variable_list y = pbes_system::detail::fresh_variables(yi, context);
  std::cout << "\ny " << data::pp(y) << std::endl;
  BOOST_CHECK(y.size() == 1);
  BOOST_CHECK(std::string(y.front().name()) != " e3_Sx0");
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return nullptr;
}


#include <vector>
#include <math.h>
#include <iostream>
#include <string>
#include <sstream>

#include <ros/ros.h>
#include <ros/console.h>

#include <common/Factor.h>
#include <common/Keyframe.h>
#include <common/ClosestKeyframe.h>
#include <common/LastKeyframe.h>
#include <common/Registration.h>
#include <common/Pose2DWithCovariance.h>
#include <common/OdometryBuffer.h>

#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

common::Pose2DWithCovariance compose(common::Pose2DWithCovariance input_1, common::Pose2DWithCovariance input_2) {
  common::Pose2DWithCovariance output;
  double cos_th = cos( input_1.pose.theta );
  double sin_th = sin( input_1.pose.theta );
  double dx = input_2.pose.x;
  double dy = input_2.pose.y;
  double dth = input_2.pose.theta;
  output.pose.x = ( cos_th * dx ) + ( -sin_th * dy );
  output.pose.y = ( sin_th * dx ) + ( cos_th *dy );
  output.pose.theta = input_1.pose.theta + dth;
  output.pose.theta = std::fmod(output.pose.theta + M_PI, 2 * M_PI) - M_PI;

  return output;
}

Eigen::MatrixXd covariance_to_eigen(common::Factor input) {
  Eigen::MatrixXd Q(3, 3);
  Q.row(0) << input.delta.covariance[0],
    input.delta.covariance[1],
    input.delta.covariance[2];
  Q.row(1) << input.delta.covariance[3],
    input.delta.covariance[4],
    input.delta.covariance[5];
  Q.row(2) << input.delta.covariance[6],
    input.delta.covariance[7],
    input.delta.covariance[8];

  return Q;
}

common::Pose2DWithCovariance eigen_to_covariance(common::Pose2DWithCovariance pose, Eigen::MatrixXd Q) {
  for(int i = 0; i < Q.rows(); i++) {
    for(int j = 0; j < Q.cols(); j++) {
      pose.covariance[( i * Q.rows() ) + j] = Q(i, j);
    }
  }

  return pose;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, 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 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

template<typename MatrixType> void schur(int size = MatrixType::ColsAtCompileTime)
{
  typedef typename ComplexSchur<MatrixType>::ComplexScalar ComplexScalar;
  typedef typename ComplexSchur<MatrixType>::ComplexMatrixType ComplexMatrixType;

  // Test basic functionality: T is triangular and A = U T U*
  for(int counter = 0; counter < g_repeat; ++counter) {
    MatrixType A = MatrixType::Random(size, size);
    ComplexSchur<MatrixType> schurOfA(A);
    VERIFY_IS_EQUAL(schurOfA.info(), Success);
    ComplexMatrixType U = schurOfA.matrixU();
    ComplexMatrixType T = schurOfA.matrixT();
    for(int row = 1; row < size; ++row) {
      for(int col = 0; col < row; ++col) {
 VERIFY(T(row,col) == (typename MatrixType::Scalar)0);
      }
    }
    VERIFY_IS_APPROX(A.template cast<ComplexScalar>(), U * T * U.adjoint());
  }

  // Test asserts when not initialized
  ComplexSchur<MatrixType> csUninitialized;
  VERIFY_RAISES_ASSERT(csUninitialized.matrixT());
  VERIFY_RAISES_ASSERT(csUninitialized.matrixU());
  VERIFY_RAISES_ASSERT(csUninitialized.info());

  // Test whether compute() and constructor returns same result
  MatrixType A = MatrixType::Random(size, size);
  ComplexSchur<MatrixType> cs1;
  cs1.compute(A);
  ComplexSchur<MatrixType> cs2(A);
  VERIFY_IS_EQUAL(cs1.info(), Success);
  VERIFY_IS_EQUAL(cs2.info(), Success);
  VERIFY_IS_EQUAL(cs1.matrixT(), cs2.matrixT());
  VERIFY_IS_EQUAL(cs1.matrixU(), cs2.matrixU());

  // Test computation of only T, not U
  ComplexSchur<MatrixType> csOnlyT(A, false);
  VERIFY_IS_EQUAL(csOnlyT.info(), Success);
  VERIFY_IS_EQUAL(cs1.matrixT(), csOnlyT.matrixT());
  VERIFY_RAISES_ASSERT(csOnlyT.matrixU());

  if (size > 1)
  {
    // Test matrix with NaN
    A(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
    ComplexSchur<MatrixType> csNaN(A);
    VERIFY_IS_EQUAL(csNaN.info(), NoConvergence);
  }
}

void test_schur_complex()
{
  CALL_SUBTEST_1(( schur<Matrix4cd>() ));
  CALL_SUBTEST_2(( schur<MatrixXcf>(internal::random<int>(1,50)) ));
  CALL_SUBTEST_3(( schur<Matrix<std::complex<float>, 1, 1> >() ));
  CALL_SUBTEST_4(( schur<Matrix<float, 3, 3, Eigen::RowMajor> >() ));

  // Test problem size constructors
  CALL_SUBTEST_5(ComplexSchur<MatrixXf>(10));
}


#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <dlib/matrix.h>

using namespace std;
using namespace cv;
using namespace dlib;

std::vector<Point2f> get_points( std::string filename, int &num_shape ) {
    std::vector<Point2f> points_set;
    FileStorage fs( filename, FileStorage::READ );
    if( !fs.isOpened() ) {
        cout << "File not found at get_points: " << filename << endl;
        num_shape--;
        return points_set;
    }

    FileNode features = fs["features"];
    for( auto point:features ){
        Point2f p2f( point["x"], point["y"] );
        points_set.push_back( p2f );
    }
    fs.release();

    return points_set;
}
std::vector<std::vector<Point2f>> get_delaunay( std::string filename, int &num_shape ) {
    std::vector<Point2f> init(68, Point2f(0, 0));
    std::vector<std::vector<Point2f>> delaunay_set;
    std::vector<Point2f> points = get_points( filename, num_shape );
    FileStorage fs( filename, FileStorage::READ );
    if( !fs.isOpened() ) {
        cout << "File not found at delaunay: " << filename << endl;
        num_shape--;
        return delaunay_set;
    }
    FileNode delaunay = fs["delaunay_index"];
    for( auto t:delaunay ){
        Vec3i v_index;
        std::vector<Point2f> v_points;
        t["index"] >> v_index;
        v_points.push_back( points[ v_index[0] ] );
        v_points.push_back( points[ v_index[1] ] );
        v_points.push_back( points[ v_index[2] ] );
        delaunay_set.push_back( v_points );
    }

    return delaunay_set;
}

void point_drawing( Mat& plane, std::vector<Point2f> points, double scale, Scalar& color ) {
    for( auto p:points ){
        circle( plane, p, 1, color, -1);
    }
}
void delaunay_drawing( Mat& plane, std::vector<std::vector<Point2f>> vertices, Scalar& color ) {
    float line_weight = 1.5;
    for( auto v:vertices ){
        line(plane, v[0], v[1], color, line_weight);
        line(plane, v[1], v[2], color, line_weight);
        line(plane, v[2], v[0], color, line_weight);
    }
}

int main( int argc, char *argv[] ) {
    int num_shape = atoi(argv[1]);
    std::string mean_filename = "../face2yaml/dataset/meanShape.yaml";
    Mat plane = Mat::zeros( Size(512,512), CV_32FC3 );
    std::vector<std::vector<Point2f>> shape_store;
    std::vector<std::vector<Point2f>> delaunay = get_delaunay( mean_filename, num_shape );
    std::vector<Point2f>  mean_points = get_points( mean_filename, num_shape );
    Scalar red_color = Scalar( 0, 0, 255 );
    Scalar blue_color = Scalar( 255, 0, 0 );
    Scalar green_color = Scalar( 0, 255, 0 );

    for(int i = 1; i <= num_shape; ++i) {
        std::vector<Point2f> left_shape_points = get_points("../face2yaml/dataset/"+ to_string(i) +"L.yaml", num_shape);

        // Centroid(x, y)
        cv::Vec2f cg;
        double scale = 0;
        int left_num_shape = left_shape_points.size();
        for(int i = 0; i < left_num_shape; ++i){
            cg[0] += left_shape_points[i].x;
            cg[1] += left_shape_points[i].y;
        }
        cg[0] = cg[0]/left_num_shape;
        cg[1] = cg[1]/left_num_shape;

        std::vector<Point2f> left_shape_mean;

        for( int i = 0; i < left_num_shape; ++i ){
            int offset = 256;
            float tx_point = left_shape_points[i].x - cg[0];
            float ty_point = left_shape_points[i].y - cg[1];
            scale += tx_point*tx_point + ty_point*ty_point;
            Point2f left_s( tx_point + offset, ty_point + offset);
            left_shape_mean.push_back(left_s);
        }

        scale = sqrt(scale/left_num_shape);
        cout << "Scale : " << scale << endl;

        point_drawing( plane, left_shape_mean, scale, green_color );

        if( i%5 == 0 ){
            float percentage = ( (float)i/num_shape )*100;
            if( percentage == 100 )
                break;
            cout << "processing ";
            cout <<  setprecision(2) << "[ " << percentage << " \% ]" << endl;
        }
    }
    // delaunay_drawing( plane, delaunay, red_color );
    cout << "Num_shape: " << num_shape << endl;
    imshow ("Plane", plane );
    cout << "[ 100\% Done ], " << num_shape << " Faces" << endl;
    waitKey(0);

    return 0;
}


// Copyright (c) 2017 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include <stdio.h>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include "utils/maths.hpp"

namespace octopus { namespace test {

static constexpr double tolerance {1e-10};

BOOST_AUTO_TEST_SUITE(utils)
BOOST_AUTO_TEST_SUITE(maths)

BOOST_AUTO_TEST_CASE(log_sum_exp_handles_edge_cases)
{
    using octopus::maths::log_sum_exp;
    static constexpr double zero {0.0};
    static constexpr double lnHalf {-0.6931471805599453};
    BOOST_CHECK_CLOSE(log_sum_exp(lnHalf, lnHalf), zero, tolerance);
    BOOST_CHECK_CLOSE(log_sum_exp(zero, zero), -lnHalf, tolerance);
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()

} // namespace test
} // namespace octopus


/*
Подсчёт суммы этого ряда:
https://i.imgur.com/JjdxXIt.png
*/

#include <iostream>
#include <iomanip>

#include <boost/multiprecision/cpp_int.hpp>

#include "../include/calcSeries.h"

using namespace boost::multiprecision;

void calcSeries(const unsigned threadNum)
{
    const size_t N = 7500000;
    const cpp_int num_87 = 87;
    const cpp_int num_13 = 13;

    const unsigned REP_NUM = 2;

    for(unsigned i = 0; i < REP_NUM; ++i)
    {

        cpp_int sum = 0;

        #pragma omp parallel num_threads(threadNum)
        {
            #pragma omp for nowait
            for(size_t i = 0; i < N; ++i)
            {
                cpp_int buff;
                buff = i+1;
                buff = pow(buff, 5) + num_87*buff + num_13*buff*buff;
                #pragma omp critical
                {
                    sum += buff;
                }
            }
        }

        //std::cout << "sum = " << sum << std::endl;

    }
}


#include "thermalDistribution.h"

#include "maxwellJuttner.h"
#include "modelParameters.h"

//#include <fmath/RungeKutta.h>
#include <fparameters/Dimension.h>
#include <fparameters/SpaceIterator.h>
#include <fparameters/parameters.h>

#include <fmath/physics.h>

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

void thermalDistribution(Particle& p, State& st)
{
	double Erest = p.mass*cLight2;

	double gMin = p.emin()/Erest;
	double gMax = p.emax()/Erest;

	p.ps.iterate([&](const SpaceIterator& i) {
	//p.distribution.fill ([&](const SpaceIterator& i){

		//const double E = i.val(DIM_E);
		double r = i.val(DIM_R);

		double norm_temp, A;

		if(p.id == "electron"){
			norm_temp = boltzmann*st.tempElectrons.get(i)/(Erest);
			A = st.denf_e.get(i);
		}
		else if(p.id == "proton"){
			norm_temp = boltzmann*st.tempIons.get(i)/(Erest);
			A = st.denf_i.get(i);
		}

		//double K2 =  boost::math::cyl_bessel_k(2, 1.0/norm_temp); //bessk(2, 1.0/norm_temp);

		p.ps.iterate([&](const SpaceIterator& j) {

			const double E = j.val(DIM_E);
			double g = E/Erest;

			double result =  maxwellRel(g, norm_temp, A/Erest);

			p.distribution.set(j,result); //en unidades de cm^-3 erg^-1

		}, { -1, i.coord[DIM_R]} );

	}, { 0, -1} );

}