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

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

// This file was modified by Oracle on 2014-2020.
// Modifications copyright (c) 2014-2020 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// 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_ALGORITHMS_DETAIL_OVERLAY_INTERSECTION_INSERT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_INTERSECTION_INSERT_HPP

#include <cstddef>
#include <deque>
#include <type_traits>

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/size.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/detail/check_iterator_range.hpp>
#include <boost/geometry/algorithms/detail/point_on_border.hpp>
#include <boost/geometry/algorithms/detail/overlay/clip_linestring.hpp>
#include <boost/geometry/algorithms/detail/overlay/follow.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_intersection_points.hpp>
#include <boost/geometry/algorithms/detail/overlay/linear_linear.hpp>
#include <boost/geometry/algorithms/detail/overlay/overlay.hpp>
#include <boost/geometry/algorithms/detail/overlay/overlay_type.hpp>
#include <boost/geometry/algorithms/detail/overlay/pointlike_areal.hpp>
#include <boost/geometry/algorithms/detail/overlay/pointlike_linear.hpp>
#include <boost/geometry/algorithms/detail/overlay/pointlike_pointlike.hpp>
#include <boost/geometry/algorithms/detail/overlay/range_in_geometry.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_as_subrange.hpp>

#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>
#include <boost/geometry/core/static_assert.hpp>

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

#include <boost/geometry/policies/robustness/rescale_policy_tags.hpp>
#include <boost/geometry/policies/robustness/segment_ratio_type.hpp>
#include <boost/geometry/policies/robustness/get_rescale_policy.hpp>

#include <boost/geometry/strategies/default_strategy.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/relate/services.hpp>

#include <boost/geometry/views/segment_view.hpp>
#include <boost/geometry/views/detail/boundary_view.hpp>

#if defined(BOOST_GEOMETRY_DEBUG_FOLLOW)
#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#endif

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template <typename PointOut>
struct intersection_segment_segment_point
{
    template
    <
        typename Segment1, typename Segment2,
        typename RobustPolicy,
        typename OutputIterator, typename Strategy
    >
    static inline OutputIterator apply(Segment1 const& segment1,
            Segment2 const& segment2,
            RobustPolicy const& ,
            OutputIterator out,
            Strategy const& strategy)
    {
        // Make sure this is only called with no rescaling
        BOOST_STATIC_ASSERT((std::is_same
           <
               no_rescale_policy_tag,
               typename rescale_policy_type<RobustPolicy>::type
           >::value));

        typedef typename point_type<PointOut>::type point_type;

        // Get the intersection point (or two points)
        typedef segment_intersection_points<point_type> intersection_return_type;

        typedef policies::relate::segments_intersection_points
            <
                intersection_return_type
            > policy_type;

        detail::segment_as_subrange<Segment1> sub_range1(segment1);
        detail::segment_as_subrange<Segment2> sub_range2(segment2);

        intersection_return_type
            is = strategy.relate().apply(sub_range1, sub_range2, policy_type());

        for (std::size_t i = 0; i < is.count; i++)
        {
            PointOut p;
            geometry::convert(is.intersections[i], p);
            *out++ = p;
        }
        return out;
    }
};

template <typename PointOut>
struct intersection_linestring_linestring_point
{
    template
    <
        typename Linestring1, typename Linestring2,
        typename RobustPolicy,
        typename OutputIterator,
        typename Strategy
    >
    static inline OutputIterator apply(Linestring1 const& linestring1,
            Linestring2 const& linestring2,
            RobustPolicy const& robust_policy,
            OutputIterator out,
            Strategy const& strategy)
    {
        // Make sure this is only called with no rescaling
        BOOST_STATIC_ASSERT((std::is_same
           <
               no_rescale_policy_tag,
               typename rescale_policy_type<RobustPolicy>::type
           >::value));

        typedef detail::overlay::turn_info<PointOut> turn_info;
        std::deque<turn_info> turns;

        geometry::get_intersection_points(linestring1, linestring2,
                                          robust_policy, turns, strategy);

        for (typename boost::range_iterator<std::deque<turn_info> const>::type
            it = boost::begin(turns); it != boost::end(turns); ++it)
        {
            PointOut p;
            geometry::convert(it->point, p);
            *out++ = p;
        }
        return out;
    }
};

/*!
\brief Version of linestring with an areal feature (polygon or multipolygon)
*/
template
<
    bool ReverseAreal,
    typename GeometryOut,
    overlay_type OverlayType,
    bool FollowIsolatedPoints
>
struct intersection_of_linestring_with_areal
{
#if defined(BOOST_GEOMETRY_DEBUG_FOLLOW)
    template <typename Turn, typename Operation>
    static inline void debug_follow(Turn const& turn, Operation op,
                                    int index)
    {
        std::cout << index
                  << " at " << op.seg_id
                  << " meth: " << method_char(turn.method)
                  << " op: " << operation_char(op.operation)
                  << " vis: " << visited_char(op.visited)
                  << " of:  " << operation_char(turn.operations[0].operation)
                  << operation_char(turn.operations[1].operation)
                  << " " << geometry::wkt(turn.point)
                  << std::endl;
    }

    template <typename Turn>
    static inline void debug_turn(Turn const& t, bool non_crossing)
    {
        std::cout << "checking turn @"
                  << geometry::wkt(t.point)
                  << "; " << method_char(t.method)
                  << ":" << operation_char(t.operations[0].operation)
                  << "/" << operation_char(t.operations[1].operation)
                  << "; non-crossing? "
                  << std::boolalpha << non_crossing << std::noboolalpha
                  << std::endl;
    }
#endif

    template <typename Linestring, typename Areal, typename Strategy, typename Turns>
    static inline bool simple_turns_analysis(Linestring const& linestring,
                                             Areal const& areal,
                                             Strategy const& strategy,
                                             Turns const& turns,
                                             int & inside_value)
    {
        using namespace overlay;

        bool found_continue = false;
        bool found_intersection = false;
        bool found_union = false;
        bool found_front = false;

        for (typename Turns::const_iterator it = turns.begin();
                it != turns.end(); ++it)
        {
            method_type const method = it->method;
            operation_type const op = it->operations[0].operation;

            if (method == method_crosses)
            {
                return false;
            }
            else if (op == operation_intersection)
            {
                found_intersection = true;
            }
            else if (op == operation_union)
            {
                found_union = true;
            }
            else if (op == operation_continue)
            {
                found_continue = true;
            }

            if ((found_intersection || found_continue) && found_union)
            {
                return false;
            }

            if (it->operations[0].position == position_front)
            {
                found_front = true;
            }
        }

        if (found_front)
        {
            if (found_intersection)
            {
                inside_value = 1; // inside
            }
            else if (found_union)
            {
                inside_value = -1; // outside
            }
            else // continue and blocked
            {
                inside_value = 0;
            }
            return true;
        }

        // if needed analyse points of a linestring
        // NOTE: range_in_geometry checks points of a linestring
        // until a point inside/outside areal is found
        // TODO: Could be replaced with point_in_geometry() because found_front is false
        inside_value = range_in_geometry(linestring, areal, strategy);

        if ( (found_intersection && inside_value == -1) // going in from outside
          || (found_continue && inside_value == -1) // going on boundary from outside
          || (found_union && inside_value == 1) ) // going out from inside
        {
            return false;
        }

        return true;
    }

    template
    <
        typename LineString, typename Areal,
        typename RobustPolicy,
        typename OutputIterator, typename Strategy
    >
    static inline OutputIterator apply(LineString const& linestring, Areal const& areal,
            RobustPolicy const& robust_policy,
            OutputIterator out,
            Strategy const& strategy)
    {
        // Make sure this is only called with no rescaling
        BOOST_STATIC_ASSERT((std::is_same
           <
               no_rescale_policy_tag,
               typename rescale_policy_type<RobustPolicy>::type
           >::value));

        if (boost::size(linestring) == 0)
        {
            return out;
        }

        typedef detail::overlay::follow
                <
                    GeometryOut,
                    LineString,
                    Areal,
                    OverlayType,
                    false, // do not remove spikes for linear geometries
                    FollowIsolatedPoints
                > follower;

        typedef typename geometry::detail::output_geometry_access
            <
                GeometryOut, linestring_tag, linestring_tag
            > linear;

        typedef typename point_type
            <
                typename linear::type
            >::type point_type;

        typedef geometry::segment_ratio
            <
                typename coordinate_type<point_type>::type
            > ratio_type;

        typedef detail::overlay::turn_info
            <
                point_type,
                ratio_type,
                detail::overlay::turn_operation_linear
                    <
                        point_type,
                        ratio_type
                    >
            > turn_info;

        std::deque<turn_info> turns;

        detail::get_turns::no_interrupt_policy policy;

        typedef detail::overlay::get_turn_info_linear_areal
            <
                detail::overlay::assign_null_policy
            > turn_policy;

        dispatch::get_turns
            <
                typename geometry::tag<LineString>::type,
                typename geometry::tag<Areal>::type,
                LineString,
                Areal,
                false,
                (OverlayType == overlay_intersection ? ReverseAreal : !ReverseAreal),
                turn_policy
            >::apply(0, linestring, 1, areal,
                     strategy, robust_policy,
                     turns, policy);

        int inside_value = 0;
        if (simple_turns_analysis(linestring, areal, strategy, turns, inside_value))
        {
            // No crossing the boundary, it is either
            // inside (interior + borders)
            // or outside (exterior + borders)
            // or on boundary

            // add linestring to the output if conditions are met
            if (follower::included(inside_value))
            {
                typename linear::type copy;
                geometry::convert(linestring, copy);
                *linear::get(out)++ = copy;
            }

            return out;
        }

#if defined(BOOST_GEOMETRY_DEBUG_FOLLOW)
        int index = 0;
        for(typename std::deque<turn_info>::const_iterator
            it = turns.begin(); it != turns.end(); ++it)
        {
            debug_follow(*it, it->operations[0], index++);
        }
#endif

        return follower::apply
                (
                    linestring, areal,
                    geometry::detail::overlay::operation_intersection,
                    turns, robust_policy, out, strategy
                );
    }
};

template <typename Turns, typename OutputIterator>
inline OutputIterator intersection_output_turn_points(Turns const& turns,
                                                      OutputIterator out)
{
    for (typename Turns::const_iterator
            it = turns.begin(); it != turns.end(); ++it)
    {
        *out++ = it->point;
    }

    return out;
}

template <typename PointOut>
struct intersection_areal_areal_point
{
    template
    <
        typename Geometry1, typename Geometry2,
        typename RobustPolicy,
        typename OutputIterator,
        typename Strategy
    >
    static inline OutputIterator apply(Geometry1 const& geometry1,
                                       Geometry2 const& geometry2,
                                       RobustPolicy const& robust_policy,
                                       OutputIterator out,
                                       Strategy const& strategy)
    {
        typedef detail::overlay::turn_info
            <
                PointOut,
                typename segment_ratio_type<PointOut, RobustPolicy>::type
            > turn_info;
        std::vector<turn_info> turns;

        detail::get_turns::no_interrupt_policy policy;

        geometry::get_turns
            <
                false, false, detail::overlay::assign_null_policy
            >(geometry1, geometry2, strategy, robust_policy, turns, policy);

        return intersection_output_turn_points(turns, out);
    }
};

template <typename PointOut>
struct intersection_linear_areal_point
{
    template
    <
        typename Geometry1, typename Geometry2,
        typename RobustPolicy,
        typename OutputIterator,
        typename Strategy
    >
    static inline OutputIterator apply(Geometry1 const& geometry1,
                                       Geometry2 const& geometry2,
                                       RobustPolicy const& robust_policy,
                                       OutputIterator out,
                                       Strategy const& strategy)
    {
        // Make sure this is only called with no rescaling
        BOOST_STATIC_ASSERT((std::is_same
           <
               no_rescale_policy_tag,
               typename rescale_policy_type<RobustPolicy>::type
           >::value));

        typedef geometry::segment_ratio<typename geometry::coordinate_type<PointOut>::type> ratio_type;

        typedef detail::overlay::turn_info
            <
                PointOut,
                ratio_type,
                detail::overlay::turn_operation_linear
                    <
                        PointOut,
                        ratio_type
                    >
            > turn_info;

        typedef detail::overlay::get_turn_info_linear_areal
            <
                detail::overlay::assign_null_policy
            > turn_policy;

        std::vector<turn_info> turns;

        detail::get_turns::no_interrupt_policy interrupt_policy;

        dispatch::get_turns
            <
                typename geometry::tag<Geometry1>::type,
                typename geometry::tag<Geometry2>::type,
                Geometry1,
                Geometry2,
                false,
                false,
                turn_policy
            >::apply(0, geometry1, 1, geometry2,
                     strategy, robust_policy,
                     turns, interrupt_policy);

        return intersection_output_turn_points(turns, out);
    }
};

template <typename PointOut>
struct intersection_areal_linear_point
{
    template
    <
        typename Geometry1, typename Geometry2,
        typename RobustPolicy,
        typename OutputIterator,
        typename Strategy
    >
    static inline OutputIterator apply(Geometry1 const& geometry1,
                                       Geometry2 const& geometry2,
                                       RobustPolicy const& robust_policy,
                                       OutputIterator out,
                                       Strategy const& strategy)
    {
        return intersection_linear_areal_point
            <
                PointOut
            >::apply(geometry2, geometry1, robust_policy, out, strategy);
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    // real types
    typename Geometry1,
    typename Geometry2,
    typename GeometryOut,
    overlay_type OverlayType,
    // orientation
    bool Reverse1 = detail::overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
    bool Reverse2 = detail::overlay::do_reverse<geometry::point_order<Geometry2>::value>::value,
    // tag dispatching:
    typename TagIn1 = typename geometry::tag<Geometry1>::type,
    typename TagIn2 = typename geometry::tag<Geometry2>::type,
    typename TagOut = typename detail::setop_insert_output_tag<GeometryOut>::type,
    // metafunction finetuning helpers:
    typename CastedTagIn1 = typename geometry::tag_cast<TagIn1, areal_tag, linear_tag, pointlike_tag>::type,
    typename CastedTagIn2 = typename geometry::tag_cast<TagIn2, areal_tag, linear_tag, pointlike_tag>::type,
    typename CastedTagOut = typename geometry::tag_cast<TagOut, areal_tag, linear_tag, pointlike_tag>::type
>
struct intersection_insert
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not or not yet implemented for these Geometry types or their order.",
        Geometry1, Geometry2, GeometryOut,
        std::integral_constant<overlay_type, OverlayType>);
};

template
<
    typename Geometry1, typename Geometry2,
    typename GeometryOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn1, typename TagIn2, typename TagOut
>
struct intersection_insert
    <
        Geometry1, Geometry2,
        GeometryOut,
        OverlayType,
        Reverse1, Reverse2,
        TagIn1, TagIn2, TagOut,
        areal_tag, areal_tag, areal_tag
    > : detail::overlay::overlay
        <
            Geometry1, Geometry2, Reverse1, Reverse2,
            detail::overlay::do_reverse<geometry::point_order<GeometryOut>::value>::value,
            GeometryOut, OverlayType
        >
{};

// Any areal type with box:
template
<
    typename Geometry, typename Box,
    typename GeometryOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn, typename TagOut
>
struct intersection_insert
    <
        Geometry, Box,
        GeometryOut,
        OverlayType,
        Reverse1, Reverse2,
        TagIn, box_tag, TagOut,
        areal_tag, areal_tag, areal_tag
    > : detail::overlay::overlay
        <
            Geometry, Box, Reverse1, Reverse2,
            detail::overlay::do_reverse<geometry::point_order<GeometryOut>::value>::value,
            GeometryOut, OverlayType
        >
{};

template
<
    typename Segment1, typename Segment2,
    typename GeometryOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Segment1, Segment2,
        GeometryOut,
        OverlayType,
        Reverse1, Reverse2,
        segment_tag, segment_tag, point_tag,
        linear_tag, linear_tag, pointlike_tag
    > : detail::intersection::intersection_segment_segment_point<GeometryOut>
{};

template
<
    typename Linestring1, typename Linestring2,
    typename GeometryOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Linestring1, Linestring2,
        GeometryOut,
        OverlayType,
        Reverse1, Reverse2,
        linestring_tag, linestring_tag, point_tag,
        linear_tag, linear_tag, pointlike_tag
    > : detail::intersection::intersection_linestring_linestring_point<GeometryOut>
{};

template
<
    typename Linestring, typename Box,
    typename GeometryOut,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Linestring, Box,
        GeometryOut,
        overlay_intersection,
        Reverse1, Reverse2,
        linestring_tag, box_tag, linestring_tag,
        linear_tag, areal_tag, linear_tag
    >
{
    template <typename RobustPolicy, typename OutputIterator, typename Strategy>
    static inline OutputIterator apply(Linestring const& linestring,
            Box const& box,
            RobustPolicy const& robust_policy,
            OutputIterator out, Strategy const& )
    {
        typedef typename point_type<GeometryOut>::type point_type;
        strategy::intersection::liang_barsky<Box, point_type> lb_strategy;
        return detail::intersection::clip_range_with_box
            <GeometryOut>(box, linestring, robust_policy, out, lb_strategy);
    }
};

template
<
    typename Linestring, typename Polygon,
    typename GeometryOut,
    overlay_type OverlayType,
    bool ReverseLinestring, bool ReversePolygon
>
struct intersection_insert
    <
        Linestring, Polygon,
        GeometryOut,
        OverlayType,
        ReverseLinestring, ReversePolygon,
        linestring_tag, polygon_tag, linestring_tag,
        linear_tag, areal_tag, linear_tag
    > : detail::intersection::intersection_of_linestring_with_areal
            <
                ReversePolygon,
                GeometryOut,
                OverlayType,
                false
            >
{};

template
<
    typename Linestring, typename Ring,
    typename GeometryOut,
    overlay_type OverlayType,
    bool ReverseLinestring, bool ReverseRing
>
struct intersection_insert
    <
        Linestring, Ring,
        GeometryOut,
        OverlayType,
        ReverseLinestring, ReverseRing,
        linestring_tag, ring_tag, linestring_tag,
        linear_tag, areal_tag, linear_tag
    > : detail::intersection::intersection_of_linestring_with_areal
            <
                ReverseRing,
                GeometryOut,
                OverlayType,
                false
            >
{};

template
<
    typename Segment, typename Box,
    typename GeometryOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Segment, Box,
        GeometryOut,
        OverlayType,
        Reverse1, Reverse2,
        segment_tag, box_tag, linestring_tag,
        linear_tag, areal_tag, linear_tag
    >
{
    template <typename RobustPolicy, typename OutputIterator, typename Strategy>
    static inline OutputIterator apply(Segment const& segment,
            Box const& box,
            RobustPolicy const& robust_policy,
            OutputIterator out, Strategy const& )
    {
        geometry::segment_view<Segment> range(segment);

        typedef typename point_type<GeometryOut>::type point_type;
        strategy::intersection::liang_barsky<Box, point_type> lb_strategy;
        return detail::intersection::clip_range_with_box
            <GeometryOut>(box, range, robust_policy, out, lb_strategy);
    }
};

template
<
    typename Geometry1, typename Geometry2,
    typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename Tag1, typename Tag2
>
struct intersection_insert
    <
        Geometry1, Geometry2,
        PointOut,
        OverlayType,
        Reverse1, Reverse2,
        Tag1, Tag2, point_tag,
        areal_tag, areal_tag, pointlike_tag
    >
    : public detail::intersection::intersection_areal_areal_point
        <
            PointOut
        >
{};

template
<
    typename Geometry1, typename Geometry2,
    typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename Tag1, typename Tag2
>
struct intersection_insert
    <
        Geometry1, Geometry2,
        PointOut,
        OverlayType,
        Reverse1, Reverse2,
        Tag1, Tag2, point_tag,
        linear_tag, areal_tag, pointlike_tag
    >
    : public detail::intersection::intersection_linear_areal_point
        <
            PointOut
        >
{};

template
<
    typename Geometry1, typename Geometry2,
    typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename Tag1, typename Tag2
>
struct intersection_insert
    <
        Geometry1, Geometry2,
        PointOut,
        OverlayType,
        Reverse1, Reverse2,
        Tag1, Tag2, point_tag,
        areal_tag, linear_tag, pointlike_tag
    >
    : public detail::intersection::intersection_areal_linear_point
        <
            PointOut
        >
{};

template
<
    typename Geometry1, typename Geometry2, typename GeometryOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert_reversed
{
    template <typename RobustPolicy, typename OutputIterator, typename Strategy>
    static inline OutputIterator apply(Geometry1 const& g1,
                Geometry2 const& g2,
                RobustPolicy const& robust_policy,
                OutputIterator out,
                Strategy const& strategy)
    {
        return intersection_insert
            <
                Geometry2, Geometry1, GeometryOut,
                OverlayType,
                Reverse2, Reverse1
            >::apply(g2, g1, robust_policy, out, strategy);
    }
};

// dispatch for intersection(areal, areal, linear)
template
<
    typename Geometry1, typename Geometry2,
    typename LinestringOut,
    bool Reverse1, bool Reverse2,
    typename Tag1, typename Tag2
>
struct intersection_insert
    <
        Geometry1, Geometry2,
        LinestringOut,
        overlay_intersection,
        Reverse1, Reverse2,
        Tag1, Tag2, linestring_tag,
        areal_tag, areal_tag, linear_tag
    >
{
    template
    <
        typename RobustPolicy, typename OutputIterator, typename Strategy
    >
    static inline OutputIterator apply(Geometry1 const& geometry1,
                                       Geometry2 const& geometry2,
                                       RobustPolicy const& robust_policy,
                                       OutputIterator oit,
                                       Strategy const& strategy)
    {
        detail::boundary_view<Geometry1 const> view1(geometry1);
        detail::boundary_view<Geometry2 const> view2(geometry2);

        return detail::overlay::linear_linear_linestring
            <
                detail::boundary_view<Geometry1 const>,
                detail::boundary_view<Geometry2 const>,
                LinestringOut,
                overlay_intersection
            >::apply(view1, view2, robust_policy, oit, strategy);
    }
};

// dispatch for difference/intersection of linear geometries
template
<
    typename Linear1, typename Linear2, typename LineStringOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn1, typename TagIn2
>
struct intersection_insert
    <
        Linear1, Linear2, LineStringOut, OverlayType,
        Reverse1, Reverse2,
        TagIn1, TagIn2, linestring_tag,
        linear_tag, linear_tag, linear_tag
    > : detail::overlay::linear_linear_linestring
        <
            Linear1, Linear2, LineStringOut, OverlayType
        >
{};

template
<
    typename Linear1, typename Linear2, typename TupledOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn1, typename TagIn2
>
struct intersection_insert
    <
        Linear1, Linear2, TupledOut, OverlayType,
        Reverse1, Reverse2,
        TagIn1, TagIn2, detail::tupled_output_tag,
        linear_tag, linear_tag, detail::tupled_output_tag
    >
    : detail::expect_output
        <
            Linear1, Linear2, TupledOut,
            // NOTE: points can be the result only in case of intersection.
            std::conditional_t
                <
                    (OverlayType == overlay_intersection),
                    point_tag,
                    void
                >,
            linestring_tag
        >
{
    // NOTE: The order of geometries in TupledOut tuple/pair must correspond to the order
    // iterators in OutputIterators tuple/pair.
    template
    <
        typename RobustPolicy, typename OutputIterators, typename Strategy
    >
    static inline OutputIterators apply(Linear1 const& linear1,
                                        Linear2 const& linear2,
                                        RobustPolicy const& robust_policy,
                                        OutputIterators oit,
                                        Strategy const& strategy)
    {
        return detail::overlay::linear_linear_linestring
            <
                Linear1, Linear2, TupledOut, OverlayType
            >::apply(linear1, linear2, robust_policy, oit, strategy);
    }
};

// dispatch for difference/intersection of point-like geometries

template
<
    typename Point1, typename Point2, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Point1, Point2, PointOut, OverlayType,
        Reverse1, Reverse2,
        point_tag, point_tag, point_tag,
        pointlike_tag, pointlike_tag, pointlike_tag
    > : detail::overlay::point_point_point
        <
            Point1, Point2, PointOut, OverlayType
        >
{};

template
<
    typename MultiPoint, typename Point, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        MultiPoint, Point, PointOut, OverlayType,
        Reverse1, Reverse2,
        multi_point_tag, point_tag, point_tag,
        pointlike_tag, pointlike_tag, pointlike_tag
    > : detail::overlay::multipoint_point_point
        <
            MultiPoint, Point, PointOut, OverlayType
        >
{};

template
<
    typename Point, typename MultiPoint, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Point, MultiPoint, PointOut, OverlayType,
        Reverse1, Reverse2,
        point_tag, multi_point_tag, point_tag,
        pointlike_tag, pointlike_tag, pointlike_tag
    > : detail::overlay::point_multipoint_point
        <
            Point, MultiPoint, PointOut, OverlayType
        >
{};

template
<
    typename MultiPoint1, typename MultiPoint2, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        MultiPoint1, MultiPoint2, PointOut, OverlayType,
        Reverse1, Reverse2,
        multi_point_tag, multi_point_tag, point_tag,
        pointlike_tag, pointlike_tag, pointlike_tag
    > : detail::overlay::multipoint_multipoint_point
        <
            MultiPoint1, MultiPoint2, PointOut, OverlayType
        >
{};

template
<
    typename PointLike1, typename PointLike2, typename TupledOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn1, typename TagIn2
>
struct intersection_insert
    <
        PointLike1, PointLike2, TupledOut, OverlayType,
        Reverse1, Reverse2,
        TagIn1, TagIn2, detail::tupled_output_tag,
        pointlike_tag, pointlike_tag, detail::tupled_output_tag
    >
    : detail::expect_output<PointLike1, PointLike2, TupledOut, point_tag>
{
    // NOTE: The order of geometries in TupledOut tuple/pair must correspond to the order
    // of iterators in OutputIterators tuple/pair.
    template
    <
        typename RobustPolicy, typename OutputIterators, typename Strategy
    >
    static inline OutputIterators apply(PointLike1 const& pointlike1,
                                        PointLike2 const& pointlike2,
                                        RobustPolicy const& robust_policy,
                                        OutputIterators oits,
                                        Strategy const& strategy)
    {
        namespace bgt = boost::geometry::tuples;

        static const bool out_point_index = bgt::find_index_if
            <
                TupledOut, geometry::detail::is_tag_same_as_pred<point_tag>::template pred
            >::value;

        bgt::get<out_point_index>(oits) = intersection_insert
            <
                PointLike1, PointLike2,
                typename bgt::element
                    <
                        out_point_index, TupledOut
                    >::type,
                OverlayType
            >::apply(pointlike1, pointlike2, robust_policy,
                     bgt::get<out_point_index>(oits),
                     strategy);

        return oits;
    }
};

// dispatch for difference/intersection of pointlike-linear geometries
template
<
    typename Point, typename Linear, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename Tag
>
struct intersection_insert
    <
        Point, Linear, PointOut, OverlayType,
        Reverse1, Reverse2,
        point_tag, Tag, point_tag,
        pointlike_tag, linear_tag, pointlike_tag
    > : detail_dispatch::overlay::pointlike_linear_point
        <
            Point, Linear, PointOut, OverlayType,
            point_tag, typename tag_cast<Tag, segment_tag, linear_tag>::type
        >
{};

template
<
    typename MultiPoint, typename Linear, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename Tag
>
struct intersection_insert
    <
        MultiPoint, Linear, PointOut, OverlayType,
        Reverse1, Reverse2,
        multi_point_tag, Tag, point_tag,
        pointlike_tag, linear_tag, pointlike_tag
    > : detail_dispatch::overlay::pointlike_linear_point
        <
            MultiPoint, Linear, PointOut, OverlayType,
            multi_point_tag,
            typename tag_cast<Tag, segment_tag, linear_tag>::type
        >
{};

// This specialization is needed because intersection() reverses the arguments
// for MultiPoint/Linestring combination.
template
<
    typename Linestring, typename MultiPoint, typename PointOut,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Linestring, MultiPoint, PointOut, overlay_intersection,
        Reverse1, Reverse2,
        linestring_tag, multi_point_tag, point_tag,
        linear_tag, pointlike_tag, pointlike_tag
    >
{
    template <typename RobustPolicy, typename OutputIterator, typename Strategy>
    static inline OutputIterator apply(Linestring const& linestring,
                                       MultiPoint const& multipoint,
                                       RobustPolicy const& robust_policy,
                                       OutputIterator out,
                                       Strategy const& strategy)
    {
        return detail_dispatch::overlay::pointlike_linear_point
            <
                MultiPoint, Linestring, PointOut, overlay_intersection,
                multi_point_tag, linear_tag
            >::apply(multipoint, linestring, robust_policy, out, strategy);
    }
};

template
<
    typename PointLike, typename Linear, typename TupledOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn1, typename TagIn2
>
struct intersection_insert
    <
        PointLike, Linear, TupledOut, OverlayType,
        Reverse1, Reverse2,
        TagIn1, TagIn2, detail::tupled_output_tag,
        pointlike_tag, linear_tag, detail::tupled_output_tag
    >
    // Reuse the implementation for PointLike/PointLike.
    : intersection_insert
        <
            PointLike, Linear, TupledOut, OverlayType,
            Reverse1, Reverse2,
            TagIn1, TagIn2, detail::tupled_output_tag,
            pointlike_tag, pointlike_tag, detail::tupled_output_tag
        >
{};

// This specialization is needed because intersection() reverses the arguments
// for MultiPoint/Linestring combination.
template
<
    typename Linestring, typename MultiPoint, typename TupledOut,
    bool Reverse1, bool Reverse2
>
struct intersection_insert
    <
        Linestring, MultiPoint, TupledOut, overlay_intersection,
        Reverse1, Reverse2,
        linestring_tag, multi_point_tag, detail::tupled_output_tag,
        linear_tag, pointlike_tag, detail::tupled_output_tag
    >
{
    template <typename RobustPolicy, typename OutputIterators, typename Strategy>
    static inline OutputIterators apply(Linestring const& linestring,
                                        MultiPoint const& multipoint,
                                        RobustPolicy const& robust_policy,
                                        OutputIterators out,
                                        Strategy const& strategy)
    {
        return intersection_insert
            <
                MultiPoint, Linestring, TupledOut, overlay_intersection
            >::apply(multipoint, linestring, robust_policy, out, strategy);
    }
};

// dispatch for difference/intersection of pointlike-areal geometries
template
<
    typename Point, typename Areal, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename ArealTag
>
struct intersection_insert
    <
        Point, Areal, PointOut, OverlayType,
        Reverse1, Reverse2,
        point_tag, ArealTag, point_tag,
        pointlike_tag, areal_tag, pointlike_tag
    > : detail_dispatch::overlay::pointlike_areal_point
        <
            Point, Areal, PointOut, OverlayType,
            point_tag, ArealTag
        >
{};

template
<
    typename MultiPoint, typename Areal, typename PointOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename ArealTag
>
struct intersection_insert
    <
        MultiPoint, Areal, PointOut, OverlayType,
        Reverse1, Reverse2,
        multi_point_tag, ArealTag, point_tag,
        pointlike_tag, areal_tag, pointlike_tag
    > : detail_dispatch::overlay::pointlike_areal_point
        <
            MultiPoint, Areal, PointOut, OverlayType,
            multi_point_tag, ArealTag
        >
{};

// This specialization is needed because intersection() reverses the arguments
// for MultiPoint/Ring and MultiPoint/Polygon combinations.
template
<
    typename Areal, typename MultiPoint, typename PointOut,
    bool Reverse1, bool Reverse2,
    typename ArealTag
>
struct intersection_insert
    <
        Areal, MultiPoint, PointOut, overlay_intersection,
        Reverse1, Reverse2,
        ArealTag, multi_point_tag, point_tag,
        areal_tag, pointlike_tag, pointlike_tag
    >
{
    template <typename RobustPolicy, typename OutputIterator, typename Strategy>
    static inline OutputIterator apply(Areal const& areal,
                                       MultiPoint const& multipoint,
                                       RobustPolicy const& robust_policy,
                                       OutputIterator out,
                                       Strategy const& strategy)
    {
        return detail_dispatch::overlay::pointlike_areal_point
            <
                MultiPoint, Areal, PointOut, overlay_intersection,
                multi_point_tag, ArealTag
            >::apply(multipoint, areal, robust_policy, out, strategy);
    }
};

template
<
    typename PointLike, typename Areal, typename TupledOut,
    overlay_type OverlayType,
    bool Reverse1, bool Reverse2,
    typename TagIn1, typename TagIn2
>
struct intersection_insert
    <
        PointLike, Areal, TupledOut, OverlayType,
        Reverse1, Reverse2,
        TagIn1, TagIn2, detail::tupled_output_tag,
        pointlike_tag, areal_tag, detail::tupled_output_tag
    >
    // Reuse the implementation for PointLike/PointLike.
    : intersection_insert
        <
            PointLike, Areal, TupledOut, OverlayType,
            Reverse1, Reverse2,
            TagIn1, TagIn2, detail::tupled_output_tag,
            pointlike_tag, pointlike_tag, detail::tupled_output_tag
        >
{};

// This specialization is needed because intersection() reverses the arguments
// for MultiPoint/Ring and MultiPoint/Polygon combinations.
template
<
    typename Areal, typename MultiPoint, typename TupledOut,
    bool Reverse1, bool Reverse2,
    typename TagIn1
>
struct intersection_insert
    <
        Areal, MultiPoint, TupledOut, overlay_intersection,
        Reverse1, Reverse2,
        TagIn1, multi_point_tag, detail::tupled_output_tag,
        areal_tag, pointlike_tag, detail::tupled_output_tag
    >
{
    template <typename RobustPolicy, typename OutputIterators, typename Strategy>
    static inline OutputIterators apply(Areal const& areal,
                                        MultiPoint const& multipoint,
                                        RobustPolicy const& robust_policy,
                                        OutputIterators out,
                                        Strategy const& strategy)
    {
        return intersection_insert
            <
                MultiPoint, Areal, TupledOut, overlay_intersection
            >::apply(multipoint, areal, robust_policy, out, strategy);
    }
};

template
<
    typename Linestring, typename Polygon,
    typename TupledOut,
    overlay_type OverlayType,
    bool ReverseLinestring, bool ReversePolygon
>
struct intersection_insert
    <
        Linestring, Polygon,
        TupledOut,
        OverlayType,
        ReverseLinestring, ReversePolygon,
        linestring_tag, polygon_tag, detail::tupled_output_tag,
        linear_tag, areal_tag, detail::tupled_output_tag
    > : detail::intersection::intersection_of_linestring_with_areal
            <
                ReversePolygon,
                TupledOut,
                OverlayType,
                true
            >
{};

template
<
    typename Linestring, typename Ring,
    typename TupledOut,
    overlay_type OverlayType,
    bool ReverseLinestring, bool ReverseRing
>
struct intersection_insert
    <
        Linestring, Ring,
        TupledOut,
        OverlayType,
        ReverseLinestring, ReverseRing,
        linestring_tag, ring_tag, detail::tupled_output_tag,
        linear_tag, areal_tag, detail::tupled_output_tag
    > : detail::intersection::intersection_of_linestring_with_areal
            <
                ReverseRing,
                TupledOut,
                OverlayType,
                true
            >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template
<
    typename GeometryOut,
    bool ReverseSecond,
    overlay_type OverlayType,
    typename Geometry1, typename Geometry2,
    typename RobustPolicy,
    typename OutputIterator,
    typename Strategy
>
inline OutputIterator insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            RobustPolicy robust_policy,
            OutputIterator out,
            Strategy const& strategy)
{
    return std::conditional_t
        <
            geometry::reverse_dispatch<Geometry1, Geometry2>::type::value,
            geometry::dispatch::intersection_insert_reversed
            <
                Geometry1, Geometry2,
                GeometryOut,
                OverlayType,
                overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
                overlay::do_reverse<geometry::point_order<Geometry2>::value, ReverseSecond>::value
            >,
            geometry::dispatch::intersection_insert
            <
                Geometry1, Geometry2,
                GeometryOut,
                OverlayType,
                geometry::detail::overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
                geometry::detail::overlay::do_reverse<geometry::point_order<Geometry2>::value, ReverseSecond>::value
            >
        >::apply(geometry1, geometry2, robust_policy, out, strategy);
}

/*!
\brief \brief_calc2{intersection} \brief_strategy
\ingroup intersection
\details \details_calc2{intersection_insert, spatial set theoretic intersection}
    \brief_strategy. \details_insert{intersection}
\tparam GeometryOut \tparam_geometry{\p_l_or_c}
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam OutputIterator \tparam_out{\p_l_or_c}
\tparam Strategy \tparam_strategy_overlay
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{intersection}
\param strategy \param_strategy{intersection}
\return \return_out

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/intersection.qbk]}
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename OutputIterator,
    typename Strategy
>
inline OutputIterator intersection_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            OutputIterator out,
            Strategy const& strategy)
{
    concepts::check<Geometry1 const>();
    concepts::check<Geometry2 const>();

    typedef typename geometry::rescale_overlay_policy_type
        <
            Geometry1,
            Geometry2,
            typename Strategy::cs_tag
        >::type rescale_policy_type;

    rescale_policy_type robust_policy
            = geometry::get_rescale_policy<rescale_policy_type>(
                geometry1, geometry2, strategy);

    return detail::intersection::insert
        <
            GeometryOut, false, overlay_intersection
        >(geometry1, geometry2, robust_policy, out, strategy);
}

/*!
\brief \brief_calc2{intersection}
\ingroup intersection
\details \details_calc2{intersection_insert, spatial set theoretic intersection}.
    \details_insert{intersection}
\tparam GeometryOut \tparam_geometry{\p_l_or_c}
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam OutputIterator \tparam_out{\p_l_or_c}
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{intersection}
\return \return_out

\qbk{[include reference/algorithms/intersection.qbk]}
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename OutputIterator
>
inline OutputIterator intersection_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            OutputIterator out)
{
    concepts::check<Geometry1 const>();
    concepts::check<Geometry2 const>();

    typedef typename strategies::relate::services::default_strategy
        <
            Geometry1, Geometry2
        >::type strategy_type;

    return intersection_insert<GeometryOut>(geometry1, geometry2, out,
                                            strategy_type());
}

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

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_INTERSECTION_INSERT_HPP


#pragma once
#include <Eigen/Dense>

#include <memory>
#include <sstream>
#include <string>
#include <vector>

namespace yavque
{
class Circuit;
class VariableImpl
{
private:
	double value_;
	std::string name_;

	std::vector<std::shared_ptr<Circuit>> deriv_circuits_ = {};
	mutable std::shared_ptr<Eigen::VectorXcd> grad_ = {};
	mutable bool grad_updated_ = false;

public:
	explicit VariableImpl(double value) : value_{value}
	{
		static Counter counter;
		std::ostringstream ss; // change to format C++20
		ss << "variable " << counter.count();
		name_ = ss.str();
	}

	VariableImpl(double value, std::string name) : value_{value}, name_{std::move(name)}
	{
	}

	double value() const { return value_; }

	void set_value(double new_value) { value_ = new_value; }

	[[nodiscard]] std::string name() const { return name_; }

	void set_name(const std::string& name) { name_ = name; }

	[[nodiscard]] std::string desc() const;

	void add_grad(const Circuit& circ);
	void add_grad(Circuit&& circ);

	std::shared_ptr<const Eigen::VectorXcd> grad() const;
	void zero_grad();
};
} // namespace yavque


#pragma once
#include "amcl_3d/particle_filter_interface.hpp"
#include "amcl_3d/particle_filter.hpp"
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <memory>
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>

namespace amcl_3d
{
class Amcl
{
  public:
    Amcl();
    ~Amcl(){};
    bool setKdMap(const pcl::PointCloud<pcl::PointXYZ>::Ptr map);
    bool measureIMU();
    bool setInitialPose(const Position &position);
    bool setInitialPose(const Position &position, const Quat &quat);
    bool setInitialPose(const Position &position, const Quat &quat, const PoseCovariance &covariance);

  private:
    pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kd_map_ptr_;
    std::shared_ptr<ParticleFilter> pf_ptr_;
};
} // namespace amcl_3d


#ifndef POLYFIT_TYPEDEFS_H_
#define POLYFIT_TYPEDEFS_H_

// Eigen
#include <Eigen/Core>

// libc++
#include <cstdint>
#include <utility>
#include <vector>
#define _USE_MATH_DEFINES
#include <math.h>

// todo remove
#define PF_DEBUG
#ifdef _DEBUG
#define PF_DEBUG
#endif

namespace polyfit {
	using vec2 = Eigen::Vector2d;
	using vec3 = Eigen::Vector3d;
	using vec4 = Eigen::Vector4d;

	using vec2i = Eigen::Vector2i;
	using vec3i = Eigen::Vector3i;
	using vec4i = Eigen::Vector4i;

	using mat2 = Eigen::Matrix<double, 2, 2>;
	using mat3 = Eigen::Matrix<double, 3, 3>;
	using mat4 = Eigen::Matrix<double, 4, 4>;

	using mat2x = Eigen::Matrix2Xd;
	using mat2xi = Eigen::Matrix2Xi;
	using mat3xi = Eigen::Matrix3Xi;
	using mat4xi = Eigen::Matrix4Xi;
	using mat4i = Eigen::Matrix4i;
	using mat23 = Eigen::Matrix<double, 2, 3>;
	using mat24 = Eigen::Matrix<double, 2, 4>;

	using mat24i = Eigen::Matrix<int, 2, 4>;

	using vecXd = Eigen::VectorXd;
	using vecXi = Eigen::VectorXi;

	using Vertex = Eigen::Index;

	using matXd = Eigen::MatrixXd;
	using matXi = Eigen::MatrixXi;

	// https://stackoverflow.com/questions/5889238/why-is-xor-the-default-way-to-combine-hashes
	inline uint64_t hash_combine(uint64_t lhs, uint64_t rhs) {
		lhs ^= rhs + 0x9e3779b9 + (lhs << 6) + (lhs >> 2);
		return lhs;
	}

	struct _StdHashVertexPair {
		uint64_t operator() (const std::pair<Vertex, Vertex>& t) const {
			return hash_combine((uint64_t) t.first, (uint64_t)t.second);
		}
	};
}

#endif // POLYFIT_TYPEDEFS_H_


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <functional>
#include <unordered_map>
#include <unordered_set>
#include <utility>

#include "DataStructures/DataBox/Tag.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tags.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"  // IWYU pragma: keep
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/Side.hpp"
#include "ErrorHandling/Error.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/SimpleWenoImpl.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoOscillationIndicator.hpp"
#include "Helpers/Evolution/DiscontinuousGalerkin/Limiters/TestHelpers.hpp"
#include "NumericalAlgorithms/Interpolation/RegularGridInterpolant.hpp"  // IWYU pragma: keep
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

// IWYU pragma: no_include "Domain/Structure/OrientationMapHelpers.hpp"
// IWYU pragma: no_forward_declare Tags::Mean
// IWYU pragma: no_forward_declare Tensor
// IWYU pragma: no_forward_declare Variables

namespace {

template <size_t VolumeDim>
struct VectorTag : db::SimpleTag {
  using type = tnsr::I<DataVector, VolumeDim>;
};

template <size_t VolumeDim, typename Tag>
struct DummyPackagedData {
  Variables<tmpl::list<Tag>> volume_data;
  tuples::TaggedTuple<::Tags::Mean<Tag>> means;
  Mesh<VolumeDim> mesh;
};

template <size_t VolumeDim>
using VariablesMap = std::unordered_map<
    std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
    Variables<tmpl::list<VectorTag<VolumeDim>>>,
    boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>;

template <size_t VolumeDim>
std::unordered_map<
    std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
    DummyPackagedData<VolumeDim, VectorTag<VolumeDim>>,
    boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
make_neighbor_data_from_neighbor_vars(
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const VariablesMap<VolumeDim>& neighbor_vars) noexcept {
  const auto make_tuple_of_means =
      [&mesh](const Variables<tmpl::list<VectorTag<VolumeDim>>>&
                  vars_to_average) noexcept {
        tuples::TaggedTuple<::Tags::Mean<VectorTag<VolumeDim>>> result;
        for (size_t d = 0; d < VolumeDim; ++d) {
          get<::Tags::Mean<VectorTag<VolumeDim>>>(result).get(d) = mean_value(
              get<VectorTag<VolumeDim>>(vars_to_average).get(d), mesh);
        }
        return result;
      };

  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      DummyPackagedData<VolumeDim, VectorTag<VolumeDim>>,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      neighbor_data{};

  for (const auto& neighbor : element.neighbors()) {
    const auto dir = neighbor.first;
    const auto id = *(neighbor.second.cbegin());
    const auto dir_and_id = std::make_pair(dir, id);
    neighbor_data[dir_and_id].volume_data = neighbor_vars.at(dir_and_id);
    neighbor_data[dir_and_id].means =
        make_tuple_of_means(neighbor_vars.at(dir_and_id));
    neighbor_data[dir_and_id].mesh = mesh;
  }

  return neighbor_data;
}

template <size_t VolumeDim>
void test_simple_weno_work(
    const tnsr::I<DataVector, VolumeDim>& local_data,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        DummyPackagedData<VolumeDim, VectorTag<VolumeDim>>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data,
    const VariablesMap<VolumeDim>& expected_neighbor_modified_vars,
    Approx local_approx = approx) noexcept {
  // First run some sanity checks on the input, and make sure the test function
  // is being called in a reasonable way
  if (element.neighbors().size() != neighbor_data.size()) {
    ERROR("Different number of neighbors from element, neighbor_data");
  }
  if (neighbor_data.size() != expected_neighbor_modified_vars.size()) {
    ERROR("Different sizes for neighbor_data, expected_neighbor_modified_vars");
  }
  for (const auto& neighbor : element.neighbors()) {
    if (neighbor.second.ids().size() > 1) {
      ERROR("Too many neighbors: h-refinement is not yet supported");
    }
    const auto dir = neighbor.first;
    const auto id = *(neighbor.second.cbegin());
    const auto dir_and_id = std::make_pair(dir, id);
    if (neighbor_data.find(dir_and_id) == neighbor_data.end()) {
      ERROR("Missing neighbor_data at an internal boundary");
    }
    if (expected_neighbor_modified_vars.find(dir_and_id) ==
        expected_neighbor_modified_vars.end()) {
      ERROR("Missing expected_neighbor_modified_vars at an internal boundary");
    }
  }

  // Buffers for simple WENO implementation
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      intrp::RegularGrid<VolumeDim>,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      interpolator_buffer{};
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      modified_neighbor_solution_buffer{};
  for (const auto& neighbor_and_data : neighbor_data) {
    const auto& neighbor = neighbor_and_data.first;
    modified_neighbor_solution_buffer.insert(
        make_pair(neighbor, DataVector(mesh.number_of_grid_points())));
  }

  // WENO should preserve the mean, so expected means = initial means
  const auto expected_vector_means = [&local_data, &mesh]() noexcept {
    std::array<double, VolumeDim> means{};
    for (size_t d = 0; d < VolumeDim; ++d) {
      gsl::at(means, d) = mean_value(local_data.get(d), mesh);
    }
    return means;
  }();

  auto vector_to_limit = local_data;
  const double neighbor_linear_weight = 0.001;
  // The "tensor" interface is a thin wrapper around the "single component"
  // interface, so no need to test both overloads separately.
  Limiters::Weno_detail::simple_weno_impl<VectorTag<VolumeDim>>(
      make_not_null(&interpolator_buffer),
      make_not_null(&modified_neighbor_solution_buffer),
      make_not_null(&vector_to_limit), neighbor_linear_weight, mesh, element,
      neighbor_data);

  for (size_t d = 0; d < VolumeDim; ++d) {
    CHECK(mean_value(vector_to_limit.get(d), mesh) ==
          approx(gsl::at(expected_vector_means, d)));
  }

  auto expected_vector = local_data;
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      expected_neighbor_polynomials;
  for (size_t i = 0; i < VolumeDim; ++i) {
    for (auto& neighbor_and_vars : expected_neighbor_modified_vars) {
      expected_neighbor_polynomials[neighbor_and_vars.first] =
          get<VectorTag<VolumeDim>>(neighbor_and_vars.second).get(i);
    }
    Limiters::Weno_detail::reconstruct_from_weighted_sum(
        make_not_null(&(expected_vector.get(i))), neighbor_linear_weight,
        Limiters::Weno_detail::DerivativeWeight::PowTwoEll, mesh,
        expected_neighbor_polynomials);
  }
  CHECK_ITERABLE_CUSTOM_APPROX(expected_vector, vector_to_limit, local_approx);
}

void test_simple_weno_1d(const std::unordered_set<Direction<1>>&
                             directions_of_external_boundaries = {}) noexcept {
  INFO("Test simple_weno_impl in 1D");
  CAPTURE(directions_of_external_boundaries);
  const auto mesh =
      Mesh<1>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element =
      TestHelpers::Limiters::make_element<1>(directions_of_external_boundaries);
  const auto logical_coords = logical_coordinates(mesh);

  // Functions to produce dummy data on each element
  const auto make_center_tensor =
      [](const tnsr::I<DataVector, 1, Frame::Logical>& coords) noexcept {
        const auto& x = get<0>(coords);
        return tnsr::I<DataVector, 1>{{{0.4 * x - 0.1 * square(x)}}};
      };
  const auto make_lower_xi_vars =
      [](const tnsr::I<DataVector, 1, Frame::Logical>& coords,
         const double offset = 0.0) noexcept {
        const auto x = get<0>(coords) + offset;
        Variables<tmpl::list<VectorTag<1>>> vars(x.size());
        get<0>(get<VectorTag<1>>(vars)) = -0.1 + 0.3 * x - 0.1 * square(x);
        return vars;
      };
  const auto make_upper_xi_vars =
      [](const tnsr::I<DataVector, 1, Frame::Logical>& coords,
         const double offset = 0.0) noexcept {
        const auto x = get<0>(coords) + offset;
        Variables<tmpl::list<VectorTag<1>>> vars(x.size());
        get<0>(get<VectorTag<1>>(vars)) = 0.6 * x - 0.3 * square(x);
        return vars;
      };

  const auto local_data = make_center_tensor(logical_coords);
  VariablesMap<1> neighbor_vars{};
  VariablesMap<1> neighbor_modified_vars{};

  const auto shift_vars_to_local_means =
      [&mesh,
       &local_data](const Variables<tmpl::list<VectorTag<1>>>& input) noexcept {
        auto result = input;
        auto& v = get<VectorTag<1>>(result);
        get<0>(v) +=
            mean_value(get<0>(local_data), mesh) - mean_value(get<0>(v), mesh);
        return result;
      };

  const auto make_neighbor_vars =
      [&logical_coords, &directions_of_external_boundaries, &neighbor_vars,
       &neighbor_modified_vars, &shift_vars_to_local_means](
          const std::pair<Direction<1>, ElementId<1>>& neighbor,
          const auto make_vars) noexcept {
        if (directions_of_external_boundaries.count(neighbor.first) == 0) {
          const double offset =
              (neighbor.first.side() == Side::Lower ? -2.0 : 2.0);
          neighbor_vars[neighbor] = make_vars(logical_coords, offset);
          neighbor_modified_vars[neighbor] =
              shift_vars_to_local_means(make_vars(logical_coords));
        }
      };

  make_neighbor_vars(std::make_pair(Direction<1>::lower_xi(), ElementId<1>(1)),
                     make_lower_xi_vars);

  make_neighbor_vars(std::make_pair(Direction<1>::upper_xi(), ElementId<1>(2)),
                     make_upper_xi_vars);

  const auto neighbor_data =
      make_neighbor_data_from_neighbor_vars(mesh, element, neighbor_vars);

  test_simple_weno_work<1>(local_data, mesh, element, neighbor_data,
                           neighbor_modified_vars);
}

void test_simple_weno_2d(const std::unordered_set<Direction<2>>&
                             directions_of_external_boundaries = {}) noexcept {
  INFO("Test simple_weno_impl in 2D");
  CAPTURE(directions_of_external_boundaries);
  const auto mesh =
      Mesh<2>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element =
      TestHelpers::Limiters::make_element<2>(directions_of_external_boundaries);
  const auto logical_coords = logical_coordinates(mesh);

  const auto make_center_tensor =
      [](const tnsr::I<DataVector, 2, Frame::Logical>& coords) noexcept {
        const auto& x = get<0>(coords);
        const auto& y = get<1>(coords);
        return tnsr::I<DataVector, 2>{
            {{x + 2.5 * y,
              0.1 + 0.2 * x - 0.4 * y + 0.3 * square(x) * square(y)}}};
      };
  const auto make_lower_xi_vars =
      [](const tnsr::I<DataVector, 2, Frame::Logical>& coords,
         const double xi_offset = 0.0) noexcept {
        const auto x = get<0>(coords) + xi_offset;
        const auto& y = get<1>(coords);
        Variables<tmpl::list<VectorTag<2>>> vars(x.size());
        get<0>(get<VectorTag<2>>(vars)) = x + 2.5 * y;
        get<1>(get<VectorTag<2>>(vars)) = 3.0 + 0.2 * y;
        return vars;
      };
  const auto make_upper_xi_vars =
      [](const tnsr::I<DataVector, 2, Frame::Logical>& coords,
         const double xi_offset = 0.0) noexcept {
        const auto x = get<0>(coords) + xi_offset;
        const auto& y = get<1>(coords);
        Variables<tmpl::list<VectorTag<2>>> vars(x.size());
        get<0>(get<VectorTag<2>>(vars)) = x + 2.5 * y;
        get<1>(get<VectorTag<2>>(vars)) = -2.4 + square(x);
        return vars;
      };
  const auto make_lower_eta_vars =
      [](const tnsr::I<DataVector, 2, Frame::Logical>& coords,
         const double eta_offset = 0.0) noexcept {
        const auto& x = get<0>(coords);
        const auto y = get<1>(coords) + eta_offset;
        Variables<tmpl::list<VectorTag<2>>> vars(x.size());
        get<0>(get<VectorTag<2>>(vars)) = x + 2.5 * y;
        get<1>(get<VectorTag<2>>(vars)) = 0.2 - y;
        return vars;
      };
  const auto make_upper_eta_vars =
      [](const tnsr::I<DataVector, 2, Frame::Logical>& coords,
         const double eta_offset = 0.0) noexcept {
        const auto& x = get<0>(coords);
        const auto y = get<1>(coords) + eta_offset;
        Variables<tmpl::list<VectorTag<2>>> vars(x.size());
        get<0>(get<VectorTag<2>>(vars)) = x + 2.5 * y;
        get<1>(get<VectorTag<2>>(vars)) = 0.4 + 0.3 * x * square(y);
        return vars;
      };

  const auto local_data = make_center_tensor(logical_coords);
  VariablesMap<2> neighbor_vars{};
  VariablesMap<2> neighbor_modified_vars{};

  const auto shift_vars_to_local_means =
      [&mesh,
       &local_data](const Variables<tmpl::list<VectorTag<2>>>& input) noexcept {
        auto result = input;
        auto& v = get<VectorTag<2>>(result);
        get<0>(v) +=
            mean_value(get<0>(local_data), mesh) - mean_value(get<0>(v), mesh);
        get<1>(v) +=
            mean_value(get<1>(local_data), mesh) - mean_value(get<1>(v), mesh);
        return result;
      };

  const auto make_neighbor_vars =
      [&logical_coords, &directions_of_external_boundaries, &neighbor_vars,
       &neighbor_modified_vars, &shift_vars_to_local_means](
          const std::pair<Direction<2>, ElementId<2>>& neighbor,
          const auto make_vars) noexcept {
        if (directions_of_external_boundaries.count(neighbor.first) == 0) {
          const double offset =
              (neighbor.first.side() == Side::Lower ? -2.0 : 2.0);
          neighbor_vars[neighbor] = make_vars(logical_coords, offset);
          neighbor_modified_vars[neighbor] =
              shift_vars_to_local_means(make_vars(logical_coords));
        }
      };

  make_neighbor_vars(std::make_pair(Direction<2>::lower_xi(), ElementId<2>(1)),
                     make_lower_xi_vars);

  make_neighbor_vars(std::make_pair(Direction<2>::upper_xi(), ElementId<2>(2)),
                     make_upper_xi_vars);

  make_neighbor_vars(std::make_pair(Direction<2>::lower_eta(), ElementId<2>(3)),
                     make_lower_eta_vars);

  make_neighbor_vars(std::make_pair(Direction<2>::upper_eta(), ElementId<2>(4)),
                     make_upper_eta_vars);

  const auto neighbor_data =
      make_neighbor_data_from_neighbor_vars(mesh, element, neighbor_vars);

  test_simple_weno_work<2>(local_data, mesh, element, neighbor_data,
                           neighbor_modified_vars);
}

void test_simple_weno_3d(const std::unordered_set<Direction<3>>&
                             directions_of_external_boundaries = {}) noexcept {
  INFO("Test simple_weno_impl in 3D");
  CAPTURE(directions_of_external_boundaries);
  const auto mesh = Mesh<3>({{3, 4, 5}}, Spectral::Basis::Legendre,
                            Spectral::Quadrature::GaussLobatto);
  const auto element =
      TestHelpers::Limiters::make_element<3>(directions_of_external_boundaries);
  const auto logical_coords = logical_coordinates(mesh);

  const auto make_center_tensor =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords) noexcept {
        const auto& x = get<0>(coords);
        const auto& y = get<1>(coords);
        const auto& z = get<2>(coords);
        return tnsr::I<DataVector, 3>{
            {{0.4 * x * y * z + square(z), z, x + square(y) + cube(z)}}};
      };
  const auto make_lower_xi_vars =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords,
         const double xi_offset = 0.0) noexcept {
        const auto x = get<0>(coords) + xi_offset;
        const auto& y = get<1>(coords);
        const auto& z = get<2>(coords);
        Variables<tmpl::list<VectorTag<3>>> vars(x.size());
        get<0>(get<VectorTag<3>>(vars)) = 0.4 * x * y * z + square(z);
        get<1>(get<VectorTag<3>>(vars)) = 0.8 * z + 0.3 * x * y;
        get<2>(get<VectorTag<3>>(vars)) = x + y;
        return vars;
      };
  const auto make_upper_xi_vars =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords,
         const double xi_offset = 0.0) noexcept {
        const auto x = get<0>(coords) + xi_offset;
        const auto& y = get<1>(coords);
        const auto& z = get<2>(coords);
        Variables<tmpl::list<VectorTag<3>>> vars(x.size());
        get<0>(get<VectorTag<3>>(vars)) = 0.4 * x * y * z + square(z);
        get<1>(get<VectorTag<3>>(vars)) = z + 0.1 * square(x);
        get<2>(get<VectorTag<3>>(vars)) = y + square(x) * z;
        return vars;
      };
  const auto make_lower_eta_vars =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords,
         const double eta_offset = 0.0) noexcept {
        const auto& x = get<0>(coords);
        const auto y = get<1>(coords) + eta_offset;
        const auto& z = get<2>(coords);
        Variables<tmpl::list<VectorTag<3>>> vars(x.size());
        get<0>(get<VectorTag<3>>(vars)) = 0.4 * x * y * z + square(z);
        get<1>(get<VectorTag<3>>(vars)) = -0.1 * y + z;
        get<2>(get<VectorTag<3>>(vars)) = -square(z);
        return vars;
      };
  const auto make_upper_eta_vars =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords,
         const double eta_offset = 0.0) noexcept {
        const auto& x = get<0>(coords);
        const auto y = get<1>(coords) + eta_offset;
        const auto& z = get<2>(coords);
        Variables<tmpl::list<VectorTag<3>>> vars(x.size());
        get<0>(get<VectorTag<3>>(vars)) = 0.4 * x * y * z + square(z);
        get<1>(get<VectorTag<3>>(vars)) = z + 0.4 * x * cube(z);
        get<2>(get<VectorTag<3>>(vars)) = y * z + square(y) + cube(z);
        return vars;
      };
  const auto make_lower_zeta_vars =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords,
         const double zeta_offset = 0.0) noexcept {
        const auto& x = get<0>(coords);
        const auto& y = get<1>(coords);
        const auto z = get<2>(coords) + zeta_offset;
        Variables<tmpl::list<VectorTag<3>>> vars(x.size());
        get<0>(get<VectorTag<3>>(vars)) = 0.4 * x * y * z + square(z);
        get<1>(get<VectorTag<3>>(vars)) = 0.9 * z - 2. * x * z;
        get<2>(get<VectorTag<3>>(vars)) = y + cube(z);
        return vars;
      };
  const auto make_upper_zeta_vars =
      [](const tnsr::I<DataVector, 3, Frame::Logical>& coords,
         const double zeta_offset = 0.0) noexcept {
        const auto& x = get<0>(coords);
        const auto& y = get<1>(coords);
        const auto z = get<2>(coords) + zeta_offset;
        Variables<tmpl::list<VectorTag<3>>> vars(x.size());
        get<0>(get<VectorTag<3>>(vars)) = 0.4 * x * y * z + square(z);
        get<1>(get<VectorTag<3>>(vars)) = 1.3 * square(y) * square(z);
        get<2>(get<VectorTag<3>>(vars)) = -x * y * z + square(y);
        return vars;
      };

  const auto local_data = make_center_tensor(logical_coords);
  VariablesMap<3> neighbor_vars{};
  VariablesMap<3> neighbor_modified_vars{};

  const auto shift_vars_to_local_means =
      [&mesh,
       &local_data](const Variables<tmpl::list<VectorTag<3>>>& input) noexcept {
        auto result = input;
        auto& v = get<VectorTag<3>>(result);
        get<0>(v) +=
            mean_value(get<0>(local_data), mesh) - mean_value(get<0>(v), mesh);
        get<1>(v) +=
            mean_value(get<1>(local_data), mesh) - mean_value(get<1>(v), mesh);
        get<2>(v) +=
            mean_value(get<2>(local_data), mesh) - mean_value(get<2>(v), mesh);
        return result;
      };

  const auto make_neighbor_vars =
      [&logical_coords, &directions_of_external_boundaries, &neighbor_vars,
       &neighbor_modified_vars, &shift_vars_to_local_means](
          const std::pair<Direction<3>, ElementId<3>>& neighbor,
          const auto make_vars) noexcept {
        if (directions_of_external_boundaries.count(neighbor.first) == 0) {
          const double offset =
              (neighbor.first.side() == Side::Lower ? -2.0 : 2.0);
          neighbor_vars[neighbor] = make_vars(logical_coords, offset);
          neighbor_modified_vars[neighbor] =
              shift_vars_to_local_means(make_vars(logical_coords));
        }
      };

  make_neighbor_vars(std::make_pair(Direction<3>::lower_xi(), ElementId<3>(1)),
                     make_lower_xi_vars);

  make_neighbor_vars(std::make_pair(Direction<3>::upper_xi(), ElementId<3>(2)),
                     make_upper_xi_vars);

  make_neighbor_vars(std::make_pair(Direction<3>::lower_eta(), ElementId<3>(3)),
                     make_lower_eta_vars);

  make_neighbor_vars(std::make_pair(Direction<3>::upper_eta(), ElementId<3>(4)),
                     make_upper_eta_vars);

  make_neighbor_vars(
      std::make_pair(Direction<3>::lower_zeta(), ElementId<3>(5)),
      make_lower_zeta_vars);

  make_neighbor_vars(
      std::make_pair(Direction<3>::upper_zeta(), ElementId<3>(6)),
      make_upper_zeta_vars);

  const auto neighbor_data =
      make_neighbor_data_from_neighbor_vars(mesh, element, neighbor_vars);

  // The 3D Simple WENO solution has slightly larger numerical error, presumably
  // arising from the 3D extrapolation
  Approx custom_approx = Approx::custom().epsilon(1.e-11).scale(1.0);
  test_simple_weno_work<3>(local_data, mesh, element, neighbor_data,
                           neighbor_modified_vars, custom_approx);
}

}  // namespace

SPECTRE_TEST_CASE("Unit.Evolution.DG.Limiters.SimpleWenoImpl",
                  "[Limiters][Unit]") {
  test_simple_weno_1d();
  test_simple_weno_2d();
  test_simple_weno_3d();

  // Test with particular boundaries labeled as external
  test_simple_weno_1d({{Direction<1>::lower_xi()}});
  test_simple_weno_2d({{Direction<2>::lower_eta()}});
  test_simple_weno_2d({{Direction<2>::lower_xi(), Direction<2>::lower_eta(),
                        Direction<2>::upper_eta()}});
  test_simple_weno_3d({{Direction<3>::lower_zeta()}});
  test_simple_weno_3d({{Direction<3>::lower_xi(), Direction<3>::upper_xi(),
                        Direction<3>::lower_eta(), Direction<3>::lower_zeta(),
                        Direction<3>::lower_zeta()}});
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_REM_PIO2_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_REM_PIO2_HPP_INCLUDED

#include <nt2/trigonometric/functions/rem_pio2.hpp>
#include <nt2/trigonometric/functions/scalar/impl/trigo/selection_tags.hpp>
#include <nt2/include/functions/simd/rem_pio2_straight.hpp>
#include <nt2/include/functions/simd/rem_pio2_medium.hpp>
#include <nt2/include/functions/simd/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/load.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/include/constants/zero.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_,  tag::cpu_,
                      (A0),
                      ((generic_ < floating_<A0> > ))
                    )
  {
    typedef typename meta::as_integer<A0>::type            itype;
    typedef boost::fusion::tuple<itype,A0,A0>        result_type;

    inline result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::fusion::at_c<0>(res) =
        nt2::rem_pio2(a0,
                      boost::fusion::at_c<1>(res),
                      boost::fusion::at_c<2>(res)
                     );
      return res;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_, tag::cpu_,
                             (A0),
                             ((generic_ < floating_<A0> > ))
                             ((generic_ < floating_<A0> > ))
                            )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& a0, A0 & xr) const
    {
      A0 xc;
      return nt2::rem_pio2(a0, xr, xc);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_, tag::cpu_,
                             (A0)(A1),
                             ((generic_ <floating_<A0>  > ))
                             ((generic_ <floating_<A0>  > ))
                             ((target_ <unspecified_<A1> >))
                 )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& a0, A0 & xr, A1 const&) const
    {
      typedef typename A1::type selector;
      return rempio2<selector, void>::rem(a0, xr);
    }
  private:
    template < class T, class dummy = void> struct rempio2
    {
      static inline result_type rem(A0 const& x, A0 & xr)
      {
        BOOST_ASSERT_MSG(false, "wrong target for rem_pio2");
        return nt2::Zero<result_type>();
      }
    };

    template < class dummy> struct rempio2 < big_, dummy>
    {
      static inline result_type rem(A0 const& x, A0 & xr)
      {
        return nt2::rem_pio2(x, xr);
      }
    };

    template < class dummy> struct rempio2 < very_small_, dummy >
    {
      static inline result_type rem(A0 const& x, A0 & xr)
      {
        return nt2::rem_pio2_straight(x, xr);
      }
    };

    template < class dummy> struct rempio2 < small_, dummy >
    {
      static inline result_type rem(A0 const& x, A0 & xr)
      {
        return nt2::rem_pio2_cephes(x, xr);
      }
    };

    template < class dummy> struct rempio2 < medium_, dummy >
    {
      static inline result_type rem(A0 const& x, A0 & xr)
      {
        return nt2::rem_pio2_medium(x, xr);
      }
    };
  };

} }
#endif


#include "cloud_walker.h"

#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

#include "../playground/boost_serialization_eigen.h"

string CloudWalker::get_file_path_base() {
    mkdir_if_not_exist(CLOUD_WALKER_DIR);
    return CLOUD_WALKER_DIR + str(format("%s-%.3f-%s-%s-%s-%s") % g_name % c % T % L % R % R_prime);
}

CloudWalker::CloudWalker(DirectedG *graph, string name, double c_, int T_, int L_, int R_, int R_prime_) {
    c = c_;
    T = T_;
    L = L_;
    R = R_;
    R_prime = R_prime_;
    g = graph;
    g_name = name;
    n = num_vertices(*g);

#ifdef SINGLE_SOURCE
    F.resize(n);
    F.setZero();
    hat_P.resize(n, n);
#endif
    vector<int> out_degrees(n, 0);
    for (int i = 0; i < n; i++) {
        out_degrees[i] = out_degree(i, *g);
    }
#ifdef SINGLE_SOURCE
    hat_P.reserve(out_degrees);
#endif

    build_or_load_index();

#ifdef SINGLE_SOURCE
    preprocess_F();
    preprocess_hat_P();
#endif
//    cout << "mem size:" << getValue() << endl;
}

void CloudWalker::build_or_load_index() {
    string d_file = get_file_path_base() + ".D";
    if (file_exists(d_file)) {
        std::ifstream ifs(d_file, ios::binary | ios::in);
        boost::archive::binary_iarchive ia(ifs);
        boost::serialization::load(ia, D, 0);
        ifs.close();
    } else {
        D.resize(n);
        D.setOnes();

        auto start = std::chrono::high_resolution_clock::now();
        preprocess_D();
        auto pre_time = std::chrono::high_resolution_clock::now();
        cout << "indexing time:"
             << float(std::chrono::duration_cast<std::chrono::microseconds>(pre_time - start).count()) / (pow(10, 6))
             << " s\n";

        // serialization
        std::ofstream ofs(d_file);
        boost::archive::binary_oarchive oa(ofs);
        boost::serialization::save(oa, D, 0);
        ofs.close();

        cout << "mem size:" << getValue() << endl;
    }
}

void CloudWalker::preprocess_D() {
    D.resize(n);
    // D.setOnes(); // initially D = I
    D.setZero();
    cout << "computing D" << endl;
    // use jacobi method to compute D

    vector<std::unordered_map<int, double>> A; // the sparse matrix A for the linear system
    for (int i = 0; i < n; i++) {
        A.push_back(std::unordered_map<int, double>());
    }

    DirectedG::vertex_descriptor i, current_pos;

    std::unordered_map<int, int> *pre_ptr, pre_pos, *next_ptr, next_pos;
    pre_ptr = &pre_pos;
    next_ptr = &next_pos;
    for (i = 0; i < n; i++) {
        // fill the sparse matrix A
        // cout << "i: " << endl;
        (*pre_ptr).clear();
        (*pre_ptr)[i] = R;
        for (int t = 0; t <= T; t++) {
            // cout << "T: " << t << endl;
            (*next_ptr).clear();
            for (auto &item : (*pre_ptr)) {
                // update a_i
                int position, number;
                tie(position, number) = item;
                A[i][position] += pow(c, t) * pow(double(number) / double(R), 2); // update the sparse matrix A
                // choose the next position
                for (int k = 0; k < number; k++) {
                    auto sampled_in_neighbor = sample_in_neighbor(position, *g);
                    if (sampled_in_neighbor != -1) {
                        (*next_ptr)[sampled_in_neighbor] += 1;
                    }
                }
            }
            swap(pre_ptr, next_ptr);
        }
    }
    // cout << A << endl;
    // solve the linear system for D
    VectorXd *new_D_ptr, *current_D_ptr, new_D(n);
    new_D.setZero();
    D.setOnes();
    new_D_ptr = &new_D;
    current_D_ptr = &D;
    for (size_t l = 0; l < L; ++l) {
        (*new_D_ptr).setZero();
        for (int i = 0; i < n; i++) {
            double dot_sum = 0;
            for (auto &item: A[i]) {
                int j;
                double value;
                tie(j, value) = item;
                if (j != i) {
                    dot_sum += (*current_D_ptr)(j) * value;
                }
            }
            if (A[i].find(i) != A[i].end()) {
                // cout << i << " "  << dot_sum << " " << A.coeff(i,i) << endl;
                auto v = A[i][i];
                // cout <<  str(format(" i: %s, A[i][i]: %s dot_sum: %s ") % i % v % dot_sum) <<  endl;
                (*new_D_ptr)(i) = (1.0 - dot_sum) / v;
            }
        }
        swap(new_D_ptr, current_D_ptr);
    }
    // cout << (A * D) << endl;
}

void CloudWalker::Tstep_distribution(int i, MatrixXd &pos_dist) {
    pos_dist.setZero();
    /* start run R random walks */
    int current_pos;
//    for (int k = 0; k < R; k++) {
    for (int k = 0; k < R_prime; k++) {
        int t = 0;
        current_pos = i;
        while (t <= T) {
//            pos_dist(t, current_pos) += (1.0 / R);
            pos_dist(t, current_pos) += (1.0 / R_prime);
            auto sampled_in_neighbor = sample_in_neighbor(current_pos, *g);
            if (sampled_in_neighbor != -1) {
                current_pos = sampled_in_neighbor;
            } else {
                break; // there is no in-neighbor for current_pos
            }
            ++t;
        }
    }
}

#ifdef SINGLE_SOURCE
void CloudWalker::preprocess_F() {
    cout << "computing F" << endl;
    DirectedG::edge_iterator edge_it, edge_end;
    tie(edge_it, edge_end) = edges(*g);
    for (; edge_it != edge_end; ++edge_it) {
        auto s = source(*edge_it, *g);
        auto t = target(*edge_it, *g);
        F[s] += 1.0 / in_degree(t, *g);
    }
}

void CloudWalker::preprocess_hat_P() {
    cout << "computing P" << endl;
    DirectedG::edge_iterator edge_it, edge_end;
    tie(edge_it, edge_end) = edges(*g);
    for (; edge_it != edge_end; ++edge_it) {
        auto s = source(*edge_it, *g);
        auto t = target(*edge_it, *g);
        int in_deg_t = in_degree(t, *g);
        if (in_deg_t > 0) {
            hat_P.coeffRef(t, s) += (1.0 / in_deg_t) / F(s);
        }
    }
    // make hat_P accumulative among each column
    hat_P.makeCompressed();
    auto value_ptr = hat_P.valuePtr();
    auto inner_ptr = hat_P.innerIndexPtr();
    auto outer_ptr = hat_P.outerIndexPtr();
    for (int k = 0; k < hat_P.outerSize(); k++) {
        auto start_ptr = outer_ptr[k];
        auto end_ptr = outer_ptr[k + 1];
        auto col_nnz = end_ptr - start_ptr;
        if (col_nnz > 1) {
            for (int offset = 1; offset < col_nnz; offset++) {
                value_ptr[start_ptr + offset] += value_ptr[start_ptr + offset - 1];
            }
        }
    }
    hat_P.makeCompressed();
}

void CloudWalker::mcss(int i, VectorXd &r) {
    r.setZero();
    auto value_ptr = hat_P.valuePtr();
    auto inner_ptr = hat_P.innerIndexPtr();
    auto outer_ptr = hat_P.outerIndexPtr();

    // uniform generator
    std::default_random_engine generator, generator1;
    std::uniform_real_distribution<double> uniform_dist(0.0, 1.0);

    MatrixXd pos_dist(T + 1, n); // to store the distribution of random walks from i
    Tstep_distribution(i, pos_dist); // T-step random walk distribution
    // cout << "T step distribution: " << endl;
    // cout << pos_dist << endl;
    VectorXd v(n);
    for (size_t t = 0; t <= T; ++t) {
        v.setZero();
        // evaluate Pe_{i}
        v = pos_dist.row(t);
        v = v.cwiseProduct(D);// now v = DPe_{i}

        for (size_t tt = 0; tt < t; ++tt) {
            // compute P^{ Tt}*v
            VectorXd one_step_dist(n);
            one_step_dist.setZero();
            v = v.cwiseProduct(F);
            double w = v.sum();
            v = v / w;
            auto &u = v; // initial distribution
            // set up the distribution initial positions
            discrete_distribution<int> dist(u.data(), u.data() + u.size());
            for (size_t k = 0; k < R_prime; ++k) { // start random walks
                auto init_pos = dist(generator1); // sample a starting node
                // sample out-neighbor, too heavy
                if (out_degree(init_pos, *g) > 0) {
                    /* out-neighbor iterator for weights */
                    // auto out_start = hat_P.valuePtr() + hat_P.outerIndexPtr()[start_pos];
                    // auto out_end = hat_P.valuePtr() + hat_P.outerIndexPtr()[start_pos+1];

                    // discrete_distribution<int> out_dist(out_start, out_end);
                    // auto out_neighbor_index = out_dist(generator2); // sample a out-neighbor index
                    // auto out_neighbor = hat_P.innerIndexPtr()[hat_P.outerIndexPtr()[start_pos]+out_neighbor_index];
                    // sample an out_neighbor
                    double sample_value = uniform_dist(generator);
                    // cout << sample_value << endl;
                    int start_pos = outer_ptr[init_pos];
                    int end_pos = outer_ptr[init_pos + 1];
                    auto value_start_iter = value_ptr + start_pos;
                    auto value_end_iter = value_ptr + end_pos;

                    auto chosen_index_ptr = upper_bound(value_start_iter, value_end_iter, sample_value);
                    int offset = chosen_index_ptr - value_start_iter;
                    int out_neighbor = inner_ptr[start_pos + offset];

                    one_step_dist(out_neighbor) += (1.0 / R_prime);
                }
            }
            // cout << "one step distribution: " << endl;
            // cout << one_step_dist << endl;
            v = w * one_step_dist;
        }
        // cout << "v of iteration: " << t << endl;
        // cout << v << endl;
        r += pow(c, t) * v;
    }
    mem_size = getValue();
}
#endif

double CloudWalker::mcsp(int u, int v, MatrixXd &pos_dist_u, MatrixXd &pos_dist_v) {
    if (u == v) { return 1; }

    auto tmp_start = std::chrono::high_resolution_clock::now();

    Tstep_distribution(u, pos_dist_u); // T-step random walk distribution
    Tstep_distribution(v, pos_dist_v);

    double sim_score = 0.0;
    double cur_decay = 1.0;
    for (auto t = 0; t <= T; t++) {
        sim_score +=
                cur_decay * pos_dist_u.row(t).transpose().cwiseProduct(D).transpose() * pos_dist_v.row(t).transpose();
        cur_decay *= c;
    }
    auto tmp_end = std::chrono::high_resolution_clock::now();
    return sim_score;
}

double CloudWalker::mcsp(int u, int v) {
    MatrixXd pos_dist_u(T + 1, n);
    MatrixXd pos_dist_v(T + 1, n);

    // 2nd: query
    return mcsp(u, v, pos_dist_u, pos_dist_v);
}


//==================================================================================================
/*!
  @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_IS_NAN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NAN_HPP_INCLUDED

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

  /*!
    @ingroup group-predicates

    This function object returns @ref True or @ref False according x is nan or not.

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

    @par Note

     Using `is_nan(x)` is similar to `x != x`

    @par Example:

      @snippet is_nan.cpp is_nan

    @par Possible output:

      @snippet is_nan.txt is_nan

  **/
  as_logical_t<Value> is_nan(Value const& x);
} }
#endif

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

#endif