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

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

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

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_HPP

#include <algorithm>
#include <type_traits>

#include <boost/config.hpp>
#include <boost/concept_check.hpp>

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

#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/point_in_point.hpp>
#include <boost/geometry/strategies/spherical/intersection.hpp>

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

#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK
#  include <boost/geometry/io/dsv/write.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

namespace comparable
{

/*
  Given a spherical segment AB and a point D, we are interested in
  computing the distance of D from AB. This is usually known as the
  cross track distance.

  If the projection (along great circles) of the point D lies inside
  the segment AB, then the distance (cross track error) XTD is given
  by the formula (see http://williams.best.vwh.net/avform.htm#XTE):

  XTD = asin( sin(dist_AD) * sin(crs_AD-crs_AB) )

  where dist_AD is the great circle distance between the points A and
  B, and crs_AD, crs_AB is the course (bearing) between the points A,
  D and A, B, respectively.

  If the point D does not project inside the arc AB, then the distance
  of D from AB is the minimum of the two distances dist_AD and dist_BD.

  Our reference implementation for this procedure is listed below
  (this was the old Boost.Geometry implementation of the cross track distance),
  where:
  * The member variable m_strategy is the underlying haversine strategy.
  * p stands for the point D.
  * sp1 stands for the segment endpoint A.
  * sp2 stands for the segment endpoint B.

  ================= reference implementation -- start =================

  return_type d1 = m_strategy.apply(sp1, p);
  return_type d3 = m_strategy.apply(sp1, sp2);

  if (geometry::math::equals(d3, 0.0))
  {
      // "Degenerate" segment, return either d1 or d2
      return d1;
  }

  return_type d2 = m_strategy.apply(sp2, p);

  return_type crs_AD = geometry::detail::course<return_type>(sp1, p);
  return_type crs_AB = geometry::detail::course<return_type>(sp1, sp2);
  return_type crs_BA = crs_AB - geometry::math::pi<return_type>();
  return_type crs_BD = geometry::detail::course<return_type>(sp2, p);
  return_type d_crs1 = crs_AD - crs_AB;
  return_type d_crs2 = crs_BD - crs_BA;

  // d1, d2, d3 are in principle not needed, only the sign matters
  return_type projection1 = cos( d_crs1 ) * d1 / d3;
  return_type projection2 = cos( d_crs2 ) * d2 / d3;

  if (projection1 > 0.0 && projection2 > 0.0)
  {
      return_type XTD
          = radius() * math::abs( asin( sin( d1 / radius() ) * sin( d_crs1 ) ));

      // Return shortest distance, projected point on segment sp1-sp2
      return return_type(XTD);
  }
  else
  {
      // Return shortest distance, project either on point sp1 or sp2
      return return_type( (std::min)( d1 , d2 ) );
  }

  ================= reference implementation -- end =================

  Motivation
  ----------
  In what follows we develop a comparable version of the cross track
  distance strategy, that meets the following goals:
  * It is more efficient than the original cross track strategy (less
    operations and less calls to mathematical functions).
  * Distances using the comparable cross track strategy can not only
    be compared with other distances using the same strategy, but also with
    distances computed with the comparable version of the haversine strategy.
  * It can serve as the basis for the computation of the cross track distance,
    as it is more efficient to compute its comparable version and
    transform that to the actual cross track distance, rather than
    follow/use the reference implementation listed above.

  Major idea
  ----------
  The idea here is to use the comparable haversine strategy to compute
  the distances d1, d2 and d3 in the above listing. Once we have done
  that we need also to make sure that instead of returning XTD (as
  computed above) that we return a distance CXTD that is compatible
  with the comparable haversine distance. To achieve this CXTD must satisfy
  the relation:
      XTD = 2 * R * asin( sqrt(XTD) )
  where R is the sphere's radius.

  Below we perform the mathematical analysis that show how to compute CXTD.

  Mathematical analysis
  ---------------------
  Below we use the following trigonometric identities:
      sin(2 * x) = 2 * sin(x) * cos(x)
      cos(asin(x)) = sqrt(1 - x^2)

  Observation:
  The distance d1 needed when the projection of the point D is within the
  segment must be the true distance. However, comparable::haversine<>
  returns a comparable distance instead of the one needed.
  To remedy this, we implicitly compute what is needed.
  More precisely, we need to compute sin(true_d1):

  sin(true_d1) = sin(2 * asin(sqrt(d1)))
               = 2 * sin(asin(sqrt(d1)) * cos(asin(sqrt(d1)))
               = 2 * sqrt(d1) * sqrt(1-(sqrt(d1))^2)
               = 2 * sqrt(d1 - d1 * d1)
  This relation is used below.

  As we mentioned above the goal is to find CXTD (named "a" below for
  brevity) such that ("b" below stands for "d1", and "c" for "d_crs1"):

      2 * R * asin(sqrt(a)) == R * asin(2 * sqrt(b-b^2) * sin(c))

  Analysis:
      2 * R * asin(sqrt(a)) == R * asin(2 * sqrt(b-b^2) * sin(c))
  <=> 2 * asin(sqrt(a)) == asin(sqrt(b-b^2) * sin(c))
  <=> sin(2 * asin(sqrt(a))) == 2 * sqrt(b-b^2) * sin(c)
  <=> 2 * sin(asin(sqrt(a))) * cos(asin(sqrt(a))) == 2 * sqrt(b-b^2) * sin(c)
  <=> 2 * sqrt(a) * sqrt(1-a) == 2 * sqrt(b-b^2) * sin(c)
  <=> sqrt(a) * sqrt(1-a) == sqrt(b-b^2) * sin(c)
  <=> sqrt(a-a^2) == sqrt(b-b^2) * sin(c)
  <=> a-a^2 == (b-b^2) * (sin(c))^2

  Consider the quadratic equation: x^2-x+p^2 == 0,
  where p = sqrt(b-b^2) * sin(c); its discriminant is:
      d = 1 - 4 * p^2 = 1 - 4 * (b-b^2) * (sin(c))^2

  The two solutions are:
      a_1 = (1 - sqrt(d)) / 2
      a_2 = (1 + sqrt(d)) / 2

  Which one to choose?
  "a" refers to the distance (on the unit sphere) of D from the
  supporting great circle Circ(A,B) of the segment AB.
  The two different values for "a" correspond to the lengths of the two
  arcs delimited D and the points of intersection of Circ(A,B) and the
  great circle perperdicular to Circ(A,B) passing through D.
  Clearly, the value we want is the smallest among these two distances,
  hence the root we must choose is the smallest root among the two.

  So the answer is:
      CXTD = ( 1 - sqrt(1 - 4 * (b-b^2) * (sin(c))^2) ) / 2

  Therefore, in order to implement the comparable version of the cross
  track strategy we need to:
  (1) Use the comparable version of the haversine strategy instead of
      the non-comparable one.
  (2) Instead of return XTD when D projects inside the segment AB, we
      need to return CXTD, given by the following formula:
          CXTD = ( 1 - sqrt(1 - 4 * (d1-d1^2) * (sin(d_crs1))^2) ) / 2;

  Complexity Analysis
  -------------------
  In the analysis that follows we refer to the actual implementation below.
  In particular, instead of computing CXTD as above, we use the more
  efficient (operation-wise) computation of CXTD shown here:

      return_type sin_d_crs1 = sin(d_crs1);
      return_type d1_x_sin = d1 * sin_d_crs1;
      return_type d = d1_x_sin * (sin_d_crs1 - d1_x_sin);
      return d / (0.5 + math::sqrt(0.25 - d));

  Notice that instead of computing:
      0.5 - 0.5 * sqrt(1 - 4 * d) = 0.5 - sqrt(0.25 - d)
  we use the following formula instead:
      d / (0.5 + sqrt(0.25 - d)).
  This is done for numerical robustness. The expression 0.5 - sqrt(0.25 - x)
  has large numerical errors for values of x close to 0 (if using doubles
  the error start to become large even when d is as large as 0.001).
  To remedy that, we re-write 0.5 - sqrt(0.25 - x) as:
      0.5 - sqrt(0.25 - d)
      = (0.5 - sqrt(0.25 - d) * (0.5 - sqrt(0.25 - d)) / (0.5 + sqrt(0.25 - d)).
  The numerator is the difference of two squares:
      (0.5 - sqrt(0.25 - d) * (0.5 - sqrt(0.25 - d))
      = 0.5^2 - (sqrt(0.25 - d))^ = 0.25 - (0.25 - d) = d,
  which gives the expression we use.

  For the complexity analysis, we distinguish between two cases:
  (A) The distance is realized between the point D and an
      endpoint of the segment AB

      Gains:
      Since we are using comparable::haversine<> which is called
      3 times, we gain:
      -> 3 calls to sqrt
      -> 3 calls to asin
      -> 6 multiplications

      Loses: None

      So the net gain is:
      -> 6 function calls (sqrt/asin)
      -> 6 arithmetic operations

      If we use comparable::cross_track<> to compute
      cross_track<> we need to account for a call to sqrt, a call
      to asin and 2 multiplications. In this case the net gain is:
      -> 4 function calls (sqrt/asin)
      -> 4 arithmetic operations

  (B) The distance is realized between the point D and an
      interior point of the segment AB

      Gains:
      Since we are using comparable::haversine<> which is called
      3 times, we gain:
      -> 3 calls to sqrt
      -> 3 calls to asin
      -> 6 multiplications
      Also we gain the operations used to compute XTD:
      -> 2 calls to sin
      -> 1 call to asin
      -> 1 call to abs
      -> 2 multiplications
      -> 1 division
      So the total gains are:
      -> 9 calls to sqrt/sin/asin
      -> 1 call to abs
      -> 8 multiplications
      -> 1 division

      Loses:
      To compute a distance compatible with comparable::haversine<>
      we need to perform a few more operations, namely:
      -> 1 call to sin
      -> 1 call to sqrt
      -> 2 multiplications
      -> 1 division
      -> 1 addition
      -> 2 subtractions

      So roughly speaking the net gain is:
      -> 8 fewer function calls and 3 fewer arithmetic operations

      If we were to implement cross_track directly from the
      comparable version (much like what haversine<> does using
      comparable::haversine<>) we need additionally
      -> 2 function calls (asin/sqrt)
      -> 2 multiplications

      So it pays off to re-implement cross_track<> to use
      comparable::cross_track<>; in this case the net gain would be:
      -> 6 function calls
      -> 1 arithmetic operation

   Summary/Conclusion
   ------------------
   Following the mathematical and complexity analysis above, the
   comparable cross track strategy (as implemented below) satisfies
   all the goal mentioned in the beginning:
   * It is more efficient than its non-comparable counter-part.
   * Comparable distances using this new strategy can also be compared
     with comparable distances computed with the comparable haversine
     strategy.
   * It turns out to be more efficient to compute the actual cross
     track distance XTD by first computing CXTD, and then computing
     XTD by means of the formula:
                XTD = 2 * R * asin( sqrt(CXTD) )
*/

template
<
    typename CalculationType = void,
    typename Strategy = comparable::haversine<double, CalculationType>
>
class cross_track
{
public:
    template <typename Point, typename PointOfSegment>
    struct return_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point,
                      PointOfSegment,
                      CalculationType
                  >::type
          >
    {};

    typedef typename Strategy::radius_type radius_type;

    cross_track() = default;

    explicit inline cross_track(typename Strategy::radius_type const& r)
        : m_strategy(r)
    {}

    inline cross_track(Strategy const& s)
        : m_strategy(s)
    {}

    // It might be useful in the future
    // to overload constructor with strategy info.
    // crosstrack(...) {}

    template <typename Point, typename PointOfSegment>
    inline typename return_type<Point, PointOfSegment>::type
    apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
    {

#if !defined(BOOST_MSVC)
        BOOST_CONCEPT_ASSERT
            (
                (concepts::PointDistanceStrategy<Strategy, Point, PointOfSegment>)
            );
#endif

        typedef typename return_type<Point, PointOfSegment>::type return_type;

        // http://williams.best.vwh.net/avform.htm#XTE
        return_type d1 = m_strategy.apply(sp1, p);
        return_type d3 = m_strategy.apply(sp1, sp2);

        if (geometry::math::equals(d3, 0.0))
        {
            // "Degenerate" segment, return either d1 or d2
            return d1;
        }

        return_type d2 = m_strategy.apply(sp2, p);

        return_type lon1 = geometry::get_as_radian<0>(sp1);
        return_type lat1 = geometry::get_as_radian<1>(sp1);
        return_type lon2 = geometry::get_as_radian<0>(sp2);
        return_type lat2 = geometry::get_as_radian<1>(sp2);
        return_type lon = geometry::get_as_radian<0>(p);
        return_type lat = geometry::get_as_radian<1>(p);

        return_type crs_AD = geometry::formula::spherical_azimuth<return_type, false>
                             (lon1, lat1, lon, lat).azimuth;

        geometry::formula::result_spherical<return_type> result =
                geometry::formula::spherical_azimuth<return_type, true>
                    (lon1, lat1, lon2, lat2);
        return_type crs_AB = result.azimuth;
        return_type crs_BA = result.reverse_azimuth - geometry::math::pi<return_type>();

        return_type crs_BD = geometry::formula::spherical_azimuth<return_type, false>
                             (lon2, lat2, lon, lat).azimuth;

        return_type d_crs1 = crs_AD - crs_AB;
        return_type d_crs2 = crs_BD - crs_BA;

        // d1, d2, d3 are in principle not needed, only the sign matters
        return_type projection1 = cos( d_crs1 ) * d1 / d3;
        return_type projection2 = cos( d_crs2 ) * d2 / d3;

#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK
        std::cout << "Course " << dsv(sp1) << " to " << dsv(p) << " "
                  << crs_AD * geometry::math::r2d<return_type>() << std::endl;
        std::cout << "Course " << dsv(sp1) << " to " << dsv(sp2) << " "
                  << crs_AB * geometry::math::r2d<return_type>() << std::endl;
        std::cout << "Course " << dsv(sp2) << " to " << dsv(sp1) << " "
                  << crs_BA * geometry::math::r2d<return_type>() << std::endl;
        std::cout << "Course " << dsv(sp2) << " to " << dsv(p) << " "
                  << crs_BD * geometry::math::r2d<return_type>() << std::endl;
        std::cout << "Projection AD-AB " << projection1 << " : "
                  << d_crs1 * geometry::math::r2d<return_type>() << std::endl;
        std::cout << "Projection BD-BA " << projection2 << " : "
                  << d_crs2 * geometry::math::r2d<return_type>() << std::endl;
        std::cout << " d1: " << (d1 )
                  << " d2: " << (d2 )
                  << std::endl;
#endif

        if (projection1 > 0.0 && projection2 > 0.0)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK
            return_type XTD = radius() * geometry::math::abs( asin( sin( d1 ) * sin( d_crs1 ) ));

            std::cout << "Projection ON the segment" << std::endl;
            std::cout << "XTD: " << XTD
                      << " d1: " << (d1 * radius())
                      << " d2: " << (d2 * radius())
                      << std::endl;
#endif
            return_type const half(0.5);
            return_type const quarter(0.25);

            return_type sin_d_crs1 = sin(d_crs1);
            /*
              This is the straightforward obvious way to continue:

              return_type discriminant
                  = 1.0 - 4.0 * (d1 - d1 * d1) * sin_d_crs1 * sin_d_crs1;
              return 0.5 - 0.5 * math::sqrt(discriminant);

              Below we optimize the number of arithmetic operations
              and account for numerical robustness:
            */
            return_type d1_x_sin = d1 * sin_d_crs1;
            return_type d = d1_x_sin * (sin_d_crs1 - d1_x_sin);
            return d / (half + math::sqrt(quarter - d));
        }
        else
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK
            std::cout << "Projection OUTSIDE the segment" << std::endl;
#endif

            // Return shortest distance, project either on point sp1 or sp2
            return return_type( (std::min)( d1 , d2 ) );
        }
    }

    template <typename T1, typename T2>
    inline radius_type vertical_or_meridian(T1 lat1, T2 lat2) const
    {
        return m_strategy.radius() * (lat1 - lat2);
    }

    inline typename Strategy::radius_type radius() const
    { return m_strategy.radius(); }

private :
    Strategy m_strategy;
};

} // namespace comparable

/*!
\brief Strategy functor for distance point to segment calculation
\ingroup strategies
\details Class which calculates the distance of a point to a segment, for points on a sphere or globe
\see http://williams.best.vwh.net/avform.htm
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-point distance strategy, defaults to haversine

\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename CalculationType = void,
    typename Strategy = haversine<double, CalculationType>
>
class cross_track
{
public :
    typedef within::spherical_point_point equals_point_point_strategy_type;

    typedef intersection::spherical_segments
        <
            CalculationType
        > relate_segment_segment_strategy_type;

    static inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy()
    {
        return relate_segment_segment_strategy_type();
    }

    typedef within::spherical_winding
        <
            void, void, CalculationType
        > point_in_geometry_strategy_type;

    static inline point_in_geometry_strategy_type get_point_in_geometry_strategy()
    {
        return point_in_geometry_strategy_type();
    }

    template <typename Point, typename PointOfSegment>
    struct return_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point,
                      PointOfSegment,
                      CalculationType
                  >::type
          >
    {};

    typedef typename Strategy::radius_type radius_type;

    inline cross_track()
    {}

    explicit inline cross_track(typename Strategy::radius_type const& r)
        : m_strategy(r)
    {}

    inline cross_track(Strategy const& s)
        : m_strategy(s)
    {}

    // It might be useful in the future
    // to overload constructor with strategy info.
    // crosstrack(...) {}

    template <typename Point, typename PointOfSegment>
    inline typename return_type<Point, PointOfSegment>::type
    apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
    {

#if !defined(BOOST_MSVC)
        BOOST_CONCEPT_ASSERT
            (
                (concepts::PointDistanceStrategy<Strategy, Point, PointOfSegment>)
            );
#endif
        typedef typename return_type<Point, PointOfSegment>::type return_type;
        typedef cross_track<CalculationType, Strategy> this_type;

        typedef typename services::comparable_type
            <
                this_type
            >::type comparable_type;

        comparable_type cstrategy
            = services::get_comparable<this_type>::apply(m_strategy);

        return_type const a = cstrategy.apply(p, sp1, sp2);
        return_type const c = return_type(2.0) * asin(math::sqrt(a));
        return c * radius();
    }

    template <typename T1, typename T2>
    inline radius_type vertical_or_meridian(T1 lat1, T2 lat2) const
    {
        return m_strategy.radius() * (lat1 - lat2);
    }

    inline typename Strategy::radius_type radius() const
    { return m_strategy.radius(); }

private :

    Strategy m_strategy;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<cross_track<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_point_segment type;
};

template <typename CalculationType, typename Strategy, typename P, typename PS>
struct return_type<cross_track<CalculationType, Strategy>, P, PS>
    : cross_track<CalculationType, Strategy>::template return_type<P, PS>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<cross_track<CalculationType, Strategy> >
{
    typedef comparable::cross_track
        <
            CalculationType, typename comparable_type<Strategy>::type
        >  type;
};

template
<
    typename CalculationType,
    typename Strategy
>
struct get_comparable<cross_track<CalculationType, Strategy> >
{
    typedef typename comparable_type
        <
            cross_track<CalculationType, Strategy>
        >::type comparable_type;
public :
    static inline comparable_type
    apply(cross_track<CalculationType, Strategy> const& strategy)
    {
        return comparable_type(strategy.radius());
    }
};

template
<
    typename CalculationType,
    typename Strategy,
    typename P,
    typename PS
>
struct result_from_distance<cross_track<CalculationType, Strategy>, P, PS>
{
private :
    typedef typename cross_track
        <
            CalculationType, Strategy
        >::template return_type<P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(cross_track<CalculationType, Strategy> const& , T const& distance)
    {
        return distance;
    }
};

// Specializations for comparable::cross_track
template <typename RadiusType, typename CalculationType>
struct tag<comparable::cross_track<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_segment type;
};

template
<
    typename RadiusType,
    typename CalculationType,
    typename P,
    typename PS
>
struct return_type<comparable::cross_track<RadiusType, CalculationType>, P, PS>
    : comparable::cross_track
        <
            RadiusType, CalculationType
        >::template return_type<P, PS>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<comparable::cross_track<RadiusType, CalculationType> >
{
    typedef comparable::cross_track<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<comparable::cross_track<RadiusType, CalculationType> >
{
private :
    typedef comparable::cross_track<RadiusType, CalculationType> this_type;
public :
    static inline this_type apply(this_type const& input)
    {
        return input;
    }
};

template
<
    typename RadiusType,
    typename CalculationType,
    typename P,
    typename PS
>
struct result_from_distance
    <
        comparable::cross_track<RadiusType, CalculationType>, P, PS
    >
{
private :
    typedef comparable::cross_track<RadiusType, CalculationType> strategy_type;
    typedef typename return_type<strategy_type, P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type apply(strategy_type const& strategy,
                                    T const& distance)
    {
        return_type const s
            = sin( (distance / strategy.radius()) / return_type(2.0) );
        return s * s;
    }
};

/*

TODO:  spherical polar coordinate system requires "get_as_radian_equatorial<>"

template <typename Point, typename PointOfSegment, typename Strategy>
struct default_strategy
    <
        segment_tag, Point, PointOfSegment,
        spherical_polar_tag, spherical_polar_tag,
        Strategy
    >
{
    typedef cross_track
        <
            void,
            std::conditional_t
                <
                    std::is_void<Strategy>::value,
                    typename default_strategy
                        <
                            point_tag, Point, PointOfSegment,
                            spherical_polar_tag, spherical_polar_tag
                        >::type,
                    Strategy
                >
        > type;
};
*/

template <typename Point, typename PointOfSegment, typename Strategy>
struct default_strategy
    <
        point_tag, segment_tag, Point, PointOfSegment,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef cross_track
        <
            void,
            std::conditional_t
                <
                    std::is_void<Strategy>::value,
                    typename default_strategy
                        <
                            point_tag, point_tag, Point, PointOfSegment,
                            spherical_equatorial_tag, spherical_equatorial_tag
                        >::type,
                    Strategy
                >
        > type;
};

template <typename PointOfSegment, typename Point, typename Strategy>
struct default_strategy
    <
        segment_tag, point_tag, PointOfSegment, Point,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef typename default_strategy
        <
            point_tag, segment_tag, Point, PointOfSegment,
            spherical_equatorial_tag, spherical_equatorial_tag,
            Strategy
        >::type type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_HPP


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

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int main(int, char**)
{
    mtl::dense2D<double> A(3, 3), B(3, 3), C(3, 3);
    A= 2.0; B= 3.0;
#if 0
    A= B * C * 8;
    cout << "B * C * 8 is\n" << A << '\n';

    A= B * (C * 8);
    cout << "B * (C * 8) is\n" << '\n';
#endif
    return 0;
}


#include <Core/SettingsFields.h>

#include <Core/Field.h>
#include <Common/getNumberOfPhysicalCPUCores.h>
#include <Common/FieldVisitors.h>
#include <common/logger_useful.h>
#include <IO/ReadHelpers.h>
#include <IO/ReadBufferFromString.h>
#include <IO/WriteHelpers.h>
#include <boost/algorithm/string/predicate.hpp>

namespace DB
{
namespace ErrorCodes
{
    extern const int SIZE_OF_FIXED_STRING_DOESNT_MATCH;
    extern const int CANNOT_PARSE_BOOL;
}

namespace
{
    template <typename T>
    T stringToNumber(const String & str)
    {
        if constexpr (std::is_same_v<T, bool>)
        {
            if (str == "0")
                return false;
            if (str == "1")
                return true;
            if (boost::iequals(str, "false"))
                return false;
            if (boost::iequals(str, "true"))
                return true;
            throw Exception("Cannot parse bool from string '" + str + "'", ErrorCodes::CANNOT_PARSE_BOOL);
        }
        else
            return parseWithSizeSuffix<T>(str);
    }

    template <typename T>
    T fieldToNumber(const Field & f)
    {
        if (f.getType() == Field::Types::String)
            return stringToNumber<T>(f.get<const String &>());
        else
            return applyVisitor(FieldVisitorConvertToNumber<T>(), f);
    }
}

template <typename T>
SettingFieldNumber<T>::SettingFieldNumber(const Field & f) : SettingFieldNumber(fieldToNumber<T>(f))
{
}

template <typename T>
SettingFieldNumber<T> & SettingFieldNumber<T>::operator=(const Field & f)
{
    *this = fieldToNumber<T>(f);
    return *this;
}

template <typename T>
String SettingFieldNumber<T>::toString() const
{
    return ::DB::toString(value);
}

template <typename T>
void SettingFieldNumber<T>::parseFromString(const String & str)
{
    *this = stringToNumber<T>(str);
}

template <typename T>
void SettingFieldNumber<T>::writeBinary(WriteBuffer & out) const
{
    if constexpr (std::is_integral_v<T> && is_unsigned_v<T>)
        writeVarUInt(static_cast<UInt64>(value), out);
    else if constexpr (std::is_integral_v<T> && is_signed_v<T>)
        writeVarInt(static_cast<Int64>(value), out);
    else
    {
        static_assert(std::is_floating_point_v<T>);
        writeStringBinary(::DB::toString(value), out);
    }
}

template <typename T>
void SettingFieldNumber<T>::readBinary(ReadBuffer & in)
{
    if constexpr (std::is_integral_v<T> && is_unsigned_v<T>)
    {
        UInt64 x;
        readVarUInt(x, in);
        *this = static_cast<T>(x);
    }
    else if constexpr (std::is_integral_v<T> && is_signed_v<T>)
    {
        Int64 x;
        readVarInt(x, in);
        *this = static_cast<T>(value);
    }
    else
    {
        static_assert(std::is_floating_point_v<T>);
        String str;
        readStringBinary(str, in);
        *this = ::DB::parseFromString<T>(str);
    }
}

template struct SettingFieldNumber<UInt64>;
template struct SettingFieldNumber<Int64>;
template struct SettingFieldNumber<float>;
template struct SettingFieldNumber<bool>;

namespace
{
    UInt64 stringToMaxThreads(const String & str)
    {
        if (startsWith(str, "auto"))
            return 0;
        return parseFromString<UInt64>(str);
    }

    UInt64 fieldToMaxThreads(const Field & f)
    {
        if (f.getType() == Field::Types::String)
            return stringToMaxThreads(f.get<const String &>());
        else
            return applyVisitor(FieldVisitorConvertToNumber<UInt64>(), f);
    }
}

SettingFieldMaxThreads::SettingFieldMaxThreads(const Field & f) : SettingFieldMaxThreads(fieldToMaxThreads(f))
{
}

SettingFieldMaxThreads & SettingFieldMaxThreads::operator=(const Field & f)
{
    *this = fieldToMaxThreads(f);
    return *this;
}

String SettingFieldMaxThreads::toString() const
{
    if (is_auto)
        return "'auto(" + ::DB::toString(value) + ")'";
    else
        return ::DB::toString(value);
}

void SettingFieldMaxThreads::parseFromString(const String & str)
{
    *this = stringToMaxThreads(str);
}

void SettingFieldMaxThreads::writeBinary(WriteBuffer & out) const
{
    writeVarUInt(is_auto ? 0 : value, out);
}

void SettingFieldMaxThreads::readBinary(ReadBuffer & in)
{
    UInt64 x = 0;
    readVarUInt(x, in);
    *this = x;
}

UInt64 SettingFieldMaxThreads::getAuto()
{
    return getNumberOfPhysicalCPUCores();
}

template <SettingFieldTimespanUnit unit_>
SettingFieldTimespan<unit_>::SettingFieldTimespan(const Field & f) : SettingFieldTimespan(fieldToNumber<UInt64>(f))
{
}

template <SettingFieldTimespanUnit unit_>
SettingFieldTimespan<unit_> & SettingFieldTimespan<unit_>::operator=(const Field & f)
{
    *this = fieldToNumber<UInt64>(f);
    return *this;
}

template <SettingFieldTimespanUnit unit_>
String SettingFieldTimespan<unit_>::toString() const
{
    return ::DB::toString(operator UInt64());
}

template <SettingFieldTimespanUnit unit_>
void SettingFieldTimespan<unit_>::parseFromString(const String & str)
{
    *this = stringToNumber<UInt64>(str);
}

template <SettingFieldTimespanUnit unit_>
void SettingFieldTimespan<unit_>::writeBinary(WriteBuffer & out) const
{
    auto num_units = operator UInt64();
    writeVarUInt(num_units, out);
}

template <SettingFieldTimespanUnit unit_>
void SettingFieldTimespan<unit_>::readBinary(ReadBuffer & in)
{
    UInt64 num_units = 0;
    readVarUInt(num_units, in);
    *this = num_units;
}

template struct SettingFieldTimespan<SettingFieldTimespanUnit::Second>;
template struct SettingFieldTimespan<SettingFieldTimespanUnit::Millisecond>;

void SettingFieldString::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(value, out);
}

void SettingFieldString::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    *this = std::move(str);
}

namespace
{
    char stringToChar(const String & str)
    {
        if (str.size() > 1)
            throw Exception("A setting's value string has to be an exactly one character long", ErrorCodes::SIZE_OF_FIXED_STRING_DOESNT_MATCH);
        if (str.empty())
            return '\0';
        return str[0];
    }

    char fieldToChar(const Field & f)
    {
        return stringToChar(f.safeGet<const String &>());
    }
}

SettingFieldChar::SettingFieldChar(const Field & f) : SettingFieldChar(fieldToChar(f))
{
}

SettingFieldChar & SettingFieldChar::operator =(const Field & f)
{
    *this = fieldToChar(f);
    return *this;
}

void SettingFieldChar::parseFromString(const String & str)
{
    *this = stringToChar(str);
}

void SettingFieldChar::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(toString(), out);
}

void SettingFieldChar::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    *this = stringToChar(str);
}

void SettingFieldURI::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(value.toString(), out);
}

void SettingFieldURI::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    *this = Poco::URI{str};
}

void SettingFieldEnumHelpers::writeBinary(const std::string_view & str, WriteBuffer & out)
{
    writeStringBinary(str, out);
}

String SettingFieldEnumHelpers::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    return str;
}

String SettingFieldCustom::toString() const
{
    return value.dump();
}

void SettingFieldCustom::parseFromString(const String & str)
{
    *this = Field::restoreFromDump(str);
}

void SettingFieldCustom::writeBinary(WriteBuffer & out) const
{
    writeStringBinary(toString(), out);
}

void SettingFieldCustom::readBinary(ReadBuffer & in)
{
    String str;
    readStringBinary(str, in);
    parseFromString(str);
}

}


#include <boost/numeric/interval/rounded_transc.hpp>


//./warp --in /home/tim/osm/hor_common/11011_05.jpg --points 11011_05.csv --out 11011_05 --width 2048 --height 2048 --fit 2 --vis
//./warp --in /home/tim/osm/os/PortCarlisle2small.jpg --points portcar.csv --out portcar --width 2048 --height 2048 --fit 1 --vis
#include <iostream>
#include <fstream>
#include <iomanip>
using namespace std;
#include "TransformPoly.h"
#include "WriteKml.h"
#include "ErrorHandling.h"
#include "ImageWarpByFunc.h"
#include "ReadDelimitedFile.h"
#include "ganzc/LatLong-OSGBconversion.h"
#include "Gbos1936.h"
#include <boost/program_options.hpp>
namespace po = boost::program_options;

class Tile
{
public:
	double latmin, latmax, lonmin,lonmax;
	int sx, sy;
	Tile();
	virtual ~Tile();
	void Project(double lat, double lon, double &ixOut, double &iyOut);
	void Project(double lat, double lon, vector<double> &out);
	void UnProject(double x, double y, double &latOut, double &lonOut);
};

Tile::Tile()
{
	latmin=0.0;latmax=1.0;lonmin=0.0;lonmax=1.0;
	sx=256;sy=256;
}

Tile::~Tile()
{

}

void Tile::Project(double lat, double lon, double &ixOut, double &iyOut)
{
	iyOut = (double)sy - (double)sy * (lat - latmin) / (latmax - latmin);
	ixOut = (double)sx * (lon - lonmin) / (lonmax - lonmin);
	//cout << lat << "," << lon << "," << ixOut << "," << iyOut << endl;
}

void Tile::Project(double lat, double lon, vector<double> &out)
{
	double iy, ix;
	Project(lat,lon,ix,iy);
	out.clear();
	out.push_back(ix);
	out.push_back(iy);
}

void Tile::UnProject(double x, double y, double &latOut, double &lonOut)
{
	latOut = ((double)sy - y) * (latmax - latmin) / (double)sy  + latmin;
	lonOut = x * (lonmax - lonmin) / (double)sx + lonmin;
}

//****************************************************

vector<double> PolyProjectWithPtr(vector<double> in, void *userPtr)
{
	vector<double> *poly = (vector<double> *)userPtr;
	vector<double> pout = PolyProject(in, *poly);
	//cout << in[0] << "\t" << in[1] << "\t" << pout[0] << "\t" << pout[1] << endl;
	return pout;
}

void AddPointPoly(class Tile &tile,class PolyProjection &polyEst, double lat, double lon, double x, double y)
{
	//cout << "AddPointPoly " << lat << "," << lon << "," << x << "," << y << endl;
	vector<double> p;
	tile.Project(lat,lon,p);
	polyEst.AddPoint(x,y,p);
}

void SplitGbosRef(string in, string &zone, long &easting, long &northing)
{
	//cout << in << endl;
	zone = in.substr(0,2);
	if(in.length() != 8) ThrowError<logic_error>("Unexpected length of map reference",__LINE__,__FILE__);
	easting = atoi(in.substr(2,3).c_str())*100;
	northing = atoi(in.substr(5,3).c_str())*100;
	//cout << zone << "," << easting << "," << northing << endl;
}

//**************************************************

int main(int argc, char *argv[])
{
	string inputImageFilename = "/home/tim/Desktop/hor_common/11011_01.jpg";
	string inputPointsFilename = "points.csv";
	int polynomialOrder = 5;
	string outputFilename = "map";
	int visualiseErrors = 0;
	int outputWidth = -1;
	int outputHeight = -1;

	/*double Lat = 54.9785 ;
	double Long = -3.1897;
	long OSGBNorthing;
	long OSGBEasting;
	char OSGBZone[4];

	cout << "Starting position(Lat, Long):  " << Lat << "   " << Long <<endl;

	LLtoOSGB(Lat, Long, OSGBEasting, OSGBNorthing, OSGBZone);
	cout << setiosflags(ios::showpoint | ios::fixed) << setprecision(6);
	cout << "Calculated OSGB position(Northing, Easting, GridSquare):  ";
	cout << OSGBZone << " " << OSGBEasting << " " << OSGBNorthing << endl;

	OSGBtoLL(OSGBNorthing, OSGBEasting, OSGBZone, Lat, Long);
	cout << "Calculated Lat, Long position(Lat, Long):  " << Lat << "   " << Long << endl <<endl;*/

	//Process program options using boost library
	po::variables_map vm;
	po::options_description desc("Allowed options");
	try{
	desc.add_options() ("in,i", po::value<string>(), "input image filename")
		("points,p",po::value<string>(),"points to define transformation")
		("out,o",po::value<string>(),"output name (extension is added automatically)")
		("vis,v","visualisation of error")
		("fit,f",po::value<int>(),"order of polynomial")
		("width,w",po::value<int>(),"output width")
		("height,h",po::value<int>(),"output height")
		("help","help message");

		//("annot-offset",po::value<double>(),"time offset of anvil annotation track")
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if(vm.count("help")) {cout << desc << endl; exit(0);}
	if(vm.count("in")) inputImageFilename = vm["in"].as<string>();
	if(vm.count("points")) inputPointsFilename = vm["points"].as<string>();
	if(vm.count("out")) outputFilename = vm["out"].as<string>();
	if(vm.count("vis")) visualiseErrors = 1;
	if(vm.count("fit")) polynomialOrder = vm["fit"].as<int>();
	if(vm.count("width")) outputWidth = vm["width"].as<int>();
	if(vm.count("height")) outputHeight = vm["height"].as<int>();

	//Check basic parameters are set
	//if(inputFilename.size() == 0)
	//{cout << "Error: input sequence must be specified" << endl << desc << endl; exit(0);}

	}
	catch(exception &e)
	{
		cerr << "error: " << e.what() << endl;
	}

	/*CImg<unsigned char> img;
	cimg::exception_mode()=0;
	try{
	//img.load("/home/tim/Desktop/hor_common/11011_01thumb.jpg");
	img.load(inputImageFilename.c_str());
	//img.resize(img.dimx(),img.dimy(),1,3);
	}
	catch(cimg_library::CImgIOException e)
	{
		cout << desc << endl;
		exit(-1);
	}
	cimg::exception_mode()=1;*/
	class ImgMagick img;
	int openRet = img.Open(inputImageFilename.c_str());
	if(openRet < 0) {cout << "open " << inputImageFilename << " failed" << endl; exit(-1);}

	if(outputWidth == -1) outputWidth = img.GetWidth();
	if(outputHeight == -1) outputHeight = img.GetHeight();
	class Tile tile;
	tile.latmin = 0.29;
	tile.latmax = 1.32;
	tile.lonmin = 0.60;
	tile.lonmax = 0.65;
	tile.sx=outputWidth;
	tile.sy=outputHeight;

	class DelimitedFile pointDef;
	if(pointDef.Open(inputPointsFilename.c_str()) < 0)
	{
		cout << "File not found" << endl;
		cout << desc << endl;
		exit(-1);
	}

	//Read points to fix max extend to get a rough tile fit
	double north=0.0, south=0.0, east=0.0, west=0.0; int setBox = 0;
	for(unsigned int i=0;i<pointDef.NumLines();i++)
	{
		class DelimitedFileLine &line = pointDef[i];
		//cout << line.NumVals() << endl;

		if(line.NumVals()==5)
		{
			if(strcmp(line[0].GetVals(),"p")==0)
			{
			double ix = line[1].GetVald();
			double iy = line[2].GetVald();
			if(ix < south || !setBox) south = ix;
			if(ix > north || !setBox) north = ix;
			if(iy < west || !setBox) west = iy;
			if(iy > east || !setBox) east = iy;
			setBox = 1;
			}
		}
		if(line.NumVals()==4) //Read in GB OS national grid data
		{
			if(strcmp(line[0].GetVals(),"os")==0)
			{
			string mapref = line[1].GetVals();

			string zone; long easting=0.0, northing=0.0;
			SplitGbosRef(mapref, zone, easting, northing);
			double lat=-1.0, lon=-1.0;
			OSGBtoLL(northing, easting, zone.c_str(), lat, lon);

			if(lat < south || !setBox) south = lat;
			if(lat > north || !setBox) north = lat;
			if(lon < west || !setBox) west = lon;
			if(lon > east || !setBox) east = lon;
			setBox = 1;
			}
		}

		if(line.NumVals()==5) //Read in GB OS national grid data
		{
			if(strcmp(line[0].GetVals(),"gbos1936")==0)
			{
			string easting = line[1].GetVals();
			string northing = line[2].GetVals();

			double lat=-1.0, lon=-1.0, alt = -1.0;
			ConvertGbos1936ToWgs84(atof(easting.c_str()), atof(northing.c_str()),0.0, lat, lon, alt);

			if(lat < south || !setBox) south = lat;
			if(lat > north || !setBox) north = lat;
			if(lon < west || !setBox) west = lon;
			if(lon > east || !setBox) east = lon;
			setBox = 1;
			}
		}

	}
	cout << "Approx bounding box " << north << "," << south << "," << east << "," << west << endl;
	if(setBox)
	{
		tile.latmin = south;
		tile.latmax = north;
		tile.lonmin = west;
		tile.lonmax = east;
	}

	class PolyProjection polyEst;
	int imageCoordSizex = -1, imageCoordSizey = -1;

	//Read points from delimited file into transformation estimator
	for(unsigned int i=0;i<pointDef.NumLines();i++)
	{
		class DelimitedFileLine &line = pointDef[i];
		if(line.NumVals()>=1)
		{
			if(strcmp(line[0].GetVals(), "imagesize")==0 && line.NumVals()>=3)
			{
				imageCoordSizex = line[1].GetVali();
				imageCoordSizey = line[2].GetVali();
				cout << "Read image size is " << imageCoordSizex << "," << imageCoordSizey << endl;
			}
		}

		if(line.NumVals()==5) //Read in WGS84 lat lon
		{
			if(strcmp(line[0].GetVals(),"p")==0)
			{
			if(imageCoordSizex >= 1 && imageCoordSizey >= 1)
			{
				double ix = line[1].GetVald();
				double iy = line[2].GetVald();
				double ox = img.GetWidth()*line[3].GetVald()/(double)imageCoordSizex;
				double oy = img.GetHeight()*line[4].GetVald()/(double)imageCoordSizey;
				//cout << ix << "," << iy << "," << ox << "," << oy << endl;
				AddPointPoly(tile, polyEst,ix,iy,ox,oy);
			}
			else
			{
				AddPointPoly(tile, polyEst, line[1].GetVald(),line[2].GetVald(),
					line[3].GetVald(),line[4].GetVald());
			}
			}
		}

		if(line.NumVals()==4) //Read in GB OS national grid data
		{
			if(strcmp(line[0].GetVals(),"os")==0)
			{
			string mapref = line[1].GetVals();

			string zone; long easting=0.0, northing=0.0;
			SplitGbosRef(mapref, zone, easting, northing);
			double lat=-1.0, lon=-1.0;
			OSGBtoLL(northing, easting, zone.c_str(), lat, lon);
			cout << mapref << " -> " << lat << "\t" << lon << endl;

			if(imageCoordSizex >= 1 && imageCoordSizey >= 1)
			{

				double ox = img.GetWidth()*line[2].GetVald()/(double)imageCoordSizex;
				double oy = img.GetHeight()*line[3].GetVald()/(double)imageCoordSizey;
				//cout << ix << "," << iy << "," << ox << "," << oy << endl;
				AddPointPoly(tile, polyEst,lat,lon,ox,oy);
			}
			else
			{
				AddPointPoly(tile, polyEst, lat,lon,
					line[2].GetVald(),line[3].GetVald());
			}
			}
		}

		if(line.NumVals()==5) //Read in GB OS national grid data
		{
			if(strcmp(line[0].GetVals(),"gbos1936")!=0) continue;
			string easting = line[1].GetVals();
			string northing = line[2].GetVals();

			double lat=-1.0, lon=-1.0, alt = -1.0;
			ConvertGbos1936ToWgs84(atof(easting.c_str()), atof(northing.c_str()),0.0, lat, lon, alt);

			if(imageCoordSizex >= 1 && imageCoordSizey >= 1)
			{

				double ox = img.GetWidth()*line[3].GetVald()/(double)imageCoordSizex;
				double oy = img.GetHeight()*line[4].GetVald()/(double)imageCoordSizey;
				//cout << ix << "," << iy << "," << ox << "," << oy << endl;
				AddPointPoly(tile, polyEst,lat,lon,ox,oy);
			}
			else
			{
				AddPointPoly(tile, polyEst, lat,lon,
					line[3].GetVald(),line[4].GetVald());
			}
		}

	}

	polyEst.order = polynomialOrder;
	vector<double> poly = polyEst.Estimate();

	//Calculate transformed image size (currently not used)
	double warpMinLat=0.0, warpMaxLat=0.0, warpMinLon=0.0, warpMaxLon=0.0;
	int warpBoxSet = 0;
	for(int i=0;i<100;i++)
	for(int j=0;j<100;j++)
	{
		vector<double>p;
		p.push_back((double)i * (double)img.GetWidth() / (double)(100-1));
 		p.push_back((double)j * (double)img.GetHeight() / (double)(100-1));
		vector<double> pt = PolyProjectWithPtr(p,(void *)&poly);
		double lat=-1.0, lon=-1.0;
		tile.UnProject(pt[0],pt[1],lat,lon);
		//cout << lat << "," << lon << endl;

		if(lat < warpMinLat || !warpBoxSet)warpMinLat = lat;
		if(lat > warpMaxLat || !warpBoxSet)warpMaxLat = lat;
		if(lon < warpMinLon || !warpBoxSet)warpMinLon = lon;
		if(lon > warpMaxLon || !warpBoxSet)warpMaxLon = lon;
		warpBoxSet = 1;
	}

	class ImgMagick endImage;
	endImage.SetNumChannels(3);
	endImage.SetWidth(tile.sx);
	endImage.SetHeight(tile.sy);

	//Transform image
	class ImageWarpByFunc imageWarpByFunc;
	imageWarpByFunc.xsize= 100;
	imageWarpByFunc.ysize= 100;
	imageWarpByFunc.Warp(img, endImage, PolyProjectWithPtr, (void *)&poly);

	/*if(visualiseErrors)
	for(unsigned int i=0;i<polyEst.transformedPoints.size();i++)
	{
		unsigned char col[3] = {255,0,0};
		//cout << polyEst.transformedPoints[i][0] << "," << polyEst.transformedPoints[i][1] << endl;
		endImage.draw_circle(polyEst.transformedPoints[i][0],polyEst.transformedPoints[i][1],3,col);

		vector<double> proj = PolyProject(polyEst.originalPoints[i], poly);
		unsigned char col2[3] = {0,0,255};
		endImage.draw_circle(proj[0],proj[1],3,col2);
		endImage.draw_line(polyEst.transformedPoints[i][0],polyEst.transformedPoints[i][1], proj[0],proj[1], col2);
	}*/

	string mapOutFilename = outputFilename + ".png";
	endImage.Save(mapOutFilename.c_str());

	class WriteKml writeKml;
	writeKml.north = tile.latmax;
	writeKml.south = tile.latmin;
	writeKml.west = tile.lonmin;
	writeKml.east = tile.lonmax;
	string kmlOutFilename = outputFilename + ".kml";
	writeKml.href = mapOutFilename;
	cout << "Writing KML to " << kmlOutFilename << endl;
	writeKml.WriteToFile(kmlOutFilename.c_str());

}


/*********************************************************************
*
* Author: Noé Pérez Higueras
*********************************************************************/

#include <local_3d_planner/local_3d_planner.h>
//#include <costmap_2d/footprint.h>
#include <string>
#include <sstream>
#include <math.h>
#include <angles/angles.h>

#include <boost/algorithm/string.hpp>

#include <ros/console.h>

// for computing path distance
#include <queue>

using namespace std;
// using namespace costmap_2d;

namespace local_3d_planner
{
/*void UpoPlanner::reconfigure(SimpleLocalPlannerConfig &cfg)
{
    SimpleLocalPlannerConfig config(cfg);

    boost::mutex::scoped_lock l(configuration_mutex_);

  acc_lim_trans_ = config.max_trans_acc;
  acc_lim_rot_ = config.max_rot_acc;
  max_vel_x_ = config.max_trans_vel;
  min_vel_x_ = config.min_trans_vel;
  max_vel_th_ = config.max_rot_vel;
  min_vel_th_ = config.min_rot_vel;
  min_in_place_vel_th_ = config.min_in_place_rot_vel;
  goal_lin_tolerance_ = config.xy_goal_tolerance;
  goal_ang_tolerance_ = config.yaw_goal_tolerance;
  wp_tolerance_ = config.wp_tolerance;
  sim_time_ = config.sim_time;
  sim_granularity_ = config.sim_granularity;
  angular_sim_granularity_ = config.angular_sim_granularity;
  dwa_ = config.sample_angular_vels;
  //printf("\nPure Planner Reconfigure. new wp_tolerance: %.2f\n", wp_tolerance_);
}*/

Local3DPlanner::Local3DPlanner(
    std::string name, tf2_ros::Buffer* tf, local_3d_planner::OdometryHelperRos* oh,
    // std::vector<geometry_msgs::Point> footprint_spec,
    double robot_radius, double local_area_radius, double controller_freq,
    double max_trans_vel, double min_trans_vel, double max_rot_vel, double min_rot_vel,
    double min_in_place_rot_vel, double max_trans_acc, double max_rot_acc,
    double yaw_goal_tolerance, double xy_goal_tolerance, double wp_tolerance,
    double sim_time, double sim_granularity, double angular_sim_granularity, bool dwa)
{
  // costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_,
  // circumscribed_radius_);

  // printf("\n\n\n---FOOTPRINT----\n");
  // printf("inscribed_radius: %.3f, circumscribed_radius: %.3f\n", inscribed_radius_,
  // circumscribed_radius_);
  // printf("Footprint_specs:\n");
  // for(unsigned int i = 0; i<footprint_spec_.size(); i++)
  //{
  //	printf("point %u: x=%.3f, y=%.3f\n", (i+1), footprint_spec_[i].x,
  //footprint_spec_[i].y);
  //}
  // printf("\n\n");

  controller_freq_ = controller_freq;
  goal_reached_ = false;

  // For pure-pursuit
  running_ = false;
  new_plan_ = false;
  wp_index_ = -1;

  acc_lim_trans_ = max_trans_acc;
  acc_lim_rot_ = max_rot_acc;
  max_vel_x_ = max_trans_vel;
  min_vel_x_ = min_trans_vel;
  max_vel_th_ = max_rot_vel;
  min_vel_th_ = min_rot_vel;
  min_in_place_vel_th_ = min_in_place_rot_vel;
  goal_lin_tolerance_ = xy_goal_tolerance;
  goal_ang_tolerance_ = yaw_goal_tolerance;
  wp_tolerance_ = wp_tolerance;
  sim_time_ = sim_time;
  sim_granularity_ = sim_granularity;
  angular_sim_granularity_ = angular_sim_granularity;

  dwa_ = dwa;
  robot_radius_ = robot_radius;
  local_area_radius_ = local_area_radius;

  collision_detector_ = new CollisionDetection(
      name, tf, oh, max_vel_x_, max_vel_th_, acc_lim_trans_, acc_lim_rot_, sim_time_,
      robot_radius_, local_area_radius_, sim_granularity_);
}

Local3DPlanner::~Local3DPlanner()
{
  delete collision_detector_;
}

bool Local3DPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan)
{
  goal_reached_ = false;

  // Copy new plan
  global_plan_.clear();
  global_plan_.resize(new_plan.size());
  for (unsigned int i = 0; i < new_plan.size(); ++i)
  {
    global_plan_[i] = new_plan[i];
  }

  // Check plan size
  if (global_plan_.size() == 0)
  {
    running_ = false;
    wp_index_ = -1;
    ROS_WARN("New local plan size = 0!");
    return true;
  }

  // Set the way-point index to the first point of the path
  wp_index_ = 0;
  running_ = true;
  new_plan_ = true;

  // Set plan goal point
  geometry_msgs::PoseStamped& goal_pose = global_plan_[global_plan_.size() - 1];
  goal_x_ = goal_pose.pose.position.x;
  goal_y_ = goal_pose.pose.position.y;
  goal_z_ = goal_pose.pose.position.z;
  goal_t_ = tf::getYaw(goal_pose.pose.orientation);

  // Set the plan starting point
  geometry_msgs::PoseStamped& start_pose = global_plan_[0];
  start_x_ = start_pose.pose.position.x;
  start_y_ = start_pose.pose.position.y;
  start_z_ = start_pose.pose.position.z;
  start_t_ = tf::getYaw(start_pose.pose.orientation);

  return true;
}

bool Local3DPlanner::isGoalReached()
{
  if (goal_reached_)
  {
    goal_reached_ = false;  // we reset the flag
    return true;
  }
  return goal_reached_;
}

void Local3DPlanner::resetGoal()
{
  goal_reached_ = false;
}

// given the current state of the robot, find a good control command
bool Local3DPlanner::findBestAction(geometry_msgs::PoseStamped global_pose,
                                    geometry_msgs::PoseStamped global_vel, geometry_msgs::Twist& cmd_vel)
{
  // ros::WallTime t1 = ros::WallTime::now();

  // boost::mutex::scoped_lock l(configuration_mutex_);

  goal_reached_ = false;
  double vx, vy = 0.0, vt;

  // Check we have a path and we are running
  if (!running_)
  {
    vx = 0.0;
    vt = 0.0;
    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = 0.0;
    cmd_vel.linear.z = 0.0;
    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = vt;
    printf("FindBestAction. PurePursuit waiting for a path\n");
    return true;
  }

  // Get current robot position and velocity in X, Y and Theta (odom)
  float rx, ry, rz, rt, rvx, rvy, rvt;
  rx = global_pose.pose.position.x;
  ry = global_pose.pose.position.y;
  rz = global_pose.pose.position.z;
  rt = tf::getYaw(global_pose.pose.orientation);
  rvx = global_vel.pose.position.x;
  rvy = global_vel.pose.position.y;
  rvt = tf::getYaw(global_vel.pose.orientation);

  // Check if we are close enough to the goal
  double dist_goal = sqrt((rx - goal_x_) * (rx - goal_x_) +
                          (ry - goal_y_) * (ry - goal_y_));  //+(rz-goal_z_)*(rz-goal_z_));
  double dist_start = sqrt((rx - start_x_) * (rx - start_x_) +
                           (ry - start_y_) * (ry - start_y_));  //+(rz-start_z_)*(rz-start_z_));

  if (dist_goal < goal_lin_tolerance_)
  {
    printf("FindBestAction. dist_goal: %.2f. rx:%.2f, ry:%.2f, rz:%.2f, goalx:%.2f, goaly:%.2f, goalz: %.2f\n",
           dist_goal, rx, ry, rz, goal_x_, goal_y_, goal_z_);
    // Stop the robot
    vx = 0.0;
    vy = 0.0;

    // Rotate at minumin velocity until reaching the goal angle
    if (fabs(goal_t_ - rt) < goal_ang_tolerance_)
    {
      vt = 0.0;
      running_ = false;
      goal_reached_ = true;
    }
    /*else if(goal_t_ > rt)
      vt = min_in_place_vel_th_;
    else
      vt = -min_in_place_vel_th_;
    */
    // Modified by Noé
    else
    {
      float ang_diff = goal_t_ - rt;
      ang_diff = normalizeAngle(ang_diff, -M_PI, M_PI);
      if (ang_diff > 0.0)
        vt = min_in_place_vel_th_;
      else
        vt = -min_in_place_vel_th_;
    }

    // Added by Noé
    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = vy;
    cmd_vel.linear.z = 0.0;
    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = vt;
    // ros::WallTime t2 = ros::WallTime::now();
    // double secs = (t2-t1).toSec();
    // printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
    printf("FindBestAction. goal reached, sending zero velocity\n");
    return true;
  }

  // Do we have a new plan? get the closest point into the plan
  if (new_plan_)
  {
    new_plan_ = false;
    double dist;
    wp_index_ = 0;
    for (int i = global_plan_.size() - 1; i >= 0; i--)
    {
      double wpx = global_plan_[i].pose.position.x;
      double wpy = global_plan_[i].pose.position.y;
      double wpz = global_plan_[i].pose.position.z;
      dist = sqrt((rx - wpx) * (rx - wpx) + (ry - wpy) * (ry - wpy));  //+(rz-wpz)*(rz-wpz));
      if (dist < wp_tolerance_)
      {
        wp_index_ = i;
        break;
      }
    }
  }

  // Get current way-point in the path
  double wpx = global_plan_[wp_index_].pose.position.x;
  double wpy = global_plan_[wp_index_].pose.position.y;
  double wpz = global_plan_[wp_index_].pose.position.z;

  // Is this way-point still valid?
  double dist_swp = sqrt((rx - wpx) * (rx - wpx) + (ry - wpy) * (ry - wpy));  //+(rz-wpz)*(rz-wpz));
  while (dist_swp < wp_tolerance_ && wp_index_ < (int)global_plan_.size() - 1)
  {
    wp_index_++;
    wpx = global_plan_[wp_index_].pose.position.x;
    wpy = global_plan_[wp_index_].pose.position.y;
    // wpz = global_plan_[wp_index_].pose.position.z;
    dist_swp = sqrt((rx - wpx) * (rx - wpx) + (ry - wpy) * (ry - wpy));  //+(rz-wpz)*(rz-wpz));
  }

  // Transform way-point into local robot frame and get desired x,y,theta
  double dx = (wpx - rx) * cos(rt) + (wpy - ry) * sin(rt);
  double dy = -(wpx - rx) * sin(rt) + (wpy - ry) * cos(rt);
  double dt = atan2(dy, dx);
  // double dz = (wpz - rz);  //???
  // features_->transformPoseTo(geometry_msgs::PoseStamped pose_in, std::string frame_out,
  // bool usetime);

  double incr = 1 / controller_freq_;

  // Check if we need rotation in place before moving the robot to reach the way-point
  if (fabs(dt) > 1.3)  // 0.87 0.7~41º  0.79~45º
  {
    // vx = rvx-incr;
    // if(vx < 0.0)
    //	vx = 0.0;
    vx = 0;
    vy = 0.0;
    vt = min_in_place_vel_th_;
    if (dt < 0.0)
      vt = -min_in_place_vel_th_;

    // printf("place_vt: %.2f\n", vt);
  }
  else  // Select the linear and angular velocities to reach the way-point
  {
    // Compute actions depending to the distance to the goal and the starting points
    double dist_th = 1.5;
    if (dist_goal < dist_th)
    {
      vx = min_vel_x_ + (max_vel_x_ - min_vel_x_) * dist_goal / dist_th;
      vy = 0.0;
      vt = min_vel_th_ + (max_vel_th_ - min_vel_th_) * dist_goal / dist_th;
      //--added by noe
      if (dt < 0.0)
        vt *= -1;
    }
    /*else if(dist_start < dist_th)
    {
      vx = min_vel_x_ + (max_vel_x_ - min_vel_x_)*dist_start/dist_th;
      vy = 0.0;
      vt = min_vel_th_ + (max_vel_th_ - min_vel_th_)*dist_start/dist_th;
    }*/
    else
    {
      vx = max_vel_x_ * (0.1 + exp(-fabs(dt)));  // * tanh(4*dist_swp); //max_vel_x_;
      if (vx > max_vel_x_)
        vx = max_vel_x_;
      vy = 0.0;
      vt = max_vel_th_ * dt;  // max_vel_th_;
    }

    // if(vx > rvx+incr)
    //	vx = rvx+incr;

    // Set the sign of the commanded angular velocity and reset in case of small
    // variations
    // if(dt < 0.0) //commented by Noé
    //	vt *= -1;
    if (fabs(dt) < 0.1)
      vt = 0.0;
  }

  // Check if the action collide with an obstacle
  double px = 0.0, py = 0.0, pz = 0.0, pth = 0.0;
  bool valid = true;
  if (vx != 0.0)
  {
    // valid = checkTrajectory(rx, ry, rt, rvx, rvy, rvt, vx, vy, vt, px, py, pth);
    ros::WallTime t_check1 = ros::WallTime::now();
    valid = collision_detector_->checkTraj(rvx, rvy, rvt, vx, vy, vt, px, py, pz, pth);
    ros::WallTime t_check2 = ros::WallTime::now();
    double secs = (t_check2 - t_check1).toSec();
    // printf("\n\nCheckTraj time: %f seconds\n\n", secs);
  }
  if (valid /*|| fabs(vx) < 0.0001*/)
  {
    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = vy;
    cmd_vel.linear.z = 0.0;
    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = vt;
    // ros::WallTime t2 = ros::WallTime::now();
    // double secs = (t2-t1).toSec();
    // printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
    // printf("FindBestAction. validTraj found, vx:%.2f, vy:%.2f, vth:%.2f\n", vx, vy,
    // vt);
    return true;
  }

  // Try to find a valid command by sampling vels
  else if (dwa_)
  {
    // printf("Dentro de DWA!!!!!!!\n");
    float vt_orig = vt;
    float vx_orig = vx;

    float ang_vel_inc = 0.1;
    float lin_vel_dec = 0.1;

    vels_ best_vels;
    best_vels.vel_x = -1.0;
    best_vels.vel_y = vy;

    // Linear vel
    for (unsigned int l = 0; l <= 3; l++)
    {
      vx = vx_orig - (lin_vel_dec * l);
      if (vx < 0.1)
        continue;

      best_vels.vel_x = vx;

      // Angular vel
      for (unsigned int v = 1; v <= 4; v++)
      {
        // To the right
        vt = vt_orig + (ang_vel_inc * v);
        if (fabs(vt) > max_vel_th_)
          vt = max_vel_th_;

        bool valid1 =
            collision_detector_->checkTraj(rvx, rvy, rvt, vx, vy, vt, px, py, pz, pth);
        double d1 = 0.0;
        if (valid1)
        {
          d1 =
              sqrt((dx - px) * (dx - px) + (dy - py) * (dy - py));  // + (dz-pz)*(dz-pz));
          best_vels.vel_th = vt;
        }

        // to the left
        vt = vt_orig - (ang_vel_inc * v);
        if (fabs(vt) > max_vel_th_)
          vt = -max_vel_th_;

        bool valid2 =
            collision_detector_->checkTraj(rvx, rvy, rvt, vx, vy, vt, px, py, pz, pth);
        if (valid2)
        {
          // If both commands are valid,
          // chose the closest to the path.
          if (valid1)
          {
            double d2 = sqrt((dx - px) * (dx - px) +
                             (dy - py) * (dy - py));  // + (dz-pz)*(dz-pz));
            if (d2 < d1)
            {
              best_vels.vel_th = vt;
            }
          }
        }
        if (valid1 || valid2)
        {
          cmd_vel.linear.x = best_vels.vel_x;
          cmd_vel.linear.y = best_vels.vel_y;
          cmd_vel.linear.z = 0.0;
          cmd_vel.angular.x = 0.0;
          cmd_vel.angular.y = 0.0;
          cmd_vel.angular.z = best_vels.vel_th;
          // printf("\nValid cmd found! vx:%.2f, vth:%.2f\n", vx, vt);
          // ros::WallTime t2 = ros::WallTime::now();
          // double secs = (t2-t1).toSec();
          // printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
          // printf("FindBestAction. Valid Traj found with DWA. vx:%.2f, vy:%.2f,
          // vth:%.2f\n", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
          return true;
        }
      }
    }
  }

  // If still a valid command is not found, try to rotate in the spot
  if (dt > 0.09 || dt < -0.09)  // 0.09rad~5º
  {
    // printf("The robot should rotate on the spot\n");
    vx = 0.0;
    vy = 0.0;
    vt = min_in_place_vel_th_;
    if (dt < 0.0)
      vt = -min_in_place_vel_th_;

    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = vy;
    cmd_vel.linear.z = 0.0;
    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = vt;
    // ros::WallTime t2 = ros::WallTime::now();
    // double secs = (t2-t1).toSec();
    // printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
    printf("FindBestAction. Rotating in the spot vx:%.2f, vy:%.2f, vth:%.2f\n", vx, vy, vt);
    return true;
  }
  else
  {
    // Stop the robot
    // printf("The robot should stop\n");
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.linear.z = 0.0;
    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = 0.0;
    // ros::WallTime t2 = ros::WallTime::now();
    // double secs = (t2-t1).toSec();
    // printf("\n\nFindBestPathTime: %f seconds\n\n", secs);
    printf("FindBestAction. No valid command was found\n");
    return false;
    // return true;
  }
}

// we need to take the footprint of the robot into account when we calculate cost to
// obstacles
/*double UpoPlanner::footprintCost(double x_i, double y_i, double theta_i){
  //check if the footprint is legal
  return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_,
circumscribed_radius_);
}*/
};


//   Copyright 2015 Patrick Putnam
//
//   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 "config.h"

#include <iostream>
#include <vector>
#include <map>
#include <algorithm>
#include <iterator>

#include "common_commandline.h"

#include "basic_allele.h"

#include "clotho/utility/timer.hpp"

#include "clotho/utility/bit_block_iterator.hpp"
#include "clotho/powerset/variable_subset.hpp"
#include "clotho/powerset/powerset_no_dup_pred.hpp"
#include "clotho/powerset/variable_subset_recombination.hpp"
#include "clotho/powerset/variable_subset_fitness.hpp"
#include "clotho/mutation/infinite_site_pred.hpp"
#include "clotho/mutation/element_generator.hpp"

#include "clotho/classifiers/region_classifier.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/discrete_distribution.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

typedef boost::random::mt19937                                              rng_type;
typedef clotho::utility::timer                                              timer_type;

typedef basic_allele                                                        allele_type;
typedef clotho::powersets::variable_subset< allele_type, unsigned long >    sequence_type;
typedef sequence_type::pointer                                              sequence_pointer;
typedef sequence_type::powerset_type                                        allele_set_type;

typedef typename allele_set_type::element_keyer_type                        element_keyer;
typedef typename element_keyer::key_type                                    key_type;

typedef clotho::classifiers::region_classifier< allele_type >               classifier_type;
typedef typename classifier_type::region_upper_bounds                       recombination_points;
typedef std::vector< allele_type >                                          allele_group;

typedef clotho::fitness::fitness_method< double, clotho::fitness::multiplicative_heterozygous_tag > het_fit_type;
typedef clotho::fitness::fitness_method< double, clotho::fitness::multiplicative_homozygous_tag >    alt_hom_fit_type;
typedef clotho::fitness::no_fit                                             ref_hom_fit_type;

typedef clotho::recombine::recombination< sequence_type, classifier_type, clotho::recombine::inspection::tag::copy_matching_classify_mismatch, clotho::recombine::walker::tag::inline_dynamic_classify >    recombination_type;

typedef clotho::fitness::fitness< sequence_type, het_fit_type, alt_hom_fit_type, ref_hom_fit_type, double > fitness_type;
typedef typename fitness_type::result_type                                  fitness_result_type;
typedef std::vector< fitness_result_type >                                  population_fitness_type;

typedef std::pair< sequence_pointer, sequence_pointer >                     individual_type;
typedef std::vector< individual_type >                                      population_type;
typedef typename population_type::iterator                                  population_iterator;

typedef std::vector< double >                                               population_fitness_type;

typedef boost::random::poisson_distribution<unsigned int, double>           poisson_dist_type;
typedef boost::random::uniform_01< key_type >                               normal_dist_type;
typedef boost::random::uniform_int_distribution< unsigned int >             uniform_dist_type;
typedef boost::random::discrete_distribution< unsigned int, double >        discrete_dist_type;

typedef clotho::mutations::infinite_site_pred< allele_set_type >::type          predicate_type;

struct config_wrapper : public simulation_config {
    boost::property_tree::ptree    m_log;
    rng_type    m_rng;

    allele_set_type  alleles;
    predicate_type   pred;

    config_wrapper( const simulation_config & cfg ) :
        simulation_config( cfg )
        , m_log()
        , m_rng( seed )
        , alleles()
        , pred(&alleles) {
    }

    void reset_alleles() {
        alleles.clear();
    }

    virtual ~config_wrapper() { }
};

template < class KeyDist >
struct custom_allele_gen {
    typedef KeyDist key_dist_type;

    key_dist_type m_kd;

    custom_allele_gen() {}

    custom_allele_gen( const key_dist_type & kd ) :
        m_kd(kd) {
    }

    custom_allele_gen( const custom_allele_gen< KeyDist > & cag ) :
        m_kd( cag.m_kd ) {
    }

    template < class URNG >
    basic_allele operator()( URNG & rng, double sel = 0.0, double dom = 1.0, bool neut = true ) {
#ifdef ALL_SELECTED_ALLELES
        neut = false;
#endif  // ALL_SELECTED_ALLELES
        return basic_allele( m_kd(rng), sel, dom, neut );
    }
};

//typedef clotho::mutations::element_generator< allele_type, normal_dist_type >   allele_generator;
typedef clotho::mutations::element_generator< allele_type, custom_allele_gen< normal_dist_type > >   allele_generator;

void write_log( config_wrapper & sim, unsigned int log_idx );

template < class Value >
void add_value_array( boost::property_tree::ptree & array, const Value & t ) {
    boost::property_tree::ptree node;

    node.put("", t );
    array.push_back( std::make_pair("", node));
}

template < class A, class B >
void add_value_array( boost::property_tree::ptree & array, const std::pair< A, B > & t ) {
    boost::property_tree::ptree a,b,c;
    a.put( "", t.first );
    b.put( "", t.second );

    c.push_back (std::make_pair( "", a ) );
    c.push_back (std::make_pair( "", b ) );
    array.push_back( std::make_pair("", c ) );
}

void add_value_array( boost::property_tree::ptree & array, const clotho::utility::timer & t );

void add_node( boost::property_tree::ptree & array, const string & path, const boost::property_tree::ptree & n );
void add_node( boost::property_tree::ptree & array, const string & path, const individual_type & ind  );

void initializePopulation(config_wrapper & cfg, population_type & pop, population_type & buffer, boost::property_tree::ptree & log );
void simulate( config_wrapper & cfg, population_type & pop, population_type & buffer, boost::property_tree::ptree & _log );
void resetPopulation( config_wrapper & cfg, population_type * p, boost::property_tree::ptree & _log );
double fitnessOfPopulation( config_wrapper & cfg, population_type * p, std::vector< double > & fit, boost::property_tree::ptree & _log );

void statsPopulation( config_wrapper & cfg, population_type * p, boost::property_tree::ptree & _log );

sequence_pointer reproduce( config_wrapper & cfg, sequence_pointer s0, sequence_pointer s1, boost::property_tree::ptree & _log);
sequence_pointer recombine( config_wrapper & cfg, sequence_pointer base, sequence_pointer alt, unsigned int nEvents, bool copy = false);
void mutate( config_wrapper & cfg, sequence_pointer seq, unsigned int nEvents );

typedef std::map< sequence_pointer, unsigned int >  ref_map_type;
typedef typename ref_map_type::iterator             ref_map_iterator;
typedef std::vector< unsigned int >                 allele_dist_type;
void buildPopulationRefMap( population_type * p, ref_map_type & m, allele_dist_type & a);

struct scale_stats {
    boost::property_tree::ptree family_size, alleles_size, max_alleles;
};

void computeScaleStats( config_wrapper & cfg, population_type * p, scale_stats & stats );
void add_node( boost::property_tree::ptree &, const string & path, const scale_stats & s );

template < class PopDist >
void reproducePopulation( config_wrapper & cfg, population_type * p, population_type * c, const PopDist & dist, boost::property_tree::ptree & _log ) {
    population_type::iterator it = c->begin();
    while( it != c->end() ) {
        unsigned int idx = dist( cfg.m_rng ), idx2 = dist( cfg.m_rng );

        population_type::iterator p0 = p->begin() + idx, p1 = p->begin() + idx2;

        boost::property_tree::ptree p0_log;
        it->first = reproduce( cfg, p0->first, p0->second, p0_log );

        boost::property_tree::ptree p1_log;
        it->second = reproduce( cfg, p1->first, p1->second, p1_log );

        if( !p0_log.empty() || !p1_log.empty() ) {
            std::ostringstream oss;
            oss << "child." << (it - c->begin());
            add_node( _log, oss.str() + ".p0", p0_log);
            add_node( _log, oss.str() + ".p1", p1_log);
        }

        ++it;
    }
}

int main( int argc, char ** argv ) {
    simulation_config cmd;
    int res = parse_commandline(argc, argv, cmd);
    if( res ) {
        return res;
    }

    for( unsigned int i = 0; i < cmd.nRep; ++i ) {
        config_wrapper cfg(cmd);

        if( cfg.nGen > 0 ) {
            population_type pop, buffer;

            initializePopulation(cfg, pop, buffer, cfg.m_log );
            simulate( cfg, pop, buffer, cfg.m_log );
        }

        write_log( cfg, i );
    }

    return res;
}

void initializePopulation( config_wrapper & cfg, population_type & pop, population_type & buffer, boost::property_tree::ptree & _log ) {
    for( unsigned int i = 0; i < cfg.nPop; ++i ) {
        pop.push_back( std::make_pair( cfg.alleles.create_subset() , cfg.alleles.create_subset()  ) );
        buffer.push_back( std::make_pair( cfg.alleles.create_subset(), cfg.alleles.create_subset() ) );
    }

    _log.put( "status", "success" );
}

void simulate( config_wrapper & cfg, population_type & pop, population_type & buffer, boost::property_tree::ptree & _log ) {
    population_type *parent = &pop, *child = &buffer;

    std::ostringstream oss;

    unsigned int log_period = ((cfg.nGen < cfg.log_period) ? cfg.nGen : cfg.log_period);

    typedef boost::property_tree::ptree log_type;
    log_type perf_run, perf_fit, perf_repro, perf_reset, perf_stats;
    log_type l_fit, l_gen, l_fam;

    scale_stats mem_stats;

    for( unsigned int i = 0; i < cfg.nGen; ++i ) {
        oss.str("");
        oss.clear();
        oss << "simulation." << i;

        timer_type run_t;
        timer_type fit_t;

        population_fitness_type fit;
        boost::property_tree::ptree fit_log;
        double e_fit = 1.;
        double pop_fit = fitnessOfPopulation( cfg, parent, fit, fit_log );
        e_fit = ((double) parent->size() / pop_fit);
        fit_t.stop();

        timer_type repro_t;
        boost::property_tree::ptree repro_log;

        discrete_dist_type dist( fit.begin(), fit.end() );

        assert( dist.probabilities().size() == fit.size());
        reproducePopulation(cfg, parent, child, dist, repro_log );
        repro_t.stop();

        timer_type reset_t;
        boost::property_tree::ptree reset_log;
        resetPopulation( cfg, parent, reset_log );
        std::swap( parent, child );

        cfg.alleles.pruneSpace();

        reset_t.stop();

        timer_type stat_t;
        boost::property_tree::ptree stat_log;
        if( !(--log_period) ) {
            std::cerr << "computing population stats: " << i << std::endl;
            statsPopulation(cfg, parent, stat_log );
            stat_t.stop();
            log_period = ((i + cfg.log_period < cfg.nGen) ? cfg.log_period : (cfg.nGen - i - 1) );
        }
        stat_t.stop();
        run_t.stop();

        computeScaleStats( cfg, parent, mem_stats );

        add_value_array( l_fit, e_fit);
        add_value_array( l_gen, i);
        add_value_array( l_fam, cfg.alleles.family_size() );

        add_value_array( perf_run, run_t );
        add_value_array( perf_fit, fit_t);
        add_value_array( perf_repro, repro_t);
        add_value_array( perf_reset, reset_t);
        add_value_array( perf_stats, stat_t);

        add_node( _log, oss.str() + ".fitness", fit_log);
        add_node( _log, oss.str() + ".reproduction", repro_log);
        add_node( _log, oss.str() + ".reset", reset_log);
        add_node( _log, oss.str() + ".stats", stat_log );
    }

    const string sim_path = "simulation.performance.data";
    _log.put( sim_path + ".runtime.scale", timer_type::hertz());
    _log.add_child( sim_path + ".generations", l_gen );
    _log.add_child( sim_path + ".runtime.total", perf_run );
    _log.add_child( sim_path + ".runtime.fitness", perf_fit );
    _log.add_child( sim_path + ".runtime.reproduction", perf_repro );
    _log.add_child( sim_path + ".runtime.reset", perf_reset );
    _log.add_child( sim_path + ".runtime.stats", perf_stats );
    _log.add_child( sim_path + ".family_size", l_fam );

    _log.add_child(sim_path + ".e_fitness", l_fit );

    add_node( _log, sim_path, mem_stats );
}

inline void add_value_array( boost::property_tree::ptree & array, const clotho::utility::timer & t ) {
    boost::property_tree::ptree node;

    node.put("", t.elapsed().count() );
    array.push_back( std::make_pair("", node));
}

inline void add_node( boost::property_tree::ptree & root, const string & path, const boost::property_tree::ptree & node ) {
    if( !node.empty() ) {
        root.add_child( path, node );
    }
}

void computeScaleStats( config_wrapper & cfg, population_type * p, scale_stats & stats ) {
    ref_map_type seq_ref_counts;

    size_t  fam_size = 0;

    allele_set_type::cfamily_iterator f_it = cfg.alleles.family_begin(), f_end = cfg.alleles.family_end();
    while( f_it != f_end ) {
        if( (*f_it) ) {
            fam_size += (*f_it)->num_blocks();
        }
        ++f_it;
    }

    size_t  max_allele_size = cfg.alleles.variable_allocated_size();
    size_t  allele_size = cfg.alleles.variable_size();

    add_value_array( stats.family_size, fam_size );
    add_value_array( stats.alleles_size, allele_size );
    add_value_array( stats.max_alleles, max_allele_size);
}

void add_node( boost::property_tree::ptree & r, const string & path, const scale_stats & s ) {
    add_node( r, path + ".memory.table_size", s.family_size );
    add_node( r, path + ".memory.alleles_size", s.alleles_size );
    add_node( r, path + ".memory.max_alleles", s.max_alleles );
}

void add_node( boost::property_tree::ptree & r, const string & path, const individual_type & ind ) {
    std::ostringstream oss;

    oss << ind.first << ": " << (*ind.first);
    r.put( path + ".first", oss.str());

    oss.str("");
    oss.clear();

    oss << ind.second << ": " << (*ind.second);
    r.put( path + ".second", oss.str() );
}

void resetPopulation( config_wrapper & cfg, population_type * p, boost::property_tree::ptree & _log ) {
    population_type::iterator it = p->begin();
    while( it != p->end() ) {
        it->first.reset();
        it->second.reset();
        ++it;
    }
}

double fitnessOfPopulation( config_wrapper & cfg, population_type * p, std::vector< double > & fit, boost::property_tree::ptree & _log ) {

    // pre-compute which alleles are selected within the population
    //
    // identify all variable (non-free) indicies (elements)
    typename allele_set_type::bitset_type selected( cfg.alleles.free_begin(), cfg.alleles.free_end());
    selected.flip();

    // foreach variable element
    size_t idx = selected.find_first();
    while( idx != allele_set_type::bitset_type::npos ) {
        typename allele_set_type::cvariable_iterator vit =  cfg.alleles.variable_begin() + idx;

        // determine if neutral
        if( vit->isNeutral() ) {
            // it is neutral therefore should be ignored in fitness calculation
            selected.reset(idx);
        }
        idx = selected.find_next(idx);
    }

    size_t nSelected = selected.count();

    std::back_insert_iterator< std::vector< double > > output = std::back_inserter(fit);

    if( nSelected == 0 ) {
        population_iterator first = p->begin();
        while( first != p->end() ) {
            (*output++) = 1.;
            ++first;
        }
        return p->size();
    }

    population_iterator first = p->begin();

    double pop_fit = 0.;
    fitness_type    f;

    ref_map_type m_refs;
    while( first != p->end() ) {
        double val = f(1., first->first, first->second, selected.m_bits.begin(), selected.m_bits.end());
        pop_fit += (val);
        (*output++) = val;
        ++first;
    }
    return pop_fit;
}

sequence_pointer reproduce( config_wrapper & cfg, sequence_pointer s0, sequence_pointer s1, boost::property_tree::ptree & _log) {
    poisson_dist_type mu_dist( cfg.mu ), rho_dist( cfg.rho );
    normal_dist_type  ndist;

    unsigned int nMut = mu_dist( cfg.m_rng );
    unsigned int nRec = rho_dist( cfg.m_rng );

    if( ndist( cfg.m_rng ) >= 0.5 ) {
        std::swap( s0, s1 );
    }

    sequence_pointer res;

    if( nRec ) {
        res = recombine( cfg, s0, s1, nRec, (nMut == 0) );
        if( nMut ) {
            assert( res && s0 != res && s1 != res );
            mutate( cfg, res, nMut );
        }
    } else if( nMut ) {
        // mutation only
        res = ((s0) ? s0->clone() : cfg.alleles.create_subset() );
        assert( res != s0 );
        mutate(cfg, res, nMut );
    } else if( s0 ) {
        // no recombination or mutation events
        res = s0;
    }
//  else {
//      // implies that no recombination or mutations occurred and
//      // and that s0 was NULL
//      // therefore res should remain NULL as it was initialized
//    }

    return res;
}

sequence_pointer recombine( config_wrapper & cfg, sequence_pointer base, sequence_pointer alt, unsigned int nEvents, bool should_copy) {
    if( base == alt ) {
        if( !base ) {
            return cfg.alleles.create_subset();
        } else if( should_copy ) {
            return base;
        } else {
            sequence_pointer res = base->clone();
            return res;
        }
    }

    recombination_points pts;
    allele_generator gen;

    // without predicate, therefore only random points are created
    gen( cfg.m_rng, nEvents, std::back_inserter(pts) );
    assert( pts.size() == nEvents );

    classifier_type cfier( pts );

    recombination_type rec;
    rec( base, alt, cfier );

    sequence_pointer res;
    if( !rec.isEmpty() ) {
        if( !should_copy ) {
            res = cfg.alleles.create_subset( *rec.getResultSequence() );
        } else if( rec.isMatchBase() ) {
            res = base;
        } else if( rec.isMatchAlt() ) {
            res = alt;
        } else {
            res = cfg.alleles.create_subset( *rec.getResultSequence() );
        }
    } else {
        res = cfg.alleles.create_subset();
    }

    return res;
}

void mutate( config_wrapper & cfg, sequence_pointer seq, unsigned int nEvents ) {
    allele_group pts;
    allele_generator gen;

    gen(cfg.m_rng, nEvents, &cfg.pred, std::back_inserter(pts) );

    assert( pts.size() == nEvents );

    while( !pts.empty() ) {
        seq->addElement( pts.back() );
        pts.pop_back();
    }
}

void buildPopulationRefMap( population_type * p, ref_map_type & m, allele_dist_type & a ) {
    population_iterator pit = p->begin();
    while( pit != p->end() ) {
        ref_map_iterator rit = m.find( pit->first );
        if( rit == m.end() ) {
            m.insert( std::make_pair( pit->first, 1 ));
        } else {
            ++(rit->second);
        }

        rit = m.find( pit->second);
        if( rit == m.end() ) {
            m.insert( std::make_pair( pit->second, 1 ) );
        } else {
            ++(rit->second);
        }
        ++pit;
    }

    for( ref_map_iterator rit = m.begin(); rit != m.end(); ++rit ) {
        unsigned int n = rit->second;
        typename sequence_type::index_type idx = rit->first->find_first();
        while( idx.second !=  sequence_type::npos ) {
            a[ idx.first ] += n;
            idx = rit->first->find_next( idx );
        }
    }
}

void statsPopulation( config_wrapper & cfg, population_type * p, boost::property_tree::ptree & _log ) {

    typedef std::map< unsigned int, unsigned int >  count_map;
    typedef count_map::iterator                     count_iterator;

    count_map allele_counts;

    ref_map_type seq_ref_counts;
    allele_dist_type allele_dist( cfg.alleles.variable_allocated_size(), 0);
    buildPopulationRefMap( p, seq_ref_counts, allele_dist );

    unsigned int nExpSeq = 0;
    unsigned int nSeq = seq_ref_counts.size(); // - ((nExpSeq > 0)? 1 : 0); // remove null sequence
    double nAlleles = 0;

    bool usual_average = ( nSeq % 2 == 0);
    unsigned int median_idx = nSeq / 2, med_allele = 0, prev_allele = 0;
    unsigned int mode_allele = 0, mode_seq = 0;
    for( ref_map_iterator sit = seq_ref_counts.begin(); sit != seq_ref_counts.end(); sit++ ) {
        unsigned int acnt = (( sit->first ) ? sit->first->count() : 0 );
        unsigned int e_rcnt = sit->second;

        count_iterator a_it = allele_counts.find( acnt );
        if( a_it == allele_counts.end() ) {
            allele_counts.insert( std::make_pair( acnt, e_rcnt ) );
        } else {
            a_it->second += e_rcnt;
        }
        nAlleles += (acnt * e_rcnt);

        unsigned int ubound = nExpSeq + e_rcnt;
        if(nExpSeq <= median_idx && median_idx < ubound) {
            if( !usual_average || ( nExpSeq <= median_idx - 1) ) {
                med_allele = acnt;
            } else {
                med_allele = (acnt + prev_allele) / 2;
            }
        } else {
            prev_allele = acnt;
        }

        if( e_rcnt > mode_seq ) {
            mode_allele = acnt;
            mode_seq = e_rcnt;
        }

        nExpSeq = ubound;
    }

    double ave_alleles_per_sequence = (nAlleles / (double) nExpSeq);

    // for validation purposes
    allele_set_type::cfamily_iterator f_it = cfg.alleles.family_begin(), f_end = cfg.alleles.family_end();
    unsigned int nNotInPop = 0;
    while( f_it != f_end ) {
        if( seq_ref_counts.find( *f_it++ ) == seq_ref_counts.end()) {
            ++nNotInPop;
        }
    }

    assert( nNotInPop == 0 );

    boost::property_tree::ptree all_dist, all_freq;
    for( count_iterator c_it = allele_counts.begin(); c_it != allele_counts.end(); ++c_it) {
        add_value_array( all_dist, *c_it );
    }

    typename allele_set_type::cvariable_iterator e_it = cfg.alleles.variable_begin();
    for( allele_dist_type::iterator a_it = allele_dist.begin(); a_it != allele_dist.end(); ++a_it, ++e_it ) {
        if( (*a_it) > 0 ) {
            std::ostringstream oss;
            oss << *e_it;
            add_value_array( all_freq, std::make_pair(oss.str(), (*a_it)));
        }
    }

    _log.put( "population.family_size", cfg.alleles.family_size() );

    _log.put( "population.reference_sequences", ((allele_counts.find(0) != allele_counts.end())? allele_counts[0] : 0 ) );

    _log.put( "population.total_sequences", nExpSeq );
    _log.put( "sequences.allele_count_distribution_format", "[alleles per sequence, number of sequences in population]" );
    _log.add_child( "sequences.allele_count_distribution", all_dist );
    _log.put( "sequences.alleles_per.mean", ave_alleles_per_sequence);
    _log.put( "sequences.alleles_per.min", allele_counts.begin()->first );
    _log.put( "sequences.alleles_per.max", allele_counts.rbegin()->first );
    _log.put( "sequences.alleles_per.median", med_allele );
    _log.put( "sequences.alleles_per.mode", mode_allele );

    _log.put( "alleles.variable_count", cfg.alleles.variable_size() );
    _log.put( "alleles.fixed_count", cfg.alleles.fixed_size() );
    _log.put( "alleles.free_size", cfg.alleles.free_size() );

    _log.put( "alleles.frequencies_format", "[allele, count in population]" );
    _log.add_child( "alleles.frequencies", all_freq );
}

void write_log( config_wrapper & cfg, unsigned int log_idx ) {
    boost::property_tree::ptree clog;
    add_config( clog, cfg );
    cfg.m_log.add_child( CONFIG_BLOCK_K, clog );

    if( cfg.out_path.empty() ) {
        boost::property_tree::write_json(std::cout, cfg.m_log );
    } else {
        std::ostringstream oss;
        oss << cfg.out_path << "_" << log_idx;
        boost::property_tree::write_json( oss.str() + "_log.json", cfg.m_log );
    }
}


#ifndef GAME_CORE_WORLD_ENTITY_TYPES_HPP
#define GAME_CORE_WORLD_ENTITY_TYPES_HPP

#include <type_traits>

#include <boost/hana/first.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/second.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include "world/entity/Ball.hpp"
#include "world/entity/EntityId.hpp"
#include "world/entity/Player.hpp"
#include "world/entity/Wall.hpp"

#include "util/EnumCast.hpp"

namespace hana = boost::hana;

namespace World {
namespace Entity {

#define MAKE_ENTITY_PAIR(Identifier, Type) hana::make_pair(hana::int_c<enum_cast(Identifier)>, hana::type_c<Type>)

#define GAME_CORE_ENTITY_COUNT 3
constexpr auto entityIdentifierMap =                 //
  hana::make_tuple(MAKE_ENTITY_PAIR(Id::Wall, Wall), //
                   MAKE_ENTITY_PAIR(Id::Ball, Ball), //
                   MAKE_ENTITY_PAIR(Id::Player, Player));

static_assert(decltype(hana::size(entityIdentifierMap))::value == GAME_CORE_ENTITY_COUNT,
              "Map size must be equal to the entity count.");

template <size_t i>
constexpr auto
  id = static_cast<Id>(std::decay<decltype(hana::first(entityIdentifierMap[hana::int_c<i>]))>::type::value);

template <size_t i>
using Type = typename std::decay<decltype(hana::second(entityIdentifierMap[hana::int_c<i>]))>::type::type;

} /* namespace Entity */
} /* namespace World */

#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_helpers.hpp>

#ifndef IIWA_14_FK_FAST_HPP
#define IIWA_14_FK_FAST_HPP

namespace IIWA_14_FK_FAST
{
    const size_t IIWA_14_NUM_ACTIVE_JOINTS = 7;
    const size_t IIWA_14_NUM_LINKS = 8;

    const std::string IIWA_14_ACTIVE_JOINT_1_NAME = "iiwa_joint_1";
    const std::string IIWA_14_ACTIVE_JOINT_2_NAME = "iiwa_joint_2";
    const std::string IIWA_14_ACTIVE_JOINT_3_NAME = "iiwa_joint_3";
    const std::string IIWA_14_ACTIVE_JOINT_4_NAME = "iiwa_joint_4";
    const std::string IIWA_14_ACTIVE_JOINT_5_NAME = "iiwa_joint_5";
    const std::string IIWA_14_ACTIVE_JOINT_6_NAME = "iiwa_joint_6";
    const std::string IIWA_14_ACTIVE_JOINT_7_NAME = "iiwa_joint_7";

    const std::string IIWA_14_LINK_1_NAME = "iiwa_link_0";
    const std::string IIWA_14_LINK_2_NAME = "iiwa_link_1";
    const std::string IIWA_14_LINK_3_NAME = "iiwa_link_2";
    const std::string IIWA_14_LINK_4_NAME = "iiwa_link_3";
    const std::string IIWA_14_LINK_5_NAME = "iiwa_link_4";
    const std::string IIWA_14_LINK_6_NAME = "iiwa_link_5";
    const std::string IIWA_14_LINK_7_NAME = "iiwa_link_6";
    const std::string IIWA_14_LINK_8_NAME = "iiwa_link_7";

    inline Eigen::Isometry3d Get_link_0_joint_1_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1575);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2025);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.2045, 0.0);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2155);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.1845, 0.0);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2155);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline Eigen::Isometry3d Get_link_6_joint_7_LinkJointTransform(const double joint_val)
    {
        const Eigen::Translation3d pre_joint_translation(0.0, 0.081, 0.0);
        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
        return (pre_joint_transform * joint_transform);
    }

    inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        assert(configuration.size() == IIWA_14_NUM_ACTIVE_JOINTS);
        EigenHelpers::VectorIsometry3d link_transforms(IIWA_14_NUM_LINKS);
        link_transforms[0] = base_transform;
        link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]);
        link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
        link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
        link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
        link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
        link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
        link_transforms[7] = link_transforms[6] * Get_link_6_joint_7_LinkJointTransform(configuration[6]);
        return link_transforms;
    }

    inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        std::vector<double> configuration_vector(IIWA_14_NUM_ACTIVE_JOINTS);
        configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_1_NAME, 0.0);
        configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_2_NAME, 0.0);
        configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_3_NAME, 0.0);
        configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_4_NAME, 0.0);
        configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_5_NAME, 0.0);
        configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_6_NAME, 0.0);
        configuration_vector[6] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_7_NAME, 0.0);
        return GetLinkTransforms(configuration_vector, base_transform);
    }

    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
        EigenHelpers::MapStringIsometry3d link_transforms_map;
        link_transforms_map[IIWA_14_LINK_1_NAME] = link_transforms[0];
        link_transforms_map[IIWA_14_LINK_2_NAME] = link_transforms[1];
        link_transforms_map[IIWA_14_LINK_3_NAME] = link_transforms[2];
        link_transforms_map[IIWA_14_LINK_4_NAME] = link_transforms[3];
        link_transforms_map[IIWA_14_LINK_5_NAME] = link_transforms[4];
        link_transforms_map[IIWA_14_LINK_6_NAME] = link_transforms[5];
        link_transforms_map[IIWA_14_LINK_7_NAME] = link_transforms[6];
        link_transforms_map[IIWA_14_LINK_8_NAME] = link_transforms[7];
        return link_transforms_map;
    }

    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
    {
        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
        EigenHelpers::MapStringIsometry3d link_transforms_map;
        link_transforms_map[IIWA_14_LINK_1_NAME] = link_transforms[0];
        link_transforms_map[IIWA_14_LINK_2_NAME] = link_transforms[1];
        link_transforms_map[IIWA_14_LINK_3_NAME] = link_transforms[2];
        link_transforms_map[IIWA_14_LINK_4_NAME] = link_transforms[3];
        link_transforms_map[IIWA_14_LINK_5_NAME] = link_transforms[4];
        link_transforms_map[IIWA_14_LINK_6_NAME] = link_transforms[5];
        link_transforms_map[IIWA_14_LINK_7_NAME] = link_transforms[6];
        link_transforms_map[IIWA_14_LINK_8_NAME] = link_transforms[7];
        return link_transforms_map;
    }
}

#endif // IIWA_14_FK_FAST_HPP