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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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_ALGORITHMS_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISTANCE_HPP

#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>
#include <boost/geometry/core/tag_cast.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>

#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/views/closeable_view.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

// To avoid spurious namespaces here:
using strategy::distance::services::return_type;

template <typename P1, typename P2, typename Strategy>
struct point_to_point
{
    static inline typename return_type<Strategy>::type apply(P1 const& p1,
                P2 const& p2, Strategy const& strategy)
    {
        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(p1, p2);
    }
};

template<typename Point, typename Segment, typename Strategy>
struct point_to_segment
{
    static inline typename return_type<Strategy>::type apply(Point const& point,
                Segment const& segment, Strategy const& )
    {
        typename strategy::distance::services::default_strategy
            <
                segment_tag,
                Point,
                typename point_type<Segment>::type,
                typename cs_tag<Point>::type,
                typename cs_tag<typename point_type<Segment>::type>::type,
                Strategy
            >::type segment_strategy;

        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);
        return segment_strategy.apply(point, p[0], p[1]);
    }
};

template
<
    typename Point,
    typename Range,
    closure_selector Closure,
    typename PPStrategy,
    typename PSStrategy
>
struct point_to_range
{
    typedef typename return_type<PSStrategy>::type return_type;

    static inline return_type apply(Point const& point, Range const& range,
            PPStrategy const& pp_strategy, PSStrategy const& ps_strategy)
    {
        return_type const zero = return_type(0);

        if (boost::size(range) == 0)
        {
            return zero;
        }

        typedef typename closeable_view<Range const, Closure>::type view_type;

        view_type view(range);

        // line of one point: return point distance
        typedef typename boost::range_iterator<view_type const>::type iterator_type;
        iterator_type it = boost::begin(view);
        iterator_type prev = it++;
        if (it == boost::end(view))
        {
            return pp_strategy.apply(point, *boost::begin(view));
        }

        // Create comparable (more efficient) strategy
        typedef typename strategy::distance::services::comparable_type<PSStrategy>::type eps_strategy_type;
        eps_strategy_type eps_strategy = strategy::distance::services::get_comparable<PSStrategy>::apply(ps_strategy);

        // start with first segment distance
        return_type d = eps_strategy.apply(point, *prev, *it);
        return_type rd = ps_strategy.apply(point, *prev, *it);

        // check if other segments are closer
        for (++prev, ++it; it != boost::end(view); ++prev, ++it)
        {
            return_type const ds = eps_strategy.apply(point, *prev, *it);
            if (geometry::math::equals(ds, zero))
            {
                return ds;
            }
            else if (ds < d)
            {
                d = ds;
                rd = ps_strategy.apply(point, *prev, *it);
            }
        }

        return rd;
    }
};

template
<
    typename Point,
    typename Ring,
    closure_selector Closure,
    typename PPStrategy,
    typename PSStrategy
>
struct point_to_ring
{
    typedef std::pair
        <
            typename return_type<PPStrategy>::type, bool
        > distance_containment;

    static inline distance_containment apply(Point const& point,
                Ring const& ring,
                PPStrategy const& pp_strategy, PSStrategy const& ps_strategy)
    {
        return distance_containment
            (
                point_to_range
                    <
                        Point,
                        Ring,
                        Closure,
                        PPStrategy,
                        PSStrategy
                    >::apply(point, ring, pp_strategy, ps_strategy),
                geometry::within(point, ring)
            );
    }
};

template
<
    typename Point,
    typename Polygon,
    closure_selector Closure,
    typename PPStrategy,
    typename PSStrategy
>
struct point_to_polygon
{
    typedef typename return_type<PPStrategy>::type return_type;
    typedef std::pair<return_type, bool> distance_containment;

    static inline distance_containment apply(Point const& point,
                Polygon const& polygon,
                PPStrategy const& pp_strategy, PSStrategy const& ps_strategy)
    {
        // Check distance to all rings
        typedef point_to_ring
            <
                Point,
                typename ring_type<Polygon>::type,
                Closure,
                PPStrategy,
                PSStrategy
            > per_ring;

        distance_containment dc = per_ring::apply(point,
                        exterior_ring(polygon), pp_strategy, ps_strategy);

        typename interior_return_type<Polygon const>::type rings
                    = interior_rings(polygon);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            distance_containment dcr = per_ring::apply(point,
                            *it, pp_strategy, ps_strategy);
            if (dcr.first < dc.first)
            {
                dc.first = dcr.first;
            }
            // If it was inside, and also inside inner ring,
            // turn off the inside-flag, it is outside the polygon
            if (dc.second && dcr.second)
            {
                dc.second = false;
            }
        }
        return dc;
    }
};

// Helper metafunction for default strategy retrieval
template <typename Geometry1, typename Geometry2>
struct default_strategy
    : strategy::distance::services::default_strategy
          <
              point_tag,
              typename point_type<Geometry1>::type,
              typename point_type<Geometry2>::type
          >
{};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

using strategy::distance::services::return_type;

template
<
    typename Geometry1, typename Geometry2,
    typename Strategy = typename detail::distance::default_strategy<Geometry1, Geometry2>::type,
    typename Tag1 = typename tag_cast<typename tag<Geometry1>::type, multi_tag>::type,
    typename Tag2 = typename tag_cast<typename tag<Geometry2>::type, multi_tag>::type,
    typename StrategyTag = typename strategy::distance::services::tag<Strategy>::type,
    bool Reverse = reverse_dispatch<Geometry1, Geometry2>::type::value
>
struct distance: not_implemented<Tag1, Tag2>
{};

// If reversal is needed, perform it
template
<
    typename Geometry1, typename Geometry2, typename Strategy,
    typename Tag1, typename Tag2, typename StrategyTag
>
struct distance
<
    Geometry1, Geometry2, Strategy,
    Tag1, Tag2, StrategyTag,
    true
>
    : distance<Geometry2, Geometry1, Strategy, Tag2, Tag1, StrategyTag, false>
{
    static inline typename return_type<Strategy>::type apply(
        Geometry1 const& g1,
        Geometry2 const& g2,
        Strategy const& strategy)
    {
        return distance
            <
                Geometry2, Geometry1, Strategy,
                Tag2, Tag1, StrategyTag,
                false
            >::apply(g2, g1, strategy);
    }
};

// If reversal is needed and we got the strategy by default, invert it before
// proceeding to the reversal.
template
<
    typename Geometry1, typename Geometry2,
    typename Tag1, typename Tag2, typename StrategyTag
>
struct distance
<
    Geometry1, Geometry2,
    typename detail::distance::default_strategy<Geometry1, Geometry2>::type,
    Tag1, Tag2, StrategyTag,
    true
>
    : distance
          <
              Geometry2, Geometry1,
              typename detail::distance::default_strategy<Geometry2, Geometry1>::type,
              Tag2, Tag1, StrategyTag,
              false
          >
{
    typedef typename detail::distance::default_strategy<Geometry2, Geometry1>::type reversed_strategy;

    static inline typename strategy::distance::services::return_type<reversed_strategy>::type apply(
        Geometry1 const& g1,
        Geometry2 const& g2,
        typename detail::distance::default_strategy<Geometry1, Geometry2>::type const&)
    {
        return distance
            <
                Geometry2, Geometry1, reversed_strategy,
                Tag2, Tag1, StrategyTag,
                false
            >::apply(g2, g1, reversed_strategy());
    }
};

// Point-point
template <typename P1, typename P2, typename Strategy>
struct distance
    <
        P1, P2, Strategy,
        point_tag, point_tag, strategy_tag_distance_point_point,
        false
    >
    : detail::distance::point_to_point<P1, P2, Strategy>
{};

// Point-line version 1, where point-point strategy is specified
template <typename Point, typename Linestring, typename Strategy>
struct distance
<
    Point, Linestring, Strategy,
    point_tag, linestring_tag, strategy_tag_distance_point_point,
    false
>
{

    static inline typename return_type<Strategy>::type apply(Point const& point,
            Linestring const& linestring,
            Strategy const& strategy)
    {
        typedef typename strategy::distance::services::default_strategy
                    <
                        segment_tag,
                        Point,
                        typename point_type<Linestring>::type,
                        typename cs_tag<Point>::type,
                        typename cs_tag<typename point_type<Linestring>::type>::type,
                        Strategy
                    >::type ps_strategy_type;

        return detail::distance::point_to_range
            <
                Point, Linestring, closed, Strategy, ps_strategy_type
            >::apply(point, linestring, strategy, ps_strategy_type());
    }
};

// Point-line version 2, where point-segment strategy is specified
template <typename Point, typename Linestring, typename Strategy>
struct distance
<
    Point, Linestring, Strategy,
    point_tag, linestring_tag, strategy_tag_distance_point_segment,
    false
>
{
    static inline typename return_type<Strategy>::type apply(Point const& point,
            Linestring const& linestring,
            Strategy const& strategy)
    {
        typedef typename Strategy::point_strategy_type pp_strategy_type;
        return detail::distance::point_to_range
            <
                Point, Linestring, closed, pp_strategy_type, Strategy
            >::apply(point, linestring, pp_strategy_type(), strategy);
    }
};

// Point-ring , where point-segment strategy is specified
template <typename Point, typename Ring, typename Strategy>
struct distance
<
    Point, Ring, Strategy,
    point_tag, ring_tag, strategy_tag_distance_point_point,
    false
>
{
    typedef typename return_type<Strategy>::type return_type;

    static inline return_type apply(Point const& point,
            Ring const& ring,
            Strategy const& strategy)
    {
        typedef typename strategy::distance::services::default_strategy
            <
                segment_tag,
                Point,
                typename point_type<Ring>::type
            >::type ps_strategy_type;

        std::pair<return_type, bool>
            dc = detail::distance::point_to_ring
            <
                Point, Ring,
                geometry::closure<Ring>::value,
                Strategy, ps_strategy_type
            >::apply(point, ring, strategy, ps_strategy_type());

        return dc.second ? return_type(0) : dc.first;
    }
};

// Point-polygon , where point-segment strategy is specified
template <typename Point, typename Polygon, typename Strategy>
struct distance
<
    Point, Polygon, Strategy,
    point_tag, polygon_tag, strategy_tag_distance_point_point,
    false
>
{
    typedef typename return_type<Strategy>::type return_type;

    static inline return_type apply(Point const& point,
            Polygon const& polygon,
            Strategy const& strategy)
    {
        typedef typename strategy::distance::services::default_strategy
            <
                segment_tag,
                Point,
                typename point_type<Polygon>::type
            >::type ps_strategy_type;

        std::pair<return_type, bool>
            dc = detail::distance::point_to_polygon
            <
                Point, Polygon,
                geometry::closure<Polygon>::value,
                Strategy, ps_strategy_type
            >::apply(point, polygon, strategy, ps_strategy_type());

        return dc.second ? return_type(0) : dc.first;
    }
};

// Point-segment version 1, with point-point strategy
template <typename Point, typename Segment, typename Strategy>
struct distance
<
    Point, Segment, Strategy,
    point_tag, segment_tag, strategy_tag_distance_point_point,
    false
> : detail::distance::point_to_segment<Point, Segment, Strategy>
{};

// Point-segment version 2, with point-segment strategy
template <typename Point, typename Segment, typename Strategy>
struct distance
<
    Point, Segment, Strategy,
    point_tag, segment_tag, strategy_tag_distance_point_segment,
    false
>
{
    static inline typename return_type<Strategy>::type apply(Point const& point,
                Segment const& segment, Strategy const& strategy)
    {

        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);
        return strategy.apply(point, p[0], p[1]);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc2{distance} \brief_strategy
\ingroup distance
\details
\details \details_calc{area}. \brief_strategy. \details_strategy_reasons

\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy \tparam_strategy{Distance}
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param strategy \param_strategy{distance}
\return \return_calc{distance}
\note The strategy can be a point-point strategy. In case of distance point-line/point-polygon
    it may also be a point-segment strategy.

\qbk{distinguish,with strategy}

\qbk{
[heading Available Strategies]
\* [link geometry.reference.strategies.strategy_distance_pythagoras Pythagoras (cartesian)]
\* [link geometry.reference.strategies.strategy_distance_haversine Haversine (spherical)]
\* [link geometry.reference.strategies.strategy_distance_cross_track Cross track (spherical\, point-to-segment)]
\* [link geometry.reference.strategies.strategy_distance_projected_point Projected point (cartesian\, point-to-segment)]
\* more (currently extensions): Vincenty\, Andoyer (geographic)
}
 */

/*
Note, in case of a Compilation Error:
if you get:
 - "Failed to specialize function template ..."
 - "error: no matching function for call to ..."
for distance, it is probably so that there is no specialization
for return_type<...> for your strategy.
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline typename strategy::distance::services::return_type<Strategy>::type distance(
                Geometry1 const& geometry1, Geometry2 const& geometry2,
                Strategy const& strategy)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    detail::throw_on_empty_input(geometry1);
    detail::throw_on_empty_input(geometry2);

    return dispatch::distance
               <
                   Geometry1,
                   Geometry2,
                   Strategy
               >::apply(geometry1, geometry2, strategy);
}

/*!
\brief \brief_calc2{distance}
\ingroup distance
\details The default strategy is used, corresponding to the coordinate system of the geometries
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\return \return_calc{distance}

\qbk{[include reference/algorithms/distance.qbk]}
 */
template <typename Geometry1, typename Geometry2>
inline typename default_distance_result<Geometry1, Geometry2>::type distance(
                Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    return distance(geometry1, geometry2,
                    typename detail::distance::default_strategy<Geometry1, Geometry2>::type());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISTANCE_HPP


// Boost.Geometry

// Copyright (c) 2016-2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_STRATEGIES_GEOGRAPHIC_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/sjoberg_intersection.hpp>
#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/formulas/unit_spheroid.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/policies/robustness/segment_ratio.hpp>

#include <boost/geometry/strategies/geographic/area.hpp>
#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/envelope_segment.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/strategies/geographic/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/geographic/side.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/strategies/intersection_result.hpp>
#include <boost/geometry/strategies/side_info.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

// CONSIDER: Improvement of the robustness/accuracy/repeatability by
// moving all segments to 0 longitude
// picking latitudes closer to 0
// etc.

template
<
    typename FormulaPolicy = strategy::andoyer,
    unsigned int Order = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
struct geographic_segments
{
    typedef side::geographic
        <
            FormulaPolicy, Spheroid, CalculationType
        > side_strategy_type;

    inline side_strategy_type get_side_strategy() const
    {
        return side_strategy_type(m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    struct point_in_geometry_strategy
    {
        typedef strategy::within::geographic_winding
            <
                typename point_type<Geometry1>::type,
                typename point_type<Geometry2>::type,
                FormulaPolicy,
                Spheroid,
                CalculationType
            > type;
    };

    template <typename Geometry1, typename Geometry2>
    inline typename point_in_geometry_strategy<Geometry1, Geometry2>::type
        get_point_in_geometry_strategy() const
    {
        typedef typename point_in_geometry_strategy
            <
                Geometry1, Geometry2
            >::type strategy_type;
        return strategy_type(m_spheroid);
    }

    template <typename Geometry>
    struct area_strategy
    {
        typedef area::geographic
            <
                typename point_type<Geometry>::type,
                FormulaPolicy,
                Order,
                Spheroid,
                CalculationType
            > type;
    };

    template <typename Geometry>
    inline typename area_strategy<Geometry>::type get_area_strategy() const
    {
        typedef typename area_strategy<Geometry>::type strategy_type;
        return strategy_type(m_spheroid);
    }

    template <typename Geometry>
    struct distance_strategy
    {
        typedef distance::geographic
            <
                FormulaPolicy,
                Spheroid,
                CalculationType
            > type;
    };

    template <typename Geometry>
    inline typename distance_strategy<Geometry>::type get_distance_strategy() const
    {
        typedef typename distance_strategy<Geometry>::type strategy_type;
        return strategy_type(m_spheroid);
    }

    typedef envelope::geographic_segment<FormulaPolicy, Spheroid, CalculationType>
        envelope_strategy_type;

    inline envelope_strategy_type get_envelope_strategy() const
    {
        return envelope_strategy_type(m_spheroid);
    }

    enum intersection_point_flag { ipi_inters = 0, ipi_at_a1, ipi_at_a2, ipi_at_b1, ipi_at_b2 };

    template <typename CoordinateType, typename SegmentRatio>
    struct segment_intersection_info
    {
        typedef typename select_most_precise
            <
                CoordinateType, double
            >::type promoted_type;

        promoted_type comparable_length_a() const
        {
            return robust_ra.denominator();
        }

        promoted_type comparable_length_b() const
        {
            return robust_rb.denominator();
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign_a(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            assign(point, a, b);
        }
        template <typename Point, typename Segment1, typename Segment2>
        void assign_b(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            assign(point, a, b);
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            if (ip_flag == ipi_inters)
            {
                // TODO: assign the rest of coordinates
                set_from_radian<0>(point, lon);
                set_from_radian<1>(point, lat);
            }
            else if (ip_flag == ipi_at_a1)
            {
                detail::assign_point_from_index<0>(a, point);
            }
            else if (ip_flag == ipi_at_a2)
            {
                detail::assign_point_from_index<1>(a, point);
            }
            else if (ip_flag == ipi_at_b1)
            {
                detail::assign_point_from_index<0>(b, point);
            }
            else // ip_flag == ipi_at_b2
            {
                detail::assign_point_from_index<1>(b, point);
            }
        }

        CoordinateType lon;
        CoordinateType lat;
        SegmentRatio robust_ra;
        SegmentRatio robust_rb;
        intersection_point_flag ip_flag;
    };

    explicit geographic_segments(Spheroid const& spheroid = Spheroid())
        : m_spheroid(spheroid)
    {}

    // Relate segments a and b
    template
    <
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename RobustPolicy
    >
    inline typename Policy::return_type apply(Segment1 const& a, Segment2 const& b,
                                              Policy const& policy,
                                              RobustPolicy const& robust_policy) const
    {
        typedef typename point_type<Segment1>::type point1_t;
        typedef typename point_type<Segment2>::type point2_t;
        point1_t a1, a2;
        point2_t b1, b2;

        detail::assign_point_from_index<0>(a, a1);
        detail::assign_point_from_index<1>(a, a2);
        detail::assign_point_from_index<0>(b, b1);
        detail::assign_point_from_index<1>(b, b2);

        return apply(a, b, policy, robust_policy, a1, a2, b1, b2);
    }

    // Relate segments a and b
    template
    <
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename RobustPolicy,
        typename Point1,
        typename Point2
    >
    inline typename Policy::return_type apply(Segment1 const& a, Segment2 const& b,
                                              Policy const&, RobustPolicy const&,
                                              Point1 a1, Point1 a2, Point2 b1, Point2 b2) const
    {
        bool is_a_reversed = get<1>(a1) > get<1>(a2);
        bool is_b_reversed = get<1>(b1) > get<1>(b2);

        if (is_a_reversed)
        {
            std::swap(a1, a2);
        }

        if (is_b_reversed)
        {
            std::swap(b1, b2);
        }

        return apply<Policy>(a, b, a1, a2, b1, b2, is_a_reversed, is_b_reversed);
    }

private:
    // Relate segments a and b
    template
    <
        typename Policy,
        typename Segment1,
        typename Segment2,
        typename Point1,
        typename Point2
    >
    inline typename Policy::return_type apply(Segment1 const& a, Segment2 const& b,
                                              Point1 const& a1, Point1 const& a2,
                                              Point2 const& b1, Point2 const& b2,
                                              bool is_a_reversed, bool is_b_reversed) const
    {
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment1>) );
        BOOST_CONCEPT_ASSERT( (concepts::ConstSegment<Segment2>) );

        typedef typename select_calculation_type
            <Segment1, Segment2, CalculationType>::type calc_t;

        typedef srs::spheroid<calc_t> spheroid_type;

        static const calc_t c0 = 0;

        // normalized spheroid
        spheroid_type spheroid = formula::unit_spheroid<spheroid_type>(m_spheroid);

        // TODO: check only 2 first coordinates here?
        using geometry::detail::equals::equals_point_point;
        bool a_is_point = equals_point_point(a1, a2);
        bool b_is_point = equals_point_point(b1, b2);

        if(a_is_point && b_is_point)
        {
            return equals_point_point(a1, b2)
                ? Policy::degenerate(a, true)
                : Policy::disjoint()
                ;
        }

        calc_t const a1_lon = get_as_radian<0>(a1);
        calc_t const a1_lat = get_as_radian<1>(a1);
        calc_t const a2_lon = get_as_radian<0>(a2);
        calc_t const a2_lat = get_as_radian<1>(a2);
        calc_t const b1_lon = get_as_radian<0>(b1);
        calc_t const b1_lat = get_as_radian<1>(b1);
        calc_t const b2_lon = get_as_radian<0>(b2);
        calc_t const b2_lat = get_as_radian<1>(b2);

        side_info sides;

        // NOTE: potential optimization, don't calculate distance at this point
        // this would require to reimplement inverse strategy to allow
        // calculation of distance if needed, probably also storing intermediate
        // results somehow inside an object.
        typedef typename FormulaPolicy::template inverse<calc_t, true, true, false, false, false> inverse_dist_azi;
        typedef typename inverse_dist_azi::result_type inverse_result;

        // TODO: no need to call inverse formula if we know that the points are equal
        // distance can be set to 0 in this case and azimuth may be not calculated
        bool is_equal_a1_b1 = equals_point_point(a1, b1);
        bool is_equal_a2_b1 = equals_point_point(a2, b1);
        bool degen_neq_coords = false;

        inverse_result res_b1_b2, res_b1_a1, res_b1_a2;
        if (! b_is_point)
        {
            res_b1_b2 = inverse_dist_azi::apply(b1_lon, b1_lat, b2_lon, b2_lat, spheroid);
            if (math::equals(res_b1_b2.distance, c0))
            {
                b_is_point = true;
                degen_neq_coords = true;
            }
            else
            {
                res_b1_a1 = inverse_dist_azi::apply(b1_lon, b1_lat, a1_lon, a1_lat, spheroid);
                if (math::equals(res_b1_a1.distance, c0))
                {
                    is_equal_a1_b1 = true;
                }
                res_b1_a2 = inverse_dist_azi::apply(b1_lon, b1_lat, a2_lon, a2_lat, spheroid);
                if (math::equals(res_b1_a2.distance, c0))
                {
                    is_equal_a2_b1 = true;
                }
                sides.set<0>(is_equal_a1_b1 ? 0 : formula::azimuth_side_value(res_b1_a1.azimuth, res_b1_b2.azimuth),
                             is_equal_a2_b1 ? 0 : formula::azimuth_side_value(res_b1_a2.azimuth, res_b1_b2.azimuth));
                if (sides.same<0>())
                {
                    // Both points are at the same side of other segment, we can leave
                    return Policy::disjoint();
                }
            }
        }

        bool is_equal_a1_b2 = equals_point_point(a1, b2);

        inverse_result res_a1_a2, res_a1_b1, res_a1_b2;
        if (! a_is_point)
        {
            res_a1_a2 = inverse_dist_azi::apply(a1_lon, a1_lat, a2_lon, a2_lat, spheroid);
            if (math::equals(res_a1_a2.distance, c0))
            {
                a_is_point = true;
                degen_neq_coords = true;
            }
            else
            {
                res_a1_b1 = inverse_dist_azi::apply(a1_lon, a1_lat, b1_lon, b1_lat, spheroid);
                if (math::equals(res_a1_b1.distance, c0))
                {
                    is_equal_a1_b1 = true;
                }
                res_a1_b2 = inverse_dist_azi::apply(a1_lon, a1_lat, b2_lon, b2_lat, spheroid);
                if (math::equals(res_a1_b2.distance, c0))
                {
                    is_equal_a1_b2 = true;
                }
                sides.set<1>(is_equal_a1_b1 ? 0 : formula::azimuth_side_value(res_a1_b1.azimuth, res_a1_a2.azimuth),
                             is_equal_a1_b2 ? 0 : formula::azimuth_side_value(res_a1_b2.azimuth, res_a1_a2.azimuth));
                if (sides.same<1>())
                {
                    // Both points are at the same side of other segment, we can leave
                    return Policy::disjoint();
                }
            }
        }

        if(a_is_point && b_is_point)
        {
            return is_equal_a1_b2
                ? Policy::degenerate(a, true)
                : Policy::disjoint()
                ;
        }

        // NOTE: at this point the segments may still be disjoint
        // NOTE: at this point one of the segments may be degenerated

        bool collinear = sides.collinear();

        if (! collinear)
        {
            // WARNING: the side strategy doesn't have the info about the other
            // segment so it may return results inconsistent with this intersection
            // strategy, as it checks both segments for consistency

            if (sides.get<0, 0>() == 0 && sides.get<0, 1>() == 0)
            {
                collinear = true;
                sides.set<1>(0, 0);
            }
            else if (sides.get<1, 0>() == 0 && sides.get<1, 1>() == 0)
            {
                collinear = true;
                sides.set<0>(0, 0);
            }
        }

        if (collinear)
        {
            if (a_is_point)
            {
                return collinear_one_degenerated<Policy, calc_t>(a, true, b1, b2, a1, a2, res_b1_b2, res_b1_a1, res_b1_a2, is_b_reversed, degen_neq_coords);
            }
            else if (b_is_point)
            {
                return collinear_one_degenerated<Policy, calc_t>(b, false, a1, a2, b1, b2, res_a1_a2, res_a1_b1, res_a1_b2, is_a_reversed, degen_neq_coords);
            }
            else
            {
                calc_t dist_a1_a2, dist_a1_b1, dist_a1_b2;
                calc_t dist_b1_b2, dist_b1_a1, dist_b1_a2;
                // use shorter segment
                if (res_a1_a2.distance <= res_b1_b2.distance)
                {
                    calculate_collinear_data(a1, a2, b1, b2, res_a1_a2, res_a1_b1, res_a1_b2, dist_a1_a2, dist_a1_b1);
                    calculate_collinear_data(a1, a2, b1, b2, res_a1_a2, res_a1_b2, res_a1_b1, dist_a1_a2, dist_a1_b2);
                    dist_b1_b2 = dist_a1_b2 - dist_a1_b1;
                    dist_b1_a1 = -dist_a1_b1;
                    dist_b1_a2 = dist_a1_a2 - dist_a1_b1;
                }
                else
                {
                    calculate_collinear_data(b1, b2, a1, a2, res_b1_b2, res_b1_a1, res_b1_a2, dist_b1_b2, dist_b1_a1);
                    calculate_collinear_data(b1, b2, a1, a2, res_b1_b2, res_b1_a2, res_b1_a1, dist_b1_b2, dist_b1_a2);
                    dist_a1_a2 = dist_b1_a2 - dist_b1_a1;
                    dist_a1_b1 = -dist_b1_a1;
                    dist_a1_b2 = dist_b1_b2 - dist_b1_a1;
                }

                // NOTE: this is probably not needed
                calc_t const c0 = 0;
                int a1_on_b = position_value(c0, dist_a1_b1, dist_a1_b2);
                int a2_on_b = position_value(dist_a1_a2, dist_a1_b1, dist_a1_b2);
                int b1_on_a = position_value(c0, dist_b1_a1, dist_b1_a2);
                int b2_on_a = position_value(dist_b1_b2, dist_b1_a1, dist_b1_a2);

                if ((a1_on_b < 1 && a2_on_b < 1) || (a1_on_b > 3 && a2_on_b > 3))
                {
                    return Policy::disjoint();
                }

                if (a1_on_b == 1)
                {
                    dist_b1_a1 = 0;
                    dist_a1_b1 = 0;
                }
                else if (a1_on_b == 3)
                {
                    dist_b1_a1 = dist_b1_b2;
                    dist_a1_b2 = 0;
                }

                if (a2_on_b == 1)
                {
                    dist_b1_a2 = 0;
                    dist_a1_b1 = dist_a1_a2;
                }
                else if (a2_on_b == 3)
                {
                    dist_b1_a2 = dist_b1_b2;
                    dist_a1_b2 = dist_a1_a2;
                }

                bool opposite = ! same_direction(res_a1_a2.azimuth, res_b1_b2.azimuth);

                // NOTE: If segment was reversed opposite, positions and segment ratios has to be altered
                if (is_a_reversed)
                {
                    // opposite
                    opposite = ! opposite;
                    // positions
                    std::swap(a1_on_b, a2_on_b);
                    b1_on_a = 4 - b1_on_a;
                    b2_on_a = 4 - b2_on_a;
                    // distances for ratios
                    std::swap(dist_b1_a1, dist_b1_a2);
                    dist_a1_b1 = dist_a1_a2 - dist_a1_b1;
                    dist_a1_b2 = dist_a1_a2 - dist_a1_b2;
                }
                if (is_b_reversed)
                {
                    // opposite
                    opposite = ! opposite;
                    // positions
                    a1_on_b = 4 - a1_on_b;
                    a2_on_b = 4 - a2_on_b;
                    std::swap(b1_on_a, b2_on_a);
                    // distances for ratios
                    dist_b1_a1 = dist_b1_b2 - dist_b1_a1;
                    dist_b1_a2 = dist_b1_b2 - dist_b1_a2;
                    std::swap(dist_a1_b1, dist_a1_b2);
                }

                segment_ratio<calc_t> ra_from(dist_b1_a1, dist_b1_b2);
                segment_ratio<calc_t> ra_to(dist_b1_a2, dist_b1_b2);
                segment_ratio<calc_t> rb_from(dist_a1_b1, dist_a1_a2);
                segment_ratio<calc_t> rb_to(dist_a1_b2, dist_a1_a2);

                return Policy::segments_collinear(a, b, opposite,
                    a1_on_b, a2_on_b, b1_on_a, b2_on_a,
                    ra_from, ra_to, rb_from, rb_to);
            }
        }
        else // crossing or touching
        {
            if (a_is_point || b_is_point)
            {
                return Policy::disjoint();
            }

            calc_t lon = 0, lat = 0;
            intersection_point_flag ip_flag;
            calc_t dist_a1_a2, dist_a1_i1, dist_b1_b2, dist_b1_i1;
            if (calculate_ip_data(a1, a2, b1, b2,
                                  a1_lon, a1_lat, a2_lon, a2_lat,
                                  b1_lon, b1_lat, b2_lon, b2_lat,
                                  res_a1_a2, res_a1_b1, res_a1_b2,
                                  res_b1_b2, res_b1_a1, res_b1_a2,
                                  sides, spheroid,
                                  lon, lat,
                                  dist_a1_a2, dist_a1_i1, dist_b1_b2, dist_b1_i1,
                                  ip_flag))
            {
                // NOTE: If segment was reversed sides and segment ratios has to be altered
                if (is_a_reversed)
                {
                    // sides
                    sides_reverse_segment<0>(sides);
                    // distance for ratio
                    dist_a1_i1 = dist_a1_a2 - dist_a1_i1;
                    // ip flag
                    ip_flag_reverse_segment(ip_flag, ipi_at_a1, ipi_at_a2);
                }
                if (is_b_reversed)
                {
                    // sides
                    sides_reverse_segment<1>(sides);
                    // distance for ratio
                    dist_b1_i1 = dist_b1_b2 - dist_b1_i1;
                    // ip flag
                    ip_flag_reverse_segment(ip_flag, ipi_at_b1, ipi_at_b2);
                }

                // intersects
                segment_intersection_info
                    <
                        calc_t,
                        segment_ratio<calc_t>
                    > sinfo;

                sinfo.lon = lon;
                sinfo.lat = lat;
                sinfo.robust_ra.assign(dist_a1_i1, dist_a1_a2);
                sinfo.robust_rb.assign(dist_b1_i1, dist_b1_b2);
                sinfo.ip_flag = ip_flag;

                return Policy::segments_crosses(sides, sinfo, a, b);
            }
            else
            {
                return Policy::disjoint();
            }
        }
    }

    template <typename Policy, typename CalcT, typename Segment, typename Point1, typename Point2, typename ResultInverse>
    static inline typename Policy::return_type
        collinear_one_degenerated(Segment const& segment, bool degenerated_a,
                                  Point1 const& a1, Point1 const& a2,
                                  Point2 const& b1, Point2 const& b2,
                                  ResultInverse const& res_a1_a2,
                                  ResultInverse const& res_a1_b1,
                                  ResultInverse const& res_a1_b2,
                                  bool is_other_reversed,
                                  bool degen_neq_coords)
    {
        CalcT dist_1_2, dist_1_o;
        if (! calculate_collinear_data(a1, a2, b1, b2, res_a1_a2, res_a1_b1, res_a1_b2, dist_1_2, dist_1_o, degen_neq_coords))
        {
            return Policy::disjoint();
        }

        // NOTE: If segment was reversed segment ratio has to be altered
        if (is_other_reversed)
        {
            // distance for ratio
            dist_1_o = dist_1_2 - dist_1_o;
        }

        return Policy::one_degenerate(segment, segment_ratio<CalcT>(dist_1_o, dist_1_2), degenerated_a);
    }

    // TODO: instead of checks below test bi against a1 and a2 here?
    //       in order to make this independent from is_near()
    template <typename Point1, typename Point2, typename ResultInverse, typename CalcT>
    static inline bool calculate_collinear_data(Point1 const& a1, Point1 const& a2, // in
                                                Point2 const& b1, Point2 const& b2, // in
                                                ResultInverse const& res_a1_a2,     // in
                                                ResultInverse const& res_a1_b1,     // in
                                                ResultInverse const& res_a1_b2,     // in
                                                CalcT& dist_a1_a2,                  // out
                                                CalcT& dist_a1_bi,                  // out
                                                bool degen_neq_coords = false)      // in
    {
        dist_a1_a2 = res_a1_a2.distance;

        dist_a1_bi = res_a1_b1.distance;
        if (! same_direction(res_a1_b1.azimuth, res_a1_a2.azimuth))
        {
            dist_a1_bi = -dist_a1_bi;
        }

        // if i1 is close to a1 and b1 or b2 is equal to a1
        if (is_endpoint_equal(dist_a1_bi, a1, b1, b2))
        {
            dist_a1_bi = 0;
            return true;
        }
        // or i1 is close to a2 and b1 or b2 is equal to a2
        else if (is_endpoint_equal(dist_a1_a2 - dist_a1_bi, a2, b1, b2))
        {
            dist_a1_bi = dist_a1_a2;
            return true;
        }

        // check the other endpoint of a very short segment near the pole
        if (degen_neq_coords)
        {
            static CalcT const c0 = 0;
            if (math::equals(res_a1_b2.distance, c0))
            {
                dist_a1_bi = 0;
                return true;
            }
            else if (math::equals(dist_a1_a2 - res_a1_b2.distance, c0))
            {
                dist_a1_bi = dist_a1_a2;
                return true;
            }
        }

        // or i1 is on b
        return segment_ratio<CalcT>(dist_a1_bi, dist_a1_a2).on_segment();
    }

    template <typename Point1, typename Point2, typename CalcT, typename ResultInverse, typename Spheroid_>
    static inline bool calculate_ip_data(Point1 const& a1, Point1 const& a2,       // in
                                         Point2 const& b1, Point2 const& b2,       // in
                                         CalcT const& a1_lon, CalcT const& a1_lat, // in
                                         CalcT const& a2_lon, CalcT const& a2_lat, // in
                                         CalcT const& b1_lon, CalcT const& b1_lat, // in
                                         CalcT const& b2_lon, CalcT const& b2_lat, // in
                                         ResultInverse const& res_a1_a2,           // in
                                         ResultInverse const& res_a1_b1,           // in
                                         ResultInverse const& res_a1_b2,           // in
                                         ResultInverse const& res_b1_b2,           // in
                                         ResultInverse const& res_b1_a1,           // in
                                         ResultInverse const& res_b1_a2,           // in
                                         side_info const& sides,                   // in
                                         Spheroid_ const& spheroid,                // in
                                         CalcT & lon, CalcT & lat,             // out
                                         CalcT& dist_a1_a2, CalcT& dist_a1_ip, // out
                                         CalcT& dist_b1_b2, CalcT& dist_b1_ip, // out
                                         intersection_point_flag& ip_flag)     // out
    {
        dist_a1_a2 = res_a1_a2.distance;
        dist_b1_b2 = res_b1_b2.distance;

        // assign the IP if some endpoints overlap
        using geometry::detail::equals::equals_point_point;
        if (equals_point_point(a1, b1))
        {
            lon = a1_lon;
            lat = a1_lat;
            dist_a1_ip = 0;
            dist_b1_ip = 0;
            ip_flag = ipi_at_a1;
            return true;
        }
        else if (equals_point_point(a1, b2))
        {
            lon = a1_lon;
            lat = a1_lat;
            dist_a1_ip = 0;
            dist_b1_ip = dist_b1_b2;
            ip_flag = ipi_at_a1;
            return true;
        }
        else if (equals_point_point(a2, b1))
        {
            lon = a2_lon;
            lat = a2_lat;
            dist_a1_ip = dist_a1_a2;
            dist_b1_ip = 0;
            ip_flag = ipi_at_a2;
            return true;
        }
        else if (equals_point_point(a2, b2))
        {
            lon = a2_lon;
            lat = a2_lat;
            dist_a1_ip = dist_a1_a2;
            dist_b1_ip = dist_b1_b2;
            ip_flag = ipi_at_a2;
            return true;
        }

        // at this point we know that the endpoints doesn't overlap
        // check cases when an endpoint lies on the other geodesic
        if (sides.template get<0, 0>() == 0) // a1 wrt b
        {
            if (res_b1_a1.distance <= res_b1_b2.distance
                && same_direction(res_b1_a1.azimuth, res_b1_b2.azimuth))
            {
                lon = a1_lon;
                lat = a1_lat;
                dist_a1_ip = 0;
                dist_b1_ip = res_b1_a1.distance;
                ip_flag = ipi_at_a1;
                return true;
            }
            else
            {
                return false;
            }
        }
        else if (sides.template get<0, 1>() == 0) // a2 wrt b
        {
            if (res_b1_a2.distance <= res_b1_b2.distance
                && same_direction(res_b1_a2.azimuth, res_b1_b2.azimuth))
            {
                lon = a2_lon;
                lat = a2_lat;
                dist_a1_ip = res_a1_a2.distance;
                dist_b1_ip = res_b1_a2.distance;
                ip_flag = ipi_at_a2;
                return true;
            }
            else
            {
                return false;
            }
        }
        else if (sides.template get<1, 0>() == 0) // b1 wrt a
        {
            if (res_a1_b1.distance <= res_a1_a2.distance
                && same_direction(res_a1_b1.azimuth, res_a1_a2.azimuth))
            {
                lon = b1_lon;
                lat = b1_lat;
                dist_a1_ip = res_a1_b1.distance;
                dist_b1_ip = 0;
                ip_flag = ipi_at_b1;
                return true;
            }
            else
            {
                return false;
            }
        }
        else if (sides.template get<1, 1>() == 0) // b2 wrt a
        {
            if (res_a1_b2.distance <= res_a1_a2.distance
                && same_direction(res_a1_b2.azimuth, res_a1_a2.azimuth))
            {
                lon = b2_lon;
                lat = b2_lat;
                dist_a1_ip = res_a1_b2.distance;
                dist_b1_ip = res_b1_b2.distance;
                ip_flag = ipi_at_b2;
                return true;
            }
            else
            {
                return false;
            }
        }

        // At this point neither the endpoints overlaps
        // nor any andpoint lies on the other geodesic
        // So the endpoints should lie on the opposite sides of both geodesics

        bool const ok = formula::sjoberg_intersection<CalcT, FormulaPolicy::template inverse, Order>
                        ::apply(a1_lon, a1_lat, a2_lon, a2_lat, res_a1_a2.azimuth,
                                b1_lon, b1_lat, b2_lon, b2_lat, res_b1_b2.azimuth,
                                lon, lat, spheroid);

        if (! ok)
        {
            return false;
        }

        typedef typename FormulaPolicy::template inverse<CalcT, true, true, false, false, false> inverse_dist_azi;
        typedef typename inverse_dist_azi::result_type inverse_result;

        inverse_result const res_a1_ip = inverse_dist_azi::apply(a1_lon, a1_lat, lon, lat, spheroid);
        dist_a1_ip = res_a1_ip.distance;
        if (! same_direction(res_a1_ip.azimuth, res_a1_a2.azimuth))
        {
            dist_a1_ip = -dist_a1_ip;
        }

        bool is_on_a = segment_ratio<CalcT>(dist_a1_ip, dist_a1_a2).on_segment();
        // NOTE: not fully consistent with equals_point_point() since radians are always used.
        bool is_on_a1 = math::equals(lon, a1_lon) && math::equals(lat, a1_lat);
        bool is_on_a2 = math::equals(lon, a2_lon) && math::equals(lat, a2_lat);

        if (! (is_on_a || is_on_a1 || is_on_a2))
        {
            return false;
        }

        inverse_result const res_b1_ip = inverse_dist_azi::apply(b1_lon, b1_lat, lon, lat, spheroid);
        dist_b1_ip = res_b1_ip.distance;
        if (! same_direction(res_b1_ip.azimuth, res_b1_b2.azimuth))
        {
            dist_b1_ip = -dist_b1_ip;
        }

        bool is_on_b = segment_ratio<CalcT>(dist_b1_ip, dist_b1_b2).on_segment();
        // NOTE: not fully consistent with equals_point_point() since radians are always used.
        bool is_on_b1 = math::equals(lon, b1_lon) && math::equals(lat, b1_lat);
        bool is_on_b2 = math::equals(lon, b2_lon) && math::equals(lat, b2_lat);

        if (! (is_on_b || is_on_b1 || is_on_b2))
        {
            return false;
        }

        ip_flag = ipi_inters;

        if (is_on_b1)
        {
            lon = b1_lon;
            lat = b1_lat;
            dist_b1_ip = 0;
            ip_flag = ipi_at_b1;
        }
        else if (is_on_b2)
        {
            lon = b2_lon;
            lat = b2_lat;
            dist_b1_ip = res_b1_b2.distance;
            ip_flag = ipi_at_b2;
        }

        if (is_on_a1)
        {
            lon = a1_lon;
            lat = a1_lat;
            dist_a1_ip = 0;
            ip_flag = ipi_at_a1;
        }
        else if (is_on_a2)
        {
            lon = a2_lon;
            lat = a2_lat;
            dist_a1_ip = res_a1_a2.distance;
            ip_flag = ipi_at_a2;
        }

        return true;
    }

    template <typename CalcT, typename P1, typename P2>
    static inline bool is_endpoint_equal(CalcT const& dist,
                                         P1 const& ai, P2 const& b1, P2 const& b2)
    {
        static CalcT const c0 = 0;
        using geometry::detail::equals::equals_point_point;
        return is_near(dist) && (equals_point_point(ai, b1) || equals_point_point(ai, b2) || math::equals(dist, c0));
    }

    template <typename CalcT>
    static inline bool is_near(CalcT const& dist)
    {
        // NOTE: This strongly depends on the Inverse method
        CalcT const small_number = CalcT(boost::is_same<CalcT, float>::value ? 0.0001 : 0.00000001);
        return math::abs(dist) <= small_number;
    }

    template <typename ProjCoord1, typename ProjCoord2>
    static inline int position_value(ProjCoord1 const& ca1,
                                     ProjCoord2 const& cb1,
                                     ProjCoord2 const& cb2)
    {
        // S1x  0   1    2     3   4
        // S2       |---------->
        return math::equals(ca1, cb1) ? 1
             : math::equals(ca1, cb2) ? 3
             : cb1 < cb2 ?
                ( ca1 < cb1 ? 0
                : ca1 > cb2 ? 4
                : 2 )
              : ( ca1 > cb1 ? 0
                : ca1 < cb2 ? 4
                : 2 );
    }

    template <typename CalcT>
    static inline bool same_direction(CalcT const& azimuth1, CalcT const& azimuth2)
    {
        // distance between two angles normalized to (-180, 180]
        CalcT const angle_diff = math::longitude_distance_signed<radian>(azimuth1, azimuth2);
        return math::abs(angle_diff) <= math::half_pi<CalcT>();
    }

    template <int Which>
    static inline void sides_reverse_segment(side_info & sides)
    {
        // names assuming segment A is reversed (Which == 0)
        int a1_wrt_b = sides.template get<Which, 0>();
        int a2_wrt_b = sides.template get<Which, 1>();
        std::swap(a1_wrt_b, a2_wrt_b);
        sides.template set<Which>(a1_wrt_b, a2_wrt_b);
        int b1_wrt_a = sides.template get<1 - Which, 0>();
        int b2_wrt_a = sides.template get<1 - Which, 1>();
        sides.template set<1 - Which>(-b1_wrt_a, -b2_wrt_a);
    }

    static inline void ip_flag_reverse_segment(intersection_point_flag & ip_flag,
                                               intersection_point_flag const& ipi_at_p1,
                                               intersection_point_flag const& ipi_at_p2)
    {
        ip_flag = ip_flag == ipi_at_p1 ? ipi_at_p2 :
                  ip_flag == ipi_at_p2 ? ipi_at_p1 :
                  ip_flag;
    }

private:
    Spheroid m_spheroid;
};

}} // namespace strategy::intersection

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INTERSECTION_HPP


/********************************************************************************
Copyright (c) 2015, TRACLabs, Inc.
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice,
       this list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright notice,
       this list of conditions and the following disclaimer in the documentation
       and/or other materials provided with the distribution.

    3. Neither the name of the copyright holder nor the names of its contributors
       may be used to endorse or promote products derived from this software
       without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
********************************************************************************/

#include <trac_ik/trac_ik.hpp>
#include <boost/date_time.hpp>
#include <boost/make_shared.hpp>
#include <Eigen/Geometry>
#include <ros/ros.h>
#include <limits>

namespace TRAC_IK {

  TRAC_IK::TRAC_IK(const KDL::Chain& _chain, const KDL::JntArray& _q_min, const KDL::JntArray& _q_max, double _maxtime, double _eps, SolveType _type):
    chain(_chain),
    jacsolver(_chain),
    eps(_eps),
    maxtime(_maxtime),
    solvetype(_type),
    nl_solver(chain,_q_min,_q_max,maxtime,eps, NLOPT_IK::SumSq),
    iksolver(chain,_q_min,_q_max,maxtime,eps,true,true),
    work(io_service)
  {

    assert(chain.getNrOfJoints()==_q_min.data.size());
    assert(chain.getNrOfJoints()==_q_max.data.size());

    for (uint i=0; i<chain.getNrOfJoints(); i++) {
      lb.push_back(_q_min(i));
      ub.push_back(_q_max(i));
    }

    for (uint i=0; i<chain.segments.size(); i++) {
      std::string type = chain.segments[i].getJoint().getTypeName();
      if (type.find("Rot")!=std::string::npos) {
        if ((_q_max(types.size())==0 && _q_min(types.size())==0) ||
            (_q_max(types.size())>=std::numeric_limits<float>::max() &&
             _q_min(types.size())<=-std::numeric_limits<float>::max()))
          types.push_back(KDL::BasicJointType::Continuous);
        else
          types.push_back(KDL::BasicJointType::RotJoint);
      }
      else if (type.find("Trans")!=std::string::npos)
        types.push_back(KDL::BasicJointType::TransJoint);
    }

    assert(types.size()==lb.size());

    threads.create_thread(boost::bind(&boost::asio::io_service::run,
                                      &io_service));
    threads.create_thread(boost::bind(&boost::asio::io_service::run,
                                      &io_service));

  }

  bool TRAC_IK::unique_solution(const KDL::JntArray& sol) {

    for (uint i=0; i< solutions.size(); i++)
      if (myEqual(sol,solutions[i]))
        return false;
    return true;

  }

  bool TRAC_IK::runKDL(const KDL::JntArray &q_init, const KDL::Frame &p_in)
  {
    KDL::JntArray q_out;

    double fulltime = maxtime;
    KDL::JntArray seed = q_init;

    boost::posix_time::time_duration timediff;
    double time_left;

    while (true) {
      timediff=boost::posix_time::microsec_clock::local_time()-start_time;
      time_left = fulltime - timediff.total_nanoseconds()/1000000000.0;

      if (time_left <= 0)
        break;

      iksolver.setMaxtime(time_left);

      int kdlRC = iksolver.CartToJnt(seed,p_in,q_out,bounds);
      if (kdlRC >=0) {
        switch (solvetype) {
        case Manip1:
        case Manip2:
          normalize_limits(q_init, q_out);
          break;
        default:
          normalize_seed(q_init, q_out);
          break;
        }
        mtx_.lock();
        if (unique_solution(q_out)) {
          solutions.push_back(q_out);
          uint curr_size=solutions.size();
          errors.resize(curr_size);
          mtx_.unlock();
          double err, penalty;
          switch (solvetype) {
          case Manip1:
            penalty = manipPenalty(q_out);
            err = penalty*TRAC_IK::ManipValue1(q_out);
            break;
          case Manip2:
            penalty = manipPenalty(q_out);
            err = penalty*TRAC_IK::ManipValue2(q_out);
            break;
          default:
            err = TRAC_IK::JointErr(q_init,q_out);
            break;
          }
          mtx_.lock();
          errors[curr_size-1] = std::make_pair(err,curr_size-1);
        }
        mtx_.unlock();
      }

      if (!solutions.empty() && solvetype == Speed)
        break;

      for (unsigned int j=0; j<seed.data.size(); j++)
        if (types[j]==KDL::BasicJointType::Continuous)
          seed(j)=fRand(q_init(j)-2*M_PI, q_init(j)+2*M_PI);
        else
          seed(j)=fRand(lb[j], ub[j]);
    }
    nl_solver.abort();

    iksolver.setMaxtime(fulltime);

    return true;
  }

  bool TRAC_IK::runNLOPT(const KDL::JntArray &q_init, const KDL::Frame &p_in)
  {
    KDL::JntArray q_out;

    double fulltime = maxtime;
    KDL::JntArray seed = q_init;

    boost::posix_time::time_duration timediff;
    double time_left;

    while (true) {
      timediff=boost::posix_time::microsec_clock::local_time()-start_time;
      time_left = fulltime - timediff.total_nanoseconds()/1000000000.0;

      if (time_left <= 0)
        break;

      nl_solver.setMaxtime(time_left);

      int nloptRC = nl_solver.CartToJnt(seed,p_in,q_out,bounds);
      if (nloptRC >=0) {
        switch (solvetype) {
        case Manip1:
        case Manip2:
          normalize_limits(q_init, q_out);
          break;
        default:
          normalize_seed(q_init, q_out);
          break;
        }
        mtx_.lock();
        if (unique_solution(q_out)) {
          solutions.push_back(q_out);
          uint curr_size=solutions.size();
          errors.resize(curr_size);
          mtx_.unlock();
          double err, penalty;
          switch (solvetype) {
          case Manip1:
            penalty = manipPenalty(q_out);
            err = penalty*TRAC_IK::ManipValue1(q_out);
            break;
          case Manip2:
            penalty = manipPenalty(q_out);
            err = penalty*TRAC_IK::ManipValue2(q_out);
            break;
          default:
            err = TRAC_IK::JointErr(q_init,q_out);
            break;
          }
          mtx_.lock();
          errors[curr_size-1] = std::make_pair(err,curr_size-1);
        }
        mtx_.unlock();
      }

      if (!solutions.empty() && solvetype == Speed)
        break;

      for (unsigned int j=0; j<seed.data.size(); j++)
        if (types[j]==KDL::BasicJointType::Continuous)
          seed(j)=fRand(q_init(j)-2*M_PI, q_init(j)+2*M_PI);
        else
          seed(j)=fRand(lb[j], ub[j]);
    }

    iksolver.abort();

    nl_solver.setMaxtime(fulltime);

    return true;
  }

  void TRAC_IK::normalize_seed(const KDL::JntArray& seed, KDL::JntArray& solution) {
    // Make sure rotational joint values are within 1 revolution of seed; then
    // ensure joint limits are met.

    bool improved = false;

    for (uint i=0; i<lb.size(); i++) {

      if (types[i]==KDL::BasicJointType::TransJoint)
        continue;

      double target = seed(i);
      double val = solution(i);

      if (val > target+M_PI) {
        //Find actual angle offset
        double diffangle = fmod(val-target,2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = target + diffangle - 2*M_PI;
      }

      if (val < target-M_PI) {
        //Find actual angle offset
        double diffangle = fmod(target-val,2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = target - diffangle + 2*M_PI;
      }

      if (types[i]==KDL::BasicJointType::Continuous) {
        solution(i) = val;
        continue;
      }

      if (val > ub[i]) {
        //Find actual angle offset
        double diffangle = fmod(val-ub[i],2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = ub[i] + diffangle - 2*M_PI;
      }

      if (val < lb[i]) {
        //Find actual angle offset
        double diffangle = fmod(lb[i]-val,2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = lb[i] - diffangle + 2*M_PI;
      }

      solution(i) = val;
    }
  }

  void TRAC_IK::normalize_limits(const KDL::JntArray& seed, KDL::JntArray& solution) {
    // Make sure rotational joint values are within 1 revolution of middle of
    // limits; then ensure joint limits are met.

    bool improved = false;

    for (uint i=0; i<lb.size(); i++) {

      if (types[i] == KDL::BasicJointType::TransJoint)
        continue;

      double target = seed(i);

      if (types[i] == KDL::BasicJointType::RotJoint)
        target = (ub[i]+lb[i])/2.0;

      double val = solution(i);

      if (val > target+M_PI) {
        //Find actual angle offset
        double diffangle = fmod(val-target,2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = target + diffangle - 2*M_PI;
      }

      if (val < target-M_PI) {
        //Find actual angle offset
        double diffangle = fmod(target-val,2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = target - diffangle + 2*M_PI;
      }

      if (types[i]==KDL::BasicJointType::Continuous) {
        solution(i) = val;
        continue;
      }

      if (val > ub[i]) {
        //Find actual angle offset
        double diffangle = fmod(val-ub[i],2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = ub[i] + diffangle - 2*M_PI;
      }

      if (val < lb[i]) {
        //Find actual angle offset
        double diffangle = fmod(lb[i]-val,2*M_PI);
        // Add that to upper bound and go back a full rotation
        val = lb[i] - diffangle + 2*M_PI;
      }

      solution(i) = val;
    }

  }

  double TRAC_IK::manipPenalty(const KDL::JntArray& arr) {
    double penalty = 1.0;
    for (uint i=0; i< arr.data.size(); i++) {
      if (types[i] == KDL::BasicJointType::Continuous)
        continue;
      double range = ub[i]-lb[i];
      penalty *= ((arr(i)-lb[i])*(ub[i]-arr(i))/(range*range));
    }
    return (1.0 - exp(-1*penalty));
  }

  double TRAC_IK::ManipValue1(const KDL::JntArray& arr) {
    KDL::Jacobian jac(arr.data.size());

    jacsolver.JntToJac(arr,jac);

    Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
    Eigen::MatrixXd singular_values = svdsolver.singularValues();

    double error = 1.0;
    for(unsigned int i=0; i < singular_values.rows(); ++i)
      error *= singular_values(i,0);
    return error;
  }

  double TRAC_IK::ManipValue2(const KDL::JntArray& arr) {
    KDL::Jacobian jac(arr.data.size());

    jacsolver.JntToJac(arr,jac);

    Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
    Eigen::MatrixXd singular_values = svdsolver.singularValues();

    return singular_values.minCoeff()/singular_values.maxCoeff();
  }

  int TRAC_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& _bounds) {

    static uint calls =0;
    static uint inconsistent =0;

    start_time = boost::posix_time::microsec_clock::local_time();

    nl_solver.reset();
    iksolver.reset();

    solutions.clear();
    errors.clear();

    bounds=_bounds;

    std::vector<boost::shared_future<bool> > pending_data;

    typedef boost::packaged_task<bool> task_t;
    boost::shared_ptr<task_t> task1 = boost::make_shared<task_t>(boost::bind(&TRAC_IK::runKDL, this, boost::cref(q_init), boost::cref(p_in)));

    boost::shared_ptr<task_t> task2 = boost::make_shared<task_t>(boost::bind(&TRAC_IK::runNLOPT, this, boost::cref(q_init), boost::cref(p_in)));

    boost::shared_future<bool> fut1(task1->get_future());
    boost::shared_future<bool> fut2(task2->get_future());

    /*
    // this was for pre-c++11
    pending_data.push_back(boost::move(fut1));
    pending_data.push_back(boost::move(fut2));
    */
    pending_data.push_back(fut1);
    pending_data.push_back(fut2);

    io_service.post(boost::bind(&task_t::operator(), task1));
    io_service.post(boost::bind(&task_t::operator(), task2));

    boost::wait_for_all(pending_data.begin(), pending_data.end());

    if (solutions.empty()) {
      q_out=q_init;
      return -3;
    }

    switch (solvetype) {
    case Manip1:
    case Manip2:
      std::sort(errors.rbegin(),errors.rend()); // rbegin/rend to sort by max
      break;
    default:
      std::sort(errors.begin(),errors.end());
      break;
    }

    q_out = solutions[errors[0].second];

    return solutions.size();
  }

  TRAC_IK::~TRAC_IK(){
    // Force all threads to return from io_service::run().
    io_service.stop();

    // Suppress all exceptions.
    try
      {
        threads.join_all();
      }
    catch ( ... ) {}

  }

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_UMFPACK_SOLVE_INCLUDE
#define MTL_MATRIX_UMFPACK_SOLVE_INCLUDE

#ifdef MTL_HAS_UMFPACK

#include <iostream>

#include <cassert>
#include <algorithm>
#include <boost/mpl/bool.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/make_copy_or_reference.hpp>
#include <boost/numeric/mtl/operation/merge_complex_vector.hpp>
#include <boost/numeric/mtl/operation/split_complex_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

extern "C" {
#  include <umfpack.h>
}

namespace mtl { namespace matrix {

    /// Namespace for Umfpack solver
    namespace umfpack {

	// conversion for value_type needed if not double or complex<double> (where possible)
	template <typename Value> struct value         	    {};
	template<> struct value<long double>           	    { typedef double               type; };
	template<> struct value<double>                	    { typedef double               type; };
	template<> struct value<float>                 	    { typedef double               type; };
	template<> struct value<std::complex<long double> > { typedef std::complex<double> type; };
	template<> struct value<std::complex<double> > 	    { typedef std::complex<double> type; };
	template<> struct value<std::complex<float> >  	    { typedef std::complex<double> type; };

	template <typename Value> struct use_long { static const bool value= sizeof(Value) > sizeof(int); };

	template <bool Larger> struct index_aux   { typedef int     type; };
#if defined(UF_long)
	template<> struct index_aux<true>         { typedef UF_long type; };
#elif defined(SuiteSparse_long)
	template<> struct index_aux<true>         { typedef SuiteSparse_long type; };
#else
	template<> struct index_aux<true>         { typedef long type; };
#endif

	template <typename Value> struct index
          : index_aux<use_long<Value>::value> {};

	template <typename Matrix, typename Value, typename Orientation>
	struct matrix_copy {};

	// If arbitrary compressed matrix -> copy
	template <typename Value, typename Parameters, typename Orientation>
	struct matrix_copy<compressed2D<Value, Parameters>, Value, Orientation>
	{
	    typedef typename value<Value>::type                      value_type;
	    typedef compressed2D<value_type, parameters<col_major> > matrix_type;
	    typedef compressed2D<Value, Parameters>                  in_matrix_type;

	    matrix_copy(const in_matrix_type& A) : matrix(A) {}
	    matrix_type matrix;
	};

	struct error : public domain_error
	{
	    error(const char *s, int code) : domain_error(s), code(code) {}
	    int code;
	};

	inline void check(int res, const char* s)
	{
	    MTL_THROW_IF(res != UMFPACK_OK, error(s, res));
	}

	/// Class for repeated Umfpack solutions
	/** Keeps symbolic and numeric preprocessing. Numeric part can be updated.
	    Only defined for compressed2D<double> and compressed2D<complex<double> >. **/
	template <typename T>
	class solver {
	  public:
	    /// Constructor referring to matrix \p A (not changed) and optionally Umfpack's strategy and alloc_init (look for the specializations)
	    // \ref solver<compressed2D<double, Parameters> > and \ref solver<compressed2D<std::complex<double>, Parameters> >)
	    explicit solver(const T& /*A*/) {}

	    /// Update numeric part, for matrices that kept the sparsity and changed the values
	    void update_numeric() {}

	    /// Update symbolic and numeric part
	    void update() {}

	    /// Solve system A*x == b with matrix passed in constructor
	    /** Please note that the order of b and x is different than in solve() !!! **/
	    template <typename VectorX, typename VectorB>
	    int operator()(VectorX& /*x*/, const VectorB& /*b*/) const {return 0;}

	    /// Solve system A*x == b with matrix passed in constructor
	    /** Please note that the order of b and x is different than in operator() !!! **/
	    template <typename VectorB, typename VectorX>
	    int operator()(const VectorB& /*b*/, VectorX& /*x*/) const {return 0;}
	};

	/// Speciatization of solver for \ref matrix::compressed2D with double values
	template <typename Parameters>
	class solver<compressed2D<double, Parameters> >
	{
	    typedef double                                    value_type;
	    typedef compressed2D<value_type, Parameters>      matrix_type;
	    typedef typename matrix_type::size_type           size_type;
	    typedef typename index<size_type>::type           index_type;

	    static const bool copy_indices= sizeof(index_type) != sizeof(size_type),
		              long_indices= use_long<size_type>::value;
	    typedef boost::mpl::bool_<long_indices>           blong;
	    typedef boost::mpl::true_                         true_;
	    typedef boost::mpl::false_                        false_;

	    // typedef parameters<col_major>     Parameters;

	    void assign_pointers()
	    {
		if (copy_indices) {
		    if (Apc == 0) Apc= new index_type[n + 1];
		    if (my_nnz != A.nnz() && Aic) { delete[] Aic; Aic= 0; }
		    if (Aic == 0) Aic= new index_type[A.nnz()];
		    std::copy(A.address_major(), A.address_major() + n + 1, Apc);
		    std::copy(A.address_minor(), A.address_minor() + A.nnz(), Aic);
		    Ap= Apc;
		    Ai= Aic;
		} else {
		    Ap= reinterpret_cast<const index_type*>(A.address_major());
		    Ai= reinterpret_cast<const index_type*>(A.address_minor());
		}
		Ax= A.address_data();
	    }

	    void init_aux(true_)
	    {
	      check(umfpack_dl_symbolic(n, n, Ap, Ai, Ax, &Symbolic, Control, Info), "Error in dl_symbolic");
	      check(umfpack_dl_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in dl_numeric");
	    }

	    void init_aux(false_)
	    {
		check(umfpack_di_symbolic(n, n, Ap, Ai, Ax, &Symbolic, Control, Info), "Error in di_symbolic");
#if 0
		std::cout << "=== INFO of umfpack_*_symbolic ===\n";
		std::cout << "    UMFPACK_STATUS: " << (Info[UMFPACK_STATUS] == UMFPACK_OK ? "OK" : "ERROR") << "\n";
		std::cout << "    UMFPACK_NROW: " << Info[UMFPACK_NROW] << "\n";
		std::cout << "    UMFPACK_NCOL: " << Info[UMFPACK_NCOL] << "\n";
		std::cout << "    UMFPACK_NZ: " << Info[UMFPACK_NZ] << "\n";
		std::cout << "    UMFPACK_SIZE_OF_UNIT: " << Info[UMFPACK_SIZE_OF_UNIT] << "\n";
		std::cout << "    UMFPACK_NDENSE_ROW: " << Info[UMFPACK_NDENSE_ROW] << "\n";
		std::cout << "    UMFPACK_NEMPTY_ROW: " << Info[UMFPACK_NEMPTY_ROW] << "\n";
		std::cout << "    UMFPACK_NDENSE_COL: " << Info[UMFPACK_NDENSE_COL] << "\n";
		std::cout << "    UMFPACK_NEMPTY_COL: " << Info[UMFPACK_NEMPTY_COL] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_DEFRAG: " << Info[UMFPACK_SYMBOLIC_DEFRAG] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_PEAK_MEMORY: " << Info[UMFPACK_SYMBOLIC_PEAK_MEMORY] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_SIZE: " << Info[UMFPACK_SYMBOLIC_SIZE] << "\n";
		std::cout << "    UMFPACK_VARIABLE_PEAK_ESTIMATE: " << Info[UMFPACK_VARIABLE_PEAK_ESTIMATE]  << "\n";
		std::cout << "    UMFPACK_NUMERIC_SIZE_ESTIMATE: " << Info[UMFPACK_NUMERIC_SIZE_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_PEAK_MEMORY_ESTIMATE: " << Info[UMFPACK_PEAK_MEMORY_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_FLOPS_ESTIMATE: " << Info[UMFPACK_FLOPS_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_LNZ_ESTIMATE: " << Info[UMFPACK_LNZ_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_UNZ_ESTIMATE: " << Info[UMFPACK_UNZ_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_SIZE_ESTIMATE: " << Info[UMFPACK_MAX_FRONT_SIZE_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_TIME: " << Info[UMFPACK_SYMBOLIC_TIME] << "\n";
		std::cout << "    UMFPACK_SYMBOLIC_WALLTIME: " << Info[UMFPACK_SYMBOLIC_WALLTIME] << "\n";

		if (Info[UMFPACK_STRATEGY_USED] == UMFPACK_STRATEGY_SYMMETRIC)
		  std::cout << "    UMFPACK_STRATEGY_USED: SYMMETRIC\n";
		else {
		  if(Info[UMFPACK_STRATEGY_USED] == UMFPACK_STRATEGY_UNSYMMETRIC)
		    std::cout << "    UMFPACK_STRATEGY_USED: UNSYMMETRIC\n";
		  else {
		    if (Info[UMFPACK_STRATEGY_USED] == UMFPACK_STRATEGY_2BY2)
		      std::cout << "    UMFPACK_STRATEGY_USED: 2BY2\n";
		    else
		      std::cout << "    UMFPACK_STRATEGY_USED: UNKOWN STRATEGY " << Info[UMFPACK_STRATEGY_USED] << "\n";
		  }
		}

		std::cout << "    UMFPACK_ORDERING_USED: " << Info[UMFPACK_ORDERING_USED] << "\n";
		std::cout << "    UMFPACK_QFIXED: " << Info[UMFPACK_QFIXED] << "\n";
		std::cout << "    UMFPACK_DIAG_PREFERRED: " << Info[UMFPACK_DIAG_PREFERRED] << "\n";
		std::cout << "    UMFPACK_ROW_SINGLETONS: " << Info[UMFPACK_ROW_SINGLETONS] << "\n";
		std::cout << "    UMFPACK_COL_SINGLETONS: " << Info[UMFPACK_COL_SINGLETONS] << "\n";
		std::cout << "    UMFPACK_PATTERN_SYMMETRY: " << Info[UMFPACK_PATTERN_SYMMETRY] << "\n";
		std::cout << "    UMFPACK_NZ_A_PLUS_AT: " << Info[UMFPACK_NZ_A_PLUS_AT] << "\n";
		std::cout << "    UMFPACK_NZDIAG: " << Info[UMFPACK_NZDIAG] << "\n";
		std::cout << "    UMFPACK_N2: " << Info[UMFPACK_N2] << "\n";
		std::cout << "    UMFPACK_S_SYMMETRIC: " << Info[UMFPACK_S_SYMMETRIC] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NROWS_ESTIMATE: " << Info[UMFPACK_MAX_FRONT_NROWS_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NCOLS_ESTIMATE: " << Info[UMFPACK_MAX_FRONT_NCOLS_ESTIMATE] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_LUNZ: " << Info[UMFPACK_SYMMETRIC_LUNZ] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_FLOPS: " << Info[UMFPACK_SYMMETRIC_FLOPS] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_NDENSE: " << Info[UMFPACK_SYMMETRIC_NDENSE] << "\n";
		std::cout << "    UMFPACK_SYMMETRIC_DMAX: " << Info[UMFPACK_SYMMETRIC_DMAX] << "\n";
#endif

		check(umfpack_di_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in di_numeric");

#if 0
		std::cout << "=== INFO of umfpack_*_numeric ===\n";
		std::cout << "    UMFPACK_STATUS: " << (Info[UMFPACK_STATUS] == UMFPACK_OK ? "OK" : "ERROR") << "\n";
		std::cout << "    UMFPACK_VARIABLE_PEAK: " << Info[UMFPACK_VARIABLE_PEAK]  << "\n";
		std::cout << "    UMFPACK_PEAK_MEMORY: " << Info[UMFPACK_PEAK_MEMORY] << "\n";
		std::cout << "    UMFPACK_FLOPS: " << Info[UMFPACK_FLOPS] << "\n";
		std::cout << "    UMFPACK_LNZ: " << Info[UMFPACK_LNZ] << "\n";
		std::cout << "    UMFPACK_UNZ: " << Info[UMFPACK_UNZ] << "\n";
		std::cout << "    UMFPACK_NUMERIC_DEFRAG: " << Info[UMFPACK_NUMERIC_DEFRAG] << "\n";
		std::cout << "    UMFPACK_NUMERIC_REALLOC: " << Info[UMFPACK_NUMERIC_REALLOC] << "\n";
		std::cout << "    UMFPACK_NUMERIC_COSTLY_REALLOC: " << Info[UMFPACK_NUMERIC_COSTLY_REALLOC] << "\n";
		std::cout << "    UMFPACK_COMPRESSED_PATTERN: " << Info[UMFPACK_COMPRESSED_PATTERN] << "\n";
		std::cout << "    UMFPACK_LU_ENTRIES: " << Info[UMFPACK_LU_ENTRIES] << "\n";
		std::cout << "    UMFPACK_NUMERIC_TIME: " << Info[UMFPACK_NUMERIC_TIME] << "\n";
		std::cout << "    UMFPACK_RCOND: " << Info[UMFPACK_RCOND] << "\n";
		std::cout << "    UMFPACK_UDIAG_NZ: " << Info[UMFPACK_UDIAG_NZ] << "\n";
		std::cout << "    UMFPACK_UMIN: " << Info[UMFPACK_UMIN] << "\n";
		std::cout << "    UMFPACK_UMAX: " << Info[UMFPACK_UMAX] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NROWS: " << Info[UMFPACK_MAX_FRONT_NROWS] << "\n";
		std::cout << "    UMFPACK_MAX_FRONT_NCOLS: " << Info[UMFPACK_MAX_FRONT_NCOLS] << "\n";
		std::cout << "    UMFPACK_ALL_LNZ: " << Info[UMFPACK_ALL_LNZ] << "\n";
		std::cout << "    UMFPACK_ALL_UNZ: " << Info[UMFPACK_ALL_UNZ] << "\n";
#endif
	    }

	    void init()
	    {
		MTL_THROW_IF(num_rows(A) != num_cols(A), matrix_not_square());
		n= num_rows(A);
		assign_pointers();
		init_aux(blong());
	    }

	  public:
	    /// Constructor referring to matrix \p A (not changed) and optionally Umfpack's strategy and alloc_init
	    solver(const matrix_type& A, int strategy = UMFPACK_STRATEGY_AUTO, double alloc_init = 0.7)
	      : A(A), Apc(0), Aic(0), my_nnz(0), Symbolic(0), Numeric(0)
	    {
		vampir_trace<5060> trace;
		// Use default setings.
		if (long_indices)
		    umfpack_dl_defaults(Control);
		else
		    umfpack_di_defaults(Control);

		Control[UMFPACK_STRATEGY] = strategy;
		Control[UMFPACK_ALLOC_INIT] = alloc_init;
		init();
	    }

	    ~solver()
	    {
		vampir_trace<5061> trace;
		if (long_indices) {
		    umfpack_dl_free_numeric(&Numeric);
		    umfpack_dl_free_symbolic(&Symbolic);
		} else {
		    umfpack_di_free_numeric(&Numeric);
		    umfpack_di_free_symbolic(&Symbolic);
		}
		if (Apc) delete[] Apc;
		if (Aic) delete[] Aic;
	    }

	    void update_numeric_aux(true_)
	    {
		umfpack_dl_free_numeric(&Numeric);
		check(umfpack_dl_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in dl_numeric");
	    }

	    void update_numeric_aux(false_)
	    {
		umfpack_di_free_numeric(&Numeric);
		check(umfpack_di_numeric(Ap, Ai, Ax, Symbolic, &Numeric, Control, Info), "Error in di_numeric");
	    }

	    /// Update numeric part, for matrices that kept the sparsity and changed the values
	    void update_numeric()
	    {
		assign_pointers();
		update_numeric_aux(blong());
	    }

	    /// Update symbolic and numeric part
	    void update()
	    {
		if (long_indices) {
		    umfpack_dl_free_numeric(&Numeric);
		    umfpack_dl_free_symbolic(&Symbolic);
		} else {
		    umfpack_di_free_numeric(&Numeric);
		    umfpack_di_free_symbolic(&Symbolic);
		}
		init();
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& xx, const VectorB& bb, true_)
	    {
	      check(umfpack_dl_solve(sys, Ap, Ai, Ax, &xx.value[0], &bb.value[0], Numeric, Control, Info), "Error in dl_solve");
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& xx, const VectorB& bb, false_)
	    {
		check(umfpack_di_solve(sys, Ap, Ai, Ax, &xx.value[0], &bb.value[0], Numeric, Control, Info), "Error in di_solve");
	    }

	    /// Solve double system
	    template <typename VectorX, typename VectorB>
	    int operator()(VectorX& x, const VectorB& b)
	    {
		vampir_trace<5062> trace;
		MTL_THROW_IF(num_rows(A) != size(x) || num_rows(A) != size(b), incompatible_size());
		make_in_out_copy_or_reference<dense_vector<value_type>, VectorX> xx(x);
		make_in_copy_or_reference<dense_vector<value_type>, VectorB>     bb(b);
		int sys= mtl::traits::is_row_major<Parameters>::value ? UMFPACK_At : UMFPACK_A;
		solve_aux(sys, xx, bb, blong());
		return UMFPACK_OK;
	    }

	    /// Solve double system
	    template <typename VectorB, typename VectorX>
	    int solve(const VectorB& b, VectorX& x) const
	    {
		// return (*this)(x, b);
		return const_cast<solver&>(*this)(x, b); // evil hack because Umfpack has no const
	    }

	  private:
	    const matrix_type&  A;
	    int                 n;
	    const index_type    *Ap, *Ai;
	    index_type          *Apc, *Aic;
	    size_type           my_nnz;
	    const double        *Ax;
	    double              Control[UMFPACK_CONTROL], Info[UMFPACK_INFO];
	    void                *Symbolic, *Numeric;
	};

	/// Speciatization of solver for \ref matrix::compressed2D with double values
	template <typename Parameters>
	class solver<compressed2D<std::complex<double>, Parameters> >
	{
	    typedef std::complex<double>                      value_type;
	    typedef compressed2D<value_type, Parameters>      matrix_type;
	    typedef typename matrix_type::size_type           size_type;
	    typedef typename index<size_type>::type           index_type;

	    static const bool copy_indices= sizeof(index_type) != sizeof(size_type),
		              long_indices= use_long<size_type>::value;

	    typedef boost::mpl::bool_<long_indices>           blong;
	    typedef boost::mpl::true_                         true_;
	    typedef boost::mpl::false_                        false_;

	    void assign_pointers()
	    {
		if (copy_indices) {
		    if (Apc == 0) Apc= new index_type[n + 1];
		    if (Aic == 0) Aic= new index_type[A.nnz()];
		    std::copy(A.address_major(), A.address_major() + n + 1, Apc);
		    std::copy(A.address_minor(), A.address_minor() + A.nnz(), Aic);
		    Ap= Apc;
		    Ai= Aic;
		} else {
		    Ap= reinterpret_cast<const index_type*>(A.address_major());
		    Ai= reinterpret_cast<const index_type*>(A.address_minor());
		}
		split_complex_vector(A.data, Ax, Az);
	    }

	    void init_aux(true_)
	    {
		check(umfpack_zl_symbolic(n, n, Ap, Ai, &Ax[0], &Az[0], &Symbolic, Control, Info), "Error in zl_symbolic");
		check(umfpack_zl_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in zl_numeric");
	    }

	    void init_aux(false_)
	    {
		check(umfpack_zi_symbolic(n, n, Ap, Ai, &Ax[0], &Az[0], &Symbolic, Control, Info), "Error in zi_symbolic");
		check(umfpack_zi_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in zi_numeric");
	    }

	    void initialize()
	    {
		MTL_THROW_IF(num_rows(A) != num_cols(A), matrix_not_square());
		n= num_rows(A);
		assign_pointers();
		init_aux(blong());
	    }
	public:
	    /// Constructor referring to matrix \p A (not changed) and optionally Umfpack's strategy and alloc_init (look for the specializations)
	    explicit solver(const compressed2D<value_type, Parameters>& A, int strategy = UMFPACK_STRATEGY_AUTO, double alloc_init = 0.7)
	      : A(A), Apc(0), Aic(0)
	    {
		vampir_trace<5060> trace;
		// Use default setings.
		if (long_indices)
		    umfpack_zl_defaults(Control);
		else
		    umfpack_zi_defaults(Control);
		// umfpack_zi_defaults(Control);

		Control[UMFPACK_STRATEGY] = strategy;
		Control[UMFPACK_ALLOC_INIT] = alloc_init;
		initialize();
	    }

	    ~solver()
	    {
		vampir_trace<5061> trace;
		if (long_indices) {
		    umfpack_zl_free_numeric(&Numeric);
		    umfpack_zl_free_symbolic(&Symbolic);
		} else {
		    umfpack_zi_free_numeric(&Numeric);
		    umfpack_zi_free_symbolic(&Symbolic);
		}
		if (Apc) delete[] Apc;
		if (Aic) delete[] Aic;
	    }

	    void update_numeric_aux(true_)
	    {
		umfpack_zl_free_numeric(&Numeric);
		check(umfpack_zl_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in dl_numeric D");
	    }

	    void update_numeric_aux(false_)
	    {
		umfpack_zi_free_numeric(&Numeric);
		check(umfpack_zi_numeric(Ap, Ai, &Ax[0], &Az[0], Symbolic, &Numeric, Control, Info), "Error in di_numeric");
	    }

	    /// Update numeric part, for matrices that kept the sparsity and changed the values
	    void update_numeric()
	    {
		assign_pointers();
		update_numeric_aux(blong());
	    }

	    /// Update symbolic and numeric part
	    void update()
	    {
		Ax.change_dim(0); Az.change_dim(0);
		if (long_indices) {
		    umfpack_zl_free_numeric(&Numeric);
		    umfpack_zl_free_symbolic(&Symbolic);
		} else {
		    umfpack_zi_free_numeric(&Numeric);
		    umfpack_zi_free_symbolic(&Symbolic);
		}
		initialize();
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& Xx, VectorX& Xz, const VectorB& Bx, const VectorB& Bz, true_)
	    {
		check(umfpack_zl_solve(sys, Ap, Ai, &Ax[0], &Az[0], &Xx[0], &Xz[0], &Bx[0], &Bz[0], Numeric, Control, Info),
		      "Error in zi_solve");
	    }

	    template <typename VectorX, typename VectorB>
	    void solve_aux(int sys, VectorX& Xx, VectorX& Xz, const VectorB& Bx, const VectorB& Bz, false_)
	    {
		check(umfpack_zi_solve(sys, Ap, Ai, &Ax[0], &Az[0], &Xx[0], &Xz[0], &Bx[0], &Bz[0], Numeric, Control, Info),
		      "Error in zi_solve");
	    }

	    /// Solve complex system
	    template <typename VectorX, typename VectorB>
	    int operator()(VectorX& x, const VectorB& b)
	    {
		vampir_trace<5062> trace;
		MTL_THROW_IF(num_rows(A) != size(x) || num_rows(A) != size(b), incompatible_size());
		dense_vector<double> Xx(size(x)), Xz(size(x)), Bx, Bz;
		split_complex_vector(b, Bx, Bz);
		int sys= mtl::traits::is_row_major<Parameters>::value ? UMFPACK_Aat : UMFPACK_A;
		solve_aux(sys, Xx, Xz, Bx, Bz, blong());
		merge_complex_vector(Xx, Xz, x);
		return UMFPACK_OK;
	    }

	    /// Solve complex system
	    template <typename VectorB, typename VectorX>
	    int solve(const VectorB& b, VectorX& x)
	    {
		return (*this)(x, b);
	    }

	private:
	    const matrix_type&   A;
	    int                  n;
	    const index_type     *Ap, *Ai;
	    index_type          *Apc, *Aic;
	    dense_vector<double> Ax, Az;
	    double               Control[UMFPACK_CONTROL], Info[UMFPACK_INFO];
	    void                 *Symbolic, *Numeric;
	};

	template <typename Value, typename Parameters>
	class solver<compressed2D<Value, Parameters> >
	  : matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation>,
	    public solver<typename matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation>::matrix_type >
	{
	    typedef matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation> copy_type;
	    typedef solver<typename matrix_copy<compressed2D<Value, Parameters>, Value, typename Parameters::orientation>::matrix_type > solver_type;
	public:
	    explicit solver(const compressed2D<Value, Parameters>& A)
		: copy_type(A), solver_type(copy_type::matrix), A(A)
	    {}

	    void update()
	    {
		copy_type::matrix= A;
		solver_type::update();
	    }

	    void update_numeric()
	    {
		copy_type::matrix= A;
		solver_type::update_numeric();
	    }
	private:
	    const compressed2D<Value, Parameters>& A;
	};
    } // umfpack

/// Solve A*x == b with umfpack
/** Only available when compiled with enabled macro MTL_HAS_UMFPACK.
    Uses classes umfpack::solver internally.
    If you want more control on single operations or to keep umfpack's
    internal factorization, use this class.
 **/
template <typename Value, typename Parameters, typename VectorX, typename VectorB>
int umfpack_solve(const compressed2D<Value, Parameters>& A, VectorX& x, const VectorB& b)
{
    umfpack::solver<compressed2D<Value, Parameters> > solver(A);
    return solver(x, b);
}

}} // namespace mtl::matrix

#endif

#endif // MTL_MATRIX_UMFPACK_SOLVE_INCLUDE