//  Copyright (c) 2007, 2013 John Maddock
//  Copyright Christopher Kormanyos 2013.
//  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)
//
// This header just defines the function entry points, and adds dispatch
// to the right implementation method.  Most of the implementation details
// are in separate headers and copyright Xiaogang Zhang.
//
#ifndef BOOST_MATH_BESSEL_HPP
#define BOOST_MATH_BESSEL_HPP

#ifdef _MSC_VER
#  pragma once
#endif

#include <boost/math/special_functions/detail/bessel_jy.hpp>
#include <boost/math/special_functions/detail/bessel_jn.hpp>
#include <boost/math/special_functions/detail/bessel_yn.hpp>
#include <boost/math/special_functions/detail/bessel_jy_zero.hpp>
#include <boost/math/special_functions/detail/bessel_ik.hpp>
#include <boost/math/special_functions/detail/bessel_i0.hpp>
#include <boost/math/special_functions/detail/bessel_i1.hpp>
#include <boost/math/special_functions/detail/bessel_kn.hpp>
#include <boost/math/special_functions/detail/iconv.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/math/tools/series.hpp>
#include <boost/math/tools/roots.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
struct sph_bessel_j_small_z_series_term
{
   typedef T result_type;

   sph_bessel_j_small_z_series_term(unsigned v_, T x)
      : N(0), v(v_)
   {
      BOOST_MATH_STD_USING
      mult = x / 2;
      term = pow(mult, T(v)) / boost::math::tgamma(v+1+T(0.5f), Policy());
      mult *= -mult;
   }
   T operator()()
   {
      T r = term;
      ++N;
      term *= mult / (N * T(N + v + 0.5f));
      return r;
   }
private:
   unsigned N;
   unsigned v;
   T mult;
   T term;
};

template <class T, class Policy>
inline T sph_bessel_j_small_z_series(unsigned v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   sph_bessel_j_small_z_series_term<T, Policy> s(v, x);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   T zero = 0;
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, zero);
#else
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
#endif
   policies::check_series_iterations<T>("boost::math::sph_bessel_j_small_z_series<%1%>(%1%,%1%)", max_iter, pol);
   return result * sqrt(constants::pi<T>() / 4);
}

template <class T, class Policy>
T cyl_bessel_j_imp(T v, T x, const bessel_no_int_tag& t, const Policy& pol)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::bessel_j<%1%>(%1%,%1%)";
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_j_imp(v, T(-x), t, pol);
         if(iround(v, pol) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
            function,
            "Got x = %1%, but we need x >= 0", x, pol);
   }

   T j, y;
   bessel_jy(v, x, &j, &y, need_j, pol);
   return j;
}

template <class T, class Policy>
inline T cyl_bessel_j_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std names.
   int ival = detail::iconv(v, pol);
   // If v is an integer, use the integer recursion
   // method, both that and Steeds method are O(v):
   if((0 == v - ival))
   {
      return bessel_jn(ival, x, pol);
   }
   return cyl_bessel_j_imp(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_bessel_j_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   return bessel_jn(v, x, pol);
}

template <class T, class Policy>
inline T sph_bessel_j_imp(unsigned n, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   if(x < 0)
      return policies::raise_domain_error<T>(
         "boost::math::sph_bessel_j<%1%>(%1%,%1%)",
         "Got x = %1%, but function requires x > 0.", x, pol);
   //
   // Special case, n == 0 resolves down to the sinus cardinal of x:
   //
   if(n == 0)
      return boost::math::sinc_pi(x, pol);
   //
   // When x is small we may end up with 0/0, use series evaluation
   // instead, especially as it converges rapidly:
   //
   if(x < 1)
      return sph_bessel_j_small_z_series(n, x, pol);
   //
   // Default case is just a naive evaluation of the definition:
   //
   return sqrt(constants::pi<T>() / (2 * x))
      * cyl_bessel_j_imp(T(T(n)+T(0.5f)), x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
T cyl_bessel_i_imp(T v, T x, const Policy& pol)
{
   //
   // This handles all the bessel I functions, note that we don't optimise
   // for integer v, other than the v = 0 or 1 special cases, as Millers
   // algorithm is at least as inefficient as the general case (the general
   // case has better error handling too).
   //
   BOOST_MATH_STD_USING
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_i_imp(v, T(-x), pol);
         if(iround(v, pol) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
         "boost::math::cyl_bessel_i<%1%>(%1%,%1%)",
            "Got x = %1%, but we need x >= 0", x, pol);
   }
   if(x == 0)
   {
      return (v == 0) ? 1 : 0;
   }
   if(v == 0.5f)
   {
      // common special case, note try and avoid overflow in exp(x):
      if(x >= tools::log_max_value<T>())
      {
         T e = exp(x / 2);
         return e * (e / sqrt(2 * x * constants::pi<T>()));
      }
      return sqrt(2 / (x * constants::pi<T>())) * sinh(x);
   }
   if(policies::digits<T, Policy>() <= 64)
   {
      if(v == 0)
      {
         return bessel_i0(x);
      }
      if(v == 1)
      {
         return bessel_i1(x);
      }
   }
   if((v > 0) && (x / v < 0.25))
      return bessel_i_small_z_series(v, x, pol);
   T I, K;
   bessel_ik(v, x, &I, &K, need_i, pol);
   return I;
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(T v, T x, const bessel_no_int_tag& /* t */, const Policy& pol)
{
   static const char* function = "boost::math::cyl_bessel_k<%1%>(%1%,%1%)";
   BOOST_MATH_STD_USING
   if(x < 0)
   {
      return policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but we need x > 0", x, pol);
   }
   if(x == 0)
   {
      return (v == 0) ? policies::raise_overflow_error<T>(function, 0, pol)
         : policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but we need x > 0", x, pol);
   }
   T I, K;
   bessel_ik(v, x, &I, &K, need_k, pol);
   return K;
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   if((floor(v) == v))
   {
      return bessel_kn(itrunc(v), x, pol);
   }
   return cyl_bessel_k_imp(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   return bessel_kn(v, x, pol);
}

template <class T, class Policy>
inline T cyl_neumann_imp(T v, T x, const bessel_no_int_tag&, const Policy& pol)
{
   static const char* function = "boost::math::cyl_neumann<%1%>(%1%,%1%)";

   BOOST_MATH_INSTRUMENT_VARIABLE(v);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   if(x <= 0)
   {
      return (v == 0) && (x == 0) ?
         policies::raise_overflow_error<T>(function, 0, pol)
         : policies::raise_domain_error<T>(
               function,
               "Got x = %1%, but result is complex for x <= 0", x, pol);
   }
   T j, y;
   bessel_jy(v, x, &j, &y, need_y, pol);
   //
   // Post evaluation check for internal overflow during evaluation,
   // can occur when x is small and v is large, in which case the result
   // is -INF:
   //
   if(!(boost::math::isfinite)(y))
      return -policies::raise_overflow_error<T>(function, 0, pol);
   return y;
}

template <class T, class Policy>
inline T cyl_neumann_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING

   BOOST_MATH_INSTRUMENT_VARIABLE(v);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   if(floor(v) == v)
   {
      if(asymptotic_bessel_large_x_limit(v, x))
      {
         T r = asymptotic_bessel_y_large_x_2(static_cast<T>(abs(v)), x);
         if((v < 0) && (itrunc(v, pol) & 1))
            r = -r;
         BOOST_MATH_INSTRUMENT_VARIABLE(r);
         return r;
      }
      else
      {
         T r = bessel_yn(itrunc(v, pol), x, pol);
         BOOST_MATH_INSTRUMENT_VARIABLE(r);
         return r;
      }
   }
   T r = cyl_neumann_imp<T>(v, x, bessel_no_int_tag(), pol);
   BOOST_MATH_INSTRUMENT_VARIABLE(r);
   return r;
}

template <class T, class Policy>
inline T cyl_neumann_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING

   BOOST_MATH_INSTRUMENT_VARIABLE(v);
   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   if(asymptotic_bessel_large_x_limit(T(v), x))
   {
      T r = asymptotic_bessel_y_large_x_2(static_cast<T>(abs(v)), x);
      if((v < 0) && (v & 1))
         r = -r;
      return r;
   }
   else
      return bessel_yn(v, x, pol);
}

template <class T, class Policy>
inline T sph_neumann_imp(unsigned v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   static const char* function = "boost::math::sph_neumann<%1%>(%1%,%1%)";
   //
   // Nothing much to do here but check for errors, and
   // evaluate the function's definition directly:
   //
   if(x < 0)
      return policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function requires x > 0.", x, pol);

   if(x < 2 * tools::min_value<T>())
      return -policies::raise_overflow_error<T>(function, 0, pol);

   T result = cyl_neumann_imp(T(T(v)+0.5f), x, bessel_no_int_tag(), pol);
   T tx = sqrt(constants::pi<T>() / (2 * x));

   if((tx > 1) && (tools::max_value<T>() / tx < result))
      return -policies::raise_overflow_error<T>(function, 0, pol);

   return result * tx;
}

template <class T, class Policy>
inline T cyl_bessel_j_zero_imp(T v, int m, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names, needed for floor.

   static const char* function = "boost::math::cyl_bessel_j_zero<%1%>(%1%, int)";

   const T half_epsilon(boost::math::tools::epsilon<T>() / 2U);

   // Handle non-finite order.
   if (!(boost::math::isfinite)(v) )
   {
     return policies::raise_domain_error<T>(function, "Order argument is %1%, but must be finite >= 0 !", v, pol);
   }

   // Handle negative rank.
   if(m < 0)
   {
      // Zeros of Jv(x) with negative rank are not defined and requesting one raises a domain error.
      return policies::raise_domain_error<T>(function, "Requested the %1%'th zero, but the rank must be positive !", m, pol);
   }

   // Get the absolute value of the order.
   const bool order_is_negative = (v < 0);
   const T vv((!order_is_negative) ? v : T(-v));

   // Check if the order is very close to zero or very close to an integer.
   const bool order_is_zero    = (vv < half_epsilon);
   const bool order_is_integer = ((vv - floor(vv)) < half_epsilon);

   if(m == 0)
   {
      if(order_is_zero)
      {
         // The zero'th zero of J0(x) is not defined and requesting it raises a domain error.
         return policies::raise_domain_error<T>(function, "Requested the %1%'th zero of J0, but the rank must be > 0 !", m, pol);
      }

      // The zero'th zero of Jv(x) for v < 0 is not defined
      // unless the order is a negative integer.
      if(order_is_negative && (!order_is_integer))
      {
         // For non-integer, negative order, requesting the zero'th zero raises a domain error.
         return policies::raise_domain_error<T>(function, "Requested the %1%'th zero of Jv for negative, non-integer order, but the rank must be > 0 !", m, pol);
      }

      // The zero'th zero does exist and its value is zero.
      return T(0);
   }

   // Set up the initial guess for the upcoming root-finding.
   // If the order is a negative integer, then use the corresponding
   // positive integer for the order.
   const T guess_root = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess<T, Policy>((order_is_integer ? vv : v), m, pol);

   // Select the maximum allowed iterations from the policy.
   boost::uintmax_t number_of_iterations = policies::get_max_root_iterations<Policy>();

   // Select the desired number of binary digits of precision.
   // Account for the radix of number representations having non-two radix!
   const int my_digits2 = policies::digits<T, Policy>();

   const T delta_lo = ((guess_root > 0.2F) ? T(0.2) : T(guess_root / 2U));

   // Perform the root-finding using Newton-Raphson iteration from Boost.Math.
   const T jvm =
      boost::math::tools::newton_raphson_iterate(
         boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::function_object_jv_and_jv_prime<T, Policy>((order_is_integer ? vv : v), order_is_zero, pol),
         guess_root,
         T(guess_root - delta_lo),
         T(guess_root + 0.2F),
         my_digits2,
         number_of_iterations);

   if(number_of_iterations >= policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<T>(function, "Unable to locate root in a reasonable time:"
         "  Current best guess is %1%", jvm, Policy());
   }

   return jvm;
}

template <class T, class Policy>
inline T cyl_neumann_zero_imp(T v, int m, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names, needed for floor.

   static const char* function = "boost::math::cyl_neumann_zero<%1%>(%1%, int)";

   // Handle non-finite order.
   if (!(boost::math::isfinite)(v) )
   {
     return policies::raise_domain_error<T>(function, "Order argument is %1%, but must be finite >= 0 !", v, pol);
   }

   // Handle negative rank.
   if(m < 0)
   {
      return policies::raise_domain_error<T>(function, "Requested the %1%'th zero, but the rank must be positive !", m, pol);
   }

   const T half_epsilon(boost::math::tools::epsilon<T>() / 2U);

   // Get the absolute value of the order.
   const bool order_is_negative = (v < 0);
   const T vv((!order_is_negative) ? v : T(-v));

   const bool order_is_integer = ((vv - floor(vv)) < half_epsilon);

   // For negative integers, use reflection to positive integer order.
   if(order_is_negative && order_is_integer)
      return boost::math::detail::cyl_neumann_zero_imp(vv, m, pol);

   // Check if the order is very close to a negative half-integer.
   const T delta_half_integer(vv - (floor(vv) + 0.5F));

   const bool order_is_negative_half_integer =
      (order_is_negative && ((delta_half_integer > -half_epsilon) && (delta_half_integer < +half_epsilon)));

   // The zero'th zero of Yv(x) for v < 0 is not defined
   // unless the order is a negative integer.
   if((m == 0) && (!order_is_negative_half_integer))
   {
      // For non-integer, negative order, requesting the zero'th zero raises a domain error.
      return policies::raise_domain_error<T>(function, "Requested the %1%'th zero of Yv for negative, non-half-integer order, but the rank must be > 0 !", m, pol);
   }

   // For negative half-integers, use the corresponding
   // spherical Bessel function of positive half-integer order.
   if(order_is_negative_half_integer)
      return boost::math::detail::cyl_bessel_j_zero_imp(vv, m, pol);

   // Set up the initial guess for the upcoming root-finding.
   // If the order is a negative integer, then use the corresponding
   // positive integer for the order.
   const T guess_root = boost::math::detail::bessel_zero::cyl_neumann_zero_detail::initial_guess<T, Policy>(v, m, pol);

   // Select the maximum allowed iterations from the policy.
   boost::uintmax_t number_of_iterations = policies::get_max_root_iterations<Policy>();

   // Select the desired number of binary digits of precision.
   // Account for the radix of number representations having non-two radix!
   const int my_digits2 = policies::digits<T, Policy>();

   const T delta_lo = ((guess_root > 0.2F) ? T(0.2) : T(guess_root / 2U));

   // Perform the root-finding using Newton-Raphson iteration from Boost.Math.
   const T yvm =
      boost::math::tools::newton_raphson_iterate(
         boost::math::detail::bessel_zero::cyl_neumann_zero_detail::function_object_yv_and_yv_prime<T, Policy>(v, pol),
         guess_root,
         T(guess_root - delta_lo),
         T(guess_root + 0.2F),
         my_digits2,
         number_of_iterations);

   if(number_of_iterations >= policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<T>(function, "Unable to locate root in a reasonable time:"
         "  Current best guess is %1%", yvm, Policy());
   }

   return yvm;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_j(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_j_imp<value_type>(v, static_cast<value_type>(x), tag_type(), forwarding_policy()), "boost::math::cyl_bessel_j<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_j(T1 v, T2 x)
{
   return cyl_bessel_j(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_bessel(unsigned v, T x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_bessel_j_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::sph_bessel<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_bessel(unsigned v, T x)
{
   return sph_bessel(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_i(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_i_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::cyl_bessel_i<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_i(T1 v, T2 x)
{
   return cyl_bessel_i(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_k(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_k_imp<value_type>(v, static_cast<value_type>(x), tag_type(), forwarding_policy()), "boost::math::cyl_bessel_k<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_k(T1 v, T2 x)
{
   return cyl_bessel_k(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_neumann(T1 v, T2 x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_neumann_imp<value_type>(v, static_cast<value_type>(x), tag_type(), forwarding_policy()), "boost::math::cyl_neumann<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_neumann(T1 v, T2 x)
{
   return cyl_neumann(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_neumann(unsigned v, T x, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_neumann_imp<value_type>(v, static_cast<value_type>(x), forwarding_policy()), "boost::math::sph_neumann<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_neumann(unsigned v, T x)
{
   return sph_neumann(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type cyl_bessel_j_zero(T v, int m, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<value_type>::is_integer, "Order must be a floating-point type.");
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_j_zero_imp<value_type>(v, m, forwarding_policy()), "boost::math::cyl_bessel_j_zero<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type cyl_bessel_j_zero(T v, int m)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   return cyl_bessel_j_zero<T, policies::policy<> >(v, m, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator cyl_bessel_j_zero(T v,
                              int start_index,
                              unsigned number_of_zeros,
                              OutputIterator out_it,
                              const Policy& pol)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   for(unsigned i = 0; i < number_of_zeros; ++i)
   {
      *out_it = boost::math::cyl_bessel_j_zero(v, start_index + i, pol);
      ++out_it;
   }
   return out_it;
}

template <class T, class OutputIterator>
inline OutputIterator cyl_bessel_j_zero(T v,
                              int start_index,
                              unsigned number_of_zeros,
                              OutputIterator out_it)
{
   return cyl_bessel_j_zero(v, start_index, number_of_zeros, out_it, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type cyl_neumann_zero(T v, int m, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<value_type>::is_integer, "Order must be a floating-point type.");
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_neumann_zero_imp<value_type>(v, m, forwarding_policy()), "boost::math::cyl_neumann_zero<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type cyl_neumann_zero(T v, int m)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   return cyl_neumann_zero<T, policies::policy<> >(v, m, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator cyl_neumann_zero(T v,
                             int start_index,
                             unsigned number_of_zeros,
                             OutputIterator out_it,
                             const Policy& pol)
{
   BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<T>::is_integer, "Order must be a floating-point type.");
   for(unsigned i = 0; i < number_of_zeros; ++i)
   {
      *out_it = boost::math::cyl_neumann_zero(v, start_index + i, pol);
      ++out_it;
   }
   return out_it;
}

template <class T, class OutputIterator>
inline OutputIterator cyl_neumann_zero(T v,
                             int start_index,
                             unsigned number_of_zeros,
                             OutputIterator out_it)
{
   return cyl_neumann_zero(v, start_index, number_of_zeros, out_it, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_BESSEL_HPP


#ifndef STAN_MATH_PRIM_PROB_BETA_PROPORTION_RNG_HPP
#define STAN_MATH_PRIM_PROB_BETA_PROPORTION_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <stan/math/prim/fun/scalar_seq_view.hpp>
#include <stan/math/prim/fun/to_ref.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a Beta random variate specified probability, location, and
 * precision parameters: beta_proportion_rng(y | mu, kappa) =
 * beta_rng(y | mu * kappa, (1 - mu) * kappa).  Any arguments other
 * than scalars must be containers of the same size.  With non-scalar
 * arguments, the return is a container matching the size of the
 * arguments with scalars broadcast as necessary.
 *
 * @tparam T_loc Type of location parameter
 * @tparam T_prec Type of precision parameter
 * @tparam RNG type of random number generator
 * @param mu (Sequence of) location parameter(s) in (0, 1)
 * @param kappa (Sequence of) positive finite precision parameter(s)
 * @param rng random number generator
 * @return (Sequence of) beta random variate(s)
 * @throw std::domain_error if mu is outside of (0, 1)
 * @throw std::domain_error if kappa is nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_prec, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_prec>::type
beta_proportion_rng(const T_loc &mu, const T_prec &kappa, RNG &rng) {
  using T_mu_ref = ref_type_t<T_loc>;
  using T_kappa_ref = ref_type_t<T_prec>;
  static const char *function = "beta_proportion_rng";
  check_consistent_sizes(function, "Location parameter", mu,
                         "Precision parameter", kappa);
  T_mu_ref mu_ref = mu;
  T_kappa_ref kappa_ref = kappa;
  check_positive(function, "Location parameter", mu_ref);
  check_less(function, "Location parameter", mu_ref, 1.0);
  check_positive_finite(function, "Precision parameter", kappa_ref);

  scalar_seq_view<T_mu_ref> mu_vec(mu_ref);
  scalar_seq_view<T_kappa_ref> kappa_vec(kappa_ref);
  size_t N = max_size(mu, kappa);
  VectorBuilder<true, double, T_loc, T_prec> output(N);

  for (size_t n = 0; n < N; ++n) {
    double alpha = mu_vec[n] * kappa_vec[n];
    double beta = kappa_vec[n] - alpha;
    output[n] = beta_rng(alpha, beta, rng);
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


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

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<CPose3D>;

class Pose3DTests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	void test_inverse(const CPose3D& p1)
	{
		const auto HM = p1.getHomogeneousMatrixVal<CMatrixDouble44>();
		CMatrixDouble44 HMi;
		p1.getInverseHomogeneousMatrix(HMi);

		auto I4 = CMatrixDouble44::Identity();

		EXPECT_NEAR(
			(HM.asEigen() * HMi.asEigen() - I4.asEigen()).array().abs().sum(),
			0, 1e-3)
			<< "HM:\n"
			<< HM << "inv(HM):\n"
			<< HMi << "inv(HM)*HM:\n"
			<< (HM * HMi) << endl;

		CPose3D p1_inv_inv = p1;

		p1_inv_inv.inverse();
		const auto HMi_from_p1_inv =
			p1_inv_inv.getHomogeneousMatrixVal<CMatrixDouble44>();

		p1_inv_inv.inverse();

		EXPECT_NEAR(
			(p1.asVectorVal() - p1_inv_inv.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			0, 1e-3)
			<< "p1: " << p1 << "p1_inv_inv: " << p1_inv_inv << endl;

		EXPECT_NEAR((HMi_from_p1_inv - HMi).sum_abs(), 0, 1e-4)
			<< "HMi_from_p1_inv:\n"
			<< HMi_from_p1_inv << "HMi:\n"
			<< HMi << endl;
	}

	void test_compose(const CPose3D& p1, const CPose3D& p2)
	{
		const CPose3D p1_c_p2 = p1 + p2;
		const CPose3D p1_i_p2 = p1 - p2;

		const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1;  // should be -> p2
		const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2;  // Should be -> p1

		EXPECT_NEAR(
			0,
			(p1_c_p2_i_p2.asVectorVal() - p2.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p2          : " << p2 << endl
			<< "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;

		EXPECT_NEAR(
			0,
			(p2_c_p1_i_p2.asVectorVal() - p1.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1          : " << p1 << endl
			<< "p2          : " << p2 << endl
			<< "p2 matrix   : " << endl
			<< p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "p1_i_p2     : " << p1_i_p2 << endl
			<< "p1_i_p2 matrix: " << endl
			<< p1_i_p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;

		// Test + operator: trg new var
		{
			CPose3D C = p1;
			CPose3D A = C + p2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - p1_c_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-6);
		}
		// Test + operator: trg same var
		{
			CPose3D A = p1;
			A = A + p2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - p1_c_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-6);
		}
		// Test =+ operator
		{
			CPose3D A = p1;
			A += p2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - p1_c_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-6);
		}
	}

	void test_to_from_2d(double x, double y, double phi)
	{
		const CPose2D p2d = CPose2D(x, y, phi);
		const CPose3D p3d = CPose3D(p2d);

		const CPose2D p2d_bis = CPose2D(p3d);

		EXPECT_FLOAT_EQ(p2d.x(), p2d_bis.x()) << "p2d: " << p2d << endl;
		EXPECT_FLOAT_EQ(p2d.y(), p2d_bis.y()) << "p2d: " << p2d << endl;
		EXPECT_FLOAT_EQ(p2d.phi(), p2d_bis.phi()) << "p2d: " << p2d << endl;

		EXPECT_FLOAT_EQ(p2d.phi(), p3d.yaw()) << "p2d: " << p2d << endl;
	}

	void test_composeFrom(const CPose3D& p1, const CPose3D& p2)
	{
		const CPose3D p1_plus_p2 = p1 + p2;

		{
			CPose3D p1_plus_p2bis;
			p1_plus_p2bis.composeFrom(p1, p2);

			EXPECT_NEAR(
				0,
				(p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-5)
				<< "p2 : " << p2 << endl
				<< "p1 : " << p1 << endl
				<< "p1_plus_p2    : " << p1_plus_p2 << endl
				<< "p1_plus_p2bis : " << p1_plus_p2bis << endl;
		}

		{
			CPose3D p1_plus_p2bis = p1;
			p1_plus_p2bis.composeFrom(p1_plus_p2bis, p2);

			EXPECT_NEAR(
				0,
				(p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-5)
				<< "p2 : " << p2 << endl
				<< "p1 : " << p1 << endl
				<< "p1_plus_p2    : " << p1_plus_p2 << endl
				<< "p1_plus_p2bis : " << p1_plus_p2bis << endl;
		}

		{
			CPose3D p1_plus_p2bis = p2;
			p1_plus_p2bis.composeFrom(p1, p1_plus_p2bis);

			EXPECT_NEAR(
				0,
				(p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-5)
				<< "p2 : " << p2 << endl
				<< "p1 : " << p1 << endl
				<< "p1_plus_p2    : " << p1_plus_p2 << endl
				<< "p1_plus_p2bis : " << p1_plus_p2bis << endl;
		}
	}

	void test_composePoint(const CPose3D& p1, double x, double y, double z)
	{
		const CPoint3D p(x, y, z);
		CPoint3D p1_plus_p = p1 + p;

		CPoint3D p1_plus_p2;
		p1.composePoint(
			p.x(), p.y(), p.z(), p1_plus_p2.x(), p1_plus_p2.y(),
			p1_plus_p2.z());

		EXPECT_NEAR(
			0,
			(p1_plus_p2.asVectorVal() - p1_plus_p.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5);

		// Repeat using same input/output variables:
		{
			double lx = p.x();
			double ly = p.y();
			double lz = p.z();

			p1.composePoint(lx, ly, lz, lx, ly, lz);
			EXPECT_NEAR(
				0,
				std::abs(lx - p1_plus_p.x()) + std::abs(ly - p1_plus_p.y()) +
					std::abs(lz - p1_plus_p.z()),
				1e-5);
		}

		// Inverse:
		CPoint3D p_recov = p1_plus_p - p1;
		CPoint3D p_recov2;
		p1.inverseComposePoint(
			p1_plus_p.x(), p1_plus_p.y(), p1_plus_p.z(), p_recov2.x(),
			p_recov2.y(), p_recov2.z());

		EXPECT_NEAR(
			0,
			(p_recov2.asVectorVal() - p_recov.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5);

		EXPECT_NEAR(
			0,
			(p.asVectorVal() - p_recov.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5);
	}

	static void func_compose_point(
		const CVectorFixedDouble<6 + 3>& x, const double& dummy,
		CVectorFixedDouble<3>& Y)
	{
		MRPT_UNUSED_PARAM(dummy);
		CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
		const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
		const CPoint3D pp = q + p;
		for (int i = 0; i < 3; i++) Y[i] = pp[i];
	}

	static void func_inv_compose_point(
		const CVectorFixedDouble<6 + 3>& x, const double& dummy,
		CVectorFixedDouble<3>& Y)
	{
		MRPT_UNUSED_PARAM(dummy);
		CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
		const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
		const CPoint3D pp = p - q;
		Y[0] = pp.x();
		Y[1] = pp.y();
		Y[2] = pp.z();
	}

	void test_composePointJacob(
		const CPose3D& p1, double x, double y, double z, bool use_aprox = false)
	{
		const CPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint;
		CMatrixFixed<double, 3, 6> df_dpose;

		TPoint3D pp;
		p1.composePoint(
			x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose, std::nullopt,
			use_aprox);

		// Numerical approx:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6 + 3> x_mean;
			for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
			x_mean[6 + 0] = x;
			x_mean[6 + 1] = y;
			x_mean[6 + 2] = z;

			double DUMMY = 0;
			CVectorFixedDouble<6 + 3> x_incrs;
			x_incrs.fill(1e-7);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6 + 3>& x, const double& dummy,
					CVectorFixedDouble<3>& Y)>(&func_compose_point),
				x_incrs, DUMMY, numJacobs);

			num_df_dpose = numJacobs.block<3, 6>(0, 0);
			num_df_dpoint = numJacobs.block<3, 3>(0, 6);
		}

		const double max_error = use_aprox ? 0.1 : 3e-3;

		EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).sum_abs(), max_error)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint << endl
			<< "Implemented method: " << endl
			<< df_dpoint << endl
			<< "Error: " << endl
			<< df_dpoint - num_df_dpoint << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			max_error)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	void test_ExpLnEqual(const CPose3D& p1)
	{
		const CPose3D p2 = Lie::SE<3>::exp(Lie::SE<3>::log(p1));
		EXPECT_NEAR((p1.asVectorVal() - p2.asVectorVal()).sum_abs(), 0, 1e-5)
			<< "p1: " << p1 << endl;
	}

	void test_invComposePointJacob(
		const CPose3D& p1, double x, double y, double z)
	{
		const CPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint;
		CMatrixFixed<double, 3, 6> df_dpose;

		TPoint3D pp;
		p1.inverseComposePoint(x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose);

		// Numerical approx:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6 + 3> x_mean;
			for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
			x_mean[6 + 0] = x;
			x_mean[6 + 1] = y;
			x_mean[6 + 2] = z;

			double DUMMY = 0;
			CVectorFixedDouble<6 + 3> x_incrs;
			x_incrs.fill(1e-7);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6 + 3>& x, const double& dummy,
					CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
				x_incrs, DUMMY, numJacobs);

			num_df_dpose = numJacobs.block<3, 6>(0, 0);
			num_df_dpoint = numJacobs.block<3, 3>(0, 6);
		}

		EXPECT_NEAR(
			0, (df_dpoint - num_df_dpoint).asEigen().array().abs().sum(), 3e-3)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint << endl
			<< "Implemented method: " << endl
			<< df_dpoint << endl
			<< "Error: " << endl
			<< df_dpoint - num_df_dpoint << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			3e-3)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	void test_default_values(const CPose3D& p, const std::string& label)
	{
		EXPECT_EQ(p.x(), 0);
		EXPECT_EQ(p.y(), 0);
		EXPECT_EQ(p.z(), 0);
		EXPECT_EQ(p.yaw(), 0);
		EXPECT_EQ(p.pitch(), 0);
		EXPECT_EQ(p.roll(), 0);
		for (size_t i = 0; i < 4; i++)
			for (size_t j = 0; j < 4; j++)
				EXPECT_NEAR(
					p.getHomogeneousMatrixVal<CMatrixDouble44>()(i, j),
					i == j ? 1.0 : 0.0, 1e-8)
					<< "Failed for (i,j)=" << i << "," << j << endl
					<< "Matrix is: " << endl
					<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
					<< "case was: " << label << endl;
	}

	static void func_compose_point_se3(
		const CVectorFixedDouble<6>& x, const CVectorFixedDouble<3>& P,
		CVectorFixedDouble<3>& Y)
	{
		CPose3D q = Lie::SE<3>::exp(x);
		const CPoint3D p(P[0], P[1], P[2]);
		const CPoint3D pp = q + p;
		for (int i = 0; i < 3; i++) Y[i] = pp[i];
	}

	static void func_invcompose_point_se3(
		const CVectorFixedDouble<6>& x, const CVectorFixedDouble<3>& P,
		CVectorFixedDouble<3>& Y)
	{
		CPose3D q = Lie::SE<3>::exp(x);
		const CPoint3D p(P[0], P[1], P[2]);
		const CPoint3D pp = p - q;
		for (int i = 0; i < 3; i++) Y[i] = pp[i];
	}

	void test_composePointJacob_se3(const CPose3D& p, const TPoint3D x_l)
	{
		CMatrixFixed<double, 3, 6> df_dse3;

		TPoint3D pp;
		p.composePoint(
			x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
			df_dse3);

		// Numerical approx:
		CMatrixFixed<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6> x_mean;
			for (int i = 0; i < 6; i++) x_mean[i] = 0;

			CVectorFixedDouble<3> P;
			for (int i = 0; i < 3; i++) P[i] = pp[i];

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-9);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& x,
					const CVectorFixedDouble<3>& P, CVectorFixedDouble<3>& Y)>(
					&func_compose_point_se3),
				x_incrs, P, num_df_dse3);
		}

		EXPECT_NEAR(
			0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
			3e-3)
			<< "p: " << p << endl
			<< "x_l:  " << x_l << endl
			<< "Numeric approximation of df_dse3: " << endl
			<< num_df_dse3.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dse3.asEigen() << endl
			<< "Error: " << endl
			<< df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
	}

	void test_invComposePointJacob_se3(const CPose3D& p, const TPoint3D x_g)
	{
		CMatrixFixed<double, 3, 6> df_dse3;

		TPoint3D pp;
		p.inverseComposePoint(
			x_g.x, x_g.y, x_g.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
			df_dse3);

		// Numerical approx:
		CMatrixFixed<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6> x_mean;
			for (int i = 0; i < 6; i++) x_mean[i] = 0;

			CVectorFixedDouble<3> P;
			for (int i = 0; i < 3; i++) P[i] = pp[i];

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-9);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& x,
					const CVectorFixedDouble<3>& P, CVectorFixedDouble<3>& Y)>(
					&func_invcompose_point_se3),
				x_incrs, P, num_df_dse3);
		}

		EXPECT_NEAR(
			0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
			3e-3)
			<< "p: " << p << endl
			<< "x_g:  " << x_g << endl
			<< "Numeric approximation of df_dse3: " << endl
			<< num_df_dse3.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dse3.asEigen() << endl
			<< "Error: " << endl
			<< df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
	}

	static void func_jacob_expe_e(
		const CVectorFixedDouble<6>& x, const double& dummy,
		CVectorFixedDouble<12>& Y)
	{
		MRPT_UNUSED_PARAM(dummy);
		const CPose3D p = Lie::SE<3>::exp(x);
		// const CMatrixDouble44 R =
		// p.getHomogeneousMatrixVal<CMatrixDouble44>();
		p.getAs12Vector(Y);
	}

	// Check dexp(e)_de
	void check_jacob_expe_e_at_0()
	{
		CVectorFixedDouble<6> x_mean;
		for (int i = 0; i < 6; i++) x_mean[i] = 0;

		const double& dummy = 0.;
		CVectorFixedDouble<6> x_incrs;
		x_incrs.fill(1e-9);
		CMatrixDouble numJacobs;
		mrpt::math::estimateJacobian(
			x_mean,
			std::function<void(
				const CVectorFixedDouble<6>& x, const double& dummy,
				CVectorFixedDouble<12>& Y)>(&func_jacob_expe_e),
			x_incrs, dummy, numJacobs);

		// Theoretical matrix:
		// [ 0   -[e1]_x ]
		// [ 0   -[e2]_x ]
		// [ 0   -[e3]_x ]
		// [ I_3    0    ]
		double vals[12 * 6] = {
			0, 0, 0, 0, 0, 0,  0, 0, 0, 0,  0, 1, 0, 0, 0, 0, -1, 0,

			0, 0, 0, 0, 0, -1, 0, 0, 0, 0,  0, 0, 0, 0, 0, 1, 0,  0,

			0, 0, 0, 0, 1, 0,  0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0,  0,

			1, 0, 0, 0, 0, 0,  0, 1, 0, 0,  0, 0, 0, 0, 1, 0, 0,  0};
		CMatrixFixed<double, 12, 6> M(vals);

		EXPECT_NEAR(
			(numJacobs.asEigen() - M.asEigen()).array().abs().maxCoeff(), 0,
			1e-5)
			<< "M:\n"
			<< M.asEigen() << "numJacobs:\n"
			<< numJacobs << "\n";
	}

	static void func_jacob_LnT_T(
		const CVectorFixedDouble<12>& x, const double& dummy,
		CVectorFixedDouble<6>& Y)
	{
		MRPT_UNUSED_PARAM(dummy);
		CPose3D p;

		p.setFrom12Vector(x);
		// ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
		auto R = p.getRotationMatrix();
		const auto Rsvd =
			R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
		R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
		p.setRotationMatrix(R);

		Y = Lie::SE<3>::log(p);
	}

	// Jacobian of Ln(T) wrt T
	void check_jacob_LnT_T(const CPose3D& p)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dlogv_dv(p);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<12> x_mean;
			p.getAs12Vector(x_mean);

			const double& dummy = 0.;
			CVectorFixedDouble<12> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<12>& x, const double& dummy,
					CVectorFixedDouble<6>& Y)>(&func_jacob_LnT_T),
				x_incrs, dummy, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen()).array().abs().sum(),
			0, 1e-3)
			<< "Pose: " << p << endl
			<< "Pose matrix:\n"
			<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}

	static void func_jacob_expe_D(
		const CVectorFixedDouble<6>& eps, const CPose3D& D,
		CVectorFixedDouble<12>& Y)
	{
		const CPose3D incr = Lie::SE<3>::exp(eps);
		const CPose3D expe_D = incr + D;
		expe_D.getAs12Vector(Y);
	}

	// Test Jacobian: d exp(e)*D / d e
	// 10.3.3 in tech report
	void test_Jacob_dexpeD_de(const CPose3D& p)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dexpeD_de(p);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<6> x_mean;
			x_mean.setZero();

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& eps, const CPose3D& D,
					CVectorFixedDouble<12>& Y)>(&func_jacob_expe_D),
				x_incrs, p, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen())
				.array()
				.abs()
				.maxCoeff(),
			0, 1e-3)
			<< "Pose: " << p << endl
			<< "Pose matrix:\n"
			<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}

	static void func_jacob_D_expe(
		const CVectorFixedDouble<6>& eps, const CPose3D& D,
		CVectorFixedDouble<12>& Y)
	{
		const CPose3D incr = Lie::SE<3>::exp(eps);
		const CPose3D expe_D = D + incr;
		expe_D.getAs12Vector(Y);
	}

	// Test Jacobian: d D*exp(e) / d e
	// 10.3.4 in tech report
	void test_Jacob_dDexpe_de(const CPose3D& p)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dDexpe_de(p);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<6> x_mean;
			x_mean.setZero();

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& eps, const CPose3D& D,
					CVectorFixedDouble<12>& Y)>(&func_jacob_D_expe),
				x_incrs, p, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen())
				.array()
				.abs()
				.maxCoeff(),
			0, 1e-3)
			<< "Pose: " << p << endl
			<< "Pose matrix:\n"
			<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}

	struct TParams_func_jacob_Aexpe_D
	{
		CPose3D A, D;
	};

	static void func_jacob_Aexpe_D(
		const CVectorFixedDouble<6>& eps,
		const TParams_func_jacob_Aexpe_D& params, CVectorFixedDouble<12>& Y)
	{
		const CPose3D incr = Lie::SE<3>::exp(eps);
		const CPose3D res = params.A + incr + params.D;
		res.getAs12Vector(Y);
	}

	// Test Jacobian: d A*exp(e)*D / d e
	// 10.3.7 in tech report
	// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
	void test_Jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dAexpeD_de(A, D);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<6> x_mean;
			x_mean.setZero();

			TParams_func_jacob_Aexpe_D params;
			params.A = A;
			params.D = D;
			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& eps,
					const TParams_func_jacob_Aexpe_D& params,
					CVectorFixedDouble<12>& Y)>(&func_jacob_Aexpe_D),
				x_incrs, params, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen())
				.array()
				.abs()
				.maxCoeff(),
			0, 1e-3)
			<< "Pose A: " << A << endl
			<< "Pose D: " << D << endl
			<< "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}
};

// Elemental tests:
TEST_F(Pose3DTests, DefaultValues)
{
	{
		CPose3D p;
		test_default_values(p, "Default");
	}
	{
		CPose3D p2;
		CPose3D p = p2;
		test_default_values(p, "p=p2");
	}
	{
		CPose3D p1, p2;
		test_default_values(p1 + p2, "p1+p2");
		CPose3D p = p1 + p2;
		test_default_values(p, "p=p1+p2");
	}
	{
		CPose3D p1, p2;
		CPose3D p = p1 - p2;
		test_default_values(p, "p1-p2");
	}
}

TEST_F(Pose3DTests, Initialization)
{
	CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
	EXPECT_NEAR(p.x(), 1, 1e-7);
	EXPECT_NEAR(p.y(), 2, 1e-7);
	EXPECT_NEAR(p.z(), 3, 1e-7);
	EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
	EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
	EXPECT_NEAR(p.roll(), 0.4, 1e-7);
}

TEST_F(Pose3DTests, OperatorBracket)
{
	CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
	EXPECT_NEAR(p[0], 1, 1e-7);
	EXPECT_NEAR(p[1], 2, 1e-7);
	EXPECT_NEAR(p[2], 3, 1e-7);
	EXPECT_NEAR(p[3], 0.2, 1e-7);
	EXPECT_NEAR(p[4], 0.3, 1e-7);
	EXPECT_NEAR(p[5], 0.4, 1e-7);
}

// List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
// degrees)
static const std::vector<mrpt::poses::CPose3D> ptc = {
	{.0, .0, .0, .0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
	{1.0, 2.0, 3.0, 80.0_deg, 5.0_deg, 5.0_deg},
	{1.0, 2.0, 3.0, -20.0_deg, -30.0_deg, -40.0_deg},
	{1.0, 2.0, 3.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 2.0, 3.0, 40.0_deg, -5.0_deg, 25.0_deg},
	{1.0, 2.0, 3.0, 40.0_deg, 20.0_deg, -15.0_deg},
	{-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{6.0, -5.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{6.0, 2.0, -9.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{0.0, 8.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 0.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg}};

// More complex tests:
TEST_F(Pose3DTests, InverseHM)
{
	for (const auto& p : ptc) test_inverse(p);
}

TEST_F(Pose3DTests, Compose)
{
	for (const auto& p1 : ptc)
		for (const auto& p2 : ptc) test_compose(p1, p2);
}
TEST_F(Pose3DTests, composeFrom)
{
	for (const auto& p1 : ptc)
		for (const auto& p2 : ptc) test_composeFrom(p1, p2);
}

TEST_F(Pose3DTests, ToFromCPose2D)
{
	for (const auto& p : ptc) test_to_from_2d(p.x(), p.y(), p.yaw());
}

TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
{
	for (const auto& p : ptc)
	{
		test_composePoint(p, 10, 11, 12);
		test_composePoint(p, -5, 1, 2);
		test_composePoint(p, 5, -1, 2);
		test_composePoint(p, 5, 1, -2);
	}
}

TEST_F(Pose3DTests, ComposePointJacob)
{
	for (const auto& p : ptc)
	{
		test_composePointJacob(p, 10, 11, 12);
		test_composePointJacob(p, -5, 1, 2);
	}
}

TEST_F(Pose3DTests, ComposePointJacobApprox)
{  // Test approximated Jacobians for very small rotations
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg), 10, 11,
		12, true);
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.1_deg, 0.0_deg, 0.0_deg), 10, 11,
		12, true);
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.1_deg, 0.0_deg), 10, 11,
		12, true);
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.1_deg), 10, 11,
		12, true);
}

TEST_F(Pose3DTests, InvComposePointJacob)
{
	for (const auto& p : ptc)
	{
		test_invComposePointJacob(p, 10, 11, 12);
		test_invComposePointJacob(p, -5, 1, 2);
		test_invComposePointJacob(p, 5, -1, 2);
		test_invComposePointJacob(p, 5, 1, -2);
	}
}

TEST_F(Pose3DTests, ComposePointJacob_se3)
{
	for (const auto& p : ptc)
	{
		test_composePointJacob_se3(p, TPoint3D(0, 0, 0));
		test_composePointJacob_se3(p, TPoint3D(10, 11, 12));
		test_composePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
	}
}
TEST_F(Pose3DTests, InvComposePointJacob_se3)
{
	for (const auto& p : ptc)
	{
		test_invComposePointJacob_se3(p, TPoint3D(0, 0, 0));
		test_invComposePointJacob_se3(p, TPoint3D(10, 11, 12));
		test_invComposePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
	}
}

TEST_F(Pose3DTests, ExpLnEqual)
{
	for (const auto& p : ptc) test_ExpLnEqual(p);
}

TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
TEST_F(Pose3DTests, Jacob_dLnT_dT)
{
	check_jacob_LnT_T(mrpt::poses::CPose3D(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg));
	// JL NOTE:
	//  This function cannot be properly tested numerically, since the logm()
	//  implementation
	//  is not generic and does NOT depends on all matrix entries, thus the
	//  numerical Jacobian
	//  contains entire columns of zeros, even if the theorethical doesn't.
	//	check_jacob_LnT_T(1.0,0,0, 0.0_deg,0.0_deg,0.0_deg ); ...
}

TEST_F(Pose3DTests, Jacob_dexpeD_de)
{
	for (const auto& p : ptc) test_Jacob_dexpeD_de(p);
}

TEST_F(Pose3DTests, Jacob_dDexpe_de)
{
	for (const auto& p : ptc) test_Jacob_dDexpe_de(p);
}

TEST_F(Pose3DTests, Jacob_dAexpeD_de)
{
	for (const auto& p1 : ptc)
		for (const auto& p2 : ptc) test_Jacob_dAexpeD_de(p1, p2);
}


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if !defined(MULTIVARIATE_PROBABILITY_DISTRIBUTION_HPP)
#define MULTIVARIATE_PROBABILITY_DISTRIBUTION_HPP

#if defined(_MSC_VER)
#	pragma warning(disable: 4267)	// warning about loss of data when converting size_t to int
#endif

#include <cmath>
#include "ncl/nxsdefs.h"

#include "states_patterns.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/format.hpp>
#include "probability_distribution.hpp"
#include "basic_cdf.hpp"
#include "basic_lot.hpp"
#include "phycas_string.hpp"
#if defined(PYTHON_ONLY) && defined(USING_NUMARRAY)
#	include <boost/python/tuple.hpp>
#	include <boost/python/numeric.hpp>
#	include "thirdparty/num_util/num_util.h"
#endif
#include "xprobdist.hpp"
//class XUnderflow{};

namespace phycas
{

class MultivariateProbabilityDistribution
	{
	public:
									MultivariateProbabilityDistribution() {lot = &myLot;}
							virtual ~MultivariateProbabilityDistribution() {}

		virtual void				SetLot(Lot *other)									= 0;
		virtual void				ResetLot()													= 0;
		virtual void				SetSeed(unsigned rnseed)									= 0;

		virtual	bool				IsDiscrete() const											= 0;
		virtual std::string 		GetDistributionName() const									= 0;
		virtual std::string 		GetDistributionDescription() const							= 0;
		virtual std::vector<double>				GetMean() const												= 0;
		virtual std::vector<double> 				GetVar() const												= 0;
		virtual std::vector<double> 				GetStdDev() const											= 0;
		virtual std::vector<double>				Sample() const												= 0;
		virtual double				ApproxCDF(const std::vector<double> &x, unsigned nsamples = 10000) const	= 0;
		virtual double				GetLnPDF(const std::vector<double> &) const								= 0;
		virtual double				GetRelativeLnPDF(const std::vector<double> &) const						= 0;
		virtual void 				SetMeanAndVariance(const std::vector<double> &m, const std::vector<double> &v)		= 0;
#		if defined(PYTHON_ONLY)
#			if defined(USING_NUMARRAY)
				virtual void 			AltSetMeanAndVariance(boost::python::numeric::array m, boost::python::numeric::array v) = 0;
				virtual boost::python::numeric::array	GetVarCovarMatrix()						= 0;
#			else
				//virtual void 			AltSetMeanAndVariance(std::vector<double> m, std::vector<double> v)				= 0;
				virtual std::vector<double>			GetVarCovarMatrix()										= 0;
#			endif
#		endif
		virtual unsigned			GetNParams()	const										= 0;

		CDF					cdf;
		Lot					myLot;
		Lot *				lot;
	};

typedef boost::shared_ptr<MultivariateProbabilityDistribution> MultivarProbDistShPtr;

} // namespace phycas

#endif


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief test_units_1.cpp

\detailed
Test unit class.

Output:
@verbatim
@endverbatim
**/

#include <limits>

#include <boost/units/cmath.hpp>

#include "test_header.hpp"

namespace bu = boost::units;

static volatile double  zero = 0;

int test_main(int,char *[])
{
    double  inf = std::numeric_limits<double>::infinity(),
            nan = 0.0/zero;

    // default constructor
    const bu::quantity<bu::energy>          E1(0.0*bu::joules),
                                            E2(inf*bu::joules),
                                            E3(nan*bu::joules);

    BOOST_CHECK((bu::isfinite)(E1) == true);
    BOOST_CHECK((bu::isfinite)(E2) == false);
    BOOST_CHECK((bu::isfinite)(E3) == false);

    BOOST_CHECK((bu::isinf)(E1) == false);
    BOOST_CHECK((bu::isinf)(E2) == true);
    BOOST_CHECK((bu::isinf)(E3) == false);

    BOOST_CHECK((bu::isnan)(E1) == false);
    BOOST_CHECK((bu::isnan)(E2) == false);
    BOOST_CHECK((bu::isnan)(E3) == true);

    BOOST_CHECK((bu::isnormal)(E1) == false);
    BOOST_CHECK((bu::isnormal)(E2) == false);
    BOOST_CHECK((bu::isnormal)(E3) == false);

    const bu::quantity<bu::energy>          E4(-2.5*bu::joules),
                                            E5(2.5*bu::joules);

    BOOST_CHECK((bu::isgreater)(E4,E5) == false);
    BOOST_CHECK((bu::isgreater)(E5,E4) == true);
    BOOST_CHECK((bu::isgreater)(E4,E4) == false);
    BOOST_CHECK((bu::isgreater)(E3,E4) == false);
    BOOST_CHECK((bu::isgreater)(E4,E3) == false);

    BOOST_CHECK((bu::isgreaterequal)(E4,E5) == false);
    BOOST_CHECK((bu::isgreaterequal)(E5,E4) == true);
    BOOST_CHECK((bu::isgreaterequal)(E4,E4) == true);
    BOOST_CHECK((bu::isgreaterequal)(E3,E4) == false);
    BOOST_CHECK((bu::isgreaterequal)(E4,E3) == false);

    BOOST_CHECK((bu::isless)(E4,E5) == true);
    BOOST_CHECK((bu::isless)(E5,E4) == false);
    BOOST_CHECK((bu::isless)(E4,E4) == false);
    BOOST_CHECK((bu::isless)(E3,E4) == false);
    BOOST_CHECK((bu::isless)(E4,E3) == false);

    BOOST_CHECK((bu::islessequal)(E4,E5) == true);
    BOOST_CHECK((bu::islessequal)(E5,E4) == false);
    BOOST_CHECK((bu::islessequal)(E4,E4) == true);
    BOOST_CHECK((bu::islessequal)(E3,E4) == false);
    BOOST_CHECK((bu::islessequal)(E4,E3) == false);

    BOOST_CHECK((bu::islessgreater)(E4,E5) == true);
    BOOST_CHECK((bu::islessgreater)(E5,E4) == true);
    BOOST_CHECK((bu::islessgreater)(E4,E4) == false);
    BOOST_CHECK((bu::islessgreater)(E3,E4) == false);
    BOOST_CHECK((bu::islessgreater)(E4,E3) == false);

    BOOST_CHECK((bu::isunordered)(E4,E5) == false);
    BOOST_CHECK((bu::isunordered)(E5,E4) == false);
    BOOST_CHECK((bu::isunordered)(E4,E4) == false);
    BOOST_CHECK((bu::isunordered)(E3,E4) == true);
    BOOST_CHECK((bu::isunordered)(E4,E3) == true);

    BOOST_CHECK((bu::abs)(E4) == E5);
    BOOST_CHECK((bu::ceil)(E4) == -2.0*bu::joules);
    BOOST_CHECK((bu::copysign)(E4,E5) == E5);
    BOOST_CHECK((bu::fabs)(E4) == E5);
    BOOST_CHECK((bu::floor)(E4) == -3.0*bu::joules);
    BOOST_CHECK((bu::fdim)(E4,E5) == 0.0*bu::joules);
    BOOST_CHECK((bu::fdim)(E5,E4) == E5-E4);

    const bu::quantity<bu::length>  L1(3.0*bu::meters),
                                    L2(4.0*bu::meters);
    const bu::quantity<bu::area>    A1(4.0*bu::square_meters),
                                    A2(L1*L2+A1);

#if 0
    BOOST_CHECK((bu::fma)(L1,L2,A1) == A2);
#endif

    BOOST_CHECK((bu::fmax)(E4,E5) == E5);
    BOOST_CHECK((bu::fmin)(E4,E5) == E4);

    // need to test fpclassify

    BOOST_CHECK(bu::hypot(L1,L2) == 5.0*bu::meters);

#if 0

//    BOOST_CHECK(bu::llrint(E4).value() == bu::detail::llrint(E4.value()));
//    BOOST_CHECK(bu::llround(E4).value() == bu::detail::llround(E4.value()));
    BOOST_CHECK((bu::nearbyint)(E4).value() == (bu::detail::nearbyint)(E4.value()));
    BOOST_CHECK((bu::rint)(E4).value() == (bu::detail::rint)(E4.value()));

#endif

    BOOST_CHECK((bu::nextafter)(E4,E5).value() == (boost::math::nextafter)(E4.value(),E5.value()));
    BOOST_CHECK((bu::nextafter)(E5,E4).value() == (boost::math::nextafter)(E5.value(),E4.value()));

    BOOST_CHECK((bu::nexttoward)(E4,E5).value() == (boost::math::nextafter)(E4.value(),E5.value()));
    BOOST_CHECK((bu::nexttoward)(E5,E4).value() == (boost::math::nextafter)(E5.value(),E4.value()));

    BOOST_CHECK((bu::round)(E4 - 0.00000000001 * bu::joules) == -3.0*bu::joules);
    BOOST_CHECK((bu::round)(E5 + 0.00000000001 * bu::joules) == 3.0*bu::joules);
    BOOST_CHECK((bu::signbit)(E4) == 1);
    BOOST_CHECK((bu::signbit)(E5) == 0);
    BOOST_CHECK((bu::trunc)(E4) == -2.0*bu::joules);
    BOOST_CHECK((bu::trunc)(E5) == 2.0*bu::joules);

    BOOST_CHECK((bu::fmod)(E4,E5) == -0.0*bu::joules);

    bu::quantity<bu::energy>    pint;

    BOOST_CHECK((bu::modf)(E4,&pint) == -0.5*bu::joules);
    BOOST_CHECK(pint == -2.0*bu::joules);

    int ex;
    const bu::quantity<bu::energy>  E6((bu::frexp)(E4,&ex));

    BOOST_CHECK(E6 == -0.625*bu::joules);
    BOOST_CHECK(ex == 2);
    BOOST_CHECK((bu::ldexp)(E6,ex) == E4);

    const bu::quantity<bu::dimensionless>   E7(1.0);

    BOOST_CHECK(bu::pow(E7,E7) == 1.0*1.0);

    const bu::quantity<bu::dimensionless>   E8((bu::exp)(E7));

    BOOST_CHECK(std::abs(E8 - std::exp(1.0)) < .000001);
    BOOST_CHECK(bu::log(E8) == E7);

    const bu::quantity<bu::dimensionless>   E9(100.0);

    BOOST_CHECK(bu::log10(E9) == 2.0);

    BOOST_CHECK(bu::sqrt(A1) == 2.0*bu::meters);

    return 0;
}


// -*- c++ -*-
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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 <boost/config.hpp>
#include <iostream>

#include <boost/graph/adjacency_list.hpp>

/*
  Thanks to Dale Gerdemann for this example, which inspired some
  changes to adjacency_list to make this work properly.
 */

/*
  Sample output:

  0  --c--> 1   --j--> 1   --c--> 2   --x--> 2
  1  --c--> 2   --d--> 3
  2  --t--> 4
  3  --h--> 4
  4

  merging vertex 1 into vertex 0

  0  --c--> 0   --j--> 0   --c--> 1   --x--> 1   --d--> 2
  1  --t--> 3
  2  --h--> 3
  3
 */

// merge_vertex(u,v,g):
// incoming/outgoing edges for v become incoming/outgoing edges for u
// v is deleted
template < class Graph, class GetEdgeProperties >
void merge_vertex(typename boost::graph_traits< Graph >::vertex_descriptor u,
    typename boost::graph_traits< Graph >::vertex_descriptor v, Graph& g,
    GetEdgeProperties getp)
{
    typedef boost::graph_traits< Graph > Traits;
    typename Traits::edge_descriptor e;
    typename Traits::out_edge_iterator out_i, out_end;
    for (boost::tie(out_i, out_end) = out_edges(v, g); out_i != out_end;
         ++out_i)
    {
        e = *out_i;
        typename Traits::vertex_descriptor targ = target(e, g);
        add_edge(u, targ, getp(e), g);
    }
    typename Traits::in_edge_iterator in_i, in_end;
    for (boost::tie(in_i, in_end) = in_edges(v, g); in_i != in_end; ++in_i)
    {
        e = *in_i;
        typename Traits::vertex_descriptor src = source(e, g);
        add_edge(src, u, getp(e), g);
    }
    clear_vertex(v, g);
    remove_vertex(v, g);
}

template < class StoredEdge > struct order_by_name
{
    typedef StoredEdge first_argument_type;
    typedef StoredEdge second_argument_type;
    typedef bool result_type;
    bool operator()(const StoredEdge& e1, const StoredEdge& e2) const
    {
        // Using std::pair operator< as an easy way to get lexicographical
        // compare over tuples.
        return std::make_pair(e1.get_target(), boost::get(boost::edge_name, e1))
            < std::make_pair(e2.get_target(), boost::get(boost::edge_name, e2));
    }
};
struct ordered_set_by_nameS
{
};

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
namespace boost
{
template < class ValueType >
struct container_gen< ordered_set_by_nameS, ValueType >
{
    typedef std::set< ValueType, order_by_name< ValueType > > type;
};
template <> struct parallel_edge_traits< ordered_set_by_nameS >
{
    typedef allow_parallel_edge_tag type;
};
}
#endif

template < class Graph > struct get_edge_name
{
    get_edge_name(const Graph& g_) : g(g_) {}

    template < class Edge >
    boost::property< boost::edge_name_t, char > operator()(Edge e) const
    {
        return boost::property< boost::edge_name_t, char >(
            boost::get(boost::edge_name, g, e));
    }
    const Graph& g;
};

int main()
{
#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    std::cout << "This program requires partial specialization." << std::endl;
#else
    using namespace boost;
    typedef property< edge_name_t, char > EdgeProperty;
    typedef adjacency_list< ordered_set_by_nameS, vecS, bidirectionalS,
        no_property, EdgeProperty >
        graph_type;

    graph_type g;

    add_edge(0, 1, EdgeProperty('j'), g);
    add_edge(0, 2, EdgeProperty('c'), g);
    add_edge(0, 2, EdgeProperty('x'), g);
    add_edge(1, 3, EdgeProperty('d'), g);
    add_edge(1, 2, EdgeProperty('c'), g);
    add_edge(1, 3, EdgeProperty('d'), g);
    add_edge(2, 4, EdgeProperty('t'), g);
    add_edge(3, 4, EdgeProperty('h'), g);
    add_edge(0, 1, EdgeProperty('c'), g);

    property_map< graph_type, vertex_index_t >::type id = get(vertex_index, g);
    property_map< graph_type, edge_name_t >::type name = get(edge_name, g);

    graph_traits< graph_type >::vertex_iterator i, end;
    graph_traits< graph_type >::out_edge_iterator ei, edge_end;

    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        std::cout << id[*i] << " ";
        for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
            std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)]
                      << "  ";
        std::cout << std::endl;
    }
    std::cout << std::endl;

    std::cout << "merging vertex 1 into vertex 0" << std::endl << std::endl;
    merge_vertex(0, 1, g, get_edge_name< graph_type >(g));

    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        std::cout << id[*i] << " ";
        for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
            std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)]
                      << "  ";
        std::cout << std::endl;
    }
    std::cout << std::endl;
#endif
    return 0;
}


//
//  Created by Yoann Coudert-osmont
//

#ifndef CVT_HPP
#define CVT_HPP

#include "graphics/Shape.h"
#include "LBFGS/clipper.hpp"

#include <boost/polygon/voronoi.hpp>
#include <Eigen/Core>

typedef glm::vec<2, double> Vec2;
typedef std::pair<Vec2, Vec2> Segment;
typedef boost::polygon::voronoi_diagram<double> VD;

namespace boost::polygon {
	template<> struct geometry_concept<Vec2> { typedef point_concept type; };
	template<> struct point_traits<Vec2> {
		typedef double coordinate_type;
		static inline coordinate_type get(const Vec2& v, orientation_2d orient) {
			return (orient == HORIZONTAL) ? v.x : v.y;
		}
	};
	template<> struct geometry_concept<Segment> { typedef segment_concept type; };
	template<> struct segment_traits<Segment> {
		typedef double coordinate_type;
		typedef Vec2 point_type;
		static inline point_type get(const Segment& segment, direction_1d dir) {
			return dir.to_int() ? segment.second : segment.first;
		}
	};
}

struct self_intersection_error : public std::exception {
	const std::string m_msg;
	self_intersection_error(const char* msg):
		m_msg("An edge of the path intersects an other edge of the path (detected by: " + std::string(msg) + ")")
		{}
	const char* what() const throw() { return m_msg.c_str(); }
};

struct Voronoi {
public:
	Voronoi(const Shape *shape);
	Voronoi(const Shape *shape, const Box<double> &box);

protected:
	const Shape *_shape;
	Box<double> _shape_box;
	ClipperLib::Paths _clipBorder;
	double _scale;
	Vec2 _mid;

	void updateScale(const Box<double> &box);
	void updateClipBorder();
	void updatePoints(std::vector<Vec2> &points, const Eigen::VectorXd &x);

	ClipperLib::Paths clip(const ClipperLib::Paths &path);
};

struct CVT : public Voronoi {
public:
	CVT(const Shape *shape): Voronoi(shape) {}
	CVT(const Shape *shape, const Box<double> &box): Voronoi(shape, box) {}

protected:
	double _prevF;
	Eigen::VectorXd _prevX;

	double foundError(const Eigen::VectorXd &x, Eigen::VectorXd &grad, const char* msg);
	double cell2Clipper(const VD::cell_type &vd_c, int ni, ClipperLib::Paths &path,
						const std::vector<Vec2> points, const std::vector<Segment> &segments,
						const Eigen::VectorXd &x, Eigen::VectorXd &grad);

	static double point_fun_grad(const std::vector<Vec2> &P, const Vec2 &a, Vec2 &grad);
};

//////////////////
//
//   POINT CVT
//
//////////////////

struct PointCVT : public Voronoi {
public:
	PointCVT(const Shape *shape): Voronoi(shape) {}
	void updateLink(const Eigen::VectorXd &x, std::vector<glm::vec2> &points, std::vector<std::vector<int>> &oriLink, std::vector<std::vector<int>> &cLink);
private:
	std::vector<Vec2> _points;
};

//////////////////
//
//   SMOOTHER
//
//////////////////

class Smoother : public CVT {
public:
	double laplacian_coeff = .125;
	double length_coeff = .4;
	double obj_coeff = 0.013;

	Smoother(const Shape *shape, const std::vector<Shape> *objZones, const std::vector<Shape> *colorZones, double vecArea, int layerIndex);

	void optimize(Eigen::VectorXd &x);
	void computeRadii(const Eigen::VectorXd &x, std::vector<glm::vec3> &path);
	double operator()(Eigen::VectorXd &x, Eigen::VectorXd &grad);

private:
	const std::vector<Shape> *_objZones, *_colorZones;
	std::vector<ClipperLib::Paths> _clipZones;
	std::vector<std::vector<std::tuple<int, double, double>>> _paths;
	std::vector<std::vector<Segment>> _segments;
	int _layerIndex;
	double _vecArea, _L0;

	const static std::vector<Vec2> _points;

	void construct_voro(const Eigen::VectorXd &x);
	double vectorfieldEnergy(const Eigen::VectorXd &x, std::vector<double> &grad, double &div, int j, int k, std::vector<Vec2> &P);
	double isotropyEnergy(const Eigen::VectorXd &x, Eigen::VectorXd &grad, double coeff);
};

#endif //CVT_H


#include <limits>
#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/safe_integer_literal.hpp>

using namespace boost::safe_numerics;

int f(int i){
    return i;
}

template<intmax_t N>
using safe_literal = safe_signed_literal<N, native, loose_trap_policy>;

int main(){
    const long x = 97;
    f(x);   // OK - implicit conversion to int
    const safe_literal<97> y;
    f(y);   // OK - y is a type with min/max = 97;
    return 0;
}