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

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/exception.hpp>

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

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/algorithms/detail/assign_values.hpp>

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

// Temporary / will be Strategy as template parameter
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

#include <boost/geometry/strategies/side_info.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Dimension, typename Segment, typename T>
static inline void segment_arrange(Segment const& s, T& s_1, T& s_2, bool& swapped)
{
    s_1 = get<0, Dimension>(s);
    s_2 = get<1, Dimension>(s);
    if (s_1 > s_2)
    {
        std::swap(s_1, s_2);
        swapped = true;
    }
}

template <std::size_t Index, typename Segment>
inline typename geometry::point_type<Segment>::type get_from_index(
            Segment const& segment)
{
    typedef typename geometry::point_type<Segment>::type point_type;
    point_type point;
    geometry::detail::assign::assign_point_from_index
        <
            Segment, point_type, Index, 0, dimension<Segment>::type::value
        >::apply(segment, point);
    return point;
}

}
#endif

/***
template <typename T>
inline std::string rdebug(T const& value)
{
    if (math::equals(value, 0)) return "'0'";
    if (math::equals(value, 1)) return "'1'";
    if (value < 0) return "<0";
    if (value > 1) return ">1";
    return "<0..1>";
}
***/

/*!
    \see http://mathworld.wolfram.com/Line-LineIntersection.html
 */
template <typename Policy, typename CalculationType = void>
struct relate_cartesian_segments
{
    typedef typename Policy::return_type return_type;
    typedef typename Policy::segment_type1 segment_type1;
    typedef typename Policy::segment_type2 segment_type2;

    //typedef typename point_type<segment_type1>::type point_type;
    //BOOST_CONCEPT_ASSERT( (concept::Point<point_type>) );

    BOOST_CONCEPT_ASSERT( (concept::ConstSegment<segment_type1>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstSegment<segment_type2>) );

    typedef typename select_calculation_type
        <segment_type1, segment_type2, CalculationType>::type coordinate_type;

    /// Relate segments a and b
    static inline return_type apply(segment_type1 const& a, segment_type2 const& b)
    {
        coordinate_type const dx_a = get<1, 0>(a) - get<0, 0>(a); // distance in x-dir
        coordinate_type const dx_b = get<1, 0>(b) - get<0, 0>(b);
        coordinate_type const dy_a = get<1, 1>(a) - get<0, 1>(a); // distance in y-dir
        coordinate_type const dy_b = get<1, 1>(b) - get<0, 1>(b);
        return apply(a, b, dx_a, dy_a, dx_b, dy_b);
    }

    // Relate segments a and b using precalculated differences.
    // This can save two or four subtractions in many cases
    static inline return_type apply(segment_type1 const& a, segment_type2 const& b,
            coordinate_type const& dx_a, coordinate_type const& dy_a,
            coordinate_type const& dx_b, coordinate_type const& dy_b)
    {
        typedef side::side_by_triangle<coordinate_type> side;
        side_info sides;

        coordinate_type const zero = 0;
        bool const a_is_point = math::equals(dx_a, zero) && math::equals(dy_a, zero);
        bool const b_is_point = math::equals(dx_b, zero) && math::equals(dy_b, zero);

        if(a_is_point && b_is_point)
        {
            if(math::equals(get<1,0>(a), get<1,0>(b)) && math::equals(get<1,1>(a), get<1,1>(b)))
            {
                 Policy::degenerate(a, true);
            }
            else
            {
                return Policy::disjoint();
            }
        }

        bool collinear_use_first = math::abs(dx_a) + math::abs(dx_b) >= math::abs(dy_a) + math::abs(dy_b);

        sides.set<0>
            (
                side::apply(detail::get_from_index<0>(b)
                    , detail::get_from_index<1>(b)
                    , detail::get_from_index<0>(a)),
                side::apply(detail::get_from_index<0>(b)
                    , detail::get_from_index<1>(b)
                    , detail::get_from_index<1>(a))
            );
        sides.set<1>
            (
                side::apply(detail::get_from_index<0>(a)
                    , detail::get_from_index<1>(a)
                    , detail::get_from_index<0>(b)),
                side::apply(detail::get_from_index<0>(a)
                    , detail::get_from_index<1>(a)
                    , detail::get_from_index<1>(b))
            );

        bool collinear = sides.collinear();

        robustness_verify_collinear(a, b, a_is_point, b_is_point, sides, collinear);
        robustness_verify_meeting(a, b, sides, collinear, collinear_use_first);

        if (sides.same<0>() || sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            if (robustness_verify_same_side(a, b, sides))
            {
                return Policy::disjoint();
            }
        }

        // Degenerate cases: segments of single point, lying on other segment, non disjoint
        if (a_is_point)
        {
            return Policy::degenerate(a, true);
        }
        if (b_is_point)
        {
            return Policy::degenerate(b, false);
        }

        typedef typename select_most_precise
            <
                coordinate_type, double
            >::type promoted_type;

        // r: ratio 0-1 where intersection divides A/B
        // (only calculated for non-collinear segments)
        promoted_type r;
        if (! collinear)
        {
            // Calculate determinants - Cramers rule
            coordinate_type const wx = get<0, 0>(a) - get<0, 0>(b);
            coordinate_type const wy = get<0, 1>(a) - get<0, 1>(b);
            coordinate_type const d = geometry::detail::determinant<coordinate_type>(dx_a, dy_a, dx_b, dy_b);
            coordinate_type const da = geometry::detail::determinant<coordinate_type>(dx_b, dy_b, wx, wy);

            coordinate_type const zero = coordinate_type();
            if (math::equals(d, zero))
            {
                // This is still a collinear case (because of FP imprecision this can occur here)
                // sides.debug();
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;
            }
            else
            {
                r = promoted_type(da) / promoted_type(d);

                if (! robustness_verify_r(a, b, r))
                {
                    return Policy::disjoint();
                }

                robustness_handle_meeting(a, b, sides, dx_a, dy_a, wx, wy, d, r);

                if (robustness_verify_disjoint_at_one_collinear(a, b, sides))
                {
                    return Policy::disjoint();
                }

            }
        }

        if(collinear)
        {
            if (collinear_use_first)
            {
                return relate_collinear<0>(a, b);
            }
            else
            {
                // Y direction contains larger segments (maybe dx is zero)
                return relate_collinear<1>(a, b);
            }
        }

        return Policy::segments_intersect(sides, r,
            dx_a, dy_a, dx_b, dy_b,
            a, b);
    }

private :

    // Ratio should lie between 0 and 1
    // Also these three conditions might be of FP imprecision, the segments were actually (nearly) collinear
    template <typename T>
    static inline bool robustness_verify_r(
                segment_type1 const& a, segment_type2 const& b,
                T& r)
    {
        T const zero = 0;
        T const one = 1;
        if (r < zero || r > one)
        {
            if (verify_disjoint<0>(a, b) || verify_disjoint<1>(a, b))
            {
                // Can still be disjoint (even if not one is left or right from another)
                // This is e.g. in case #snake4 of buffer test.
                return false;
            }

            //std::cout << "ROBUSTNESS: correction of r " << r << std::endl;
            // sides.debug();

            // ROBUSTNESS: the r value can in epsilon-cases much larger than 1, while (with perfect arithmetic)
            // it should be one. It can be 1.14 or even 1.98049 or 2 (while still intersecting)

            // If segments are crossing (we can see that with the sides)
            // and one is inside the other, there must be an intersection point.
            // We correct for that.
            // This is (only) in case #ggl_list_20110820_christophe in unit tests

            // If segments are touching (two sides zero), of course they should intersect
            // This is (only) in case #buffer_rt_i in the unit tests)

            // If one touches in the middle, they also should intersect (#buffer_rt_j)

            // Note that even for ttmath r is occasionally > 1, e.g. 1.0000000000000000000000036191231203575

            if (r > one)
            {
                r = one;
            }
            else if (r < zero)
            {
                r = zero;
            }
        }
        return true;
    }

    static inline void robustness_verify_collinear(
                segment_type1 const& a, segment_type2 const& b,
                bool a_is_point, bool b_is_point,
                side_info& sides,
                bool& collinear)
    {
        if ((sides.zero<0>() && ! b_is_point && ! sides.zero<1>()) || (sides.zero<1>() && ! a_is_point && ! sides.zero<0>()))
        {
            // If one of the segments is collinear, the other must be as well.
            // So handle it as collinear.
            // (In float/double epsilon margins it can easily occur that one or two of them are -1/1)
            // sides.debug();
            sides.set<0>(0,0);
            sides.set<1>(0,0);
            collinear = true;
        }
    }

    static inline void robustness_verify_meeting(
                segment_type1 const& a, segment_type2 const& b,
                side_info& sides,
                bool& collinear, bool& collinear_use_first)
    {
        if (sides.meeting())
        {
            // If two segments meet each other at their segment-points, two sides are zero,
            // the other two are not (unless collinear but we don't mean those here).
            // However, in near-epsilon ranges it can happen that two sides are zero
            // but they do not meet at their segment-points.
            // In that case they are nearly collinear and handled as such.
            if (! point_equals
                    (
                        select(sides.zero_index<0>(), a),
                        select(sides.zero_index<1>(), b)
                    )
                )
            {
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;

                if (collinear_use_first && analyse_equal<0>(a, b))
                {
                    collinear_use_first = false;
                }
                else if (! collinear_use_first && analyse_equal<1>(a, b))
                {
                    collinear_use_first = true;
                }

            }
        }
    }

    // Verifies and if necessary correct missed touch because of robustness
    // This is the case at multi_polygon_buffer unittest #rt_m
    static inline bool robustness_verify_same_side(
                segment_type1 const& a, segment_type2 const& b,
                side_info& sides)
    {
        int corrected = 0;
        if (sides.one_touching<0>())
        {
            if (point_equals(
                        select(sides.zero_index<0>(), a),
                        select(0, b)
                    ))
            {
                sides.correct_to_zero<1, 0>();
                corrected = 1;
            }
            if (point_equals
                    (
                        select(sides.zero_index<0>(), a),
                        select(1, b)
                    ))
            {
                sides.correct_to_zero<1, 1>();
                corrected = 2;
            }
        }
        else if (sides.one_touching<1>())
        {
            if (point_equals(
                        select(sides.zero_index<1>(), b),
                        select(0, a)
                    ))
            {
                sides.correct_to_zero<0, 0>();
                corrected = 3;
            }
            if (point_equals
                    (
                        select(sides.zero_index<1>(), b),
                        select(1, a)
                    ))
            {
                sides.correct_to_zero<0, 1>();
                corrected = 4;
            }
        }

        return corrected == 0;
    }

    static inline bool robustness_verify_disjoint_at_one_collinear(
                segment_type1 const& a, segment_type2 const& b,
                side_info const& sides)
    {
        if (sides.one_of_all_zero())
        {
            if (verify_disjoint<0>(a, b) || verify_disjoint<1>(a, b))
            {
                return true;
            }
        }
        return false;
    }

    // If r is one, or zero, segments should meet and their endpoints.
    // Robustness issue: check if this is really the case.
    // It turns out to be no problem, see buffer test #rt_s1 (and there are many cases generated)
    // It generates an "ends in the middle" situation which is correct.
    template <typename T, typename R>
    static inline void robustness_handle_meeting(segment_type1 const& a, segment_type2 const& b,
                side_info& sides,
                T const& dx_a, T const& dy_a, T const& wx, T const& wy,
                T const& d, R const& r)
    {
        return;

        T const db = geometry::detail::determinant<T>(dx_a, dy_a, wx, wy);

        R const zero = 0;
        R const one = 1;
        if (math::equals(r, zero) || math::equals(r, one))
        {
            R rb = db / d;
            if (rb <= 0 || rb >= 1 || math::equals(rb, 0) || math::equals(rb, 1))
            {
                if (sides.one_zero<0>() && ! sides.one_zero<1>()) // or vice versa
                {
#if defined(BOOST_GEOMETRY_COUNT_INTERSECTION_EQUAL)
                    extern int g_count_intersection_equal;
                    g_count_intersection_equal++;
#endif
                    sides.debug();
                    std::cout << "E r=" << r << " r.b=" << rb << " ";
                }
            }
        }
    }

    template <std::size_t Dimension>
    static inline bool verify_disjoint(segment_type1 const& a,
                    segment_type2 const& b)
    {
        coordinate_type a_1, a_2, b_1, b_2;
        bool a_swapped = false, b_swapped = false;
        detail::segment_arrange<Dimension>(a, a_1, a_2, a_swapped);
        detail::segment_arrange<Dimension>(b, b_1, b_2, b_swapped);
        return math::smaller(a_2, b_1) || math::larger(a_1, b_2);
    }

    template <typename Segment>
    static inline typename point_type<Segment>::type select(int index, Segment const& segment)
    {
        return index == 0
            ? detail::get_from_index<0>(segment)
            : detail::get_from_index<1>(segment)
            ;
    }

    // We cannot use geometry::equals here. Besides that this will be changed
    // to compare segment-coordinate-values directly (not necessary to retrieve point first)
    template <typename Point1, typename Point2>
    static inline bool point_equals(Point1 const& point1, Point2 const& point2)
    {
        return math::equals(get<0>(point1), get<0>(point2))
            && math::equals(get<1>(point1), get<1>(point2))
            ;
    }

    // We cannot use geometry::equals here. Besides that this will be changed
    // to compare segment-coordinate-values directly (not necessary to retrieve point first)
    template <typename Point1, typename Point2>
    static inline bool point_equality(Point1 const& point1, Point2 const& point2,
                    bool& equals_0, bool& equals_1)
    {
        equals_0 = math::equals(get<0>(point1), get<0>(point2));
        equals_1 = math::equals(get<1>(point1), get<1>(point2));
        return equals_0 && equals_1;
    }

    template <std::size_t Dimension>
    static inline bool analyse_equal(segment_type1 const& a, segment_type2 const& b)
    {
        coordinate_type const a_1 = geometry::get<0, Dimension>(a);
        coordinate_type const a_2 = geometry::get<1, Dimension>(a);
        coordinate_type const b_1 = geometry::get<0, Dimension>(b);
        coordinate_type const b_2 = geometry::get<1, Dimension>(b);
        return math::equals(a_1, b_1)
            || math::equals(a_2, b_1)
            || math::equals(a_1, b_2)
            || math::equals(a_2, b_2)
            ;
    }

    template <std::size_t Dimension>
    static inline return_type relate_collinear(segment_type1 const& a,
                                               segment_type2 const& b)
    {
        coordinate_type a_1, a_2, b_1, b_2;
        bool a_swapped = false, b_swapped = false;
        detail::segment_arrange<Dimension>(a, a_1, a_2, a_swapped);
        detail::segment_arrange<Dimension>(b, b_1, b_2, b_swapped);
        if (math::smaller(a_2, b_1) || math::larger(a_1, b_2))
        //if (a_2 < b_1 || a_1 > b_2)
        {
            return Policy::disjoint();
        }
        return relate_collinear(a, b, a_1, a_2, b_1, b_2, a_swapped, b_swapped);
    }

    /// Relate segments known collinear
    static inline return_type relate_collinear(segment_type1 const& a
            , segment_type2 const& b
            , coordinate_type a_1, coordinate_type a_2
            , coordinate_type b_1, coordinate_type b_2
            , bool a_swapped, bool b_swapped)
    {
        // All ca. 150 lines are about collinear rays
        // The intersections, if any, are always boundary points of the segments. No need to calculate anything.
        // However we want to find out HOW they intersect, there are many cases.
        // Most sources only provide the intersection (above) or that there is a collinearity (but not the points)
        // or some spare sources give the intersection points (calculated) but not how they align.
        // This source tries to give everything and still be efficient.
        // It is therefore (and because of the extensive clarification comments) rather long...

        // \see http://mpa.itc.it/radim/g50history/CMP/4.2.1-CERL-beta-libes/file475.txt
        // \see http://docs.codehaus.org/display/GEOTDOC/Point+Set+Theory+and+the+DE-9IM+Matrix
        // \see http://mathworld.wolfram.com/Line-LineIntersection.html

        // Because of collinearity the case is now one-dimensional and can be checked using intervals
        // This function is called either horizontally or vertically
        // We get then two intervals:
        // a_1-------------a_2 where a_1 < a_2
        // b_1-------------b_2 where b_1 < b_2
        // In all figures below a_1/a_2 denotes arranged intervals, a1-a2 or a2-a1 are still unarranged

        // Handle "equal", in polygon neighbourhood comparisons a common case

        bool const opposite = a_swapped ^ b_swapped;
        bool const both_swapped = a_swapped && b_swapped;

        // Check if segments are equal or opposite equal...
        bool const swapped_a1_eq_b1 = math::equals(a_1, b_1);
        bool const swapped_a2_eq_b2 = math::equals(a_2, b_2);

        if (swapped_a1_eq_b1 && swapped_a2_eq_b2)
        {
            return Policy::segment_equal(a, opposite);
        }

        bool const swapped_a2_eq_b1 = math::equals(a_2, b_1);
        bool const swapped_a1_eq_b2 = math::equals(a_1, b_2);

        bool const a1_eq_b1 = both_swapped ? swapped_a2_eq_b2 : a_swapped ? swapped_a2_eq_b1 : b_swapped ? swapped_a1_eq_b2 : swapped_a1_eq_b1;
        bool const a2_eq_b2 = both_swapped ? swapped_a1_eq_b1 : a_swapped ? swapped_a1_eq_b2 : b_swapped ? swapped_a2_eq_b1 : swapped_a2_eq_b2;

        bool const a1_eq_b2 = both_swapped ? swapped_a2_eq_b1 : a_swapped ? swapped_a2_eq_b2 : b_swapped ? swapped_a1_eq_b1 : swapped_a1_eq_b2;
        bool const a2_eq_b1 = both_swapped ? swapped_a1_eq_b2 : a_swapped ? swapped_a1_eq_b1 : b_swapped ? swapped_a2_eq_b2 : swapped_a2_eq_b1;

        // The rest below will return one or two intersections.
        // The delegated class can decide which is the intersection point, or two, build the Intersection Matrix (IM)
        // For IM it is important to know which relates to which. So this information is given,
        // without performance penalties to intersection calculation

        bool const has_common_points = swapped_a1_eq_b1 || swapped_a1_eq_b2 || swapped_a2_eq_b1 || swapped_a2_eq_b2;

        // "Touch" -> one intersection point -> one but not two common points
        // -------->             A (or B)
        //         <----------   B (or A)
        //        a_2==b_1         (b_2==a_1 or a_2==b1)

        // The check a_2/b_1 is necessary because it excludes cases like
        // ------->
        //     --->
        // ... which are handled lateron

        // Corresponds to 4 cases, of which the equal points are determined above
        // #1: a1---->a2 b1--->b2   (a arrives at b's border)
        // #2: a2<----a1 b2<---b1   (b arrives at a's border)
        // #3: a1---->a2 b2<---b1   (both arrive at each others border)
        // #4: a2<----a1 b1--->b2   (no arrival at all)
        // Where the arranged forms have two forms:
        //    a_1-----a_2/b_1-------b_2 or reverse (B left of A)
        if ((swapped_a2_eq_b1 || swapped_a1_eq_b2) && ! swapped_a1_eq_b1 && ! swapped_a2_eq_b2)
        {
            if (a2_eq_b1) return Policy::collinear_touch(get<1, 0>(a), get<1, 1>(a), 0, -1);
            if (a1_eq_b2) return Policy::collinear_touch(get<0, 0>(a), get<0, 1>(a), -1, 0);
            if (a2_eq_b2) return Policy::collinear_touch(get<1, 0>(a), get<1, 1>(a), 0, 0);
            if (a1_eq_b1) return Policy::collinear_touch(get<0, 0>(a), get<0, 1>(a), -1, -1);
        }

        // "Touch/within" -> there are common points and also an intersection of interiors:
        // Corresponds to many cases:
        // #1a: a1------->a2  #1b:        a1-->a2
        //          b1--->b2         b1------->b2
        // #2a: a2<-------a1  #2b:        a2<--a1
        //          b1--->b2         b1------->b2
        // #3a: a1------->a2  #3b:        a1-->a2
        //          b2<---b1         b2<-------b1
        // #4a: a2<-------a1  #4b:        a2<--a1
        //          b2<---b1         b2<-------b1

        // Note: next cases are similar and handled by the code
        // #4c: a1--->a2
        //      b1-------->b2
        // #4d: a1-------->a2
        //      b1-->b2

        // For case 1-4: a_1 < (b_1 or b_2) < a_2, two intersections are equal to segment B
        // For case 5-8: b_1 < (a_1 or a_2) < b_2, two intersections are equal to segment A
        if (has_common_points)
        {
            // Either A is in B, or B is in A, or (in case of robustness/equals)
            // both are true, see below
            bool a_in_b = (b_1 < a_1 && a_1 < b_2) || (b_1 < a_2 && a_2 < b_2);
            bool b_in_a = (a_1 < b_1 && b_1 < a_2) || (a_1 < b_2 && b_2 < a_2);

            if (a_in_b && b_in_a)
            {
                // testcase "ggl_list_20110306_javier"
                // In robustness it can occur that a point of A is inside B AND a point of B is inside A,
                // still while has_common_points is true (so one point equals the other).
                // If that is the case we select on length.
                coordinate_type const length_a = geometry::math::abs(a_1 - a_2);
                coordinate_type const length_b = geometry::math::abs(b_1 - b_2);
                if (length_a > length_b)
                {
                    a_in_b = false;
                }
                else
                {
                    b_in_a = false;
                }
            }

            int const arrival_a = a_in_b ? 1 : -1;
            if (a2_eq_b2) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, 0, 0, false);
            if (a1_eq_b2) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, arrival_a, 0, true);
            if (a2_eq_b1) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, 0, -arrival_a, true);
            if (a1_eq_b1) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, arrival_a, -arrival_a, false);
        }

        // "Inside", a completely within b or b completely within a
        // 2 cases:
        // case 1:
        //        a_1---a_2        -> take A's points as intersection points
        //   b_1------------b_2
        // case 2:
        //   a_1------------a_2
        //       b_1---b_2         -> take B's points
        if (a_1 > b_1 && a_2 < b_2)
        {
            // A within B
            return Policy::collinear_a_in_b(a, opposite);
        }
        if (b_1 > a_1 && b_2 < a_2)
        {
            // B within A
            return Policy::collinear_b_in_a(b, opposite);
        }

        /*

        Now that all cases with equal,touch,inside,disjoint,
        degenerate are handled the only thing left is an overlap

        Either a1 is between b1,b2
        or a2 is between b1,b2 (a2 arrives)

        Next table gives an overview.
        The IP's are ordered following the line A1->A2

             |                                 |
             |          a_2 in between         |       a_1 in between
             |                                 |
        -----+---------------------------------+--------------------------
             |   a1--------->a2                |       a1--------->a2
             |          b1----->b2             |   b1----->b2
             |   (b1,a2), a arrives            |   (a1,b2), b arrives
             |                                 |
        -----+---------------------------------+--------------------------
        a sw.|   a2<---------a1*               |       a2<---------a1*
             |           b1----->b2            |   b1----->b2
             |   (a1,b1), no arrival           |   (b2,a2), a and b arrive
             |                                 |
        -----+---------------------------------+--------------------------
             |   a1--------->a2                |       a1--------->a2
        b sw.|           b2<-----b1            |   b2<-----b1
             |   (b2,a2), a and b arrive       |   (a1,b1), no arrival
             |                                 |
        -----+---------------------------------+--------------------------
        a sw.|    a2<---------a1*              |       a2<---------a1*
        b sw.|            b2<-----b1           |   b2<-----b1
             |   (a1,b2), b arrives            |   (b1,a2), a arrives
             |                                 |
        -----+---------------------------------+--------------------------
        * Note that a_1 < a_2, and a1 <> a_1; if a is swapped,
          the picture might seem wrong but it (supposed to be) is right.
        */

        if (b_1 < a_2 && a_2 < b_2)
        {
            // Left column, from bottom to top
            return
                both_swapped ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<1, 0>(b), get<1, 1>(b), -1,  1, opposite)
                : b_swapped  ? Policy::collinear_overlaps(get<1, 0>(b), get<1, 1>(b), get<1, 0>(a), get<1, 1>(a),  1,  1, opposite)
                : a_swapped  ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<0, 0>(b), get<0, 1>(b), -1, -1, opposite)
                :              Policy::collinear_overlaps(get<0, 0>(b), get<0, 1>(b), get<1, 0>(a), get<1, 1>(a),  1, -1, opposite)
                ;
        }
        if (b_1 < a_1 && a_1 < b_2)
        {
            // Right column, from bottom to top
            return
                both_swapped ? Policy::collinear_overlaps(get<0, 0>(b), get<0, 1>(b), get<1, 0>(a), get<1, 1>(a),  1, -1, opposite)
                : b_swapped  ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<0, 0>(b), get<0, 1>(b), -1, -1, opposite)
                : a_swapped  ? Policy::collinear_overlaps(get<1, 0>(b), get<1, 1>(b), get<1, 0>(a), get<1, 1>(a),  1,  1, opposite)
                :              Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<1, 0>(b), get<1, 1>(b), -1,  1, opposite)
                ;
        }
        // Nothing should goes through. If any we have made an error
        // std::cout << "Robustness issue, non-logical behaviour" << std::endl;
        return Policy::error("Robustness issue, non-logical behaviour");
    }
};

}} // namespace strategy::intersection

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP


#include <iostream>
#include <mtl/dense1D.h>
#include <mtl/mtl.h>

/*
  example output:

  60

  */

int
main()
{
  using namespace mtl;
  //begin
  dense1D<double> x(10, 2), y(10, 3);
  double s = dot(x, y);
  //end
  std::cout << s << std::endl;
  return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/CQuaternion.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/math/homog_matrices.h>  // homogeneousMatrixInverse()

#include <Eigen/Dense>

using namespace mrpt::math;

static_assert(std::is_trivially_copyable_v<TPose3D>);

TPose3D::TPose3D(const TPoint2D& p)
	: x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
{
}
TPose3D::TPose3D(const TPose2D& p)
	: x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
{
}
TPose3D::TPose3D(const TPoint3D& p)
	: x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
{
}
void TPose3D::asString(std::string& s) const
{
	s = mrpt::format(
		"[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
		RAD2DEG(roll));
}
void TPose3D::getAsQuaternion(
	mrpt::math::CQuaternion<double>& q,
	mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 3>> out_dq_dr) const
{
	// See:
	// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
	const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
	const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);

	const double ccc = cr * cp * cy;
	const double ccs = cr * cp * sy;
	const double css = cr * sp * sy;
	const double sss = sr * sp * sy;
	const double scc = sr * cp * cy;
	const double ssc = sr * sp * cy;
	const double csc = cr * sp * cy;
	const double scs = sr * cp * sy;

	q.w(ccc + sss);
	q.x(scc - css);
	q.y(csc + scs);
	q.z(ccs - ssc);

	// Compute 4x3 Jacobian: for details, see technical report:
	//   Parameterizations of SE(3) transformations: equivalences, compositions
	//   and uncertainty, J.L. Blanco (2010).
	//   https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
	if (out_dq_dr)
	{
		alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[4 * 3] = {
			-0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
			-0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
			0.5 * q[1],	 0.5 * (ccc - sss),	 0.5 * q[3],
			0.5 * q[0],	 0.5 * (-css - scc), -0.5 * q[2]};
		out_dq_dr.value().get().loadFromArray(nums);
	}
}
void TPose3D::composePoint(const TPoint3D& l, TPoint3D& g) const
{
	CMatrixDouble33 R;
	this->getRotationMatrix(R);
	TPoint3D res;
	res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
	res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
	res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;

	g = res;
}
TPoint3D TPose3D::composePoint(const TPoint3D& l) const
{
	TPoint3D g;
	composePoint(l, g);
	return g;
}

void TPose3D::inverseComposePoint(const TPoint3D& g, TPoint3D& l) const
{
	CMatrixDouble44 H;
	this->getInverseHomogeneousMatrix(H);
	TPoint3D res;
	res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
	res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
	res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);

	l = res;
}
TPoint3D TPose3D::inverseComposePoint(const TPoint3D& g) const
{
	TPoint3D l;
	inverseComposePoint(g, l);
	return l;
}

void TPose3D::getRotationMatrix(mrpt::math::CMatrixDouble33& R) const
{
	const double cy = cos(yaw);
	const double sy = sin(yaw);
	const double cp = cos(pitch);
	const double sp = sin(pitch);
	const double cr = cos(roll);
	const double sr = sin(roll);

	alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double rot_vals[] = {
		cy * cp,
		cy * sp * sr - sy * cr,
		cy * sp * cr + sy * sr,
		sy * cp,
		sy * sp * sr + cy * cr,
		sy * sp * cr - cy * sr,
		-sp,
		cp * sr,
		cp * cr};
	R.loadFromArray(rot_vals);
}
void TPose3D::SO3_to_yaw_pitch_roll(
	const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
	double& roll)
{
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());

	// Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
	pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));

	// Roll:
	if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
		10 * std::numeric_limits<double>::epsilon())
	{
		// Gimbal lock between yaw and roll. This one is arbitrarily forced to
		// be zero.
		// Check
		// https://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
		// If cos(pitch)==0, the homogeneous matrix is:
		// When sin(pitch)==1:
		//  /0  cysr-sycr cycr+sysr x\   /0  sin(r-y) cos(r-y)  x\.
		//  |0  sysr+cycr sycr-cysr y| = |0  cos(r-y) -sin(r-y) y|
		//  |-1     0         0     z|   |-1    0         0     z|
		//  \0      0         0     1/   \0     0         0     1/
		//
		// And when sin(pitch)=-1:
		//  /0 -cysr-sycr -cycr+sysr x\   /0 -sin(r+y) -cos(r+y) x\.
		//  |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y)  -sin(r+y) y|
		//  |1      0          0     z|   |1    0          0     z|
		//  \0      0          0     1/   \0    0          0     1/
		//
		// Both cases are in a "gimbal lock" status. This happens because pitch
		// is vertical.

		roll = 0.0;
		if (pitch > 0) yaw = atan2(R(1, 2), R(0, 2));
		else
			yaw = atan2(-R(1, 2), -R(0, 2));
	}
	else
	{
		roll = atan2(R(2, 1), R(2, 2));
		// Yaw:
		yaw = atan2(R(1, 0), R(0, 0));
	}
}

void TPose3D::fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44& HG)
{
	SO3_to_yaw_pitch_roll(
		CMatrixDouble33(HG.blockCopy<3, 3>(0, 0)), yaw, pitch, roll);
	x = HG(0, 3);
	y = HG(1, 3);
	z = HG(2, 3);
}
void TPose3D::composePose(const TPose3D other, TPose3D& result) const
{
	CMatrixDouble44 me_H, o_H;
	this->getHomogeneousMatrix(me_H);
	other.getHomogeneousMatrix(o_H);
	result.fromHomogeneousMatrix(
		CMatrixDouble44(me_H.asEigen() * o_H.asEigen()));
}
void TPose3D::getHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const
{
	CMatrixDouble33 R;
	getRotationMatrix(R);
	HG.block<3, 3>(0, 0) = R.asEigen();
	HG(0, 3) = x;
	HG(1, 3) = y;
	HG(2, 3) = z;
	HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
	HG(3, 3) = 1.;
}
void TPose3D::getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const
{  // Get current HM & inverse in-place:
	this->getHomogeneousMatrix(HG);
	mrpt::math::homogeneousMatrixInverse(HG);
}

void TPose3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION_FMT(
			"Malformed expression in ::fromString, s=\"%s\"", s.c_str());
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	x = m(0, 0);
	y = m(0, 1);
	z = m(0, 2);
	yaw = DEG2RAD(m(0, 3));
	pitch = DEG2RAD(m(0, 4));
	roll = DEG2RAD(m(0, 5));
}

TPose3D mrpt::math::operator-(const TPose3D& p)
{
	CMatrixDouble44 H;
	p.getInverseHomogeneousMatrix(H);
	TPose3D ret;
	ret.fromHomogeneousMatrix(H);
	return ret;
}
TPose3D mrpt::math::operator-(const TPose3D& b, const TPose3D& a)
{
	// b - a = A^{-1} * B
	CMatrixDouble44 Hainv, Hb;
	a.getInverseHomogeneousMatrix(Hainv);
	b.getHomogeneousMatrix(Hb);
	TPose3D ret;
	ret.fromHomogeneousMatrix(CMatrixDouble44(Hainv.asEigen() * Hb.asEigen()));
	return ret;
}


#ifndef PYBNESIAN_UTIL_COMBINATIONS_HPP
#define PYBNESIAN_UTIL_COMBINATIONS_HPP

#include <vector>
#include <unordered_set>
#include <boost/math/special_functions/binomial.hpp>
#include <util/parameter_traits.hpp>

namespace util {

template <typename T>
class Combinations {
public:
    Combinations() = default;

    // TODO: Check m_k > fixed.size().
    template <typename Iter, util::enable_if_iterator_t<Iter, int> = 0>
    Combinations(Iter begin, Iter end, int k)
        : m_elements(begin, end),
          m_fixed(),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {}

    template <typename Iter,
              typename IterFixed,
              util::enable_if_iterator_t<Iter, int> = 0,
              util::enable_if_iterator_t<IterFixed, int> = 0>
    Combinations(Iter begin, Iter end, IterFixed begin_fixed, IterFixed end_fixed, int k)
        : m_elements(begin, end),
          m_fixed(begin_fixed, end_fixed),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {
        static_assert(std::is_same_v<typename std::iterator_traits<Iter>::value_type,
                                     typename std::iterator_traits<IterFixed>::value_type>,
                      "The type of fixed and movable elements should be the same.");
    }

    template <typename V, util::enable_if_vector_of_type_t<V, T, int> = 0>
    Combinations(V elements, int k)
        : m_elements(elements),
          m_fixed(),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {}

    template <typename V,
              typename V2,
              util::enable_if_vector_of_type_t<V, T, int> = 0,
              util::enable_if_vector_of_type_t<V2, T, int> = 0>
    Combinations(V elements, V2 fixed, int k)
        : m_elements(elements),
          m_fixed(fixed),
          m_k(k),
          m_num_combinations(
              std::round(boost::math::binomial_coefficient<double>(m_elements.size(), m_k - m_fixed.size()))) {}

    class combination_iterator {
    public:
        using iterator_category = std::input_iterator_tag;
        using value_type = std::vector<T>;
        using difference_type = int;
        using pointer = std::vector<T>*;
        using reference = std::vector<T>&;

        combination_iterator() = default;

        combination_iterator(const Combinations<T>* self, int idx) : m_self(self), m_subset(), m_indices(), m_idx(idx) {
            m_subset.reserve(m_self->m_k);

            for (size_t i = 0; i < m_self->m_fixed.size(); ++i) {
                m_subset.push_back(m_self->m_fixed[i]);
            }

            auto p = m_self->m_k - m_self->m_fixed.size();
            m_indices.reserve(p);

            for (size_t i = 0; i < p; ++i) {
                m_subset.push_back(m_self->m_elements[i]);
                m_indices.push_back(i);
            }
        }

        combination_iterator(const combination_iterator& other)
            : m_self(other.m_self), m_subset(other.m_subset), m_indices(other.m_indices), m_idx(other.m_idx) {}

        combination_iterator& operator=(const combination_iterator& other) {
            m_self = other.m_self;
            m_subset = other.m_subset;
            m_indices = other.m_indices;
            m_idx = other.m_idx;
            return *this;
        }

        void next_subset() {
            int offset = m_self->m_fixed.size();
            int p = m_self->m_k - offset;

            for (int i = p - 1; i >= 0; --i) {
                auto k = i + offset;
                auto max_index = m_self->m_elements.size() - p + i;

                if (m_indices[i] < max_index) {
                    ++m_indices[i];
                    m_subset[k] = m_self->m_elements[m_indices[i]];

                    for (int j = i + 1; j < p; ++j) {
                        m_indices[j] = m_indices[j - 1] + 1;
                        m_subset[j + offset] = m_self->m_elements[m_indices[j]];
                    }

                    break;
                }
            }
        }

        combination_iterator& operator++() {
            ++m_idx;
            next_subset();
            return *this;
        }

        combination_iterator operator++(int) {
            combination_iterator return_it(*this);
            ++m_idx;
            next_subset();
            return return_it;
        }

        reference operator*() { return m_subset; }

        pointer operator->() { return &m_subset; }

        bool operator==(const combination_iterator& rhs) { return (m_idx == rhs.m_idx) && (m_self == rhs.m_self); }

        bool operator!=(const combination_iterator& rhs) { return !(*this == rhs); }

    private:
        const Combinations<T>* m_self;
        std::vector<T> m_subset;
        std::vector<size_t> m_indices;
        int m_idx;
    };

    combination_iterator begin() const { return combination_iterator(this, 0); }
    combination_iterator end() const { return combination_iterator(this, num_combinations()); }

    int num_combinations() const { return m_num_combinations; }

private:
    std::vector<T> m_elements;
    std::vector<T> m_fixed;
    int m_k;
    int m_num_combinations;
};

template <typename Iter>
Combinations(Iter, Iter, int) -> Combinations<typename std::iterator_traits<Iter>::value_type>;
template <typename Iter, typename IterFixed>
Combinations(Iter, Iter, IterFixed, IterFixed, int) -> Combinations<typename std::iterator_traits<Iter>::value_type>;
template <typename V>
Combinations(V, int) -> Combinations<typename V::value_type>;
template <typename V, typename V2>
Combinations(V, V2, int) -> Combinations<typename V::value_type>;

template <typename T>
class Combinations2Sets {
public:
    template <typename Iter,
              typename Iter2,
              util::enable_if_iterator_t<Iter, int> = 0,
              util::enable_if_iterator_t<Iter2, int> = 0>
    Combinations2Sets(Iter begin_set1, Iter end_set1, Iter2 begin_set2, Iter2 end_set2, int k)
        : Combinations2Sets(std::vector(begin_set1, end_set1), std::vector(begin_set2, end_set2), k) {
        static_assert(std::is_same_v<typename std::iterator_traits<Iter>::value_type,
                                     typename std::iterator_traits<Iter2>::value_type>,
                      "The elements of both sets should be of the same type");
    }

    template <typename V,
              typename V2,
              util::enable_if_vector_of_type_t<V, T, int> = 0,
              util::enable_if_vector_of_type_t<V2, T, int> = 0>
    Combinations2Sets(V v1, V2 v2, int k)
        : m_comb1(), m_comb2(), m_comb2_valid_combinations(), m_num_combinations(-1), m_k(k) {
        std::sort(v1.begin(), v1.end());
        std::sort(v2.begin(), v2.end());

        std::unordered_set<T> common_elements;
        std::set_intersection(
            v1.begin(), v1.end(), v2.begin(), v2.end(), std::inserter(common_elements, common_elements.end()));

        m_comb1 = Combinations<T>(std::move(v1), m_k);
        if (static_cast<int>(common_elements.size()) < k) {
            m_comb2 = Combinations<T>(std::move(v2), m_k);
            m_comb2_valid_combinations = m_comb2.num_combinations();
        } else {
            for (size_t i = 0, common_start = v2.size() - common_elements.size(); i < common_start; ++i) {
                if (common_elements.count(v2[i]) > 0) {
                    for (size_t j = v2.size() - 1; j >= common_start; --j) {
                        if (common_elements.count(v2[j]) == 0) {
                            std::swap(v2[i], v2[j]);
                        }
                    }
                }
            }

            m_comb2 = Combinations<T>(std::move(v2), m_k);
            m_comb2_valid_combinations =
                m_comb2.num_combinations() -
                std::round(boost::math::binomial_coefficient<double>(common_elements.size(), m_k));
        }

        m_num_combinations = m_comb1.num_combinations() + m_comb2_valid_combinations;
    }

    class combination2set_iterator {
    public:
        using iterator_category = std::input_iterator_tag;
        using value_type = std::vector<T>;
        using difference_type = int;
        using pointer = std::vector<T>*;
        using reference = std::vector<T>&;

        combination2set_iterator(const Combinations2Sets<T>& self, int idx) : m_self(self), it() {
            if (idx < m_self.m_comb1.num_combinations() || m_self.m_comb2_valid_combinations == 0) {
                it = typename Combinations<T>::combination_iterator(&m_self.m_comb1, idx);
            } else {
                it = typename Combinations<T>::combination_iterator(&m_self.m_comb2,
                                                                    idx - m_self.m_comb1.num_combinations());
            }
        }

        combination2set_iterator& operator++() {
            ++it;
            if (it == m_self.m_comb1.end() && m_self.m_comb2_valid_combinations > 0) {
                it = m_self.m_comb2.begin();
            }
            return *this;
        }

        combination2set_iterator operator++(int) {
            combination2set_iterator return_it(*this);
            ++it;
            if (it == m_self.m_comb1.end() && m_self.m_comb2_valid_combinations > 0) {
                it = m_self.m_comb2.begin();
            }
            return return_it;
        }

        reference operator*() { return *it; }

        pointer operator->() { return &it.m_subset; }

        bool operator==(const combination2set_iterator& rhs) { return it == rhs.it; }

        bool operator!=(const combination2set_iterator& rhs) { return !(*this == rhs); }

    private:
        const Combinations2Sets<T>& m_self;
        typename Combinations<T>::combination_iterator it;
    };

    combination2set_iterator begin() { return combination2set_iterator(*this, 0); }
    combination2set_iterator end() { return combination2set_iterator(*this, num_combinations()); }

    int num_combinations() const { return m_num_combinations; }

private:
    Combinations<T> m_comb1;
    Combinations<T> m_comb2;
    int m_comb2_valid_combinations;
    int m_num_combinations;
    int m_k;
};

template <typename Iter, typename Iter2>
Combinations2Sets(Iter, Iter, Iter2, Iter2, int) -> Combinations2Sets<typename std::iterator_traits<Iter>::value_type>;
template <typename V, typename V2>
Combinations2Sets(V, V2, int) -> Combinations2Sets<typename V::value_type>;

template <typename T>
class AllSubsets {
public:
    AllSubsets() = default;

    template <typename Iter, util::enable_if_iterator_t<Iter, int> = 0>
    AllSubsets(Iter begin, Iter end, int min_k, int max_k)
        : m_elements(begin, end), m_fixed(), m_min_k(min_k), m_max_k(max_k), m_num_combinations(0) {
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations += std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i));
        }
    }

    template <typename Iter,
              typename IterFixed,
              util::enable_if_iterator_t<Iter, int> = 0,
              util::enable_if_iterator_t<IterFixed, int> = 0>
    AllSubsets(Iter begin, Iter end, IterFixed begin_fixed, IterFixed end_fixed, int min_k, int max_k)
        : m_elements(begin, end),
          m_fixed(begin_fixed, end_fixed),
          m_min_k(min_k),
          m_max_k(max_k),
          m_num_combinations(0) {
        static_assert(std::is_same_v<typename std::iterator_traits<Iter>::value_type,
                                     typename std::iterator_traits<IterFixed>::value_type>,
                      "The type of fixed and movable elements should be the same.");
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations +=
                std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i - m_fixed.size()));
        }
    }

    template <typename V, util::enable_if_vector_of_type_t<V, T, int> = 0>
    AllSubsets(V elements, int min_k, int max_k)
        : m_elements(elements), m_min_k(min_k), m_max_k(max_k), m_num_combinations(0) {
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations += std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i));
        }
    }

    template <typename V,
              typename V2,
              util::enable_if_vector_of_type_t<V, T, int> = 0,
              util::enable_if_vector_of_type_t<V2, T, int> = 0>
    AllSubsets(V elements, V2 fixed, int min_k, int max_k)
        : m_elements(elements), m_fixed(fixed), m_min_k(min_k), m_max_k(max_k), m_num_combinations(0) {
        for (int i = min_k; i <= max_k; ++i) {
            m_num_combinations +=
                std::round(boost::math::binomial_coefficient<double>(m_elements.size(), i - m_fixed.size()));
        }
    }

    class allsubsets_iterator {
    public:
        using iterator_category = std::input_iterator_tag;
        using value_type = std::vector<T>;
        using difference_type = int;
        using pointer = std::vector<T>*;
        using reference = std::vector<T>&;

        allsubsets_iterator() = default;

        allsubsets_iterator(const AllSubsets<T>* self, int idx)
            : m_self(self), m_idx(idx), m_current_comb(), m_current_iter(), m_current_k(m_self->m_min_k) {
            if (idx == 0) {
                m_current_comb = Combinations(m_self->m_elements.begin(),
                                              m_self->m_elements.end(),
                                              m_self->m_fixed.begin(),
                                              m_self->m_fixed.end(),
                                              m_current_k);
                m_current_iter = m_current_comb.begin();
            }
        }

        allsubsets_iterator& operator=(const allsubsets_iterator& other) {
            m_self = other.m_self;
            m_idx = other.m_idx;
            m_current_comb = other.m_current_comb;
            m_current_iter = other.m_current_iter;
            m_current_k = other.m_current_k;
            return *this;
        }

        allsubsets_iterator& operator++() {
            ++m_current_iter;
            ++m_idx;

            if (m_current_iter == m_current_comb.end() && m_current_k < m_self->m_max_k) {
                ++m_current_k;
                m_current_comb = Combinations(m_self->m_elements.begin(),
                                              m_self->m_elements.end(),
                                              m_self->m_fixed.begin(),
                                              m_self->m_fixed.end(),
                                              m_current_k);
                m_current_iter = m_current_comb.begin();
            }

            return *this;
        }

        allsubsets_iterator operator++(int) {
            allsubsets_iterator return_it(*this);

            ++m_current_iter;
            ++m_idx;
            if (m_current_iter == m_current_comb.end() && m_current_k < m_self->m_max_k) {
                ++m_current_k;
                m_current_comb = Combinations(m_self->m_elements.begin(),
                                              m_self->m_elements.end(),
                                              m_self->m_fixed.begin(),
                                              m_self->m_fixed.end(),
                                              m_current_k);
                m_current_iter = m_current_comb.begin();
            }

            return return_it;
        }

        reference operator*() { return *m_current_iter; }

        pointer operator->() { return &m_current_iter.m_subset; }

        bool operator==(const allsubsets_iterator& rhs) { return (m_idx == rhs.m_idx) && (m_self == rhs.m_self); }

        bool operator!=(const allsubsets_iterator& rhs) { return !(*this == rhs); }

    private:
        const AllSubsets<T>* m_self;
        int m_idx;
        Combinations<T> m_current_comb;
        typename Combinations<T>::combination_iterator m_current_iter;
        int m_current_k;
    };

    allsubsets_iterator begin() const { return allsubsets_iterator(this, 0); }
    allsubsets_iterator end() const { return allsubsets_iterator(this, num_combinations()); }

    int num_combinations() const { return m_num_combinations; }

private:
    std::vector<T> m_elements;
    std::vector<T> m_fixed;
    int m_min_k;
    int m_max_k;
    int m_num_combinations;
};

template <typename Iter>
AllSubsets(Iter, Iter, int, int) -> AllSubsets<typename std::iterator_traits<Iter>::value_type>;
template <typename Iter, typename IterFixed>
AllSubsets(Iter, Iter, IterFixed, IterFixed, int, int) -> AllSubsets<typename std::iterator_traits<Iter>::value_type>;
template <typename V>
AllSubsets(V, int, int) -> AllSubsets<typename V::value_type>;
template <typename V, typename V2>
AllSubsets(V, V2, int, int) -> AllSubsets<typename V::value_type>;

}  // namespace util

#endif  // PYBNESIAN_UTIL_COMBINATIONS_HPP


#include "dense-reconstruction/disparity-conversion-utils.h"

#include <limits>
#include <memory>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

namespace dense_reconstruction {
namespace stereo {

void convertDisparityMapToPointCloud(
    const cv::Mat& input_disparity, const cv::Mat& left_image,
    const double baseline, const double focal_length, const double cx,
    const double cy, const int sad_window_size, const int min_disparity,
    const int num_disparities, resources::PointCloud* pointcloud) {
  CHECK_NOTNULL(pointcloud);
  CHECK(pointcloud->empty());
  CHECK_GT(focal_length, 0.0);

  if (left_image.type() != CV_8U) {
    LOG(ERROR)
        << "Pointcloud generation is currently only supported on 8 bit images";
    return;
  }

  const int side_bound = sad_window_size / 2;

  const int max_size = input_disparity.rows * input_disparity.cols;

  pointcloud->xyz.reserve(3 * max_size);
  pointcloud->colors.reserve(3 * max_size);

  cv::Mat input_valid =
      cv::Mat(input_disparity.rows, input_disparity.cols, CV_8U);

  const int lower_x_border = side_bound + min_disparity + num_disparities;
  const int lower_y_border = side_bound;
  const int upper_x_border = input_disparity.cols - side_bound;
  const int upper_y_border = input_disparity.rows - side_bound;

  const int disparity_threshold = (min_disparity + num_disparities - 1) * 16;

  for (int y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) {
    for (int x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) {
      // The last check is because the sky has a bad habit of having a disparity
      // at just less than the max disparity.
      const int16_t disparity_value =
          input_disparity.at<int16_t>(y_pixels, x_pixels);
      if ((x_pixels < lower_x_border) || (y_pixels < lower_y_border) ||
          (x_pixels > upper_x_border) || (y_pixels > upper_y_border) ||
          (disparity_value < 0) || (disparity_value >= disparity_threshold)) {
        input_valid.at<uint8_t>(y_pixels, x_pixels) = 0;
      } else {
        input_valid.at<uint8_t>(y_pixels, x_pixels) = 1;
      }
    }
  }

  // Build pointcloud.
  for (int y_pixels = lower_y_border; y_pixels < upper_y_border; ++y_pixels) {
    for (int x_pixels = lower_x_border; x_pixels < upper_x_border; ++x_pixels) {
      const bool is_valid = input_valid.at<uint8_t>(y_pixels, x_pixels) > 0u;
      const int16_t input_value =
          input_disparity.at<int16_t>(y_pixels, x_pixels);

      double disparity_value;

      // If the filled disparity is valid it must be a freespace ray.
      if (is_valid) {
        disparity_value = static_cast<double>(input_value);
      } else {
        continue;
      }

      if (disparity_value < 1e-6) {
        continue;
      }

      // The 16x is needed as opencv stores disparity maps as 16 * the true
      // values.

      // NOTE(mfehr): This is the reprojection formula based on:
      // Source: http://answers.opencv.org/upfiles/13535653931527438.jpg
      // We undistort such that cx_left and cx_right are identical, therefore
      // the formula can be simplified to:
      // X = -b(x-Cx)/d
      // Y = -b(y - Cy) / d
      // Z = -(f*b)/d
      // with t = -b/d we get:
      // X = t * (x-Cx)
      // Y = t * (y - Cy)
      // Z = t * f

      const double t = -16.0 * baseline / disparity_value;
      const double z = t * focal_length;
      const double x = t * (x_pixels - cx);
      const double y = t * (y_pixels - cy);

      LOG_IF(WARNING, z < 0.0) << "Z is negative, which is weird!";

      uint8_t r, g, b;
      if (left_image.channels() == 3) {
        const cv::Vec3b& color = left_image.at<cv::Vec3b>(y_pixels, x_pixels);
        b = color[0];
        g = color[1];
        r = color[2];
      } else if (left_image.channels() == 4) {
        const cv::Vec4b& color = left_image.at<cv::Vec4b>(y_pixels, x_pixels);
        b = color[0];
        g = color[1];
        r = color[2];
      } else {
        b = left_image.at<uint8_t>(y_pixels, x_pixels);
        g = b;
        r = b;
      }

      pointcloud->xyz.push_back(x);
      pointcloud->xyz.push_back(y);
      pointcloud->xyz.push_back(z);

      pointcloud->colors.push_back(r);
      pointcloud->colors.push_back(g);
      pointcloud->colors.push_back(b);
    }
  }
  CHECK_LE(pointcloud->size(), 3 * max_size);
}

// Convert disparity map to a depth map in the target camera frame.
// NOTE(mfehr): This is definitely not the most efficient way to do this, but it
// uses the available infrastructure. If speed is an issue, this needs to
// be improved.
void convertDisparityMapToDepthMap(
    const cv::Mat& disparity_map, const cv::Mat& first_image_undistorted,
    const double baseline, const double focal_length, const double cx,
    const double cy, const int sad_window_size, const int min_disparity,
    const int num_disparities, const aslam::Camera& target_camera,
    cv::Mat* depth_map) {
  CHECK_NOTNULL(depth_map);
  CHECK(!disparity_map.empty());
  CHECK(!first_image_undistorted.empty());

  // Convert disparity to point cloud.
  resources::PointCloud point_cloud;
  convertDisparityMapToPointCloud(
      disparity_map, first_image_undistorted, baseline, focal_length, cx, cy,
      sad_window_size, min_disparity, num_disparities, &point_cloud);

  // Convert point cloud format to Eigen matrix.
  const size_t point_cloud_size = point_cloud.size();
  const size_t max_index = point_cloud_size * 3u;
  Eigen::Matrix3Xd p_C1_mat(3, point_cloud_size);
  size_t point_idx = 0u;
  for (size_t idx = 0u; idx < max_index; idx += 3u, ++point_idx) {
    CHECK_LT(point_idx, point_cloud_size);
    p_C1_mat.col(point_idx) = Eigen::Vector3d(
        point_cloud.xyz[idx], point_cloud.xyz[idx + 1u],
        point_cloud.xyz[idx + 2u]);
  }

  // Init depth map.
  const size_t height = target_camera.imageHeight();
  const size_t width = target_camera.imageWidth();
  *depth_map = cv::Mat(height, width, CV_16UC1, cv::Scalar(0u));

  // Project all 3D points into the image.
  Eigen::Matrix2Xd img_points_px;
  std::vector<aslam::ProjectionResult> projection_results;
  target_camera.project3Vectorized(
      p_C1_mat, &img_points_px, &projection_results);

  // Convert Z coordinate of points to millimeters.
  constexpr double kMetersToMillimeters = 1e3;

  // Statistics.
  for (point_idx = 0u; point_idx < point_cloud_size; ++point_idx) {
    if (!projection_results[point_idx].isKeypointVisible()) {
      continue;
    }

    const Eigen::Ref<const Eigen::Vector2d>& img_point =
        img_points_px.col(point_idx);

    const int depth_in_mm = std::min(
        static_cast<int>(std::numeric_limits<uint16_t>::max()),
        static_cast<int>(p_C1_mat(2, point_idx) * kMetersToMillimeters));
    CHECK_GE(depth_in_mm, 0);

    const int img_u = static_cast<int>(std::round(img_point[0]));
    const int img_v = static_cast<int>(std::round(img_point[1]));
    depth_map->at<uint16_t>(img_v, img_u) = static_cast<uint16_t>(depth_in_mm);
  }
}

}  // namespace stereo
}  // namespace dense_reconstruction