//  Copyright (c) 2013 Christopher Kormanyos
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// This work is based on an earlier work:
// "Algorithm 910: A Portable C++ Multiple-Precision System for Special-Function Calculations",
// in ACM TOMS, {VOL 37, ISSUE 4, (February 2011)} (C) ACM, 2011. http://doi.acm.org/10.1145/1916461.1916469
//
// This header contains implementation details for estimating the zeros
// of cylindrical Bessel and Neumann functions on the positive real axis.
// Support is included for both positive as well as negative order.
// Various methods are used to estimate the roots. These include
// empirical curve fitting and McMahon's asymptotic approximation
// for small order, uniform asymptotic expansion for large order,
// and iteration and root interlacing for negative order.
//
#ifndef _BESSEL_JY_ZERO_2013_01_18_HPP_
  #define _BESSEL_JY_ZERO_2013_01_18_HPP_

  #include <algorithm>
  #include <boost/math/constants/constants.hpp>
  #include <boost/math/special_functions/math_fwd.hpp>
  #include <boost/math/special_functions/cbrt.hpp>
  #include <boost/math/special_functions/detail/airy_ai_bi_zero.hpp>

  namespace boost { namespace math {
  namespace detail
  {
    namespace bessel_zero
    {
      template<class T>
      T equation_nist_10_21_19(const T& v, const T& a)
      {
        // Get the initial estimate of the m'th root of Jv or Yv.
        // This subroutine is used for the order m with m > 1.
        // The order m has been used to create the input parameter a.

        // This is Eq. 10.21.19 in the NIST Handbook.
        const T mu                  = (v * v) * 4U;
        const T mu_minus_one        = mu - T(1);
        const T eight_a_inv         = T(1) / (a * 8U);
        const T eight_a_inv_squared = eight_a_inv * eight_a_inv;

        const T term3 = ((mu_minus_one *  4U) *     ((mu *    7U) -     T(31U) )) / 3U;
        const T term5 = ((mu_minus_one * 32U) *   ((((mu *   83U) -    T(982U) ) * mu) +    T(3779U) )) / 15U;
        const T term7 = ((mu_minus_one * 64U) * ((((((mu * 6949U) - T(153855UL)) * mu) + T(1585743UL)) * mu) - T(6277237UL))) / 105U;

        return a + ((((                      - term7
                       * eight_a_inv_squared - term5)
                       * eight_a_inv_squared - term3)
                       * eight_a_inv_squared - mu_minus_one)
                       * eight_a_inv);
      }

      template<typename T>
      class equation_as_9_3_39_and_its_derivative
      {
      public:
        equation_as_9_3_39_and_its_derivative(const T& zt) : zeta(zt) { }

        boost::math::tuple<T, T> operator()(const T& z) const
        {
          BOOST_MATH_STD_USING // ADL of std names, needed for acos, sqrt.

          // Return the function of zeta that is implicitly defined
          // in A&S Eq. 9.3.39 as a function of z. The function is
          // returned along with its derivative with respect to z.

          const T zsq_minus_one_sqrt = sqrt((z * z) - T(1));

          const T the_function(
              zsq_minus_one_sqrt
            - (  acos(T(1) / z) + ((T(2) / 3U) * (zeta * sqrt(zeta)))));

          const T its_derivative(zsq_minus_one_sqrt / z);

          return boost::math::tuple<T, T>(the_function, its_derivative);
        }

      private:
        const equation_as_9_3_39_and_its_derivative& operator=(const equation_as_9_3_39_and_its_derivative&);
        const T zeta;
      };

      template<class T>
      static T equation_as_9_5_26(const T& v, const T& ai_bi_root)
      {
        BOOST_MATH_STD_USING // ADL of std names, needed for log, sqrt.

        // Obtain the estimate of the m'th zero of Jv or Yv.
        // The order m has been used to create the input parameter ai_bi_root.
        // Here, v is larger than about 2.2. The estimate is computed
        // from Abramowitz and Stegun Eqs. 9.5.22 and 9.5.26, page 371.
        //
        // The inversion of z as a function of zeta is mentioned in the text
        // following A&S Eq. 9.5.26. Here, we accomplish the inversion by
        // performing a Taylor expansion of Eq. 9.3.39 for large z to order 2
        // and solving the resulting quadratic equation, thereby taking
        // the positive root of the quadratic.
        // In other words: (2/3)(-zeta)^(3/2) approx = z + 1/(2z) - pi/2.
        // This leads to: z^2 - [(2/3)(-zeta)^(3/2) + pi/2]z + 1/2 = 0.
        //
        // With this initial estimate, Newton-Raphson iteration is used
        // to refine the value of the estimate of the root of z
        // as a function of zeta.

        const T v_pow_third(boost::math::cbrt(v));
        const T v_pow_minus_two_thirds(T(1) / (v_pow_third * v_pow_third));

        // Obtain zeta using the order v combined with the m'th root of
        // an airy function, as shown in  A&S Eq. 9.5.22.
        const T zeta = v_pow_minus_two_thirds * (-ai_bi_root);

        const T zeta_sqrt = sqrt(zeta);

        // Set up a quadratic equation based on the Taylor series
        // expansion mentioned above.
        const T b = -((((zeta * zeta_sqrt) * 2U) / 3U) + boost::math::constants::half_pi<T>());

        // Solve the quadratic equation, taking the positive root.
        const T z_estimate = (-b + sqrt((b * b) - T(2))) / 2U;

        // Establish the range, the digits, and the iteration limit
        // for the upcoming root-finding.
        const T range_zmin = (std::max<T>)(z_estimate - T(1), T(1));
        const T range_zmax = z_estimate + T(1);

        const int my_digits10 = static_cast<int>(static_cast<float>(boost::math::tools::digits<T>() * 0.301F));

        // Select the maximum allowed iterations based on the number
        // of decimal digits in the numeric type T, being at least 12.
        const boost::uintmax_t iterations_allowed = static_cast<boost::uintmax_t>((std::max)(12, my_digits10 * 2));

        boost::uintmax_t iterations_used = iterations_allowed;

        // Calculate the root of z as a function of zeta.
        const T z = boost::math::tools::newton_raphson_iterate(
          boost::math::detail::bessel_zero::equation_as_9_3_39_and_its_derivative<T>(zeta),
          z_estimate,
          range_zmin,
          range_zmax,
          (std::min)(boost::math::tools::digits<T>(), boost::math::tools::digits<float>()),
          iterations_used);

        static_cast<void>(iterations_used);

        // Continue with the implementation of A&S Eq. 9.3.39.
        const T zsq_minus_one      = (z * z) - T(1);
        const T zsq_minus_one_sqrt = sqrt(zsq_minus_one);

        // This is A&S Eq. 9.3.42.
        const T b0_term_5_24 = T(5) / ((zsq_minus_one * zsq_minus_one_sqrt) * 24U);
        const T b0_term_1_8  = T(1) / ( zsq_minus_one_sqrt * 8U);
        const T b0_term_5_48 = T(5) / ((zeta * zeta) * 48U);

        const T b0 = -b0_term_5_48 + ((b0_term_5_24 + b0_term_1_8) / zeta_sqrt);

        // This is the second line of A&S Eq. 9.5.26 for f_k with k = 1.
        const T f1 = ((z * zeta_sqrt) * b0) / zsq_minus_one_sqrt;

        // This is A&S Eq. 9.5.22 expanded to k = 1 (i.e., one term in the series).
        return (v * z) + (f1 / v);
      }

      namespace cyl_bessel_j_zero_detail
      {
        template<class T>
        T equation_nist_10_21_40_a(const T& v)
        {
          const T v_pow_third(boost::math::cbrt(v));
          const T v_pow_minus_two_thirds(T(1) / (v_pow_third * v_pow_third));

          return v * (((((                         + T(0.043)
                          * v_pow_minus_two_thirds - T(0.0908))
                          * v_pow_minus_two_thirds - T(0.00397))
                          * v_pow_minus_two_thirds + T(1.033150))
                          * v_pow_minus_two_thirds + T(1.8557571))
                          * v_pow_minus_two_thirds + T(1));
        }

        template<class T, class Policy>
        class function_object_jv
        {
        public:
          function_object_jv(const T& v,
                             const Policy& pol) : my_v(v),
                                                  my_pol(pol) { }

          T operator()(const T& x) const
          {
            return boost::math::cyl_bessel_j(my_v, x, my_pol);
          }

        private:
          const T my_v;
          const Policy& my_pol;
          const function_object_jv& operator=(const function_object_jv&);
        };

        template<class T, class Policy>
        class function_object_jv_and_jv_prime
        {
        public:
          function_object_jv_and_jv_prime(const T& v,
                                          const bool order_is_zero,
                                          const Policy& pol) : my_v(v),
                                                               my_order_is_zero(order_is_zero),
                                                               my_pol(pol) { }

          boost::math::tuple<T, T> operator()(const T& x) const
          {
            // Obtain Jv(x) and Jv'(x).
            // Chris's original code called the Bessel function implementation layer direct,
            // but that circumvented optimizations for integer-orders.  Call the documented
            // top level functions instead, and let them sort out which implementation to use.
            T j_v;
            T j_v_prime;

            if(my_order_is_zero)
            {
              j_v       =  boost::math::cyl_bessel_j(0, x, my_pol);
              j_v_prime = -boost::math::cyl_bessel_j(1, x, my_pol);
            }
            else
            {
                      j_v       = boost::math::cyl_bessel_j(  my_v,      x, my_pol);
              const T j_v_m1     (boost::math::cyl_bessel_j(T(my_v - 1), x, my_pol));
                      j_v_prime = j_v_m1 - ((my_v * j_v) / x);
            }

            // Return a tuple containing both Jv(x) and Jv'(x).
            return boost::math::make_tuple(j_v, j_v_prime);
          }

        private:
          const T my_v;
          const bool my_order_is_zero;
          const Policy& my_pol;
          const function_object_jv_and_jv_prime& operator=(const function_object_jv_and_jv_prime&);
        };

        template<class T> bool my_bisection_unreachable_tolerance(const T&, const T&) { return false; }

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

          // Compute an estimate of the m'th root of cyl_bessel_j.

          T guess;

          // There is special handling for negative order.
          if(v < 0)
          {
            if((m == 1) && (v > -0.5F))
            {
              // For small, negative v, use the results of empirical curve fitting.
              // Mathematica(R) session for the coefficients:
              //  Table[{n, BesselJZero[n, 1]}, {n, -(1/2), 0, 1/10}]
              //  N[%, 20]
              //  Fit[%, {n^0, n^1, n^2, n^3, n^4, n^5, n^6}, n]
              guess = (((((    - T(0.2321156900729)
                           * v - T(0.1493247777488))
                           * v - T(0.15205419167239))
                           * v + T(0.07814930561249))
                           * v - T(0.17757573537688))
                           * v + T(1.542805677045663))
                           * v + T(2.40482555769577277);

              return guess;
            }

            // Create the positive order and extract its positive floor integer part.
            const T vv(-v);
            const T vv_floor(floor(vv));

            // The to-be-found root is bracketed by the roots of the
            // Bessel function whose reflected, positive integer order
            // is less than, but nearest to vv.

            T root_hi = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess(vv_floor, m, pol);
            T root_lo;

            if(m == 1)
            {
              // The estimate of the first root for negative order is found using
              // an adaptive range-searching algorithm.
              root_lo = T(root_hi - 0.1F);

              const bool hi_end_of_bracket_is_negative = (boost::math::cyl_bessel_j(v, root_hi, pol) < 0);

              while((root_lo > boost::math::tools::epsilon<T>()))
              {
                const bool lo_end_of_bracket_is_negative = (boost::math::cyl_bessel_j(v, root_lo, pol) < 0);

                if(hi_end_of_bracket_is_negative != lo_end_of_bracket_is_negative)
                {
                  break;
                }

                root_hi = root_lo;

                // Decrease the lower end of the bracket using an adaptive algorithm.
                if(root_lo > 0.5F)
                {
                  root_lo -= 0.5F;
                }
                else
                {
                  root_lo *= 0.75F;
                }
              }
            }
            else
            {
              root_lo = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess(vv_floor, m - 1, pol);
            }

            // Perform several steps of bisection iteration to refine the guess.
            boost::uintmax_t number_of_iterations(12U);

            // Do the bisection iteration.
            const boost::math::tuple<T, T> guess_pair =
               boost::math::tools::bisect(
                  boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::function_object_jv<T, Policy>(v, pol),
                  root_lo,
                  root_hi,
                  boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::my_bisection_unreachable_tolerance<T>,
                  number_of_iterations);

            return (boost::math::get<0>(guess_pair) + boost::math::get<1>(guess_pair)) / 2U;
          }

          if(m == 1U)
          {
            // Get the initial estimate of the first root.

            if(v < 2.2F)
            {
              // For small v, use the results of empirical curve fitting.
              // Mathematica(R) session for the coefficients:
              //  Table[{n, BesselJZero[n, 1]}, {n, 0, 22/10, 1/10}]
              //  N[%, 20]
              //  Fit[%, {n^0, n^1, n^2, n^3, n^4, n^5, n^6}, n]
              guess = (((((    - T(0.0008342379046010)
                           * v + T(0.007590035637410))
                           * v - T(0.030640914772013))
                           * v + T(0.078232088020106))
                           * v - T(0.169668712590620))
                           * v + T(1.542187960073750))
                           * v + T(2.4048359915254634);
            }
            else
            {
              // For larger v, use the first line of Eqs. 10.21.40 in the NIST Handbook.
              guess = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::equation_nist_10_21_40_a(v);
            }
          }
          else
          {
            if(v < 2.2F)
            {
              // Use Eq. 10.21.19 in the NIST Handbook.
              const T a(((v + T(m * 2U)) - T(0.5)) * boost::math::constants::half_pi<T>());

              guess = boost::math::detail::bessel_zero::equation_nist_10_21_19(v, a);
            }
            else
            {
              // Get an estimate of the m'th root of airy_ai.
              const T airy_ai_root(boost::math::detail::airy_zero::airy_ai_zero_detail::initial_guess<T>(m));

              // Use Eq. 9.5.26 in the A&S Handbook.
              guess = boost::math::detail::bessel_zero::equation_as_9_5_26(v, airy_ai_root);
            }
          }

          return guess;
        }
      } // namespace cyl_bessel_j_zero_detail

      namespace cyl_neumann_zero_detail
      {
        template<class T>
        T equation_nist_10_21_40_b(const T& v)
        {
          const T v_pow_third(boost::math::cbrt(v));
          const T v_pow_minus_two_thirds(T(1) / (v_pow_third * v_pow_third));

          return v * (((((                         - T(0.001)
                          * v_pow_minus_two_thirds - T(0.0060))
                          * v_pow_minus_two_thirds + T(0.01198))
                          * v_pow_minus_two_thirds + T(0.260351))
                          * v_pow_minus_two_thirds + T(0.9315768))
                          * v_pow_minus_two_thirds + T(1));
        }

        template<class T, class Policy>
        class function_object_yv
        {
        public:
          function_object_yv(const T& v,
                             const Policy& pol) : my_v(v),
                                                  my_pol(pol) { }

          T operator()(const T& x) const
          {
            return boost::math::cyl_neumann(my_v, x, my_pol);
          }

        private:
          const T my_v;
          const Policy& my_pol;
          const function_object_yv& operator=(const function_object_yv&);
        };

        template<class T, class Policy>
        class function_object_yv_and_yv_prime
        {
        public:
          function_object_yv_and_yv_prime(const T& v,
                                          const Policy& pol) : my_v(v),
                                                               my_pol(pol) { }

          boost::math::tuple<T, T> operator()(const T& x) const
          {
            const T half_epsilon(boost::math::tools::epsilon<T>() / 2U);

            const bool order_is_zero = ((my_v > -half_epsilon) && (my_v < +half_epsilon));

            // Obtain Yv(x) and Yv'(x).
            // Chris's original code called the Bessel function implementation layer direct,
            // but that circumvented optimizations for integer-orders.  Call the documented
            // top level functions instead, and let them sort out which implementation to use.
            T y_v;
            T y_v_prime;

            if(order_is_zero)
            {
              y_v       =  boost::math::cyl_neumann(0, x, my_pol);
              y_v_prime = -boost::math::cyl_neumann(1, x, my_pol);
            }
            else
            {
                      y_v       = boost::math::cyl_neumann(  my_v,      x, my_pol);
              const T y_v_m1     (boost::math::cyl_neumann(T(my_v - 1), x, my_pol));
                      y_v_prime = y_v_m1 - ((my_v * y_v) / x);
            }

            // Return a tuple containing both Yv(x) and Yv'(x).
            return boost::math::make_tuple(y_v, y_v_prime);
          }

        private:
          const T my_v;
          const Policy& my_pol;
          const function_object_yv_and_yv_prime& operator=(const function_object_yv_and_yv_prime&);
        };

        template<class T> bool my_bisection_unreachable_tolerance(const T&, const T&) { return false; }

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

          // Compute an estimate of the m'th root of cyl_neumann.

          T guess;

          // There is special handling for negative order.
          if(v < 0)
          {
            // Create the positive order and extract its positive floor and ceiling integer parts.
            const T vv(-v);
            const T vv_floor(floor(vv));

            // The to-be-found root is bracketed by the roots of the
            // Bessel function whose reflected, positive integer order
            // is less than, but nearest to vv.

            // The special case of negative, half-integer order uses
            // the relation between Yv and spherical Bessel functions
            // in order to obtain the bracket for the root.
            // In these special cases, cyl_neumann(-n/2, x) = sph_bessel_j(+n/2, x)
            // for v = -n/2.

            T root_hi;
            T root_lo;

            if(m == 1)
            {
              // The estimate of the first root for negative order is found using
              // an adaptive range-searching algorithm.
              // Take special precautions for the discontinuity at negative,
              // half-integer orders and use different brackets above and below these.
              if(T(vv - vv_floor) < 0.5F)
              {
                root_hi = boost::math::detail::bessel_zero::cyl_neumann_zero_detail::initial_guess(vv_floor, m, pol);
              }
              else
              {
                root_hi = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess(T(vv_floor + 0.5F), m, pol);
              }

              root_lo = T(root_hi - 0.1F);

              const bool hi_end_of_bracket_is_negative = (boost::math::cyl_neumann(v, root_hi, pol) < 0);

              while((root_lo > boost::math::tools::epsilon<T>()))
              {
                const bool lo_end_of_bracket_is_negative = (boost::math::cyl_neumann(v, root_lo, pol) < 0);

                if(hi_end_of_bracket_is_negative != lo_end_of_bracket_is_negative)
                {
                  break;
                }

                root_hi = root_lo;

                // Decrease the lower end of the bracket using an adaptive algorithm.
                if(root_lo > 0.5F)
                {
                  root_lo -= 0.5F;
                }
                else
                {
                  root_lo *= 0.75F;
                }
              }
            }
            else
            {
              if(T(vv - vv_floor) < 0.5F)
              {
                root_lo  = boost::math::detail::bessel_zero::cyl_neumann_zero_detail::initial_guess(vv_floor, m - 1, pol);
                root_hi = boost::math::detail::bessel_zero::cyl_neumann_zero_detail::initial_guess(vv_floor, m, pol);
                root_lo += 0.01F;
                root_hi += 0.01F;
              }
              else
              {
                root_lo = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess(T(vv_floor + 0.5F), m - 1, pol);
                root_hi = boost::math::detail::bessel_zero::cyl_bessel_j_zero_detail::initial_guess(T(vv_floor + 0.5F), m, pol);
                root_lo += 0.01F;
                root_hi += 0.01F;
              }
            }

            // Perform several steps of bisection iteration to refine the guess.
            boost::uintmax_t number_of_iterations(12U);

            // Do the bisection iteration.
            const boost::math::tuple<T, T> guess_pair =
               boost::math::tools::bisect(
                  boost::math::detail::bessel_zero::cyl_neumann_zero_detail::function_object_yv<T, Policy>(v, pol),
                  root_lo,
                  root_hi,
                  boost::math::detail::bessel_zero::cyl_neumann_zero_detail::my_bisection_unreachable_tolerance<T>,
                  number_of_iterations);

            return (boost::math::get<0>(guess_pair) + boost::math::get<1>(guess_pair)) / 2U;
          }

          if(m == 1U)
          {
            // Get the initial estimate of the first root.

            if(v < 2.2F)
            {
              // For small v, use the results of empirical curve fitting.
              // Mathematica(R) session for the coefficients:
              //  Table[{n, BesselYZero[n, 1]}, {n, 0, 22/10, 1/10}]
              //  N[%, 20]
              //  Fit[%, {n^0, n^1, n^2, n^3, n^4, n^5, n^6}, n]
              guess = (((((    - T(0.0025095909235652)
                           * v + T(0.021291887049053))
                           * v - T(0.076487785486526))
                           * v + T(0.159110268115362))
                           * v - T(0.241681668765196))
                           * v + T(1.4437846310885244))
                           * v + T(0.89362115190200490);
            }
            else
            {
              // For larger v, use the second line of Eqs. 10.21.40 in the NIST Handbook.
              guess = boost::math::detail::bessel_zero::cyl_neumann_zero_detail::equation_nist_10_21_40_b(v);
            }
          }
          else
          {
            if(v < 2.2F)
            {
              // Use Eq. 10.21.19 in the NIST Handbook.
              const T a(((v + T(m * 2U)) - T(1.5)) * boost::math::constants::half_pi<T>());

              guess = boost::math::detail::bessel_zero::equation_nist_10_21_19(v, a);
            }
            else
            {
              // Get an estimate of the m'th root of airy_bi.
              const T airy_bi_root(boost::math::detail::airy_zero::airy_bi_zero_detail::initial_guess<T>(m));

              // Use Eq. 9.5.26 in the A&S Handbook.
              guess = boost::math::detail::bessel_zero::equation_as_9_5_26(v, airy_bi_root);
            }
          }

          return guess;
        }
      } // namespace cyl_neumann_zero_detail
    } // namespace bessel_zero
  } } } // namespace boost::math::detail

#endif // _BESSEL_JY_ZERO_2013_01_18_HPP_


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_DEGINRAD_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_DEGINRAD_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup trigo_constant
 * \defgroup trigo_constant_deginrad Deginrad
 * \par Description
 * Constant Deginrad : radian in degree  multiplier, \f$\frac\pi{180}\f$.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/deginrad.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::_deginrad_(A0)>::type
 *     Deginrad();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Deginrad
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    // 8.47842766036889956997e-32
    BOOST_SIMD_CONSTANT_REGISTER( Deginrad, double
                                , 0, 0x3c8efa35
                                , 0x3f91df46a2529d39ll
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Deginrad, Deginrad);

  static const long double long_deginrad = 0.017453292519943295769236907684886l;
}

#endif


#include <cmath>
#include <cstdint>
#include <functional>
#include <iostream>
#include <limits>
#include <map>
#include <random>
#include <stdexcept>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/path_processing.hpp>
#include <common_robotics_utilities/print.hpp>
#include <common_robotics_utilities/simple_astar_search.hpp>
#include <common_robotics_utilities/simple_graph.hpp>
#include <common_robotics_utilities/simple_graph_search.hpp>
#include <common_robotics_utilities/simple_knearest_neighbors.hpp>
#include <common_robotics_utilities/simple_prm_planner.hpp>
#include <common_robotics_utilities/simple_rrt_planner.hpp>
#include <common_robotics_utilities/zlib_helpers.hpp>

#include <gtest/gtest.h>

namespace common_robotics_utilities
{
namespace planning_test
{
using TestMap = Eigen::Matrix<char, Eigen::Dynamic, Eigen::Dynamic>;
using Waypoint = std::pair<ssize_t, ssize_t>;
using WaypointAllocator = std::allocator<Waypoint>;
using WaypointVector = std::vector<Waypoint, WaypointAllocator>;
using WaypointPlanningTree = simple_rrt_planner::PlanningTree<Waypoint>;

bool WaypointsEqual(const Waypoint& first, const Waypoint& second)
{
  return (first.first == second.first && first.second == second.second);
}

struct WaypointEqualer
{
  bool operator()(const Waypoint& first, const Waypoint& second) const
  {
    return WaypointsEqual(first, second);
  }
};

struct WaypointHasher
{
  size_t operator()(const Waypoint& waypoint) const
  {
    std::size_t hash_val = 0;
    common_robotics_utilities::utility::hash_combine(
        hash_val, waypoint.first, waypoint.second);
    return hash_val;
  }
};

double WaypointDistance(const Waypoint& start, const Waypoint& end)
{
  const double delta_rows = static_cast<double>(end.first - start.first);
  const double delta_cols = static_cast<double>(end.second - start.second);
  return std::sqrt((delta_rows * delta_rows) + (delta_cols * delta_cols));
}

Waypoint InterpolateWaypoint(
    const Waypoint& start, const Waypoint& end, const double ratio)
{
  const double real_ratio = utility::ClampValueAndWarn(ratio, 0.0, 1.0);
  const double delta_rows = static_cast<double>(end.first - start.first);
  const double delta_cols = static_cast<double>(end.second - start.second);
  const double raw_interp_rows = delta_rows * real_ratio;
  const double raw_interp_cols = delta_cols * real_ratio;
  const ssize_t interp_row
      = start.first + static_cast<ssize_t>(std::round(raw_interp_rows));
  const ssize_t interp_col
      = start.second + static_cast<ssize_t>(std::round(raw_interp_cols));
  return Waypoint(interp_row, interp_col);
}

WaypointVector ResampleWaypoints(const WaypointVector& waypoints)
{
  return path_processing::ResamplePath<Waypoint>(
      waypoints, 0.5, WaypointDistance, InterpolateWaypoint);
}

void DrawEnvironment(const TestMap& environment)
{
  std::cout << environment << std::endl;
}

void SetCell(TestMap& map, const ssize_t row, const ssize_t col, const char val)
{
  if (map(row, col) != '#')
  {
    map(row, col) = val;
  }
}

void DrawPaths(
    const TestMap& environment,
    const WaypointVector& starts,
    const WaypointVector& goals,
    const std::vector<WaypointVector>& paths)
{
  TestMap working_copy = environment;
  for (const auto& path : paths)
  {
    if (path.size() > 0)
    {
      SetCell(working_copy, path.at(0).first, path.at(0).second, '+');
      for (size_t idx = 1; idx < path.size(); idx++)
      {
        const Waypoint& previous = path.at(idx - 1);
        const Waypoint& current = path.at(idx);
        const auto edge_path = ResampleWaypoints({previous, current});
        for (const auto& state : edge_path)
        {
          const char current_val = working_copy(state.first, state.second);
          if (current_val != '+')
          {
            SetCell(working_copy, state.first, state.second, '-');
          }
        }
        SetCell(working_copy, current.first, current.second, '+');
      }
    }
  }
  for (const auto& start : starts)
  {
    SetCell(working_copy, start.first, start.second, 'S');
  }
  for (const auto& goal : goals)
  {
    SetCell(working_copy, goal.first, goal.second, 'G');
  }
  DrawEnvironment(working_copy);
}

void DrawPath(
    const TestMap& environment,
    const WaypointVector& starts,
    const WaypointVector& goals,
    const WaypointVector& path)
{
  return DrawPaths(environment, starts, goals, {path});
}

TestMap MakeTestMap(
    const std::string& test_map_string, const ssize_t rows, const ssize_t cols)
{
  if (static_cast<ssize_t>(test_map_string.size()) != (rows * cols))
  {
    throw std::invalid_argument("test_map_string is the wrong size");
  }
  TestMap test_map(rows, cols);
  memcpy(test_map.data(), test_map_string.data(),
         test_map_string.size() * sizeof(char));
  test_map.transposeInPlace();
  return test_map;
}

bool CheckWaypointCollisionFree(const TestMap& map, const Waypoint& waypoint)
{
  return (map(waypoint.first, waypoint.second) != '#');
}

bool CheckEdgeCollisionFree(
    const TestMap& map, const Waypoint& start, const Waypoint& end,
    const double step_size)
{
  const double distance = WaypointDistance(start, end);
  const double raw_num_intervals = distance / step_size;
  const int32_t num_states
      = std::max(static_cast<int32_t>(std::ceil(raw_num_intervals)), 1);
  for (int32_t state = 0; state <= num_states; state++)
  {
    const double interpolation_ratio
        = static_cast<double>(state) / static_cast<double>(num_states);
    const Waypoint interpolated
        = InterpolateWaypoint(start, end, interpolation_ratio);
    if (!CheckWaypointCollisionFree(map, interpolated))
    {
      return false;
    }
  }
  return true;
}

template<typename PRNG>
WaypointVector SmoothWaypoints(
    const WaypointVector& waypoints,
    const std::function<bool(const Waypoint&, const Waypoint&)>& check_edge_fn,
    PRNG& prng)
{
  // Parameters for shortcut smoothing
  const uint32_t max_iterations = 100;
  const uint32_t max_failed_iterations = 100;
  const uint32_t max_backtracking_steps = 1;
  const double max_shortcut_fraction = 0.5;
  const double resample_shortcuts_interval = 0.5;
  const bool check_for_marginal_shortcuts = false;
  return path_processing::ShortcutSmoothPath<PRNG, Waypoint>(
      waypoints, max_iterations, max_failed_iterations, max_backtracking_steps,
      max_shortcut_fraction, resample_shortcuts_interval,
      check_for_marginal_shortcuts, check_edge_fn, WaypointDistance,
      InterpolateWaypoint, prng);
}

void DrawRoadmap(
    const TestMap& environment,
    const simple_graph::Graph<Waypoint>& roadmap)
{
  TestMap working_copy = environment;
  const auto& roadmap_nodes = roadmap.GetNodesImmutable();
  for (const auto& roadmap_node : roadmap_nodes)
  {
    const std::vector<simple_graph::GraphEdge>& out_edges
        = roadmap_node.GetOutEdgesImmutable();
    for (const simple_graph::GraphEdge& edge : out_edges)
    {
      const Waypoint& self
          = roadmap.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable();
      const Waypoint& other
          = roadmap.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable();
      const auto edge_path = ResampleWaypoints({self, other});
      for (const auto& waypoint : edge_path)
      {
        const char current_val = working_copy(waypoint.first, waypoint.second);
        if (current_val != '+')
        {
          SetCell(working_copy, waypoint.first, waypoint.second, '-');
        }
      }
      SetCell(working_copy, self.first, self.second, '+');
      SetCell(working_copy, other.first, other.second, '+');
    }
  }
  DrawEnvironment(working_copy);
}

WaypointVector GenerateAllPossible8ConnectedChildren(
    const Waypoint& waypoint)
{
  return WaypointVector{
      Waypoint(waypoint.first - 1, waypoint.second - 1),
      Waypoint(waypoint.first - 1, waypoint.second),
      Waypoint(waypoint.first - 1, waypoint.second + 1),
      Waypoint(waypoint.first, waypoint.second - 1),
      Waypoint(waypoint.first, waypoint.second + 1),
      Waypoint(waypoint.first + 1, waypoint.second - 1),
      Waypoint(waypoint.first + 1, waypoint.second),
      Waypoint(waypoint.first + 1, waypoint.second + 1)};
}

template<typename PRNG>
Waypoint SampleWaypoint(const TestMap& map, PRNG& rng)
{
  std::uniform_int_distribution<ssize_t> row_dist(1, map.rows() - 1);
  std::uniform_int_distribution<ssize_t> col_dist(1, map.rows() - 1);
  return Waypoint(row_dist(rng), col_dist(rng));
}

GTEST_TEST(PlanningTest, Test)
{
  const std::string test_env_raw = "####################"
                                   "#                  #"
                                   "#  ####            #"
                                   "#  ####    #####   #"
                                   "#  ####    #####   #"
                                   "#          #####   #"
                                   "#          #####   #"
                                   "#                  #"
                                   "#      #########   #"
                                   "#     ##########   #"
                                   "#    ###########   #"
                                   "#   ############   #"
                                   "#                  #"
                                   "#                  #"
                                   "#    ##            #"
                                   "#    ##   ######## #"
                                   "#    ##   ######## #"
                                   "#    ##   ######## #"
                                   "#                  #"
                                   "####################";
  const TestMap test_env = MakeTestMap(test_env_raw, 20, 20);
  std::cout << "Planning environment" << std::endl;
  DrawEnvironment(test_env);
  const int64_t prng_seed = 42;
  std::mt19937_64 prng(prng_seed);
  const WaypointVector keypoints
      = {Waypoint(1, 1), Waypoint(18, 18), Waypoint(7, 13), Waypoint(9, 5)};
  // Bind helper functions used by multiple planners
  const std::function<bool(const Waypoint&)> check_state_validity_fn
      = [&] (const Waypoint& waypoint)
  {
    return CheckWaypointCollisionFree(test_env, waypoint);
  };
  const std::function<bool(const Waypoint&, const Waypoint&)>
      check_edge_validity_fn = [&] (const Waypoint& start, const Waypoint& end)
  {
    // We check both forward and backward because rounding in the waypoint
    // interpolation can create edges that are valid in only one direction.
    return (CheckEdgeCollisionFree(test_env, start, end, 0.5) &&
            CheckEdgeCollisionFree(test_env, end, start, 0.5));
  };
  const std::function<Waypoint(void)> state_sampling_fn = [&] (void)
  {
    return SampleWaypoint(test_env, prng);
  };
  // Functions to check planning results
  const std::function<void(const WaypointVector&)> check_path =
      [&] (const WaypointVector& path)
  {
    ASSERT_GE(static_cast<int32_t>(path.size()), 2);
    for (size_t idx = 1; idx < path.size(); idx++)
    {
      // We check both forward and backward because rounding in the waypoint
      // interpolation can create edges that are valid in only one direction.
      const bool forward_valid =
          check_edge_validity_fn(path.at(idx - 1), path.at(idx));
      const bool backward_valid =
          check_edge_validity_fn(path.at(idx), path.at(idx - 1));
      const bool edge_valid = forward_valid && backward_valid;
      ASSERT_TRUE(edge_valid);
    }
  };
  const std::function<void(
      const TestMap&, const WaypointVector&,
      const WaypointVector&, const WaypointVector&)> check_plan =
      [&] (const TestMap& environment, const WaypointVector& starts,
           const WaypointVector& goals,
           const WaypointVector& path)
  {
    DrawPath(environment, starts, goals, path);
    std::cout << "Checking raw path" << std::endl;
    check_path(path);
    const auto smoothed_path =
        SmoothWaypoints(path, check_edge_validity_fn, prng);
    std::cout << "Checking smoothed path" << std::endl;
    check_path(smoothed_path);
    const auto resampled_path = ResampleWaypoints(smoothed_path);
    std::cout << "Checking resampled path" << std::endl;
    check_path(resampled_path);
  };
  // RRT and BiRRT parameters
  const double rrt_step_size = 3.0;
  const double rrt_goal_bias = 0.1;
  const double rrt_timeout = 5.0;
  const double birrt_tree_sampling_bias = 0.5;
  const double birrt_p_switch_trees = 0.25;
  // Make RRT helpers
  auto rrt_nearest_neighbors_fn
      = simple_rrt_planner
          ::MakeKinematicLinearRRTNearestNeighborsFunction<Waypoint>(
              WaypointDistance, false);
  auto rrt_extend_fn
      = simple_rrt_planner::MakeKinematicRRTExtendPropagationFunction<Waypoint>(
          WaypointDistance, InterpolateWaypoint, check_edge_validity_fn,
          rrt_step_size);
  auto rrt_connect_fn
      = simple_rrt_planner
          ::MakeKinematicRRTConnectPropagationFunction<Waypoint>(
              WaypointDistance, InterpolateWaypoint, check_edge_validity_fn,
              rrt_step_size);
  auto birrt_nearest_neighbors_fn
      = simple_rrt_planner
          ::MakeKinematicLinearBiRRTNearestNeighborsFunction<Waypoint>(
              WaypointDistance, false);
  auto birrt_extend_fn
      = simple_rrt_planner
          ::MakeKinematicBiRRTExtendPropagationFunction<Waypoint>(
              WaypointDistance, InterpolateWaypoint, check_edge_validity_fn,
              rrt_step_size);
  auto birrt_connect_fn
      = simple_rrt_planner
          ::MakeKinematicBiRRTConnectPropagationFunction<Waypoint>(
              WaypointDistance, InterpolateWaypoint, check_edge_validity_fn,
              rrt_step_size);
  // Build a roadmap on the environment
  const size_t K = 5;
  const int64_t roadmap_size = 100;
  const std::function<bool(const int64_t)> roadmap_termination_fn
      = [] (const int64_t current_roadmap_size)
  {
    return (current_roadmap_size >= roadmap_size);
  };
  simple_graph::Graph<Waypoint> roadmap;
  simple_prm_planner::GrowRoadMap<Waypoint>(
      roadmap, state_sampling_fn, WaypointDistance, check_state_validity_fn,
      check_edge_validity_fn, roadmap_termination_fn, K, false, true, false);
  ASSERT_TRUE(roadmap.CheckGraphLinkage());
  std::cout << "Roadmap built" << std::endl;
  simple_prm_planner::UpdateRoadMapEdges<Waypoint>(
      roadmap, check_edge_validity_fn, WaypointDistance, false);
  ASSERT_TRUE(roadmap.CheckGraphLinkage());
  std::cout << "Roadmap updated" << std::endl;
  // Test graph pruning
  const std::unordered_set<int64_t> nodes_to_prune = {10, 20, 30, 40, 50, 60};
  const auto serial_pruned_roadmap
      = roadmap.MakePrunedCopy(nodes_to_prune, false);
  ASSERT_TRUE(serial_pruned_roadmap.CheckGraphLinkage());
  const auto parallel_pruned_roadmap
      = roadmap.MakePrunedCopy(nodes_to_prune, true);
  ASSERT_TRUE(parallel_pruned_roadmap.CheckGraphLinkage());
  DrawRoadmap(test_env, roadmap);
  // Serialize & load & check the roadmap
  serialization::Serializer<Waypoint> serialize_waypoint_fn
      = [] (const Waypoint& wp, std::vector<uint8_t>& serialization_buffer)
  {
    return serialization::SerializePair<ssize_t, ssize_t>(
        wp, serialization_buffer, serialization::SerializeMemcpyable<ssize_t>,
        serialization::SerializeMemcpyable<ssize_t>);
  };
  serialization::Deserializer<Waypoint> deserialize_waypoint_fn
      = [] (const std::vector<uint8_t>& deserialization_buffer,
            const uint64_t starting_offset)
  {
    return serialization::DeserializePair<ssize_t, ssize_t>(
        deserialization_buffer, starting_offset,
        serialization::DeserializeMemcpyable<ssize_t>,
        serialization::DeserializeMemcpyable<ssize_t>);
  };
  std::vector<uint8_t> buffer;
  simple_graph::Graph<Waypoint>::Serialize(
      roadmap, buffer, serialize_waypoint_fn);
  const std::string temp_file = "/tmp/temp_planning_test_roadmap.rmp";
  zlib_helpers::CompressAndWriteToFile(buffer, temp_file);
  const std::vector<uint8_t> load_buffer
      = zlib_helpers::LoadFromFileAndDecompress(temp_file);
  ASSERT_EQ(buffer.size(), load_buffer.size());
  const auto loaded = simple_graph::Graph<Waypoint>::Deserialize(
                        load_buffer, 0, deserialize_waypoint_fn);
  const simple_graph::Graph<Waypoint>& loaded_roadmap = loaded.Value();
  ASSERT_EQ(roadmap.Size(), loaded_roadmap.Size());
  std::cout << "Old roadmap nodes: " << roadmap.Size() << "\n"
            << "Old roadmap binary size: " << buffer.size() << "\n"
            << "New roadmap nodes: " << loaded_roadmap.Size() << "\n"
            << "New roadmap binary size: " << load_buffer.size() << std::endl;
  for (size_t idx = 0; idx < loaded_roadmap.Size(); idx++)
  {
    const auto& old_node = roadmap.GetNodeImmutable(static_cast<int64_t>(idx));
    const auto& new_node
        = loaded_roadmap.GetNodeImmutable(static_cast<int64_t>(idx));
    const Waypoint& old_waypoint = old_node.GetValueImmutable();
    const Waypoint& new_waypoint = new_node.GetValueImmutable();
    ASSERT_EQ(old_waypoint.first, new_waypoint.first);
    ASSERT_EQ(old_waypoint.second, new_waypoint.second);
    const auto& old_in_edges = old_node.GetInEdgesImmutable();
    const auto& new_in_edges = new_node.GetInEdgesImmutable();
    ASSERT_EQ(old_in_edges.size(), new_in_edges.size());
    for (size_t edx = 0; edx < new_in_edges.size(); edx++)
    {
      const auto& old_edge = old_in_edges.at(edx);
      const auto& new_edge = new_in_edges.at(edx);
      ASSERT_EQ(old_edge.GetFromIndex(), new_edge.GetFromIndex());
      ASSERT_EQ(old_edge.GetToIndex(), new_edge.GetToIndex());
    }
    const auto& old_out_edges = old_node.GetOutEdgesImmutable();
    const auto& new_out_edges = new_node.GetOutEdgesImmutable();
    ASSERT_EQ(old_out_edges.size(), new_out_edges.size());
    for (size_t edx = 0; edx < new_out_edges.size(); edx++)
    {
      const auto& old_edge = old_out_edges.at(edx);
      const auto& new_edge = new_out_edges.at(edx);
      ASSERT_EQ(old_edge.GetFromIndex(), new_edge.GetFromIndex());
      ASSERT_EQ(old_edge.GetToIndex(), new_edge.GetToIndex());
    }
  }
  std::cout << "Loaded Roadmap" << std::endl;
  DrawRoadmap(test_env, loaded_roadmap);
  // Run planning tests
  for (size_t sdx = 0; sdx < keypoints.size(); sdx++)
  {
    for (size_t gdx = 0; gdx < keypoints.size(); gdx++)
    {
      if (sdx != gdx)
      {
        // Get start & goal waypoints
        const Waypoint& start = keypoints.at(sdx);
        const Waypoint& goal = keypoints.at(gdx);
        // Plan with PRM
        std::cout << "PRM Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        const auto path = simple_prm_planner::QueryPath<Waypoint>(
            {start}, {goal}, loaded_roadmap, WaypointDistance,
            check_edge_validity_fn, K, false, true, false, true).Path();
        check_plan(test_env, {start}, {goal}, path);
        // Plan with Lazy-PRM
        std::cout << "Lazy-PRM Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        const auto lazy_path = simple_prm_planner::LazyQueryPath<Waypoint>(
            {start}, {goal}, loaded_roadmap, WaypointDistance,
            check_edge_validity_fn, K, false, true, false, true).Path();
        check_plan(test_env, {start}, {goal}, lazy_path);
        // Plan with A*
        std::cout << "A* Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        const auto astar_path
            = simple_astar_search::PerformAstarSearch<
                Waypoint, WaypointVector, WaypointHasher, WaypointEqualer>(
                    start, goal, GenerateAllPossible8ConnectedChildren,
                    check_edge_validity_fn, WaypointDistance, WaypointDistance,
                    true).Path();
        check_plan(test_env, {start}, {goal}, astar_path);
        // Plan with RRT-Extend
        std::cout << "RRT-Extend Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        const auto rrt_sample_fn
            = simple_rrt_planner::MakeStateAndGoalsSamplingFunction<Waypoint>(
                state_sampling_fn, {goal}, rrt_goal_bias, prng);
        const simple_rrt_planner::CheckGoalReachedFunction<Waypoint>
            rrt_goal_reached_fn = [&] (const Waypoint& state)
        {
          return WaypointsEqual(goal, state);
        };
        WaypointPlanningTree rrt_extend_tree;
        rrt_extend_tree.emplace_back(
            simple_rrt_planner::SimpleRRTPlannerState<Waypoint>(start));
        const auto rrt_extend_path
            = simple_rrt_planner::RRTPlanSinglePath<
                Waypoint, Waypoint, WaypointVector>(
                    rrt_extend_tree, rrt_sample_fn, rrt_nearest_neighbors_fn,
                    rrt_extend_fn, {}, rrt_goal_reached_fn, {},
                    simple_rrt_planner
                        ::MakeRRTTimeoutTerminationFunction(rrt_timeout))
                        .Path();
        check_plan(test_env, {start}, {goal}, rrt_extend_path);
        // Plan with RRT-Connect
        std::cout << "RRT-Connect Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        WaypointPlanningTree rrt_connect_tree;
        rrt_connect_tree.emplace_back(
            simple_rrt_planner::SimpleRRTPlannerState<Waypoint>(start));
        const auto rrt_connect_path
            = simple_rrt_planner::RRTPlanSinglePath<
                Waypoint, Waypoint, WaypointVector>(
                    rrt_connect_tree, rrt_sample_fn, rrt_nearest_neighbors_fn,
                    rrt_connect_fn, {}, rrt_goal_reached_fn, {},
                    simple_rrt_planner
                        ::MakeRRTTimeoutTerminationFunction(rrt_timeout))
                        .Path();
        check_plan(test_env, {start}, {goal}, rrt_connect_path);
        // Plan with BiRRT-Extend
        std::cout << "BiRRT-Extend Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        const simple_rrt_planner::StatesConnectedFunction<Waypoint>
            birrt_states_connected_fn
                = [] (const Waypoint& first, const Waypoint& second, const bool)
        {
          return WaypointsEqual(first, second);
        };
        WaypointPlanningTree birrt_extend_start_tree;
        birrt_extend_start_tree.emplace_back(
            simple_rrt_planner::SimpleRRTPlannerState<Waypoint>(start));
        WaypointPlanningTree birrt_extend_goal_tree;
        birrt_extend_goal_tree.emplace_back(
            simple_rrt_planner::SimpleRRTPlannerState<Waypoint>(goal));
        const auto birrt_extent_path
            = simple_rrt_planner::BiRRTPlanSinglePath<
                std::mt19937_64, Waypoint, WaypointVector>(
                    birrt_extend_start_tree, birrt_extend_goal_tree,
                    state_sampling_fn, birrt_nearest_neighbors_fn,
                    birrt_extend_fn, {}, birrt_states_connected_fn, {},
                    birrt_tree_sampling_bias, birrt_p_switch_trees,
                    simple_rrt_planner
                        ::MakeBiRRTTimeoutTerminationFunction(rrt_timeout),
                    prng).Path();
        check_plan(test_env, {start}, {goal}, birrt_extent_path);
        // Plan with BiRRT-Connect
        std::cout << "BiRRT-Connect Path (" << print::Print(start) << " to "
                  << print::Print(goal) << ")" << std::endl;
        WaypointPlanningTree birrt_connect_start_tree;
        birrt_connect_start_tree.emplace_back(
            simple_rrt_planner::SimpleRRTPlannerState<Waypoint>(start));
        WaypointPlanningTree birrt_connect_goal_tree;
        birrt_connect_goal_tree.emplace_back(
            simple_rrt_planner::SimpleRRTPlannerState<Waypoint>(goal));
        const auto birrt_connect_path
            = simple_rrt_planner::BiRRTPlanSinglePath<
                std::mt19937_64, Waypoint, WaypointVector>(
                    birrt_connect_start_tree, birrt_connect_goal_tree,
                    state_sampling_fn, birrt_nearest_neighbors_fn,
                    birrt_connect_fn, {}, birrt_states_connected_fn, {},
                    birrt_tree_sampling_bias, birrt_p_switch_trees,
                    simple_rrt_planner
                        ::MakeBiRRTTimeoutTerminationFunction(rrt_timeout),
                    prng).Path();
        check_plan(test_env, {start}, {goal}, birrt_connect_path);
      }
    }
  }
  // Plan with PRM
  const WaypointVector starts = {keypoints.at(0), keypoints.at(1)};
  const WaypointVector goals = {keypoints.at(2), keypoints.at(3)};
  std::cout << "Multi start/goal PRM Path (" << print::Print(starts) << " to "
            << print::Print(goals) << ")" << std::endl;
  const auto path = simple_prm_planner::QueryPath<Waypoint>(
      starts, goals, loaded_roadmap, WaypointDistance, check_edge_validity_fn,
      K, false, true, false).Path();
  check_plan(test_env, starts, goals, path);
}
}  // namespace planning_test
}  // namespace common_robotics_utilities

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


/*!
 * @file
 * An example of logistic regression training and testing.
 * The data is taken from:
 *
 * command to run:
 * mpirung -n 4 ./bin/logreg "data/logreg/train.csv"
 *
 * For running on some different data-set specify the columns etc. in `fromFile`
 * Also change the `dim` parameter and inFile variable.
 * Testing data files can be given as arguments after training data file.
 *
 * benchmarks at the bottom
 * */
#include <array>
#include <iostream>
#include <stdexcept>
#include <boost/mpi.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/io.hpp>
#include <ezl/algorithms/reduceAlls.hpp>
#include <ezl/algorithms/reduces.hpp>
#include <ezl/algorithms/fromFile.hpp>

using namespace std;

double sigmoid(double x) {
  constexpr auto e = 2.718281828;
  return 1.0 / (1.0 + pow(e, -x));
}

template <size_t dim>
double calcNorm(const array<double, dim> &weights,
                const array<double, dim> &weightsNew) {
  auto sum = 0.;
  for (size_t i = 0; i < weights.size(); ++i) {
    auto minus = weights[i] - weightsNew[i];
    sum += (minus * minus);
  }
  return sqrt(sum);
}

template <size_t dim>
auto calcGrad(const double &y, const array<double, dim> &x,
                const array<double, dim> &w) {
  array<double, dim> grad;
  auto dot = std::inner_product(::begin(w), ::end(w), ::begin(x), 0);
  //auto s = (sigmoid(y * dot) - 1) * y;
  auto s = sigmoid(dot) - y;
  for (size_t i = 0; i < w.size(); ++i) {
    grad[i] = s * x[i];
  }
  return grad;
}

void logreg(int argc, char* argv[]) {
  if (argc < 2) {
    cerr << "Please provide arguments as glob pattern for train file(s), "
            "followed by test file pattern(s). Check source for defaults or "
            "for running on some other data-format.";
    return;
  }

  constexpr auto dim = 3;  // number of features
  constexpr auto maxIters = 1000;

  // specify columns and other read properties if required.
  auto reader =
      ezl::fromFile<double, array<double, dim>>(argv[1]).colSeparator(",");

  // load once in memory
  auto data = ezl::rise(reader)
                  .runResult();

  if (data.empty()) {
    cout<<"no data";
    return;
  }

  auto sumArray = [](auto &a, auto &b) -> auto & {
    transform(begin(a), end(a), begin(b), begin(b), plus<double>());
    return b;
  };

  array<double, dim> w{};  // weights initialised to zero;
  // build flow for final gradient value in all procs
  auto train = ezl::rise(ezl::fromMem(data))
                   .map([&w](auto& y, auto& x) {
                     return calcGrad(y, x, w);
                   }).colsTransform()
                   .reduce(sumArray, array<double, dim>{}).inprocess()
                   .reduce(sumArray, array<double, dim>{})
                     .prll(1., ezl::llmode::task | ezl::llmode::all)
                   .build();

  auto iters = 0;
  auto norm = 0.;
  while (iters++ < maxIters) {
    array<double, dim> wn, grad;
    tie(grad) =  ezl::flow(train).runResult()[0]; // running flow
    constexpr static auto gamma = 0.002;
    transform(begin(w), end(w), begin(grad), begin(wn),
                   [](double a, double b) { return a - gamma * b;});
    norm = calcNorm(wn, w);
    w = move(wn);
    constexpr auto epsilon = 0.0001;
    if(norm < epsilon)  break;
  }
  cout<<"iterations: "<<iters-1<<endl;  // TODO: message
  cout<<"norm: "<<norm<<endl;
  cout<<"final weights: "<<w<<endl;

  // building testing flow
  auto testFlow = ezl::rise(reader)
                      .map<2>([&w](auto x) {
                        auto pred = 0.;
                        for (size_t i = 0; i < get<0>(x).size(); ++i) {
                          pred += w[i] * get<0>(x)[i];
                        }
                        return (sigmoid(pred) > 0.5);
                      }).colsTransform()
                      .reduce<1, 2>(ezl::count(), 0)
                        .dump("", "real-y, predicted-y, count")
                      .build();

  for (int i = 1; i < argc; ++i) {
    reader = reader.filePattern(argv[i]);
    cout<<"Testing for "<<argv[i]<<endl;
    ezl::flow(testFlow).run();
  }
}

int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    logreg(argc, argv);
  } catch (const exception& ex) {
    cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}

/*!
 * benchmark results: i7(hdd); input: 450MBs
 *  *nprocs* | 1   | 2   | 4    |
 *  ---      |---  |---  |---   |
 *  *time(s)*| 120 | 63  | 38   |
 *
 * benchmark results: Linux(nfs-3); input: 2.9GBs; units: secs
 *  *nprocs* | 1x12      | 2x12      | 4x12      | 8x12      |  12x12   |
 *  ---      |---        |---        |---        | ---       |          |
 *  *time(s)*| 190       | 91        | 50        | 36        |  34      |
 */


#include <cmath>
#include <iostream>
#include <algorithm>
#include <vector>
#include <thread>
#include <boost/math/special_functions/bessel.hpp>
#include "spida/transform/hankelR.h"
#include "spida/grid/besselR.h"
#if defined(HAVE_OPENBLAS)
    #include "cblas.h"
#endif

namespace spida {

  HankelTransformR::HankelTransformR(const BesselRootGridR& grid) :
      m_nr(grid.getNr()),
      m_Ymk(m_nr*m_nr),
      m_YmkC(m_nr*m_nr)
  {
      m_alpha = grid.getjN()/pow(grid.getMaxSR(),2);
      initDHT(grid);
  }

  void HankelTransformR::R_To_SR(const double* in,double* out)
  {
/*
enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102};
enum CBLAS_TRANSPOSE    {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113};
cblas_dgemv(const enum CBLAS_ORDER Order,
           const enum CBLAS_TRANSPOSE TransA, const int M, const int N,
           const double alpha, const double *A, const int lda,
           const double *X, const int incX, const double beta,
           double *Y, const int incY);
dgemv y = alpha*A*x + beta*y
lda -> first dimension of A
*/

      #if defined(HAVE_OPENBLAS)
      cblas_dgemv(CblasRowMajor,CblasNoTrans,m_nr,m_nr,m_alpha,m_Ymk.data(),m_nr,in,1,0.0,out,1);
      #else
      for(int m = 0; m < m_nr; m++){
          double sum = 0.0;
          for(int k = 0; k < m_nr; k++)
              sum += m_Ymk[m*m_nr+k]*in[k];
          out[m] = m_alpha*sum;
      }
      #endif
  }

  void HankelTransformR::R_To_SR(const dcmplx* in,dcmplx* out)
  {
      #if defined(HAVE_OPENBLAS)
      const double beta = 0.0;
      cblas_zgemv(CblasRowMajor,CblasNoTrans,m_nr,m_nr,&m_alpha,m_YmkC.data(),m_nr,in,1,&beta,out,1);
      #else
      for(int m = 0; m < m_nr; m++){
          dcmplx sum = 0.0;
          for(int k = 0; k < m_nr; k++)
              sum += m_YmkC[m*m_nr+k]*in[k];
          out[m] = m_alpha*sum;
      }
      #endif
  }

  void HankelTransformR::SR_To_R(const double* in,double* out)
  {
      #if defined(HAVE_OPENBLAS)
      cblas_dgemv(CblasRowMajor,CblasNoTrans,m_nr,m_nr,1.0/m_alpha,m_Ymk.data(),m_nr,in,1,0.0,out,1);
      #else
      for(int k = 0; k < m_nr; k++){
          double sum = 0.0;
          for(int m = 0; m < m_nr; m++)
              sum += m_Ymk[k*m_nr+m]*in[m];
          out[k] = sum/m_alpha;
      }
      #endif
  }

  void HankelTransformR::SR_To_R(const dcmplx* in,dcmplx* out)
  {
      #if defined(HAVE_OPENBLAS)
      const dcmplx a = 1.0/m_alpha;
      const dcmplx beta = 0.0;
      cblas_zgemv(CblasRowMajor,CblasNoTrans,m_nr,m_nr,&a,m_YmkC.data(),m_nr,in,1,&beta,out,1);
      #else
      for(int k = 0; k < m_nr; k++){
          dcmplx sum = 0.0;
          for(int m = 0; m < m_nr; m++)
              sum += m_YmkC[k*m_nr+m]*in[m];
          out[k] = sum/m_alpha;
      }
      #endif
  }

  void HankelTransformR::initDHT(const BesselRootGridR& grid){
      const std::vector<double>& J0 = grid.getBesselRoots();
      std::vector<double> J1(m_nr);

      for(auto i = 0; i < m_nr; i++)
          J1[i] = boost::math::cyl_bessel_j<double>(1.0,J0[i]);

      double jN = grid.getjN();
      for(auto m = 0; m < m_nr; m++){
          for(auto k = 0; k < m_nr; k++){
              double beta_mk = 2.0/(jN*pow(J1[k],2));
              double arg = J0[m]*J0[k]/jN;
              double J0_mk = boost::math::cyl_bessel_j<double>(0.0,arg);
              m_Ymk[m*m_nr+k] = beta_mk*J0_mk;
              m_YmkC[m*m_nr+k] = beta_mk*J0_mk;
          }
      }
  }

  HankelTransformRb::HankelTransformRb(const BesselRootGridR& grid,unsigned threads) :
      m_threads(threads),
      m_nr(grid.getNr()),
      m_Ymk(grid.getNr()*grid.getNr())
  {
      m_alpha = grid.getjN()/pow(grid.getMaxSR(),2);
      initDHT(grid);
  }

  void HankelTransformRb::R_To_SR(const double* in,double* out)
  {
      for(unsigned m = 0; m < m_nr; m++){
          double sum = 0.0;
          for(unsigned k = 0; k < m_nr; k++)
              sum += m_Ymk[m*m_nr+k]*in[k];
          out[m] = m_alpha*sum;
      }
  }

  void HankelTransformRb::R_To_SR(const dcmplx* in,dcmplx* out)
  {
      std::vector<std::thread> workers;
      for(unsigned tid = 0; tid < m_threads; tid++){
          workers.push_back(std::thread([](\
                          unsigned tid,\
                          unsigned nthreads,\
                          unsigned nr,\
                          std::vector<double>& Ymk,\
                          double alpha,\
                          const dcmplx* v,\
                          dcmplx* w){
              for(unsigned m = tid; m < nr; m+=nthreads){
                  dcmplx sum = 0.0;
                  for(unsigned k = 0; k < nr; k++)
                      sum += Ymk[m*nr+k]*v[k];
                  w[m] = alpha*sum;
              }
          },tid,m_threads,m_nr,std::ref(m_Ymk),m_alpha,in,out));
      }

      for(auto& worker : workers){
          worker.join();
      }
  }

  void HankelTransformRb::SR_To_R(const double* in,double* out)
  {

      for(unsigned k = 0; k < m_nr; k++){
          double sum = 0.0;
          for(unsigned m = 0; m < m_nr; m++)
              sum += m_Ymk[k*m_nr+m]*in[m];
          out[k] = sum/m_alpha;
      }
  }

  void HankelTransformRb::SR_To_R(const dcmplx* in,dcmplx* out)
  {
      std::vector<std::thread> workers;
      for(unsigned tid = 0; tid < m_threads; tid++){
          workers.push_back(std::thread([](\
                          unsigned tid,\
                          unsigned nthreads,\
                          unsigned nr,\
                          std::vector<double>& Ymk,\
                          double alpha,\
                          const dcmplx* v,\
                          dcmplx* w){
              for(unsigned k = tid; k < nr; k+=nthreads){
                  dcmplx sum = 0.0;
                  for(unsigned m = 0; m < nr; m++)
                      sum += Ymk[k*nr+m]*v[m];
                  w[k] = sum/alpha;
              }
          },tid,m_threads,m_nr,std::ref(m_Ymk),m_alpha,in,out));
      }

      for(auto& worker : workers){
          worker.join();
      }
  }

  void HankelTransformRb::initDHT(const BesselRootGridR& grid){
      const std::vector<double>& J0 = grid.getBesselRoots();
      std::vector<double> J1(m_nr);

      for(auto i = 0; i < m_nr; i++)
          J1[i] = boost::math::cyl_bessel_j<double>(1.0,J0[i]);

      double jN = grid.getjN();
      for(auto m = 0; m < m_nr; m++){
          for(auto k = 0; k < m_nr; k++){
              double beta_mk = 2.0/(jN*pow(J1[k],2));
              double arg = J0[m]*J0[k]/jN;
              double J0_mk = boost::math::cyl_bessel_j<double>(0.0,arg);
              m_Ymk[m*m_nr+k] = beta_mk*J0_mk;
          }
      }
  }

}


#include <iostream>
#include <math.h>

#include <boost/multiprecision/cpp_int.hpp>

using namespace std;

int target = 1000;

int main(int argc, char** argv) {
  boost::multiprecision::cpp_int n = 1;
  for (int i = 0; i < target; i++) {
    n *= 2;
  }
  unsigned long long sum = 0;
  while (n) {
    sum += static_cast<unsigned long long>(n % 10);
    n /= 10;
  }
  cout << "Sum of all digits: " << sum << endl;
  return 0;
}


#include <boost/test/unit_test.hpp>
#include "collision.h"

BOOST_AUTO_TEST_SUITE (test_collision)

BOOST_AUTO_TEST_CASE (test_sphere_line_collision_1)
{
	math::sphere<2> sph;
	sph.centre.set(10, 10);
	sph.radius = 1;

	math::line<2> l;
	l.A.set(-100, 0);
	l.B.set(100, 0);

	math::contact_info<2> ci;
	math::collide(l, sph, math::vec<2>(-2, -1), ci);

	BOOST_REQUIRE (ci.happened == true);
	BOOST_REQUIRE (math::abs(ci.time - 9.0f) < math::EPSILON);
}

BOOST_AUTO_TEST_CASE (test_sphere_line_collision_2)
{
	math::sphere<2> sph;
	sph.centre.set(10, 0.5f);
	sph.radius = 1;

	math::line<2> l;
	l.A.set(0, 0);
	l.B.set(0, 1);

	math::contact_info<2> ci;
	math::collide(l, sph, math::vec<2>(-1, 0), ci);

	BOOST_REQUIRE(ci.happened == true);
	BOOST_REQUIRE(math::abs(ci.time - 9.0f) < math::EPSILON);
	BOOST_REQUIRE((ci.position - math::vec<2>(0, 0.5f)).length_sq() < math::EPSILON);
	BOOST_REQUIRE((ci.normal - math::vec<2>(1, 0)).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE (test_sphere_line_collision_3)
{
	math::line<2> l;
	l.A.set(1, -10);
	l.B.set(1, 10);

	math::sphere<2> s;
	s.centre.set(10, 10);
	s.radius = 1;

	math::contact_info<2> ci;
	math::collide(l, s, math::vec<2>(-1, -1), ci);

	BOOST_REQUIRE ((ci.position - math::vec<2>(1, 2)).length() < math::EPSILON);
	BOOST_REQUIRE ((ci.time - 8) < math::EPSILON);
}

BOOST_AUTO_TEST_CASE (test_sphere_obb_collision_1)
{
	math::obb<2> obb;
	obb.origin.set(0, 0);
	obb.tangent.set(1, 0);
	obb.normal.set(0, 1);

	math::sphere<2> sph;
	sph.centre.set(0.5, 10);
	sph.radius = 1;

	math::contact_info<2> ci;
	math::collide(obb, sph, math::vec<2>(0, -1), ci);

	BOOST_REQUIRE ((ci.position - math::vec<2>(0.5, 1)).length() < math::EPSILON);
	BOOST_REQUIRE ((ci.normal - math::vec<2>(0, 1)).length() < math::EPSILON);
	BOOST_REQUIRE (math::abs(ci.time - 8) < math::EPSILON);
}

BOOST_AUTO_TEST_CASE (test_sphere_obb_collision_2)
{
	math::obb<2> obb;
	obb.origin.set(0, 0);
	obb.tangent.set(1, 0);
	obb.normal.set(0, 1);

	math::sphere<2> sph;
	sph.centre.set(10, 0.5f);
	sph.radius = 1;

	math::contact_info<2> ci;

	math::collide(obb, sph, math::vec<2>(-1.0f, -0.01f), ci);

	BOOST_REQUIRE ((ci.position - math::vec<2>(1, 0.42f)).length() < math::EPSILON);
	BOOST_REQUIRE ((ci.normal - math::vec<2>(1, 0)).length() < math::EPSILON);
	BOOST_REQUIRE ((ci.time - 8) < math::EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


/* test_cauchy_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/cauchy_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::cauchy_distribution<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 0.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN -(std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN -(std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MIN -(std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS (-100000.0, 0.000001)
#define BOOST_RANDOM_TEST2_PARAMS (100000.0, 0.000001)
#define BOOST_RANDOM_TEST1_MAX 0.0
#define BOOST_RANDOM_TEST2_MIN 0.0

#include "test_distribution.ipp"


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <boost/math/special_functions/gegenbauer.hpp>
#include <eve/function/gegenbauer.hpp>
#include <eve/function/diff/gegenbauer.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of gegenbauer on wide"
        , eve::test::simd::ieee_reals

        )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using wi_t = eve::as_integer_t<T>;
  using i_t  = eve::as_integer_t<v_t>;
  TTS_EXPR_IS( eve::gegenbauer(i_t(), T(), T())  , T);
  TTS_EXPR_IS( eve::gegenbauer(wi_t(), T(), T())  , T);
  TTS_EXPR_IS( eve::gegenbauer(i_t(), T(), v_t())  , T);
  TTS_EXPR_IS( eve::gegenbauer(wi_t(), T(), v_t())  , T);
};

//==================================================================================================
//== gegenbauer tests
//==================================================================================================
EVE_TEST( "Check behavior of gegenbauer on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::between(0.0, 1.0), eve::test::as_integer(eve::test::ramp(0)))
        )
  <typename T, typename I>(T const& a0,I const & i0)
{
  using v_t = eve::element_type_t<T>;
  v_t l = v_t(-3)/v_t(8);
  auto eve__gegenbauerv  =  [l](auto n, auto x) { return eve::gegenbauer(n, l, x); };
  for(unsigned int n=0; n < 5; ++n)
  {
    auto boost_gegenbauer =  [&](auto i, auto) { return boost::math::gegenbauer(n, l, a0.get(i)); };
    TTS_ULP_EQUAL(eve__gegenbauerv(n, a0), T(boost_gegenbauer), 180);
  }
  auto boost_gegenbauerv =  [&](auto i, auto) { return boost::math::gegenbauer(i0.get(i), l, a0.get(i)); };
  TTS_ULP_EQUAL(eve__gegenbauerv(i0    , a0), T(boost_gegenbauerv), 180);
  for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
  {
    auto boost_gegenbauer2 =  [&](auto i, auto) { return boost::math::gegenbauer(i0.get(i), l, a0.get(j)); };
    TTS_ULP_EQUAL(eve__gegenbauerv(i0 , a0.get(j)), T(boost_gegenbauer2), 180);
  }
  for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
  {
    for(unsigned int n=0; n < eve::cardinal_v<T>; ++n)
    {
      TTS_ULP_EQUAL(eve__gegenbauerv(i0.get(j) , a0.get(n)), v_t(boost::math::gegenbauer(i0.get(j), l, a0.get(n))), 180);
    }
  }
};

//==================================================================================================
//== gegenbauer diff  tests
//==================================================================================================
EVE_TEST( "Check behavior of gegenbauer diff on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::between(0.0, 1.0), eve::test::as_integer(eve::test::ramp(0)))
        )
  <typename T, typename I>(T const& a0,I const & i0)
{
   using v_t = eve::element_type_t<T>;
  v_t l = v_t(-3)/v_t(8);
   auto eve__gegenbauerv  =  [l](auto n, auto x) { return eve::diff( eve::gegenbauer)(n, l, x); };
  for(unsigned int n=0; n < 5; ++n)
  {
    auto boost_gegenbauer =  [&](auto i, auto) { return boost::math::gegenbauer_derivative(n, l, a0.get(i), 1u); };
    TTS_ULP_EQUAL(eve__gegenbauerv(n, a0), T(boost_gegenbauer), 180);
  }
  auto boost_gegenbauerv =  [&](auto i, auto) { return boost::math::gegenbauer_derivative(i0.get(i), l, a0.get(i), 1u); };
  TTS_ULP_EQUAL(eve__gegenbauerv(i0    , a0), T(boost_gegenbauerv), 180);
  for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
  {
    auto boost_gegenbauer2 =  [&](auto i, auto) { return boost::math::gegenbauer_derivative(i0.get(i), l, a0.get(j), 1u); };
    TTS_ULP_EQUAL(eve__gegenbauerv(i0 , a0.get(j)), T(boost_gegenbauer2), 180);
  }
  for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
  {
    for(unsigned int n=0; n < eve::cardinal_v<T>; ++n)
    {
      TTS_ULP_EQUAL(eve__gegenbauerv(i0.get(j) , a0.get(n)), v_t(boost::math::gegenbauer_derivative(i0.get(j), l, a0.get(n), 1u)), 180);
    }
 }
};


/*
This is a modified version SCRIMP with optimized arithmetics.
The code builds on SCRIMP++, as published by Zhua, Yeh, Zimmerman et al. at https://sites.google.com/site/scrimpplusplus/ and contains parts from their code

Details of the SCRIMP algorithm can be found at:
(author information ommited for ICDM review),
"SCRIMP++: Motif Discovery at Interactive Speeds", submitted to ICDM 2018.
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <vector>
#include <algorithm>
#include <string>
#include <sstream>
#include <chrono>

#include <boost/filesystem.hpp>

#include <ScrimpSequOpt.hpp>
#include <logging.hpp>
#include <papiwrapper.hpp>

using namespace matrix_profile;

static FactoryRegistration<ScrimpSequOpt> s_sequRegistr("scrimp_sequ_opt");
static const int notification_interval_iter = 10000;

void ScrimpSequOpt::precompute_window_statistics(const int windowSize,
    const aligned_tsdtype_vec& A,
    const int ProfileLength,
    aligned_tsdtype_vec& AMeanScaledSigSqrM,
    aligned_tsdtype_vec& ASigmaInv,
    const idx_dtype ts_len)
{
	//TODO: refactore raw pointers, const method
	const size_t timeSeriesLength =(ts_len==-1)?A.size():ts_len;
	std::vector<tsa_dtype> AMean(A.size()-windowSize+1);
	AMeanScaledSigSqrM.resize(A.size()-windowSize+1);
	ASigmaInv.resize(A.size()-windowSize+1);
	const tsa_dtype sqrt_m = sqrt(static_cast<tsa_dtype>(windowSize));

	tsa_dtype* ACumSum = new tsa_dtype[timeSeriesLength];
	ACumSum[0] = A[0];
	for (int i = 1; i < timeSeriesLength; i++)
		ACumSum[i] = A[i] + ACumSum[i - 1];
	tsa_dtype* ASqCumSum = new tsa_dtype[timeSeriesLength];
	ASqCumSum[0] = A[0] * A[0];
	for (int i = 1; i < timeSeriesLength; i++)
		ASqCumSum[i] = A[i] * A[i] + ASqCumSum[i - 1];
	tsa_dtype* ASum = new tsa_dtype[ProfileLength];
	ASum[0] = ACumSum[windowSize - 1];
	for (int i = 0; i < timeSeriesLength - windowSize; i++)
		ASum[i + 1] = ACumSum[windowSize + i] - ACumSum[i];
	tsa_dtype* ASumSq = new tsa_dtype[ProfileLength];
	ASumSq[0] = ASqCumSum[windowSize - 1];
	for (int i = 0; i < timeSeriesLength - windowSize; i++)
		ASumSq[i + 1] = ASqCumSum[windowSize + i] - ASqCumSum[i];
	for (int i = 0; i < ProfileLength; i++){
		    AMean[i] = ASum[i] / windowSize;
	    }
	tsa_dtype* ASigmaSq = new tsa_dtype[ProfileLength];
	for (int i = 0; i < ProfileLength; i++)
		ASigmaSq[i] = ASumSq[i] / windowSize - AMean[i] * AMean[i];

	for (int i = 0; i < ProfileLength; i++) {
		ASigmaInv[i] = 1.0/sqrt(ASigmaSq[i]);
		AMeanScaledSigSqrM[i] = AMean[i]*ASigmaInv[i]*sqrt_m;
	}
	delete [] ACumSum;
	delete [] ASqCumSum;
	delete [] ASum;
	delete [] ASumSq;
	delete [] ASigmaSq;
}

void ScrimpSequOpt::init_diagonals(const int first_diag, const int last_diag, aligned_tsdtype_vec& initial_zs, const aligned_tsdtype_vec& A, const int windowSize)
{
	const size_t profileLength = A.size() - windowSize+1;
//	initial_zs.reserve(profileLength);
	assert(initial_zs.size() >= profileLength);
	//evaluate the fist distance value in the current diagonal
	for (size_t diag = first_diag; diag <= last_diag; ++diag) {
		tsa_dtype lastz=0;
		for (int k = 0; k < windowSize; k++)
		{
			lastz += A[k+diag]*A[k];
		}
		initial_zs[diag] = lastz;
//std::cout << "inited " << diag << " with " << lastz << std::endl;
	}
}

void ScrimpSequOpt::init_all_diagonals(aligned_tsdtype_vec& initial_zs, const aligned_tsdtype_vec& A, const int windowSize) {
	//init diagonals 0 to profileLength-1
	init_diagonals(0, A.size()-windowSize+1, initial_zs, A, windowSize);
}

void ScrimpSequOpt::eval_diagonal(aligned_int_vec& profileIndex, const aligned_tsdtype_vec& A, const aligned_tsdtype_vec& initial_zs, const int windowSize, const aligned_tsdtype_vec& ASigmaInv, const aligned_tsdtype_vec& AMeanScaledSigSqrM, const int diag, aligned_tsdtype_vec& profile)
{
	const int profileLength = AMeanScaledSigSqrM.size();
	tsa_dtype corrScore;
#ifdef PROFILING
	long updateCtr = 0;
#endif

	tsa_dtype tmpz = initial_zs[diag]; //rather use a local, to avoid innecessary writes to the referenced memory
	for (int j=diag; j<profileLength; j++)
	{
		int i=j-diag;

		corrScore = (tmpz* (ASigmaInv[j] * ASigmaInv[i]) - AMeanScaledSigSqrM[j] * AMeanScaledSigSqrM[i]) ;
		tmpz += A[j+windowSize]*A[i+windowSize]  - A[j]*A[i];

		if (corrScore > profile[j])
		{
			profile[j] = corrScore;
			profileIndex [j] = i;
#ifdef PROFILING
			updateCtr+=1;
#endif
		}
		if (corrScore > profile[i])
		{
			profile[i] = corrScore;
			profileIndex [i] = j;
#ifdef PROFILING
			updateCtr+=1;
#endif
		}
	}

#ifdef PROFILING
	_profileUpdateCounter += updateCtr;
#endif
}

void ScrimpSequOpt::compute_matrix_profile(const Scrimppp_params& params)
{
	std::chrono::high_resolution_clock::time_point tstart, tend;
	std::chrono::duration<double> time_elapsed;
	aligned_tsdtype_vec A = fetch_time_series<aligned_tsdtype_vec::allocator_type>(params); //load the time series data
	aligned_tsdtype_vec AMeanScaledSqrtM(A.size());
	aligned_tsdtype_vec ASigmaInv(A.size());
	int windowSize = params.query_window_len;
	int exclusionZone = windowSize / 4;
	int timeSeriesLength = A.size();
	int ProfileLength = timeSeriesLength - windowSize + 1;
	//Initialize Matrix Profile and Matrix Profile Index
	aligned_tsdtype_vec profile(ProfileLength, 0.0);
	aligned_int_vec profileIndex(ProfileLength, 0);
	aligned_tsdtype_vec initial_zs(timeSeriesLength); // stores products between two Timeseries values with a distinct offset
	std::vector<int> idx; // store indices of the diagonals, defining their evaluation order
	idx.reserve(ProfileLength-exclusionZone-1);

	//several monitors for performance measurement
	PerfCounters setup_perf("setup");
	PerfCounters init_diag_perf("diagonal initialization");
	PerfCounters eval_diag_perf("diagonal evaluation");

	//validation of parameters
	if (timeSeriesLength < windowSize) {
		throw std::invalid_argument("ERROR: Time series is shorter than the window length, can not proceed");
	}

	EXEC_INFO( "Sequential SCRIMP matrix profile computation with profile length " << ProfileLength << " and window size " << windowSize);

	{
		ScopedPerfAccumulator monitor(setup_perf);
		//precompute the mean and standard deviations of the sliding windows along the time series
		precompute_window_statistics(windowSize, A, ProfileLength, AMeanScaledSqrtM, ASigmaInv);

		//start time measurment
		tstart = std::chrono::high_resolution_clock::now();

		/******************** SCRIMP ********************/
		//Random shuffle the computation order of the diagonals of the distance matrix
		for (int i = exclusionZone+1; i < ProfileLength; i++) {
			idx.push_back(i);
		}
		std::random_shuffle(idx.begin(), idx.end());
	}

	// compute the first correlation values in the diagonals (i.e. compute the correlation between the first windows)
	{
		ScopedPerfAccumulator monitor(init_diag_perf);
		init_all_diagonals(initial_zs, A, windowSize);
	}
	//iteratively evaluate the diagonals of the distance matrix
	for (int ri = 0; ri < idx.size(); ri++)
	    {
		//select a random diagonal
		int diag = idx[ri];

		//evaluate the second to the last distance values along the diagonal in the matrix and update the matrix profile/matrix profile index.
		{
			ScopedPerfAccumulator monitor(eval_diag_perf);
			eval_diagonal(profileIndex, A, initial_zs, windowSize, ASigmaInv, AMeanScaledSqrtM, diag, profile);
		}

		//Show time per 10000 iterations
		if ((ri+1) % notification_interval_iter == 0)
		{
			tend = std::chrono::high_resolution_clock::now();
			time_elapsed = tend - tstart;
			EXEC_INFO ( "finished " << ri+1 << " iterations after " << std::setprecision(std::numeric_limits<tsa_dtype>::digits10 + 2) << time_elapsed.count() << " seconds.");
		}
	}

	// apply a correction of the distance values, as we dropped a factor of 2 to avoid unnecessary computations
	tsa_dtype twice_m = 2.0*static_cast<tsa_dtype>(windowSize);
	for (auto iter=profile.begin(); iter<profile.end(); ++iter) {
		(*iter) = twice_m - 2.0 * (*iter);
	}

	// end timer
	// tend = time(0);
	tend = std::chrono::high_resolution_clock::now();
	time_elapsed = tend - tstart;

	PERF_LOG ( "total computation time: " << std::setprecision(std::numeric_limits<tsa_dtype>::digits10 + 2) << time_elapsed.count() << " seconds." );
	const double triang_len = ProfileLength-exclusionZone;
	PERF_LOG ( "throughput computations: " << triang_len * triang_len / time_elapsed.count() << " matrix entries/second");

	//store the result
	store_matrix_profile(profile, profileIndex, params);

	setup_perf.log_perf();
	init_diag_perf.log_perf();
	eval_diag_perf.log_perf();
#ifdef PROFILING
	PERF_LOG ( "number of matrix profile updates: " << _profileUpdateCounter);
#endif

}


/**
 * @file   svd_armadillo.cc
 * @author Jiangwen Su <uukuguy@gmail.com>
 * @date   2014-12-12 01:06:49
 *
 * @brief
 *
 *
 */

#include "docset.h"
#include "document.h"
#include "term.h"
#include "logger.h"
#include "lexicon.h"
#include "dmat.h"
#include "smat.h"
#include "svd.h"

//#include <armadillo>
#include <mlpack/methods/quic_svd/quic_svd.hpp>
#include <mlpack/methods/regularized_svd/regularized_svd.hpp>

typedef struct svd_data_t{
    Document *pDocument;
    arma::mat &A;
} svd_data_t;

//void lexicon_term_loop_armadillo(term_t *term, void *user_data)
//{
    //svd_data_t *svd_data = (svd_data_t*)user_data;
    //doc_t *doc = svd_data->doc;
    //arma::mat &A = svd_data->A;

    //if ( doc_has_term(doc, term) == 1 ) {
        //uint32_t row = term_get_id(term);
        //uint32_t col = doc_get_id(doc);
        ////uint32_t row = doc_get_id(doc);
        ////uint32_t col = term_get_id(term);

        //double term_tfidf = doc_get_term_tfidf(doc, term);
        //A(row, col) = term_tfidf;
    //}
//}

dmat_t *docset_save_corrmat_armadillo(docset_t *docset, arma::mat& C, const char *filename)
{
    uint32_t numRows = C.n_rows;
    uint32_t numCols = C.n_cols;
    //dmat_free(docset->corrmat);

    dmat_t *corrmat = dmat_new(numRows, numCols);
    double *values = dmat_get_values(corrmat);
    for ( uint32_t row = 0 ; row < numRows ; row++ ){
        for ( uint32_t col = 0 ; col < numCols ; col++ ){
            double v = C(row, col);
            *values = v;
            values++;
        }
    }

    //docset->corrmat = corrmat;

    //dmat_save_to_csv(matrix, filename);

    return corrmat;
}

#include <fstream>
#include <iomanip>
int save_singular_value_armadillo(arma::vec &s, const char *filename)
{
    std::fstream out(filename, std::ios::out);
    out << std::setprecision(6);
    for ( uint32_t i = 0 ; i < s.size() ; i++ ){
        out << s(i) << std::endl;
    }
    return 0;
}

double vector_angle_cosine_armadillo(arma::vec &v1, arma::vec &v2, uint32_t size)
{
	// A(dot)B = |A||B|Cos(theta)
	// so Cos(theta) = A(dot)B / |A||B|

	double a_dot_b=0;
	for ( uint32_t i = 0 ; i < size ; i++ ) {
		a_dot_b += v1(i) * v2(i);
	}

	double A=0;
	for ( uint32_t j = 0 ; j < size ; j++ ) {
        A += v1(j) * v1(j);
	}
	A = sqrt(A);

	double B=0;
	for ( uint32_t k = 0 ; k < size ; k++ ) {
        B += v2(k) * v2(k);
	}
	B = sqrt(B);

	return a_dot_b / (A * B);
}

int docset_query_armadillo(docset_t *docset, const char *query_string, uint32_t dimensions)
{
    if ( query_string == NULL || strlen(query_string) == 0 )
        return -1;

    Docset *pDocset = (Docset*)(docset->pDocset);
    const Lexicon &lexicon = pDocset->get_lexicon();

    uint32_t numTerms = pDocset->get_total_terms();
    uint32_t numDocs = pDocset->get_total_docs();
    arma::vec q_vct(numTerms, arma::fill::zeros);

    std::istringstream query_stream(query_string);
    std::string word;
	while (query_stream >> word) {
        Term *pTerm = lexicon.get_term_by_text(word);
        if ( pTerm != NULL ){
            uint32_t term_id = pTerm->m_id;
            q_vct(term_id) = q_vct(term_id) + 1;
        }
    };

    //arma::vec &s = *(arma::vec*)docset->svd_s;
    //arma::mat &U = *(arma::mat*)docset->svd_U;
    //arma::mat &V = *(arma::mat*)docset->svd_V;
    arma::vec s;
    arma::mat U;
    arma::mat V;

    arma::vec d_vct(dimensions, arma::fill::zeros);
	// Dq = Xq' T S^-1
	for (uint32_t i = 0; i < dimensions; i++) {
		double sum = 0;
		for (uint32_t j = 0; j < numTerms; j++) {
            sum += q_vct(j) * U(j,i);
		}
        d_vct(i) = sum * ( 1 / s(i));
	}

	//compare each document with Dq
	for ( uint32_t n = 0 ; n < numDocs ; n++ ) {
        arma::vec t_vct(dimensions, arma::fill::zeros);
		// fill temp document vector
		for ( uint32_t m = 0 ; m < dimensions ; m++) {
            t_vct(m) = V(n,m) * s(m);
		}
        AddCorrelation(n, vector_angle_cosine_armadillo(d_vct, t_vct, dimensions));
	}

    uint32_t n = 0;
    std::vector<struct doc_cor>::const_iterator it;
	for ( it = g_cor.begin() ; it < g_cor.end() ; it++, n++ ) {
        doc_cor cor = *it;
        uint32_t doc_id = cor.doc_id;
        double correlation = cor.correlation;

        Document *pDocument = pDocset->get_document_by_id(doc_id);
        const char *doc_name = "<not found>";
        if ( pDocument != NULL ){
            doc_name = pDocument->m_title.c_str();
        }

        if ( n > g_cor.size() - 10 ) {
            warning_log("%d:<%d,%.3f>%s", n, doc_id, correlation, doc_name);
        } else if ( n < 10 ) {
            notice_log("%d:<%d,%.3f>%s", n, doc_id, correlation, doc_name);
        }

    }
    return 0;
}

int export_vector_to_csv(arma::vec V, const char *filename)
{
    std::fstream out(filename, std::ios::out);

    for ( uint32_t n = 0 ; n < V.size() ; n++ ){
        out << "C" << n;
        if ( n < V.size() - 1 )
            out << ",";
    }
    out << std::endl;

    for ( uint32_t n = 0 ; n < V.size() ; n++ ){
        out << V(n);
        if ( n < V.size() - 1 )
            out << ",";
    }
    out << std::endl;

    return 0;
}

int export_matrix_to_csv(arma::mat& C, const char *filename)
{
    uint32_t numRows = C.n_rows;
    uint32_t numCols = C.n_cols;

    std::fstream out(filename, std::ios::out);

    out << "id,";
    for ( uint32_t col = 0 ; col < numCols ; col++ ){
        out << "C" << col;
        if ( col < numCols - 1 )
            out << ",";
    }
    out << std::endl;

    for ( uint32_t row = 0 ; row < numRows ; row++ ){
        out << "R" << row << ", ";
        for ( uint32_t col = 0 ; col < numCols ; col++ ){
            double v = C(row, col);
            out << v;
            if ( col < numCols - 1 )
                out << ",";
        }
        out << std::endl;
    }
    out << std::endl;

    return 0;
}

/* ==================== docset_do_svd_armadillo() ==================== */
void docset_do_svd_armadillo(docset_t *docset, uint32_t dimensions)
{
    GET_TIME_MILLIS(msec0);

    Docset *pDocset = (Docset*)(docset->pDocset);
    uint32_t numRows = pDocset->get_total_terms();
    uint32_t numCols = pDocset->get_total_docs();
    //uint32_t totalNonZeroValues = pDocset->calculate_nonzerovalues();

    //smat_t *tfm = smat_new(numRows, numCols, totalNonZeroValues);
    smat_t *tfm = pDocset->calculate_tfmatrix();

    arma::mat A(numRows, numCols, arma::fill::zeros);
    //arma::sp_mat A(numRows, numCols);

    SMAT_LOOP_BEGIN(tfm, numRows, numCols);
    A(row, col) = value;
    SMAT_LOOP_END();
    //uint32_t v = 0;
    //for ( uint32_t col = 0 ; col < numCols ; col++ ){
        //for ( ; v < tfm->pointr[col + 1]; v++) {
            //uint32_t row = tfm->rowind[v];
            //double value = tfm->values[v];
            //A(row, col) = value;
        //}

    //}

    smat_free(tfm);

    GET_TIME_MILLIS(msec1);
    notice_log("svd prepare: %llu.%03llu sec.", (msec1 - msec0) / 1000, (msec1 - msec0) % 1000);

    //docset->svd_U = (void*)new arma::mat();
    //docset->svd_s = (void*)new arma::vec();
    //docset->svd_V = (void*)new arma::mat();
    //arma::mat &U = *(arma::mat*)docset->svd_U;
    //arma::vec &s = *(arma::vec*)docset->svd_s;
    //arma::mat &V = *(arma::mat*)docset->svd_V;
    arma::mat U;
    arma::vec s;
    arma::mat V;

    //uint32_t rank = 10;
    //uint32_t iterations = 10;
    //double alpha = 0.01;
    //double lambda = 0.02;
    //mlpack::svd::RegularizedSVD<> svd(A, U, V, rank, iterations, alpha, lambda);

    mlpack::svd::QUIC_SVD svd(A, U, V, s, 0.03, 0.1);

    //const char *side = "both";
    ////const char *side = "left";
    ////const char *side = "right";
    ////const char *mode = "d";
    //const char *mode = "s";
    //arma::svd_econ(U, s, V, A, side, mode);

    uint32_t sv_cnt = s.size();
    printf("Singular Values (%d,%d)\n", sv_cnt, sv_cnt);
    for ( uint32_t n = 0 ; n < sv_cnt ; n++ ){
        if ( n < 20 ) {
            printf("%.6f ", s(n));
        } else if ( n == 20 ){
            printf("\n......\n");
        } else if ( n > sv_cnt - 20) {
            printf("%.6f ", s(n));
        }
    }
    printf("\nU(%d,%d) s(%d,%d) V(%d, %d)\n", U.n_rows, U.n_cols, s.n_rows, s.n_cols, V.n_rows, V.n_cols);

    GET_TIME_MILLIS(msec2);

    save_singular_value_armadillo(s, "test-s");

    // Reduce dimensions
    arma::mat S(s.n_rows, s.n_rows, arma::fill::zeros);
    for ( uint32_t i = 0 ; i < s.n_rows ; i++ ){
        if ( i >= dimensions )
            s(i) = 0.0;
        S(i,i) = s(i);
    }

    //printf("Building CorrMatrix...\n");
    //arma::mat C = U * S * V;

    GET_TIME_MILLIS(msec21);

    printf("Saving CorrMatrix...\n");
    //docset_save_corrmat_armadillo(docset, C, "test.corrmat");

    export_matrix_to_csv(U, "./U.csv");
    export_matrix_to_csv(V, "./V.csv");
    export_vector_to_csv(s, "./s.csv");

    GET_TIME_MILLIS(msec3);

    notice_log("svd do: %llu.%03llu sec.", (msec2 - msec1) / 1000, (msec2 - msec1) % 1000);
    notice_log("Build CorrMatrix do: %llu.%03llu sec.", (msec21 - msec2) / 1000, (msec21 - msec2) % 1000);
    notice_log("Save CorrMatrix do: %llu.%03llu sec.", (msec3 - msec21) / 1000, (msec3 - msec21) % 1000);

    notice_log("svd total: %llu.%03llu sec.", (msec3 - msec0) / 1000, (msec3 - msec0) % 1000);

}


#include "pow.h"

#include <cmath>

#include <boost/test/unit_test.hpp>

using namespace boost;
using namespace std;

namespace
{
	float expf4to1(float x)
	{
		__declspec(align(16)) float v[4];
		_mm_store_ps(v, expf4(_mm_set1_ps(x)));
		return v[0];
	}

	float expf8to1(float x)
	{
		__declspec(align(32)) float v[8];
		_mm256_store_ps(v, expf8(_mm256_set1_ps(x)));
		return v[0];
	}
}

BOOST_AUTO_TEST_CASE(Expf)
{
	const float tolerance = 0.001f;

	const float args[] = { 0.01f, 0.1f, 0.5f, 1.0f, 2.0f, 5.0f };
	for (float arg : args)
	{
		BOOST_CHECK_CLOSE(expf4to1( arg), exp( arg), tolerance);
		BOOST_CHECK_CLOSE(expf4to1(-arg), exp(-arg), tolerance);
		BOOST_CHECK_CLOSE(expf8to1( arg), exp( arg), tolerance);
		BOOST_CHECK_CLOSE(expf8to1(-arg), exp(-arg), tolerance);
	}
}

BOOST_AUTO_TEST_CASE(Expf4_Fullness)
{
	const float x = 0.1f;

	__declspec(align(16)) float v[4];
	_mm_store_ps(v, expf4(_mm_set1_ps(x)));

	for (size_t i = 1; i != 4; ++i)
		BOOST_CHECK_EQUAL(v[0], v[i]);
}

BOOST_AUTO_TEST_CASE(Expf8_Fullness)
{
	const float x = 0.1f;

	__declspec(align(32)) float v[8];
	_mm256_store_ps(v, expf8(_mm256_set1_ps(x)));

	for (size_t i = 1; i != 8; ++i)
		BOOST_CHECK_EQUAL(v[0], v[i]);
}


//  (C) Copyright John Maddock 2006.
//  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)

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/test_data.hpp>
#include <fstream>
#include "mp_t.hpp"

using namespace boost::math::tools;

struct beta_data_generator
{
   mp_t operator()(mp_t a, mp_t b)
   {
      if(a < b)
         throw std::domain_error("");
      // very naively calculate spots:
      mp_t g1, g2, g3;
      int s1, s2, s3;
      g1 = boost::math::lgamma(a, &s1);
      g2 = boost::math::lgamma(b, &s2);
      g3 = boost::math::lgamma(a+b, &s3);
      g1 += g2 - g3;
      g1 = exp(g1);
      g1 *= s1 * s2 * s3;
      return g1;
   }
};

int main()
{
   parameter_info<mp_t> arg1, arg2;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the beta function:\n"
      "  beta(a, b)\n\n";

   bool cont;
   std::string line;

   do{
      get_user_parameter_info(arg1, "a");
      get_user_parameter_info(arg2, "b");
      data.insert(beta_data_generator(), arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=beta_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "beta_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "beta_data");

   return 0;
}


//  (C) Copyright Nick Thompson 2019.
//  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_TOOLS_CONDITION_NUMBERS_HPP
#define BOOST_MATH_TOOLS_CONDITION_NUMBERS_HPP
#include <cmath>
#include <boost/math/differentiation/finite_difference.hpp>

namespace boost::math::tools {

template<class Real, bool kahan=true>
class summation_condition_number {
public:
    summation_condition_number(Real const x = 0)
    {
        using std::abs;
        m_l1 = abs(x);
        m_sum = x;
        m_c = 0;
    }

    void operator+=(Real const & x)
    {
        using std::abs;
        // No need to Kahan the l1 calc; it's well conditioned:
        m_l1 += abs(x);
        if constexpr(kahan)
        {
            Real y = x - m_c;
            Real t = m_sum + y;
            m_c = (t-m_sum) -y;
            m_sum = t;
        }
        else
        {
            m_sum += x;
        }
    }

    inline void operator-=(Real const & x)
    {
        this->operator+=(-x);
    }

    // Is operator*= relevant? Presumably everything gets rescaled,
    // (m_sum -> k*m_sum, m_l1->k*m_l1, m_c->k*m_c),
    // but is this sensible? More important is it useful?
    // In addition, it might change the condition number.

    [[nodiscard]] Real operator()() const
    {
        using std::abs;
        if (m_sum == Real(0) && m_l1 != Real(0))
        {
            return std::numeric_limits<Real>::infinity();
        }
        return m_l1/abs(m_sum);
    }

    [[nodiscard]] Real sum() const
    {
        // Higham, 1993, "The Accuracy of Floating Point Summation":
        // "In [17] and [18], Kahan describes a variation of compensated summation in which the final sum is also corrected
        // thus s=s+e is appended to the algorithm above)."
        return m_sum + m_c;
    }

    [[nodiscard]] Real l1_norm() const
    {
        return m_l1;
    }

private:
    Real m_l1;
    Real m_sum;
    Real m_c;
};

template<class F, class Real>
Real evaluation_condition_number(F const & f, Real const & x)
{
    using std::abs;
    using std::isnan;
    using std::sqrt;
    using boost::math::differentiation::finite_difference_derivative;

    Real fx = f(x);
    if (isnan(fx))
    {
        return std::numeric_limits<Real>::quiet_NaN();
    }
    bool caught_exception = false;
    Real fp;
    try
    {
        fp = finite_difference_derivative(f, x);
    }
    catch(...)
    {
        caught_exception = true;
    }

    if (isnan(fp) || caught_exception)
    {
        // Check if the right derivative exists:
        fp = finite_difference_derivative<decltype(f), Real, 1>(f, x);
        if (isnan(fp))
        {
            // Check if a left derivative exists:
            const Real eps = (std::numeric_limits<Real>::epsilon)();
            Real h = - 2 * sqrt(eps);
            h = boost::math::differentiation::detail::make_xph_representable(x, h);
            Real yh = f(x + h);
            Real y0 = f(x);
            Real diff = yh - y0;
            fp = diff / h;
            if (isnan(fp))
            {
                return std::numeric_limits<Real>::quiet_NaN();
            }
        }
    }

    if (fx == 0)
    {
        if (x==0 || fp==0)
        {
            return std::numeric_limits<Real>::quiet_NaN();
        }
        return std::numeric_limits<Real>::infinity();
    }

    return abs(x*fp/fx);
}

}
#endif


#ifndef POLYVEC_DERIV_TEST_IS_INCLUDED
#define POLYVEC_DERIV_TEST_IS_INCLUDED

#include <cstring>
#include <iomanip>
#include <iostream>

#include <Eigen/Core>

namespace polyvec{

namespace derivtest {
//
// Finite difference test
//

inline void
run(const Eigen::VectorXd &state, const Eigen::VectorXd &delta,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> eval,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> eval_jacobian,
    std::ostream &cout = std::cout, int n_halving = 5, double h = 0.5) {
  char dump[4096];
  double diff0(0), diff1(0);

  cout << std::setw(8) << "n" << std::setw(16) << "DIFF0" << std::setw(16)
       << "DIFF1" << std::endl;

  Eigen::VectorXd value0 = eval(state);
  for (int ih = 0; ih < n_halving; ++ih) {
    h /= 2.;

    Eigen::VectorXd hdelta = h * delta;
    Eigen::VectorXd value1exact = eval(state + hdelta);

    eval(state);
    Eigen::VectorXd linear_correction = eval_jacobian(hdelta);
    Eigen::VectorXd value1lin = value0 + linear_correction;

    double diff0new = (value0 - value1exact).norm();
    double diff1new = (value1lin - value1exact).norm();
    sprintf(dump, "%8d %8.4e(%4.1f) %8.4e(%4.1f)  \n", ih, diff0new,
            diff0 / diff0new, diff1new, diff1 / diff1new);
    cout << dump;
    cout.flush();
    diff0 = diff0new;
    diff1 = diff1new;
  }
} // All done

inline void
run(const Eigen::VectorXd &state, const Eigen::VectorXd &delta,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> eval,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> eval_jacobian,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> eval_hessian,
    std::ostream &cout = std::cout, int n_halving = 5, double h = 0.5) {
  char dump[4096];
  double diff0(0), diff1(0), diff2(0);

  cout << std::setw(8) << "n" << std::setw(16) << "DIFF0" << std::setw(16)
       << "DIFF1" << std::setw(16) << "DIFF2\n";

  Eigen::VectorXd value0 = eval(state);
  for (int ih = 0; ih < n_halving; ++ih) {
    h /= 2.;

    Eigen::VectorXd hdelta = h * delta;
    Eigen::VectorXd value1exact = eval(state + hdelta);

    eval(state);
    Eigen::VectorXd value1lin = value0 + eval_jacobian(hdelta);
    Eigen::VectorXd value1quad = value1lin + 0.5 * eval_hessian(hdelta);

    double diff0new = (value0 - value1exact).norm();
    double diff1new = (value1lin - value1exact).norm();
    double diff2new = (value1quad - value1exact).norm();
    sprintf(dump, "%8d %8.4e(%4.0f) %8.4e(%4.0f)  %8.4e(%4.0f)  \n", ih,
            diff0new, diff0 / diff0new, diff1new, diff1 / diff1new, diff2new,
            diff2 / diff2new);
    cout << dump;
    cout.flush();
    diff0 = diff0new;
    diff1 = diff1new;
    diff2 = diff2new;
  }
} // All done

} // namespace derivtest
} // namespace polyvec

#endif /* CONTRIB_DERIV_TEST_IS_INCLUDED */