// 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


#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


#include "Common/Common.h"
#include "GL/glew.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Demos/Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "FluidModel.h"
#include "TimeStepFluidModel.h"
#include <iostream>
#include "Demos/Utils/Logger.h"
#include "Demos/Utils/Timing.h"
#include "Demos/Utils/FileSystem.h"

#define _USE_MATH_DEFINES
#include "math.h"
#include "Demos/Simulation/Constraints.h"
#include "Demos/Visualization/Visualization.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

INIT_TIMING
INIT_LOGGING

using namespace PBD;
using namespace Eigen;
using namespace std;

void timeStep ();
void buildModel ();
void renderWorld();
void createBreakingDam();
void addWall(const Vector3r &minX, const Vector3r &maxX, std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives,bool active);
void initBoundaryData(std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives);
void render ();
void cleanup();
void reset();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void createSphereBuffers(Real radius, int resolution);
void renderSphere(const Vector3r &x, const float color[]);
void releaseSphereBuffers();
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setTempUpdateMethod(const void *value, void *clientData);
void TW_CALL getTempUpdateMethod(void *value, void *clientData);
void TW_CALL setViscosity(const void *value, void *clientData);
void TW_CALL getViscosity(void *value, void *clientData);
void TW_CALL setStiffness(const void *value, void *clientData);
void TW_CALL getStiffness(void *value, void *clientData);
void TW_CALL setDilation(const void *value, void *clientData);
void TW_CALL getDilation(void *value, void *clientData);
void TW_CALL setMelting(const void *value, void *clientData);
void TW_CALL getMelting(void *value, void *clientData);
void TW_CALL setEvaporation(const void *value, void *clientData);
void TW_CALL getEvaporation(void *value, void *clientData);
void TW_CALL setDifusion(const void *value, void *clientData);
void TW_CALL getDifusion(void *value, void *clientData);
void TW_CALL getTempContact(void *value, void *clientData);
void TW_CALL setTempContact(const void *value, void *clientData);
void TW_CALL getContact(void *value, void *clientData);
void TW_CALL setContact(const void *value, void *clientData);
void TW_CALL getXPBD(void *value, void *clientData);
void TW_CALL setXPBD(const void *value, void *clientData);

void TW_CALL getRadioSol(void *value, void *clientData);
void TW_CALL setRadioSol(const void *value, void *clientData);
void TW_CALL getNumConstraint(void *value, void *clientData);
void TW_CALL setNumConstraint(const void *value, void *clientData);

void TW_CALL getDesWidth(void *value, void *clientData);
void TW_CALL setDesWidth(const void *value, void *clientData);

void TW_CALL getDesHeight(void *value, void *clientData);
void TW_CALL setDesHeight(const void *value, void *clientData);
//Core
FluidModel model;
TimeStepFluidModel simulation;

Real compact = 0.98;// 0.928;

//const Real particleRadius = 0.025;
const Real particleRadius = 0.025;//0.015625//;
int width = 30;
int depth = 30;
int height = 80;
bool doPause = true;
bool thermal = true;
bool record = false;
bool capture = false;
bool rendering = true;
bool renderingWall = false;
bool renderingAir = false;

std::vector<unsigned int> selectedParticles;
Vector3r oldMousePos;
// initiate buffers
GLuint elementbuffer;
GLuint normalbuffer;
GLuint vertexbuffer;
int vertexBufferSize = 0;
GLint context_major_version, context_minor_version;
string exePath;
string dataPath;

GLuint textureFloor;
ofstream file;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	std::string logPath = FileSystem::normalizePath(FileSystem::getProgramPath() + "/log");
	FileSystem::makeDirs(logPath);
	logger.addSink(unique_ptr<ConsoleSink>(new ConsoleSink(LogLevel::INFO)));
	logger.addSink(unique_ptr<FileSink>(new FileSink(LogLevel::DEBUG, logPath + "/PBD.log")));

	exePath = FileSystem::getProgramPath();
	dataPath = exePath + "/" + std::string(PBD_DATA_PATH);

	// OpenGL
	MiniGL::init (argc, argv, 1280+620, 780, 0, 0, "Fluid demo");

	MiniGL::setClientIdleFunc (60, timeStep);

	MiniGL::initLights ();
	MiniGL::loadTexture(textureFloor);

	MiniGL::setKeyFunc(0, 'r', reset);
	//MiniGL::setKeyFunc(1, '1', demo1);
	//MiniGL::setKeyFunc(2, '2', demo2);
	MiniGL::setSelectionFunc(selection);

	//MiniGL::desWidth = 350;
	//MiniGL::desHeight = 100;

	MiniGL::getOpenGLVersion(context_major_version, context_minor_version);
	MiniGL::setClientSceneFunc(render);

	//MiniGL::setViewport (40.0, 0.1f, 500.0, Vector3r (0.0, 3.0, 8.0), Vector3r (0.0, 0.0, 0.0));
	MiniGL::setViewport(40.0, 0.1f, 800.0, Vector3r(0.0, 2.1, 7.0), Vector3r(0.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_REAL, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.0001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &simulation, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");

	TwAddVarCB(MiniGL::getTweakBar(), "XPBD", TW_TYPE_BOOL32, setXPBD, getXPBD, &model, " label='XPBD'  group=Simulation ");
	//TwAddVarRW(MiniGL::getTweakBar(), "XPDB", TW_TYPE_BOOLCPP, &XPBD, " label='XPBD' group=Simulation key=x ");
	TwAddVarCB(MiniGL::getTweakBar(), "Viscosity", TW_TYPE_REAL, setViscosity, getViscosity, &model, " label='Viscosity'  min=0.0 max = 5 step=0.001 precision=4 group=Simulation ");

	TwAddVarCB(MiniGL::getTweakBar(), "Radio Solidification", TW_TYPE_REAL, setRadioSol, getRadioSol, &model, " label='Radio Solidification'  min=0.045 max = 0.1 step=0.01 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Num Constraints", TW_TYPE_REAL, setNumConstraint, getNumConstraint, &model, " label='Num Constraints'  min=1 max = 10 step=1 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, &model, " label='Stiffness'  min=0.01 max = 1 step=0.01 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Coef Dilation", TW_TYPE_REAL, setDilation, getDilation, &model, " label='Coef Dilation'  min=0 max = 0.01 step=0.001 precision=4 group=Simulation ");

	TwAddVarCB(MiniGL::getTweakBar(), "Temp Melting", TW_TYPE_REAL, setMelting, getMelting, &model, " label='Temp Melting'  min=0.0 max = 300 step=1.0 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Temp Evaporation", TW_TYPE_REAL, setEvaporation, getEvaporation, &model, " label='Temp Evaporation'  min=1 max = 500.0 step=1.0 precision=4 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Coef Difusion", TW_TYPE_REAL, setDifusion, getDifusion, &model, " label='Coef Difusion'  min=1 max = 1000 step=5 precision=4 group=Simulation ");

	TwType enumType2 = TwDefineEnum("TemperatureUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "TemperatureUpdateMethod", enumType2, setTempUpdateMethod, getTempUpdateMethod, &model, " label='Heat Transfer method' enum='0 {Our Method}, 1 {Cleary Method}' group=Simulation");

	TwAddVarCB(MiniGL::getTweakBar(), "Contact", TW_TYPE_BOOL32, setContact, getContact, &model, " label='Contact'  group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "Temp Contact", TW_TYPE_REAL, setTempContact, getTempContact, &model, " label='Temp Contact'  min=-50 max = 500 step=10 precision=4 group=Simulation ");

	TwAddVarRW(MiniGL::getTweakBar(), "Thermal", TW_TYPE_BOOLCPP, &thermal, " label='Thermal' group=Simulation key=t ");

	TwAddVarRW(MiniGL::getTweakBar(), "start", TW_TYPE_BOOLCPP, &record, " label='start' group=Record key=g ");
	TwAddVarRW(MiniGL::getTweakBar(), "capture", TW_TYPE_BOOLCPP, &capture, " label='capture' group=Record key=c ");
	TwAddVarCB(MiniGL::getTweakBar(), "Des Width", TW_TYPE_INT32, setDesWidth, getDesWidth, &model, " label='Des Width'  min=0.0 max = 900 step=1.0  group=Record ");
	TwAddVarCB(MiniGL::getTweakBar(), "Des Height", TW_TYPE_INT32, setDesHeight, getDesHeight, &model, " label='Des Height'  min=0.0 max = 900 step=1.0 group=Record ");
	TwAddVarRW(MiniGL::getTweakBar(), "rendering", TW_TYPE_BOOLCPP, &rendering, " label='rendering' group=Record key=n ");
	TwAddVarRW(MiniGL::getTweakBar(), "renderingWall", TW_TYPE_BOOLCPP, &renderingWall, " label='renderingWall' group=Record key=m ");
	TwAddVarRW(MiniGL::getTweakBar(), "renderingAir", TW_TYPE_BOOLCPP, &renderingAir, " label='renderingAir' group=Record key=i ");

	buildModel();

	if (context_major_version >= 3)
		createSphereBuffers((Real)particleRadius, 4);
		//createSphereBuffers((Real)particleRadius, 25);

	glutMainLoop ();

	cleanup ();
	file.close();

	//Timing::printAverageTimes();

	return 0;
}

void cleanup()
{
	delete TimeManager::getCurrent();
	if (context_major_version >= 3)
		releaseSphereBuffers();
}

int test = 0;
int index = 0;
int frameIndex = 0;

int numFrames = 0;
int totalFrames = 0;
int numFile = 0;
int seconds = 10;
int totalFrame = 64 * seconds;

int timeEmision = 0;
int numberFiles = 10;
int firtTimeAugmented = 0;
int tempAugmented = 0;
bool addWallTop = true;
bool temperatureFloor = true;

int numParticlesAir = 0;
void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	model.reset();
	simulation.reset();
	TimeManager::getCurrent()->setTime(0.0);

	test++;
	index = 0;
	numFile = 0;
	numFrames = 0;
	frameIndex=0;
	capture = false;
	record = false;
	doPause = true;
}

void mouseMove(int x, int y)
{
	Vector3r mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Vector3r diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const Real h = tm->getTimeStepSize();

	ParticleData &pd = model.getParticles();
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		pd.getVelocity(selectedParticles[j]) += 5.0*diff/h;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;
	selectedParticles.clear();
	ParticleData &pd = model.getParticles();
	Selection::selectRect(start, end, &pd.getPosition(0), &pd.getPosition(pd.size() - 1), selectedParticles);
	if (selectedParticles.size() > 0)
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

string nameFile = "condensation";

//*
void initData(int numParticles) {

	char bufPath[255];
	sprintf(bufPath, "/%s%d.txt",nameFile ,numFile++);
	file = ofstream(dataPath + bufPath, ios::out | ios::trunc | ios::binary);
	file.seekp(0, ios::beg);
	file.write(reinterpret_cast<char *>(&numParticles), sizeof(int));
}
void saveData(ParticleData* p) {

	if (numFrames == totalFrame) {
		file.close();

		if (numFile >= numberFiles) {
			reset();
			return;
			//exit(0);
		}
		char bufPath[255];
		sprintf(bufPath, "/%s%d.txt", nameFile, numFile++);
		file = ofstream(dataPath + bufPath, ios::out | ios::trunc | ios::binary);
		file.seekp(0, ios::beg);
		numFrames = 0;
	}

	int numParticles = p->getNumberOfParticles();

	file.seekp(0, ios::end);
	float x, y, z, t;
	for (int i = numParticlesAir; i < numParticles; i++) {
		Vector3r position = p->getPosition(i);

		t = p->getTemp(i);
		x = position[0];
		y = position[1];
		z = position[2];

		file.write(reinterpret_cast<char *>(&t), sizeof(float));
		file.write(reinterpret_cast<char *>(&x), sizeof(float));
		file.write(reinterpret_cast<char *>(&y), sizeof(float));
		file.write(reinterpret_cast<char *>(&z), sizeof(float));
	}
	numFrames++;
	totalFrames++;
}
//*/
void timeStep()
{

	if (capture) {
		MiniGL::saveFrame("D:\\heat-video\\frame", test, TimeManager::getCurrent()->getTime(), 255, 4);
		capture = false;
	}
	if (doPause) {
		return;
	}

	if (timeEmision > 0 && totalFrames >= 64 * timeEmision)
		model.setContact(true);
	if (model.getContact() && firtTimeAugmented > 0 && totalFrames >= 64 * firtTimeAugmented) {
		model.setTempContact(model.getTempContact() + tempAugmented);
		firtTimeAugmented = 0;
	}

	// Simulation code
	for (unsigned int i = 0; i < 4; i++) //Iterations Per Frame
	{
		simulation.step(model);
		// Draw simulation model
		/*ParticleData &pd = model.getParticles();
		const unsigned int nParticles = pd.size();
		for (unsigned int i = 0; i < nParticles; i++)
		{
			const Real t = 6.0f;
			if (pd.getPosition(i)[1] > 3) {
				pd.setTemp(i,t);
			}
		}*/

	}
	/* Measure Temp
	ParticleData &pd = model.getParticles();
	LOG_INFO << TimeManager::getCurrent()->getTime() << "\t" << pd.getTemp(0) << "\t" << pd.getTemp(pd.getNumberOfParticles() - 1) << "\t" << pd.getTemp(0) + pd.getTemp(pd.getNumberOfParticles() - 1);
	//*/
	saveData(&model.getParticles());

	/*
	if (TimeManager::getCurrent()->getTime()>5.0f) {
		Timing::printAverageTimes();
		exit(0);
	}*/

	if ( record) {
		MiniGL::saveFrame("D:\\heat-video\\frame", test, TimeManager::getCurrent()->getTime(), 255, 4);
	}

}

void buildModel ()
{
	//TimeManager::getCurrent ()->setTimeStepSize (0.05025);
	TimeManager::getCurrent()->setTimeStepSize(0.00390625);
	//TimeManager::getCurrent()->setTimeStepSize(0.001953125);

	createBreakingDam();

}

void renderWorld()
{

	/*glDepthMask(GL_TRUE);
	glEnable(GL_DEPTH_TEST);*/

	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, textureFloor);

	//float color[4] = { 1.0f, 1.0f, 1.0f, 0.9f };
	float color[4] = {0.9f, 0.9f, 0.9f, 1.0f};
	//MiniGL::hsvToRgb(1.0f, 1.0f, 1.0f, surfaceColor);
	//*

	float c = 40.0f;

	glBegin(GL_QUADS);
	float speccolor[4] = { 1.0, 1.0, 1.0, 1.0 };
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, speccolor);
	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 100.0);

	glTexCoord2d(-2.5, -2.5); glVertex3d(-c, 0.0, -c);
	glTexCoord2d(2.5, -2.5); glVertex3d(c, 0.0, -c);
	glTexCoord2d(2.5, 2.5); glVertex3d(c, 0.0, c);
	glTexCoord2d(-2.5, 2.5); glVertex3d(-c, 0.0, c);
	glEnd();
	glBindTexture(GL_TEXTURE_2D, 0);
}

void render ()
{
	if (!rendering) {
		MiniGL::drawTime(TimeManager::getCurrent()->getTime());
		return;
	}
	//renderWorld();

	// Draw simulation model
	const ParticleData &pd = model.getParticles();
	const unsigned int nParticles = pd.size();

	int particlesIni = 0;

	if (renderingAir) {
		particlesIni = numParticlesAir;
	}

	//glPointSize(4.0);

	const Real supportRadius = model.getSupportRadius();
	Real vmax = 0.4*2.0*supportRadius / TimeManager::getCurrent()->getTimeStepSize();
	Real vmin = 0.0;

	if (context_major_version > 3)
	{
		if (thermal) {

			for (unsigned int i = particlesIni; i < nParticles; i++)
			{
				Real v = pd.getVelocity(i).norm();
				v = 0.5*((v - vmin) / (vmax - vmin));
				v = min(128.0*v*v, 0.2);
				float fluidColor[4] = { 1.0f, 1.0f, 1.0f, 1.0f  };
				Real t = min((pd.getTemp(i)) *0.0065, 0.651);

				t = max(t, -0.05);
				//Real t = min((pd.getTemp(i)) *0.0075, 0.61);

				/*Real t = min((pd.getTemp(i)) *0.0081, 0.648);
				MiniGL::hsvToRgb(0.648 - t, 1.0f, 0.8f + (float)v, fluidColor);*/

				//if (t < 0)
					//t = 0;
				MiniGL::hsvToRgb(0.65 - t, 1.0f, 0.8f + (float)v, fluidColor);

				renderSphere(pd.getPosition(i), fluidColor);
			}

		}
		else {
			const vector<newDistanceConstraint> c = model.getConstraints();
			const unsigned int nc = c.size();

			for (unsigned int i = particlesIni; i < nParticles; i++)
			{
				Real v = pd.getVelocity(i).norm();
				v = 0.5*((v - vmin) / (vmax - vmin));
				v = min(128.0*v*v, 0.2);
				float fluidColor[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
				MiniGL::hsvToRgb((pd.getState(i) == 0 ? 1 : 0.55), 1.0f, 0.8f + (float)v, fluidColor);
				renderSphere(pd.getPosition(i), fluidColor);
			}

			float color[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
			MiniGL::hsvToRgb(0.25f, 1.0f, 1.0f, color);

			glLineWidth(3);
			glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
			glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);
			glBegin(GL_LINES);

			for (unsigned int i = 0; i < nc; i++)
			{
				Vector3r x1 = pd.getPosition(c[i].m_bodies[0]);
				Vector3r x2 = pd.getPosition(c[i].m_bodies[1]);

				glVertex3f(x1[0], x1[1], x1[2]);
				glVertex3f(x2[0], x2[1], x2[2]);

			}
			glEnd();
		}
		if (renderingWall) {
			float surfaceColor[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
			for (unsigned int i = 0; i < model.numBoundaryParticles(); i++)
				renderSphere(model.getBoundaryX(i), surfaceColor);
		}
	}
	else
	{
		glDisable(GL_LIGHTING);
		glBegin(GL_POINTS);
		for (unsigned int i = particlesIni; i < nParticles; i++)
		{
			Real v = pd.getVelocity(i).norm();
			v = 0.5*((v - vmin) / (vmax - vmin));
			v = min(128.0*v*v, 0.5);
			float fluidColor[4] = { 0.2f, 0.2f, 0.2f, 1.0 };
			MiniGL::hsvToRgb(0.55f, 1.0f, 0.5f + (float)v, fluidColor);

			glColor3fv(fluidColor);
			glVertex3v(&pd.getPosition(i)[0]);
		}
		glEnd();

		// 	glBegin(GL_POINTS);
		// 	for (unsigned int i = 0; i < model.numBoundaryParticles(); i++)
		// 	{
		// 		glColor3fv(surfaceColor);
		// 		glVertex3fv(&model.getBoundaryX(i)[0]);
		// 	}
		// 	glEnd();*/

		//glEnable(GL_LIGHTING);
	}

	float red[4] = { 1.0f, 1.0f, 1.0f, 1 };
	for (unsigned int j = 0; j < selectedParticles.size(); j++)
	{
		MiniGL::drawSphere(pd.getPosition(selectedParticles[j]), 0.05f, red);
	}

	/*
	TetModel *tetModel = model.getTetModel();
	const IndexedFaceMesh &surfaceMesh = tetModel->getSurfaceMesh();
	Visualization::drawMesh(pd, surfaceMesh, tetModel->getIndexOffset(), surfaceColor);
	//*/
	MiniGL::drawTime(TimeManager::getCurrent()->getTime());

	auto str = "s = "+ std::to_string(TimeManager::getCurrent()->getTime());

	//MiniGL::drawBitmapText(-0.33,0.25, str.c_str(), str.size()-4, red);
	//frameIndex++;

	renderWorld();
}

/** Create a breaking dam scenario
*/
void createBreakingDam()
{
	const Real diam = 2.0*particleRadius*compact;
	const Real startX = particleRadius;
	const Real startY = particleRadius;
	const Real startZ = particleRadius;

	model.setParticleRadius(particleRadius);

	/* Test for simulation complete (check in)
	width = 80;
	height = 70;
	depth = 60;
	float temperatureContact = 50;
	float temperatureSolid = -5;

	timeEmision = 2;//2s
	numberFiles = 7;

	firtTimeAugmented = 25;//20s
	tempAugmented = 80;

	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false,0.09);
	//model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.6, startY, -0.5), false, 0.07);
	model.setDilation(0.0001);
	model.setDifusion(25);
	model.setTempContact(temperatureContact);
	nameFile = "simulation";
	//*/

	/* Test for simulation complete - inverse (check in)
	width = 40;
	height = 100;
	depth = 40;
	float temperatureGas = 105.0;
	float temperatureLiquid = 5.1;
	float temperatureContact = 0;
	float temperatureSolid = -50;

	numberFiles = 3;

	model.addGas(diam, temperatureGas, Vector3r(width, 4, depth), Vector3r(0, 45*diam, 0));

	//model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, 2*startY, -1.2), false);
	//model.addModel(dataPath + "/bunny-14032.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.2, startY, -0.9), false, 0.09);
	model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.75, startY, -0.6), false, 0.07);

	model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(50);
	model.setDilation(0.0001);
	model.setNumMaxConstraints(4);
	model.setRadioSolidification(0.055);

	temperatureFloor = false;

	nameFile = "simulationI";

	//*/

	/* Test for heat and cool wather -> compare Transfer Heat (check in)
	width = 60;
	height = 20;//80;
	depth = 30;

	numberFiles = 4;

	float temperatureLiquid = 1, temperatureLiquid2 = 99;
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.5, 10, depth), Vector3r(-width*diam*0.25, 0, 0));
	model.addFluid(diam, temperatureLiquid2, Vector3r(width*0.5, 10, depth), Vector3r(width*diam*0.25, 0, 0));

	nameFile = "heat";
	//*/

	/* Test for liquid evaporation - convection  (check in)
	height = 70;
	depth = 30;//30;
	width = 30;
	float temperatureLiquid = 20;//80;
	float temperatureContact = 140;

	timeEmision = 2;//2s
	numberFiles = 8;

	model.addFluid(diam, temperatureLiquid, Vector3r(width, 20, depth), Vector3r(0,0.0045,0));
	//model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(40);

	nameFile = "evaporation";
	//*/

	/* Test for condensation (check in)
	width = 120;
	height = 175;
	depth = 40;
	float temperatureGas = 120.0;
	//float temperatureLiquid = 15.1;
	float temperatureContact = 20;
	float temperatureSolid = 0.0;

	numberFiles = 2;

	/*float temperatureAir = 10;
	//float temperatureAir1 = 10;
	float temperatureAir2 = 7;
	float temperatureAir3 = 3;
	float temperatureAir4 = 0;
	//model.addAir(diam, temperatureAir, Vector3r(width, 3, depth), Vector3r(0, height*diam - 155 * 2 * startY, 0),14);
	//model.addAir(diam, temperatureAir2, Vector3r(width, 3, depth), Vector3r(0, height*diam - 115 * 2 * startY, 0),13);
	//model.addAir(diam, temperatureAir3, Vector3r(width, 4, depth), Vector3r(0, height*diam - 77 * 2 * startY, 0),11);
	//model.addAir(diam, temperatureAir4, Vector3r(width, 4, depth), Vector3r(0, height*diam - 35 * 2 * startY, 0),10);

	numParticlesAir = model.getParticles().size();* /

	model.addGas(diam, temperatureGas, Vector3r(width, 17, depth), Vector3r(0, diam , 0));

	model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(20);
	model.setViscosity(0.001);

	nameFile = "condensation";
	//*/

	/* Test for melting bunny with heat liquid (check in)

	// change time step = 0.001953125, comment code add border top
	width = 80;
	height = 100;
	depth = 60;
	float temperatureLiquid = 99.0f;
	float temperatureSolid = -5;

	numberFiles = 2;

	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false, 0.09);
	model.addFluid(diam, temperatureLiquid, Vector3r(5, 300, 5), Vector3r(0.35, 3.0, 0.4));

	model.setDilation(0.0001);
	model.setDifusion(30);

	nameFile = "melting";
	addWallTop = false;
	//*/

	/* Test for solidify bunny (check) -
	width = 80;
	height = 100;
	depth = 60;
	float temperatureLiquid = 10.0f;
	float temperatureSolid = -40;

	numberFiles = 1;

	model.addFluid(diam, temperatureLiquid, Vector3r(8, 200,4), Vector3r(-0.25, 3.2, 0.1));
	//model.addFluid(diam, temperatureLiquid, Vector3r(5, 130, 2), Vector3r(-0.0, 3.0, 0.4));
	//model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid,0.55f, Vector3r(-0.75, startY, -0.6),false,0.07);
	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid,0.55f, Vector3r(-1.5, startY, -1.2),false,0.09);
	model.setDilation(0.0001);
	model.setNumMaxConstraints(6);
	model.setRadioSolidification(0.1);
	model.setDifusion(20);

	nameFile = "solidify";
	addWallTop = false;
	//*/

	/* Test for ( dilatation) --
	width = 60;
	height = 60;
	depth = 50;
	float temperatureSolid = -50;
	float temperarureContact = -1;
	//model.addSolid(diam, temperatureSolid,Vector3r(20,20,20),Vector3r(0, 0,0));
	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false, 0.09);
	//model.setContact(true);
	model.setDilation(0.01);
	model.setTempContact(temperarureContact);
	model.setDifusion(50);
	//*/

	/* Test for armadillo on liquid - caendo -- (check in)
	width = 60;
	height = 80;
	depth = 60;
	float temperatureLiquid = 50;
	float temperatureSolid = -40;

	numberFiles = 15;

	model.addFluid(diam, temperatureLiquid, Vector3r(width, 40, depth), Vector3r(0, 0, 0));
	model.addModel(dataPath + "/armadillo4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.25, 4.0, -depth*diam * 0.25), false,0.07);
	model.setContact(false);
	model.setDilation(0.0001);
	model.setTempContact(temperatureLiquid);
	model.setDifusion(20);

	nameFile = "armadillo";
	addWallTop = false;
	//*/

	/////////////////////////////////////////////////

	/* Test for solidification del liquido
	width = 100;
	height = 50;
	depth = 30;
	float temperatureLiquid = 10;
	float temperatureSolid = 0;
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.25, 20, depth), Vector3r(-width*diam*3/8 + particleRadius, startY, 0));

	model.setContact(true);
	model.setTempContact(-5);
	//model.addSolid(diam, 0, Vector3r(20, 20, 20), Vector3r(0.24, 2 * startY, 0));

	nameFile = "simulationI";

	//*/

	/* Test for multiples ice on liquid - caendo --
	width = 80;
	height = 120;
	depth = 50;
	float temperatureLiquid = 10;
	float temperatureSolid = 0;
	model.addFluid(diam, temperatureLiquid, Vector3r(width, 20, depth), Vector3r(startX, 2*startY, startZ));
	model.addSolid(diam, temperatureSolid, Vector3r(14, 14, 14), Vector3r(-1.5, 1.7, -0.5));
	model.addSolid(diam, temperatureSolid, Vector3r(17, 17, 17), Vector3r(1.3, 1.5, 0.5));
	model.addSolid(diam, temperatureSolid, Vector3r(10, 10, 10), Vector3r(-0.5, 2.7, -0.5));
	model.addSolid(diam, temperatureSolid, Vector3r(5, 5, 5), Vector3r(0.0, 3, -0.0));
	model.addSolid(diam, temperatureSolid, Vector3r(5, 5, 5), Vector3r(1.0, 3.2, -0.2));
	model.addSolid(diam, temperatureSolid, Vector3r(10, 10, 10), Vector3r(-1.2, 2.5, 0.5));
	//model.addModel(dataPath + "/armadillo4k.txt", diam, temperatureSolid, 0.45f, Vector3r(-width*diam * 0.25, 2.0, -depth*diam * 0.25),false);
	model.setContact(true);
	model.setDilation(0.0001);
	model.setTempContact(temperatureLiquid);
	model.setDifusion(25);
	//*/

	/* Test for armadillo on liquid - caendo -- (check in)
	width = 100;
	height = 250;
	depth = 70;
	float temperatureLiquid = 50;
	float temperatureSolid = 0;
	model.addFluid(diam, temperatureLiquid, Vector3r(width, 15, depth), Vector3r(startX, 2 * startY, startZ));
	model.addModel(dataPath + "/armadillo4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-width*diam * 0.25, 2.0, -depth*diam * 0.25), false);
	model.setContact(false);
	model.setDilation(0.0001);
	model.setTempContact(temperatureLiquid);
	model.setDifusion(20);
	//*/

	/* Test for meeltin ( derretimiento) --
	width = 50;
	height = 50;
	depth = 50;
	float temperatureSolid = 0;
	float temperarureContact = 10;
	model.addSolid(diam, temperatureSolid,Vector3r(30,40,20),Vector3r(particleRadius, 2 * startY,0));
	model.setContact(true);
	model.setDilation(0.0001);
	model.setTempContact(temperarureContact);
	model.setDifusion(10);
	//*/

	/* Test for heat and cool wather -> compare Transfer Heat cleary
	width = 80;
	height = 80;
	depth = 40;
	float temperatureLiquid = 6, temperatureLiquid2 = 80;
	model.heatTransferModel = 1;//model cleary
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.5, 15, depth), Vector3r(-width*diam*0.25 , 0, 0));
	model.addFluid(diam, temperatureLiquid2, Vector3r(width*0.5, 15, depth), Vector3r(width*diam*0.25 , 0, 0));

	model.setContact(false);
	model.setTempContact(temperatureLiquid2);
	//*/

	/* Test for liquidos caendo
	model.addFluid(diam, 21, Vector3r(3, 20, 3), Vector3r(-0.36, 2.0, 0.0));
	model.addFluid(diam, 55, Vector3r(3, 20, 3), Vector3r(0.16, 0.8, 0.18));
	model.addFluid(diam, 99, Vector3r(3, 20, 3), Vector3r(0.22, 2.8, -0.2));
	model.addSolid(diam, 0, Vector3r(25, 10, 15), Vector3r(0.025, 0, 0));
	//*/

	/* Test for simulation complete - inverse (check in)
	width = 100;
	height = 290;
	depth = 60;
	float temperatureGas = 109.0;
	//float temperatureLiquid = 15.1;
	float temperatureContact = 0;
	float temperatureSolid = 0.0;

	float temperatureAir = 10;
	//float temperatureAir1 = 10;
	float temperatureAir2 = 7;
	float temperatureAir3 = 3;
	float temperatureAir4 = 0;
	model.addAir(diam, temperatureAir, Vector3r(width, 3, depth), Vector3r(particleRadius, height*diam - 155 * 2 * startY, particleRadius), 14);
	model.addAir(diam, temperatureAir2, Vector3r(width, 3, depth), Vector3r(particleRadius, height*diam - 115 * 2 * startY, particleRadius), 13);
	model.addAir(diam, temperatureAir3, Vector3r(width, 4, depth), Vector3r(particleRadius, height*diam - 77 * 2 * startY, particleRadius), 11);
	model.addAir(diam, temperatureAir4, Vector3r(width, 4, depth), Vector3r(particleRadius, height*diam - 35 * 2 * startY, particleRadius), 10);

	numParticlesAir = model.getParticles().size();

	model.addGas(diam, temperatureGas, Vector3r(width, 20, depth), Vector3r(particleRadius, height*diam - 265 * 2 * startY, particleRadius));
	//model.addModel(dataPath + "/bunny.txt", diam, temperatureSolid,0.55f, Vector3r(-0.5, 2*startY, -0.3),false);
	//model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, 2 * startY, -1.2), false);

	//printf("%f",);
	//model.addFluid(diam, temperatureLiquid, Vector3r(width, 10, depth), Vector3r(particleRadius, 2 * startY, particleRadius));

	model.setContact(true);
	model.setTempContact(temperatureContact);
	model.setDifusion(30);
	//*/

	/* Test for heat and cool wather -> measure temp (check in)
	width = 50;
	height = 20;
	depth = 1;
	float temperatureLiquid = 1, temperatureLiquid2 = 99;
	model.addFluid(diam, temperatureLiquid, Vector3r(width*0.5, 10, depth), Vector3r(-width*diam*0.25, 0, 0));
	model.addFluid(diam, temperatureLiquid2, Vector3r(width*0.5, 10, depth), Vector3r(width*diam*0.25, 0, 0));
	nameFile = "measure temp";
	//*/

	/* Test for melting bunny - number particles (check in)
	width = 80;
	height = 250;
	depth = 60;
	float temperatureSolid = -5;

	timeEmision = 2;
	numberFiles = 1;

	//model.addModel(dataPath + "/bunny-46607.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false,0.09);nameFile = "46607Particles";//0.09
	//model.addModel(dataPath + "/bunny-27152.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.5, startY, -1.2), false, 0.09); nameFile = "27152Particles";//0.09
	//model.addModel(dataPath + "/bunny-14032.txt", diam, temperatureSolid, 0.55f, Vector3r(-1.2, startY, -0.9), false, 0.09);nameFile = "14032Particles";//0.09
	model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.8, startY, -0.7), false,0.08);nameFile = "6001Particles";//0.08
	//model.addModel(dataPath + "/bunny-1849.txt", diam, temperatureSolid, 0.55f, Vector3r(-0.6, startY, -0.5), false,0.07);nameFile = "1849Particles";//0.07

	model.setDilation(0.001);
	model.setDifusion(50);
	model.setTempContact(50);

	//*/
	/* Test for solidify bunny (check)
	width = 80;
	height = 300;
	depth = 60;
	float temperatureLiquid = 5.6f;
	float temperatureSolid = 0;

	model.addFluid(diam, temperatureLiquid, Vector3r(8, 200,4), Vector3r(-0.25, 3.2, 0.1));
	//model.addFluid(diam, temperatureLiquid, Vector3r(5, 130, 2), Vector3r(-0.0, 3.0, 0.4));
	//model.addModel(dataPath + "/bunny.txt", diam, temperatureSolid,0.55f, Vector3r(-0.3, 2*startY, 0.0),false);
	model.addModel(dataPath + "/bunny4k.txt", diam, temperatureSolid,0.55f, Vector3r(-1.5, 2*startY, -1.2),false);
	model.setDilation(0.0001);
	model.setNumMaxConstraints(6);
	model.setRadioSolidification(0.05);
	model.setDifusion(50);
	//*/

	//* Test for multiples demos - number particles (check in)
	width = 80;
	height = 90;
	depth = 60;
	float temperatureSolid = 50;

	//timeEmision = 1;
	numberFiles = 1;

	float scale = 0.6f;

	//model.addModel(dataPath + "/bunny-46607.txt", diam, temperatureSolid, scale, Vector3r(-1.65, startY, -1.3), false,0.09);//0.09
	//model.addModel(dataPath + "/bunny-27152.txt", diam, temperatureSolid, scale, Vector3r(-1.5, startY, -1.2), false, 0.09);//0.09
	//model.addModel(dataPath + "/bunny-14032.txt", diam, temperatureSolid, scale, Vector3r(-1.2, startY, -0.9), false, 0.09);//0.09
	//model.addModel(dataPath + "/bunny-6001.txt", diam, temperatureSolid, scale, Vector3r(-0.8, startY, -0.7), false,0.08);//0.08
	model.addModel(dataPath + "/bunny-1849.txt", diam, temperatureSolid, scale, Vector3r(-0.6, startY, -0.5), false,0.07);//0.07

	model.setDilation(0.001);
	model.setDifusion(50);
	model.setTempContact(-10);
	model.setContact(true);
	model.setRadioSolidification(0.05);
	model.setNumMaxConstraints(3);
	nameFile = "demos";

	//*/

	std::vector<Vector3r> boundaryParticles;
	std::vector<bool> boundaryActives;
	initBoundaryData(boundaryParticles, boundaryActives);

	model.initModel( (unsigned int)boundaryParticles.size(), boundaryParticles.data(), &boundaryActives);

	LOG_INFO << "Number particles:" << (model.getParticles().size() - numParticlesAir);
	//LOG_INFO << "Number of particles Total: " << model.getParticles().size();
	initData(model.getParticles().size() - numParticlesAir);
}

void addWall(const Vector3r &minX, const Vector3r &maxX, std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives,bool active)
{
	const Real particleDistance = 2*model.getParticleRadius()*compact;

	const Vector3r diff = maxX - minX;
	const unsigned int stepsX = (unsigned int)(round(diff[0] / particleDistance)) + 1u;
	const unsigned int stepsY = (unsigned int)(round(diff[1] / particleDistance)) + 1u;
	const unsigned int stepsZ = (unsigned int)(round(diff[2] / particleDistance)) + 1u;

	//printf("%d %d %d %f\n",stepsX, (unsigned int)(41.00),(unsigned int)(diff[0] / particleDistance), diff[0] / particleDistance);

	const unsigned int startIndex = (unsigned int) boundaryParticles.size();
	boundaryParticles.resize(startIndex + stepsX*stepsY*stepsZ);
	boundaryActives.resize(startIndex + stepsX*stepsY*stepsZ);

	#pragma omp parallel default(shared)
	{
		#pragma omp for schedule(static)
		for (int j = 0; j < (int)stepsX; j++)
		{
			for (unsigned int k = 0; k < stepsY; k++)
			{
				for (unsigned int l = 0; l < stepsZ; l++)
				{
					const Vector3r currPos = minX + Vector3r(j*particleDistance, k*particleDistance, l*particleDistance);
					boundaryParticles[startIndex + j*stepsY*stepsZ + k*stepsZ + l] = currPos;
					boundaryActives[startIndex + j*stepsY*stepsZ + k*stepsZ + l] = active;
				}
			}
		}
	}
}
void initBoundaryData(std::vector<Vector3r> &boundaryParticles, std::vector<bool> &boundaryActives)
{

	const Real containerWidth = (width)*particleRadius*compact;
	const Real containerDepth = (depth)*particleRadius*compact;
	const Real containerHeight = (height)*particleRadius*compact;

	const Real diameter = 2*particleRadius*compact;// compact;// *0.8;

	const Real x1 = -containerWidth  - particleRadius * compact;
	const Real x2 = containerWidth + particleRadius * compact;
	const Real y1 = -(sqrt(3) - 0.80)*particleRadius;
	const Real y2 = 2*containerHeight+ diameter - (sqrt(3) - 0.80)*particleRadius;
	const Real z1 = -containerDepth - particleRadius * compact;
	const Real z2 = containerDepth + particleRadius * compact;

	float rest = 0.001f;

	// Floor
	addWall(Vector3r(x1, y1, z1), Vector3r(x2, y1, z2), boundaryParticles, boundaryActives,temperatureFloor);
	// Top
	if(addWallTop)
		addWall(Vector3r(x1, y2, z1), Vector3r(x2, y2, z2), boundaryParticles, boundaryActives, true);
	// Left
	addWall(Vector3r(x1 - rest, y1+ diameter, z1+ diameter), Vector3r(x1 - rest, y2 - diameter, z2- diameter), boundaryParticles, boundaryActives, false);
	// Right
	addWall(Vector3r(x2 + rest, y1 + diameter, z1+ diameter), Vector3r(x2 + rest, y2 - diameter, z2- diameter), boundaryParticles, boundaryActives, false);
	// Back
	addWall(Vector3r(x1, y1 + diameter, z1), Vector3r(x2, y2 - diameter, z1), boundaryParticles, boundaryActives, false);
	// Front
	addWall(Vector3r(x1, y1 + diameter, z2), Vector3r(x2, y2- diameter, z2), boundaryParticles, boundaryActives, false);
}

void createSphereBuffers(Real radius, int resolution)
{
	Real PI = static_cast<Real>(M_PI);
	// vectors to hold our data
	// vertice positions
	std::vector<Vector3r> v;
	// normals
	std::vector<Vector3r> n;
	std::vector<unsigned short> indices;

	// initiate the variable we are going to use
	Real X1, Y1, X2, Y2, Z1, Z2;
	Real inc1, inc2, inc3, inc4, radius1, radius2;

	for (int w = 0; w < resolution; w++)
	{
		for (int h = (-resolution / 2); h < (resolution / 2); h++)
		{
			inc1 = (w / (Real)resolution) * 2 * PI;
			inc2 = ((w + 1) / (Real)resolution) * 2 * PI;
			inc3 = (h / (Real)resolution)*PI;
			inc4 = ((h + 1) / (Real)resolution)*PI;

			X1 = sin(inc1);
			Y1 = cos(inc1);
			X2 = sin(inc2);
			Y2 = cos(inc2);

			// store the upper and lower radius, remember everything is going to be drawn as triangles
			radius1 = radius*cos(inc3);
			radius2 = radius*cos(inc4);

			Z1 = radius*sin(inc3);
			Z2 = radius*sin(inc4);

			// insert the triangle coordinates
			v.push_back(Vector3r(radius1*X1, Z1, radius1*Y1));
			v.push_back(Vector3r(radius1*X2, Z1, radius1*Y2));
			v.push_back(Vector3r(radius2*X2, Z2, radius2*Y2));

			indices.push_back((unsigned short)v.size() - 3);
			indices.push_back((unsigned short)v.size() - 2);
			indices.push_back((unsigned short)v.size() - 1);

			v.push_back(Vector3r(radius1*X1, Z1, radius1*Y1));
			v.push_back(Vector3r(radius2*X2, Z2, radius2*Y2));
			v.push_back(Vector3r(radius2*X1, Z2, radius2*Y1));

			indices.push_back((unsigned short)v.size() - 3);
			indices.push_back((unsigned short)v.size() - 2);
			indices.push_back((unsigned short)v.size() - 1);

			// insert the normal data
			n.push_back(Vector3r(X1, Z1, Y1));
			n.push_back(Vector3r(X2, Z1, Y2));
			n.push_back(Vector3r(X2, Z2, Y2));
			n.push_back(Vector3r(X1, Z1, Y1));
			n.push_back(Vector3r(X2, Z2, Y2));
			n.push_back(Vector3r(X1, Z2, Y1));
		}
	}

	for (unsigned int i = 0; i < n.size(); i++)
		n[i].normalize();

	glGenBuffersARB(1, &vertexbuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, vertexbuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, v.size() * sizeof(Vector3r), &v[0], GL_STATIC_DRAW);

	glGenBuffersARB(1, &normalbuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, normalbuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, n.size() * sizeof(Vector3r), &n[0], GL_STATIC_DRAW);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	// Generate a buffer for the indices as well
	glGenBuffersARB(1, &elementbuffer);
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, elementbuffer);
	glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB, indices.size() * sizeof(unsigned short), &indices[0], GL_STATIC_DRAW);
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0);

	// store the number of indices for later use
	vertexBufferSize = (unsigned int)indices.size();

	// clean up after us
	indices.clear();
	n.clear();
	v.clear();
}

void renderSphere(const Vector3r &x, const float color[])
{
	glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);

	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, vertexbuffer);
	glVertexPointer(3, GL_REAL, 0, 0);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, normalbuffer);
	glNormalPointer(GL_REAL, 0, 0);

	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, elementbuffer);

	glPushMatrix();
	glTranslated(x[0], x[1], x[2]);
	glDrawElements(GL_TRIANGLES, (GLsizei)vertexBufferSize, GL_UNSIGNED_SHORT, 0);
	glPopMatrix();
	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, 0);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_VERTEX_ARRAY);
}

void releaseSphereBuffers()
{
	if (elementbuffer != 0)
	{
		glDeleteBuffersARB(1, &elementbuffer);
		elementbuffer = 0;
	}
	if (normalbuffer != 0)
	{
		glDeleteBuffersARB(1, &normalbuffer);
		normalbuffer = 0;
	}
	if (vertexbuffer != 0)
	{
		glDeleteBuffersARB(1, &vertexbuffer);
		vertexbuffer = 0;
	}
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(Real *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepFluidModel*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepFluidModel*)clientData)->getVelocityUpdateMethod();
}
//////

void TW_CALL setTempUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((FluidModel*)clientData)->heatTransferModel  = (unsigned int)val;
}

void TW_CALL getTempUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((FluidModel*)clientData)->heatTransferModel ;
}

//////
void TW_CALL setViscosity(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setViscosity(val);
}

void TW_CALL getViscosity(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getViscosity();
}

void TW_CALL getRadioSol(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getRadioSolidification();
}
void TW_CALL setRadioSol(const void *value, void *clientData) {
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setRadioSolidification(val);
}
void TW_CALL getNumConstraint(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getNumMaxConstraints();
}
void TW_CALL setNumConstraint(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setNumMaxConstraints(val);
}
void TW_CALL setStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setClothStiffness(val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getClothStiffness();
}

void TW_CALL setDilation(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setDilation(val);
}

void TW_CALL getDilation(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getDilation();
}

void TW_CALL setMelting(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setMelting(val);
}

void TW_CALL getMelting(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getMelting();
}

void TW_CALL setEvaporation(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setEvaporation(val);
}

void TW_CALL getEvaporation(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getEvaporation();
}

void TW_CALL setDifusion(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setDifusion(val);
}

void TW_CALL getDifusion(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getDifusion();
}

void TW_CALL setTempContact(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((FluidModel*)clientData)->setTempContact(val);
}

void TW_CALL getTempContact(void *value, void *clientData)
{
	*(Real *)(value) = ((FluidModel*)clientData)->getTempContact();
}

void TW_CALL setContact(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((FluidModel*)clientData)->setContact(val);
}

void TW_CALL getContact(void *value, void *clientData)
{
	*(bool *)(value) = ((FluidModel*)clientData)->getContact();
}

void TW_CALL setXPBD(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((FluidModel*)clientData)->setXPBD(val);
}

void TW_CALL getXPBD(void *value, void *clientData)
{
	*(bool *)(value) = ((FluidModel*)clientData)->getXPBD();
}

void TW_CALL setDesWidth(const void *value, void *clientData)
{
	int val = *( int *)(value);
	MiniGL::desWidth = val;
	MiniGL::reziseDesWidth();
}

void TW_CALL getDesWidth(void *value, void *clientData)
{
	*(int*)(value) = MiniGL::desWidth;
}

void TW_CALL setDesHeight(const void *value, void *clientData)
{
	int val = *(int *)(value);
	MiniGL::desHeight = val;
	MiniGL::reziseDesHeight();
}

void TW_CALL getDesHeight(void *value, void *clientData)
{
	*(int*)(value) = MiniGL::desHeight;
}


/*
 * odeint_rk4_array
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>

#include <boost/timer.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>

#include "lorenz.hpp"

typedef boost::timer timer_type;

typedef boost::array< double , 3 > state_type;

using namespace boost::numeric::odeint;

//typedef boost::numeric::odeint::runge_kutta4_classic< state_type > rk4_odeint_type;

// use the never resizer explicitely for optimal performance with gcc,
// for the intel compiler this doesnt matter and the above definition
// gives the same performance
typedef runge_kutta4_classic< state_type , double , state_type , double ,
                              array_algebra, default_operations, never_resizer > rk4_odeint_type;

const int loops = 21;
const int num_of_steps = 20000000;
const double dt = 1E-10;

int main()
{
    double min_time = 1E6; // something big
    rk4_odeint_type stepper;
    std::clog.precision(16);
    std::cout.precision(16);
    for( int n=0; n<loops; n++ )
    {
        state_type x = {{ 8.5, 3.1, 1.2 }};
        double t = 0.0;
        timer_type timer;
        for( size_t i = 0 ; i < num_of_steps ; ++i )
        {
            stepper.do_step( lorenz(), x, t, dt );
            t += dt;
        }
        min_time = std::min( timer.elapsed() , min_time );
        std::clog << timer.elapsed() << '\t' << x[0] << std::endl;
    }
    std::cout << "Minimal Runtime: " << min_time << std::endl;
}