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


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

}


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


#pragma once

#include "qnum.hpp"
#include "flex.hpp"
#include <Eigen/Core>
#include "autodiff/reverse.hpp"
/// Eigen3 supporting types and helpers

// Eigen specializations
namespace Eigen
{
  using namespace qnum;
  using namespace flex;
  using namespace autodiff;
  /// Traits specialization for qspace_number_t.
  /// See Eigen/src/Core/NumTraits.h for documentation.
  template<typename T, int E> struct NumTraits<qspace_number_t<T, E>>
    : GenericNumTraits<qspace_number_t<T, E>>
  {
    typedef qspace_number_t<T, E> Real;
    typedef qspace_number_t<T, E> NonInteger;
    typedef qspace_number_t<T, E> Nested;
    typedef qspace_number_t<T, E> Literal;

    enum {
      IsComplex = 0,
      IsInteger = 0,
      ReadCost = 1,
      AddCost = 1,
      MulCost = 1,
      IsSigned = 1,
      RequireInitialization = 1,
    };

    static inline Real epsilon() { return Real::from_literal(1, false); }
    static inline Real dummy_precision() { Real v; return v; }
    static inline Real highest() { return Real::from_literal(Real::T_max(), true); }
    static inline Real lowest() { return Real::from_literal(Real::T_min(), true); }
    static inline int digits10() { return std::numeric_limits<T>::digits10; }
  };

  namespace internal {
    /// Partial specialization for random implementation for qspace numbers.
    /// See MathFunctions.h L535
    template<typename T, int E> struct random_impl<qspace_number_t<T, E>>
      : random_default_impl
        <
        qspace_number_t<T, E>,
        NumTraits<qspace_number_t<T, E>>::IsComplex,
        NumTraits<qspace_number_t<T, E>>::IsInteger
        >
    {
      typedef qspace_number_t<T, E> _Q;
      static inline _Q run(const _Q& x, const _Q& y) {
        if (x > y) return x;

        int rn = std::rand() * RAND_MAX + std::rand();
        typename _Q::Tu xu = static_cast<typename _Q::Tu>(x.val);
        typename _Q::Tu yu = static_cast<typename _Q::Tu>(y.val);
        auto ru = static_cast<typename _Q::Tu>(rn) % yu - xu;
        return _Q::from_literal(static_cast<T>(ru), false);
      }
      static inline _Q run() {
        int rn = std::rand() * RAND_MAX + std::rand();
        return _Q::from_literal(static_cast<T>(rn) >> _Q::ext_bits(), false);
      }
    };

    template<typename T, int E> struct random_impl<Variable<qspace_number_t<T, E>>>
      : random_default_impl
        <
        Variable<qspace_number_t<T, E>>,
        NumTraits<Variable<qspace_number_t<T, E>>>::IsComplex,
        NumTraits<Variable<qspace_number_t<T, E>>>::IsInteger
        >
    {
      typedef qspace_number_t<T, E> _Q;
      static inline Variable<_Q> run(const Variable<_Q>& x, const Variable<_Q>& y) {
        return Variable<_Q>(random_impl<_Q>::run(x.expr->val, y.expr->val));
      }
      static inline Variable<_Q> run() {
        return Variable<_Q>(random_impl<_Q>::run());
      }
    };
  }

  /// Traits specialization for flexfloat.
  /// See Eigen/src/Core/NumTraits.h for documentation.
  template<uint8_t E, uint8_t F> struct NumTraits<flexfloat<E, F>>
    : NumTraits<double>
  {
    typedef flexfloat<E, F> Real;
    typedef flexfloat<E, F> NonInteger;
    typedef flexfloat<E, F> Nested;
    typedef flexfloat<E, F> Literal;

    enum {
      RequireInitialization = 1,
    };

    static inline Real epsilon() {
      Real v;
      flexfloat_t ff = (flexfloat_t)v;
      flexfloat_set_bits(&ff, 1);
      return (Real)ff;
    }
    static inline Real dummy_precision() {
      Real v;
      return v;
    }
    static inline Real highest() { return Real(FLT_MAX); }
    static inline Real lowest() { return Real(FLT_MIN);}
    static inline int digits10() { return 10; } // XXX wrong
  };

  namespace internal {
    /// Partial specialization for random implementation for flexfloat.
    /// See MathFunctions.h L535
    template<uint8_t E, uint8_t F> struct random_impl<flexfloat<E, F>>
      : random_default_impl
        <
        flexfloat<E, F>,
        NumTraits<flexfloat<E, F>>::IsComplex,
        NumTraits<flexfloat<E, F>>::IsInteger
        >
    {
      typedef flexfloat<E, F> _Q;
      static inline _Q run(const _Q& x, const _Q& y) {
        if (x > y) return x;
        double rn = std::rand();
        rn = rn / RAND_MAX * (y - x) + x;
        return (_Q)rn;
      }
      static inline _Q run() {
        double rn = std::rand();
        rn = (rn / RAND_MAX) * 2.0 - 1.0;
        return (_Q)rn;
      }
    };

    template<uint8_t E, uint8_t F> struct random_impl<Variable<flexfloat<E, F>>>
      : random_default_impl
        <
        Variable<flexfloat<E, F>>,
        NumTraits<Variable<flexfloat<E, F>>>::IsComplex,
        NumTraits<Variable<flexfloat<E, F>>>::IsInteger
        >
    {
      typedef flexfloat<E, F> _Q;
      static inline Variable<_Q> run(const Variable<_Q>& x, const Variable<_Q>& y) {
        return Variable<_Q>(random_impl<_Q>::run(x.expr->val, y.expr->val));
      }
      static inline Variable<_Q> run() {
        return Variable<_Q>(random_impl<_Q>::run());
      }
    };
  }
}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/constants.h>
#include <scitbx/math/mean_and_variance.h>
#include <scitbx/vec3.h>
#include <cctbx/miller.h>

#include <vector>
#include <map>

using namespace boost::python;

namespace xfel {

struct mark2_iteration {
  typedef scitbx::af::shared<double> farray;
  typedef scitbx::af::shared<int> iarray;
  farray values;
  farray tox;
  farray toy;
  farray spotcx;
  farray spotcy;
  farray spotfx;
  farray spotfy;
  iarray master_tiles;
  double functional;
  farray gradients_,curvatures_;

  farray model_calcx, model_calcy;
  double calc_minus_To_x, calc_minus_To_y;
  double rotated_o_x, rotated_o_y;
  double partial_partial_theta_x, partial_partial_theta_y;
  double partial_sq_theta_x, partial_sq_theta_y;

  farray sine,cosine;

  mark2_iteration(){}
  mark2_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles):
    values(values),tox(tox),toy(toy),spotcx(spotcx),spotcy(spotcy),
    master_tiles(master_tiles),
    model_calcx(spotcx.size(),scitbx::af::init_functor_null<double>()),
    model_calcy(spotcx.size(),scitbx::af::init_functor_null<double>()),
    functional(0.),
    gradients_(3*64),
    curvatures_(3*64)
  {
    SCITBX_ASSERT(tox.size()==64);
    SCITBX_ASSERT(toy.size()==64);
    SCITBX_ASSERT(values.size()==3*64);
    for (int tidx=0; tidx < 64; ++tidx){
      cosine.push_back(std::cos(values[128+tidx]*scitbx::constants::pi_180));
      sine.push_back(std::sin(values[128+tidx]*scitbx::constants::pi_180));
    }

    for (int ridx=0; ridx < spotcx.size(); ++ridx){
      int itile = master_tiles[ridx];
      calc_minus_To_x = spotcx[ridx] - tox[itile];
      calc_minus_To_y = spotcy[ridx] - toy[itile];

      rotated_o_x = calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];
      rotated_o_y = calc_minus_To_x * sine[itile] +   calc_minus_To_y * cosine[itile];

      model_calcx[ridx] = rotated_o_x + (tox[itile] + values[2*itile]);
      model_calcy[ridx] = rotated_o_y + (toy[itile] + values[2*itile+1]);

      partial_partial_theta_x = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];
      partial_partial_theta_y =  calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];

      partial_sq_theta_x = -calc_minus_To_x * cosine[itile] + calc_minus_To_y * sine[itile];
      partial_sq_theta_y = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];

      double delx = model_calcx[ridx] - spotfx[ridx];
      double dely = model_calcy[ridx] - spotfy[ridx];
      double delrsq(delx*delx + dely*dely);
      functional += delrsq; // sum of square differences

      gradients_[2*itile]  += 2. *  delx;
      gradients_[2*itile+1]+= 2. *  dely;

      gradients_[128+itile] += scitbx::constants::pi_180 * 2.* (
        delx * partial_partial_theta_x +
        dely * partial_partial_theta_y
      );

      curvatures_[2*itile] += 2.;
      curvatures_[2*itile+1] += 2.;

      curvatures_[128+itile] += scitbx::constants::pi_180 * scitbx::constants::pi_180 * 2. * (
        ( partial_partial_theta_x*partial_partial_theta_x +
          partial_partial_theta_y*partial_partial_theta_y ) +
        ( delx*partial_sq_theta_x + dely*partial_sq_theta_y )
      );
    }
  }
  mark2_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles,iarray frames,int const& nframes,
                  bool const& inheritable):
    values(values),tox(tox),toy(toy),spotcx(spotcx),spotcy(spotcy),
    master_tiles(master_tiles),
    model_calcx(spotcx.size(),scitbx::af::init_functor_null<double>()),
    model_calcy(spotcx.size(),scitbx::af::init_functor_null<double>()),
    functional(0.){}

  mark2_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles,iarray frames,int const& nframes):
    values(values),tox(tox),toy(toy),spotcx(spotcx),spotcy(spotcy),
    master_tiles(master_tiles),
    model_calcx(spotcx.size(),scitbx::af::init_functor_null<double>()),
    model_calcy(spotcx.size(),scitbx::af::init_functor_null<double>()),
    functional(0.),
    gradients_(3*64+2*nframes),
    curvatures_(3*64+2*nframes)
  {
    SCITBX_ASSERT(tox.size()==64);
    SCITBX_ASSERT(toy.size()==64);
    SCITBX_ASSERT(values.size()==3*64+2*nframes);
    for (int tidx=0; tidx < 64; ++tidx){
      cosine.push_back(std::cos(values[128+tidx]*scitbx::constants::pi_180));
      sine.push_back(std::sin(values[128+tidx]*scitbx::constants::pi_180));
    }

    for (int ridx=0; ridx < spotcx.size(); ++ridx){
      int itile = master_tiles[ridx];
      int frame_param_no = frames[ridx];
      calc_minus_To_x = spotcx[ridx] - tox[itile];
      calc_minus_To_y = spotcy[ridx] - toy[itile];

      rotated_o_x = calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];
      rotated_o_y = calc_minus_To_x * sine[itile] +   calc_minus_To_y * cosine[itile];

      model_calcx[ridx] = rotated_o_x + (tox[itile] + values[2*itile]);
      model_calcy[ridx] = rotated_o_y + (toy[itile] + values[2*itile+1]);

      partial_partial_theta_x = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];
      partial_partial_theta_y =  calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];

      partial_sq_theta_x = -calc_minus_To_x * cosine[itile] + calc_minus_To_y * sine[itile];
      partial_sq_theta_y = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];

      double delx = model_calcx[ridx] - spotfx[ridx];
      double dely = model_calcy[ridx] - spotfy[ridx];
      double delrsq(delx*delx + dely*dely);
      functional += delrsq; // sum of square differences

      gradients_[2*itile]  += 2. *  delx;
      gradients_[2*itile+1]+= 2. *  dely;
      if (frame_param_no < nframes){
        gradients_[192+2*frame_param_no]  += 2. *  delx;
        gradients_[193+2*frame_param_no]  += 2. *  dely;
      }

      gradients_[128+itile] += scitbx::constants::pi_180 * 2.* (
        delx * partial_partial_theta_x +
        dely * partial_partial_theta_y
      );

      curvatures_[2*itile] += 2.;
      curvatures_[2*itile+1] += 2.;
      if (frame_param_no < nframes){
        curvatures_[192+2*frame_param_no] += 2.;
        curvatures_[193+2*frame_param_no] += 2.;
      }

      curvatures_[128+itile] += scitbx::constants::pi_180 * scitbx::constants::pi_180 * 2. * (
        ( partial_partial_theta_x*partial_partial_theta_x +
          partial_partial_theta_y*partial_partial_theta_y ) +
        ( delx*partial_sq_theta_x + dely*partial_sq_theta_y )
      );
    }
  }

  double f(){ return functional; }
  farray gradients(){ return gradients_; }
  farray curvatures(){ return curvatures_; }
};

struct mark3_collect_data{
  //adapt all-frame data to individual-frame parameter refinement
  typedef scitbx::af::shared<double> farray;
  typedef scitbx::af::shared<int> iarray;
  typedef scitbx::af::shared<cctbx::miller::index<> > marray;
  typedef scitbx::af::shared<bool> barray;
  marray HKL;
  std::map<int,int> frame_first_index, frame_match_count;
  farray result_model_cx,result_model_cy;
  barray result_flags;
  scitbx::af::shared<scitbx::vec3<double> > result_part_distance;

  mark3_collect_data(){}
  mark3_collect_data(iarray frame_id, marray indices):
    HKL(indices),
    result_model_cx(frame_id.size(),scitbx::af::init_functor_null<double>()),
    result_model_cy(frame_id.size(),scitbx::af::init_functor_null<double>()),
    result_flags(frame_id.size(),scitbx::af::init_functor_null<bool>()),
    result_part_distance(frame_id.size(),scitbx::af::init_functor_null<scitbx::vec3<double> >())
  {
    SCITBX_ASSERT(frame_id.size()==indices.size());

    for (int idx=0; idx < frame_id.size(); ++idx){
      int iframe = frame_id[idx];
      if (frame_first_index.find(iframe)==frame_first_index.end()){
        frame_first_index[iframe]=idx;
        frame_match_count[iframe]=1;
      } else {
        SCITBX_ASSERT(
          frame_first_index[iframe]+frame_match_count[iframe] == idx);// each frame all contiguous
        frame_match_count[iframe]+=1;
      }
    }
  }

  int
  get_first_index(int const& frame_id) const {
    return frame_first_index.find(frame_id)->second;
  }

  marray
  frame_indices(int const& frame_id)const{
    marray result;
    int last = frame_first_index.find(frame_id)->second + frame_match_count.find(frame_id)->second;
    for (int idx=frame_first_index.find(frame_id)->second; idx < last; ++idx){
      result.push_back(HKL[idx]);
    }
    return result;
  }

  barray
  selection(int const& frame_id)const{
    barray result(result_model_cx.size());
    int last = frame_first_index.find(frame_id)->second + frame_match_count.find(frame_id)->second;
    for (int idx=frame_first_index.find(frame_id)->second; idx < last; ++idx){
      result[idx] = true;
    }
    return result;
  }

  void collect(scitbx::af::shared<scitbx::vec3<double> > hi_E_limit,
               scitbx::af::shared<scitbx::vec3<double> > lo_E_limit,
               barray observed_flag,
               int const& frame_id){
    int first = frame_first_index.find(frame_id)->second;
    for (int im = 0; im < hi_E_limit.size(); ++im){
      result_model_cx[first + im] = (hi_E_limit[im][1] + lo_E_limit[im][1])/2.;
      result_model_cy[first + im] = (hi_E_limit[im][0] + lo_E_limit[im][0])/2.;
      result_flags[first + im] = observed_flag[im];
      SCITBX_ASSERT (observed_flag[im]); // no current support for masked-out spots
    }
  }
  void collect_mean_position(scitbx::af::shared<scitbx::vec3<double> > mean_position,
               barray observed_flag,
               int const& frame_id){
    int first = frame_first_index.find(frame_id)->second;
    for (int im = 0; im < mean_position.size(); ++im){
      result_model_cx[first + im] = mean_position[im][1];
      result_model_cy[first + im] = mean_position[im][0];
      result_flags[first + im] = observed_flag[im];
      SCITBX_ASSERT (observed_flag[im]); // no current support for masked-out spots
    }
  }
  void collect_distance(scitbx::af::shared<scitbx::vec3<double> > part_distance,
               int const& frame_id){
    int first = frame_first_index.find(frame_id)->second;
    for (int im = 0; im < part_distance.size(); ++im){
      result_part_distance[first + im] = part_distance[im];
    }
  }
};

struct mark5_iteration: public mark2_iteration {
  typedef scitbx::af::shared<scitbx::vec3<double> > vec3array;
  typedef scitbx::vec3<double>                      vec3;

  mark5_iteration(farray values, farray tox, farray toy, farray spotcx, farray spotcy,
                  farray spotfx, farray spotfy,
                  iarray master_tiles,iarray frames,int const& nframes,
                  vec3array partial_r_partial_distance):
    mark2_iteration(values, tox, toy, spotcx, spotcy, spotfx, spotfy,
                    master_tiles, frames, nframes, true)
  {
    gradients_ = farray(3*64+3*nframes);
    curvatures_ = farray(3*64+3*nframes);
    SCITBX_ASSERT(tox.size()==64);
    SCITBX_ASSERT(toy.size()==64);
    SCITBX_ASSERT(values.size()==3*64+3*nframes);
    for (int tidx=0; tidx < 64; ++tidx){
      cosine.push_back(std::cos(values[128+tidx]*scitbx::constants::pi_180));
      sine.push_back(std::sin(values[128+tidx]*scitbx::constants::pi_180));
    }

    for (int ridx=0; ridx < spotcx.size(); ++ridx){
      int itile = master_tiles[ridx];
      int frame_param_no = frames[ridx];

      calc_minus_To_x = spotcx[ridx] - tox[itile];
      calc_minus_To_y = spotcy[ridx] - toy[itile];

      rotated_o_x = calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];
      rotated_o_y = calc_minus_To_x * sine[itile] +   calc_minus_To_y * cosine[itile];

      model_calcx[ridx] = rotated_o_x + (tox[itile] + values[2*itile]);
      model_calcy[ridx] = rotated_o_y + (toy[itile] + values[2*itile+1]);

      partial_partial_theta_x = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];
      partial_partial_theta_y =  calc_minus_To_x * cosine[itile] - calc_minus_To_y * sine[itile];

      partial_sq_theta_x = -calc_minus_To_x * cosine[itile] + calc_minus_To_y * sine[itile];
      partial_sq_theta_y = -calc_minus_To_x * sine[itile] - calc_minus_To_y * cosine[itile];

      double delx = model_calcx[ridx] - spotfx[ridx];
      double dely = model_calcy[ridx] - spotfy[ridx];
      double delrsq(delx*delx + dely*dely);
      functional += delrsq; // sum of square differences

      vec3 part_r_part_d = partial_r_partial_distance[ridx];
      double rotated_part_r_c_x = part_r_part_d[0] * cosine[itile] - part_r_part_d[1] * sine[itile];
      double rotated_part_r_c_y = part_r_part_d[0] * sine[itile] +   part_r_part_d[1] * cosine[itile];

      //if (frame_param_no==10){
      //        printf("%5d %8.4f %8.4f", itile, part_r_part_d[0], part_r_part_d[1]);
      //        printf("%5d %8.4f %8.4f\n", itile, rotated_part_r_c_x, rotated_part_r_c_y);
      //}

      gradients_[2*itile]  += 2. *  delx;
      gradients_[2*itile+1]+= 2. *  dely;
      if (frame_param_no < nframes){
        gradients_[192+2*frame_param_no]  += 2. *  delx;
        gradients_[193+2*frame_param_no]  += 2. *  dely;
        gradients_[192 + 2*nframes + frame_param_no] += 2. * (
          delx * rotated_part_r_c_y + dely * rotated_part_r_c_x
        );//  SOMETHING IS ROTTEN IN DENMARK
      }

      gradients_[128+itile] += scitbx::constants::pi_180 * 2.* (
        delx * partial_partial_theta_x +
        dely * partial_partial_theta_y
      );

      curvatures_[2*itile] += 2.;
      curvatures_[2*itile+1] += 2.;
      if (frame_param_no < nframes){
        curvatures_[192+2*frame_param_no] += 2.;
        curvatures_[193+2*frame_param_no] += 2.;
        curvatures_[192 + 2*nframes + frame_param_no] +=2 * (
          rotated_part_r_c_x * rotated_part_r_c_x + rotated_part_r_c_y * rotated_part_r_c_y
        );
      }

      curvatures_[128+itile] += scitbx::constants::pi_180 * scitbx::constants::pi_180 * 2. * (
        ( partial_partial_theta_x*partial_partial_theta_x +
          partial_partial_theta_y*partial_partial_theta_y ) +
        ( delx*partial_sq_theta_x + dely*partial_sq_theta_y )
      );
    }

  }
};

namespace boost_python { namespace {

  void
  metrology_init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    class_<mark2_iteration>("mark2_iteration",no_init)
      .def(init< >())
      .def(init<mark2_iteration::farray, mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::iarray >(
        (arg_("values"),arg("tox"),arg_("toy"),arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"))))
      .def(init<mark2_iteration::farray, mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::farray, mark2_iteration::farray,
                mark2_iteration::iarray, mark2_iteration::iarray, int const& >(
        (arg_("values"),arg("tox"),arg_("toy"),arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"), arg_("frames"), arg_("nframes"))))
      .def("f",&mark2_iteration::f)
      .def("gradients",&mark2_iteration::gradients)
      .def("curvatures",&mark2_iteration::curvatures)
      .add_property("model_calcx", make_getter(&mark2_iteration::model_calcx, rbv()))
      .add_property("model_calcy", make_getter(&mark2_iteration::model_calcy, rbv()))
    ;
    class_<mark3_collect_data>("mark3_collect_data",no_init)
      .def(init<mark3_collect_data::iarray,mark3_collect_data::marray>())
      .def("get_first_index",&mark3_collect_data::get_first_index)
      .def("frame_indices",&mark3_collect_data::frame_indices)
      .def("selection",&mark3_collect_data::selection)
      .def("collect",&mark3_collect_data::collect)
      .def("collect_mean_position",&mark3_collect_data::collect_mean_position)
      .add_property("cx", make_getter(&mark3_collect_data::result_model_cx, rbv()))
      .add_property("cy", make_getter(&mark3_collect_data::result_model_cy, rbv()))
      .add_property("flags", make_getter(&mark3_collect_data::result_flags, rbv()))
      .def("collect_distance",&mark3_collect_data::collect_distance)
      .add_property("part_distance", make_getter(&mark3_collect_data::result_part_distance, rbv()))
    ;
    class_<mark5_iteration, bases<mark2_iteration> >("mark5_iteration",no_init)
      .def(init<mark5_iteration::farray, mark5_iteration::farray, mark5_iteration::farray,
                mark5_iteration::farray, mark5_iteration::farray,
                mark5_iteration::farray, mark5_iteration::farray,
                mark5_iteration::iarray, mark5_iteration::iarray, int const&,
                mark5_iteration::vec3array >(
        (arg_("values"),arg("tox"),arg_("toy"),arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"), arg_("frames"), arg_("nframes"), arg_("part_distance"))))
    ;
}
}}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(xfel_metrology_ext)
{
  xfel::boost_python::metrology_init_module();

}


/*****************************************************************************
*
* Functions for calculating the gap
*
* Copyright (C) 2018 by Hidemaro Suwa
* e-mail:suwamaro@phys.s.u-tokyo.ac.jp
*
*****************************************************************************/

#include "calc_gap.h"
#include "rpa_util.h"
#include <armadillo>

cx_double calc_intensity_cubic(int L, hoppings const& ts, double mu, double U, double delta, double qx, double qy, double qz, cx_double omega, bool zz){
  double k1 = 2. * M_PI / (double)L;

  cx_double A = 0, B = 0, C = 0, D = 0;
  /* Summing up at all the wavevectors */
  for(int x=-L/2; x < L/2; x++){
    double kx = k1 * x;
    for(int y=-L/2; y < L/2; y++){
      double ky = k1 * y;
      for(int z=-L/2; z < L/2; z++){
	double kz = k1 * z;

	add_to_sus_mat2( ts, mu, A, B, C, D, qx, qy, qz, kx, ky, kz, delta, omega, zz );
	// double e_free = energy_free_electron( t, mu, kx, ky, kz );
	// double e_eps = 1e-12;
	// if ( e_free < e_eps ) {
	//   /* Summing up over all k inside the Brillouin zone. */
	//   double diff_x = wave_vector_in_BZ( kx - qx );
	//   double diff_y = wave_vector_in_BZ( ky - qy );
	//   double diff_z = wave_vector_in_BZ( kz - qz );
	//   double e_free2 = energy_free_electron( t, mu, diff_x, diff_y, diff_z );
	//   add_to_sus_mat( A, B, D, e_free, e_free2, delta, omega );
	// }
      }
    }
  }

  int n_sites = L * L * L;
  A *= 2. / (double)n_sites;
  B *= 2. / (double)n_sites;
  C *= 2. / (double)n_sites;
  D *= 2. / (double)n_sites;

  /* RPA */
  arma::cx_mat chi0_mat(2,2);
  chi0_mat(0,0) = A;   // (A, A) correlation
  chi0_mat(0,1) = B;   // (A, B)
  chi0_mat(1,0) = C;   // (B, A)
  chi0_mat(1,1) = D;   // (B, B)

  /* Transverse = < \sigma^- \sigma^+ >; Longitudinal (zz) = < \sigma^z \sigma^z > */
  /* Note that 2 < \sigma^- \sigma^+ > = < \sigma^z \sigma^z > (U -> 0 for the SU(2) case) */
  double factor_channel = 1.0;
  if ( zz ) { factor_channel = 0.5; }

  arma::cx_mat denom = arma::eye<arma::cx_mat>(2,2) - factor_channel * U * chi0_mat;
  arma::cx_mat chi_mat = chi0_mat * arma::inv(denom);

  // sigma-to-spin factor
  double factor_operator = 0.5;
  cx_double chi = factor_operator * factor_operator * ( chi_mat(0,0) - chi_mat(1,0) - chi_mat(0,1) + chi_mat(1,1) );

  return chi;

  // A *= 2. / (double)n_sites;
  // B *= 2. / (double)n_sites;
  // D *= 2. / (double)n_sites;

  // /* RPA */
  // arma::cx_mat chi0_mat(2,2);
  // chi0_mat(0,0) = A;   // (A, A) correlation
  // chi0_mat(0,1) = B;   // (A, B)
  // chi0_mat(1,0) = B;   // (B, A)
  // chi0_mat(1,1) = D;   // (B, B)
  // arma::cx_mat denom = arma::eye<arma::cx_mat>(2,2) - U * chi0_mat;
  // arma::cx_mat chi_mat = chi0_mat * arma::inv(denom);

  // // Double counting from A and B
  // double factor_sublattice = 0.5;
  // cx_double chi = factor_sublattice * factor_sublattice * ( chi_mat(0,0) - chi_mat(1,0) - chi_mat(0,1) + chi_mat(1,1) );

  // // // for check
  // // std::cout << qx << "  " << qy << "  " << omega << "  " << A << "  " << B << "  " << D << "  " << chi_mat(0,0) << "  " << chi_mat(0,1) << "  " << chi_mat(1,0) << "  " << chi_mat(1,1) << "  " << chi << std::endl;

  // return chi;
}


#include "powerlawCommon.h"
#include <boost/program_options.hpp>

/**
 * @author: W.M. Otte (wim@invivonmr.uu.nl); Image Sciences Institute, UMC Utrecht, NL.
 * @date: 19-11-2009
 *
 * Estimate powerlaw scaling parameter from input distribution.
 *
 * ***************************************************************************
 * Method: "Power-law distributions in empirical data", Clauset et al, 2009
 * http://www.santafe.edu/~aaronc/powerlaws/
 * ***************************************************************************
 */
class PowerLawFit
{

public:

	typedef double ValueType;
	typedef std::vector< ValueType > VectorType;

	/**
	 * Power law fit.
	 */
	void run( const std::string& inputFileName, bool nosmall, bool finite,
					double startXmin, double incrementXmin, double endXmin,
						bool bootstrap, unsigned int bootstrapIterations, bool verbose )
	{
		// [ 1 ] read input from text file ...
		VectorType values = getInput( inputFileName );

		// [ 2 ] bootstrap or single fit ...
		VectorType results;

		if ( bootstrap )
		{
			graph::Powerlaw< ValueType >::BootstrapFit( values, results, nosmall, finite, startXmin, incrementXmin, endXmin, bootstrapIterations, verbose );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) <<  std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
				std::cout << "Alpha_sd," << results.at( 3 ) <<  std::endl;
				std::cout << "Xmin_sd," << results.at( 4 ) << std::endl;
				std::cout << "Log-likelihood_sd," << results.at( 5 ) << std::endl;

			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"bootstrap estimation failed! -> check input ..." << std::endl;
			}

		}
		else
		{
			graph::Powerlaw< ValueType >::SingleFit( values, results, nosmall, finite,
					startXmin, incrementXmin, endXmin );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) << std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"single estimation failed! -> check input ..." << std::endl;
			}
		}
	}

protected:

	/**
	 * Return input from given text file as vector.
	 */
	VectorType getInput( const std::string& input )
	{
		std::ifstream inFile;

		inFile.open( input.c_str() );
		if ( !inFile )
		{
			std::cout << "*** ERROR ***: Unable to open: " << input << "." << std::endl;
			exit( EXIT_FAILURE );
		}

		double x;
		VectorType output;

		while ( inFile >> x )
		{
			output.push_back( x );
		}
		inFile.close();

		/**
		 * Negative values will be converted to complex numbers in matlab,
		 * but not with the stl ...
		 *
		 * No support is given (yet) for complex number mle.
		 */
		if ( *( std::min_element( output.begin(), output.end() ) ) < 0 )
		{
			std::cerr << "*** ERROR ***: Negative input not supported!" << std::endl;
			exit (EXIT_FAILURE );
		}

		return output;
	}
};

// ************************************************************************************

/**
 * Throw error if required option is not specified.
 */
void required_option( const boost::program_options::variables_map& vm,
		const std::string& required_option )
{
	if ( vm.count( required_option ) == 0 )
		throw std::logic_error( "Option: '" + required_option + "' is required!" );
}

/**
 * Fit powerlaw to list of numbers.
 */
int main(int argc, char* argv[])
{
	namespace po = boost::program_options;

	// application description ...
	std::string description = "Fits a power-law distributional model to data.\n";

	// options ...
	std::string input;

	bool nosmall;
	bool finite;
	bool bootstrap;
	bool verbose;

	double startXmin;
	double incrementXmin;
	double endXmin;

	unsigned int bootstrapIterations;

	try {

        po::options_description desc("Available options");

        desc.add_options()

            ( "input,i", po::value< std::string >( &input )
            		, "string: input file with distribution values in column format." )

            ( "finite,f", po::value< bool >( &finite )
            		->default_value( false )
            		->zero_tokens()
            		, "bool: use an experimental finite-size correction." )

            ( "verbose,v", po::value< bool >( &verbose )
					->default_value( false )
            		->zero_tokens()
            		, "bool: print bootstrap status." )

            ( "nosmall,s", po::value< bool >( &nosmall )
            		->default_value( false )
            		->zero_tokens()
            		, "bool: truncate the search over xmin values before the finite-size bias becomes significant." )

            ( "bootstrap,b", po::value< bool >( &bootstrap )
            		->default_value( false )
            		->zero_tokens()
            		, "bool: run non-parametric bootstrap instead of single estimation." )

            ( "start-xmin,x", po::value< double >( &startXmin )
            		->default_value( 1.5 )
            		, "float: start value for discrete xmin estimation." )

            ( "increment-xmin,y", po::value< double >( &incrementXmin )
            		->default_value( 0.01 )
            		, "float: increment value for discrete xmin estimation." )

            ( "end-xmin,z", po::value< double >( &endXmin )
            		->default_value( 3.5 )
            		, "float: end value for discrete xmin estimation." )

            ( "bootstrap-iterations,n", po::value< unsigned int >( &bootstrapIterations )
            		->default_value( 1000 )
            		, "uint: bootstrap iterations." )

            ( "help,h", "bool: produce help message." )
        ;

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

        // help message ...
        if ( vm.count( "help" ) )
        {
            std::cout << argv[0] << ": " << description << std::endl;
        	std::cout << desc << "\n";

        	return EXIT_SUCCESS;
        }

        // required options ...
        required_option( vm, "input" );

        // run application ...
        PowerLawFit powerlawFit;

    	powerlawFit.run( input, nosmall, finite,
							startXmin, incrementXmin, endXmin,
											bootstrap, bootstrapIterations, verbose );

    }
    catch( std::exception& e )
    {
        std::cerr << "*** ERROR ***: " << e.what() << "\n";
        std::cerr << "Use \"" << argv[0]
        		  << " --help\" for information about application usage."
        		  << std::endl;

        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}