// boost\math\distributions\non_central_t.hpp

// Copyright John Maddock 2008.

// 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_MATH_SPECIAL_NON_CENTRAL_T_HPP
#define BOOST_MATH_SPECIAL_NON_CENTRAL_T_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/distributions/non_central_beta.hpp> // for nc beta
#include <boost/math/distributions/normal.hpp> // for normal CDF and quantile
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/detail/generic_quantile.hpp> // quantile

namespace boost
{
   namespace math
   {

      template <class RealType, class Policy>
      class non_central_t_distribution;

      namespace detail{

         template <class T, class Policy>
         T non_central_t2_p(T n, T delta, T x, T y, const Policy& pol, T init_val)
         {
            BOOST_MATH_STD_USING
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = ldexp(1.0f, -boost::math::policies::digits<T, Policy>());
            T d2 = delta * delta / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = boost::math::itrunc(d2);
            // Starting Poisson weight:
            T pois = gamma_p_derivative(T(k+1), d2, pol)
               * tgamma_delta_ratio(T(k + 1), T(0.5f))
               * delta / constants::root_two<T>();
            if(pois == 0)
               return init_val;
            // Starting beta term:
            T beta = x < y
               ? ibeta(T(k + 1), n / 2, x, pol)
               : ibetac(n / 2, T(k + 1), y, pol);
            // Recurance term:
            T xterm = x < y
               ? ibeta_derivative(T(k + 1), n / 2, x, pol)
               : ibeta_derivative(n / 2, T(k + 1), y, pol);
            xterm *= y / (n / 2 + k);
            T poisf(pois), betaf(beta), xtermf(xterm);
            T sum = init_val;
            if((xterm == 0) && (beta == 0))
               return init_val;

            //
            // Backwards recursion first, this is the stable
            // direction for recursion:
            //
            boost::uintmax_t count = 0;
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if(fabs(term/sum) < errtol)
                  break;
               pois *= (i + 0.5f) / d2;
               beta += xterm;
               xterm *= (i) / (x * (n / 2 + i - 1));
               ++count;
            }
            for(int i = k + 1; ; ++i)
            {
               poisf *= d2 / (i + 0.5f);
               xtermf *= (x * (n / 2 + i - 1)) / (i);
               betaf -= xtermf;
               T term = poisf * betaf;
               sum += term;
               if(fabs(term/sum) < errtol)
                  break;
               ++count;
               if(count > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_t_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class T, class Policy>
         T non_central_t2_q(T n, T delta, T x, T y, const Policy& pol, T init_val)
         {
            BOOST_MATH_STD_USING
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = ldexp(1.0f, -boost::math::policies::digits<T, Policy>());
            T d2 = delta * delta / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = boost::math::itrunc(d2);
            // Starting Poisson weight:
            T pois = gamma_p_derivative(T(k+1), d2, pol)
               * tgamma_delta_ratio(T(k + 1), T(0.5f))
               * delta / constants::root_two<T>();
            if(pois == 0)
               return init_val;
            // Starting beta term:
            T beta = x < y
               ? ibetac(T(k + 1), n / 2, x, pol)
               : ibeta(n / 2, T(k + 1), y, pol);
            // Recurance term:
            T xterm = x < y
               ? ibeta_derivative(T(k + 1), n / 2, x, pol)
               : ibeta_derivative(n / 2, T(k + 1), y, pol);
            xterm *= y / (n / 2 + k);
            T poisf(pois), betaf(beta), xtermf(xterm);
            T sum = init_val;
            if((xterm == 0) && (beta == 0))
               return init_val;

            //
            // Forward recursion first, this is the stable direction:
            //
            boost::uintmax_t count = 0;
            for(int i = k + 1; ; ++i)
            {
               poisf *= d2 / (i + 0.5f);
               xtermf *= (x * (n / 2 + i - 1)) / (i);
               betaf += xtermf;

               T term = poisf * betaf;
               sum += term;
               if(fabs(term/sum) < errtol)
                  break;
               if(count > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_t_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
               ++count;
            }
            //
            // Backwards recursion:
            //
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if(fabs(term/sum) < errtol)
                  break;
               pois *= (i + 0.5f) / d2;
               beta -= xterm;
               xterm *= (i) / (x * (n / 2 + i - 1));
               ++count;
               if(count > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_t_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class T, class Policy>
         T non_central_t_cdf(T n, T delta, T t, bool invert, const Policy& pol)
         {
            //
            // For t < 0 we have to use reflect:
            //
            if(t < 0)
            {
               t = -t;
               delta = -delta;
               invert = !invert;
            }
            //
            // x and y are the corresponding random
            // variables for the noncentral beta distribution,
            // with y = 1 - x:
            //
            T x = t * t / (n + t * t);
            T y = n / (n + t * t);
            T d2 = delta * delta;
            T a = 0.5f;
            T b = n / 2;
            T c = a + b + d2 / 2;
            //
            // Crossover point for calculating p or q is the same
            // as for the noncentral beta:
            //
            T cross = 1 - (b / c) * (1 + d2 / (2 * c * c));
            T result;
            if(x < cross)
            {
               //
               // Calculate p:
               //
               if(x != 0)
               {
                  result = non_central_beta_p(a, b, d2, x, y, pol);
                  result = non_central_t2_p(n, delta, x, y, pol, result);
                  result /= 2;
               }
               else
                  result = 0;
               result += cdf(boost::math::normal_distribution<T, Policy>(), -delta);
            }
            else
            {
               //
               // Calculate q:
               //
               invert = !invert;
               if(x != 0)
               {
                  result = non_central_beta_q(a, b, d2, x, y, pol);
                  result = non_central_t2_q(n, delta, x, y, pol, result);
                  result /= 2;
               }
               else
                  result = cdf(complement(boost::math::normal_distribution<T, Policy>(), -delta));
            }
            if(invert)
               result = 1 - result;
            return result;
         }

         template <class T, class Policy>
         T non_central_t_quantile(T v, T delta, T p, T q, const Policy&)
         {
            BOOST_MATH_STD_USING
            static const char* function = "quantile(non_central_t_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<T, 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;

               T r;
               if(!detail::check_df(
                  function,
                  v, &r, Policy())
                  ||
               !detail::check_finite(
                  function,
                  delta,
                  &r,
                  Policy())
                  ||
               !detail::check_probability(
                  function,
                  p,
                  &r,
                  Policy()))
                     return r;

            value_type guess = 0;
            if(v > 3)
            {
               value_type mean = delta * sqrt(v / 2) * tgamma_delta_ratio((v - 1) * 0.5f, T(0.5f));
               value_type var = ((delta * delta + 1) * v) / (v - 2) - mean * mean;
               if(p < q)
                  guess = quantile(normal_distribution<value_type, forwarding_policy>(mean, var), p);
               else
                  guess = quantile(complement(normal_distribution<value_type, forwarding_policy>(mean, var), q));
            }
            //
            // We *must* get the sign of the initial guess correct,
            // or our root-finder will fail, so double check it now:
            //
            value_type pzero = non_central_t_cdf(
               static_cast<value_type>(v),
               static_cast<value_type>(delta),
               static_cast<value_type>(0),
               !(p < q),
               forwarding_policy());
            int s;
            if(p < q)
               s = boost::math::sign(p - pzero);
            else
               s = boost::math::sign(pzero - q);
            if(s != boost::math::sign(guess))
            {
               guess = s;
            }

            value_type result = detail::generic_quantile(
               non_central_t_distribution<value_type, forwarding_policy>(v, delta),
               (p < q ? p : q),
               guess,
               (p >= q),
               function);
            return policies::checked_narrowing_cast<T, forwarding_policy>(
               result,
               function);
         }

         template <class T, class Policy>
         T non_central_t2_pdf(T n, T delta, T x, T y, const Policy& pol, T init_val)
         {
            BOOST_MATH_STD_USING
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = ldexp(1.0f, -boost::math::policies::digits<T, Policy>());
            T d2 = delta * delta / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = boost::math::itrunc(d2);
            // Starting Poisson weight:
            T pois = gamma_p_derivative(T(k+1), d2, pol)
               * tgamma_delta_ratio(T(k + 1), T(0.5f))
               * delta / constants::root_two<T>();
            // Starting beta term:
            T xterm = x < y
               ? ibeta_derivative(T(k + 1), n / 2, x, pol)
               : ibeta_derivative(n / 2, T(k + 1), y, pol);
            T poisf(pois), xtermf(xterm);
            T sum = init_val;
            if((pois == 0) || (xterm == 0))
               return init_val;

            //
            // Backwards recursion first, this is the stable
            // direction for recursion:
            //
            boost::uintmax_t count = 0;
            for(int i = k; i >= 0; --i)
            {
               T term = xterm * pois;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
                  break;
               pois *= (i + 0.5f) / d2;
               xterm *= (i) / (x * (n / 2 + i));
               ++count;
               if(count > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "pdf(non_central_t_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            for(int i = k + 1; ; ++i)
            {
               poisf *= d2 / (i + 0.5f);
               xtermf *= (x * (n / 2 + i)) / (i);
               T term = poisf * xtermf;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
                  break;
               ++count;
               if(count > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "pdf(non_central_t_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class T, class Policy>
         T non_central_t_pdf(T n, T delta, T t, const Policy& pol)
         {
            BOOST_MATH_STD_USING
            //
            // For t < 0 we have to use the reflection formula:
            //
            if(t < 0)
            {
               t = -t;
               delta = -delta;
            }
            if(t == 0)
            {
               //
               // Handle this as a special case, using the formula
               // from Weisstein, Eric W.
               // "Noncentral Student's t-Distribution."
               // From MathWorld--A Wolfram Web Resource.
               // http://mathworld.wolfram.com/NoncentralStudentst-Distribution.html
               //
               // The formula is simplified thanks to the relation
               // 1F1(a,b,0) = 1.
               //
               return tgamma_delta_ratio(n / 2 + 0.5f, T(0.5f))
                  * sqrt(n / constants::pi<T>())
                  * exp(-delta * delta / 2) / 2;
            }
            //
            // x and y are the corresponding random
            // variables for the noncentral beta distribution,
            // with y = 1 - x:
            //
            T x = t * t / (n + t * t);
            T y = n / (n + t * t);
            T a = 0.5f;
            T b = n / 2;
            T d2 = delta * delta;
            //
            // Calculate pdf:
            //
            T dt = n * t / (n * n + 2 * n * t * t + t * t * t * t);
            T result = non_central_beta_pdf(a, b, d2, x, y, pol);
            T tol = tools::epsilon<T>() * result * 500;
            result = non_central_t2_pdf(n, delta, x, y, pol, result);
            if(result <= tol)
               result = 0;
            result *= dt;
            return result;
         }

         template <class T, class Policy>
         T mean(T v, T delta, const Policy& pol)
         {
            BOOST_MATH_STD_USING
            return delta * sqrt(v / 2) * tgamma_delta_ratio((v - 1) * 0.5f, T(0.5f), pol);
         }

         template <class T, class Policy>
         T variance(T v, T delta, const Policy& pol)
         {
            T result = ((delta * delta + 1) * v) / (v - 2);
            T m = mean(v, delta, pol);
            result -= m * m;
            return result;
         }

         template <class T, class Policy>
         T skewness(T v, T delta, const Policy& pol)
         {
            BOOST_MATH_STD_USING
            T mean = boost::math::detail::mean(v, delta, pol);
            T l2 = delta * delta;
            T var = ((l2 + 1) * v) / (v - 2) - mean * mean;
            T result = -2 * var;
            result += v * (l2 + 2 * v - 3) / ((v - 3) * (v - 2));
            result *= mean;
            result /= pow(var, T(1.5f));
            return result;
         }

         template <class T, class Policy>
         T kurtosis_excess(T v, T delta, const Policy& pol)
         {
            BOOST_MATH_STD_USING
            T mean = boost::math::detail::mean(v, delta, pol);
            T l2 = delta * delta;
            T var = ((l2 + 1) * v) / (v - 2) - mean * mean;
            T result = -3 * var;
            result += v * (l2 * (v + 1) + 3 * (3 * v - 5)) / ((v - 3) * (v - 2));
            result *= -mean * mean;
            result += v * v * (l2 * l2 + 6 * l2 + 3) / ((v - 4) * (v - 2));
            result /= var * var;
            return result;
         }

#if 0
         //
         // This code is disabled, since there can be multiple answers to the
         // question, and it's not clear how to find the "right" one.
         //
         template <class RealType, class Policy>
         struct t_degrees_of_freedom_finder
         {
            t_degrees_of_freedom_finder(
               RealType delta_, RealType x_, RealType p_, bool c)
               : delta(delta_), x(x_), p(p_), comp(c) {}

            RealType operator()(const RealType& v)
            {
               non_central_t_distribution<RealType, Policy> d(v, delta);
               return comp ?
                  p - cdf(complement(d, x))
                  : cdf(d, x) - p;
            }
         private:
            RealType delta;
            RealType x;
            RealType p;
            bool comp;
         };

         template <class RealType, class Policy>
         inline RealType find_t_degrees_of_freedom(
            RealType delta, RealType x, RealType p, RealType q, const Policy& pol)
         {
            const char* function = "non_central_t<%1%>::find_degrees_of_freedom";
            if((p == 0) || (q == 0))
            {
               //
               // Can't a thing if one of p and q is zero:
               //
               return policies::raise_evaluation_error<RealType>(function,
                  "Can't find degrees of freedom when the probability is 0 or 1, only possible answer is %1%",
                  RealType(std::numeric_limits<RealType>::quiet_NaN()), Policy());
            }
            t_degrees_of_freedom_finder<RealType, Policy> f(delta, x, p < q ? p : q, p < q ? false : true);
            tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
            boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
            //
            // Pick an initial guess:
            //
            RealType guess = 200;
            std::pair<RealType, RealType> ir = tools::bracket_and_solve_root(
               f, guess, RealType(2), false, tol, max_iter, pol);
            RealType result = ir.first + (ir.second - ir.first) / 2;
            if(max_iter >= policies::get_max_root_iterations<Policy>())
            {
               policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
                  " or there is no answer to problem.  Current best guess is %1%", result, Policy());
            }
            return result;
         }

         template <class RealType, class Policy>
         struct t_non_centrality_finder
         {
            t_non_centrality_finder(
               RealType v_, RealType x_, RealType p_, bool c)
               : v(v_), x(x_), p(p_), comp(c) {}

            RealType operator()(const RealType& delta)
            {
               non_central_t_distribution<RealType, Policy> d(v, delta);
               return comp ?
                  p - cdf(complement(d, x))
                  : cdf(d, x) - p;
            }
         private:
            RealType v;
            RealType x;
            RealType p;
            bool comp;
         };

         template <class RealType, class Policy>
         inline RealType find_t_non_centrality(
            RealType v, RealType x, RealType p, RealType q, const Policy& pol)
         {
            const char* function = "non_central_t<%1%>::find_t_non_centrality";
            if((p == 0) || (q == 0))
            {
               //
               // Can't do a thing if one of p and q is zero:
               //
               return policies::raise_evaluation_error<RealType>(function,
                  "Can't find non centrality parameter when the probability is 0 or 1, only possible answer is %1%",
                  RealType(std::numeric_limits<RealType>::quiet_NaN()), Policy());
            }
            t_non_centrality_finder<RealType, Policy> f(v, x, p < q ? p : q, p < q ? false : true);
            tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
            boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
            //
            // Pick an initial guess that we know is the right side of
            // zero:
            //
            RealType guess;
            if(f(0) < 0)
               guess = 1;
            else
               guess = -1;
            std::pair<RealType, RealType> ir = tools::bracket_and_solve_root(
               f, guess, RealType(2), false, tol, max_iter, pol);
            RealType result = ir.first + (ir.second - ir.first) / 2;
            if(max_iter >= policies::get_max_root_iterations<Policy>())
            {
               policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
                  " or there is no answer to problem.  Current best guess is %1%", result, Policy());
            }
            return result;
         }
#endif
      } // namespace detail

      template <class RealType = double, class Policy = policies::policy<> >
      class non_central_t_distribution
      {
      public:
         typedef RealType value_type;
         typedef Policy policy_type;

         non_central_t_distribution(RealType v_, RealType lambda) : v(v_), ncp(lambda)
         {
            const char* function = "boost::math::non_central_t_distribution<%1%>::non_central_t_distribution(%1%,%1%)";
            RealType r;
            detail::check_df(
               function,
               v, &r, Policy());
            detail::check_finite(
               function,
               lambda,
               &r,
               Policy());
         } // non_central_t_distribution constructor.

         RealType degrees_of_freedom() const
         { // Private data getter function.
            return v;
         }
         RealType non_centrality() const
         { // Private data getter function.
            return ncp;
         }
#if 0
         //
         // This code is disabled, since there can be multiple answers to the
         // question, and it's not clear how to find the "right" one.
         //
         static RealType find_degrees_of_freedom(RealType delta, RealType x, RealType p)
         {
            const char* function = "non_central_t<%1%>::find_degrees_of_freedom";
            typedef typename policies::evaluation<RealType, 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;
            value_type result = detail::find_t_degrees_of_freedom(
               static_cast<value_type>(delta),
               static_cast<value_type>(x),
               static_cast<value_type>(p),
               static_cast<value_type>(1-p),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
         template <class A, class B, class C>
         static RealType find_degrees_of_freedom(const complemented3_type<A,B,C>& c)
         {
            const char* function = "non_central_t<%1%>::find_degrees_of_freedom";
            typedef typename policies::evaluation<RealType, 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;
            value_type result = detail::find_t_degrees_of_freedom(
               static_cast<value_type>(c.dist),
               static_cast<value_type>(c.param1),
               static_cast<value_type>(1-c.param2),
               static_cast<value_type>(c.param2),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
         static RealType find_non_centrality(RealType v, RealType x, RealType p)
         {
            const char* function = "non_central_t<%1%>::find_t_non_centrality";
            typedef typename policies::evaluation<RealType, 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;
            value_type result = detail::find_t_non_centrality(
               static_cast<value_type>(v),
               static_cast<value_type>(x),
               static_cast<value_type>(p),
               static_cast<value_type>(1-p),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
         template <class A, class B, class C>
         static RealType find_non_centrality(const complemented3_type<A,B,C>& c)
         {
            const char* function = "non_central_t<%1%>::find_t_non_centrality";
            typedef typename policies::evaluation<RealType, 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;
            value_type result = detail::find_t_non_centrality(
               static_cast<value_type>(c.dist),
               static_cast<value_type>(c.param1),
               static_cast<value_type>(1-c.param2),
               static_cast<value_type>(c.param2),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
#endif
      private:
         // Data member, initialized by constructor.
         RealType v;   // degrees of freedom
         RealType ncp; // non-centrality parameter
      }; // template <class RealType, class Policy> class non_central_t_distribution

      typedef non_central_t_distribution<double> non_central_t; // Reserved name of type double.

      // Non-member functions to give properties of the distribution.

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> range(const non_central_t_distribution<RealType, Policy>& /* dist */)
      { // Range of permissible values for random variable k.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
      }

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> support(const non_central_t_distribution<RealType, Policy>& /* dist */)
      { // Range of supported values for random variable k.
         // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
      }

      template <class RealType, class Policy>
      inline RealType mode(const non_central_t_distribution<RealType, Policy>& dist)
      { // mode.
         static const char* function = "mode(non_central_t_distribution<%1%> const&)";
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy()))
               return (RealType)r;

         BOOST_MATH_STD_USING

         RealType m = v < 3 ? 0 : detail::mean(v, l, Policy());
         RealType var = v < 4 ? 1 : detail::variance(v, l, Policy());

         return detail::generic_find_mode(
            dist,
            m,
            function,
            sqrt(var));
      }

      template <class RealType, class Policy>
      inline RealType mean(const non_central_t_distribution<RealType, Policy>& dist)
      {
         BOOST_MATH_STD_USING
         const char* function = "mean(const non_central_t_distribution<%1%>&)";
         typedef typename policies::evaluation<RealType, 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;
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy()))
               return (RealType)r;
         if(v <= 1)
            return policies::raise_domain_error<RealType>(
               function,
               "The non central t distribution has no defined mean for degrees of freedom <= 1: got v=%1%.", v, Policy());
         // return l * sqrt(v / 2) * tgamma_delta_ratio((v - 1) * 0.5f, RealType(0.5f));
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::mean(static_cast<value_type>(v), static_cast<value_type>(l), forwarding_policy()), function);

      } // mean

      template <class RealType, class Policy>
      inline RealType variance(const non_central_t_distribution<RealType, Policy>& dist)
      { // variance.
         const char* function = "variance(const non_central_t_distribution<%1%>&)";
         typedef typename policies::evaluation<RealType, 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_MATH_STD_USING
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy()))
               return (RealType)r;
         if(v <= 2)
            return policies::raise_domain_error<RealType>(
               function,
               "The non central t distribution has no defined variance for degrees of freedom <= 2: got v=%1%.", v, Policy());
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::variance(static_cast<value_type>(v), static_cast<value_type>(l), forwarding_policy()), function);
      }

      // RealType standard_deviation(const non_central_t_distribution<RealType, Policy>& dist)
      // standard_deviation provided by derived accessors.

      template <class RealType, class Policy>
      inline RealType skewness(const non_central_t_distribution<RealType, Policy>& dist)
      { // skewness = sqrt(l).
         const char* function = "skewness(const non_central_t_distribution<%1%>&)";
         typedef typename policies::evaluation<RealType, 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;
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy()))
               return (RealType)r;
         if(v <= 3)
            return policies::raise_domain_error<RealType>(
               function,
               "The non central t distribution has no defined skewness for degrees of freedom <= 3: got v=%1%.", v, Policy());;
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::skewness(static_cast<value_type>(v), static_cast<value_type>(l), forwarding_policy()), function);
      }

      template <class RealType, class Policy>
      inline RealType kurtosis_excess(const non_central_t_distribution<RealType, Policy>& dist)
      {
         const char* function = "kurtosis_excess(const non_central_t_distribution<%1%>&)";
         typedef typename policies::evaluation<RealType, 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;
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy()))
               return (RealType)r;
         if(v <= 4)
            return policies::raise_domain_error<RealType>(
               function,
               "The non central t distribution has no defined kurtosis for degrees of freedom <= 4: got v=%1%.", v, Policy());;
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::kurtosis_excess(static_cast<value_type>(v), static_cast<value_type>(l), forwarding_policy()), function);
      } // kurtosis_excess

      template <class RealType, class Policy>
      inline RealType kurtosis(const non_central_t_distribution<RealType, Policy>& dist)
      {
         return kurtosis_excess(dist) + 3;
      }

      template <class RealType, class Policy>
      inline RealType pdf(const non_central_t_distribution<RealType, Policy>& dist, const RealType& t)
      { // Probability Density/Mass Function.
         const char* function = "cdf(non_central_t_distribution<%1%>, %1%)";
         typedef typename policies::evaluation<RealType, 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;

         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy())
            ||
         !detail::check_x(
            function,
            t,
            &r,
            Policy()))
               return (RealType)r;
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::non_central_t_pdf(static_cast<value_type>(v),
               static_cast<value_type>(l),
               static_cast<value_type>(t),
               Policy()),
            function);
      } // pdf

      template <class RealType, class Policy>
      RealType cdf(const non_central_t_distribution<RealType, Policy>& dist, const RealType& x)
      {
         const char* function = "boost::math::non_central_t_distribution<%1%>::cdf(%1%)";
         typedef typename policies::evaluation<RealType, 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;

         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy())
            ||
         !detail::check_x(
            function,
            x,
            &r,
            Policy()))
               return (RealType)r;

         if(l == 0)
            return cdf(students_t_distribution<RealType, Policy>(v), x);

         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::non_central_t_cdf(
               static_cast<value_type>(v),
               static_cast<value_type>(l),
               static_cast<value_type>(x),
               false, Policy()),
            function);
      } // cdf

      template <class RealType, class Policy>
      RealType cdf(const complemented2_type<non_central_t_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function
         const char* function = "boost::math::non_central_t_distribution<%1%>::cdf(%1%)";
         typedef typename policies::evaluation<RealType, 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;

         non_central_t_distribution<RealType, Policy> const& dist = c.dist;
         RealType x = c.param;
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v, &r, Policy())
            ||
         !detail::check_finite(
            function,
            l,
            &r,
            Policy())
            ||
         !detail::check_x(
            function,
            x,
            &r,
            Policy()))
               return (RealType)r;

         if(l == 0)
            return cdf(complement(students_t_distribution<RealType, Policy>(v), x));

         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            detail::non_central_t_cdf(
               static_cast<value_type>(v),
               static_cast<value_type>(l),
               static_cast<value_type>(x),
               true, Policy()),
            function);
      } // ccdf

      template <class RealType, class Policy>
      inline RealType quantile(const non_central_t_distribution<RealType, Policy>& dist, const RealType& p)
      { // Quantile (or Percent Point) function.
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         return detail::non_central_t_quantile(v, l, p, 1-p, Policy());
      } // quantile

      template <class RealType, class Policy>
      inline RealType quantile(const complemented2_type<non_central_t_distribution<RealType, Policy>, RealType>& c)
      { // Quantile (or Percent Point) function.
         non_central_t_distribution<RealType, Policy> const& dist = c.dist;
         RealType q = c.param;
         RealType v = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         return detail::non_central_t_quantile(v, l, 1-q, q, Policy());
      } // quantile complement.

   } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_NON_CENTRAL_T_HPP


/*!
 * \file TangleMoveFigure.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019-2020 Jun Yoshida.
 * The project is released under the MIT License.
 * \date January 29, 2020: created
 */

#pragma once

#include <array>
#include <vector>
#include <functional>
#include <Eigen/Dense>

#include "../PlTang.hpp"
#include "../PlTangMove.hpp"
#include "../Graph.hpp"
#include "../GenericGraph.hpp"
#include "PlTangFigure.hpp"
#include "PathFigure3D.hpp"

/* Debug
#include <iostream>
#include <iomanip>
// */

template <class T>
class TangleMoveFigure;

template <size_t R, size_t C>
class TangleMoveFigure<PlTang<R,C>> : public PathFigure3D
{
private:
    //! Internal representation of drawn object.
    using PathGraph = GenericGraph<std::map<size_t,Eigen::Vector3d>>;

    //! Tangles in the domain and the codomain of the bordism.
    PlTang<R,C> m_tangDom{}, m_tangCod{};

    //! Sequence of grouped moves.
    std::vector<PlTangMove<2,2>::MoveSeq> m_mvseqs{};

    //! Basis matrix for the 3d-space in rendering.
    Eigen::Matrix3d m_base = Eigen::Matrix3d::Identity();

    //! Vector normal to ridge-lines.
    Eigen::Vector2d m_critV;

    struct PathElem {
        bord2::PathElemType type;
        Eigen::Vector3d v;
        double midheight[2];
    };

    //! Paths in drawing.
    std::vector<std::vector<PathElem>> m_paths{};

public:
    TangleMoveFigure(PlTang<R,C> const& tang,
                     std::vector<PlTangMove<2,2>::MoveSeq> const& mvseqs,
                     Eigen::Matrix3d const& base)
        : m_tangDom(tang), m_tangCod(tang),
          m_mvseqs(mvseqs),
          m_base(base),
          m_critV(1.0, 0.0)
    {
        for(auto& seq : mvseqs) {
            for(auto& mv : seq) {
                m_tangCod.replace(mv.x, mv.y, mv.move.getAfter());
            }
        }

        makePath();
    }

    virtual ~TangleMoveFigure() = default;

    void setTangleMove(PlTang<R,C> const& tang,
                       std::vector<PlTangMove<2,2>::MoveSeq> const& mvseqs
        ) noexcept
    {
        m_tangDom = tang;
        m_tangCod = tang;
        m_mvseqs = mvseqs;

        for(auto& seq : mvseqs) {
            for(auto& mv : seq) {
                m_tangCod.replace(mv.x, mv.y, mv.move.getAfter());
            }
        }

        makePath();
    }

    Eigen::Matrix3d& baseMatrix() noexcept {
        return m_base;
    }

    Eigen::Matrix3d const& baseMatrix() const noexcept {
        return m_base;
    }

    /**!
     * \section Member functions derived from PathFigure
     **/
    virtual void draw(SchemeType &scheme) const override
    {
        Eigen::Vector3d orig
            = -m_base * Eigen::Vector3d(
                static_cast<double>(m_tangDom.hlength())/2.0,
                static_cast<double>(m_tangDom.vlength())/2.0,
                static_cast<double>(m_mvseqs.size())/2.0 );

        scheme.save();
        scheme.translate(orig);

        /* Draw the domain tangle */{
            AdapterScheme<PathScheme<Eigen::Vector3d>, Eigen::Vector2d> adpscheme(
                &scheme,
                [this, &orig](Eigen::Vector2d const&p) {
                    return m_base.block<3,2>(0,0)*p;
                } );

            PlTangFigure pltangfig{ m_tangDom,
                    Eigen::Vector2d(1.0, 0.0),
                    Eigen::Vector2d(0.0, 1.0) };

            pltangfig.draw(adpscheme);
            adpscheme.release();
        }

        // If no moves, stop here.
        if(m_paths.empty())
            return;

        /* Draw the cobordisms */
        for(auto& path : m_paths) {
            if (path.size() <= 1)
                continue;

            scheme.moveTo(m_base * path[0].v);

            for(size_t i = 1; i < path.size(); ++i) {
                switch(path[i].type) {
                case bord2::Line:
                    scheme.lineTo(m_base*path[i].v);
                    break;

                case bord2::Bezier:
                    {
                        auto c1 = Eigen::Vector3d(
                            path[i-1].v(0),
                            path[i-1].v(1),
                            path[i].midheight[0] );
                        auto c2 = Eigen::Vector3d(
                            path[i].v(0),
                            path[i].v(1),
                            path[i].midheight[1] );
                        scheme.bezierTo(m_base*c1, m_base*c2, m_base*path[i].v);
                    }
                    break;
                default:
                    break;
                }
            }
        }

        /* Draw the boundary */
        std::for_each(
            m_tangDom.codomain().popBegin(),
            m_tangDom.codomain().popEnd(),
            [this, &scheme](size_t x) {
                scheme.moveTo(
                    m_base*Eigen::Vector3d(x+0.5, 0.0, 0.0));
                scheme.lineTo(
                    m_base*Eigen::Vector3d(x+0.5, 0.0, m_mvseqs.size()));
            } );
        std::for_each(
            m_tangDom.domain().popBegin(),
            m_tangDom.domain().popEnd(),
            [this, &scheme](size_t x) {
                scheme.moveTo(
                    m_base*Eigen::Vector3d(
                        x+0.5, m_tangDom.vlength(), 0.0));
                scheme.lineTo(
                    m_base*Eigen::Vector3d(
                        x+0.5, m_tangDom.vlength(), m_mvseqs.size()));
            } );

        /* Draw the codomain tangle */{
            AdapterScheme<PathScheme<Eigen::Vector3d>, Eigen::Vector2d> adpscheme(
                &scheme,
                [this, &orig](Eigen::Vector2d const&p) {
                    return m_base.block<3,2>(0,0)*p
                        + m_mvseqs.size()*m_base.col(2);
                } );

            PlTangFigure pltangfig{ m_tangCod,
                    Eigen::Vector2d(1.0, 0.0),
                    Eigen::Vector2d(0.0, 1.0) };

            pltangfig.draw(adpscheme);
            adpscheme.release();
        }
        scheme.restore();
    }

    /**!
     * \section Member functions derived from PathFigure3d
     **/
    virtual void updateProjector(Eigen::Matrix<double,2,3> const &proj) override
    {
        m_critV = (proj*m_base.block<3,2>(0,0)).row(0).adjoint();
        makePath();
    }

protected:
    template <class F>
    static Eigen::Vector3d modifZ(Eigen::Vector3d v, F&& f) {
        static_assert(
            std::is_convertible<
                decltype(std::declval<F&&>()(std::declval<double>())),
                double>::value,
            "The function type should be double(double)");

            v(2) = f(v(2));
        return v;
    }

    std::pair<bool, Eigen::Vector2d> getCrit(char c) const {
        Eigen::Vector2d v0, v1{Eigen::Vector2d(0.5,0.5)}, v2;

        // Parametrize the curves in counter-clockwise direction.
        switch(c) {
        case 'L':
            v0 = Eigen::Vector2d(0.5, 0.0);
            v2 = Eigen::Vector2d(1.0, 0.5);
            break;

        case 'J':
            v0 = Eigen::Vector2d(0.0, 0.5);
            v2 = Eigen::Vector2d(0.5, 0.0);
            break;

        case 'r':
            v0 = Eigen::Vector2d(1.0, 0.5);
            v2 = Eigen::Vector2d(0.5, 1.0);
            break;

        case '7':
            v0 = Eigen::Vector2d(0.5, 1.0);
            v2 = Eigen::Vector2d(0.0, 0.5);
            break;

        default:
            return std::make_pair(false, Eigen::Vector2d{0.0, 0.0});
        }

        double numer = m_critV.dot(v2 - v1);
        double denom = m_critV.dot(v0 - 2.0*v1 + v2);
        double t = numer / denom;

        if (0.0 < t && t <= 1.0)
            return std::make_pair(
                true,
                (t*t*v0 + 2*t*(1-t)*v1 + (1-t)*(1-t)*v2) );
        else
            return std::make_pair(false, Eigen::Vector2d());
    }

    //! Append the identity on the pathgraph.
    //! \param f The predicator which returns true on the cells where the identity are put.
    template <class F>
    inline void putIdentity(PlTang<R,C> const& tang, size_t level, PathGraph& graph, F const& f) const noexcept {
        tang.traverse(
            [&](size_t i, size_t j, char c) {
                if(!f(i,j))
                    return;
                auto crv = getCrit(c);
                if (!crv.first)
                    return;
                auto cellorig = Eigen::Vector3d(i,j,level);
                Eigen::Vector3d beg = Eigen::Matrix<double,3,2>::Identity()*crv.second + cellorig;
                auto end = beg + Eigen::Vector3d(0,0,1);
                graph.connect(
                    findAppend(graph, beg),
                    findAppend(graph, end) );
            } );
    }

    void makePath() {
        // Clear the old paths
        m_paths.clear();

        PlTang<R,C> tang_cur = m_tangDom;
        PathGraph graph;
        std::set<size_t> tmp_keys{};
        std::vector<size_t> levels(m_mvseqs.size(), 0);

        // Counter for the layers.
        size_t q = 0;

        // Traverse all the grouped moves
        for (size_t r = 0; r < m_mvseqs.size(); ++r) {
            auto commBeg = m_mvseqs[r].begin();
            auto commData = noncommHead(
                m_tangCod,
                commBeg,
                m_mvseqs[r].end());

            // Empty move yields the identity.
            if(m_mvseqs[r].empty()) {
                putIdentity(
                    tang_cur, q, graph,
                    [](size_t, size_t) { return true; });
                ++q;
                levels[r] = 1;
                continue;
            }

            // Non-empty move.
            while (commBeg != m_mvseqs[r].end()) {
                PlTangMove<2,2>::MoveElem mv = *commBeg;

                applyMove(tang_cur, mv);

                auto critmap = mv.move.mapVertexIf(
                    [&mv, this](size_t i, size_t j, char c) noexcept
                    -> std::pair<bool,Eigen::Vector2d> {
                        auto crv = getCrit(c);
                        return std::pair<bool,Eigen::Vector2d>(
                            crv.first,
                            crv.second + Eigen::Vector2d(mv.x+i, mv.y+j));
                    } );

                // Connect critical points following the connection graph.
                mv.move.getGraph().forEachEdge(
                    [&](size_t m, size_t n) {
                        auto itrm = critmap.find(m);
                        auto itrn = critmap.find(n);

                        if (itrm == critmap.end() && itrn == critmap.end())
                            return;

                        auto vm = itrm == critmap.end()
                            ? Eigen::Vector3d(mv.x + m%2 + 0.5,
                                              mv.y + m/2 + 0.5,
                                              m >= 4 ? q+levels[r]+1 : q+levels[r] )
                            : Eigen::Vector3d(itrm->second(0),
                                              itrm->second(1),
                                              m >= 4 ? q+levels[r]+1 : q+levels[r] );
                        auto vn = itrn == critmap.end()
                            ? Eigen::Vector3d(mv.x + n%2 + 0.5,
                                              mv.y + n/2 + 0.5,
                                              n >= 4 ? q+levels[r]+1 : q+levels[r] )
                            : Eigen::Vector3d(itrn->second(0),
                                              itrn->second(1),
                                n >= 4 ? q+levels[r]+1 : q+levels[r] );

                        auto keym = findAppend(graph, vm);
                        auto keyn = findAppend(graph, vn);

                        // Connect two vertices via a mediate vertex.
                        auto keymid = graph.append(
                            Eigen::Vector3d(mv.x+1.0, mv.y+1.0, q+levels[r]+0.5));
                        graph.connect(keym, keymid);
                        graph.connect(keyn, keymid);
                        tmp_keys.insert(keymid);
                    } );
                // Advance the cursor
                ++commBeg;

                // If the cursor reaches the non-commutative move;
                if(commBeg == commData.first) {
                    // Draw the identities.
                    putIdentity(
                        tang_cur, q+levels[r], graph,
                        [&commData, &tang_cur](size_t i, size_t j) -> bool {
                            return !commData.second[i+j*tang_cur.hlength()];
                        } );

                    // Advance the count of levels.
                    ++levels[r];
                    // Compute the next cluster of mutually commutative moves.
                    commData = noncommHead(m_tangDom,
                                           commBeg,
                                           m_mvseqs[r].end());
                }
            } // end of while(...)

            // Now the variable levels[r] equals the number of levels of the bordism constructed from the grouped moves.

            // Advance the counter of total levels.
            q += levels[r];

        } // end of traverse of the array m_mvseqs.

        // Hight modificator.
        auto normh = [&levels] (double hei) -> double {
            size_t r = 0, q = 0;
            while(hei > q+levels[r] && r < levels.size()) {
                q+=levels[r];
                ++r;
            }
            return (hei-q)/levels[r] + r;
        };

        while(graph.inhabited()) {
            auto comp = graph.trimComponent(graph.minkey());
            auto mayend = comp.findEnd();
            auto maynode = comp.findKey(
                [end = tmp_keys.end(), &tmp_keys](std::pair<size_t, Eigen::Vector3d> const &v)
                -> bool
                {
                    return tmp_keys.find(v.first) == end;
                } );
            m_paths.emplace_back();

            double prevheight = 0.0;
            bool prev_is_node = true;
            auto key = mayend.first
                ? mayend.second
                : (maynode ? maynode->first : comp.minkey());

            auto flag = comp.trackPathCyc(
                key,
                [&](std::pair<size_t, Eigen::Vector3d> const& v) {
                    bool is_node = tmp_keys.find(v.first) == tmp_keys.end();
                    if(m_paths.back().empty()) {
                        m_paths.back().push_back({
                            bord2::BeginPoint,
                            modifZ(v.second, normh),
                            0.0, 0.0});
                        prevheight = normh(v.second(2));
                    }
                    else if(is_node) {
                        double height = normh(v.second(2));
                        m_paths.back().push_back({
                            prev_is_node ? bord2::Line : bord2::Bezier,
                            modifZ(v.second, normh),
                            prevheight,
                            height < prevheight
                                     ? normh(v.second(2) + 2.0/3)
                                     : normh(v.second(2) - 2.0/3) });
                        prevheight = height;
                    }
                    else {
                        if (normh(v.second(2)) > prevheight)
                            prevheight = normh(v.second(2)+1.0/6);
                        else
                            prevheight = normh(v.second(2)-1.0/6);
                    }
                    prev_is_node = is_node;
                });
        }
    }
};


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/eps.hpp>
#include <simd_test.hpp>

#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/eps.hpp>

STF_CASE_TPL ("eps for IEEE types",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::functional::eps;
  using r_t = decltype(eps(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
 #ifndef STF_NO_INVALIDS
  STF_IEEE_EQUAL(eps(bs::Inf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(eps(bs::Minf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(eps(bs::Nan<T>()), bs::Nan<r_t>());
 #endif

  STF_EQUAL(eps(T{-1}), bs::Eps<r_t>());
  STF_EQUAL(eps(T{1}) , bs::Eps<r_t>());

 #if !defined(STF_NO_DENORMALS)
  STF_EQUAL(eps(T{0}), bs::Mindenormal<r_t>());
 #endif
}

STF_CASE_TPL ("eps on integers",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::functional::eps;
  using r_t = decltype(eps(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(eps(T(-1)), T{1});
  STF_EQUAL(eps(T(0)) , T{1});
  STF_EQUAL(eps(T(-1)), T{1});
  STF_EQUAL(eps(T(42)), T{1});
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Rice University
*  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 the Rice University 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: Javier V. Gomez, Mark Moll */

#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/cforest/CForest.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <boost/program_options.hpp>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace ot = ompl::tools;
namespace po = boost::program_options;

ompl::base::OptimizationObjectivePtr getPathLengthObjective(const ompl::base::SpaceInformationPtr& si)
{
    return ompl::base::OptimizationObjectivePtr(new ompl::base::PathLengthOptimizationObjective(si));
}

bool isStateValid(double radiusSquared, const ob::State *state)
{
    const ob::SE2StateSpace::StateType *s = state->as<ob::SE2StateSpace::StateType>();
    double x=s->getX(), y=s->getY();
    x = std::abs(x - std::floor(x));
    y = std::abs(y - std::floor(y));
    x = std::min(x, 1. - x);
    y = std::min(y, 1. - y);
    return x*x + y*y > radiusSquared;
}

int main(int argc, char **argv)
{
    int distance, gridLimit, runCount;
    double obstacleRadius, turningRadius, runtimeLimit;

    ob::StateSpacePtr space(new ob::SE2StateSpace());

    po::options_description desc("Options");

    desc.add_options()
        ("help", "show help message")
        ("dubins", "use Dubins state space")
        ("dubinssym", "use symmetrized Dubins state space")
        ("reedsshepp", "use Reeds-Shepp state space")
        ("distance", po::value<int>(&distance)->default_value(3), "integer grid distance between start and goal")
        ("obstacle-radius", po::value<double>(&obstacleRadius)->default_value(.25), "radius of obstacles")
        ("turning-radius", po::value<double>(&turningRadius)->default_value(.5), "turning radius of robot (ignored for default point robot)")
        ("grid-limit", po::value<int>(&gridLimit)->default_value(10), "size of the grid")
        ("runtime-limit", po::value<double>(&runtimeLimit)->default_value(2), "time limit for every test")
        ("run-count", po::value<int>(&runCount)->default_value(100), "number of times to run each planner")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
        std::cout << desc << "\n";
        return 1;
    }

    if (vm.count("dubins"))
        space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius));
    if (vm.count("dubinssym"))
        space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius, true));
    if (vm.count("reedsshepp"))
        space = ob::StateSpacePtr(new ob::ReedsSheppStateSpace(turningRadius));

    // set the bounds for the R^2 part of SE(2)
    ob::RealVectorBounds bounds(2);
    bounds.setLow(-.5 * gridLimit);
    bounds.setHigh(.5 * gridLimit);
    space->as<ob::SE2StateSpace>()->setBounds(bounds);

    // define a simple setup class
    og::SimpleSetup ss(space);

    // set state validity checking for this space
    ss.setStateValidityChecker(std::bind(&isStateValid, obstacleRadius*obstacleRadius, std::placeholders::_1));

    // define start & goal states
    ob::ScopedState<ob::SE2StateSpace> start(space), goal(space);
    start->setXY(0., 0.5);
    start->setYaw(0.);
    goal->setXY(0., (double)distance + .5);
    goal->setYaw(0);
    ss.setStartAndGoalStates(start, goal);

    // setting collision checking resolution to 0.05 (absolute)
    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit);
    ss.getProblemDefinition()->setOptimizationObjective(getPathLengthObjective(ss.getSpaceInformation()));

    // by default, use the Benchmark class
    double memoryLimit = 4096;
    ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount);
    ot::Benchmark b(ss, "CircleGrid");

    b.addPlanner(ob::PlannerPtr(new og::RRTstar(ss.getSpaceInformation())));
    b.addPlanner(ob::PlannerPtr(new og::CForest(ss.getSpaceInformation())));
    b.benchmark(request);
    b.saveResultsToFile("circleGrid.log");

    exit(0);
}


//==================================================================================================
/*!
  @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_CONSTANT_THREE_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_THREE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the constant three.

    @return The Three constant for the proper type
  **/
  template<typename T> T Three();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant three.

      @return The Three constant for the proper type
    **/
    Value Three();
  }
} }
#endif

#include <boost/simd/constant/scalar/three.hpp>
#include <boost/simd/constant/simd/three.hpp>

#endif


#include <iostream>
#include <stdio.h>

#include <boost/chrono.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometries/linestring.hpp>

#include <opencv_workbench/wb/WB.h>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

#include <opencv_workbench/wb/Blob.h>

// to store queries results
#include <list>

using namespace std;

typedef bg::model::point<double, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef std::pair<box, wb::Blob*> value;
typedef bgi::rtree< value, bgi::dynamic_rstar > rtree_t;

//bool is_within(value const& v, point, double dist)
//{
//     //return v.is_within();
//     return false;
//}

void region_query(rtree_t &rtree, double eps, wb::Blob *b, std::list<wb::Blob*> &neighbors)
{
     std::list<value> returned_values;
     //point sought = point(b->centroid().x, b->centroid().y);
     //rtree.query(bgi::nearest(sought, 20), std::back_inserter(returned_values));

     // find values intersecting some area defined by a box
     box bounds = rtree.bounds();
     rtree.query(bgi::intersects(bounds), std::back_inserter(returned_values));

     cout << "Blob ID: " << b->id() << " has neighbors... (" << returned_values.size() << ")" << endl;

     for (std::list<value>::iterator it = returned_values.begin(); it != returned_values.end(); it++) {
          double dist = wb::distance<cv::Point2f>(b->centroid(), it->second->centroid());
          if (dist > eps) {
               //break;
               continue;
          }
          cout << "\t" << it->second->id() << " : " << dist << endl;
          neighbors.push_back(it->second);
     }
}

int main()
{
     std::list<wb::Blob*> blobs_;

     wb::Blob *b1 = new wb::Blob();
     b1->set_id(1);
     b1->set_bbox(BoundingBox(0,2,0,2));
     b1->set_centroid(cv::Point(1,1));
     blobs_.push_back(b1);

     wb::Blob *b2 = new wb::Blob();
     b2->set_id(2);
     b2->set_bbox(BoundingBox(5,7,5,7));
     b2->set_centroid(cv::Point(6,6));
     blobs_.push_back(b2);

     wb::Blob *b3 = new wb::Blob();
     b3->set_id(3);
     b3->set_bbox(BoundingBox(8,9,5,6));
     b3->set_centroid(cv::Point(8,5));
     blobs_.push_back(b3);

     wb::Blob *b4 = new wb::Blob();
     b4->set_id(4);
     b4->set_bbox(BoundingBox(7,9,1,2));
     b4->set_centroid(cv::Point(8,1));
     blobs_.push_back(b4);

     wb::Blob *b5 = new wb::Blob();
     b5->set_id(5);
     b5->set_bbox(BoundingBox(0,3,4,7));
     b5->set_centroid(cv::Point(1,5));
     blobs_.push_back(b5);

     wb::Blob *b6 = new wb::Blob();
     b6->set_id(6);
     b6->set_bbox(BoundingBox(1,2,5,6));
     b6->set_centroid(cv::Point(1,5));
     blobs_.push_back(b6);

     // Test distance between rectangles
     for (std::list<wb::Blob*>::iterator it1 = blobs_.begin();
          it1 != blobs_.end(); it1++ ) {
          for (std::list<wb::Blob*>::iterator it2 = blobs_.begin();
               it2 != blobs_.end(); it2++ ) {
               double dist = BoundingBox::distance((*it1)->bbox(), (*it2)->bbox());
               cout << "Blobs: " << (*it1)->id() << " and " << (*it2)->id() << " : " << dist << endl;
          }
     }

     ////////////////////
     // Boost RTree
     ////////////////////

     // Build the R* Tree
     rtree_t rtree(bgi::dynamic_rstar(blobs_.size()));
     for (std::list<wb::Blob*>::iterator it = blobs_.begin(); it != blobs_.end();
          it++ ) {
          // create a box
          box b(point((*it)->bbox().xmin(), (*it)->bbox().ymin()), point((*it)->bbox().xmax(), (*it)->bbox().ymax()));
          // insert new value
          rtree.insert(std::make_pair(b, *it));
     }

     unsigned int MinPts = 3;
     double eps = 8;

     // 0 - unset, -1 is noise, 1+ are cluster ids.
     std::map<int, std::list<wb::Blob*> > clusters_;

     int UNSET_ID = 0;
     int NOISE_ID = -1;
     int cluster_id = 1;
     // for each point P in dataset D
     for (std::list<wb::Blob*>::iterator it1 = blobs_.begin(); it1 != blobs_.end();
          it1++ ) {

          if ((*it1)->visited()) {
               continue;
          }
          (*it1)->set_visited(true);

          std::list<wb::Blob*> neighbors;
          region_query(rtree,eps,*it1,neighbors);

          if (neighbors.size() < MinPts) {
               (*it1)->set_cluster_id(NOISE_ID);
               clusters_[NOISE_ID].push_back(*it1);
               cout << "NOISE - Blob ID: " << (*it1)->id() << endl;
          } else {
               //add P to cluster C
               (*it1)->set_cluster_id(cluster_id);
               clusters_[cluster_id].push_back(*it1);

               //for each point P' in NeighborPts
               std::list<wb::Blob*>::iterator it2 = neighbors.begin();
               while (it2 != neighbors.end()) {
                    //if P' is not visited
                    if (!(*it2)->visited()) {
                         (*it2)->set_visited(true); //mark P' as visited

                         // Get neighbors2
                         std::list<wb::Blob*> neighbors2;
                         region_query(rtree,eps,*it2,neighbors2);
                         if (neighbors2.size() >= MinPts) {
                              //NeighborPts = NeighborPts joined with NeighborPts'
                              for (std::list<wb::Blob*>::iterator it_copy = neighbors2.begin();
                                   it_copy != neighbors2.end(); it_copy++) {
                                   neighbors.push_back(*it_copy);
                              }
                         }
                    }

                    //if P' is not yet member of any cluster
                    if ((*it2)->cluster_id() == UNSET_ID) {
                         (*it2)->set_cluster_id(cluster_id);
                         clusters_[cluster_id].push_back(*it2);  //add P' to cluster C
                    }
                    it2++;
               }
               cluster_id++; // Next cluster id
          }
     }

     // Print out cluster results:
     for (std::map<int, std::list<wb::Blob*> >::iterator it1 = clusters_.begin();
               it1 != clusters_.end(); it1++) {

          if (it1->first == NOISE_ID) {
               cout << "NOISE IDS: " << endl;
          } else if (it1->first == UNSET_ID) {
               cout << "WARNING: UNSET_ID FOUND: " << endl;
          } else {
               cout << "Cluster: " << it1->first << endl;
          }

          for (std::list<wb::Blob*>::iterator it2 = it1->second.begin();
               it2 != it1->second.end(); it2++) {
               cout << "\tBlob: " << (*it2)->id() << endl;
          }
     }

     /// box bounds = rtree.bounds();
     ///
     /// // find values intersecting some area defined by a box
     /// std::list<value> result_s;
     /// rtree.query(bgi::intersects(bounds), std::back_inserter(result_s));
     ///
     /// for (std::list<value>::iterator it = result_s.begin(); it != result_s.end(); it++) {
     ///      cout << "ID: " << it->second->id() << endl;
     ///      it->second->set_id( it->second->id()+1 );
     /// }
     ///
     /// result_s.clear();
     /// rtree.query(bgi::intersects(bounds), std::back_inserter(result_s));
     ///
     /// for (std::list<value>::iterator it = result_s.begin(); it != result_s.end(); it++) {
     ///      cout << "ID: " << it->second->id() << endl;
     /// }

     return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/operator/include/functions/divides.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <vector>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/cover.hpp>

NT2_TEST_CASE_TPL ( divides_all_types,  NT2_SIMD_TYPES)
{
  using nt2::divides;
  using nt2::tag::divides_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                nT;

  typedef typename nt2::meta::call<divides_(T, T)>::type r_t;

  // random verifications
  nt2::uint32_t NR  = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR);
  nt2::roll(in1, nt2::Valmin<T>()/2, nt2::Valmax<T>()/2);
  nt2::roll(in2, nt2::One<T>(), nt2::Valmax<T>()/2);

  std::vector<r_t> ref(NR);
  for(nt2::uint32_t i=0; i < NR ; ++i) ref[i] = divides(in1[i], in2[i]);
  NT2_COVER_ULP_EQUAL(divides_, ((nT, in1))((nT, in2)), ref, 0);

}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_BarnUnit.hpp
//! \author Alex Robinson
//! \brief  The barn unit declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_BARN_UNIT_HPP
#define UTILITY_BARN_UNIT_HPP

// Boost Includes
#include <boost/units/systems/cgs/area.hpp>
#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/io.hpp>

namespace Utility{

namespace Units{

typedef boost::units::make_scaled_unit<boost::units::cgs::area, boost::units::scale<10,boost::units::static_rational<-24> > >::type Barn;

BOOST_UNITS_STATIC_CONSTANT( barn, Barn );

} // end Units namespace

} // end Utility namespace

namespace boost{

namespace units{

std::string name_string( const Utility::Units::Barn& )
{
  return "Barn";
}

std::string symbol_string( const Utility::Units::Barn& )
{
  return "b";
}

} // end units namespace

} // end boost namespace

#endif // end UTILITY_BARN_UNIT_HPP

//---------------------------------------------------------------------------//
// end Utility_BarnUnit.hpp
//---------------------------------------------------------------------------//


#include "prepare_test_data.h"

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

static void vec4_add(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(0.0f, 0.0f, 0.0f, 0.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = testData[0] + testData[1];
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_add_loop(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(0.0f, 0.0f, 0.0f, 0.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        for (const auto& vec : testData) {
            res += vec;
        }
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_add_accumulate(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(0.0f, 0.0f, 0.0f, 0.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = std::accumulate(testData.begin(), testData.end(), Eigen::Vector4f(0.0f, 0.0f, 0.0f, 0.0f));
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_mult(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(0.0f, 0.0f, 0.0f, 0.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = testData[0].cwiseProduct(testData[1]);
        benchmark::ClobberMemory();
    }

    benchmark::DoNotOptimize(res);
}

static void vec4_mult_scalar(benchmark::State& state) {
    const auto testData = prepare_test_data_init_list<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(0.0f, 0.0f, 0.0f, 0.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = testData[0] * testData[1].y();
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_mult_loop(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(1.0f, 1.0f, 1.0f, 1.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        for (const auto& vec : testData) {
            res = res.cwiseProduct(vec);
        }
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_mult_loop_scalar(benchmark::State& state) {
    const auto testData = prepare_test_data_init_list<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(1.0f, 1.0f, 1.0f, 1.0f);

    for (auto _ : state) {
        for (const auto& vec : testData) {
            res *= vec[1];
        }
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_mult_accumulate(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(1.0f, 1.0f, 1.0f, 1.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = std::accumulate(testData.begin(),
                              testData.end(),
                              Eigen::Vector4f(1.0f, 1.0f, 1.0f, 1.0f),
                              [](Eigen::Vector4f lhs, Eigen::Vector4f rhs) { return lhs.cwiseProduct(rhs);
        });
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

Eigen::Vector4f compute_1(float a, float b)
{

    Eigen::Vector4f const av(a, b, b, a);
    Eigen::Vector4f const bv(a, b, a, b);

    Eigen::Vector4f const cv(bv.cwiseProduct(av));
    Eigen::Vector4f const dv(av + cv);

    return dv;
}

Eigen::Vector4f compute_2(float a, float b)
{
    Eigen::Vector4f const c(b * a, b * a, b * a, b * a);
    Eigen::Vector4f const d = Eigen::Vector4f(a, a, a, a) + c;

    return d;
}

Eigen::Vector4f compute_3(Eigen::Vector4f a, Eigen::Vector4f b)
{
    return a.cwiseProduct(b) + a.cwiseProduct(b);
}

static void vec4_compute_1(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(1.0f, 1.0f, 1.0f, 1.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = compute_1(testData[0].x(), testData[1].y());
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_compute_2(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(1.0f, 1.0f, 1.0f, 1.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = compute_2(testData[0].x(), testData[1].y());
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void vec4_compute_3(benchmark::State& state) {
    const auto testData = prepare_test_data<Eigen::Vector4f>(state.range(0));

    Eigen::Vector4f res(1.0f, 1.0f, 1.0f, 1.0f);

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = compute_3(testData[0], testData[1]);
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}
// Register the function as a benchmark
BENCHMARK(vec4_add)->Arg(2);
BENCHMARK(vec4_add_loop)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);
BENCHMARK(vec4_add_accumulate)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);
BENCHMARK(vec4_mult)->Arg(2);
BENCHMARK(vec4_mult_scalar)->Arg(2);
BENCHMARK(vec4_mult_loop)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);
BENCHMARK(vec4_mult_loop_scalar)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);
BENCHMARK(vec4_mult_accumulate)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);
BENCHMARK(vec4_compute_1)->Arg(2);
BENCHMARK(vec4_compute_2)->Arg(2);
BENCHMARK(vec4_compute_3)->Arg(2);

// Run the benchmark
BENCHMARK_MAIN();


#ifndef MESHMONK_HPP
#define MESHMONK_HPP

#include <iostream>
#include <stdio.h>
#include <math.h>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/IO/reader/OBJReader.hh>
#include <OpenMesh/Core/IO/writer/OBJWriter.hh>
#include <OpenMesh/Tools/Decimater/DecimaterT.hh>
#include <OpenMesh/Tools/Decimater/ModQuadricT.hh>
#include <Eigen/Dense>
#include "src/PyramidNonrigidRegistration.hpp"
#include "src/RigidRegistration.hpp"
#include "src/NonrigidRegistration.hpp"
#include "src/InlierDetector.hpp"
#include "src/CorrespondenceFilter.hpp"
#include "src/SymmetricCorrespondenceFilter.hpp"
#include "src/RigidTransformer.hpp"
#include "src/ViscoElasticTransformer.hpp"
#include "src/Downsampler.hpp"
#include "src/ScaleShifter.hpp"
#include "global.hpp"
#include "src/helper_functions.hpp"

typedef OpenMesh::DefaultTraits MyTraits;
typedef OpenMesh::TriMesh_ArrayKernelT<MyTraits>  TriMesh;
typedef OpenMesh::Decimater::DecimaterT<TriMesh>    DecimaterType;
typedef OpenMesh::Decimater::ModQuadricT<TriMesh>::Handle HModQuadric;
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat; //matrix Mx3 of type unsigned int
typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::MatrixX3f Vec3Mat;
typedef Eigen::Matrix4f Mat4Float;

namespace meshmonk{

#ifdef __cplusplus
extern "C"
#endif // __cplusplus
{
    //######################################################################################
    //################################  TEST SHIZZLE  ######################################
    //######################################################################################
    /*
    We're implementing this function simply to test MEX'ing in MATLAB.
    */
    void test_meshmonk_mexing(FeatureMat& floatingFeatures, const FeatureMat& targetFeatures, const float multiplier = 2.0f);

    /*
    Raw data version of test_meshmonk_mexing()
    */
    void test_meshmonk_mexing_raw(float floatingFeaturesRaw[], const float targetFeaturesRaw[],
                                    const size_t numFloatingElements, const size_t numTargetElements,
                                    const float multiplier = 2.0f);

    //######################################################################################
    //################################  REGISTRATION  ######################################
    //######################################################################################
    /*
    Full Pyramid Nonrigid Registration
    This is the function you'll normally want to call to nonrigidly register a floating mesh to a target mesh.
    */
    void pyramid_registration(FeatureMat& floatingFeatures, const FeatureMat& targetFeatures,
                                const FacesMat& floatingFaces, const FacesMat& targetFaces,
                                const VecDynFloat& floatingFlags, const VecDynFloat& targetFlags,
                                const size_t numIterations = 60, const size_t numPyramidLayers = 3,
                                const float downsampleFloatStart = 90, const float downsampleTargetStart = 90,
                                const float downsampleFloatEnd = 0, const float downsampleTargetEnd = 0,
                                const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false,
                                const float inlierKappa = 4.0f, const bool inlierUseOrientation = true,
                                const float transformSigma = 3.0f,
                                const size_t transformNumViscousIterationsStart = 50, const size_t transformNumViscousIterationsEnd = 1,
                                const size_t transformNumElasticIterationsStart = 50, const size_t transformNumElasticIterationsEnd = 1);

    /*
    Standard Nonrigid Registration
    This is the standard nonrigid registration procedure without pyramid approach, so computationally a bit slower.
    */
    void nonrigid_registration(FeatureMat& floatingFeatures, const FeatureMat& targetFeatures,
                                const FacesMat& floatingFaces, const FacesMat& targetFaces,
                                const VecDynFloat& floatingFlags, const VecDynFloat& targetFlags,
                                const size_t numIterations = 60,
                                const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false,
                                const float inlierKappa = 4.0f, const bool inlierUseOrientation = true,
                                const float transformSigma = 3.0f,
                                const size_t transformNumViscousIterationsStart = 50, const size_t transformNumViscousIterationsEnd = 1,
                                const size_t transformNumElasticIterationsStart = 50, const size_t transformNumElasticIterationsEnd = 1);

    /*
    Rigid Registration
    */
    void rigid_registration(FeatureMat& floatingFeatures, const FeatureMat& targetFeatures,
                                const FacesMat& floatingFaces, const FacesMat& targetFaces,
                                const VecDynFloat& floatingFlags, const VecDynFloat& targetFlags,
                                Mat4Float& transformationMatrix,
                                const size_t numIterations = 20,
                                const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false,
                                const float inlierKappa = 4.0f, const bool inlierUseOrientation = true,
                                const bool useScaling = false);

    //######################################################################################
    //############################  REGISTRATION MODULES  ##################################
    //######################################################################################

    //# Correspondences
    void compute_correspondences(const FeatureMat& floatingFeatures, const FeatureMat& targetFeatures,
                                const VecDynFloat& floatingFlags, const VecDynFloat& targetFlags,
                                FeatureMat& correspondingFeatures, VecDynFloat& correspondingFlags,
                                const bool symmetric = true, const size_t numNeighbours = 5,
                                const float flagThreshold = 0.99f, const bool equalizePushPull = false);

    //# Inliers
    void compute_inlier_weights(const FeatureMat& floatingFeatures, const FeatureMat& correspondingFeatures,
                                const VecDynFloat& correspondingFlags, VecDynFloat& inlierWeights,
                                const float kappa = 4.0f, const bool useOrientation = true);

    //# Rigid Transformation
    void compute_rigid_transformation(FeatureMat& floatingFeatures, const FeatureMat& correspondingFeatures,
                                    const VecDynFloat& inlierWeights, Mat4Float& transformationMatrix,
                                    const bool useScaling = false);

    //# Nonrigid Transformation
    void compute_nonrigid_transformation(FeatureMat& floatingFeatures, const FeatureMat& correspondingFeatures,
                                        const FacesMat& floatingFaces, const VecDynFloat& floatingFlags,
                                        const VecDynFloat& inlierWeights,
                                        const size_t numSmoothingNeighbours = 10, const float sigmaSmoothing = 3.0f,
                                        const size_t numViscousIterations = 50, const size_t numElasticIterations = 50);

    //# Downsampler
    void downsample_mesh(const FeatureMat& features, const FacesMat& faces,
                        const VecDynFloat& flags,
                        FeatureMat& downsampledFeatures, FacesMat& downsampledFaces,
                        VecDynFloat& downsampledFlags, VecDynInt& originalIndices,
                        const float downsampleRatio = 0.8f);

    //# ScaleShifter
    //## The scaleshifter is meant to transition from one scale in the pyramid to the next.
    void scale_shift_mesh(const FeatureMat& previousFeatures, const VecDynInt& previousIndices,
                        FeatureMat& newFeatures, const VecDynInt& newIndices);

    //######################################################################################
    //###############################  MESH OPERATIONS  ####################################
    //######################################################################################
    void compute_normals(const Vec3Mat &inPositions, const FacesMat &inFaces,
                        Vec3Mat &outNormals);

    //######################################################################################
    //################################  INPUT/OUTPUT  ######################################
    //######################################################################################

//    void read_obj_files(const std::string floatingMeshPath, const std::string targetMeshPath,
//                        FeatureMat& floatingFeatures, FeatureMat& targetFeatures,
//                        FacesMat& floatingFaces, FacesMat& targetFaces);
//
//    void write_obj_files(FeatureMat& features, FacesMat& faces, const std::string meshPath);

    //######################################################################################
    //################################  MEX WRAPPING  ######################################
    //######################################################################################
    /*
    We're wrapping some functionality in the library so it can be easily mexed in Matlab
    */
    void pyramid_registration_mex(float floatingFeaturesArray[], const float targetFeaturesArray[],
                                const size_t numFloatingElements, const size_t numTargetElements,
                                const int floatingFacesArray[], const int targetFacesArray[],
                                const size_t numFloatingFaces, const size_t numTargetFaces,
                                const float floatingFlagsArray[], const float targetFlagsArray[],
                                const size_t numIterations = 60, const size_t numPyramidLayers = 3,
                                const float downsampleFloatStart = 90, const float downsampleTargetStart = 90,
                                const float downsampleFloatEnd = 0, const float downsampleTargetEnd = 0,
                                const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false,
                                const float inlierKappa = 4.0f, const bool inlierUseOrientation = true,
                                const float transformSigma = 3.0f,
                                const size_t transformNumViscousIterationsStart = 50, const size_t transformNumViscousIterationsEnd = 1,
                                const size_t transformNumElasticIterationsStart = 50, const size_t transformNumElasticIterationsEnd = 1);

    void nonrigid_registration_mex(float floatingFeaturesArray[], const float targetFeaturesArray[],
                                const size_t numFloatingElements, const size_t numTargetElements,
                                const int floatingFacesArray[], const int targetFacesArray[],
                                const size_t numFloatingFaces, const size_t numTargetFaces,
                                const float floatingFlagsArray[], const float targetFlagsArray[],
                                const size_t numIterations = 60,
                                const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false,
                                const float inlierKappa = 4.0f, const bool inlierUseOrientation = true,
                                const float transformSigma = 3.0f,
                                const size_t transformNumViscousIterationsStart = 50, const size_t transformNumViscousIterationsEnd = 1,
                                const size_t transformNumElasticIterationsStart = 50, const size_t transformNumElasticIterationsEnd = 1);

    void rigid_registration_mex(float floatingFeaturesArray[], const float targetFeaturesArray[],
                                const size_t numFloatingElements, const size_t numTargetElements,
                                const int floatingFacesArray[], const int targetFacesArray[],
                                const size_t numFloatingFaces, const size_t numTargetFaces,
                                const float floatingFlagsArray[], const float targetFlagsArray[],
                                float transformationMatrixArray[],
                                const size_t numIterations = 60,
                                const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false,
                                const float inlierKappa = 4.0f, const bool inlierUseOrientation = true,
                                const bool useScaling = false);

    void compute_correspondences_mex(const float floatingFeaturesArray[], const float targetFeaturesArray[],
                                    const size_t numFloatingElements, const size_t numTargetElements,
                                    const float floatingFlagsArray[], const float targetFlagsArray[],
                                    float correspondingFeaturesArray[], float correspondingFlagsArray[],
                                    const bool correspondencesSymmetric = true, const size_t correspondencesNumNeighbours = 5,
                                    const float correspondencesFlagThreshold = 0.99f, const bool correspondencesEqualizePushPull = false);

    void compute_inlier_weights_mex(const float floatingFeaturesArray[], const float correspondingFeaturesArray[],
                                    const size_t numFloatingElements,
                                    const float correspondingFlagsArray[], float inlierWeightsArray[],
                                    const float inlierKappa/*= 4.0f*/, const bool useOrientation/*= true*/);

    void compute_rigid_transformation_mex(float floatingFeaturesArray[], const size_t numFloatingElements,
                                        const float correspondingFeaturesArray[], const float inlierWeightsArray[],
                                        float transformationMatrixArray[],
                                        const bool useScaling /*= false*/);

    void compute_nonrigid_transformation_mex(float floatingFeaturesArray[], const float correspondingFeaturesArray[],
                                            const size_t numFloatingElements,
                                            const int floatingFacesArray[], const size_t numFloatingFaces,
                                            const float floatingFlagsArray[], const float inlierWeightsArray[],
                                            const size_t transformNumNeighbours/*= 10*/, const float transformSigma/*= 3.0f*/,
                                            const size_t transformNumViscousIterations/*= 50*/, const size_t transformNumElasticIterations/*= 50*/);

    void downsample_mesh_mex(const float featuresArray[], const size_t numElements,
                            const int facesArray[], const size_t numFaces,
                            const float flagsArray[],
                            float sampledFeaturesArray[], const size_t numSampledElements,
                            int sampledFacesArray[], const size_t numSampledFaces,
                            float sampledFlagsArray[],
                            int originalIndicesArray[],
                            const float downsampleRatio/* = 0.8f*/);

    void scaleshift_mesh_mex(const float oldFeaturesArray[], const size_t numOldElements,
                            const int oldIndicesArray[],
                            float newFeaturesArray[], const size_t numNewElements,
                            const int newIndicesArray[]);

    void compute_normals_mex(const float positionsArray[], const size_t numElements,
                            const int facesArray[], const size_t numFaces,
                            float normalsArray[]);

#ifdef __cplusplus
}//extern C
#endif // __cplusplus

}//namespace meshmonk

#endif //MESHMONK_HPP