// boost\math\special_functions\negative_binomial.hpp

// Copyright Paul A. Bristow 2007.
// Copyright John Maddock 2007.

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

// http://en.wikipedia.org/wiki/negative_binomial_distribution
// http://mathworld.wolfram.com/NegativeBinomialDistribution.html
// http://documents.wolfram.com/teachersedition/Teacher/Statistics/DiscreteDistributions.html

// The negative binomial distribution NegativeBinomialDistribution[n, p]
// is the distribution of the number (k) of failures that occur in a sequence of trials before
// r successes have occurred, where the probability of success in each trial is p.

// In a sequence of Bernoulli trials or events
// (independent, yes or no, succeed or fail) with success_fraction probability p,
// negative_binomial is the probability that k or fewer failures
// preceed the r th trial's success.
// random variable k is the number of failures (NOT the probability).

// Negative_binomial distribution is a discrete probability distribution.
// But note that the negative binomial distribution
// (like others including the binomial, Poisson & Bernoulli)
// is strictly defined as a discrete function: only integral values of k are envisaged.
// However because of the method of calculation using a continuous gamma function,
// it is convenient to treat it as if a continous function,
// and permit non-integral values of k.

// However, by default the policy is to use discrete_quantile_policy.

// To enforce the strict mathematical model, users should use conversion
// on k outside this function to ensure that k is integral.

// MATHCAD cumulative negative binomial pnbinom(k, n, p)

// Implementation note: much greater speed, and perhaps greater accuracy,
// might be achieved for extreme values by using a normal approximation.
// This is NOT been tested or implemented.

#ifndef BOOST_MATH_SPECIAL_NEGATIVE_BINOMIAL_HPP
#define BOOST_MATH_SPECIAL_NEGATIVE_BINOMIAL_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/beta.hpp> // for ibeta(a, b, x) == Ix(a, b).
#include <boost/math/distributions/complement.hpp> // complement.
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks domain_error & logic_error.
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/distributions/detail/inv_discrete_quantile.hpp>

#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>

#include <limits> // using std::numeric_limits;
#include <utility>

#if defined (BOOST_MSVC)
#  pragma warning(push)
// This believed not now necessary, so commented out.
//#  pragma warning(disable: 4702) // unreachable code.
// in domain_error_imp in error_handling.
#endif

namespace boost
{
  namespace math
  {
    namespace negative_binomial_detail
    {
      // Common error checking routines for negative binomial distribution functions:
      template <class RealType, class Policy>
      inline bool check_successes(const char* function, const RealType& r, RealType* result, const Policy& pol)
      {
        if( !(boost::math::isfinite)(r) || (r <= 0) )
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of successes argument is %1%, but must be > 0 !", r, pol);
          return false;
        }
        return true;
      }
      template <class RealType, class Policy>
      inline bool check_success_fraction(const char* function, const RealType& p, RealType* result, const Policy& pol)
      {
        if( !(boost::math::isfinite)(p) || (p < 0) || (p > 1) )
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Success fraction argument is %1%, but must be >= 0 and <= 1 !", p, pol);
          return false;
        }
        return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& r, const RealType& p, RealType* result, const Policy& pol)
      {
        return check_success_fraction(function, p, result, pol)
          && check_successes(function, r, result, pol);
      }
      template <class RealType, class Policy>
      inline bool check_dist_and_k(const char* function, const RealType& r, const RealType& p, RealType k, RealType* result, const Policy& pol)
      {
        if(check_dist(function, r, p, result, pol) == false)
        {
          return false;
        }
        if( !(boost::math::isfinite)(k) || (k < 0) )
        { // Check k failures.
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of failures argument is %1%, but must be >= 0 !", k, pol);
          return false;
        }
        return true;
      } // Check_dist_and_k

      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, const RealType& r, RealType p, RealType prob, RealType* result, const Policy& pol)
      {
        if(check_dist(function, r, p, result, pol) && detail::check_probability(function, prob, result, pol) == false)
        {
          return false;
        }
        return true;
      } // check_dist_and_prob
    } //  namespace negative_binomial_detail

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

      negative_binomial_distribution(RealType r, RealType p) : m_r(r), m_p(p)
      { // Constructor.
        RealType result;
        negative_binomial_detail::check_dist(
          "negative_binomial_distribution<%1%>::negative_binomial_distribution",
          m_r, // Check successes r > 0.
          m_p, // Check success_fraction 0 <= p <= 1.
          &result, Policy());
      } // negative_binomial_distribution constructor.

      // Private data getter class member functions.
      RealType success_fraction() const
      { // Probability of success as fraction in range 0 to 1.
        return m_p;
      }
      RealType successes() const
      { // Total number of successes r.
        return m_r;
      }

      static RealType find_lower_bound_on_p(
        RealType trials,
        RealType successes,
        RealType alpha) // alpha 0.05 equivalent to 95% for one-sided test.
      {
        static const char* function = "boost::math::negative_binomial<%1%>::find_lower_bound_on_p";
        RealType result = 0;  // of error checks.
        RealType failures = trials - successes;
        if(false == detail::check_probability(function, alpha, &result, Policy())
          && negative_binomial_detail::check_dist_and_k(
          function, successes, RealType(0), failures, &result, Policy()))
        {
          return result;
        }
        // Use complement ibeta_inv function for lower bound.
        // This is adapted from the corresponding binomial formula
        // here: http://www.itl.nist.gov/div898/handbook/prc/section2/prc241.htm
        // This is a Clopper-Pearson interval, and may be overly conservative,
        // see also "A Simple Improved Inferential Method for Some
        // Discrete Distributions" Yong CAI and K. KRISHNAMOORTHY
        // http://www.ucs.louisiana.edu/~kxk4695/Discrete_new.pdf
        //
        return ibeta_inv(successes, failures + 1, alpha, static_cast<RealType*>(0), Policy());
      } // find_lower_bound_on_p

      static RealType find_upper_bound_on_p(
        RealType trials,
        RealType successes,
        RealType alpha) // alpha 0.05 equivalent to 95% for one-sided test.
      {
        static const char* function = "boost::math::negative_binomial<%1%>::find_upper_bound_on_p";
        RealType result = 0;  // of error checks.
        RealType failures = trials - successes;
        if(false == negative_binomial_detail::check_dist_and_k(
          function, successes, RealType(0), failures, &result, Policy())
          && detail::check_probability(function, alpha, &result, Policy()))
        {
          return result;
        }
        if(failures == 0)
           return 1;
        // Use complement ibetac_inv function for upper bound.
        // Note adjusted failures value: *not* failures+1 as usual.
        // This is adapted from the corresponding binomial formula
        // here: http://www.itl.nist.gov/div898/handbook/prc/section2/prc241.htm
        // This is a Clopper-Pearson interval, and may be overly conservative,
        // see also "A Simple Improved Inferential Method for Some
        // Discrete Distributions" Yong CAI and K. KRISHNAMOORTHY
        // http://www.ucs.louisiana.edu/~kxk4695/Discrete_new.pdf
        //
        return ibetac_inv(successes, failures, alpha, static_cast<RealType*>(0), Policy());
      } // find_upper_bound_on_p

      // Estimate number of trials :
      // "How many trials do I need to be P% sure of seeing k or fewer failures?"

      static RealType find_minimum_number_of_trials(
        RealType k,     // number of failures (k >= 0).
        RealType p,     // success fraction 0 <= p <= 1.
        RealType alpha) // risk level threshold 0 <= alpha <= 1.
      {
        static const char* function = "boost::math::negative_binomial<%1%>::find_minimum_number_of_trials";
        // Error checks:
        RealType result = 0;
        if(false == negative_binomial_detail::check_dist_and_k(
          function, RealType(1), p, k, &result, Policy())
          && detail::check_probability(function, alpha, &result, Policy()))
        { return result; }

        result = ibeta_inva(k + 1, p, alpha, Policy());  // returns n - k
        return result + k;
      } // RealType find_number_of_failures

      static RealType find_maximum_number_of_trials(
        RealType k,     // number of failures (k >= 0).
        RealType p,     // success fraction 0 <= p <= 1.
        RealType alpha) // risk level threshold 0 <= alpha <= 1.
      {
        static const char* function = "boost::math::negative_binomial<%1%>::find_maximum_number_of_trials";
        // Error checks:
        RealType result = 0;
        if(false == negative_binomial_detail::check_dist_and_k(
          function, RealType(1), p, k, &result, Policy())
          &&  detail::check_probability(function, alpha, &result, Policy()))
        { return result; }

        result = ibetac_inva(k + 1, p, alpha, Policy());  // returns n - k
        return result + k;
      } // RealType find_number_of_trials complemented

    private:
      RealType m_r; // successes.
      RealType m_p; // success_fraction
    }; // template <class RealType, class Policy> class negative_binomial_distribution

    typedef negative_binomial_distribution<double> negative_binomial; // Reserved name of type double.

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

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

    template <class RealType, class Policy>
    inline RealType mean(const negative_binomial_distribution<RealType, Policy>& dist)
    { // Mean of Negative Binomial distribution = r(1-p)/p.
      return dist.successes() * (1 - dist.success_fraction() ) / dist.success_fraction();
    } // mean

    //template <class RealType, class Policy>
    //inline RealType median(const negative_binomial_distribution<RealType, Policy>& dist)
    //{ // Median of negative_binomial_distribution is not defined.
    //  return policies::raise_domain_error<RealType>(BOOST_CURRENT_FUNCTION, "Median is not implemented, result is %1%!", std::numeric_limits<RealType>::quiet_NaN());
    //} // median
    // Now implemented via quantile(half) in derived accessors.

    template <class RealType, class Policy>
    inline RealType mode(const negative_binomial_distribution<RealType, Policy>& dist)
    { // Mode of Negative Binomial distribution = floor[(r-1) * (1 - p)/p]
      BOOST_MATH_STD_USING // ADL of std functions.
      return floor((dist.successes() -1) * (1 - dist.success_fraction()) / dist.success_fraction());
    } // mode

    template <class RealType, class Policy>
    inline RealType skewness(const negative_binomial_distribution<RealType, Policy>& dist)
    { // skewness of Negative Binomial distribution = 2-p / (sqrt(r(1-p))
      BOOST_MATH_STD_USING // ADL of std functions.
      RealType p = dist.success_fraction();
      RealType r = dist.successes();

      return (2 - p) /
        sqrt(r * (1 - p));
    } // skewness

    template <class RealType, class Policy>
    inline RealType kurtosis(const negative_binomial_distribution<RealType, Policy>& dist)
    { // kurtosis of Negative Binomial distribution
      // http://en.wikipedia.org/wiki/Negative_binomial is kurtosis_excess so add 3
      RealType p = dist.success_fraction();
      RealType r = dist.successes();
      return 3 + (6 / r) + ((p * p) / (r * (1 - p)));
    } // kurtosis

     template <class RealType, class Policy>
    inline RealType kurtosis_excess(const negative_binomial_distribution<RealType, Policy>& dist)
    { // kurtosis excess of Negative Binomial distribution
      // http://mathworld.wolfram.com/Kurtosis.html table of kurtosis_excess
      RealType p = dist.success_fraction();
      RealType r = dist.successes();
      return (6 - p * (6-p)) / (r * (1-p));
    } // kurtosis_excess

    template <class RealType, class Policy>
    inline RealType variance(const negative_binomial_distribution<RealType, Policy>& dist)
    { // Variance of Binomial distribution = r (1-p) / p^2.
      return  dist.successes() * (1 - dist.success_fraction())
        / (dist.success_fraction() * dist.success_fraction());
    } // variance

    // RealType standard_deviation(const negative_binomial_distribution<RealType, Policy>& dist)
    // standard_deviation provided by derived accessors.
    // RealType hazard(const negative_binomial_distribution<RealType, Policy>& dist)
    // hazard of Negative Binomial distribution provided by derived accessors.
    // RealType chf(const negative_binomial_distribution<RealType, Policy>& dist)
    // chf of Negative Binomial distribution provided by derived accessors.

    template <class RealType, class Policy>
    inline RealType pdf(const negative_binomial_distribution<RealType, Policy>& dist, const RealType& k)
    { // Probability Density/Mass Function.
      BOOST_FPU_EXCEPTION_GUARD

      static const char* function = "boost::math::pdf(const negative_binomial_distribution<%1%>&, %1%)";

      RealType r = dist.successes();
      RealType p = dist.success_fraction();
      RealType result = 0;
      if(false == negative_binomial_detail::check_dist_and_k(
        function,
        r,
        dist.success_fraction(),
        k,
        &result, Policy()))
      {
        return result;
      }

      result = (p/(r + k)) * ibeta_derivative(r, static_cast<RealType>(k+1), p, Policy());
      // Equivalent to:
      // return exp(lgamma(r + k) - lgamma(r) - lgamma(k+1)) * pow(p, r) * pow((1-p), k);
      return result;
    } // negative_binomial_pdf

    template <class RealType, class Policy>
    inline RealType cdf(const negative_binomial_distribution<RealType, Policy>& dist, const RealType& k)
    { // Cumulative Distribution Function of Negative Binomial.
      static const char* function = "boost::math::cdf(const negative_binomial_distribution<%1%>&, %1%)";
      using boost::math::ibeta; // Regularized incomplete beta function.
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      RealType p = dist.success_fraction();
      RealType r = dist.successes();
      // Error check:
      RealType result = 0;
      if(false == negative_binomial_detail::check_dist_and_k(
        function,
        r,
        dist.success_fraction(),
        k,
        &result, Policy()))
      {
        return result;
      }

      RealType probability = ibeta(r, static_cast<RealType>(k+1), p, Policy());
      // Ip(r, k+1) = ibeta(r, k+1, p)
      return probability;
    } // cdf Cumulative Distribution Function Negative Binomial.

      template <class RealType, class Policy>
      inline RealType cdf(const complemented2_type<negative_binomial_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function Negative Binomial.

      static const char* function = "boost::math::cdf(const negative_binomial_distribution<%1%>&, %1%)";
      using boost::math::ibetac; // Regularized incomplete beta function complement.
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      RealType const& k = c.param;
      negative_binomial_distribution<RealType, Policy> const& dist = c.dist;
      RealType p = dist.success_fraction();
      RealType r = dist.successes();
      // Error check:
      RealType result = 0;
      if(false == negative_binomial_detail::check_dist_and_k(
        function,
        r,
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      // Calculate cdf negative binomial using the incomplete beta function.
      // Use of ibeta here prevents cancellation errors in calculating
      // 1-p if p is very small, perhaps smaller than machine epsilon.
      // Ip(k+1, r) = ibetac(r, k+1, p)
      // constrain_probability here?
     RealType probability = ibetac(r, static_cast<RealType>(k+1), p, Policy());
      // Numerical errors might cause probability to be slightly outside the range < 0 or > 1.
      // This might cause trouble downstream, so warn, possibly throw exception, but constrain to the limits.
      return probability;
    } // cdf Cumulative Distribution Function Negative Binomial.

    template <class RealType, class Policy>
    inline RealType quantile(const negative_binomial_distribution<RealType, Policy>& dist, const RealType& P)
    { // Quantile, percentile/100 or Percent Point Negative Binomial function.
      // Return the number of expected failures k for a given probability p.

      // Inverse cumulative Distribution Function or Quantile (percentile / 100) of negative_binomial Probability.
      // MAthCAD pnbinom return smallest k such that negative_binomial(k, n, p) >= probability.
      // k argument may be integral, signed, or unsigned, or floating point.
      // BUT Cephes/CodeCogs says: finds argument p (0 to 1) such that cdf(k, n, p) = y
      static const char* function = "boost::math::quantile(const negative_binomial_distribution<%1%>&, %1%)";
      BOOST_MATH_STD_USING // ADL of std functions.

      RealType p = dist.success_fraction();
      RealType r = dist.successes();
      // Check dist and P.
      RealType result = 0;
      if(false == negative_binomial_detail::check_dist_and_prob
        (function, r, p, P, &result, Policy()))
      {
        return result;
      }

      // Special cases.
      if (P == 1)
      {  // Would need +infinity failures for total confidence.
        result = policies::raise_overflow_error<RealType>(
            function,
            "Probability argument is 1, which implies infinite failures !", Policy());
        return result;
       // usually means return +std::numeric_limits<RealType>::infinity();
       // unless #define BOOST_MATH_THROW_ON_OVERFLOW_ERROR
      }
      if (P == 0)
      { // No failures are expected if P = 0.
        return 0; // Total trials will be just dist.successes.
      }
      if (P <= pow(dist.success_fraction(), dist.successes()))
      { // p <= pdf(dist, 0) == cdf(dist, 0)
        return 0;
      }
      /*
      // Calculate quantile of negative_binomial using the inverse incomplete beta function.
      using boost::math::ibeta_invb;
      return ibeta_invb(r, p, P, Policy()) - 1; //
      */
      RealType guess = 0;
      RealType factor = 5;
      if(r * r * r * P * p > 0.005)
         guess = detail::inverse_negative_binomial_cornish_fisher(r, p, RealType(1-p), P, RealType(1-P), Policy());

      if(guess < 10)
      {
         //
         // Cornish-Fisher Negative binomial approximation not accurate in this area:
         //
         guess = (std::min)(RealType(r * 2), RealType(10));
      }
      else
         factor = (1-P < sqrt(tools::epsilon<RealType>())) ? 2 : (guess < 20 ? 1.2f : 1.1f);
      BOOST_MATH_INSTRUMENT_CODE("guess = " << guess);
      //
      // Max iterations permitted:
      //
      boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
      typedef typename Policy::discrete_quantile_type discrete_type;
      return detail::inverse_discrete_quantile(
         dist,
         P,
         1-P,
         guess,
         factor,
         RealType(1),
         discrete_type(),
         max_iter);
    } // RealType quantile(const negative_binomial_distribution dist, p)

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<negative_binomial_distribution<RealType, Policy>, RealType>& c)
    {  // Quantile or Percent Point Binomial function.
       // Return the number of expected failures k for a given
       // complement of the probability Q = 1 - P.
       static const char* function = "boost::math::quantile(const negative_binomial_distribution<%1%>&, %1%)";
       BOOST_MATH_STD_USING

       // Error checks:
       RealType Q = c.param;
       const negative_binomial_distribution<RealType, Policy>& dist = c.dist;
       RealType p = dist.success_fraction();
       RealType r = dist.successes();
       RealType result = 0;
       if(false == negative_binomial_detail::check_dist_and_prob(
          function,
          r,
          p,
          Q,
          &result, Policy()))
       {
          return result;
       }

       // Special cases:
       //
       if(Q == 1)
       {  // There may actually be no answer to this question,
          // since the probability of zero failures may be non-zero,
          return 0; // but zero is the best we can do:
       }
       if (-Q <= boost::math::powm1(dist.success_fraction(), dist.successes(), Policy()))
       {  // q <= cdf(complement(dist, 0)) == pdf(dist, 0)
          return 0; //
       }
       if(Q == 0)
       {  // Probability 1 - Q  == 1 so infinite failures to achieve certainty.
          // Would need +infinity failures for total confidence.
          result = policies::raise_overflow_error<RealType>(
             function,
             "Probability argument complement is 0, which implies infinite failures !", Policy());
          return result;
          // usually means return +std::numeric_limits<RealType>::infinity();
          // unless #define BOOST_MATH_THROW_ON_OVERFLOW_ERROR
       }
       //return ibetac_invb(r, p, Q, Policy()) -1;
       RealType guess = 0;
       RealType factor = 5;
       if(r * r * r * (1-Q) * p > 0.005)
          guess = detail::inverse_negative_binomial_cornish_fisher(r, p, RealType(1-p), RealType(1-Q), Q, Policy());

       if(guess < 10)
       {
          //
          // Cornish-Fisher Negative binomial approximation not accurate in this area:
          //
          guess = (std::min)(RealType(r * 2), RealType(10));
       }
       else
          factor = (Q < sqrt(tools::epsilon<RealType>())) ? 2 : (guess < 20 ? 1.2f : 1.1f);
       BOOST_MATH_INSTRUMENT_CODE("guess = " << guess);
       //
       // Max iterations permitted:
       //
       boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
       typedef typename Policy::discrete_quantile_type discrete_type;
       return detail::inverse_discrete_quantile(
          dist,
          1-Q,
          Q,
          guess,
          factor,
          RealType(1),
          discrete_type(),
          max_iter);
    } // quantile complement

 } // namespace math
} // namespace boost

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

#if defined (BOOST_MSVC)
# pragma warning(pop)
#endif

#endif // BOOST_MATH_SPECIAL_NEGATIVE_BINOMIAL_HPP


//////////////////////////////////////////////////////////////////////////////
// unit_test_fun.hpp                                                        //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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_STATISTICS_DETAIL_NON_PARAMETRIC_KOLMOGOROV_SMIRNOV_UNIT_TEST_FUN_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_KOLMOGOROV_SMIRNOV_UNIT_TEST_FUN_HPP_ER_2009
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/statistic.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kolmogorov_smirnov{

        template<typename D>
    	struct unit_test_fun{

            typedef typename D::value_type value_type;

            typedef boost::numeric::bounds<value_type> bounds_;

            unit_test_fun() : ks0( bounds_::highest() ){}

            template<typename AccSet>
            void operator()(const AccSet& acc,const D& d,std::ostream& os)const{
                namespace ns = boost::statistics::detail::kolmogorov_smirnov;
            	value_type ks1 = ns::statistic<value_type>( acc, d );
                bool ok = false; // (ks1 - ks0) < bounds_::smallest();
                BOOST_WARN( ok );
                os
                    << '('
                    << boost::accumulators::extract::count(acc)
                    << ','
                    << ks1
                    << ','
                    << ok
                    << ')'
                    << std::endl;

                // Warn not check for 2 reasons:
                // - The inequality is only probabilistic
                // - For very large sample size, numeric error may dominate

                this->ks0 = ks1;

            }

            private:
            mutable value_type ks0;
    	};

}// kolmogorov_smirnov
}// detail
}// statistics
}// boost

#endif


/* test_extreme_value_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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: test_extreme_value_distribution.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

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

#define BOOST_RANDOM_DISTRIBUTION boost::random::extreme_value_distribution<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 1.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 (-100.0)
#define BOOST_RANDOM_TEST1_MAX 0

#define BOOST_RANDOM_TEST2_PARAMS (100.0)
#define BOOST_RANDOM_TEST2_MIN 0

#include "test_distribution.ipp"


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[assign_2d_point
//` Shows the usage of assign to set point coordinates, and, besides that, shows how you can initialize ttmath points with high precision

#include <iostream>
#include <iomanip>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#if defined(HAVE_TTMATH)
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

int main()
{
    using boost::geometry::assign_values;

    boost::geometry::model::d2::point_xy<double> p1;
    assign_values(p1, 1.2345, 2.3456);

#if defined(HAVE_TTMATH)
    boost::geometry::model::d2::point_xy<ttmath::Big<1,4> > p2;
    assign_values(p2, "1.2345", "2.3456"); /*< It is possible to assign coordinates with other types than the coordinate type.
        For ttmath, you can e.g. conveniently use strings. The advantage is that it then has higher precision, because
        if doubles are used for assignments the double-precision is used.
        >*/
#endif

    std::cout
        << std::setprecision(20)
        << boost::geometry::dsv(p1) << std::endl
#if defined(HAVE_TTMATH)
        << boost::geometry::dsv(p2) << std::endl
#endif
        ;

    return 0;
}

//]

//[assign_2d_point_output
/*`
Output:
[pre
(1.2344999999999999, 2.3456000000000001)
(1.2345, 2.3456)
]
*/
//]


// deal.II includes ----------------------------------------------------------------------
#include <deal.II/base/quadrature.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>

// system includes -----------------------------------------------------------------------
#include <Eigen/Dense>
#include <algorithm>
#include <array>
#include <boost/multi_array.hpp>

#ifndef _L2ERRORS_H_
#define _L2ERRORS_H_

namespace boltzmann {

class Errors
{
 private:
  //  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
  //  typedef boost::multi_array<double, 2> matrix_t;
  typedef dealii::DoFHandler<2> dh_t;
  typedef dealii::Vector<double> vec_t;

 public:
  /**
   * Compute L2-distance for tensor-product discretization
   *
   * @param dh DoFHandler
   * @param y1 coefficients
   * @param y2 coefficients
   * @param SN spectral-basis overlap coefficients
   * @param INDEXER map(physical dof, velocity dof) -> global dof
   *
   * @return l2 error
   */
  template <typename VECTOR, typename INDEXER>
  double compute(
      const dh_t& dh, const double* y1, const double* y2, const VECTOR& SN, const INDEXER& indexer);

  /**
   * @brief same as compute, but return also norm of y2 (to compute relative error)
   *
   * @param dh
   * @param y1
   * @param y2
   * @param SN
   * @param indexer
   *
   * @return
   */
  template <typename VECTOR, typename INDEXER>
  std::array<double, 2> compute2(
      const dh_t& dh, const double* y1, const double* y2, const VECTOR& SN, const INDEXER& indexer);

  const vec_t& get_cell_wise_error() const { return cell_wise_error; }

 private:
  dealii::Vector<double> cell_wise_error;
};

template <typename VECTOR, typename INDEXER>
double
Errors::compute(
    const dh_t& dh, const double* y1, const double* y2, const VECTOR& SN, const INDEXER& indexer)
{
  dealii::QGauss<2> quad(2);

  const dealii::UpdateFlags update_flags =
      (dealii::update_values | dealii::update_JxW_values | dealii::update_quadrature_points);
  auto& fe = dh.get_fe();
  dealii::FEValues<2> fe_values(fe, quad, update_flags);

  int dofs_per_cell = fe_values.dofs_per_cell;
  int n_qpoints = quad.size();

  // number of velocity dofs
  const int N = SN.size();
  cell_wise_error.reinit(dh.get_triangulation().n_active_cells());
  std::fill(cell_wise_error.begin(), cell_wise_error.end(), 0.0);

  double error = 0;
  std::vector<unsigned int> local_indices(dofs_per_cell);

  typedef Eigen::Map<const Eigen::VectorXd> vvec_t;

  auto vcontrib = [&](vvec_t& v1, vvec_t& v2) {
    double sum = 0;
    for (int i = 0; i < N; ++i) {
      sum += v1(i) * v2(i) * SN[i];
    }
    return sum;
  };

  double global_error = 0;
  for (auto cell : dh.active_cell_iterators()) {
    double cell_error = 0;
    cell->get_dof_indices(local_indices);
    fe_values.reinit(cell);

    double area = 0;
    for (int q = 0; q < n_qpoints; ++q) {
      area += fe_values.JxW(q);
    }

    for (int ix1 = 0; ix1 < dofs_per_cell; ++ix1) {
      // index to global block
      unsigned int ig1 = indexer.to_global(local_indices[ix1], 0);
      vvec_t v1(y1 + ig1, N);
      vvec_t v2(y2 + ig1, N);

      // off diagonal contribution
      for (int ix2 = 0; ix2 < dofs_per_cell; ++ix2) {
        unsigned int ig2 = indexer.to_global(local_indices[ix2], 0);
        vvec_t v1p(y1 + ig2, N);
        vvec_t v2p(y2 + ig2, N);

        double overlap = 0;
        for (int q = 0; q < n_qpoints; ++q) {
          overlap +=
              fe_values.shape_value(ix1, q) * fe_values.shape_value(ix2, q) * fe_values.JxW(q);
        }

        double verr = vcontrib(v1, v1p) + vcontrib(v2, v2p) - 2 * vcontrib(v1, v2p);
        cell_error += overlap * verr;
      }
    }
    cell_wise_error[cell->index()] = cell_error / area;
    global_error += cell_error;
  }

  return global_error;
}

/**
 * Compute L2-distance for tensor-product discretization
 *
 * @param dh DoFHandler
 * @param y1 coefficients
 * @param y2 coefficients
 * @param SN spectral-basis overlap coefficients
 * @param INDEXER map(physical dof, velocity dof) -> global dof
 *
 * @return l2 error |y1-y2|, |y2|
 */
template <typename VECTOR, typename INDEXER>
std::array<double, 2>
Errors::compute2(
    const dh_t& dh, const double* y1, const double* y2, const VECTOR& SN, const INDEXER& indexer)
{
  dealii::QGauss<2> quad(2);

  const dealii::UpdateFlags update_flags =
      (dealii::update_values | dealii::update_JxW_values | dealii::update_quadrature_points);
  auto& fe = dh.get_fe();
  dealii::FEValues<2> fe_values(fe, quad, update_flags);

  int dofs_per_cell = fe_values.dofs_per_cell;
  int n_qpoints = quad.size();

  // number of velocity dofs
  const int N = SN.size();
  cell_wise_error.reinit(dh.get_triangulation().n_active_cells());
  std::fill(cell_wise_error.begin(), cell_wise_error.end(), 0.0);

  double error = 0;
  double norm = 0;
  std::vector<unsigned int> local_indices(dofs_per_cell);

  typedef Eigen::Map<const Eigen::VectorXd> vvec_t;

  auto vcontrib = [&](vvec_t& v1, vvec_t& v2) {
    double sum = 0;
    for (int i = 0; i < N; ++i) {
      sum += v1(i) * v2(i) * SN[i];
    }
    return sum;
  };

  double global_error = 0;
  for (auto cell : dh.active_cell_iterators()) {
    double cell_error = 0;
    cell->get_dof_indices(local_indices);
    fe_values.reinit(cell);

    double area = 0;
    for (int q = 0; q < n_qpoints; ++q) {
      area += fe_values.JxW(q);
    }

    for (int ix1 = 0; ix1 < dofs_per_cell; ++ix1) {
      // index to global block
      unsigned int ig1 = indexer.to_global(local_indices[ix1], 0);
      vvec_t v1(y1 + ig1, N);
      vvec_t v2(y2 + ig1, N);

      // off diagonal contribution
      for (int ix2 = 0; ix2 < dofs_per_cell; ++ix2) {
        unsigned int ig2 = indexer.to_global(local_indices[ix2], 0);
        vvec_t v1p(y1 + ig2, N);
        vvec_t v2p(y2 + ig2, N);

        double overlap = 0;
        for (int q = 0; q < n_qpoints; ++q) {
          overlap +=
              fe_values.shape_value(ix1, q) * fe_values.shape_value(ix2, q) * fe_values.JxW(q);
        }

        double verr = vcontrib(v1, v1p) + vcontrib(v2, v2p) - 2 * vcontrib(v1, v2p);
        cell_error += overlap * verr;
      }
    }

    // compute |y2|
    for (int ix = 0; ix < dofs_per_cell; ++ix) {
      unsigned int ig = indexer.to_global(local_indices[ix], 0);
      double sum = 0;
      for (int q = 0; q < n_qpoints; ++q) {
        const double f = fe_values.shape_value(ix, q);
        sum += f * f * fe_values.JxW(q);
      }
      vvec_t v2(y1 + ig, N);
      sum *= vcontrib(v2, v2);
      norm += sum;
    }

    cell_wise_error[cell->index()] = cell_error / area;
    global_error += cell_error;
  }
  return std::array<double, 2>({global_error, norm});
}

template <typename DH, typename VECTOR>
double
l2norm(const DH& dh, const VECTOR& vec)
{
  assert(dh.n_dofs() == vec.size());
  dealii::QGauss<2> quad(2);

  const dealii::UpdateFlags update_flags =
      (dealii::update_values | dealii::update_JxW_values | dealii::update_quadrature_points);
  auto& fe = dh.get_fe();
  dealii::FEValues<2> fe_values(fe, quad, update_flags);
  int dofs_per_cell = fe_values.dofs_per_cell;
  int n_qpoints = quad.size();

  double sum = 0;
  std::vector<unsigned int> local_indices(dofs_per_cell);

  for (auto cell : dh.active_cell_iterators()) {
    double cell_error = 0;
    cell->get_dof_indices(local_indices);
    fe_values.reinit(cell);

    for (int ix1 = 0; ix1 < dofs_per_cell; ++ix1) {
      const double c = vec[local_indices[ix1]];
      for (int q = 0; q < n_qpoints; ++q) {
        const double f = c * fe_values.shape_value(ix1, q);
        sum += f * f * fe_values.JxW(q);
      }
    }
  }

  return sum;
}

}  // end namespace boltzmann

#endif /* _L2ERRORS_H_ */


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <g2o/core/robust_kernel_impl.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/plot.hpp>

#include "data.hpp"
#include "helper.hpp"
#include "stereo_processor/g2o_edges/scale_edge.hpp"

class ScaleOptimizer{
public:
  double optimize(const std::vector<cv::Point2f>& fts, const PointsWithUncertainties& pts, double& scale,
          const CameraModel& cam1, const cv::Mat& img0, const cv::Mat& img1, int pymd, int max_opt_step);

  double optimize_pymd(const std::vector<cv::Point2f>& fts, const std::vector<Eigen::Vector3d>& pts, const std::vector<double>& uncertainties,
             double& scale, const cv::Mat& img0, const cv::Mat& img1, double tx, const Eigen::Matrix3d& K1, int max_opt_step);
};


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#ifndef FullPatchVarianceDifference_HPP
#define FullPatchVarianceDifference_HPP

#include <boost/graph/graph_traits.hpp>

// Parent class
#include "Visitors/AcceptanceVisitors/AcceptanceVisitorParent.h"

// Custom
#include <ITKHelpers/ITKHelpers.h>

// ITK
#include "itkImageRegion.h"

/**

 */
template <typename TGraph, typename TImage>
struct FullPatchVarianceDifference : public AcceptanceVisitorParent<TGraph>
{
  TImage* Image;

  const unsigned int HalfWidth;

  typedef typename boost::graph_traits<TGraph>::vertex_descriptor VertexDescriptorType;

  FullPatchVarianceDifference(TImage* const image, const unsigned int halfWidth) :
  Image(image), HalfWidth(halfWidth)
  {

  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source, float& computedEnergy) const override
  {
    itk::Index<2> targetPixel = ITKHelpers::CreateIndex(target);
    itk::ImageRegion<2> targetRegion = ITKHelpers::GetRegionInRadiusAroundPixel(targetPixel, HalfWidth);
    typename TImage::PixelType targetRegionAverage = ITKHelpers::VarianceInRegion(Image, targetRegion);

    itk::Index<2> sourcePixel = ITKHelpers::CreateIndex(source);
    itk::ImageRegion<2> sourceRegion = ITKHelpers::GetRegionInRadiusAroundPixel(sourcePixel, HalfWidth);
    typename TImage::PixelType sourceRegionAverage = ITKHelpers::VarianceInRegion(Image, sourceRegion);

    // Compute the difference
    computedEnergy = (targetRegionAverage - sourceRegionAverage).GetNorm();
    std::cout << "FullPatchVarianceDifference Energy: " << computedEnergy << std::endl;
    return true;
  }

};

#endif


#include <boost/test/unit_test.hpp>
#include "inputs/interp_tests.h"
#include "slice4missing.hpp"
#include "utils/algorithms.hpp"
#include "utils/interpolations.h"

BOOST_AUTO_TEST_CASE(test_knn)
{
    std::vector<double> input_data(std::begin(values), std::end(values));
    auto missing_begin = std::begin(xi_interp);
    auto missing_end = std::end(xi_interp);
    BOOST_TEST_REQUIRE(utils::size(expected_nearest_interp) == utils::size(xi_interp));  // make sure input is valid!!
    std::size_t count = 0u;
    while (missing_begin != missing_end) {
        auto expected = expected_nearest_interp[count];
        auto input = slice_data(input_data, missing_begin, 1u);
        const auto x_cord = xi_values(missing_begin, 1);
        auto calc = utils::K_neighborhood(input, x_cord,
                static_cast<math::fx_list_type::value_type>(*missing_begin));/*, xi, x*/
        if (!round_cmp(expected, calc, 0.0001)) {
            std::cout<<"input is [";
            out_range(std::cout, input, " ");
            std::cout<<"] at location [";
            out_range(std::cout, x_cord, " ");
            std::cout<<"] ";
            BOOST_TEST(false, "- fix value "<<calc<<" != expected "<<expected<<" index "<<*missing_begin<<" for value "<<input_data[*missing_begin]);
        }
        ++missing_begin;
        ++count;
    }
}

BOOST_AUTO_TEST_CASE(test_knn_gaps)
{
    // this test is running using the functions that the actual system would use to
    // extract the data in the format of [value, row number] pair and not just two
    // arrays - one for data and one for the row numbers
    auto dataset = from_double_array(std::begin(values), std::end(values), "rand_name");
    auto missing_begin = std::begin(xi_interp);
    auto missing_end = std::end(xi_interp);
    BOOST_TEST_REQUIRE(utils::size(expected_nearest_interp) == utils::size(xi_interp));  // make sure input is valid!!
    std::size_t count = 0u;
    while (missing_begin != missing_end) {
        auto expected = expected_nearest_interp[count];
        auto subset = slice(dataset, missing_begin, 1u);    // the the subset that we need to fix
        BOOST_TEST_REQUIRE(utils::size(subset) == 2u);        // win size * 2
        const auto vals = transform_slice(std::begin(subset), std::end(subset));
        const auto cords = row_nums_transform(std::begin(subset), std::end(subset));
        BOOST_TEST_REQUIRE(vals.size() == cords.size());
        BOOST_TEST_REQUIRE(vals.size() == subset.size());
        auto calc = utils::K_neighborhood(vals, cords,
                static_cast<math::fx_list_type::value_type>(*missing_begin));/*, xi, x*/
        if (!round_cmp(expected, calc, 0.0001)) {
            std::cout<<"input is [";
            out_range(std::cout, subset, " ");
            std::cout<<"] at location [";
            out_range(std::cout, cords, " ");
            std::cout<<"] ";
            BOOST_TEST(false, "- fix value "<<calc<<" != expected "<<expected<<" index "<<*missing_begin<<" for value "<<values[*missing_begin]);
        }
        ++missing_begin;
        ++count;
    }
}


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

#if ! defined(BASIC_LOT_HPP)
#define BASIC_LOT_HPP

#include <cmath>
#include <ctime>
#include <boost/shared_ptr.hpp>

namespace phycas
{

/*----------------------------------------------------------------------------------------------------------------------
|	This class was called Lot because the noun lot is defined as "an object used in deciding something by chance"
|	according to The New Merriam-Webster Dictionary.
*/
class Lot
	{
	public:
								Lot();
								Lot(unsigned);
								~Lot();

		// Accessors
		unsigned				GetSeed() const;
		unsigned 				GetInitSeed() const;

		// Modifiers
		void 					UseClockToSeed();
		void 					SetSeed(unsigned s);

		// Utilities
        unsigned                MultinomialDraw(const double * probs, unsigned n, double totalProb=1.0);
		unsigned 				SampleUInt(unsigned);
		unsigned				GetRandBits(unsigned nbits);

		double 					Uniform();
		double 					Normal();
		bool					Boolean();

	private:

		unsigned 				last_seed_setting;
		unsigned				curr_seed;
	};

typedef boost::shared_ptr<Lot> LotShPtr;

inline bool Lot::Boolean()
	{
	return (Uniform() < 0.5);
	}

inline double Lot::Normal()
	{
    double u = Uniform();
    double x = sqrt(-2.0*log(u));
    double v = Uniform();
    double y = cos(2.0*3.141592653589793238846*v);
	return x*y;
	}

} // namespace phycas

#endif


// boost1.67-1.67.0/libs/random/example/password.cpp

// password.cpp
//
// Copyright (c) 2010
// Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[password
/*`
    For the source of this example see
    [@boost://libs/random/example/password.cpp password.cpp].

    This example demonstrates generating a random 8 character
    password.
 */

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <iostream>

int main() {
    /*<< We first define the characters that we're going
         to allow.  This is pretty much just the characters
         on a standard keyboard.
    >>*/
    std::string chars(
        "abcdefghijklmnopqrstuvwxyz"
        "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
        "1234567890"
        "!@#$%^&*()"
        "`~-_=+[{]}\\|;:'\",<.>/? ");
    /*<< We use __random_device as a source of entropy, since we want
         passwords that are not predictable.
    >>*/
    boost::random::random_device rng;
    /*<< Finally we select 8 random characters from the
         string and print them to cout.
    >>*/
    boost::random::uniform_int_distribution<> index_dist(0, chars.size() - 1);
    for(int i = 0; i < 8; ++i) {
        std::cout << chars[index_dist(rng)];
    }
    std::cout << std::endl;
}

//]


#include <vector>
#include <iostream>
#include <complex>
#include <boost/multi_array.hpp>
#include "../common/utilities.hpp"
#include "../common/configuration.hpp"
#include "../common/results.hpp"
#include "../common/symplectic.hpp"
#include "../common/loop_control.hpp"
#include "DNLS.hpp"

using namespace std;

/*
 * The Yoshida 6th order symplectic algorithm, implemented as per arXiv:1012.3242 . It is suggested in the paper
 * that the linear operator evolution can be performed as a FFT, and here we implement it so.
 *
 * Contrary to DNKG_FPUT_Toda or dDNKG, each symplectic step is broken into a number of FFTs, hence different kernel
 * invocations. This makes the algorithm much slower than the others implemented. This is mitigated by the use of
 * cuFFT callbacks, in order to coalesce the linear/nonlinear evolutions in the load callback of the FFTs. In my
 * experience cuFFT callbacks can be of great benefit, but also in some cases (depending on architecture, clocks,
 * number of GPUs used, etc.) they can degrade performance. By default, they are used for both linear and nonlinear
 * evolutions, but the user can disable one or both with the switches --no_linear_callback and --no_nonlinear_callback.
 */
namespace DNLS {

	int main(int argc, char *argv[]) {

		double beta;
		bool no_linear_callback, no_nonlinear_callback;
		{
			using namespace boost::program_options;
			parse_cmdline parser("Options for "s + argv[0]);
			parser.options.add_options()
					("no_linear_callback", "do not use cuFFT callback for linear evolution")
					("no_nonlinear_callback", "do not use cuFFT callback for nonlinear evolution")
					("beta", value(&beta)->required(), "fourth order nonlinearity");
			parser.run(argc, argv);
			no_linear_callback = parser.vm.count("no_linear_callback");
			no_nonlinear_callback = parser.vm.count("no_nonlinear_callback");
		}
		auto ctx = cuda_ctx.activate(mpi_node_coord);

		auto omega_host = dispersion();
		cudalist<double> omega(gconf.chain_length);
		cudaMemcpy(omega, omega_host.data(), gconf.chain_length * sizeof(double), cudaMemcpyHostToDevice) && assertcu;

		cudalist<cufftDoubleComplex> psis_k(gconf.shard_elements);
		results res(true);

		enum {
			s_move = 0, s_cb_linear, s_cb_nonlinear, s_dump, s_linenergies, s_results, s_total
		};
		cudaStream_t streams[s_total];
		memset(streams, 0, sizeof(streams));
		destructor([&] { for (auto stream : streams) cudaStreamDestroy(stream); });
		for (auto &stream : streams) cudaStreamCreateWithFlags(&stream, cudaStreamNonBlocking) && assertcu;

		/*
		 * Precompute the evolution factors in Fourier space (linear operator), including FFT normalization, for all
		 * the symplectic integration steps.
		 */
		cudalist<cufftDoubleComplex> evolve_linear_tables_all(7 * gconf.chain_length);
		cudaMemcpy(evolve_linear_tables_all, evolve_linear_table().origin(),
		           7 * gconf.chain_length * sizeof(cufftDoubleComplex), cudaMemcpyHostToDevice) && assertcu;

		/*
		 * Three FFT plans are required because of the nonlinear/linear evolution callbacks. The plain fft plan is used
		 * to make the linear energies, and in case that the user disabled callbacks.
		 * The settings for the callbacks (the symplectic time step, or the linear evolution table) are put on global
		 * variables asynchronously with additional streams.
		 */
		cufftHandle fft_plain = 0, fft_plain_entropy = 0, fft_elvolve_psik = 0, fft_elvolve_psi = 0;
		destructor([&] {
			cufftDestroy(fft_plain);
			cufftDestroy(fft_plain_entropy);
			cufftDestroy(fft_elvolve_psik);
			cufftDestroy(fft_elvolve_psi);
		});

		cudalist<double, true> beta_dt_symplectic_all;
		auto beta_dt_symplectic_ptr = get_device_address(callback::beta_dt_symplectic);
		cudalist<void> area;
		{
			auto init_plan = [&](cufftHandle &fft) {
				size_t dummy;
				cufftCreate(&fft) && assertcufft;
				cufftSetAutoAllocation(fft, false) && assertcufft;
				cufftMakePlan1d(fft, gconf.chain_length, CUFFT_Z2Z, gconf.shard_copies, &dummy) && assertcufft;
				cufftSetStream(fft, streams[s_move]) && assertcufft;
			};
			size_t maxsize = 0;
			auto update_max_size = [&](cufftHandle fft) {   //use only one work area
				size_t size;
				cufftGetSize(fft, &size) && assertcufft;
				maxsize = max(maxsize, size);
			};
			init_plan(fft_plain);
			update_max_size(fft_plain);
			init_plan(fft_plain_entropy);
			cufftSetStream(fft_plain_entropy, streams[s_linenergies]) && assertcufft;
			update_max_size(fft_plain_entropy);
			if (!no_linear_callback) {
				init_plan(fft_elvolve_psik);
				auto evolve_linear_ptr_host = get_device_object(callback::evolve_linear_ptr);
				auto evolve_linear_tables_all_ptr = *evolve_linear_tables_all;
				cufftXtSetCallback(fft_elvolve_psik, (void **) &evolve_linear_ptr_host, CUFFT_CB_LD_COMPLEX_DOUBLE,
				                   (void **) &evolve_linear_tables_all_ptr) && assertcufft;
				set_device_object(gconf.chain_length, callback::chainlen);
				update_max_size(fft_elvolve_psik);
			}
			if (!no_nonlinear_callback) {
				init_plan(fft_elvolve_psi);
				auto evolve_nonlinear_ptr_host = get_device_object(callback::evolve_nonlinear_ptr);
				cufftXtSetCallback(fft_elvolve_psi, (void **) &evolve_nonlinear_ptr_host, CUFFT_CB_LD_COMPLEX_DOUBLE,
				                   0) && assertcufft;
				beta_dt_symplectic_all = cudalist<double, true>(8, true);
				loopk(8) beta_dt_symplectic_all[k] = beta * gconf.dt * (k == 7 ? 2. : 1.) * symplectic_c[k];
				update_max_size(fft_elvolve_psi);
			}
			area = maxsize;
			cufftSetWorkArea(fft_plain, area) && assertcufft;
			cufftSetWorkArea(fft_plain_entropy, area) && assertcufft;
			if (!no_linear_callback) cufftSetWorkArea(fft_elvolve_psik, area) && assertcufft;
			if (!no_nonlinear_callback) cufftSetWorkArea(fft_elvolve_psi, area) && assertcufft;
		}

		loop_control_gpu loop_ctl(streams[s_move]);
		auto dumper = [&] {
			cudaMemcpyAsync(gres.shard_host, gres.shard_gpu, gconf.sizeof_shard, cudaMemcpyDeviceToHost, streams[s_dump]) &&
			assertcu;
			completion done_copy(streams[s_dump]);
			done_copy.blocks(streams[s_move]);
			done_copy.blocks(streams[s_results]);
		};

		cudaDeviceSynchronize() && assertcu;
		destructor(cudaDeviceSynchronize);
		while (1) {
			bool full_dump = loop_ctl % gconf.dump_interval == 0;
			if (full_dump) dumper();
			cufftExecZ2Z(fft_plain_entropy, gres.shard_gpu, psis_k, CUFFT_FORWARD) && assertcufft;
			completion(streams[s_linenergies]).blocks(streams[s_move]);
			make_linenergies(psis_k, omega, streams[s_linenergies]);
			completion(streams[s_linenergies]).blocks(streams[s_results]);
			add_cuda_callback(streams[s_results], loop_ctl.callback_err,
			                  [&, full_dump, t = *loop_ctl](cudaError_t status) {
				                  if (loop_ctl.callback_err) return;
				                  status && assertcu;
				                  res.calc_linenergies(1. / gconf.shard_elements).calc_entropies().check_entropy().write_entropy(t);
				                  if (full_dump) res.write_linenergies(t).write_shard(t);
			                  });
			completion done_results(streams[s_results]);
			done_results.blocks(streams[s_dump]);
			done_results.blocks(streams[s_linenergies]);

			if (loop_ctl.break_now()) break;

			for (uint32_t i = 0; i < gconf.kernel_batching; i++)
				for (int k = 0; k < 7; k++) {
					if (fft_elvolve_psi) {
						cudaMemcpyAsync(beta_dt_symplectic_ptr, &beta_dt_symplectic_all[!k && i ? 7 : k],
						                sizeof(double), cudaMemcpyHostToDevice, streams[s_cb_nonlinear]);
						completion(streams[s_cb_nonlinear]).blocks(streams[s_move]);
					} else evolve_nonlinear(beta * gconf.dt * (!k && i ? 2. : 1.) * symplectic_c[k], streams[s_move]);
					cufftExecZ2Z(fft_elvolve_psi ?: fft_plain, gres.shard_gpu, gres.shard_gpu, CUFFT_FORWARD) && assertcufft;
					completion(streams[s_move]).blocks(streams[s_cb_nonlinear]);
					if (fft_elvolve_psik) {
						memset_device_object(callback::evolve_linear_table_idx, k, streams[s_cb_linear]);
						completion(streams[s_cb_linear]).blocks(streams[s_move]);
					} else evolve_linear(&evolve_linear_tables_all[k * gconf.chain_length], streams[s_move]);
					cufftExecZ2Z(fft_elvolve_psik ?: fft_plain, gres.shard_gpu, gres.shard_gpu, CUFFT_INVERSE) && assertcufft;
					completion(streams[s_move]).blocks(streams[s_cb_linear]);
				}
			completion finish_move = evolve_nonlinear(beta * gconf.dt * symplectic_c[7], streams[s_move]);
			finish_move.blocks(streams[s_linenergies]);
			finish_move.blocks(streams[s_dump]);

			loop_ctl += gconf.kernel_batching;
		}

		if (loop_ctl % gconf.dump_interval != 0) {
			dumper();
			completion(streams[s_linenergies]).wait();
			res.write_linenergies(loop_ctl);
			completion(streams[s_dump]).wait();
			res.write_shard(loop_ctl);
		}
		return 0;
	}
}

ginit = [] {
	::programs()["DNLS"] = DNLS::main;
};


// Copyright (c) 2020 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_kurtosis.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>
#include <boost/accumulators/statistics/weighted_skewness.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
#include <boost/geometry.hpp>
#include <optional>
#include <pybind11/numpy.h>
#include "pyinterp/axis.hpp"
#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/geometry/point.hpp"
#include "pyinterp/detail/math/binning.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp {

/// Group a number of more or less continuous values into a smaller number of
/// "bins" located on a grid.
template <typename T>
class Binning2D {
 public:
  /// Default constructor
  ///
  /// @param x Definition of the bin edges for the X axis of the grid.
  /// @param y Definition of the bin edges for the Y axis of the grid.
  /// @param wgs WGS of the coordinate system used to manipulate geographic
  /// coordinates. If this parameter is not set, the handled coordinates will be
  /// considered as Cartesian coordinates. Otherwise, "x" and "y" are considered
  /// to represents the longitudes and latitudes on a grid.
  Binning2D(std::shared_ptr<Axis<double>> x, std::shared_ptr<Axis<double>> y,
            std::optional<geodetic::System> wgs)
      : x_(std::move(x)),
        y_(std::move(y)),
        acc_(x_->size(), y_->size()),
        wgs_(std::move(wgs)) {}

  /// Inserts new values in the grid from Z values for X, Y data coordinates.
  void push(const pybind11::array_t<T>& x, const pybind11::array_t<T>& y,
            const pybind11::array_t<T>& z, const bool simple) {
    detail::check_array_ndim("x", 1, x, "y", 1, y, "z", 1, z);
    detail::check_ndarray_shape("x", x, "y", y, "z", z);

    if (simple) {
      // Nearest
      push_nearest(x, y, z);
    } else if (!wgs_) {
      // Cartesian linear
      push_linear<detail::geometry::Point2D,
                  boost::geometry::strategy::area::cartesian<>>(
          x, y, z, boost::geometry::strategy::area::cartesian<>());
    } else {
      // Geographic linear
      auto strategy = boost::geometry::strategy::area::geographic<>(
          boost::geometry::srs::spheroid(wgs_->semi_major_axis(),
                                         wgs_->semi_minor_axis()));
      push_linear<detail::geometry::SpheriodPoint2D,
                  boost::geometry::strategy::area::geographic<>>(x, y, z,
                                                                 strategy);
    }
  }

  /// Reset the statistics.
  void clear() {
    acc_ =
        std::move(Eigen::Matrix<Accumulators, Eigen::Dynamic, Eigen::Dynamic>(
            x_->size(), y_->size()));
  }

  /// Compute the count of points within each bin.
  [[nodiscard]] auto count() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::count);
  }

  /// Compute the minimum of values for points within each bin.
  [[nodiscard]] auto min() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::min);
  }

  /// Compute the maximum of values for points within each bin.
  [[nodiscard]] auto max() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::max);
  }

  /// Compute the mean of values for points within each bin.
  [[nodiscard]] auto mean() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::weighted_mean);
  }

  /// Compute the median of values for points within each bin.
  [[nodiscard]] auto median() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::weighted_median);
  }

  /// Compute the variance of values for points within each bin.
  [[nodiscard]] auto variance() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::weighted_variance);
  }

  /// Compute the kurtosis of values for points within each bin.
  [[nodiscard]] auto kurtosis() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::weighted_kurtosis);
  }

  /// Compute the skewness of values for points within each bin.
  [[nodiscard]] auto skewness() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::weighted_skewness);
  }

  /// Compute the sum of values for points within each bin.
  [[nodiscard]] auto sum() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::weighted_sum);
  }

  /// Compute the sum of weights within each bin.
  [[nodiscard]] auto sum_of_weights() const -> pybind11::array_t<T> {
    return calculate_statistics(boost::accumulators::sum_of_weights);
  }

  /// Gets the X-Axis
  [[nodiscard]] inline auto x() const -> std::shared_ptr<Axis<double>> {
    return x_;
  }

  /// Gets the Y-Axis
  [[nodiscard]] inline auto y() const -> std::shared_ptr<Axis<double>> {
    return y_;
  }

 private:
  /// Statistics handled by this object.
  using Accumulators = boost::accumulators::accumulator_set<
      T,
      boost::accumulators::stats<
          boost::accumulators::tag::count, boost::accumulators::tag::max,
          boost::accumulators::tag::min,
          boost::accumulators::tag::sum_of_weights,
          boost::accumulators::tag::weighted_kurtosis,
          boost::accumulators::tag::weighted_mean,
          boost::accumulators::tag::weighted_median(
              boost::accumulators::with_p_square_quantile),
          boost::accumulators::tag::weighted_skewness,
          boost::accumulators::tag::weighted_sum,
          boost::accumulators::tag::weighted_variance(
              boost::accumulators::lazy)>,
      T>;

  /// Grid axis
  std::shared_ptr<Axis<double>> x_;
  std::shared_ptr<Axis<double>> y_;

  /// Statistics grid
  Eigen::Matrix<Accumulators, Eigen::Dynamic, Eigen::Dynamic> acc_;

  /// Geodetic coordinate system required to calculate areas (optional if the
  /// user wishes to handle Cartesian coordinates).
  std::optional<geodetic::System> wgs_;

  /// Calculation of a given statistical variable.
  template <typename Func>
  [[nodiscard]] auto calculate_statistics(const Func& func) const
      -> pybind11::array_t<T> {
    pybind11::array_t<T> z({x_->size(), y_->size()});
    auto _z = z.template mutable_unchecked<2>();
    {
      pybind11::gil_scoped_release release;

      for (Eigen::Index ix = 0; ix < acc_.rows(); ++ix) {
        for (Eigen::Index iy = 0; iy < acc_.cols(); ++iy) {
          _z(ix, iy) = func(acc_(ix, iy));
        }
      }
    }
    return z;
  }

  /// Insertion of data on the nearest bin.
  void push_nearest(const pybind11::array_t<T>& x,
                    const pybind11::array_t<T>& y,
                    const pybind11::array_t<T>& z) {
    auto _x = x.template unchecked<1>();
    auto _y = y.template unchecked<1>();
    auto _z = z.template unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      const auto& x_axis = static_cast<pyinterp::detail::Axis<double>&>(*x_);
      const auto& y_axis = static_cast<pyinterp::detail::Axis<double>&>(*y_);

      for (pybind11::ssize_t idx = 0; idx < x.size(); ++idx) {
        auto value = _z(idx);

        if (!std::isnan(value)) {
          auto ix = x_axis.find_index(_x(idx), true);
          auto iy = y_axis.find_index(_y(idx), true);

          if (ix != -1 && iy != -1) {
            acc_(ix, iy)(value, boost::accumulators::weight = 1);
          }
        }
      }
    }
  }

  /// Update statistics for the linear binning (ignore zero weights).
  void update_acc(const int64_t ix, const int64_t iy, const T& value,
                  const T& weight) {
    if (!detail::math::is_almost_zero(weight,
                                      std::numeric_limits<T>::epsilon())) {
      acc_(ix, iy)(value, boost::accumulators::weight = weight);
    }
  }

  /// Set bins with nearest binning.
  template <template <class> class Point, typename Strategy>
  void push_linear(const pybind11::array_t<T>& x, const pybind11::array_t<T>& y,
                   const pybind11::array_t<T>& z, const Strategy& strategy) {
    auto _x = x.template unchecked<1>();
    auto _y = y.template unchecked<1>();
    auto _z = z.template unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      const auto& x_axis = static_cast<pyinterp::detail::Axis<double>&>(*x_);
      const auto& y_axis = static_cast<pyinterp::detail::Axis<double>&>(*y_);

      for (pybind11::ssize_t idx = 0; idx < x.size(); ++idx) {
        auto value = _z(idx);
        if (std::isnan(value)) {
          continue;
        }

        auto x_indexes = x_axis.find_indexes(_x(idx));
        auto y_indexes = y_axis.find_indexes(_y(idx));

        if (x_indexes.has_value() && y_indexes.has_value()) {
          int64_t ix0;
          int64_t ix1;
          int64_t iy0;
          int64_t iy1;

          std::tie(ix0, ix1) = *x_indexes;
          std::tie(iy0, iy1) = *y_indexes;

          auto x0 = x_axis(ix0);

          auto weights = detail::math::binning_2d<Point, Strategy, double>(
              Point<double>(x_axis.is_angle()
                                ? detail::math::normalize_angle<double>(
                                      _x(idx), x0, 360.0)
                                : _x(idx),
                            _y(idx)),
              Point<double>(x0, y_axis(iy0)),
              Point<double>(x_axis(ix1), y_axis(iy1)), strategy);

          update_acc(ix0, iy0, value, static_cast<T>(std::get<0>(weights)));
          update_acc(ix0, iy1, value, static_cast<T>(std::get<1>(weights)));
          update_acc(ix1, iy1, value, static_cast<T>(std::get<2>(weights)));
          update_acc(ix1, iy0, value, static_cast<T>(std::get<3>(weights)));
        }
      }
    }
  }
};

}  // namespace pyinterp


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;

namespace mp = boost::multiprecision;

int main(int argc, char const* argv[])
{
	mp::cpp_int x = 1;
	mp::cpp_int prev = 0;

	while(1){
		cout << prev << endl;
		mp::cpp_int y = x + prev;
		prev = x;
		x = y;
	}

	exit(0);
}


#include <Eigen/LU>
#include <numeric>
#include "EKF.h"

using namespace Eigen;

static double clamp(double f, double fs) {
    if (f > fs / 2) {
        f = fs / 2 - (f - fs / 2);
    }
    return std::clamp(abs(f), 60.0, fs / 2 - 60.0);
}

void EKF::step(EKF::State & state)
{
    static MatrixXd P_pred, H, S, K;
    static VectorXd m_pred, y_pred, y_obs;

    const bool voiced = state.voiced;
    const int numF = state.numF;
    const int cepOrder = state.cepOrder;
    const double fs = state.fs;
    auto& y = state.y;
    auto& F = state.F;
    auto Ft = state.F.transpose();
    auto& Q = state.Q;
    auto& R = state.R;

    m_pred = F * state.m_up;
    P_pred = F * state.P_up * Ft + Q;

    for (int i = 0; i < 2 * numF; ++i) {
        if (m_pred(i) > fs / 2) {
            m_pred(i) = fs / 2 - (m_pred(i) - fs / 2);
        }
    }

    auto curFVals = m_pred.head(numF);
    auto curBVals = m_pred.tail(numF);

    // Linearize about m_pred using Taylor expansion.
    H = EKF::getH_FBW(curFVals, curBVals, numF, cepOrder, fs);
    auto Ht = H.transpose();

    if (/*voiced*/true) {
        S = H * P_pred * Ht + R;
        K = (P_pred * Ht) * S.inverse();
    }
    else {
        K.setZero(2 * numF, cepOrder);
    }

    y_pred = fb2cp(curFVals, curBVals, cepOrder, fs);

    y_obs.setZero(cepOrder);
    if (y.size() < cepOrder) {
        y_obs.head(y.size()) = y;
    }
    else {
        y_obs = y.head(cepOrder);
    }

    state.m_up = m_pred + K * (y_obs - y_pred);
    state.P_up = P_pred - K * H * P_pred;

    // Sort the formants by absolute frequency.

    std::vector<int> inds(numF);
    std::iota(inds.begin(), inds.end(), 0);
    std::sort(inds.begin(), inds.end(),
            [&](int i, int j) {
                return clamp(state.m_up(i), fs) < clamp(state.m_up(j), fs);
            });

    VectorXd m_ = state.m_up;
    MatrixXd P_ = state.P_up;

    for (int k = 0; k < numF; ++k) {
        m_(k) = clamp(state.m_up(inds[k]), fs);
        m_(numF + k) = clamp(state.m_up(numF + inds[k]), fs);

        P_.row(k) = state.P_up.col(inds[k]);
        P_.row(numF + k) = state.P_up.col(numF + inds[k]);
    }

    state.m_up = std::move(m_);
    state.P_up = std::move(P_);

}


#include <cstdlib>
#include <iostream>
#include <complex>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/triangular.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/unit_lower.hpp>
#include <boost/numeric/bindings/unit_upper.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::triangular_matrix<complex, ublas::lower, ublas::column_major> matrix_l;
    typedef ublas::triangular_matrix<complex, ublas::upper, ublas::column_major> matrix_u;
    typedef typename vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix_l A_l(n, n);
    matrix_u A_u(n, n);
    for (size_type j=0; j<n; ++j) {
      A_u(j, j)=rand_normal<complex>::get();
      A_l(j, j)=rand_normal<complex>::get();
      for (size_type i=0; i<j; ++i) {
        A_u(i, j)=rand_normal<complex>::get();
        A_l(j, i)=rand_normal<complex>::get();
       }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    {
      vector x1(ublas::prod(A_l, x));
      vector x2(x);
      blas::tpmv(A_l, x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (lower): " << print_vec(x1) << '\n'
    		<< "using blas  (lower): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(A_u, x));
      vector x2(x);
      blas::tpmv(A_u, x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (upper): " << print_vec(x1) << '\n'
    		<< "using blas  (upper): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::trans(A_l), x));
      vector x2(x);
      blas::tpmv(blas::trans(blas::lower(A_l)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (trans, lower): " << print_vec(x1) << '\n'
    		<< "using blas  (trans, lower): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::trans(A_u), x));
      vector x2(x);
      blas::tpmv(blas::trans(blas::upper(A_u)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (trans, upper): " << print_vec(x1) << '\n'
    		<< "using blas  (trans, upper): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::herm(A_l), x));
      vector x2(x);
      blas::tpmv(blas::conj(blas::lower(A_l)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (htrans, lower): " << print_vec(x1) << '\n'
    		<< "using blas  (htrans, lower): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::herm(A_u), x));
      vector x2(x);
      blas::tpmv(blas::conj(blas::upper(A_u)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (htrans, upper): " << print_vec(x1) << '\n'
    		<< "using blas  (htrans, upper): " << print_vec(x2) << '\n'
    		<< '\n';
    }

    for (size_type i=0; i<n; ++i) {
      A_l(i, i)=1;
      A_u(i, i)=1;
    }

    {
      vector x1(ublas::prod(A_l, x));
      vector x2(x);
      blas::tpmv(blas::unit_lower(A_l), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (unit_lower): " << print_vec(x1) << '\n'
    		<< "using blas  (unit_lower): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(A_u, x));
      vector x2(x);
      blas::tpmv(blas::unit_upper(A_u), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (unit_upper): " << print_vec(x1) << '\n'
    		<< "using blas  (unit_upper): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::trans(A_l), x));
      vector x2(x);
      blas::tpmv(blas::trans(blas::unit_lower(A_l)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (trans, unit_lower): " << print_vec(x1) << '\n'
    		<< "using blas  (trans, unit_lower): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::trans(A_u), x));
      vector x2(x);
      blas::tpmv(blas::trans(blas::unit_upper(A_u)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (trans, unit_upper): " << print_vec(x1) << '\n'
    		<< "using blas  (trans, unit_upper): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::herm(A_l), x));
      vector x2(x);
      blas::tpmv(blas::conj(blas::unit_lower(A_l)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (htrans, unit_lower): " << print_vec(x1) << '\n'
    		<< "using blas  (htrans, unit_lower): " << print_vec(x2) << '\n'
    		<< '\n';
    }
    {
      vector x1(ublas::prod(ublas::herm(A_u), x));
      vector x2(x);
      blas::tpmv(blas::conj(blas::unit_upper(A_u)), x2);
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (htrans, unit_upper): " << print_vec(x1) << '\n'
    		<< "using blas  (htrans, unit_upper): " << print_vec(x2) << '\n'
    		<< '\n';
    }
  }
  return EXIT_SUCCESS;
}


/*
 * Copyright (C) 2005-2019 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef otbTrainNeuralNetwork_hxx
#define otbTrainNeuralNetwork_hxx
#include <boost/lexical_cast.hpp>
#include "otbLearningApplicationBase.h"
#include "otbNeuralNetworkMachineLearningModel.h"

namespace otb
{
namespace Wrapper
{

template <class TInputValue, class TOutputValue>
void LearningApplicationBase<TInputValue, TOutputValue>::InitNeuralNetworkParams()
{
  AddChoice("classifier.ann", "Artificial Neural Network classifier");
  SetParameterDescription("classifier.ann", "http://docs.opencv.org/modules/ml/doc/neural_networks.html");

  // TrainMethod
  AddParameter(ParameterType_Choice, "classifier.ann.t", "Train Method Type");
  AddChoice("classifier.ann.t.back", "Back-propagation algorithm");
  SetParameterDescription("classifier.ann.t.back",
                          "Method to compute the gradient of the loss function and adjust weights "
                          "in the network to optimize the result.");
  AddChoice("classifier.ann.t.reg", "Resilient Back-propagation algorithm");
  SetParameterDescription("classifier.ann.t.reg",
                          "Almost the same as the Back-prop algorithm except that it does not "
                          "take into account the magnitude of the partial derivative (coordinate "
                          "of the gradient) but only its sign.");

  SetParameterString("classifier.ann.t", "reg");
  SetParameterDescription("classifier.ann.t", "Type of training method for the multilayer perceptron (MLP) neural network.");

  // LayerSizes
  // There is no ParameterType_IntList, so i use a ParameterType_StringList and convert it.
  /*std::vector<std::string> layerSizes;
   layerSizes.push_back("100");
   layerSizes.push_back("100"); */
  AddParameter(ParameterType_StringList, "classifier.ann.sizes", "Number of neurons in each intermediate layer");
  // SetParameterStringList("classifier.ann.sizes", layerSizes);
  SetParameterDescription("classifier.ann.sizes", "The number of neurons in each intermediate layer (excluding input and output layers).");

  // ActivateFunction
  AddParameter(ParameterType_Choice, "classifier.ann.f", "Neuron activation function type");
  AddChoice("classifier.ann.f.ident", "Identity function");
  AddChoice("classifier.ann.f.sig", "Symmetrical Sigmoid function");
  AddChoice("classifier.ann.f.gau", "Gaussian function (Not completely supported)");
  SetParameterString("classifier.ann.f", "sig");
  SetParameterDescription("classifier.ann.f",
                          "This function determine whether the output of the node is positive or not "
                          "depending on the output of the transfert function.");

  // Alpha
  AddParameter(ParameterType_Float, "classifier.ann.a", "Alpha parameter of the activation function");
  SetParameterFloat("classifier.ann.a", 1.);
  SetParameterDescription("classifier.ann.a", "Alpha parameter of the activation function (used only with sigmoid and gaussian functions).");

  // Beta
  AddParameter(ParameterType_Float, "classifier.ann.b", "Beta parameter of the activation function");
  SetParameterFloat("classifier.ann.b", 1.);
  SetParameterDescription("classifier.ann.b", "Beta parameter of the activation function (used only with sigmoid and gaussian functions).");

  // BackPropDWScale
  AddParameter(ParameterType_Float, "classifier.ann.bpdw", "Strength of the weight gradient term in the BACKPROP method");
  SetParameterFloat("classifier.ann.bpdw", 0.1);
  SetParameterDescription("classifier.ann.bpdw",
                          "Strength of the weight gradient term in the BACKPROP method. The "
                          "recommended value is about 0.1.");

  // BackPropMomentScale
  AddParameter(ParameterType_Float, "classifier.ann.bpms", "Strength of the momentum term (the difference between weights on the 2 previous iterations)");
  SetParameterFloat("classifier.ann.bpms", 0.1);
  SetParameterDescription("classifier.ann.bpms",
                          "Strength of the momentum term (the difference between weights on the 2 previous "
                          "iterations). This parameter provides some inertia to smooth the random "
                          "fluctuations of the weights. It can vary from 0 (the feature is disabled) "
                          "to 1 and beyond. The value 0.1 or so is good enough.");

  // RegPropDW0
  AddParameter(ParameterType_Float, "classifier.ann.rdw", "Initial value Delta_0 of update-values Delta_{ij} in RPROP method");
  SetParameterFloat("classifier.ann.rdw", 0.1);
  SetParameterDescription("classifier.ann.rdw", "Initial value Delta_0 of update-values Delta_{ij} in RPROP method (default = 0.1).");

  // RegPropDWMin
  AddParameter(ParameterType_Float, "classifier.ann.rdwm", "Update-values lower limit Delta_{min} in RPROP method");
  SetParameterFloat("classifier.ann.rdwm", 1e-7);
  SetParameterDescription("classifier.ann.rdwm",
                          "Update-values lower limit Delta_{min} in RPROP method. It must be positive "
                          "(default = 1e-7).");

  // TermCriteriaType
  AddParameter(ParameterType_Choice, "classifier.ann.term", "Termination criteria");
  AddChoice("classifier.ann.term.iter", "Maximum number of iterations");
  SetParameterDescription("classifier.ann.term.iter",
                          "Set the number of iterations allowed to the network for its "
                          "training. Training will stop regardless of the result when this "
                          "number is reached");
  AddChoice("classifier.ann.term.eps", "Epsilon");
  SetParameterDescription("classifier.ann.term.eps",
                          "Training will focus on result and will stop once the precision is"
                          "at most epsilon");
  AddChoice("classifier.ann.term.all", "Max. iterations + Epsilon");
  SetParameterDescription("classifier.ann.term.all", "Both termination criteria are used. Training stop at the first reached");
  SetParameterString("classifier.ann.term", "all");
  SetParameterDescription("classifier.ann.term", "Termination criteria.");

  // Epsilon
  AddParameter(ParameterType_Float, "classifier.ann.eps", "Epsilon value used in the Termination criteria");
  SetParameterFloat("classifier.ann.eps", 0.01);
  SetParameterDescription("classifier.ann.eps", "Epsilon value used in the Termination criteria.");

  // MaxIter
  AddParameter(ParameterType_Int, "classifier.ann.iter", "Maximum number of iterations used in the Termination criteria");
  SetParameterInt("classifier.ann.iter", 1000);
  SetParameterDescription("classifier.ann.iter", "Maximum number of iterations used in the Termination criteria.");
}

template <class TInputValue, class TOutputValue>
void LearningApplicationBase<TInputValue, TOutputValue>::TrainNeuralNetwork(typename ListSampleType::Pointer trainingListSample,
                                                                            typename TargetListSampleType::Pointer trainingLabeledListSample,
                                                                            std::string                            modelPath)
{
  typedef otb::NeuralNetworkMachineLearningModel<InputValueType, OutputValueType> NeuralNetworkType;
  typename NeuralNetworkType::Pointer classifier = NeuralNetworkType::New();
  classifier->SetRegressionMode(this->m_RegressionFlag);
  classifier->SetInputListSample(trainingListSample);
  classifier->SetTargetListSample(trainingLabeledListSample);

  switch (GetParameterInt("classifier.ann.t"))
  {
  case 0: // BACKPROP
    classifier->SetTrainMethod(CvANN_MLP_TrainParams::BACKPROP);
    break;
  case 1: // RPROP
    classifier->SetTrainMethod(CvANN_MLP_TrainParams::RPROP);
    break;
  default: // DEFAULT = RPROP
    classifier->SetTrainMethod(CvANN_MLP_TrainParams::RPROP);
    break;
  }

  std::vector<unsigned int> layerSizes;
  std::vector<std::string>  sizes = GetParameterStringList("classifier.ann.sizes");

  unsigned int nbImageBands = trainingListSample->GetMeasurementVectorSize();
  layerSizes.push_back(nbImageBands);
  for (unsigned int i = 0; i < sizes.size(); i++)
  {
    unsigned int nbNeurons = boost::lexical_cast<unsigned int>(sizes[i]);
    layerSizes.push_back(nbNeurons);
  }

  unsigned int nbClasses = 0;
  if (this->m_RegressionFlag)
  {
    layerSizes.push_back(1);
  }
  else
  {
    std::set<TargetValueType> labelSet;
    TargetSampleType          currentLabel;
    for (unsigned int itLab = 0; itLab < trainingLabeledListSample->Size(); ++itLab)
    {
      currentLabel = trainingLabeledListSample->GetMeasurementVector(itLab);
      labelSet.insert(currentLabel[0]);
    }
    nbClasses = labelSet.size();
    layerSizes.push_back(nbClasses);
  }

  classifier->SetLayerSizes(layerSizes);

  switch (GetParameterInt("classifier.ann.f"))
  {
  case 0: // ident
    classifier->SetActivateFunction(CvANN_MLP::IDENTITY);
    break;
  case 1: // sig
    classifier->SetActivateFunction(CvANN_MLP::SIGMOID_SYM);
    break;
  case 2: // gaussian
    classifier->SetActivateFunction(CvANN_MLP::GAUSSIAN);
    break;
  default: // DEFAULT = RPROP
    classifier->SetActivateFunction(CvANN_MLP::SIGMOID_SYM);
    break;
  }

  classifier->SetAlpha(GetParameterFloat("classifier.ann.a"));
  classifier->SetBeta(GetParameterFloat("classifier.ann.b"));
  classifier->SetBackPropDWScale(GetParameterFloat("classifier.ann.bpdw"));
  classifier->SetBackPropMomentScale(GetParameterFloat("classifier.ann.bpms"));
  classifier->SetRegPropDW0(GetParameterFloat("classifier.ann.rdw"));
  classifier->SetRegPropDWMin(GetParameterFloat("classifier.ann.rdwm"));

  switch (GetParameterInt("classifier.ann.term"))
  {
  case 0: // CV_TERMCRIT_ITER
    classifier->SetTermCriteriaType(CV_TERMCRIT_ITER);
    break;
  case 1: // CV_TERMCRIT_EPS
    classifier->SetTermCriteriaType(CV_TERMCRIT_EPS);
    break;
  case 2: // CV_TERMCRIT_ITER + CV_TERMCRIT_EPS
    classifier->SetTermCriteriaType(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS);
    break;
  default: // DEFAULT = CV_TERMCRIT_ITER + CV_TERMCRIT_EPS
    classifier->SetTermCriteriaType(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS);
    break;
  }
  classifier->SetEpsilon(GetParameterFloat("classifier.ann.eps"));
  classifier->SetMaxIter(GetParameterInt("classifier.ann.iter"));
  classifier->Train();
  classifier->Save(modelPath);
}

} // end namespace wrapper
} // end namespace otb

#endif


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

#include "cvxgen/interface_vel.h"
#include "cvxgen/interface_accel.h"
#include "cvxgen/interface_jerk.h"

#include <unsupported/Eigen/Polynomials>

#define VEL 1
#define ACCEL 2
#define JERK 3

#define N_VEL 15
#define N_ACCEL 15
#define N_JERK 10

template <int INPUT_ORDER>
class Solver
{
public:
  Solver();
  void set_x0(double x0[]);
  void set_xf(double xf[]);
  void set_max(double max_values[INPUT_ORDER]);

  void genNewTraj();

  int getN();
  void setq(double q);
  double** getState();
  double** getInput();
  double getCost();

protected:
  bool checkConvergence(double xf_opt[3 * INPUT_ORDER]);
  void callOptimizer();
  double getDTInitial();
  double dt_;  // time step found by the solver
  int N_;
  double xf_[3 * INPUT_ORDER];
  double x0_[3 * INPUT_ORDER];
  double v_max_;
  double a_max_;
  double j_max_;
  double q_;  // weight to the 2nd term in the cost function
  double** x_;
  double** u_;
};

template <int INPUT_ORDER>
double Solver<INPUT_ORDER>::getCost()
{
  double cost;
  switch (INPUT_ORDER)
  {
    case VEL:
      cost = vel_get_cost();
      break;
    case ACCEL:
      cost = accel_get_cost();
      break;
    case JERK:
      cost = jerk_get_cost();
      break;
  }
}

template <int INPUT_ORDER>
double** Solver<INPUT_ORDER>::getState()
{
  return x_;
}

template <int INPUT_ORDER>
double** Solver<INPUT_ORDER>::getInput()
{
  return u_;
}

template <int INPUT_ORDER>
Solver<INPUT_ORDER>::Solver()
{
  v_max_ = 20;
  a_max_ = 2;
  j_max_ = 20;
  switch (INPUT_ORDER)
  {
    case VEL:
      vel_initialize_optimizer();
      N_ = N_VEL;
      break;
    case ACCEL:
      accel_initialize_optimizer();
      N_ = N_ACCEL;
      break;
    case JERK:
      jerk_initialize_optimizer();
      N_ = N_JERK;
      break;
  }
}

template <int INPUT_ORDER>
int Solver<INPUT_ORDER>::getN()
{
  return N_;
}

template <int INPUT_ORDER>
void Solver<INPUT_ORDER>::setq(double q)
{
  q_ = q;
}

template <int INPUT_ORDER>
void Solver<INPUT_ORDER>::set_x0(double x0[])
{
  for (int i = 0; i < 3 * INPUT_ORDER; i++)
  {
    x0_[i] = x0[i];
  }
}

template <int INPUT_ORDER>
void Solver<INPUT_ORDER>::set_xf(double xf[])
{
  for (int i = 0; i < 3 * INPUT_ORDER; i++)
  {
    xf_[i] = xf[i];
  }
}

template <int INPUT_ORDER>
void Solver<INPUT_ORDER>::set_max(double max_values[INPUT_ORDER])
{
  switch (INPUT_ORDER)
  {
    case VEL:
      v_max_ = max_values[0];
      break;
    case ACCEL:
      v_max_ = max_values[0];
      a_max_ = max_values[1];
      break;
    case JERK:
      v_max_ = max_values[0];
      a_max_ = max_values[1];
      j_max_ = max_values[2];
      break;
  }
}

template <int INPUT_ORDER>
bool Solver<INPUT_ORDER>::checkConvergence(double xf_opt[3 * INPUT_ORDER])
{
  bool converged = false;
  float d2 = 0;   // distance in position squared
  float dv2 = 0;  // distance in velocity squared
  float da2 = 0;  // distance in acceleration squared

  switch (INPUT_ORDER)
  {
    case VEL:
      for (int i = 0; i < 3; i++)
      {
        d2 += pow(xf_[i] - xf_opt[i], 2);
      }
      converged = (sqrt(d2) < 0.2) ? true : false;
      break;
    case ACCEL:
      for (int i = 0; i < 3; i++)
      {
        d2 += pow(xf_[i] - xf_opt[i], 2);
        dv2 += pow(xf_[i + 3] - xf_opt[i + 3], 2);
      }
      converged = (sqrt(d2) < 0.2 && sqrt(dv2) < 0.2) ? true : false;
      break;
    case JERK:
      for (int i = 0; i < 3; i++)
      {
        d2 += pow(xf_[i] - xf_opt[i], 2);
        dv2 += pow(xf_[i + 3] - xf_opt[i + 3], 2);
        da2 += pow(xf_[i + 6] - xf_opt[i + 6], 2);
      }
      converged = (sqrt(d2) < 0.2 && sqrt(dv2) < 0.2 && 1) ? true : false;
      break;
  }

  return converged;
}

template <int INPUT_ORDER>
void Solver<INPUT_ORDER>::genNewTraj()
{
  callOptimizer();

  switch (INPUT_ORDER)
  {
    case VEL:
      x_ = vel_get_state();
      u_ = vel_get_control();

      break;
    case ACCEL:
      x_ = accel_get_state();
      u_ = accel_get_control();

      break;
    case JERK:
      x_ = jerk_get_state();
      u_ = jerk_get_control();

      break;
  }
}

template <int INPUT_ORDER>
void Solver<INPUT_ORDER>::callOptimizer()
{
  bool converged = false;

  double dt = getDTInitial();

  double** x;
  int i = 0;
  int r = 0;
  while (1)
  {
    dt = dt + 3 * 0.05;
    i = i + 1;
    switch (INPUT_ORDER)
    {
      case VEL:
      {
        vel_load_default_data(dt, v_max_, x0_, xf_, q_);
        r = vel_optimize();
        if (r == 1)
        {
          x = vel_get_state();
          converged = checkConvergence(x[N_]);
        }
        break;
      }
      case ACCEL:
      {
        accel_load_default_data(dt, v_max_, a_max_, x0_, xf_, q_);
        r = accel_optimize();
        if (r == 1)
        {
          x = accel_get_state();
          converged = checkConvergence(x[N_]);
        }
        break;
      }
      case JERK:
      {
        jerk_load_default_data(dt, v_max_, a_max_, j_max_, x0_, xf_, q_);
        r = jerk_optimize();
        if (r == 1)
        {
          x = jerk_get_state();
          converged = checkConvergence(x[N_]);
        }
        break;
      }
    }
    if (converged == 1)
    {
      break;
    }
  }

  if (i > 1)
  {
    printf("Iterations = %d\n", i);
    printf("Iterations>1, if you increase dt at the beginning, it would be faster\n");
  }
  dt_ = dt;
}

inline double MinPositiveElement(std::vector<double> v)
{
  std::sort(v.begin(), v.end());  // sorted in ascending order
  double min_value = 0;
  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] > 0)
    {
      min_value = v[i];
      break;
    }
  }
  return min_value;
}

template <int INPUT_ORDER>
double Solver<INPUT_ORDER>::getDTInitial()
{
  double dt_initial = 0;
  float t_vx = 0;
  float t_vy = 0;
  float t_vz = 0;
  float t_ax = 0;
  float t_ay = 0;
  float t_az = 0;
  float t_jx = 0;
  float t_jy = 0;
  float t_jz = 0;

  t_vx = (xf_[0] - x0_[0]) / v_max_;
  t_vy = (xf_[1] - x0_[1]) / v_max_;
  t_vz = (xf_[2] - x0_[2]) / v_max_;

  switch (INPUT_ORDER)
  {
    case JERK:
    {
      float jerkx = copysign(1, xf_[0] - x0_[0]) * j_max_;
      float jerky = copysign(1, xf_[1] - x0_[1]) * j_max_;
      float jerkz = copysign(1, xf_[2] - x0_[2]) * j_max_;
      float a0x = x0_[6];
      float a0y = x0_[7];
      float a0z = x0_[8];
      float v0x = x0_[3];
      float v0y = x0_[4];
      float v0z = x0_[5];

      // polynomial ax3+bx2+cx+d=0 --> coeff=[d c b a]
      Eigen::Vector4d coeff_jx(x0_[0] - xf_[0], v0x, a0x / 2.0, jerkx / 6.0);
      Eigen::Vector4d coeff_jy(x0_[1] - xf_[1], v0y, a0y / 2.0, jerky / 6.0);
      Eigen::Vector4d coeff_jz(x0_[2] - xf_[2], v0z, a0z / 2.0, jerkz / 6.0);

      /*  std::cout << "Coefficients for jerk" << std::endl;
        std::cout << "Coeffx=" << coeff_jx.transpose() << std::endl;
        std::cout << "Coeffy=" << coeff_jy.transpose() << std::endl;
        std::cout << "Coeffz=" << coeff_jz.transpose() << std::endl;*/

      Eigen::PolynomialSolver<double, Eigen::Dynamic> psolve_jx(coeff_jx);
      Eigen::PolynomialSolver<double, Eigen::Dynamic> psolve_jy(coeff_jy);
      Eigen::PolynomialSolver<double, Eigen::Dynamic> psolve_jz(coeff_jz);

      std::vector<double> realRoots_jx;
      std::vector<double> realRoots_jy;
      std::vector<double> realRoots_jz;
      psolve_jx.realRoots(realRoots_jx);
      psolve_jy.realRoots(realRoots_jy);
      psolve_jz.realRoots(realRoots_jz);

      t_jx = MinPositiveElement(realRoots_jx);
      t_jy = MinPositiveElement(realRoots_jy);
      t_jz = MinPositiveElement(realRoots_jz);

      // printf("Times: t_jx, t_jy, t_jz:\n");
      // std::cout << t_jx << "  " << t_jy << "  " << t_jz << std::endl;

      // Here there is no a break
    }
    case ACCEL:
    {
      float accelx = copysign(1, xf_[0] - x0_[0]) * a_max_;
      float accely = copysign(1, xf_[1] - x0_[1]) * a_max_;
      float accelz = copysign(1, xf_[2] - x0_[2]) * a_max_;
      float v0x = x0_[3];
      float v0y = x0_[4];
      float v0z = x0_[5];

      // polynomial ax2+bx+c=0 --> coeff=[c b a]
      Eigen::Vector3d coeff_ax(x0_[0] - xf_[0], v0x, 0.5 * accelx);
      Eigen::Vector3d coeff_ay(x0_[1] - xf_[1], v0y, 0.5 * accely);
      Eigen::Vector3d coeff_az(x0_[2] - xf_[2], v0z, 0.5 * accelz);

      Eigen::PolynomialSolver<double, Eigen::Dynamic> psolve_ax(coeff_ax);
      Eigen::PolynomialSolver<double, Eigen::Dynamic> psolve_ay(coeff_ay);
      Eigen::PolynomialSolver<double, Eigen::Dynamic> psolve_az(coeff_az);

      std::vector<double> realRoots_ax;
      std::vector<double> realRoots_ay;
      std::vector<double> realRoots_az;
      psolve_ax.realRoots(realRoots_ax);
      psolve_ay.realRoots(realRoots_ay);
      psolve_az.realRoots(realRoots_az);

      t_ax = MinPositiveElement(realRoots_ax);
      t_ay = MinPositiveElement(realRoots_ay);
      t_az = MinPositiveElement(realRoots_az);
    }
    case VEL:
    {
      // I'm done
      break;
    }
  }
  dt_initial = std::max({ t_vx, t_vy, t_vz, t_ax, t_ay, t_az, t_jx, t_jy, t_jz }) / N_;
  if (dt_initial > 10000)  // happens when there is no solution to the previous eq.
  {
    printf("There is not a solution to find the intial dt");
    dt_initial = 0;
  }
  return dt_initial;
}

#endif