// Boost.Geometry

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INTERSECTION_HPP

#include <algorithm>

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

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

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

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

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

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

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

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

        typedef srs::spheroid<calc_t> spheroid_type;

        static const calc_t c0 = 0;

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

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

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

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

        side_info sides;

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

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

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

        bool is_equal_a1_b2 = equals_point_point(a1, b2);

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

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

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

        bool collinear = sides.collinear();

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

        if (! ok)
        {
            return false;
        }

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

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

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

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

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

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

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

        ip_flag = ipi_inters;

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

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

        return true;
    }

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

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

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

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

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

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

private:
    Spheroid m_spheroid;
};

}} // namespace strategy::intersection

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INTERSECTION_HPP


/*
 * Copyright 2016 Malte Splietker
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *         limitations under the License.
 */

#include <cmath>

#include <boost/log/trivial.hpp>

#include "BrightnessNotifier.h"

BrightnessNotifier::BrightnessNotifier() {
    display = XOpenDisplay(0);
    screen = XRRGetScreenResources(display, DefaultRootWindow(display));
}

void BrightnessNotifier::notificationStart() {
    oscillationStartTime = system_clock::now();
}

void BrightnessNotifier::notificationLoop() {
    system_clock::time_point now = system_clock::now();
    milliseconds diffTime = duration_cast<milliseconds>(now - oscillationStartTime);

    /* Next oscillation wave */
    if (diffTime.count() > oscillationIntervalLength * 1000) {
        diffTime = milliseconds(0);
        oscillationStartTime = system_clock::now();
    }

    double rad = (diffTime.count() / (oscillationIntervalLength * 1000)) * 2 * M_PI;
    double newBrightness = 1 - (intensity / 2) + cos(rad) * (intensity / 2);
    BOOST_LOG_TRIVIAL(debug) << "new brightness: " << newBrightness << std::endl;
    setBrightness(newBrightness);
}

void BrightnessNotifier::notificationStop() {
    setBrightness(1);
}

double BrightnessNotifier::getBrightness() {
    double brightness = 0;

    int size = XRRGetCrtcGammaSize(display, screen->crtcs[0]);

    XRRCrtcGamma *gamma = XRRGetCrtcGamma(display, screen->crtcs[0]);

    unsigned short *array = gamma->red;
    int maxIndex = 0;
    for (int i = size - 1; i > 0; i--) {
        if (array[i] < 0xffff) {
            maxIndex = i;
            break;
        }
    }

    int middle = maxIndex / 2;
    double i1 = (double) (middle + 1) / size;
    double v1 = (double) (array[middle]) / 65535;
    double i2 = (double) (maxIndex + 1) / size;
    double v2 = (double) (array[maxIndex]) / 65535;
    if (v2 < 0.0001) { /* The screen is black */
        brightness = 0;
    } else {
        if ((maxIndex + 1) == size)
            brightness = v2;
        else
            brightness = exp((log(v2) * log(i1) - log(v1) * log(i2)) / log(i1 / i2));
    }

    XRRFreeGamma(gamma);

    if (brightness > 1) {
        brightness = 1;
    }

    return brightness;
}

void BrightnessNotifier::setBrightness(double brightness) {
    if (brightness > 1) {
        brightness = 1;
    } else if (brightness < 0) {
        brightness = 0;
    }

    for (int crtcIndex = 0; crtcIndex < screen->ncrtc; crtcIndex++) {
        int size = XRRGetCrtcGammaSize(display, screen->crtcs[crtcIndex]);

        XRRCrtcGamma *gamma = XRRAllocGamma(size);

        for (int i = 0; i < size; i++) {
            unsigned short gammaValue = (unsigned short) ((double) i / (double) (size - 1) * brightness * 65535.0);
            gamma->red[i] = gammaValue;
            gamma->green[i] = gammaValue;
            gamma->blue[i] = gammaValue;
        }

        XRRSetCrtcGamma(display, screen->crtcs[crtcIndex], gamma);

        free(gamma);
    }
}


//
// Copyright (c) 2018 Loïc HAMOT
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#include "stdafx.h"
#include "Skills.h"
#include "Rules.h"

#include <boost/locale.hpp>

size_t powInt(size_t val, size_t power)
{
	return size_t(pow(val, power) + 0.5);
}

//! Skill non abstrait de rempacement, pour ceux qui ne sont pas encore codés
class DummySkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const&) const {return true;};
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const&) const
	{
		return std::string();
	}
public:
	//! Constructeur
	//! @param name Nom du skill (avant traduction)
	explicit DummySkill(std::string const& name): ISkill(name) {}
};

//! Compétance Conquest
class ConquestSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const&) const {return true;};
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("You can create up to {1} planets.")) %
		        getMaxPlanetCount(player)).str();
	}
public:
	ConquestSkill(): ISkill("Conquest") {}
};

//! Compétance Strategy
class StrategySkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const&) const {return true;};
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("You can create up to {1} fleets.")) %
		        getMaxFleetCount(player)).str();
	}
public:
	StrategySkill(): ISkill("Strategy") {}
};

//! Compétance Cohesion
class CohesionSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const&) const {return true;};
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("Your fleets can contains up to {1} ships.")) %
		        getMaxFleetSize(player)).str();
	}
public:
	CohesionSkill(): ISkill("Cohesion") {}
};

//! Compétance Service d'information
//! @toto: tout
class InfoServiceSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& player) const
	{
		return player.skilltab[Skill::InformationService] < 1;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& //player
	                                     ) const
	{
		using namespace boost::locale;
		return translate("You can filter your messages");
	}
public:
	InfoServiceSkill(): ISkill("InformationService") {}
};

//! Compétance Ferme de serveur
class ServerFarmSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const&) const {return true;};
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("You can now store up to {1} events.")) %
		        getMaxEventCount(player)).str();
	}
public:
	ServerFarmSkill(): ISkill("ServerFarm") {}
};

//! Compétance Chronos
class ChronosSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& player) const
	{
		return player.skilltab[Skill::Chronos] < 1;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel
	                            ) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& //player
	                                     ) const
	{
		using namespace boost::locale;
		return translate("You can use the \"age()\" procedure "
		                 "on your fleets and planets");
	}
public:
	ChronosSkill(): ISkill("Chronos") {}
};

//! Compétance Memoire
class MemorySkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& //player
	                           ) const
	{
		return true;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel + 1);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("Your store up to {1} items in the \"memory\""
		                         " of your fleets and planets")) %
		        memoryPtreeSize(player)).str();
	}
public:
	MemorySkill(): ISkill("Memory") {}
};

//! Compétance Portée d'Emition
class EmissionRangeSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& //player
	                           ) const
	{
		return true;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(3, skillCurrentLevel + 1);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("Your messages can reachs up to {1} cases")) %
		        playerEmissionRange(player)).str();
	}
public:
	EmissionRangeSkill() : ISkill("EmissionRange") {}
};

//! Compétance Evasion
class EvasionSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& //player
	                           ) const
	{
		return true;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		size_t const level = player.skilltab[Skill::Escape];
		return (format(translate( //xgettext:no-c-format
		                 "A fleet with {1} ships has 50% of probability to escape.")) %
		        (level * level)).str();
	}
public:
	EvasionSkill(): ISkill("Evasion") {}
};

//! Compétance Journal
class LogSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& player) const
	{
		return player.skilltab[Skill::Log] < 1;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		if(player.skilltab[Skill::Log])
			return translate("You can use the \"log\" "
			                 "function to create custom events");
		else
			return translate("You can't use the \"log\" "
			                 "function to create custom events");
	}
public:
	LogSkill() : ISkill("Log") {}
};

//! Compétance Simulation
//! @toto: tout
class SimulationSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& //player
	                           ) const
	{
		return true;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel + 1);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		if(player.skilltab[Skill::Simulation])
			return (format(translate("Each call to \"simulates\" "
			                         "function process {1} simulations")) %
			        playerFightSimulationCount(player)).str();
		else
			return translate(
			         "Each call to \"simulates\" function return false");
	}
public:
	SimulationSkill() : ISkill("Simulation") {}
};

//! Compétance Boite noir
class BlackBoxSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& player) const
	{
		return player.skilltab[Skill::BlackBox] < 1;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel + 1);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return player.skilltab[Skill::BlackBox] == 0 ?
		       (format(translate("You can't receive fight report from destroyed fleets"))).str() :
		       (format(translate("You receive fight report from destroyed fleets"))).str();
	}

public:
	BlackBoxSkill() : ISkill("BlackBox") {}
};

//! Compétance Débit d'Emition
class EmissionRateSkill : public ISkill
{
	virtual bool canUpgradeImpl(Player const& //player
	                           ) const
	{
		return true;
	}
	virtual size_t skillCostImpl(size_t skillCurrentLevel) const
	{
		return powInt(2, skillCurrentLevel + 2);
	}
	virtual std::string effectMessageImpl(Player const& player) const
	{
		using namespace boost::locale;
		return (format(translate("Your messages emit up to {1} values per round")) %
		        emitionPTreeSize(player)).str();
	}
public:
	EmissionRateSkill() : ISkill("EmissionRate") {}
};

//! Initialise la liste de skill
std::vector<std::shared_ptr<ISkill> > InitSkills()
{
	std::vector<std::shared_ptr<ISkill> > list;
	list.push_back(std::make_shared<ConquestSkill>());
	list.push_back(std::make_shared<StrategySkill>());
	list.push_back(std::make_shared<CohesionSkill>());
	list.push_back(std::make_shared<InfoServiceSkill>());
	list.push_back(std::make_shared<ServerFarmSkill>());
	list.push_back(std::make_shared<ChronosSkill>());
	list.push_back(std::make_shared<MemorySkill>());
	list.push_back(std::make_shared<EmissionRangeSkill>());
	list.push_back(std::make_shared<SimulationSkill>());
	list.push_back(std::make_shared<BlackBoxSkill>());
	list.push_back(std::make_shared<LogSkill>());
	list.push_back(std::make_shared<EvasionSkill>());
	list.push_back(std::make_shared<EmissionRateSkill>());
	return list;
};

std::vector<std::shared_ptr<ISkill> > const Skill::List = InitSkills();


/**
 * Code to compute CNN (ImageNet) features for a given image using CAFFE
 * (c) Rohit Girdhar
 */

#include <memory>
#include <chrono>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp> // for to_lower
#include "caffe/caffe.hpp"
#include "utils.hpp"

using namespace std;
using namespace std::chrono;
using namespace caffe;
using namespace cv;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

template<typename Dtype>
void computeFeaturesPipeline(Net<Dtype>& caffe_test_net,
    const vector<Mat>& Is,
    const vector<string>& layers,
    int BATCH_SIZE,
    vector<vector<vector<Dtype>>>& output,
    bool verbose,
    const string& POOLTYPE,
    bool NORMALIZE);
void genSegImg(float xmin, float ymin, float xmax, float ymax, const Mat& S, Mat& res, int, int);

int
main(int argc, char *argv[]) {
  ::google::InitGoogleLogging(argv[0]);
#ifdef CPU_ONLY
  Caffe::set_mode(Caffe::CPU);
  LOG(INFO) << "Extracting Features in CPU mode";
#else
  Caffe::set_mode(Caffe::GPU);
#endif

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "Show this help")
    ("loc-network-path,n", po::value<string>()->required(),
     "Path to the localization prototxt file")
    ("loc-model-path,m", po::value<string>()->required(),
     "Path to localization caffemodel")
    ("seg-network-path,p", po::value<string>()->required(),
     "Path to the segmentation prototxt file")
    ("seg-model-path,q", po::value<string>()->required(),
     "Path to segmentation caffemodel")
    ;

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
  if (vm.count("help")) {
    LOG(INFO) << desc;
    return -1;
  }
  try {
    po::notify(vm);
  } catch(po::error& e) {
    LOG(ERROR) << e.what();
    return -1;
  }

  fs::path LOC_NETWORK_PATH = fs::path(vm["loc-network-path"].as<string>());
  fs::path LOC_MODEL_PATH =
    fs::path(vm["loc-model-path"].as<string>());
  fs::path SEG_NETWORK_PATH = fs::path(vm["seg-network-path"].as<string>());
  fs::path SEG_MODEL_PATH =
    fs::path(vm["seg-model-path"].as<string>());

  Net<float> loc_caffe_test_net(LOC_NETWORK_PATH.string(), caffe::TEST);
  loc_caffe_test_net.CopyTrainedLayersFrom(LOC_MODEL_PATH.string());
  int LOC_BATCH_SIZE = loc_caffe_test_net.blob_by_name("data")->num();
  vector<string> loc_layers = {"fc8_loc"};
  Net<float> seg_caffe_test_net(SEG_NETWORK_PATH.string(), caffe::TEST);
  seg_caffe_test_net.CopyTrainedLayersFrom(SEG_MODEL_PATH.string());
  int SEG_BATCH_SIZE = seg_caffe_test_net.blob_by_name("data")->num();
  vector<string> seg_layers = {"fc8_seg"};

  string imgpath = "/srv2/rgirdhar/Work/Datasets/processed/0006_ExtendedPAL/corpus/AbuSimbel/people_1.jpg";
  //string imgpath = "/home/rgirdhar/memexdata/Dataset/processed/0001_Backpage/Images/corpus/ImagesTexas/Texas_2012_10_10_1349841918000_4_0.jpg";
  //string imgpath = "/home/rgirdhar/memexdata/Dataset/processed/0001_Backpage/Images/corpus/ImagesTexas/Texas_2012_10_10_1349846732000_6_5.jpg";
  //string imgpath = "/home/rgirdhar/memexdata/Dataset/processed/0001_Backpage/Images/corpus/ImagesCalifornia/California_image_2012_7_9_1341859695000_2_0.jpg";
  vector<Mat> Is;
  Mat I = imread(imgpath);
  if (!I.data) {
    LOG(ERROR) << "Unable to read " << imgpath;
    return -1;
  }
  resize(I, I, Size(256, 256));
  Is.push_back(I);
  // [layer[image[feature]]]
  vector<vector<vector<float>>> loc_output;
  computeFeaturesPipeline(loc_caffe_test_net, Is, loc_layers,
      LOC_BATCH_SIZE, loc_output, /* verbose= */ false,
      /* POOLTYPE= */ "", /* NORMALIZE= */ false);

  float OFFSET = (256 - 227) / 2.0;
  /*
  loc_output[0][0][0] += OFFSET;
  loc_output[0][0][1] += OFFSET;
  loc_output[0][0][2] += OFFSET;
  loc_output[0][0][3] += OFFSET;
  */

  cout << loc_output[0][0][0] << " "
       << loc_output[0][0][1] << " "
       << loc_output[0][0][2] << " "
       << loc_output[0][0][3];

  int width = I.cols;
  int height = I.rows;
  float xmin = std::min(width - 1.0f, std::max(0.0f, loc_output[0][0][0] + OFFSET));
  float ymin = std::min(height - 1.0f, std::max(0.0f, loc_output[0][0][1] + OFFSET));
  float xmax = std::min(width - 1.0f, std::max(0.0f, loc_output[0][0][2] + OFFSET));
  float ymax = std::min(height - 1.0f, std::max(0.0f, loc_output[0][0][3] + OFFSET));

  Rect roi(xmin,
           ymin,
           xmax - xmin,
           ymax - ymin);
  Mat T = I(roi);
  resize(T, T, Size(55, 55));

  caffe::BlobProto mean_blob_proto;
  Mat mean;
  caffe::ReadProtoFromBinaryFile("/home/rgirdhar/data/Work/Code/0005_ObjSegment/nips14_loc_seg_testonly/Caffe_Segmentation/segscripts/models/mean.binaryproto", &mean_blob_proto);
  caffe::Blob<unsigned int> mean_blob;
  mean_blob.FromProto(mean_blob_proto);
  convertBlobToMat(mean_blob, mean);
  mean = mean(roi);
  resize(mean, mean, Size(55, 55));
  /*
  double minEl, maxEl;
  minMaxLoc(mean, &minEl, &maxEl);
  cout << " mx  " << minEl << " " << maxEl << endl;
  minMaxLoc(T, &minEl, &maxEl);
  cout << " mx  " << minEl << " " << maxEl << endl;
  */

  for (int c = 0; c < 3; c++) {
    for (int h = 0; h < 55; h++) {
      for (int w = 0; w < 55; w++) {
        T.at<Vec3b>(h, w)[c] = (uint8_t) T.at<Vec3b>(h, w)[c] - mean.at<Vec3b>(h, w)[c];
      }
    }
  }

  //T = T - mean;
  flip(T, T, 1);
//  cout << T;
  imwrite("mean.jpg", T);

  vector<Mat> Ts;
  Ts.push_back(T);
  vector<vector<vector<float>>> seg_output;
  computeFeaturesPipeline(seg_caffe_test_net, Ts, seg_layers,
      SEG_BATCH_SIZE, seg_output, /* verbose= */ false,
      /* POOLTYPE= */ "", /* NORMALIZE= */ false);
  Mat Res(50, 50, CV_32FC1);
  for (int i = 0; i < 50; i++) {
    for (int j = 0; j < 50; j++) {
      Res.at<float>(i, j) = seg_output[0][0][i * 50 + j];
    }
  }
//  flip(Res, Res, 1);
  Mat seg;
  genSegImg(xmin, ymin, xmax, ymax, Res, seg, I.rows, I.cols);
  Mat seg_uint;
  seg.convertTo(seg_uint, CV_8UC1);
  equalizeHist(seg_uint, seg_uint);
  //resize(seg_uint, seg_uint, I.size());
  imwrite("final.jpg", seg_uint);
  vector<Mat> channels(3);
  split(I, channels);
  divide(seg_uint, Scalar(255), seg_uint);
  multiply(seg_uint, channels[0], channels[0]);
  multiply(seg_uint, channels[1], channels[1]);
  multiply(seg_uint, channels[2], channels[2]);
  merge(channels, I);
  // normalize(Res, Res, 0, 255, NORM_MINMAX, CV_8UC1);

  // equalizeHist(Res, Res);
  imwrite("over.jpg", I);
  return 0;
}

template<typename Dtype>
void computeFeaturesPipeline(Net<Dtype>& caffe_test_net,
    const vector<Mat>& Is,
    const vector<string>& layers,
    int BATCH_SIZE,
    vector<vector<vector<Dtype>>>& output,
    bool verbose,
    const string& POOLTYPE,
    bool NORMALIZE) {
  computeFeatures(caffe_test_net, Is, layers, BATCH_SIZE, output, verbose);
  if (! POOLTYPE.empty()) {
    // assuming all layers need to be pooled
    for (int l = 0; l < output.size(); l++) {
      poolFeatures(output[l], POOLTYPE);
    }
  }
  if (NORMALIZE) {
    // assuming all layers need to be normalized
    for (int i = 0; i < output.size(); i++) {
      l2NormalizeFeatures(output[i]);
    }
  }
}

void genSegImg(float xmin, float ymin, float xmax, float ymax, const Mat& S, Mat& res,
    int NW_IMG_HT, int NW_IMG_WID) {
  res = Mat(NW_IMG_HT, NW_IMG_WID, CV_32FC1);
  res.setTo(0);
  int x1 = xmin;
  int y1 = ymin;
  int x2 = xmax;
  int y2 = ymax;
  int height = MAX(1, y2 - y1 + 1);
  int width = MAX(1, x2 - x1 + 1);
  Mat extractedImage = res(Rect(x1, y1, width, height));
  Mat S2;
  resize(S, S2, Size(width, height));
  S2.copyTo(extractedImage);
}


// Copyright Henrik Steffen Gaßmann 2020.
//
// Distributed under the Boost Software License, Version 1.0.
//         (See accompanying file LICENSE or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstdint>

#include <algorithm>
#include <array>
#include <compare>
#include <type_traits>
#include <utility>

#include <boost/mp11/algorithm.hpp>

#include <dplx/cncr/math_supplement.hpp>
#include <dplx/cncr/type_utils.hpp>

#include <dplx/dp/decoder/api.hpp>
#include <dplx/dp/decoder/std_string.hpp>
#include <dplx/dp/decoder/utils.hpp>
#include <dplx/dp/detail/hash.hpp>
#include <dplx/dp/detail/perfect_hash.hpp>
#include <dplx/dp/detail/type_utils.hpp>
#include <dplx/dp/fwd.hpp>
#include <dplx/dp/layout_descriptor.hpp>
#include <dplx/dp/object_def.hpp>
#include <dplx/dp/tag_invoke.hpp>

namespace dplx::dp
{

inline constexpr struct property_id_hash_fn
{
    template <typename T>
        requires tag_invocable<property_id_hash_fn, T const &>
    constexpr auto operator()(T const &value) const
            noexcept(nothrow_tag_invocable<property_id_hash_fn, T const &>)
                    -> std::uint64_t
    {
        return cpo::tag_invoke(*this, value);
    }
    template <typename T>
        requires tag_invocable<property_id_hash_fn, T const &, std::uint64_t>
    constexpr auto operator()(T const &value, std::uint64_t seed) const
            noexcept(nothrow_tag_invocable<property_id_hash_fn,
                                           T const &,
                                           std::uint64_t>) -> std::uint64_t
    {
        return cpo::tag_invoke(*this, value, seed);
    }

    template <cncr::integer T>
    friend constexpr auto tag_invoke(property_id_hash_fn, T value) noexcept
            -> std::uint64_t
    {
        return static_cast<std::uint64_t>(value);
    }
    template <cncr::integer T>
    friend constexpr auto
    tag_invoke(property_id_hash_fn, T value, std::uint64_t seed) noexcept
            -> std::uint64_t
    {
        return detail::xxhash3(value, seed);
    }

    friend constexpr auto tag_invoke(property_id_hash_fn,
                                     std::u8string_view str,
                                     std::uint64_t const seed = 0) noexcept
            -> std::uint64_t
    {
        return detail::fnvx_hash(str.data(), str.size(), seed);
    }

} property_id_hash;

template <std::size_t N, input_stream Stream>
class basic_decoder<fixed_u8string<N>, Stream>
{
    using parse = item_parser<Stream>;

public:
    auto operator()(Stream &inStream, fixed_u8string<N> &out) const
            -> result<void>
    {
        DPLX_TRY(parse::u8string_finite(inStream, out, N));
        return oc::success();
    }
};

} // namespace dplx::dp

namespace dplx::dp::detail
{

template <std::size_t NumBits>
constexpr auto compress_bitset(std::initializer_list<bool> vs) noexcept
{
    constexpr auto digits = static_cast<std::size_t>(digits_v<std::size_t>);

    constexpr auto numBuckets = cncr::div_ceil(NumBits, digits);
    std::array<std::size_t, numBuckets> buckets{};

    auto const *it = vs.begin();
    auto const *const end = vs.end();
    for (std::size_t shift = 0, offset = 0; it != end; ++it, ++shift)
    {
        if (shift == digits)
        {
            shift = 0;
            offset += 1;
        }

        buckets[offset] |= static_cast<std::size_t>(*it) << shift;
    }
    return buckets;
}

template <template <auto...> typename ObjectDefLike, auto... Properties>
constexpr auto
compress_optional_props(ObjectDefLike<Properties...> const &) noexcept
{
    return detail::compress_bitset<sizeof...(Properties)>(
            {Properties.required...});
}

template <auto const &descriptor>
inline constexpr auto required_prop_mask_for
        = detail::compress_optional_props(descriptor);

inline constexpr std::size_t unknown_property_id = ~static_cast<std::size_t>(0);

template <typename IdType, std::size_t NumIds, bool use_perfect_hash>
struct property_id_lookup_fn;

template <typename IdType, std::size_t NumIds>
struct property_id_lookup_fn<IdType, NumIds, true>
{
private:
    using id_type = IdType;
    using array_type = std::array<id_type, NumIds>;

    perfect_hasher<id_type, NumIds, property_id_hash_fn> hash;
    array_type const &ids;

public:
    constexpr property_id_lookup_fn(array_type const &ids)
        : hash(ids)
        , ids(ids)
    {
    }

    template <typename TLike>
    constexpr auto operator()(TLike &&id) const noexcept -> std::size_t
    {
        std::size_t const idx = hash(id);
        if (ids[idx] != id)
        {
            return unknown_property_id;
        }
        return idx;
    }
};

template <typename IdType, std::size_t NumIds>
struct property_id_lookup_fn<IdType, NumIds, false>
{
private:
    using id_type = IdType;
    using array_type = std::array<id_type, NumIds>;

    array_type const &ids;

public:
    constexpr property_id_lookup_fn(array_type const &ids)
        : ids(ids)
    {
    }

    template <typename TLike>
    constexpr auto operator()(TLike &&id) const noexcept -> std::size_t
    {
        auto const *const begin = ids.data();
        auto const *const end = ids.data() + ids.size();
        id_type const *it;
        if constexpr (NumIds <= 64)
        {
            if (it = std::find(begin, end, id); it == end)
            {
                return unknown_property_id;
            }
        }
        else
        {
            if (it = std::lower_bound(begin, end, id); it == end || *it != id)
            {
                return unknown_property_id;
            }
        }
        return static_cast<std::size_t>(it - begin);
    }
};

template <auto const &Descriptor, typename T, input_stream Stream>
class decode_object_property_fn
{
    static constexpr auto const &descriptor = Descriptor;
    static constexpr auto ids = descriptor.ids;
    static constexpr std::size_t const num_prop_ids = descriptor.ids.size();

#if DPLX_DP_WORKAROUND(DPLX_COMP_GNUC, <=, 10, 1, 0)
    // fixed_u8string id comparison operator is borked
#else
    static_assert(std::is_sorted(descriptor.ids.begin(), descriptor.ids.end()));
#endif

    using odef_type = cncr::remove_cref_t<decltype(descriptor)>;
    using id_type = typename odef_type::id_type;
    using id_runtime_type = typename odef_type::id_runtime_type;

    static constexpr property_id_lookup_fn<id_type,
                                           descriptor.ids.size(),
                                           false>
            lookup{descriptor.ids};

    using decode_value_fn = mp_decode_value_fn<T, Stream>;

    struct decode_prop_fn : public decode_value_fn
    {
        template <std::size_t I>
        auto operator()(boost::mp11::mp_size_t<I>) -> result<std::size_t>
        {
            constexpr auto &propertyDef = descriptor.template property<I>();
            DPLX_TRY(decode_value_fn::operator()(propertyDef));
            return I;
        }
    };

public:
    auto operator()(Stream &inStream, T &dest) const -> result<std::size_t>
    {
        DPLX_TRY(auto &&id, decode(as_value<id_runtime_type>, inStream));

        auto const idx = lookup(id);
        if (idx == unknown_property_id)
        {
            return errc::unknown_property;
        }

        return boost::mp11::mp_with_index<num_prop_ids>(
                idx, decode_prop_fn{{inStream, dest}});
    }
};

template <auto const &Descriptor, typename T, input_stream Stream>
inline constexpr decode_object_property_fn<Descriptor, T, Stream>
        decode_object_property{};

template <typename T>
constexpr auto index_of_limit(T const *elems,
                              std::size_t const num,
                              T const limit) noexcept -> std::size_t
{
    for (std::size_t i = 0; i < num; ++i)
    {
        if (elems[i] >= limit)
        {
            return i;
        }
    }
    return num;
}

template <auto const &descriptor, typename T, input_stream Stream>
    requires cncr::unsigned_integer<
            typename cncr::remove_cref_t<decltype(descriptor)>::id_type>
class decode_object_property_fn<descriptor, T, Stream>
{
    using parse = item_parser<Stream>;
    using descriptor_type = cncr::remove_cref_t<decltype(descriptor)>;
    using id_type = typename descriptor_type::id_type;

#if DPLX_DP_WORKAROUND(DPLX_COMP_GNUC, <=, 10, 1, 0)
    // fixed_u8string id comparison operator is borked
#else
    static_assert(std::is_sorted(descriptor.ids.begin(), descriptor.ids.end()));
#endif
    static_assert(detail::digits_v<id_type> <= detail::digits_v<std::uint64_t>);

    static constexpr id_type small_id_limit = detail::inline_value_max + 1;
    static constexpr auto small_ids_end = detail::index_of_limit(
            descriptor.ids.data(), descriptor.ids.size(), small_id_limit);

    static constexpr std::size_t id_map_size
            = descriptor.ids.size() - small_ids_end;

    static constexpr auto copy_large_ids() noexcept
            -> std::array<id_type, id_map_size>
    {
        std::array<id_type, id_map_size> ids{};
        for (std::size_t i = 0; i < id_map_size; ++i)
        {
            ids[i] = descriptor.ids[i + small_ids_end];
        }
        return ids;
    }
    static constexpr auto large_ids = copy_large_ids();
    static constexpr property_id_lookup_fn<id_type, id_map_size, false> lookup{
            large_ids};

    using decode_value_fn = mp_decode_value_fn<T, Stream>;

    struct decode_prop_small_id_fn : decode_value_fn
    {
        template <std::size_t I>
        auto operator()(boost::mp11::mp_size_t<I>) -> result<std::size_t>
        {
            constexpr auto propPos = static_cast<std::size_t>(
                    std::find(descriptor.ids.data(),
                              descriptor.ids.data() + small_ids_end,
                              static_cast<id_type>(I))
                    - descriptor.ids.data());

            if constexpr (propPos == small_ids_end)
            {
                return errc::unknown_property;
            }
            else
            {
                constexpr auto &propertyDef
                        = descriptor.template property<propPos>();
                DPLX_TRY(decode_value_fn::operator()(propertyDef));
                return propPos;
            }
        }
    };

    struct decode_prop_large_id_fn : public decode_value_fn
    {
        template <std::size_t I>
        auto operator()(boost::mp11::mp_size_t<I>) -> result<std::size_t>
        {
            constexpr auto &propertyDef
                    = descriptor.template property<I + small_ids_end>();
            DPLX_TRY(decode_value_fn::operator()(propertyDef));
            return I + small_ids_end;
        }
    };

public:
    auto operator()(Stream &inStream, T &dest) const -> result<std::size_t>
    {
        DPLX_TRY(auto id, parse::template integer<id_type>(inStream));

        if (id < small_id_limit)
        {
            if constexpr (small_ids_end == 0)
            {
                return errc::unknown_property;
            }
            else
            {
                return boost::mp11::mp_with_index<small_id_limit>(
                        static_cast<std::size_t>(id),
                        decode_prop_small_id_fn{{inStream, dest}});
            }
        }
        else
        {
            if constexpr (small_ids_end == descriptor.ids.size())
            {
                return errc::unknown_property;
            }
            else
            {
                auto const idx = lookup(static_cast<id_type>(id));
                if (idx == unknown_property_id)
                {
                    return errc::unknown_property;
                }

                return boost::mp11::mp_with_index<id_map_size>(
                        idx, decode_prop_large_id_fn{{inStream, dest}});
            }
        }
    }
};

} // namespace dplx::dp::detail

namespace dplx::dp
{

struct object_head_info
{
    std::int32_t num_properties;
    std::uint32_t version;
};

template <input_stream Stream, bool isVersioned = true>
inline auto parse_object_head(Stream &inStream,
                              std::bool_constant<isVersioned> = {})
        -> result<object_head_info>
{
    using parse = item_parser<Stream>;
    DPLX_TRY(auto &&mapInfo, parse::generic(inStream));
    if (mapInfo.type != type_code::map || mapInfo.indefinite())
    {
        return errc::item_type_mismatch;
    }
    if (mapInfo.value == 0)
    {
        return object_head_info{0, null_def_version};
    }

    DPLX_TRY(auto &&remainingBytes, dp::available_input_size(inStream));
    // every prop consists of two items each being at least 1B big
    if (mapInfo.value > (remainingBytes / 2))
    {
        return errc::end_of_stream;
    }
    if (mapInfo.value >= static_cast<std::uint64_t>(
                std::numeric_limits<std::int32_t>::max() / 2))
    {
        return errc::too_many_properties;
    }
    auto numProps = static_cast<std::int32_t>(mapInfo.value);

    if constexpr (!isVersioned)
    {
        return object_head_info{numProps, null_def_version};
    }
    else
    {
        // the version property id is posint 0
        // and always encoded as a single byte
        DPLX_TRY(auto &&maybeVersionReadProxy, dp::read(inStream, 1));
        if (std::ranges::data(maybeVersionReadProxy)[0] != std::byte{})
        {
            DPLX_TRY(dp::consume(inStream, maybeVersionReadProxy, 0));
            return object_head_info{numProps, null_def_version};
        }

        if constexpr (dp::lazy_input_stream<Stream>)
        {
            DPLX_TRY(dp::consume(inStream, maybeVersionReadProxy));
        }

        // 0xffff'ffff => max() is reserved as null_def_version
        DPLX_TRY(auto version, parse::template integer<std::uint32_t>(
                                       inStream, 0xffff'fffeU));

        return object_head_info{numProps - 1, version};
    }
}

template <auto const &descriptor, typename T, input_stream Stream>
inline auto decode_object_property(Stream &stream, T &dest)
        -> result<std::size_t>
{
    return detail::decode_object_property<descriptor, T, Stream>(stream, dest);
}

template <auto const &descriptor, typename T, input_stream Stream>
inline auto decode_object_properties(Stream &stream,
                                     T &dest,
                                     std::int32_t numProperties) -> result<void>
{
    constexpr auto &decode_object_property
            = detail::decode_object_property<descriptor, T, Stream>;

    if constexpr (descriptor.has_optional_properties)
    {
        std::array<std::size_t,
                   detail::required_prop_mask_for<descriptor>.size()>
                foundProps{};

        for (std::int32_t i = 0; i < numProperties; ++i)
        {
            DPLX_TRY(auto &&which, decode_object_property(stream, dest));

            auto const offset = which / detail::digits_v<std::size_t>;
            auto const shift = which % detail::digits_v<std::size_t>;

            foundProps[offset] |= static_cast<std::size_t>(1) << shift;
        }

        std::size_t acc = 0;
        for (std::size_t i = 0; i < foundProps.size(); ++i)
        {
            auto const requiredProps
                    = detail::required_prop_mask_for<descriptor>[i];

            acc += (foundProps[i] & requiredProps) == requiredProps;
        }
        if (acc != foundProps.size())
        {
            return errc::required_object_property_missing;
        }
    }
    else
    {
        if (descriptor.num_properties != numProperties)
        {
            return errc::required_object_property_missing;
        }

        for (std::int32_t i = 0; i < numProperties; ++i)
        {
            DPLX_TRY(decode_object_property(stream, dest));
        }
    }
    return success();
}

template <packable_object T, input_stream Stream>
    requires(detail::versioned_decoder_enabled(layout_descriptor_for_v<T>))
class basic_decoder<T, Stream>
{
public:
    auto operator()(Stream &inStream, T &dest) const -> result<void>
    {
        DPLX_TRY(
                auto &&headInfo,
                dp::parse_object_head<Stream, layout_descriptor_for_v<T>.version
                                                      != null_def_version>(
                        inStream));

        if constexpr (layout_descriptor_for_v<T>.version != null_def_version)
        {
            if (layout_descriptor_for_v<T>.version != headInfo.version)
            {
                return errc::item_version_mismatch;
            }
        }

        return dp::decode_object_properties<layout_descriptor_for_v<T>, T,
                                            Stream>(inStream, dest,
                                                    headInfo.num_properties);
    }
};

} // namespace dplx::dp


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

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

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

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

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

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

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

namespace TRAC_IK {

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

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

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

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

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

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

  }

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

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

  }

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

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

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

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

      if (time_left <= 0)
        break;

      iksolver.setMaxtime(time_left);

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

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

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

    iksolver.setMaxtime(fulltime);

    return true;
  }

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

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

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

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

      if (time_left <= 0)
        break;

      nl_solver.setMaxtime(time_left);

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

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

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

    iksolver.abort();

    nl_solver.setMaxtime(fulltime);

    return true;
  }

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

    bool improved = false;

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

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

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

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

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

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

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

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

      solution(i) = val;
    }
  }

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

    bool improved = false;

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

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

      double target = seed(i);

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

      double val = solution(i);

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

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

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

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

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

      solution(i) = val;
    }

  }

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

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

    jacsolver.JntToJac(arr,jac);

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

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

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

    jacsolver.JntToJac(arr,jac);

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

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

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

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

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

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

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

    bounds=_bounds;

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

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

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

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

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

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

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

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

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

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

    return solutions.size();
  }

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

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

  }

}