//  (C) Copyright Jeremy William Murphy 2016.

//  Use, modification and distribution are 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_INTEGER_COMMON_FACTOR_RT_HPP
#define BOOST_INTEGER_COMMON_FACTOR_RT_HPP

#include <boost/assert.hpp>
#include <boost/core/enable_if.hpp>

#include <boost/config.hpp>  // for BOOST_NESTED_TEMPLATE, etc.
#include <boost/limits.hpp>  // for std::numeric_limits
#include <climits>           // for CHAR_MIN
#include <boost/detail/workaround.hpp>
#include <iterator>
#include <algorithm>
#include <limits>
#ifndef BOOST_NO_CXX11_HDR_TYPE_TRAITS
#include <type_traits>
#endif
#ifdef BOOST_NO_CXX11_HDR_FUNCTIONAL
#include <functional>
#endif

#if ((defined(BOOST_MSVC) && (BOOST_MSVC >= 1600)) || (defined(__clang__) && defined(__c2__)) || (defined(BOOST_INTEL) && defined(_MSC_VER))) && (defined(_M_IX86) || defined(_M_X64))
#include <intrin.h>
#endif

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127 4244)  // Conditional expression is constant
#endif

#if !defined(BOOST_NO_CXX11_HDR_TYPE_TRAITS) && !defined(BOOST_NO_CXX11_NOEXCEPT)
#define BOOST_GCD_NOEXCEPT(T) noexcept(std::is_arithmetic<T>::value)
#else
#define BOOST_GCD_NOEXCEPT(T)
#endif

namespace boost {

   template <class I>
   class rational;

   namespace integer {

      namespace gcd_detail{

         //
         // some helper functions which really should be constexpr already, but sadly aren't:
         //
#ifndef BOOST_NO_CXX14_CONSTEXPR
         template <class T>
         inline constexpr T constexpr_min(T const& a, T const& b) BOOST_GCD_NOEXCEPT(T)
         {
            return a < b ? a : b;
         }
         template <class T>
         inline constexpr auto constexpr_swap(T&a, T& b) BOOST_GCD_NOEXCEPT(T) -> decltype(a.swap(b))
         {
            return a.swap(b);
         }
         template <class T, class U>
         inline constexpr void constexpr_swap(T&a, U& b...) BOOST_GCD_NOEXCEPT(T)
         {
            T t(static_cast<T&&>(a));
            a = static_cast<T&&>(b);
            b = static_cast<T&&>(t);
         }
#else
         template <class T>
         inline T constexpr_min(T const& a, T const& b) BOOST_GCD_NOEXCEPT(T)
         {
            return a < b ? a : b;
         }
         template <class T>
         inline void constexpr_swap(T&a, T& b) BOOST_GCD_NOEXCEPT(T)
         {
            using std::swap;
            swap(a, b);
         }
#endif

      template <class T, bool a =
#ifndef BOOST_NO_CXX11_HDR_TYPE_TRAITS
         std::is_unsigned<T>::value ||
#endif
         (std::numeric_limits<T>::is_specialized && !std::numeric_limits<T>::is_signed)>
      struct gcd_traits_abs_defaults
      {
         inline static BOOST_CXX14_CONSTEXPR const T& abs(const T& val) BOOST_GCD_NOEXCEPT(T) { return val; }
      };
      template <class T>
      struct gcd_traits_abs_defaults<T, false>
      {
         inline static T BOOST_CXX14_CONSTEXPR abs(const T& val) BOOST_GCD_NOEXCEPT(T)
         {
            // This sucks, but std::abs is not constexpr :(
            return val < T(0) ? -val : val;
         }
      };

      enum method_type
      {
         method_euclid = 0,
         method_binary = 1,
         method_mixed = 2
      };

      struct any_convert
      {
         template <class T>
         any_convert(const T&);
      };

      struct unlikely_size
      {
         char buf[9973];
      };

      unlikely_size operator <<= (any_convert, any_convert);
      unlikely_size operator >>= (any_convert, any_convert);

      template <class T>
      struct gcd_traits_defaults : public gcd_traits_abs_defaults<T>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(T& val) BOOST_GCD_NOEXCEPT(T)
         {
            unsigned r = 0;
            while(0 == (val & 1u))
            {
#ifdef _MSC_VER  // VC++ can't handle operator >>= in constexpr code for some reason
               val = val >> 1;
#else
               val >>= 1;
#endif
               ++r;
            }
            return r;
         }
         inline static BOOST_CXX14_CONSTEXPR bool less(const T& a, const T& b) BOOST_GCD_NOEXCEPT(T)
         {
            return a < b;
         }

         static T& get_value();

#ifndef BOOST_NO_SFINAE
         static const bool has_operator_left_shift_equal = sizeof(get_value() <<= 2) != sizeof(unlikely_size);
         static const bool has_operator_right_shift_equal = sizeof(get_value() >>= 2) != sizeof(unlikely_size);
#else
         static const bool has_operator_left_shift_equal = true;
         static const bool has_operator_right_shift_equal = true;
#endif
         static const method_type method = std::numeric_limits<T>::is_specialized && std::numeric_limits<T>::is_integer && has_operator_left_shift_equal && has_operator_right_shift_equal ? method_mixed : method_euclid;
      };
      //
      // Default gcd_traits just inherits from defaults:
      //
      template <class T>
      struct gcd_traits : public gcd_traits_defaults<T> {};

      //
      // Some platforms have fast bitscan operations, that allow us to implement
      // make_odd much more efficiently, unfortunately we can't use these if we want
      // the functions to be constexpr as the compiler intrinsics aren't constexpr.
      //
#if defined(BOOST_NO_CXX14_CONSTEXPR) && ((defined(BOOST_MSVC) && (BOOST_MSVC >= 1600)) || (defined(__clang__) && defined(__c2__)) || (defined(BOOST_INTEL) && defined(_MSC_VER))) && (defined(_M_IX86) || defined(_M_X64))
#pragma intrinsic(_BitScanForward,)
      template <>
      struct gcd_traits<unsigned long> : public gcd_traits_defaults<unsigned long>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(unsigned long val) BOOST_NOEXCEPT
         {
            unsigned long result;
            _BitScanForward(&result, val);
            return result;
         }
         BOOST_FORCEINLINE static unsigned make_odd(unsigned long& val) BOOST_NOEXCEPT
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };

#ifdef _M_X64
#pragma intrinsic(_BitScanForward64)
      template <>
      struct gcd_traits<unsigned __int64> : public gcd_traits_defaults<unsigned __int64>
      {
         BOOST_FORCEINLINE static unsigned find_lsb(unsigned __int64 mask) BOOST_NOEXCEPT
         {
            unsigned long result;
            _BitScanForward64(&result, mask);
            return result;
         }
         BOOST_FORCEINLINE static unsigned make_odd(unsigned __int64& val) BOOST_NOEXCEPT
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
#endif
      //
      // Other integer type are trivial adaptations of the above,
      // this works for signed types too, as by the time these functions
      // are called, all values are > 0.
      //
      template <> struct gcd_traits<long> : public gcd_traits_defaults<long>
      { BOOST_FORCEINLINE static unsigned make_odd(long& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<unsigned int> : public gcd_traits_defaults<unsigned int>
      { BOOST_FORCEINLINE static unsigned make_odd(unsigned int& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<int> : public gcd_traits_defaults<int>
      { BOOST_FORCEINLINE static unsigned make_odd(int& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<unsigned short> : public gcd_traits_defaults<unsigned short>
      { BOOST_FORCEINLINE static unsigned make_odd(unsigned short& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<short> : public gcd_traits_defaults<short>
      { BOOST_FORCEINLINE static unsigned make_odd(short& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<unsigned char> : public gcd_traits_defaults<unsigned char>
      { BOOST_FORCEINLINE static unsigned make_odd(unsigned char& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<signed char> : public gcd_traits_defaults<signed char>
      { BOOST_FORCEINLINE static signed make_odd(signed char& val)BOOST_NOEXCEPT{ signed result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
      template <> struct gcd_traits<char> : public gcd_traits_defaults<char>
      { BOOST_FORCEINLINE static unsigned make_odd(char& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
      template <> struct gcd_traits<wchar_t> : public gcd_traits_defaults<wchar_t>
      { BOOST_FORCEINLINE static unsigned make_odd(wchar_t& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; } };
#endif
#ifdef _M_X64
      template <> struct gcd_traits<__int64> : public gcd_traits_defaults<__int64>
      { BOOST_FORCEINLINE static unsigned make_odd(__int64& val)BOOST_NOEXCEPT{ unsigned result = gcd_traits<unsigned __int64>::find_lsb(val); val >>= result; return result; } };
#endif

#elif defined(BOOST_GCC) || defined(__clang__) || (defined(BOOST_INTEL) && defined(__GNUC__))

      template <>
      struct gcd_traits<unsigned> : public gcd_traits_defaults<unsigned>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned find_lsb(unsigned mask)BOOST_NOEXCEPT
         {
            return __builtin_ctz(mask);
         }
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(unsigned& val)BOOST_NOEXCEPT
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
      template <>
      struct gcd_traits<unsigned long> : public gcd_traits_defaults<unsigned long>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned find_lsb(unsigned long mask)BOOST_NOEXCEPT
         {
            return __builtin_ctzl(mask);
         }
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(unsigned long& val)BOOST_NOEXCEPT
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
      template <>
      struct gcd_traits<boost::ulong_long_type> : public gcd_traits_defaults<boost::ulong_long_type>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned find_lsb(boost::ulong_long_type mask)BOOST_NOEXCEPT
         {
            return __builtin_ctzll(mask);
         }
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(boost::ulong_long_type& val)BOOST_NOEXCEPT
         {
            unsigned result = find_lsb(val);
            val >>= result;
            return result;
         }
      };
      //
      // Other integer type are trivial adaptations of the above,
      // this works for signed types too, as by the time these functions
      // are called, all values are > 0.
      //
      template <> struct gcd_traits<boost::long_long_type> : public gcd_traits_defaults<boost::long_long_type>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(boost::long_long_type& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<boost::ulong_long_type>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<long> : public gcd_traits_defaults<long>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(long& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<int> : public gcd_traits_defaults<int>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(int& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned long>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<unsigned short> : public gcd_traits_defaults<unsigned short>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(unsigned short& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<short> : public gcd_traits_defaults<short>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(short& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<unsigned char> : public gcd_traits_defaults<unsigned char>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(unsigned char& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<signed char> : public gcd_traits_defaults<signed char>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR signed make_odd(signed char& val)BOOST_NOEXCEPT { signed result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
      template <> struct gcd_traits<char> : public gcd_traits_defaults<char>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(char& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
      template <> struct gcd_traits<wchar_t> : public gcd_traits_defaults<wchar_t>
      {
         BOOST_FORCEINLINE static BOOST_CXX14_CONSTEXPR unsigned make_odd(wchar_t& val)BOOST_NOEXCEPT { unsigned result = gcd_traits<unsigned>::find_lsb(val); val >>= result; return result; }
      };
#endif
#endif
   //
   // The Mixed Binary Euclid Algorithm
   // Sidi Mohamed Sedjelmaci
   // Electronic Notes in Discrete Mathematics 35 (2009) 169-176
   //
   template <class T>
   BOOST_CXX14_CONSTEXPR T mixed_binary_gcd(T u, T v) BOOST_GCD_NOEXCEPT(T)
   {
      if(gcd_traits<T>::less(u, v))
         constexpr_swap(u, v);

      unsigned shifts = 0;

      if(u == T(0))
         return v;
      if(v == T(0))
         return u;

      shifts = constexpr_min(gcd_traits<T>::make_odd(u), gcd_traits<T>::make_odd(v));

      while(gcd_traits<T>::less(1, v))
      {
         u %= v;
         v -= u;
         if(u == T(0))
            return v << shifts;
         if(v == T(0))
            return u << shifts;
         gcd_traits<T>::make_odd(u);
         gcd_traits<T>::make_odd(v);
         if(gcd_traits<T>::less(u, v))
            constexpr_swap(u, v);
      }
      return (v == 1 ? v : u) << shifts;
   }

    /** Stein gcd (aka 'binary gcd')
     *
     * From Mathematics to Generic Programming, Alexander Stepanov, Daniel Rose
     */
    template <typename SteinDomain>
    BOOST_CXX14_CONSTEXPR SteinDomain Stein_gcd(SteinDomain m, SteinDomain n) BOOST_GCD_NOEXCEPT(SteinDomain)
    {
        BOOST_ASSERT(m >= 0);
        BOOST_ASSERT(n >= 0);
        if (m == SteinDomain(0))
            return n;
        if (n == SteinDomain(0))
            return m;
        // m > 0 && n > 0
        int d_m = gcd_traits<SteinDomain>::make_odd(m);
        int d_n = gcd_traits<SteinDomain>::make_odd(n);
        // odd(m) && odd(n)
        while (m != n)
        {
            if (n > m)
               constexpr_swap(n, m);
            m -= n;
            gcd_traits<SteinDomain>::make_odd(m);
        }
        // m == n
        m <<= constexpr_min(d_m, d_n);
        return m;
    }

    /** Euclidean algorithm
     *
     * From Mathematics to Generic Programming, Alexander Stepanov, Daniel Rose
     *
     */
    template <typename EuclideanDomain>
    inline BOOST_CXX14_CONSTEXPR EuclideanDomain Euclid_gcd(EuclideanDomain a, EuclideanDomain b) BOOST_GCD_NOEXCEPT(EuclideanDomain)
    {
        while (b != EuclideanDomain(0))
        {
            a %= b;
            constexpr_swap(a, b);
        }
        return a;
    }

    template <typename T>
    inline BOOST_CXX14_CONSTEXPR BOOST_DEDUCED_TYPENAME enable_if_c<gcd_traits<T>::method == method_mixed, T>::type
       optimal_gcd_select(T const &a, T const &b) BOOST_GCD_NOEXCEPT(T)
    {
       return gcd_detail::mixed_binary_gcd(a, b);
    }

    template <typename T>
    inline BOOST_CXX14_CONSTEXPR BOOST_DEDUCED_TYPENAME enable_if_c<gcd_traits<T>::method == method_binary, T>::type
       optimal_gcd_select(T const &a, T const &b) BOOST_GCD_NOEXCEPT(T)
    {
       return gcd_detail::Stein_gcd(a, b);
    }

    template <typename T>
    inline BOOST_CXX14_CONSTEXPR BOOST_DEDUCED_TYPENAME enable_if_c<gcd_traits<T>::method == method_euclid, T>::type
       optimal_gcd_select(T const &a, T const &b) BOOST_GCD_NOEXCEPT(T)
    {
       return gcd_detail::Euclid_gcd(a, b);
    }

    template <class T>
    inline BOOST_CXX14_CONSTEXPR T lcm_imp(const T& a, const T& b) BOOST_GCD_NOEXCEPT(T)
    {
       T temp = boost::integer::gcd_detail::optimal_gcd_select(a, b);
#if BOOST_WORKAROUND(BOOST_GCC_VERSION, < 40500)
       return (temp != T(0)) ? T(a / temp * b) : T(0);
#else
       return temp != T(0) ? T(a / temp * b) : T(0);
#endif
    }

} // namespace detail

template <typename Integer>
inline BOOST_CXX14_CONSTEXPR Integer gcd(Integer const &a, Integer const &b) BOOST_GCD_NOEXCEPT(Integer)
{
    if(a == (std::numeric_limits<Integer>::min)())
       return a == static_cast<Integer>(0) ? gcd_detail::gcd_traits<Integer>::abs(b) : boost::integer::gcd(static_cast<Integer>(a % b), b);
    else if (b == (std::numeric_limits<Integer>::min)())
       return b == static_cast<Integer>(0) ? gcd_detail::gcd_traits<Integer>::abs(a) : boost::integer::gcd(a, static_cast<Integer>(b % a));
    return gcd_detail::optimal_gcd_select(static_cast<Integer>(gcd_detail::gcd_traits<Integer>::abs(a)), static_cast<Integer>(gcd_detail::gcd_traits<Integer>::abs(b)));
}

template <typename Integer>
inline BOOST_CXX14_CONSTEXPR Integer lcm(Integer const &a, Integer const &b) BOOST_GCD_NOEXCEPT(Integer)
{
   return gcd_detail::lcm_imp(static_cast<Integer>(gcd_detail::gcd_traits<Integer>::abs(a)), static_cast<Integer>(gcd_detail::gcd_traits<Integer>::abs(b)));
}
#ifndef BOOST_NO_CXX11_VARIADIC_TEMPLATES
//
// This looks slightly odd, but the variadic forms must have 3 or more arguments, and the variadic argument pack may be empty.
// This matters not at all for most compilers, but Oracle C++ selects the wrong overload in the 2-arg case unless we do this.
//
template <typename Integer, typename... Args>
inline BOOST_CXX14_CONSTEXPR Integer gcd(Integer const &a, Integer const &b, const Integer& c, Args const&... args) BOOST_GCD_NOEXCEPT(Integer)
{
   Integer t = gcd(b, c, args...);
   return t == 1 ? 1 : gcd(a, t);
}

template <typename Integer, typename... Args>
inline BOOST_CXX14_CONSTEXPR Integer lcm(Integer const &a, Integer const &b, Integer const& c, Args const&... args) BOOST_GCD_NOEXCEPT(Integer)
{
   return lcm(a, lcm(b, c, args...));
}
#endif
//
// Special handling for rationals:
//
template <typename Integer>
inline typename boost::enable_if_c<std::numeric_limits<Integer>::is_specialized, boost::rational<Integer> >::type gcd(boost::rational<Integer> const &a, boost::rational<Integer> const &b)
{
   return boost::rational<Integer>(static_cast<Integer>(gcd(a.numerator(), b.numerator())), static_cast<Integer>(lcm(a.denominator(), b.denominator())));
}

template <typename Integer>
inline typename boost::enable_if_c<std::numeric_limits<Integer>::is_specialized, boost::rational<Integer> >::type lcm(boost::rational<Integer> const &a, boost::rational<Integer> const &b)
{
   return boost::rational<Integer>(static_cast<Integer>(lcm(a.numerator(), b.numerator())), static_cast<Integer>(gcd(a.denominator(), b.denominator())));
}
/**
 * Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
 * Chapter 4.5.2, Algorithm C: Greatest common divisor of n integers.
 *
 * Knuth counts down from n to zero but we naturally go from first to last.
 * We also return the termination position because it might be useful to know.
 *
 * Partly by quirk, partly by design, this algorithm is defined for n = 1,
 * because the gcd of {x} is x. It is not defined for n = 0.
 *
 * @tparam  I   Input iterator.
 * @return  The gcd of the range and the iterator position at termination.
 */
template <typename I>
std::pair<typename std::iterator_traits<I>::value_type, I>
gcd_range(I first, I last) BOOST_GCD_NOEXCEPT(I)
{
    BOOST_ASSERT(first != last);
    typedef typename std::iterator_traits<I>::value_type T;

    T d = *first++;
    while (d != T(1) && first != last)
    {
        d = gcd(d, *first);
        first++;
    }
    return std::make_pair(d, first);
}
template <typename I>
std::pair<typename std::iterator_traits<I>::value_type, I>
lcm_range(I first, I last) BOOST_GCD_NOEXCEPT(I)
{
    BOOST_ASSERT(first != last);
    typedef typename std::iterator_traits<I>::value_type T;

    T d = *first++;
    while (d != T(1) && first != last)
    {
        d = lcm(d, *first);
        first++;
    }
    return std::make_pair(d, first);
}

template < typename IntegerType >
class gcd_evaluator
#ifdef BOOST_NO_CXX11_HDR_FUNCTIONAL
   : public std::binary_function<IntegerType, IntegerType, IntegerType>
#endif
{
public:
#ifndef BOOST_NO_CXX11_HDR_FUNCTIONAL
   typedef IntegerType first_argument_type;
   typedef IntegerType second_argument_type;
   typedef IntegerType result_type;
#endif
   IntegerType operator()(IntegerType const &a, IntegerType const &b)const
   {
      return boost::integer::gcd(a, b);
   }
};

template < typename IntegerType >
class lcm_evaluator
#ifdef BOOST_NO_CXX11_HDR_FUNCTIONAL
   : public std::binary_function<IntegerType, IntegerType, IntegerType>
#endif
{
public:
#ifndef BOOST_NO_CXX11_HDR_FUNCTIONAL
   typedef IntegerType first_argument_type;
   typedef IntegerType second_argument_type;
   typedef IntegerType result_type;
#endif
   IntegerType operator()(IntegerType const &a, IntegerType const &b)const
   {
      return boost::integer::lcm(a, b);
   }
};

}  // namespace integer
}  // namespace boost

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif  // BOOST_INTEGER_COMMON_FACTOR_RT_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>
#include <tudat/math/basic/mathematicalConstants.h>
#include "tudat/astro/basic_astro/unitConversions.h"
#include <tudat/astro/basic_astro/orbitalElementConversions.h>

#include <tudat/io/basicInputOutput.h>
#include <tudat/io/applicationOutput.h>

#include "tudat/simulation/propagation_setup/propagationPatchedConicFullProblem.h"
#include "tudat/simulation/propagation_setup/propagationLambertTargeterFullProblem.h"

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/trajectory_design/trajectory.h"
#include "tudat/astro/trajectory_design/exportTrajectory.h"
#include "tudat/astro/trajectory_design/planetTrajectory.h"
#include <tudat/simulation/simulation.h>

int main( )
{
    using namespace tudat;
    using namespace tudat::input_output;
    using namespace tudat::input_output::parsed_data_vector_utilities;
    using namespace tudat::transfer_trajectories;

    ///  Characteristics of the interplanetary trajectory

    // Define central body of the trajectory and body to be propagated.
    std::vector< std::string > centralBody;
    centralBody.push_back( "Sun" );
    std::string bodyToPropagate = "spacecraft";

    // Specify the number and types of legs and type of legs.
    int numberOfLegs = 5;
    std::vector< TransferLegType > legTypeVector( numberOfLegs );
    legTypeVector[ 0 ] = mga1DsmVelocity_Departure;
    legTypeVector[ 1 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 2 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 3 ] = mga1DsmVelocity_Swingby;
    legTypeVector[ 4 ] = capture;

    // Name of the bodies involved in the trajectory
    std::vector< std::string > transferBodyTrajectory;
    transferBodyTrajectory.push_back("Earth");
    transferBodyTrajectory.push_back("Earth");
    transferBodyTrajectory.push_back("Venus");
    transferBodyTrajectory.push_back("Venus");
    transferBodyTrajectory.push_back("Mercury");

    // Create variable vector.
    std::vector< double > variableVector;

    // Add the time of flight and start epoch.
    variableVector.push_back( 1171.64503236 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 399.999999715 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 178.372255301 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 299.223139512 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 180.510754824 * physical_constants::JULIAN_DAY);
    variableVector.push_back( 1.0 ); // The capture time is irrelevant for the final leg.

    // Add the additional variables.
    // 1st leg.
    variableVector.push_back( 0.234594654679 );
    variableVector.push_back( 1408.99421278 );
    variableVector.push_back( 0.37992647165 * 2.0 * 3.14159265358979 );
    variableVector.push_back( std::acos(  2.0 * 0.498004040298 - 1. ) - 3.14159265358979 / 2.0 );
    // 2nd leg.
    variableVector.push_back( 0.0964769387134 );
    variableVector.push_back( 1.35077257078 );
    variableVector.push_back( 1.80629232251 * 6.378e6 );
    variableVector.push_back( 0.0 );
    // 3rd leg.
    variableVector.push_back( 0.829948744508);
    variableVector.push_back( 1.09554368115 );
    variableVector.push_back( 3.04129845698 * 6.052e6 );
    variableVector.push_back( 0.0 );
    // 4th leg.
    variableVector.push_back( 0.317174785637 );
    variableVector.push_back( 1.34317576594 );
    variableVector.push_back( 1.10000000891 * 6.052e6 );
    variableVector.push_back( 0.0 );

    // Create minimum pericenter radii vector
    std::vector< double > minimumPericenterRadii;
    minimumPericenterRadii.push_back( TUDAT_NAN ); minimumPericenterRadii.push_back( TUDAT_NAN ); minimumPericenterRadii.push_back( TUDAT_NAN );
    minimumPericenterRadii.push_back( TUDAT_NAN ); minimumPericenterRadii.push_back( TUDAT_NAN );

    // Create departure and capture variables.
    std::vector< double > semiMajorAxes;
    semiMajorAxes.push_back( std::numeric_limits< double >::infinity( ) ); semiMajorAxes.push_back( std::numeric_limits< double >::infinity( ) );
    std::vector< double > eccentricities;
    eccentricities.push_back( 0.0 ); eccentricities.push_back( 0.0 );

    // Define integrator settings.
    double initialTime = 0.0;
    double fixedStepSize = 1000.0;
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > > (
                numerical_integrators::rungeKutta4, initialTime, fixedStepSize);

    /// Ideal case

    // Create system of bodies.
    std::vector< double > gravitationalParametersTransferBodies;
    gravitationalParametersTransferBodies.push_back( 3.9860119e14 );
    gravitationalParametersTransferBodies.push_back( 3.9860119e14 );
    gravitationalParametersTransferBodies.push_back( 3.24860e14 );
    gravitationalParametersTransferBodies.push_back( 3.24860e14 );
    gravitationalParametersTransferBodies.push_back( 2.2321e13 );

    std::vector< ephemerides::EphemerisPointer > ephemerisVectorTransferBodies;
    ephemerisVectorTransferBodies.push_back( std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter) ) ;
    ephemerisVectorTransferBodies.push_back( std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter) ) ;
    ephemerisVectorTransferBodies.push_back( std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus) );
    ephemerisVectorTransferBodies.push_back( std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus) );
    ephemerisVectorTransferBodies.push_back( std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mercury) );

    simulation_setup::SystemOfBodies bodies = propagators::setupBodyMapFromUserDefinedEphemeridesForPatchedConicsTrajectory(
            centralBody[0], bodyToPropagate, transferBodyTrajectory,
            ephemerisVectorTransferBodies, gravitationalParametersTransferBodies, "ECLIPJ2000" );

    // Create acceleration map.
    std::vector< basic_astrodynamics::AccelerationMap > accelerationMap = propagators::setupAccelerationMapPatchedConicsTrajectory(
                transferBodyTrajectory.size(), centralBody[0], bodyToPropagate, bodies);

    // Calculate the patched conics solution and the propagation results of the associated full dynamics problem for each leg.
    std::map< int, std::map< double, Eigen::Vector6d > > patchedConicsTrajectory;
    std::map< int, std::map< double, Eigen::Vector6d > > fullProblemTrajectory;
    std::map< int, std::map< double, Eigen::VectorXd > > dependentVariablesTrajectory;

    propagators::fullPropagationPatchedConicsTrajectory(
                bodies, accelerationMap, transferBodyTrajectory, centralBody[0], bodyToPropagate,
            legTypeVector, variableVector, minimumPericenterRadii, semiMajorAxes, eccentricities, integratorSettings,
            patchedConicsTrajectory, fullProblemTrajectory, dependentVariablesTrajectory, false );

    for( auto itr : patchedConicsTrajectory )
    {
        std::cout << "Leg " << itr.first << "\n\n";
        std::cout << "Departure: " << fullProblemTrajectory[ itr.first ].begin( )->second -
                  patchedConicsTrajectory[ itr.first ].begin( )->second<< "\n\n";
        std::cout << "Arrival: " << fullProblemTrajectory[ itr.first ].rbegin( )->second -
                     patchedConicsTrajectory[ itr.first ].rbegin( )->second << "\n\n";
    }

    /// Perturbed case

    // Define accelerations acting on the spacecraft.
    std::map< std::string, std::vector< std::shared_ptr< simulation_setup::AccelerationSettings > > > bodyToPropagateAccelerations;
    bodyToPropagateAccelerations[ "Sun" ].push_back(std::make_shared< simulation_setup::AccelerationSettings >(
                                                                basic_astrodynamics::central_gravity ) );
    bodyToPropagateAccelerations[ "Earth" ].push_back(std::make_shared< simulation_setup::AccelerationSettings >(
                                                                basic_astrodynamics::central_gravity ) );
    bodyToPropagateAccelerations[ "Venus" ].push_back(std::make_shared< simulation_setup::AccelerationSettings >(
                                                                basic_astrodynamics::central_gravity ) );
    bodyToPropagateAccelerations[ "Mercury" ].push_back(std::make_shared< simulation_setup::AccelerationSettings >(
                                                                basic_astrodynamics::central_gravity ) );
    simulation_setup::SelectedAccelerationMap accelerationMapPerturbedCase;
    accelerationMapPerturbedCase[ "spacecraft" ] = bodyToPropagateAccelerations;

    // Create the acceleration map.
    basic_astrodynamics::AccelerationMap accelerationModelMapPerturbedCase = simulation_setup::createAccelerationModelsMap(
                bodies, accelerationMapPerturbedCase, {bodyToPropagate}, {centralBody} );
    std::vector<  basic_astrodynamics::AccelerationMap > accelerationMapVectorPerturbedCase;
    for (int i = 0 ; i < numberOfLegs ; i++){
        accelerationMapVectorPerturbedCase.push_back( accelerationModelMapPerturbedCase );
    }

    // Calculate the patched conics trajectory and propagate the full dynamics problem jointly.
    std::map< int, std::map< double, Eigen::Vector6d > > patchedConicsTrajectoryPerturbedCase;
    std::map< int, std::map< double, Eigen::Vector6d > > fullProblemTrajectoryPerturbedCase;
    std::map< int, std::map< double, Eigen::VectorXd > > dependentVariablesPerturbedCase;

    propagators::fullPropagationPatchedConicsTrajectory( bodies, accelerationMapVectorPerturbedCase,
            transferBodyTrajectory, centralBody[0], bodyToPropagate, legTypeVector, variableVector, minimumPericenterRadii,
            semiMajorAxes, eccentricities, integratorSettings, patchedConicsTrajectoryPerturbedCase,
            fullProblemTrajectoryPerturbedCase, dependentVariablesPerturbedCase, true );

    for( auto itr : patchedConicsTrajectoryPerturbedCase )
    {
        std::cout << "Leg " << itr.first << "\n\n";
        std::cout << "Departure: " << fullProblemTrajectoryPerturbedCase[ itr.first ].begin( )->second -
                  patchedConicsTrajectoryPerturbedCase[ itr.first ].begin( )->second<< "\n\n";
        std::cout << "Arrival: " << fullProblemTrajectoryPerturbedCase[ itr.first ].rbegin( )->second -
                     patchedConicsTrajectoryPerturbedCase[ itr.first ].rbegin( )->second << "\n\n";
    }

    /// Outputs

    for( std::map< int, std::map< double, Eigen::Vector6d > >::iterator itr = patchedConicsTrajectory.begin( );
         itr != patchedConicsTrajectory.end( ); itr++ ){

        input_output::writeDataMapToTextFile( fullProblemTrajectory[itr->first],
                                              "fullProblemInterplanetaryTrajectory_0_leg_" + std::to_string(itr->first) + ".dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( fullProblemTrajectoryPerturbedCase[itr->first],
                                              "fullProblemInterplanetaryTrajectory_1_leg_" + std::to_string(itr->first) + ".dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( patchedConicsTrajectory[itr->first],
                                              "patchedConicsInterplanetaryTrajectory_0_leg_" + std::to_string(itr->first) + ".dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( patchedConicsTrajectoryPerturbedCase[itr->first],
                                              "patchedConicsInterplanetaryTrajectory_1_leg_" + std::to_string(itr->first) + ".dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

    }

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}


#pragma once

//#define POLTMPPRIOR

#include "conditionals.hpp"

#include <memory>
#include <boost/format.hpp>
#include "lot.hpp"
#include "data.hpp"
#include "tree.hpp"
#include "model.hpp"
#include "likelihood.hpp"
#include "tree_manip.hpp"
#include "updater.hpp"
#include "omega_updater.hpp"
#include "pinvar_updater.hpp"
#include "statefreq_updater.hpp"
#include "exchangeability_updater.hpp"
#include "subset_relrate_updater.hpp"
#include "tree_updater.hpp"
#include "polytomy_updater.hpp"
#include "tree_length_updater.hpp"
#if defined(HOLDER_ETAL_PRIOR)
#   include "gamma_shape_updater.hpp"
#   include "edge_length_updater.hpp"
#else
#   include "gamma_ratevar_updater.hpp"
#   include "edge_proportion_updater.hpp"
#endif
#if defined(POLGSS)
#   include "conditional_clade_store.hpp"
#endif

namespace lorad {

    class Chain {

        friend class Likelihood;

        public:

            typedef std::vector<Updater::SharedPtr> updater_vect_t;
            typedef std::shared_ptr<Chain>          SharedPtr;

                                                    Chain();
                                                    ~Chain();

            void                                    clear();

            void                                    startTuning();
            void                                    stopTuning();

            void                                    setTreeFromNewick(std::string & newick);
#if defined(POLGSS)
            unsigned                                createUpdaters(Model::SharedPtr model, Lot::SharedPtr lot, Likelihood::SharedPtr likelihood, ConditionalCladeStore::SharedPtr conditional_clade_store);
#else
            unsigned                                createUpdaters(Model::SharedPtr model, Lot::SharedPtr lot, Likelihood::SharedPtr likelihood);
#endif

            TreeManip::SharedPtr                    getTreeManip();
            Model::SharedPtr                        getModel();
            double                                  getLogLikelihood() const;

            void                                    setHeatingPower(double p);
            double                                  getHeatingPower() const;

            void                                    setNextHeatingPower(double p);
            void                                    storeLogLikelihood();
            double                                  calcLogSteppingstoneRatio() const;

            void                                    setChainIndex(unsigned idx);
            double                                  getChainIndex() const;

            Updater::SharedPtr                      findUpdaterByName(std::string name);
            std::vector<std::string>                getUpdaterNames() const;
            std::vector<double>                     getAcceptPercentages() const;
            std::vector<unsigned>                   getNumUpdates() const;
            std::vector<double>                     getLambdas() const;
            void                                    setLambdas(std::vector<double> & v);

            double                                  calcLogLikelihood() const;
            double                                  calcLogJointPrior(bool verbose = false) const;
#if defined(POLGSS)
            double                                  calcLogReferenceDensity() const;
            void                                    setSteppingstoneMode(unsigned mode);
#endif

            void                                    start();
            void                                    stop();
            void                                    nextStep(int iteration);

        private:

            Model::SharedPtr                        _model;
            Lot::SharedPtr                          _lot;
            TreeManip::SharedPtr                    _tree_manipulator;

            updater_vect_t                          _updaters;

            updater_vect_t                          _prior_calculators;
            unsigned                                _chain_index;
            double                                  _heating_power;

            double                                  _next_heating_power;
            std::vector<double>                     _ss_loglikes;

#if defined(POLGSS)
            std::vector<double>                     _ss_logpriors;
            std::vector<double>                     _ss_logrefdists;
            unsigned                                _ss_mode;
#else
            bool                                    _heat_likelihood_only;
#endif
            double                                  _log_likelihood;
    };

    inline Chain::Chain() {
        clear();
    }

    inline Chain::~Chain() {
    }

    inline void Chain::clear() {
        _log_likelihood = 0.0;
        _updaters.clear();
        _chain_index = 0;
        setHeatingPower(1.0);
        _next_heating_power = 1.0;
        _ss_loglikes.clear();
#if defined(POLGSS)
        _ss_logpriors.clear();
        _ss_logrefdists.clear();
        _ss_mode = 0;
#else
        _heat_likelihood_only = false;
#endif
        startTuning();
    }

    inline void Chain::startTuning() {
        for (auto u : _updaters)
            u->setTuning(true);
    }

    inline void Chain::stopTuning() {
        for (auto u : _updaters)
            u->setTuning(false);
    }

    inline void Chain::setTreeFromNewick(std::string & newick) {
        assert(_updaters.size() > 0);
        if (!_tree_manipulator)
            _tree_manipulator.reset(new TreeManip);
        _tree_manipulator->buildFromNewick(newick, /*rooted*/ false, /*allow_polytomies*/ true);
        for (auto u : _updaters)
            u->setTreeManip(_tree_manipulator);
    }

#if defined(POLGSS)
    inline unsigned Chain::createUpdaters(Model::SharedPtr model, Lot::SharedPtr lot, Likelihood::SharedPtr likelihood, ConditionalCladeStore::SharedPtr conditional_clade_store) {
#else
    inline unsigned Chain::createUpdaters(Model::SharedPtr model, Lot::SharedPtr lot, Likelihood::SharedPtr likelihood) {
#endif
        _model = model;
        _lot = lot;
        _updaters.clear();
        _prior_calculators.clear();

        double wstd             = 1.0;
        double wtreelength      = 1.0;
        double wtreetopology    = 10.0;
        double wedgelengths     = 10.0;
        double wpolytomy        = 0.0;
        double sum_weights      = 0.0;

        if (_model->isAllowPolytomies()) {
            wstd             = 1.0;
            wtreelength      = 2.0;
            wtreetopology    = 9.0;
            wpolytomy        = 9.0;
        }

        // Add state frequency parameter updaters to _updaters
        Model::state_freq_params_t & statefreq_shptr_vect = _model->getStateFreqParams();
        for (auto statefreq_shptr : statefreq_shptr_vect) {
            Updater::SharedPtr u = StateFreqUpdater::SharedPtr(new StateFreqUpdater(statefreq_shptr));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(0.0001);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters(std::vector<double>(statefreq_shptr->getStateFreqsSharedPtr()->size(), 1.0));
#if defined(POLGSS)
            u->setRefDistParameters(statefreq_shptr->getStateFreqRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }

        // Add exchangeability parameter updaters to _updaters
        Model::exchangeability_params_t & exchangeability_shptr_vect = _model->getExchangeabilityParams();
        for (auto exchangeability_shptr : exchangeability_shptr_vect) {
            Updater::SharedPtr u = ExchangeabilityUpdater::SharedPtr(new ExchangeabilityUpdater(exchangeability_shptr));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(0.0001);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters({1.0, 1.0, 1.0, 1.0, 1.0, 1.0});
#if defined(POLGSS)
            u->setRefDistParameters(exchangeability_shptr->getExchangeabilityRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }

#if defined(HOLDER_ETAL_PRIOR)
        // Add gamma shape parameter updaters to _updaters
        Model::shape_params_t & shape_shptr_vect = _model->getShapeParams();
        for (auto shape_shptr : shape_shptr_vect) {
            Updater::SharedPtr u = GammaShapeUpdater::SharedPtr(new GammaShapeUpdater(shape_shptr));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(1.0);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters({1.0, 1.0});
#if defined(POLGSS)
            u->setRefDistParameters(shape_shptr->getShapeRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }
#else
        // Add rate variance parameter updaters to _updaters
        Model::ratevar_params_t & ratevar_shptr_vect = _model->getRateVarParams();
        for (auto ratevar_shptr : ratevar_shptr_vect) {
            Updater::SharedPtr u = GammaRateVarUpdater::SharedPtr(new GammaRateVarUpdater(ratevar_shptr));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(1.0);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters({1.0, 1.0});
#if defined(POLGSS)
            u->setRefDistParameters(ratevar_shptr->getRateVarRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }
#endif

        // Add pinvar parameter updaters to _updaters
        Model::pinvar_params_t & pinvar_shptr_vect = _model->getPinvarParams();
        for (auto pinvar_shptr : pinvar_shptr_vect) {
            Updater::SharedPtr u = PinvarUpdater::SharedPtr(new PinvarUpdater(pinvar_shptr));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(0.5);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters({1.0, 1.0});
#if defined(POLGSS)
            u->setRefDistParameters(pinvar_shptr->getPinvarRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }

        // Add omega parameter updaters to _updaters
        Model::omega_params_t & omega_shptr_vect = _model->getOmegaParams();
        for (auto omega_shptr : omega_shptr_vect) {
            Updater::SharedPtr u = OmegaUpdater::SharedPtr(new OmegaUpdater(omega_shptr));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(1.0);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters({1.0, 1.0});
#if defined(POLGSS)
            throw XLorad("Omega parameter not yet fully implemented");
//            u->setRefDistParameters(omega_shptr->getOmegaRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }

        // Add subset relative rate parameter updater to _updaters
        if (!_model->isFixedSubsetRelRates()) {
            Updater::SharedPtr u = SubsetRelRateUpdater::SharedPtr(new SubsetRelRateUpdater(_model));
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(1.0);
            u->setTargetAcceptanceRate(0.3);
            u->setPriorParameters(std::vector<double>(_model->getNumSubsets(), 1.0));
#if defined(POLGSS)
            u->setRefDistParameters(_model->getSubsetRelRatesRefDistParamsVect());
#endif
            u->setWeight(wstd); sum_weights += wstd;
            _updaters.push_back(u);
            _prior_calculators.push_back(u);
        }

        // Add tree updater and tree length updater to _updaters
#if defined(HOLDER_ETAL_PRIOR)
        double edgelen_exponential_rate = 10.0;
#else
        double tree_length_shape = 1.0;
        double tree_length_scale = 10.0;
        double dirichlet_param   = 1.0;
#endif

#if defined(HOLDER_ETAL_PRIOR)
        Updater::SharedPtr uu = EdgeLengthUpdater::SharedPtr(new EdgeLengthUpdater());
        uu->setLikelihood(likelihood);
        uu->setLot(lot);
        uu->setLambda(0.2);
        uu->setTargetAcceptanceRate(0.3);
        uu->setPriorParameters({edgelen_exponential_rate});
#if defined(POLGSS)
        uu->setRefDistParameters(_model->getEdgeLenRefDistParamsVect());
#endif
        uu->setWeight(wedgelengths); sum_weights += wedgelengths;
        _updaters.push_back(uu);
#else
        Updater::SharedPtr uu = EdgeProportionUpdater::SharedPtr(new EdgeProportionUpdater());
        uu->setLikelihood(likelihood);
        uu->setLot(lot);
        uu->setLambda(0.0001);
        uu->setTargetAcceptanceRate(0.3);
        uu->setPriorParameters({tree_length_shape, tree_length_scale, dirichlet_param});
#if defined(POLGSS)
        uu->setRefDistParameters(_model->getEdgeProportionsRefDistParamsVect());
#endif
        uu->setWeight(wedgelengths); sum_weights += wedgelengths;
        _updaters.push_back(uu);
#endif

        if (!_model->isFixedTree()) {
            Updater::SharedPtr u = TreeUpdater::SharedPtr(new TreeUpdater());
#if defined(POLGSS)
            u->setConditionalCladeStore(conditional_clade_store);
#endif
            u->setLikelihood(likelihood);
            u->setLot(lot);
            u->setLambda(0.5);
            u->setTargetAcceptanceRate(0.3);
#if defined(HOLDER_ETAL_PRIOR)
            u->setPriorParameters({edgelen_exponential_rate});
#else
            u->setPriorParameters({tree_length_shape, tree_length_scale, dirichlet_param});
#endif
            u->setTopologyPriorOptions(_model->isResolutionClassTopologyPrior(), _model->getTopologyPriorC());
            u->setWeight(wtreetopology); sum_weights += wtreetopology;
            _updaters.push_back(u);

            if (_model->isAllowPolytomies()) {
                Updater::SharedPtr u = PolytomyUpdater::SharedPtr(new PolytomyUpdater());
                u->setLikelihood(likelihood);
                u->setLot(lot);
                u->setLambda(0.05);
                u->setTargetAcceptanceRate(0.5);
#if defined(HOLDER_ETAL_PRIOR)
                u->setPriorParameters({edgelen_exponential_rate});
#else
                u->setPriorParameters({tree_length_shape, tree_length_scale, dirichlet_param});
#endif
                u->setTopologyPriorOptions(_model->isResolutionClassTopologyPrior(), _model->getTopologyPriorC());
                u->setWeight(wpolytomy); sum_weights += wpolytomy;
                _updaters.push_back(u);
            }
        }

        Updater::SharedPtr u = TreeLengthUpdater::SharedPtr(new TreeLengthUpdater());
        u->setLikelihood(likelihood);
        u->setLot(lot);
        u->setLambda(0.2);
        u->setTargetAcceptanceRate(0.3);
#if defined(HOLDER_ETAL_PRIOR)
        u->setPriorParameters({edgelen_exponential_rate});
#else
        u->setPriorParameters({tree_length_shape, tree_length_scale, dirichlet_param});
#endif
        u->setTopologyPriorOptions(_model->isResolutionClassTopologyPrior(), _model->getTopologyPriorC());
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
        u->setRefDistParameters(_model->getEdgeLenRefDistParamsVect());
#else
        u->setRefDistParameters(_model->getTreeLengthRefDistParamsVect());
#endif
#endif
        u->setWeight(wtreelength); sum_weights += wtreelength;
        _updaters.push_back(u);
        _prior_calculators.push_back(u);

        for (auto u : _updaters) {
            u->calcProb(sum_weights);
        }

        return (unsigned)_updaters.size();
    }

    inline TreeManip::SharedPtr Chain::getTreeManip() {
        return _tree_manipulator;
    }

    inline Model::SharedPtr Chain::getModel() {
        return _model;
    }

    inline double Chain::getHeatingPower() const {
        return _heating_power;
    }

    inline void Chain::setHeatingPower(double p) {
        _heating_power = p;
        for (auto u : _updaters)
            u->setHeatingPower(p);
    }

    inline void Chain::setNextHeatingPower(double p) {
#if defined(POLGSS)
        // Steppingstone mode:
        //   0: no steppingstone
        //   1: steppingstone (Xie et al. 2011)
        //   2: generalized steppingstone (Fan et al. 2011)
        for (auto u : _updaters)
            u->setSteppingstoneMode(_ss_mode);
#else
        _heat_likelihood_only = true; // next heating power only set if doing steppingstone
        for (auto u : _updaters)
            u->setHeatLikelihoodOnly(true);
#endif
        _next_heating_power = p;
    }

    inline void Chain::storeLogLikelihood() {
        double logLike = getLogLikelihood();
        _ss_loglikes.push_back(logLike);
#if defined(POLGSS)
        if (_ss_mode == 2) {
            double logPrior = calcLogJointPrior();
            _ss_logpriors.push_back(logPrior);
            double logRefDist = calcLogReferenceDensity();
            _ss_logrefdists.push_back(logRefDist);
        }
#endif
    }

    inline double Chain::calcLogSteppingstoneRatio() const {
        // Find the maximum log likelihood sampled by this chain
        unsigned sample_size = (unsigned)_ss_loglikes.size();
        assert(sample_size > 0);

        double log_ratio = 0.0;

#if defined(POLGSS)
        if (_ss_mode == 2) {
            assert(_ss_logpriors.size() == sample_size);
            assert(_ss_logrefdists.size() == sample_size);
            double beta_diff = _next_heating_power - _heating_power;
            std::vector<double> log_ratios(sample_size, 0.0);
            double max_log_ratio = 0.0;
            for (unsigned i = 0; i < sample_size; ++i) {
                double log_ratio = _ss_loglikes[i] + _ss_logpriors[i] - _ss_logrefdists[i];
                if (i == 0 || log_ratio > max_log_ratio)
                    max_log_ratio = log_ratio;
                log_ratios[i] = log_ratio;
            }

            // Compute sum, factoring out maxLnL
            double sum_of_terms = 0.0;
            for (auto logR : log_ratios) {
                sum_of_terms += exp(beta_diff*(logR - max_log_ratio));
            }

            // Compute the log of the steppingstone ratio
            assert(sum_of_terms > 0.0);
            log_ratio = beta_diff*max_log_ratio + log(sum_of_terms) - log(sample_size);
            }
        else {
#endif
            double maxLogL = *(std::max_element(_ss_loglikes.begin(), _ss_loglikes.end()));

            // Compute sum, factoring out maxLnL
            double sum_of_terms = 0.0;
            for (auto logL : _ss_loglikes) {
                sum_of_terms += exp((_next_heating_power - _heating_power)*(logL - maxLogL));
            }

            // Compute the log of the steppingstone ratio
            assert(sum_of_terms > 0.0);
            log_ratio = (_next_heating_power - _heating_power)*maxLogL + log(sum_of_terms) - log(sample_size);
#if defined(POLGSS)
        }
#endif
        return log_ratio;
    }

    inline double Chain::getChainIndex() const {
        return _chain_index;
    }

    inline void Chain::setChainIndex(unsigned idx) {
        _chain_index = idx;
    }

    inline Updater::SharedPtr Chain::findUpdaterByName(std::string name) {
        Updater::SharedPtr retval = nullptr;
        for (auto u : _updaters) {
            if (u->getUpdaterName() == name) {
                retval = u;
                break;
            }
        }
        assert(retval != nullptr);
        return retval;
    }

    inline std::vector<std::string> Chain::getUpdaterNames() const {
        std::vector<std::string> v;
        for (auto u : _updaters)
            v.push_back(u->getUpdaterName());
        return v;
    }

    inline std::vector<double> Chain::getAcceptPercentages() const {
        std::vector<double> v;
        for (auto u : _updaters)
            v.push_back(u->getAcceptPct());
        return v;
    }

    inline std::vector<unsigned> Chain::getNumUpdates() const {
        std::vector<unsigned> v;
        for (auto u : _updaters)
            v.push_back(u->getNumUpdates());
        return v;
    }

    inline std::vector<double> Chain::getLambdas() const {
        std::vector<double> v;
        for (auto u : _updaters)
            v.push_back(u->getLambda());
        return v;
    }

    inline void Chain::setLambdas(std::vector<double> & v) {
        assert(v.size() == _updaters.size());
        unsigned index = 0;
        for (auto u : _updaters) {
            u->setLambda(v[index++]);
        }
    }

    inline double Chain::calcLogLikelihood() const {
        return _updaters[0]->calcLogLikelihood();
    }

    inline double Chain::calcLogJointPrior(bool verbose) const {
        double lnP = 0.0;
        for (auto u : _prior_calculators) {
            std::string this_name = u->getUpdaterName();
            if (this_name == "Tree Length") {
#if defined(HOLDER_ETAL_PRIOR)
                double edgelen_prior = u->calcLogEdgeLengthPrior();
                lnP += edgelen_prior;
#else
                auto edgelen_prior = u->calcLogEdgeLengthPrior();
                lnP += edgelen_prior.first;
                lnP += edgelen_prior.second;
#endif
                if (verbose) {
#if defined(HOLDER_ETAL_PRIOR)
                    ::om.outputConsole(boost::format("%12.5f <-- Edge Lengths\n") % edgelen_prior);
#else
                    ::om.outputConsole(boost::format("%12.5f <-- Tree Length\n") % edgelen_prior.first);
                    ::om.outputConsole(boost::format("%12.5f <-- Edge Length Proportions\n") % edgelen_prior.second);
#endif
                }
                if (!_model->isFixedTree()) {
                    double topology_prior = u->calcLogTopologyPrior();
                    if (verbose)
                        ::om.outputConsole(boost::format("%12.5f <-- Tree Topology\n") % topology_prior);
                    lnP += topology_prior;
                }
            }
            else {
                double this_log_prior = u->calcLogPrior();
                if (verbose)
                    ::om.outputConsole(boost::format("%12.5f <-- %s\n") % this_log_prior % this_name);
                lnP += this_log_prior;
            }
        }
        if (verbose)
            ::om.outputConsole(boost::format("%12.5f <-- Log Joint Prior\n") % lnP);
        return lnP;
    }

#if defined(POLGSS)
    inline double Chain::calcLogReferenceDensity() const {
        double lnP = 0.0;
#if defined(POLTMPPRIOR)
        ::om.outputConsole("\nChain::calcLogReferenceDensity():\n");
        for (auto u : _updaters) {
            assert(u->_name != "Polytomies");
            if (u->_name == "Polytomies") {
                throw XLorad("Generalized stepping-stone marginal likelihood estimation cannot be performed if polytomies are allowed");
            }
            double log_reference_density = u->calcLogRefDist();
            ::om.outputConsole(boost::format("%12.5f <-- %s\n") % log_reference_density % u->getUpdaterName());
            lnP += log_reference_density;
        }
        ::om.outputConsole(boost::format("%12.5f <-- joint log reference density\n") % lnP);
#else
        for (auto u : _updaters) {
            assert(u->_name != "Polytomies");
            if (u->_name == "Polytomies") {
                throw XLorad("Generalized stepping-stone marginal likelihood estimation cannot be performed if polytomies are allowed");
            }
            if ((u->_name != "Edge Length") && (u->_name != "Edge Proportions")) {
                double log_reference_density = u->calcLogRefDist();
                lnP += log_reference_density;
            }
        }
#endif
        return lnP;
    }

    inline void Chain::setSteppingstoneMode(unsigned mode) {
        _ss_mode = mode;
    }
#endif

    inline void Chain::start() {
        _tree_manipulator->selectAllPartials();
        _tree_manipulator->selectAllTMatrices();
        _log_likelihood = calcLogLikelihood();
    }

    inline void Chain::stop() {
    }

    inline void Chain::nextStep(int iteration) {
        assert(_lot);
        double u = _lot->uniform();
        double cumprob = 0.0;
        unsigned i = 0;
        for (auto updater : _updaters) {
            cumprob += updater->_prob;
            if (u <= cumprob)
                break;
            i++;
        }
        assert(i < _updaters.size());
        _log_likelihood = _updaters[i]->update(_log_likelihood);
    }

    inline double Chain::getLogLikelihood() const {
        return _log_likelihood;
    }

}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018. Toyota Research Institute
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of CNRS-LAAS and AIST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** @author Hongkai Dai*/
#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h"

#include <gtest/gtest.h>
#include <Eigen/Dense>

#include "fcl/narrowphase/detail/gjk_solver_libccd.h"

namespace fcl {
namespace detail {

// Given two spheres, sphere 1 has radius1, and centered at point A, whose
// position is p_FA measured and expressed in frame F; sphere 2 has radius2,
// and centered at point B, whose position is p_FB measured and expressed in
// frame F. Computes the signed distance between the two spheres, together with
// the two closest points Na on sphere 1 and Nb on sphere 2, returns the
// position of Na and Nb expressed in frame F.
// We use the monogram notation on spatial vectors. The monogram notation is
// explained in
// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
template <typename S>
S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3<S>& p_FA,
                              const Vector3<S>& p_FB, Vector3<S>* p_FNa,
                              Vector3<S>* p_FNb) {
  S min_distance = (p_FA - p_FB).norm() - radius1 - radius2;
  const Vector3<S> p_AB_F =
      p_FB - p_FA;  // The vector AB measured and expressed
                    // in frame F.
  *p_FNa = p_FA + p_AB_F.normalized() * radius1;
  *p_FNb = p_FB - p_AB_F.normalized() * radius2;
  return min_distance;
}

template <typename S>
void TestSphereToSphereGJKSignedDistance(S radius1, S radius2,
                                         const Vector3<S>& p_FA,
                                         const Vector3<S>& p_FB, S tol,
                                         S min_distance_expected,
                                         const Vector3<S>& p_FNa_expected,
                                         const Vector3<S>& p_FNb_expected) {
  // Test if GJKSignedDistance computes the right distance. Here we used sphere
  // to sphere as the geometries. The distance between sphere and sphere should
  // be computed using distance between primitives, instead of the GJK
  // algorithm. But here we choose spheres for simplicity.

  fcl::Sphere<S> s1(radius1);
  fcl::Sphere<S> s2(radius2);
  fcl::Transform3<S> tf1, tf2;
  tf1.setIdentity();
  tf2.setIdentity();
  tf1.translation() = p_FA;
  tf2.translation() = p_FB;
  void* o1 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s1, tf1);
  void* o2 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s2, tf2);

  S dist;
  Vector3<S> p1, p2;
  GJKSolver_libccd<S> gjkSolver;
  bool res = GJKSignedDistance(
      o1, detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(), o2,
      detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(),
      gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist,
      &p1, &p2);

  EXPECT_EQ(res, min_distance_expected >= 0);

  EXPECT_NEAR(dist, min_distance_expected, tol);
  EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol));
  EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol));

  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);
  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o2);
}

template <typename S>
struct SphereSpecification {
  SphereSpecification<S>(S radius_, const Vector3<S>& center_)
      : radius{radius_}, center{center_} {}
  S radius;
  Vector3<S> center;
};

template <typename S>
void TestNonCollidingSphereGJKSignedDistance(S tol) {
  std::vector<SphereSpecification<S>> spheres;
  spheres.emplace_back(0.5, Vector3<S>(0, 0, -1.2));
  spheres.emplace_back(0.5, Vector3<S>(1.25, 0, 0));
  spheres.emplace_back(0.3, Vector3<S>(-0.2, 0, 0));
  spheres.emplace_back(0.4, Vector3<S>(-0.2, 0, 1.1));
  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
    for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
      if ((spheres[i].center - spheres[j].center).norm() >
          spheres[i].radius + spheres[j].radius) {
        // Not in collision.
        Vector3<S> p_FNa, p_FNb;
        const S min_distance_expected = ComputeSphereSphereDistance(
            spheres[i].radius, spheres[j].radius, spheres[i].center,
            spheres[j].center, &p_FNa, &p_FNb);
        TestSphereToSphereGJKSignedDistance<S>(
            spheres[i].radius, spheres[j].radius, spheres[i].center,
            spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb);
      } else {
        GTEST_FAIL() << "The two spheres collide."
                     << "\nSpheres[" << i << "] with radius "
                     << spheres[i].radius << ", centered at "
                     << spheres[i].center.transpose() << "\nSpheres[" << j
                     << "] with radius " << spheres[j].radius
                     << ", centered at " << spheres[j].center.transpose()
                     << "\n";
      }
    }
  }
}

GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) {
  // TODO(hongkai.dai@tri.global): By setting gjkSolver.distance_tolerance to
  // the default value (1E-6), the tolerance we get on the closest points are
  // only up to 1E-3. Should investigate why there is such a big difference.
  TestNonCollidingSphereGJKSignedDistance<double>(1E-3);
  TestNonCollidingSphereGJKSignedDistance<float>(1E-3);
}
}  // namespace detail
}  // namespace fcl

//==============================================================================
int main(int argc, char* argv[]) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include "mapdata.h++"
#include "pathfinder.h++"
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <algorithm>
#include <ext/algorithm>
#include <functional>
#include <ext/functional>
using namespace std;
using namespace boost::lambda;
#if defined _GNU_SOURCE
using __gnu_cxx::compose1;
#endif

const unsigned char
PathFinder::Directions::even_col_mask = 0xd7 ;// 10111110b
const unsigned char
PathFinder::Directions::odd_col_mask  = 0x7d ;// 11101011b

const
Scr::Vector
PathFinder::Directions::AvailDirections[] =
{
	Scr::Vector(-1,0),
	Scr::Vector(-1,1),
	Scr::Vector(0,1),
	Scr::Vector(1,1),
	Scr::Vector(1,0),
	Scr::Vector(1,-1),
	Scr::Vector(0,-1),
	Scr::Vector(-1,-1)
};

PathFinder::Directions::Directions(MapData * pMD, Scr::Position p)
{

	for (
		unsigned char legal_directions =
			( p.col&1 ) ? odd_col_mask : even_col_mask, i=0;
		legal_directions;
		legal_directions>>=1,i++)
		if (legal_directions&1)
		{
			Scr::Position np(p+AvailDirections[i]);
			if ( np.row < pMD->GetHeight()
			and  np.col < pMD->GetWidth() )
				push_back(np);
		}		// we don't have to check lower bound, as row and col
				// are unsigned, and become LARGE when are expected to
				// be negative.
}

PathFinder::PFDataEntry::PFDataEntry(MapData * pMD, Scr::Position p, int * l):
	D1(pMD,p)
{
	copy(l,l+L::static_size,L1.elems);
}
PathFinder::PFDataEntry::PFDataEntry(MapData * pMD, Scr::Position p, L l):
	D1(pMD,p),L1(l)
{
}
bool PathFinder::BetterCoords(Scr::Position p1,Scr::Position p2)
{
	// if field isn't described, it certainly isn't part of ANY
	// correct path
	if (!PFD1[p1.row][p1.col].get())
		return false;
	if  (!PFD1[p2.row][p2.col].get())
		return true;

	// Unoccupied fields are better, as they allow us save AP
	return
		lexicographical_compare(PFD1[p1.row][p1.col]->L1.begin(),
								PFD1[p1.row][p1.col]->L1.end(),
								PFD1[p2.row][p2.col]->L1.begin(),
								PFD1[p2.row][p2.col]->L1.end());
}

void PathFinder::MarkPathRecursive(Scr::Position p, PFDataEntry::L l)
{
	if (not PFD1[p.row][p.col].get())
	{
		PFD1[p.row][p.col].reset(new PFDataEntry(pMD1,p,l));
		Path & LD1 = PFD1[p.row][p.col]->D1;
		LD1.erase(
			remove_if(LD1.begin(),
					  LD1.end(),
					  compose1(logical_not<bool>(),OD1)),
			LD1.end());
	}
	else if	(lexicographical_compare(l.begin(), l.end(),
									 PFD1[p.row][p.col]->L1.begin(),
									 PFD1[p.row][p.col]->L1.end()))
		PFD1[p.row][p.col]->L1=l;

	else
		return ;

	PFDataEntry & PFDE1 = *PFD1[p.row][p.col];

	if (l[0]+Distance(p,finish) >lm)
	{
		StartPoints.push_back(p);
		return ;
	}
	PFDataEntry::L l2(l);
	if ( p == finish )
	{
		lm=l[0];
		StartPoints.push_back(p);
		return ;
	}
	for (Path::iterator i =
			 PFDE1.D1.begin();
		 i!=
			 PFDE1.D1.end();
		 ++i
		)
	{
		if (pMD1->GetEntity(i->row,i->col).get())
			++(l2[1]);
		l2[0]=l[0]+CC1(*i);
		l2[2]=abs(static_cast<long>(i->row-start.row));
		MarkPathRecursive(*i,l2 );
	}
}

// the most trivial algorithm.
// it takes one second for 1000 tiles paths on 1000x2000 boards
// (Pentium IV)
void PathFinder::ComputePath()
{
	P1.clear();
	if (start==finish)
	{
		P1.push_front(start);
		return;
	}
	lm = Distance(start,finish)+CC1(start)+CC1(finish)-1;

	unsigned lm_t = Distance(start,finish)*10;
	PFDataEntry::L L0={{0,0,0}};
	if (not PFD1[start.row][start.col].get())
		MarkPathRecursive(start,L0);

	while( (not PFD1[finish.row][finish.col].get())
		   and (not StartPoints.empty())
		   and (lm <= lm_t)
		)

	{
			Path tmp;
			tmp.swap(StartPoints);
			for (Path::iterator i = tmp.begin(),
					 j=tmp.end();
				 i != j; i++ )
			{
				PFDataEntry::L
					l(PFD1[i->row][i->col]->L1);
				;;
				(PFD1[i->row][i->col]->L1[0])++;
				MarkPathRecursive(*i,l);
			}
			lm++;
			//StartPoints.clear();
	}
	if (PFD1[finish.row][finish.col].get() == NULL)
		return;

	P1.push_front(finish);
	if (start==finish)
		return;
	//return;
	while (* P1.begin() != start)
	{//P1.begin()->D1
		Path::iterator min;
		//min = min_element()
		Path & LD1(PFD1[P1.begin()->row][P1.begin()->col]->
				   D1);
		if (LD1.empty())
			return; // no way!
 		P1.push_front(
 			*min_element(LD1.begin(),LD1.end(),
 				bind(&PathFinder::BetterCoords,this,_1,_2)));

	}

	//Temporary tracing proc.

// 	cout << "\x1b[H\n\n";
// 	for (int i = 0 ; i!= pMD1->GetHeight() ; i++)
// 		cout << "\n\n";
// //	return;
// 	system("clear");
// 	cout << lm << "  \n";;
// 	for (int i = 0 ; i!= pMD1->GetHeight() ; i++)
// 	{
// 		for (int j = 0 ; j< pMD1->GetWidth() ; j+=2)
// 		{
// 			if (PFD1[i][j].get())
// 				cout << setw(3)  << PFD1[i][j]->L1[1] <<
// 					(pMD1->GetEntity(i,j).get()?'X':
// 					',')
// 					 << PFD1[i][j]->L1[2];
// 			else
// 				cout << "   --";
// 			cout << "      ";
// 		}
// 		cout << "\n";

// 		for (int j = 1 ; j< pMD1->GetWidth() ; j+=2)
// 		{
// 			cout << "      ";
// 			if (PFD1[i][j].get())
// 				cout << setw(3)  << PFD1[i][j]->L1[1] <<
// 					(pMD1->GetEntity(i,j).get()?'X':
// 					',')
// 					 << PFD1[i][j]->L1[2];
// 			else
// 				cout << "   --";

// 		}
// 		cout << "\n";
// 		}/**/
/*		cout  << endl;
 */

}

PathFinder::PathFinder(MapData * pMD,
					   Scr::Position s,
					   Scr::Position f,
					   ObstacleDetector OD,
				CostCalculator CC)
	:
	pMD1(pMD),start(s),finish(f),OD1(OD),CC1(CC)
{

 	PFD1.reset(new boost::scoped_array<pPFDataEntry>[pMD1->GetHeight()]);

 	for ( unsigned int i = 0 ; i!= pMD1->GetHeight() ; ++i )
 		PFD1[i].reset(new pPFDataEntry[pMD1->GetWidth()]);
	ComputePath();
}

void PathFinder::Reset()
{
	StartPoints.clear();
 	for ( unsigned int i = 0 ; i!= pMD1->GetHeight() ; ++i )
 		PFD1[i].reset(new pPFDataEntry[pMD1->GetWidth()]);
}
void PathFinder::ChangeStart(Scr::Position s)
{
	start=s;
	Reset();
	ComputePath();
}
void PathFinder::ChangeFinish(Scr::Position f)
{
	finish=f;
	ComputePath();
}

void PathFinder::ConditionalReset(Scr::Position s,
								  Scr::Position f)
{
	finish=f;
	if (start!=s)
	{
		start=s;
		Reset();
	}
	ComputePath();
}


// Copyright (c) 2010 Satoshi Nakamoto
// Copyright (c) 2009-2014 The Bitcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "amount.h"
#include "chainparams.h"
#include "core_io.h"
#include "init.h"
#include "net.h"
#include "main.h"
#include "miner.h"
#include "pow.h"
#include "rpcserver.h"
#include "util.h"
#ifdef ENABLE_WALLET
#include "db.h"
#include "wallet.h"
#endif

#include <stdint.h>

#include <boost/assign/list_of.hpp>

#include "json/json_spirit_utils.h"
#include "json/json_spirit_value.h"

using namespace json_spirit;
using namespace std;

/**
 * Return average network hashes per second based on the last 'lookup' blocks,
 * or from the last difficulty change if 'lookup' is nonpositive.
 * If 'height' is nonnegative, compute the estimate at the time when a given block was found.
 */
Value GetNetworkHashPS(int lookup, int height) {

    CBlockIndex *pb = chainActive.Tip();

    if (height >= 0 && height < chainActive.Height())
        pb = chainActive[height];

    if (pb == NULL || !pb->nHeight)
        return 0;

    //难度调整
    // If lookup is -1, then use blocks since last difficulty change.
    if (lookup <= 0)
        lookup = pb->nHeight % 2016 + 1; //每过2016个块,调整难度..

    // If lookup is larger than chain, then set it to chain length.
    if (lookup > pb->nHeight)
        lookup = pb->nHeight;

    CBlockIndex *pb0 = pb;
    int64_t minTime = pb0->GetBlockTime();
    int64_t maxTime = minTime;
    for (int i = 0; i < lookup; i++) {
        pb0 = pb0->pprev;
        int64_t time = pb0->GetBlockTime();
        minTime = std::min(time, minTime);
        maxTime = std::max(time, maxTime);
    }

    // In case there's a situation where minTime == maxTime, we don't want a divide by zero exception.
    if (minTime == maxTime)
        return 0;

    uint256 workDiff = pb->nChainWork - pb0->nChainWork;
    int64_t timeDiff = maxTime - minTime;

    return (int64_t)(workDiff.getdouble() / timeDiff);
}

#ifdef ENABLE_WALLET

#endif

// NOTE: Assumes a conclusive result; if result is inconclusive, it must be handled by caller
static Value BIP22ValidationResult(const CValidationState& state)
{
    if (state.IsValid())
        return Value::null;

    std::string strRejectReason = state.GetRejectReason();
    if (state.IsError())
        throw JSONRPCError(RPC_VERIFY_ERROR, strRejectReason);
    if (state.IsInvalid())
    {
        if (strRejectReason.empty())
            return "rejected";
        return strRejectReason;
    }
    // Should be impossible
    return "valid?";
}

class submitblock_StateCatcher : public CValidationInterface
{
public:
    uint256 hash;
    bool found;
    CValidationState state;

    submitblock_StateCatcher(const uint256 &hashIn) : hash(hashIn), found(false), state() {};

protected:
    virtual void BlockChecked(const CBlock& block, const CValidationState& stateIn) {
        if (block.GetHash() != hash)
            return;
        found = true;
        state = stateIn;
    };
};

Value estimatefee(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 1)
        throw runtime_error(
            "estimatefee nblocks\n"
            "\nEstimates the approximate fee per kilobyte\n"
            "needed for a transaction to begin confirmation\n"
            "within nblocks blocks.\n"
            "\nArguments:\n"
            "1. nblocks     (numeric)\n"
            "\nResult:\n"
            "n :    (numeric) estimated fee-per-kilobyte\n"
            "\n"
            "-1.0 is returned if not enough transactions and\n"
            "blocks have been observed to make an estimate.\n"
            "\nExample:\n"
            + HelpExampleCli("estimatefee", "6")
            );

    RPCTypeCheck(params, boost::assign::list_of(int_type));

    int nBlocks = params[0].get_int();
    if (nBlocks < 1)
        nBlocks = 1;

    CFeeRate feeRate = mempool.estimateFee(nBlocks);
    if (feeRate == CFeeRate(0))
        return -1.0;

    return ValueFromAmount(feeRate.GetFeePerK());
}

Value estimatepriority(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 1)
        throw runtime_error(
            "estimatepriority nblocks\n"
            "\nEstimates the approximate priority\n"
            "a zero-fee transaction needs to begin confirmation\n"
            "within nblocks blocks.\n"
            "\nArguments:\n"
            "1. nblocks     (numeric)\n"
            "\nResult:\n"
            "n :    (numeric) estimated priority\n"
            "\n"
            "-1.0 is returned if not enough transactions and\n"
            "blocks have been observed to make an estimate.\n"
            "\nExample:\n"
            + HelpExampleCli("estimatepriority", "6")
            );

    RPCTypeCheck(params, boost::assign::list_of(int_type));

    int nBlocks = params[0].get_int();
    if (nBlocks < 1)
        nBlocks = 1;

    return mempool.estimatePriority(nBlocks);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SPLIT_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SPLIT_MULTIPLIES_HPP_INCLUDED

namespace boost { namespace simd
{
#if defined(DOXYGEN_ONLY)
 /*!
    @ingroup group-swar
    Function object implementing split_multiplies capabilities

    SIMD register type-based multiplies and split

    @c split_multiplies multiplies two x-bit SIMD registers and returns two 2x-bit registers
    each having half the cardinal of the original inputs.
  **/
  const boost::dispatch::functor<tag::split_multiplies_> split_multiplies = {};
#endif
} }

#include <boost/simd/function/simd/split_multiplies.hpp>

#endif


/*
 * Copyright (c) 2013, Quarkslab
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 * - Neither the name of Quarkslab nor the names of its contributors may be used
 * to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>

#include <boost/random.hpp>

#include <leeloo/ip_list_intervals.h>
#include <leeloo/random.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

int main(int argc, char** argv)
{
	if (argc < 2) {
		std::cerr << "Usage: " << argv[0] << " range1 [-][range2] [-][range3] ..." << std::endl << std::endl;
		std::cerr << "This tool will aggregate all the given ranges and will write a random order of the corresponding IP." << std::endl << std::endl;
		std::cerr << "where range can be described as:" << std::endl;
		std::cerr << "\tCIDR notation:\t192.168.1.0/24" << std::endl;
		std::cerr << "\tRanges:\t\t10.4-5.8.9-250" << std::endl;
		std::cerr << "\tSingle IP:\t192.168.1.10" << std::endl << std::endl;
		std::cerr << "A '-' symbol before any range will remove it from the final set." << std::endl;
		return 1;
	}

	leeloo::ip_list_intervals l;
	for (int i = 1; i < argc; i++) {
		const char* const ip_range = argv[i];
		bool ret;
		if (strlen(ip_range) <= 1) {
			ret = false;
		}
		else
		if (ip_range[0] == '-') {
			ret = l.remove(&ip_range[1]);
		}
		else {
			ret = l.add(ip_range);
		}

		if (!ret) {
			std::cerr << "Warning: '" << ip_range << "' is an invalid IP range." << std::endl;
		}
	}

	// Aggregate all IPs
	l.aggregate();

	std::cerr << "Number of IPs after aggregation: " << l.size() << std::endl;

	l.create_index_cache(256);

	// Initialize a random generator
	boost::random::mt19937 mt_rand(time(NULL));

	l.random_sets(16,
		[](const uint32_t* ips, const size_t n)
		{
			for (size_t i = 0; i < n; i++) {
				struct in_addr addr;
				addr.s_addr = htonl(ips[i]);
				std::cout << inet_ntoa(addr) << std::endl;
			}
		},
		leeloo::random_engine<uint32_t>(mt_rand));

	return 0;
}