/** @file
 *****************************************************************************

 Sample parameters for the lattice-based vector encryption scheme for the
 lattice-based R1CS ppSNARG. The LWE parameters are chosen to provide 80-bits
 of security, and correctness error 2^{-40} for verifying QAPs with degree up
 to 10000 (over a finite field of size ~10000). Parameter selection based on
 the security analysis in [LP10].

 The plaintext dimension is chosen based on the number of queries needed to
 acheive soundness error 2^{-40} for the QAP-based linear PCP for verifying
 R1CS systems with up to 10000 constraints (and a field of size ~10000).

 References:

  [LP10]: Richard Lindner and Chris Peikert. Better Key Sizes (and Attacks) for
          LWE-Based Encryption. In CT-RSA, 2011.

 *****************************************************************************
 * @author     Samir Menon, Brennan Shacklett, and David J. Wu
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#ifndef LWE_PARAM_HPP_
#define LWE_PARAM_HPP_

#include <math.h>
#include <stdint.h>
#include <NTL/ZZ.h>

namespace LWE {

// Lattice dimension (parameters chosen to ensure 80-bits of security)
const uint32_t n = 1455;

// Noise distribution standard deviation
const double stddev = 6.0;

// 15 queries (~ 2^-40 soundness error for circuits of size < 10000)
const uint32_t l = 15;
const uint32_t pt_dim = l*4;

// Plaintext modulus
const uint64_t p_int = 65537;
const NTL::ZZ p(p_int);

// Ciphertext modulus
const NTL::ZZ q(1ul << 58);
}

#endif // LWE_PARAM_HPP_


#include <learning/independences/discrete/chi_square.hpp>
#include <factors/discrete/discrete_indices.hpp>
#include <boost/math/distributions/chi_squared.hpp>

namespace learning::independences::discrete {

double ChiSquare::pvalue(const std::string& v1, const std::string& v2) const {
    std::vector<std::string> dummy_v2{v2};
    auto [cardinality, strides] = factors::discrete::create_cardinality_strides(m_df, v1, dummy_v2);
    auto joint_counts = factors::discrete::joint_counts(m_df, v1, dummy_v2, cardinality, strides);

    auto v1_marg = factors::discrete::marginal_counts(joint_counts, 0, cardinality, strides);
    auto v2_marg = factors::discrete::marginal_counts(joint_counts, 1, cardinality, strides);

    auto inv_obs = 1. / joint_counts.sum();

    double statistic = 0;
    for (int i = 0; i < cardinality(0); ++i) {
        for (int j = 0; j < cardinality(1); ++j) {
            auto expected = static_cast<double>(v1_marg(i) * v2_marg(j)) * inv_obs;

            if (expected != 0) {
                auto index = i + j * strides(1);

                auto d = joint_counts(index) - expected;
                statistic += d * d / expected;
            }
        }
    }

    auto df = (cardinality(0) - 1) * (cardinality(1) - 1);

    boost::math::chi_squared_distribution chidist(static_cast<double>(df));
    return cdf(complement(chidist, statistic));
}

double ChiSquare::pvalue(const std::string& v1, const std::string& v2, const std::string& ev) const {
    std::vector<std::string> dummy_vars{v2, ev};
    auto [cardinality, strides] = factors::discrete::create_cardinality_strides(m_df, v1, dummy_vars);
    auto joint_counts = factors::discrete::joint_counts(m_df, v1, dummy_vars, cardinality, strides);

    auto evidence_marg = factors::discrete::marginal_counts(joint_counts, 2, cardinality, strides);

    auto evidence_configurations = cardinality(2);
    auto vars_configurations = strides(2);

    double statistic = 0;

    for (auto k = 0; k < evidence_configurations; ++k) {
        if (evidence_marg(k) == 0) continue;

        auto offset = k * vars_configurations;
        auto evidence_segment = joint_counts.segment(offset, vars_configurations);

        auto v1_marg = factors::discrete::marginal_counts(evidence_segment, 0, cardinality, strides);
        auto v2_marg = factors::discrete::marginal_counts(evidence_segment, 1, cardinality, strides);

        auto inv_obs = 1. / evidence_marg(k);

        for (int i = 0; i < cardinality(0); ++i) {
            for (int j = 0; j < cardinality(1); ++j) {
                auto expected = static_cast<double>(v1_marg(i) * v2_marg(j)) * inv_obs;

                if (expected != 0) {
                    auto index = offset + i + j * strides(1);

                    auto d = joint_counts(index) - expected;
                    statistic += d * d / expected;
                }
            }
        }
    }

    auto df = (cardinality(0) - 1) * (cardinality(1) - 1) * cardinality(2);

    boost::math::chi_squared_distribution chidist(static_cast<double>(df));
    return cdf(complement(chidist, statistic));
}

double ChiSquare::pvalue(const std::string& v1, const std::string& v2, const std::vector<std::string>& ev) const {
    std::vector<std::string> dummy_vars{v2};
    dummy_vars.reserve(ev.size() + 1);
    dummy_vars.insert(dummy_vars.end(), ev.begin(), ev.end());

    auto [cardinality, strides] = factors::discrete::create_cardinality_strides(m_df, v1, dummy_vars);
    auto joint_counts = factors::discrete::joint_counts(m_df, v1, dummy_vars, cardinality, strides);

    auto evidence_configurations = cardinality.tail(ev.size()).prod();
    auto vars_configurations = cardinality(0) * cardinality(1);

    double statistic = 0;

    for (auto k = 0; k < evidence_configurations; ++k) {
        auto offset = k * vars_configurations;

        int total_sum = 0;
        auto marginal_v1 = VectorXi::Zero(cardinality(0)).eval();
        auto marginal_v2 = VectorXi::Zero(cardinality(1)).eval();

        for (auto i = 0; i < cardinality(0); ++i) {
            for (auto j = 0; j < cardinality(1); ++j) {
                auto c = joint_counts(offset + i + j * strides(1));
                marginal_v1(i) += c;
                marginal_v2(j) += c;
                total_sum += c;
            }
        }

        if (total_sum == 0) continue;

        auto inv_obs = 1. / static_cast<double>(total_sum);

        for (auto i = 0; i < cardinality(0); ++i) {
            for (auto j = 0; j < cardinality(1); ++j) {
                auto expected = static_cast<double>(marginal_v1(i) * marginal_v2(j)) * inv_obs;

                if (expected != 0) {
                    auto c = joint_counts(offset + i + j * strides(1));
                    auto d = c - expected;

                    statistic += d * d / expected;
                }
            }
        }
    }

    auto df = (cardinality(0) - 1) * (cardinality(1) - 1) * evidence_configurations;

    boost::math::chi_squared_distribution chidist(static_cast<double>(df));
    return cdf(complement(chidist, statistic));
}

}  // namespace learning::independences::discrete


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_MATH_DETAIL_HYPERGEOMETRIC_SERIES_HPP
#define BOOST_MATH_DETAIL_HYPERGEOMETRIC_SERIES_HPP

#include <cmath>
#include <cstdint>
#include <boost/math/tools/series.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/policies/error_handling.hpp>

  namespace boost { namespace math { namespace detail {

  // primary template for term of Taylor series
  template <class T, unsigned p, unsigned q>
  struct hypergeometric_pFq_generic_series_term;

  // partial specialization for 0F1
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 0u, 1u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& b, const T& z)
       : n(0), term(1), b(b), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= ((1 / ((b + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T b, z;
  };

  // partial specialization for 1F0
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 1u, 0u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a, const T& z)
       : n(0), term(1), a(a), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a + n) / (n + 1)) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a, z;
  };

  // partial specialization for 1F1
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 1u, 1u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a, const T& b, const T& z)
       : n(0), term(1), a(a), b(b), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a + n) / ((b + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a, b, z;
  };

  // partial specialization for 1F2
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 1u, 2u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a, const T& b1, const T& b2, const T& z)
       : n(0), term(1), a(a), b1(b1), b2(b2), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a + n) / ((b1 + n) * (b2 + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a, b1, b2, z;
  };

  // partial specialization for 2F0
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 2u, 0u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a1, const T& a2, const T& z)
       : n(0), term(1), a1(a1), a2(a2), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a1 + n) * (a2 + n) / (n + 1)) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a1, a2, z;
  };

  // partial specialization for 2F1
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 2u, 1u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a1, const T& a2, const T& b, const T& z)
       : n(0), term(1), a1(a1), a2(a2), b(b), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a1 + n) * (a2 + n) / ((b + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a1, a2, b, z;
  };

  // we don't need to define extra check and make a polinom from
  // series, when p(i) and q(i) are negative integers and p(i) >= q(i)
  // as described in functions.wolfram.alpha, because we always
  // stop summation when result (in this case numerator) is zero.
  template <class T, unsigned p, unsigned q, class Policy>
  inline T sum_pFq_series(detail::hypergeometric_pFq_generic_series_term<T, p, q>& term, const Policy& pol)
  {
    BOOST_MATH_STD_USING
    std::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();

    const T result = boost::math::tools::sum_series(term, boost::math::policies::get_epsilon<T, Policy>(), max_iter);

    policies::check_series_iterations<T>("boost::math::hypergeometric_pFq_generic_series<%1%>(%1%,%1%,%1%)", max_iter, pol);
    return result;
  }

  template <class T, class Policy>
  inline T hypergeometric_0F1_generic_series(const T& b, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 0u, 1u> s(b, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T hypergeometric_1F0_generic_series(const T& a, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 1u, 0u> s(a, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T log_pochhammer(T z, unsigned n, const Policy pol, int* s = 0)
  {
     BOOST_MATH_STD_USING
#if 0
     if (z < 0)
     {
        if (n < -z)
        {
           if(s)
            *s = (n & 1 ? -1 : 1);
           return log_pochhammer(T(-z + (1 - (int)n)), n, pol);
        }
        else
        {
           int cross = itrunc(ceil(-z));
           return log_pochhammer(T(-z + (1 - cross)), cross, pol, s) + log_pochhammer(T(cross + z), n - cross, pol);
        }
     }
     else
#endif
     {
        if (z + n < 0)
        {
           T r = log_pochhammer(T(-z - n + 1), n, pol, s);
           if (s)
              *s *= (n & 1 ? -1 : 1);
           return r;
        }
        int s1, s2;
        T r = boost::math::lgamma(T(z + n), &s1, pol) - boost::math::lgamma(z, &s2, pol);
        if(s)
           *s = s1 * s2;
        return r;
     }
  }

  template <class T, class Policy>
  inline T hypergeometric_1F1_generic_series(const T& a, const T& b, const T& z, const Policy& pol, long long& log_scaling, const char* function)
  {
     BOOST_MATH_STD_USING
     T sum(0), term(1), upper_limit(sqrt(boost::math::tools::max_value<T>())), diff;
     T lower_limit(1 / upper_limit);
     unsigned n = 0;
     long long log_scaling_factor = lltrunc(boost::math::tools::log_max_value<T>()) - 2;
     T scaling_factor = exp(T(log_scaling_factor));
     T term_m1 = 0;
     long long local_scaling = 0;
     //
     // When a is very small, then (a+n)/n => 1 faster than
     // z / (b+n) => 1, as a result the series starts off
     // converging, then at some unspecified time very gradually
     // starts to diverge, potentially resulting in some very large
     // values being missed.  As a result we need a check for small
     // a in the convergence criteria.  Note that this issue occurs
     // even when all the terms are positive.
     //
     bool small_a = fabs(a) < 0.25;

     unsigned summit_location = 0;
     bool have_minima = false;
     T sq = 4 * a * z + b * b - 2 * b * z + z * z;
     if (sq >= 0)
     {
        T t = (-sqrt(sq) - b + z) / 2;
        if (t > 1)  // Don't worry about a minima between 0 and 1.
           have_minima = true;
        t = (sqrt(sq) - b + z) / 2;
        if (t > 0)
           summit_location = itrunc(t);
     }

     if (summit_location > boost::math::policies::get_max_series_iterations<Policy>() / 4)
     {
        //
        // Skip forward to the location of the largest term in the series and
        // evaluate outwards from there:
        //
        int s1, s2;
        term = log_pochhammer(a, summit_location, pol, &s1) + summit_location * log(z) - log_pochhammer(b, summit_location, pol, &s2) - lgamma(T(summit_location + 1), pol);
        //std::cout << term << " " << log_pochhammer(boost::multiprecision::mpfr_float(a), summit_location, pol, &s1) + summit_location * log(boost::multiprecision::mpfr_float(z)) - log_pochhammer(boost::multiprecision::mpfr_float(b), summit_location, pol, &s2) - lgamma(boost::multiprecision::mpfr_float(summit_location + 1), pol) << std::endl;
        local_scaling = lltrunc(term);
        log_scaling += local_scaling;
        term = s1 * s2 * exp(term - local_scaling);
        //std::cout << term << " " << exp(log_pochhammer(boost::multiprecision::mpfr_float(a), summit_location, pol, &s1) + summit_location * log(boost::multiprecision::mpfr_float(z)) - log_pochhammer(boost::multiprecision::mpfr_float(b), summit_location, pol, &s2) - lgamma(boost::multiprecision::mpfr_float(summit_location + 1), pol) - local_scaling) << std::endl;
        n = summit_location;
     }
     else
        summit_location = 0;

     T saved_term = term;
     long long saved_scale = local_scaling;

     do
     {
        sum += term;
        //std::cout << n << " " << term * exp(boost::multiprecision::mpfr_float(local_scaling)) << " " << rising_factorial(boost::multiprecision::mpfr_float(a), n) * pow(boost::multiprecision::mpfr_float(z), n) / (rising_factorial(boost::multiprecision::mpfr_float(b), n) * factorial<boost::multiprecision::mpfr_float>(n)) << std::endl;
        if (fabs(sum) >= upper_limit)
        {
           sum /= scaling_factor;
           term /= scaling_factor;
           log_scaling += log_scaling_factor;
           local_scaling += log_scaling_factor;
        }
        if (fabs(sum) < lower_limit)
        {
           sum *= scaling_factor;
           term *= scaling_factor;
           log_scaling -= log_scaling_factor;
           local_scaling -= log_scaling_factor;
        }
        term_m1 = term;
        term *= (((a + n) / ((b + n) * (n + 1))) * z);
        if (n - summit_location > boost::math::policies::get_max_series_iterations<Policy>())
           return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
        ++n;
        diff = fabs(term / sum);
     } while ((diff > boost::math::policies::get_epsilon<T, Policy>()) || (fabs(term_m1) < fabs(term)) || (small_a && n < 10));

     //
     // See if we need to go backwards as well:
     //
     if (summit_location)
     {
        //
        // Backup state:
        //
        term = saved_term * exp(T(local_scaling - saved_scale));
        n = summit_location;
        term *= (b + (n - 1)) * n / ((a + (n - 1)) * z);
        --n;

        do
        {
           sum += term;
           //std::cout << n << " " << term * exp(boost::multiprecision::mpfr_float(local_scaling)) << " " << rising_factorial(boost::multiprecision::mpfr_float(a), n) * pow(boost::multiprecision::mpfr_float(z), n) / (rising_factorial(boost::multiprecision::mpfr_float(b), n) * factorial<boost::multiprecision::mpfr_float>(n)) << std::endl;
           if (n == 0)
              break;
           if (fabs(sum) >= upper_limit)
           {
              sum /= scaling_factor;
              term /= scaling_factor;
              log_scaling += log_scaling_factor;
              local_scaling += log_scaling_factor;
           }
           if (fabs(sum) < lower_limit)
           {
              sum *= scaling_factor;
              term *= scaling_factor;
              log_scaling -= log_scaling_factor;
              local_scaling -= log_scaling_factor;
           }
           term_m1 = term;
           term *= (b + (n - 1)) * n / ((a + (n - 1)) * z);
           if (summit_location - n > boost::math::policies::get_max_series_iterations<Policy>())
              return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
           --n;
           diff = fabs(term / sum);
        } while ((diff > boost::math::policies::get_epsilon<T, Policy>()) || (fabs(term_m1) < fabs(term)));
     }

     if (have_minima && n && summit_location)
     {
        //
        // There are a few terms starting at n == 0 which
        // haven't been accounted for yet...
        //
        unsigned backstop = n;
        n = 0;
        term = exp(T(-local_scaling));
        do
        {
           sum += term;
           //std::cout << n << " " << term << " " << sum << std::endl;
           if (fabs(sum) >= upper_limit)
           {
              sum /= scaling_factor;
              term /= scaling_factor;
              log_scaling += log_scaling_factor;
           }
           if (fabs(sum) < lower_limit)
           {
              sum *= scaling_factor;
              term *= scaling_factor;
              log_scaling -= log_scaling_factor;
           }
           //term_m1 = term;
           term *= (((a + n) / ((b + n) * (n + 1))) * z);
           if (n > boost::math::policies::get_max_series_iterations<Policy>())
              return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
           if (++n == backstop)
              break; // we've caught up with ourselves.
           diff = fabs(term / sum);
        } while ((diff > boost::math::policies::get_epsilon<T, Policy>())/* || (fabs(term_m1) < fabs(term))*/);
     }
     //std::cout << sum << std::endl;
     return sum;
  }

  template <class T, class Policy>
  inline T hypergeometric_1F2_generic_series(const T& a, const T& b1, const T& b2, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 1u, 2u> s(a, b1, b2, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T hypergeometric_2F0_generic_series(const T& a1, const T& a2, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 2u, 0u> s(a1, a2, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T hypergeometric_2F1_generic_series(const T& a1, const T& a2, const T& b, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 2u, 1u> s(a1, a2, b, z);
    return detail::sum_pFq_series(s, pol);
  }

  } } } // namespaces

#endif // BOOST_MATH_DETAIL_HYPERGEOMETRIC_SERIES_HPP


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    long long int n, C; cin >> n >> C;
    vector<pair<long long int, long long int>> v;
    for (int i = 0; i < n; i++) {
        long long int a, b, c; cin >> a >> b >> c;
        v.emplace_back(a - 1, c), v.emplace_back(b, -c);
    }
    sort(v.begin(), v.end());
    cpp_int ans = 0;
    long long int p = 0, t = 0;
    for (auto [x, y] : v) {
        if (x != t) ans += min(C, p) * (x - t), t = x;
        p += y;
    }
    cout << ans << endl;
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/math/stabilisedglls.hpp
    \brief Numerically stabilised general linear least squares
    \ingroup math
*/

#ifndef quantext_stabilised_glls_hpp
#define quantext_stabilised_glls_hpp

#include <ql/math/array.hpp>
#include <ql/math/comparison.hpp>
#include <ql/math/generallinearleastsquares.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/make_shared.hpp>
#include <boost/type_traits.hpp>

#include <vector>

namespace QuantExt {
using namespace QuantLib;
using namespace boost::accumulators;

//! Numerically stabilised general linear least squares
/*! The input data is lineaerly transformed before performing the linear least squares fit.
  The linear least squares fit on the transformed data is done using the
  GeneralLinearLeastSquares class.
    \ingroup math
 */

class StabilisedGLLS {
public:
    enum Method {
        None,      // No stabilisation
        MaxAbs,    // Divide x and y values by max of abs of values (per x coordinate, y)
        MeanStdDev // Subtract mean and divide by std dev (per x coordinate, y)
    };
    template <class xContainer, class yContainer, class vContainer>
    StabilisedGLLS(const xContainer& x, const yContainer& y, const vContainer& v, const Method method = MeanStdDev);

    const Array& transformedCoefficients() const { return glls_->coefficients(); }
    const Array& transformedResiduals() const { return glls_->residuals(); }
    const Array& transformedStandardErrors() const { return glls_->standardErrors(); }
    const Array& transformedError() const { return glls_->error(); }

    //! Transformation parameters (u => (u + shift) * multiplier for u = x, y)
    const Array& xMultiplier() const { return xMultiplier_; }
    const Array& xShift() const { return xShift_; }
    const Real yMultiplier() const { return yMultiplier_; }
    const Real yShift() const { return yShift_; }

    Size size() const { return glls_->residuals().size(); }
    Size dim() const { return glls_->coefficients().size(); }

    //! evaluate regression function in terms of original x, y
    template <class xType, class vContainer>
    Real eval(xType x, vContainer& v, typename boost::enable_if<typename boost::is_arithmetic<xType>::type>::type* = 0);

    //! evaluate regression function in terms of original x, y
    template <class xType, class vContainer>
    Real eval(xType x, vContainer& v,
              typename boost::disable_if<typename boost::is_arithmetic<xType>::type>::type* = 0);

protected:
    Array a_, err_, residuals_, standardErrors_, xMultiplier_, xShift_;
    Real yMultiplier_, yShift_;
    Method method_;
    boost::shared_ptr<GeneralLinearLeastSquares> glls_;

    template <class xContainer, class yContainer, class vContainer>
    void calculate(
        xContainer x, yContainer y, vContainer v,
        typename boost::enable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type* = 0);

    template <class xContainer, class yContainer, class vContainer>
    void calculate(
        xContainer x, yContainer y, vContainer v,
        typename boost::disable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type* = 0);
};

template <class xContainer, class yContainer, class vContainer>
inline StabilisedGLLS::StabilisedGLLS(const xContainer& x, const yContainer& y, const vContainer& v,
                                      const Method method)
    : a_(v.end() - v.begin(), 0.0), err_(v.end() - v.begin(), 0.0), residuals_(y.end() - y.begin()),
      standardErrors_(v.end() - v.begin()), method_(method) {
    calculate(x, y, v);
}

template <class xContainer, class yContainer, class vContainer>
void StabilisedGLLS::calculate(
    xContainer x, yContainer y, vContainer v,
    typename boost::enable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type*) {

    std::vector<Real> xData(x.end() - x.begin(), 0.0), yData(y.end() - y.begin(), 0.0);
    xMultiplier_ = Array(1, 1.0);
    xShift_ = Array(1, 0.0);
    yMultiplier_ = 1.0;
    yShift_ = 0.0;

    switch (method_) {
    case None:
        break;
    case MaxAbs: {
        Real mx = 0.0, my = 0.0;
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            mx = std::max(std::abs(x[i]), mx);
        }
        if (!close_enough(mx, 0.0))
            xMultiplier_[0] = 1.0 / mx;
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            my = std::max(std::abs(y[i]), my);
        }
        if (!close_enough(my, 0.0))
            yMultiplier_ = 1.0 / my;
        break;
    }
    case MeanStdDev: {
        accumulator_set<Real, stats<tag::mean, tag::variance> > acc;
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            acc(x[i]);
        }
        xShift_[0] = -mean(acc);
        Real tmp = variance(acc);
        if (!close_enough(tmp, 0.0))
            xMultiplier_[0] = 1.0 / std::sqrt(tmp);
        accumulator_set<Real, stats<tag::mean, tag::variance> > acc2;
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            acc2(y[i]);
        }
        yShift_ = -mean(acc2);
        Real tmp2 = variance(acc2);
        if (!close_enough(tmp2, 0.0))
            yMultiplier_ = 1.0 / std::sqrt(tmp2);
        break;
    }
    default:
        QL_FAIL("unknown stabilisation method");
    }

    for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
        xData[i] = (x[i] + xShift_[0]) * xMultiplier_[0];
    }
    for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
        yData[i] = (y[i] + yShift_) * yMultiplier_;
    }

    glls_ = boost::make_shared<GeneralLinearLeastSquares>(xData, yData, v);
}

template <class xContainer, class yContainer, class vContainer>
void StabilisedGLLS::calculate(
    xContainer x, yContainer y, vContainer v,
    typename boost::disable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type*) {

    QL_REQUIRE(x.end() - x.begin() > 0, "StabilisedGLLS::calculate(): x container is empty");
    QL_REQUIRE(x[0].end() - x[0].begin() > 0, "StabilisedGLLS:calculate(): x contains empty point(s)");

    std::vector<Array> xData(x.end() - x.begin(), Array(x[0].end() - x[0].begin(), 0.0));
    std::vector<Real> yData(y.end() - y.begin(), 0.0);
    xMultiplier_ = Array(x[0].end() - x[0].begin(), 1.0);
    xShift_ = Array(x[0].end() - x[0].begin(), 0.0);
    yMultiplier_ = 1.0;
    yShift_ = 0.0;

    switch (method_) {
    case None:
        break;
    case MaxAbs: {
        Array m(x[0].end() - x[0].begin(), 0.0);
        Real my = 0.0;
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            for (Size j = 0; j < m.size(); ++j) {
                m[j] = std::max(std::abs(x[i][j]), m[j]);
            }
        }
        for (Size j = 0; j < m.size(); ++j) {
            if (!close_enough(m[j], 0.0))
                xMultiplier_[j] = 1.0 / m[j];
        }
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            my = std::max(std::abs(y[i]), my);
        }
        if (!close_enough(my, 0.0))
            yMultiplier_ = 1.0 / my;
        break;
    }
    case MeanStdDev: {
        std::vector<accumulator_set<Real, stats<tag::mean, tag::variance> > > acc(x[0].end() - x[0].begin());
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            for (Size j = 0; j < acc.size(); ++j) {
                acc[j](x[i][j]);
            }
        }
        for (Size j = 0; j < acc.size(); ++j) {
            xShift_[j] = -mean(acc[j]);
            Real tmp = variance(acc[j]);
            if (!close_enough(tmp, 0.0))
                xMultiplier_[j] = 1.0 / std::sqrt(tmp);
        }
        accumulator_set<Real, stats<tag::mean, tag::variance> > acc2;
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            acc2(y[i]);
        }
        yShift_ = -mean(acc2);
        Real tmp2 = variance(acc2);
        if (!close_enough(tmp2, 0.0))
            yMultiplier_ = 1.0 / std::sqrt(tmp2);
        break;
    }
    default:
        QL_FAIL("unknown stabilisation method");
        break;
    }

    for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
        for (Size j = 0; j < xMultiplier_.size(); ++j) {
            xData[i][j] = (x[i][j] + xShift_[j]) * xMultiplier_[j];
        }
    }
    for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
        yData[i] = (y[i] + yShift_) * yMultiplier_;
    }

    glls_ = boost::make_shared<GeneralLinearLeastSquares>(xData, yData, v);
}

template <class xType, class vContainer>
Real StabilisedGLLS::eval(xType x, vContainer& v,
                          typename boost::enable_if<typename boost::is_arithmetic<xType>::type>::type*) {
    QL_REQUIRE(v.size() == glls_->dim(),
               "StabilisedGLLS::eval(): v size (" << v.size() << ") must be equal to dim (" << glls_->dim());
    Real tmp = 0.0;
    for (Size i = 0; i < v.size(); ++i) {
        tmp += glls_->coefficients()[i] * v[i]((x + xShift_[0]) * xMultiplier_[0]);
    }
    return tmp / yMultiplier_ - yShift_;
}

template <class xType, class vContainer>
Real StabilisedGLLS::eval(xType x, vContainer& v,
                          typename boost::disable_if<typename boost::is_arithmetic<xType>::type>::type*) {
    QL_REQUIRE(v.size() == glls_->dim(),
               "StabilisedGLLS::eval(): v size (" << v.size() << ") must be equal to dim (" << glls_->dim());
    Real tmp = 0.0;
    for (Size i = 0; i < v.size(); ++i) {
        xType xNew(x.end() - x.begin());
        for (Size j = 0; j < static_cast<Size>(x.end() - x.begin()); ++j) {
            xNew[j] = (x[j] + xShift_[j]) * xMultiplier_[j];
        }
        tmp += glls_->coefficients()[i] * v[i](xNew);
    }
    return tmp / yMultiplier_ - yShift_;
}

} // namespace QuantExt

#endif


#include "ECF_base.h"
#include "floatingpoint/FloatingPoint.h"
#include "AlgCuckooSearch.h"
#include <boost/random/normal_distribution.hpp>
#include <boost/random.hpp>
#include <ctime>
#include <cstdlib>
#include <vector>

CuckooSearch::CuckooSearch()
{
	name_ = "CuckooSearch";
	selBestOp = static_cast<SelectionOperatorP> (new SelBestOp);
}

void CuckooSearch::registerParameters(StateP state)
{
	registerParameter(state, "pa", (voidP) new double(0.75), ECF::DOUBLE);
}

bool CuckooSearch::initialize(StateP state)
{
	selBestOp->initialize(state);

	voidP pDiscovery = getParameterValue(state, "pa");
	pa = *((double*)pDiscovery.get());
	if (pa < 0 || pa > 1)
	{
		ECF_LOG_ERROR(state, "Error - pa must be in interval [0,1]");
		throw "";
	}

	// reading boudaries and problem dimension
	voidP lBound = state->getGenotypes()[0]->getParameterValue(state, "lbound");
	lbound = *((double*)lBound.get());
	voidP uBound = state->getGenotypes()[0]->getParameterValue(state, "ubound");
	ubound = *((double*)uBound.get());
	voidP sptr = state->getGenotypes()[0]->getParameterValue(state, "dimension");
	numDimension = *((uint*)sptr.get());

	// algorithm accepts a single FloatingPoint or Binary genotype
	// or a genotype derived from the abstract RealValueGenotype class
	GenotypeP activeGenotype = state->getGenotypes()[0];
	RealValueGenotypeP rv = boost::dynamic_pointer_cast<RealValueGenotype> (activeGenotype);
	if(!rv) {
		ECF_LOG_ERROR(state, "Error: Cuckoo Search algorithm accepts only a RealValueGenotype derived genotype! (FloatingPoint or Binary)");
		throw ("");
	}

	return true;
}

bool CuckooSearch::advanceGeneration(StateP state, DemeP deme)
{
	double sigma = 0.696574502;
	boost::mt19937 rng;
	boost::normal_distribution<> nd(0.0, 1.0);
	boost::variate_generator<boost::mt19937&,
	boost::normal_distribution<> > var_nor(rng, nd);

	IndividualP best = selBestOp->select(*deme);
	FloatingPointP bestFp = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (best->getGenotype(0));

	// cuckoos via Levy flights (by Mantegna's algorithm)
	// new individual is added to population only if it is better than original individual
	for (uint i = 0; i < deme->size(); i++) {
		IndividualP trial = (IndividualP)deme->at(i)->copy();
		FloatingPointP trialFp = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (trial->getGenotype(0));
		for (uint j = 0; j < numDimension; j++)	{
			double u = var_nor() * sigma;
			double v = var_nor();
			double step = u / pow(fabs(v), 2 / (double)3);
			double randn = var_nor();
			double diff = trialFp->realValue[j] - bestFp->realValue[j];
			double stepsize = 0.01 * step * diff;
			trialFp->realValue[j] = trialFp->realValue[j] + stepsize*randn;
			if (trialFp->realValue[j] > ubound)
				trialFp->realValue[j] = ubound;
			if (trialFp->realValue[j] < lbound)
				trialFp->realValue[j] = lbound;
		}
		evaluate(trial);
		if (trial->fitness->isBetterThan(deme->at(i)->fitness))
			replaceWith(deme->at(i), trial);
	}

	// copy all individuals
	std::vector<IndividualP> nest1;
	std::vector<IndividualP> nest2;
	for (uint i = 0; i < deme->size(); i++) {
		IndividualP indCp = (IndividualP)deme->at(i)->copy();
		nest1.push_back(indCp);
		nest2.push_back(indCp);
	}

	// replace some individuals/nests by constructing new nests
	// nest is replaced only if it is better than original
	random_shuffle(nest1.begin(), nest1.end());
	random_shuffle(nest2.begin(), nest2.end());
	double randNum = (double)rand() / RAND_MAX;
	for (uint i = 0; i < deme->size(); i++) {
		IndividualP trial = (IndividualP)deme->at(i)->copy();
		FloatingPointP trialFp1 = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (nest1.at(i)->getGenotype(0));
		FloatingPointP trialFp2 = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (nest2.at(i)->getGenotype(0));
		FloatingPointP trialFp = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (trial->getGenotype(0));

		for (uint j = 0; j < numDimension; j++) {
			if ((double)rand() / RAND_MAX < pa) {
				double stepsize = (trialFp1->realValue[j] - trialFp2->realValue[j])*randNum;
				trialFp->realValue[j] += stepsize;
				if (trialFp->realValue[j] > ubound)
					trialFp->realValue[j] = ubound;
				if (trialFp->realValue[j] < lbound)
					trialFp->realValue[j] = lbound;
			}
		}

		evaluate(trial);
		if (trial->fitness->isBetterThan(deme->at(i)->fitness))
			replaceWith(deme->at(i), trial);
	}
	return true;

}


// Author: Daisuke Kanaizumi
// Affiliation: Department of Applied Mathematics, Waseda University

#ifndef QGAMMA_HPP
#define QGAMMA_HPP

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/complex.hpp>
#include <kv/constants.hpp>
#include <limits>
#include <algorithm>
#include <kv/Heine.hpp>
#include <kv/Pochhammer.hpp>
#include <kv/qPochhammerVer2.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ub = boost::numeric::ublas;
namespace kv{
template <class T> interval<T> q_gamma(const interval<T>& z,const interval<T>& q){
   // q must be positive
   // verification program for q-gamma function
   interval<T>res;
   if(q<1 && q>0){
     if(pow(q,z)<1){
       res=pow(1-q,1-z)*Karpelevich(interval<T>(pow(q,z)),interval<T>(q));
     }
     else{
       res=Euler(interval<T>(q))*pow(1-q,1-z)/infinite_qPochhammer(interval<T>(pow(q,z)),interval<T>(q));
     }
     /*if(abs(res).upper()==std::numeric_limits<T>::infinity()){
       // Use asymptotic expansion
       // M Mansour (2006) An asymptotic expansion of the q-gamma function Γ q (x), Journal of Nonlinear Mathematical Physics, 13:4, 479-483, DOI: 10.2991/jnmp.2006.13.4.2
       res=sqrt(1+q)*pow(1-q,0.5-z)*Euler(interval<T>(q*q))*pow(1-q*q,0.5)/infinite_qPochhammer(interval<T>(pow(q*q,0.5)),interval<T>(q*q))*interval<T>(1.,(exp(pow(q,z)/(1-q-pow(q,z)))).upper());
       }*/

   }
   if(q>1){ // Moak q-gamma function
     if(pow(q,-z)<1){
       res=pow(q-1,1-z)*pow(q,z*(z-1)/2)*Karpelevich(interval<T>(pow(q,-z)),interval<T>(1/q));
     }
     else{
       res=Euler(interval<T>(1/q))*pow(q-1,1-z)*pow(q,z*(z-1)/2)/infinite_qPochhammer(interval<T>(pow(q,-z)),interval<T>(1/q));
     }
   }
   return res;
 }

  template <class T> complex<interval<T> >q_gamma(const complex<interval<T> >& z,const interval<T>& q){
    complex<interval<T> >res;
    if(q<1 && q>0){
      res=Euler(interval<T>(q))*pow(1-q,1-z)/infinite_qPochhammer(complex<interval<T> >(pow(q,z)),interval<T>(q));
    }
    if(q>1){
      res=Euler(interval<T>(1/q))*pow(q-1,1-z)*pow(q,z*(z-1)/2)/infinite_qPochhammer(complex<interval<T> >(pow(q,-z)),interval<T>(1/q));
    }
    return res;
 }
template <class T> ub::matrix<interval<T> >MExp(const ub::matrix<interval<T> >& A){
  int n,M;
  M=100;
  n=A.size1();//A:square matrix
  ub::matrix< interval<T> > B(n, n),res(n, n),sum(n, n),pro(n,n);
  interval<T> error,norm;
  T b;
  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      sum(i,j)=0.;
      if(i==j)pro(i,j)=1.;
      else pro(i,j)=0.;
    }
  }
  for(int N=0;N<=M;N++){
    sum+=(1./Pochhammer(interval<T>(1.,1.),N))*pro;
    pro=prod(pro,A);
  }
  norm=abs(A(0,0));
  for(int i1=0;i1<n;i1++){
    for(int j1=0;j1<n;j1++){
      if (A(i1,j1)>abs(norm)) norm=abs(A(i1,j1));
    }
  }

  error=exp(norm)*pow(norm,M+1)/Pochhammer(interval<T>(1.,1.),M+1);
  b=(abs(error)).upper();

  for(int k1=0;k1<n;k1++){
    for(int l1=0;l1<n;l1++){
      B(k1,l1).assign(-1.,1.);
      B(k1,l1)=b*B(k1,l1);
    }
  }
  res=sum+B;
  return res;
}
template <class T> ub::matrix<interval<T> >q_gamma(const ub::matrix<interval<T> >& A,const interval<T>& q){
  int n;
  n=A.size1();//A:square matrix
  interval<T>buf;
  ub::matrix< interval<T> > I(n, n),B(n, n),res(n, n),exp(n,n),inv(n,n),qp(n,n),qa(n,n),AA(n,n);

  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      if(i==j){
	I(i,j)=1.;
	inv(i,j)=1.;
      }
      else{
	I(i,j)=0.;
	inv(i,j)=0.;
      }
    }
  }
  AA=log(q)*A;
  qa=MExp(AA);
  qp=infinite_qPochhammer(ub::matrix<interval<T> > (qa), interval<T> (q));
//std::cout<<qp<<std::endl;
  for(int i1=0;i1<n;i1++){
    buf=1./qp(i1,i1);
    for(int j1=0;j1<n;j1++){
      qp(i1,j1)*=buf;
      inv(i1,j1)*=buf;
    }

    for(int j2=0;j2<n;j2++){
      if(i1!=j2){
	buf=qp(j2,i1);
	for(int k=0;k<n;k++){
	  qp(j2,k)-=qp(i1,k)*buf;
	  inv(j2,k)-=inv(i1,k)*buf;
	}
      }
    }
  }
  B=I-A;
  B=log(1-q)*B;
  std::cout<<B<<std::endl;
  exp=MExp(B);
  res=infinite_qPochhammer(interval<T>(q),interval<T>(q))
    *prod(inv,exp);
  return res;
}
  template <class T> complex<interval<T> >qgamma_Gauss_multi(const complex<interval<T> >& z,const interval<T>& q, int  p=3){
  if(q<1 && q>0){
    // M Mansour (2006) An asymptotic expansion of the q-gamma function Γ q (x), Journal of Nonlinear Mathematical Physics, 13:4, 479-483, DOI: 10.2991/jnmp.2006.13.4.2
    // G Gasper , M Rahman, Basic Hypergeometric Series 2nd Edition, Cambridge University Press, 2004.
    interval<T> pq,pro2;
    pq=(1-pow(q,p))/(1-q);//pq OK
    complex<interval<T> >res,pro1;
    pro1=1.;
    pro2=1.;
    for(int i=0;i<=p-1;i++){
      pro1=pro1*q_gamma(complex<interval<T> >((z+i)/p),interval<T>(pow(q,p)));
      // pro1 OK
    }
    for(int j=1;j<=p-1;j++){
      interval<T> jj;
      jj=j;
      pro2=pro2*q_gamma(interval<T> (jj/p),interval<T>(pow(q,p)));
    }

    res=pro1*pow(pq,z-1)/pro2;
    return res;
  }
  else{
    throw std::domain_error("implemented for 0<q<1");
  }
}
template <class T> complex<interval<T> >qgamma_Legendre(const complex<interval<T> >& z,const interval<T>& q){
  if(q<1 && q>0){
    interval<T>qg;
    qg=q_gamma(interval<T>(0.5),interval<T>(q*q));
    complex<interval<T> >res;

    res=q_gamma(complex<interval<T> >(z*0.5),interval<T>(q*q))
      *q_gamma(complex<interval<T> >((z+1)*0.5),interval<T>(q*q))
      *pow(1+q,z-1)/qg;

    return res;
  }
  else{
    throw std::domain_error("implemented for 0<q<1");
  }
}
  template <class T> complex<interval<T> >qgamma_shift(const complex<interval<T> >& z,const interval<T>& q, int p=3){
    // computing the q-gamma function with functional equation
    // G Gasper , M Rahman, Basic Hypergeometric Series 2nd Edition, Cambridge University Press, 2004.
    complex<interval<T> >pro;
    pro=1.;
    for(int i=1;i<=p;i++){
      pro=pro*(1-pow(q,z-i))/(1-q);
    }
    pro=pro*q_gamma(complex<interval<T> >(z-p),interval<T>(q));
  }
  template <class T> interval<T> q_digamma(const interval<T>& x,const interval<T>& q){
    // q,x must be positive
   // verification program for q-digamma function
   // Reference: Kamel Brahim (2009), Turan-Type Inequalities for some q-Special Functions
   // Journal of inequalities in pure and applied mathematics, Volume 10
   interval<T>res,sum,qq,first,ratio;
   T rad;
   int N=100;
   sum=0.;
   qq=1.;
    if (q>=1){
     throw std::domain_error("value of q must be under 1");
   }
   if (q<=0){
     throw std::domain_error("q must be positive");
   }
   if (x<=0){
     throw std::domain_error("implemented for positive x");
   }
   for(int n=1;n<=N-1;n++){
     qq=qq*q;
     sum=sum+pow(q,n*x)/(1-qq);
   }
   qq=qq*q;
   first=pow(q,N*x)/(1-qq);
   ratio=(1-qq)*pow(q,x)/(1-qq*q);
 if(abs(ratio)<1){
      rad=(first/(1-ratio)).upper();
      res=-log(1-q)+log(q)*(sum+rad*interval<T>(-1.,1.));
      return res;
    }
    else{
      std::cout<<"ratio is more than 1"<<std::endl;
    }
 }
 template <class T> interval<T> q_beta(const interval<T>& a,const interval<T>& b,const interval<T>& q){
   // q must be positive
   // verification program for q-beta function
   interval<T>res;
   res=q_gamma(interval<T>(a),interval<T>(q))*q_gamma(interval<T>(b),interval<T>(q))/q_gamma(interval<T>(a+b),interval<T>(q));
   return res;
 }
  template <class T> complex<interval<T> >q_beta(const complex<interval<T> >& a,const complex<interval<T> >& b,const interval<T>& q){
   // q must be positive
   // verification program for q-beta function
    complex<interval<T> >res;
    res=q_gamma(complex<interval<T> >(a),interval<T>(q))*q_gamma(complex<interval<T> >(b),interval<T>(q))/q_gamma(complex<interval<T> >(a+b),interval<T>(q));
    return res;
  }
 template <class T> interval<T> symmetric_q_gamma(const interval<T>& z,const interval<T>& q){
   // verification program for symmetric q-gamma function
   // reference
   // Brahim and Sidomou, On Some Symmetric q-Special Functions, 2013
   interval<T>res;
   res=pow(q,-(z-1)*(z-2)/2)*q_gamma(interval<T>(z),interval<T>(q*q));
   return res;
 }
 template <class T> interval<T> symmetric_q_beta(const interval<T>& a,const interval<T>& b,const interval<T>& q){
   // q,a,b must be positive
   // verification program for symmetric q-beta function
   // reference
   // Brahim and Sidomou, On Some Symmetric q-Special Functions, 2013
   interval<T>res;
   res=symmetric_q_gamma(interval<T>(a),interval<T>(q))*symmetric_q_gamma(interval<T>(b),interval<T>(q))/symmetric_q_gamma(interval<T>(a+b),interval<T>(q));
   return res;
 }
  template <class T> complex<interval<T> >symmetric_q_gamma(const complex<interval<T> >& z,const interval<T>& q){
   // verification program for symmetric q-gamma function
   // reference
   // Brahim and Sidomou, On Some Symmetric q-Special Functions, 2013
    complex<interval<T> >res;
    res=pow(q,-(z-1)*(z-2)/2)*q_gamma(complex<interval<T> >(z),interval<T>(q*q));
   return res;
 }
  template <class T> complex<interval<T> >incomplete_q_gamma(const complex<interval<T> >& z,const complex<interval<T> >& a,const interval<T>& q){
    // verification program for incomplete q-gamma function
    // expansion formula is used
    // reference
    // Ahmed Salem, A q-analogue of the exponential integral, 2013
    // warning: "a" should neither be negative integer nor zero
    complex<interval<T> >res,qq;
    qq=q;
    res=pow(z*(1-q),a)*q_gamma(complex<interval<T> >(a),interval<T>(q))
      *Heine(complex<interval<T> >(z*(1-q)),complex<interval<T> >(pow(q,a)),complex<interval<T> >(0.),interval<T>(q),complex<interval<T> >(qq));
    return res;
  }
 template <class T> complex<interval<T> >elliptic_gamma(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q){
    // verification program for elliptic gamma function
    // reference: M. A. Bershtein, A. I. Shechechkin (arXiv, 2016)
    // q-deformed Painlev\`e \tau function and q-deformed conformal blocks, Appendix A
    complex<interval<T> >res;
    /* if (abs(z)>=1){
      throw std::domain_error("implemented only for |z|<1");
      }*/
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    res=inf_elliptic_Pochhammer(complex<interval<T> >(p*q/z),complex<interval<T> >(p),complex<interval<T> >(q))
      /inf_elliptic_Pochhammer(complex<interval<T> >(z),complex<interval<T> >(p),complex<interval<T> >(q));
    return res;
  }
  template <class T> complex<interval<T> > modified_Jacobi_theta(const complex<interval<T> >&a,const interval<T> &q){
    complex<interval<T> >res;
    res=infinite_qPochhammer(complex<interval<T> >(a),interval<T>(q))
      *infinite_qPochhammer(complex<interval<T> >(q/a),interval<T>(q));
    return res;
  }
  template <class T> interval<T>  modified_Jacobi_theta(const interval<T> &a,const interval<T> &q){
    interval<T> res;
    res=qPVer2(interval<T> (a),interval<T>(q))
      *qPVer2(interval<T> (q/a),interval<T>(q));
    return res;
  }
  template <class T> complex<interval<T> >elliptic_gamma_tilde(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q){
    complex<interval<T> >res;
    if (abs(z)>=1){
      throw std::domain_error("implemented only for |z|<1");
    }
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    res=infinite_qPochhammer(interval<T> (q),interval<T> (q))/infinite_qPochhammer(interval<T> (p),interval<T> (p))
      *pow(modified_Jacobi_theta(q,p),1-log(z)/log(q))*elliptic_gamma(complex<interval<T> >(z),interval<T>(p),interval<T>(q));
    return res;
  }
  template <class T> complex<interval<T> >elliptic_gamma_shift(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q,int n){
    complex<interval<T> >res,pro1,pro2;
    interval<T> r;
    r=pow(q,n);
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    for(int i=1;i<=n-1;i++){
      pro1=pro1*elliptic_gamma_tilde(complex<interval<T> >(i/T(n)),interval<T>(p),interval<T>(r));
    }
    for(int j=0;j<=n-1;j++){
      pro2=pro2*elliptic_gamma_tilde(complex<interval<T> >((z+j)/T(n)),interval<T>(p),interval<T>(r));
    }
    res=pow(modified_Jacobi_theta(r,p)/modified_Jacobi_theta(q,p),z-1)*pro2/pro1;
    return res;
  }
 template <class T> complex<interval<T> >elliptic_gamma_shift2(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q,int n){
    complex<interval<T> >res,pro1,pro2;
    interval<T> r;
    r=pow(q,n);
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    for(int i=1;i<=n-1;i++){
      pro1=pro1*elliptic_gamma_tilde(complex<interval<T> >(T(i)/T(n)),interval<T>(p),interval<T>(r));
    }
    for(int j=0;j<=n-1;j++){
      pro2=pro2*elliptic_gamma_shift(complex<interval<T> >((z+T(j))/T(n)),interval<T>(p),interval<T>(r),int(n));
    }
    res=pow(modified_Jacobi_theta(r,p)/modified_Jacobi_theta(q,p),z-1)*pro2/pro1;
    return res;
  }
}

#endif


#define DEBUG 1
/**
 * File    : E.cpp
 * Author  : Kazune Takahashi
 * Created : 12/14/2019, 10:43:31 PM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
ll MOD{0};
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
istream &operator>>(istream &stream, Mint &a) { return stream >> a.x; }
ostream &operator<<(ostream &stream, const Mint &a) { return stream << a.x; }

// ----- main() -----

// I wrote this code referring to yataka1999-san's solution
// https://atcoder.jp/contests/agc035/submissions/6380939

Mint solve_even(ll N, ll K);
Mint solve_odd(ll N, ll K);

Mint solve_even(ll N, ll K)
{
  Mint ans{1};
  for (auto t = 0LL; t < 2; t++)
  {
    auto L{(N - t + 1) / 2};
    vector<Mint> to(L + 1), from(L + 1);
    to[0] = 1;
    for (auto i = 0LL; i < L; i++)
    {
      swap(to, from);
      to = vector<Mint>(L + 1);
      for (auto j = 0LL; j <= i; j++)
      {
        to[j + 1] += from[j];
        to[0] += from[j];
      }
      for (auto j = K / 2 + 1; j <= i + 1; j++)
      {
        to[j] = 0;
      }
    }
    Mint sum{0};
    for (auto j = 0LL; j <= L; j++)
    {
      sum += to[j];
    }
    ans *= sum;
  }
  return ans;
}

Mint solve_odd(ll N, ll K)
{
  vector<vector<vector<Mint>>> from(N + 1, vector<vector<Mint>>(N + 1, vector<Mint>(N + 1)));
  vector<vector<vector<Mint>>> to(N + 1, vector<vector<Mint>>(N + 1, vector<Mint>(N + 1)));
  to[0][0][N] = 1;
  for (auto i = 0LL; i < N; i++)
  {
    swap(to, from);
    to = vector<vector<vector<Mint>>>(N + 1, vector<vector<Mint>>(N + 1, vector<Mint>(N + 1)));
    auto even{i / 2 + 1};
    auto odd{(i + 1) / 2};
    for (auto j = 0LL; j <= even; j++)
    {
      for (auto k = 0LL; k <= odd; k++)
      {
        for (auto t = i; t <= N; t++)
        {
          if (from[j][k][t] == 0)
          {
            continue;
          }
          auto dst{(t % 2 == i % 2) ? N : t};
          to[0][j][dst] += from[j][k][t];
          if (t != i)
          {
            dst = j >= K / 2 + 1 ? min(t, i - 2 * k + K) : t;
            to[k + 1][j][dst] += from[j][k][t];
          }
        }
      }
    }
  }
  Mint ans{0};
  for (auto j = 0LL; j <= N; j++)
  {
    for (auto k = 0LL; k <= N; k++)
    {
      ans += to[j][k][N];
    }
  }
  return ans;
}

int main()
{
  ll N, K;
  cin >> N >> K >> MOD;
  if (K % 2 == 0)
  {
    cout << solve_even(N, K) << endl;
  }
  else
  {
    cout << solve_odd(N, K) << endl;
  }
}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <libv/lma/time/tictoc.hpp>

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

Vector llt(Matrix u, Vector x)
{
  for(int i = 0 ; i < u.rows() ; ++i)
  {
    for(int k = 0 ; k < i ; ++k)
      u(i,i) -= u(k,i) * u(k,i);

    assert(u(i,i)>0);
    u(i,i) = std::sqrt(u(i,i));

    for(int j = i + 1; j < u.cols() ; ++j)
    {
      for(int k = 0 ; k < i ; ++k)
        u(i,j) -= u(k,i) * u(k,j);
      u(i,j) /= u(i,i);
    }
  }

  for(int j = 0 ; j < x.size() ; ++j)
  {
    for(int i = 0 ; i < j ; ++i)
      x(j) -= u(i,j) * x(i);
    x(j) /= u(j,j);
  }

  for(int j = x.size() - 1 ; j >=0  ; --j)
  {
    for(int i = j+1 ; i < x.size() ; ++i)
      x(j) -= u(j,i) * x(i);
    x(j) /= u(j,j);
  }

  return x;
}

int main()
{
  size_t n = 10;
  Matrix a(n,n);
  Vector b(n),x(n);

  a = Matrix::Random(n,n);
  a = (a + a.transpose()).eval();

  for(size_t i = 0 ; i < n ; ++i)
  {
    if (a(i,i)<0) a(i,i) = - a(i,i);
    a(i,i) += 10.0;
    x(i) = i+1;
  }

  b = a * x;

  for(size_t i = 0 ; i < n ; ++i)
    for(size_t j = 0 ; j < n ; ++j)
    {
      if (j<i) a(i,j) = 0;
    }
  std::cout << a << std::endl;
  std::cout << " determinant " << a.determinant() << std::endl;
//   std::cout << "\nb = " << b.transpose() << std::endl;

  size_t N = 1000000;
//   size_t N = 1;

  utils::Tic<true> tic("llt");
  for(size_t i = 0 ; i < N; ++i)
    x = llt(a,b);
  tic.disp();
  std::cout << "\nX = " << x.transpose() << std::endl;

  Vector X;

  utils::Tic<true> tic2("LLT");
  for(size_t i = 0 ; i < N ; ++i)
  {
    Eigen::LLT<Matrix,Eigen::Upper> LLT(a);
    X = LLT.solve(b);
  }
  tic2.disp();
//   std::cout << "\nX = " << x.transpose() << std::endl;

//   std::cout << "A * x = " << (a * x).transpose() << std::endl;

//   Matrix L = LLT.matrixL();
//   Matrix U = LLT.matrixU();
//   std::cout << "\nL =\n" << L << std::endl;
//   std::cout << "\nU =\n" << U << std::endl;
  std::cout << "\nX = " << X.transpose() << std::endl;
//   std::cout << " CHECK " << (a*X - b).transpose() << std::endl;

  return x == X;
}


/*
 * KMeans.cpp
 *
 *  Created on: Mar 12, 2016
 *      Author: zxi
 */

#include "Clustering.h"
#include <Eigen/Eigenvalues>

#include <iostream>
#include <algorithm>
#include <cfloat>
#include <ctime>

namespace masc {
namespace clustering {

#define TIMING(code, verbosity, output) \
  { \
    auto s = clock(); \
    (code); \
    auto e = clock(); \
    if (this->m_verbosity >= (verbosity) ) \
      std::cout<< (output) << " takes " << (e-s)*1.0 / CLOCKS_PER_SEC << " s" << std::endl; \
  } \

template<class T>
VectorXi ClusteringBase<T>::labelsInertia(const MatrixXd& X,
    const MatrixXd& centriods, VectorXd* distances, double* inertia) {

  const int n_samples = X.rows();
  const int n_k = centriods.rows();

  *distances = VectorXd(n_samples);
  *inertia = 0.0;

  VectorXi labels(n_samples);

  for (int i = 0; i < n_samples; ++i) {
    double min_dist = FLT_MAX;
    int label = -1;
    for (int j = 0; j < n_k; ++j) {
      double dist = (X.row(i) - centriods.row(j)).norm();
      if (dist < min_dist) {
        label = j;
        min_dist = dist;
      }
    }

    *inertia += min_dist;
    labels(i) = label;
    (*distances)(i) = min_dist;
  }

  return labels;
}

template<class T>
KMeansBase<T>::KMeansBase(int n_clusters, int n_init, int max_iter, double tol) :
    m_n_clusters(n_clusters), m_n_init(n_init), m_max_iter(max_iter), m_tol(tol) {
//TODO
}

template<class T>
KMeansBase<T>::~KMeansBase() {
  // nothing to do here
}

template<class T>
MatrixXd KMeansBase<T>::initCentroids(const MatrixXd& X) {
  const int n_samples = X.rows();
  const int n_features = X.cols();

  MatrixXd centroids(m_n_clusters, n_features);

  MatrixXd best_centroids;
  VectorXd distences;
  double best_score = FLT_MAX;

  std::uniform_int_distribution<int> dist(0, n_samples);

  for (int r = 0; r < m_n_init; ++r) {

    for (int i = 0; i < m_n_clusters; ++i) {
      int sample = dist(this->m_rd);
      centroids.row(i) = X.row(sample);
    }

    double score;
    this->labelsInertia(X, centroids, &distences, &score);

    if (score < best_score) {
      best_score = score;
      best_centroids = centroids;
    }

  }

  return best_centroids;
}

template<class T>
MatrixXd KMeansBase<T>::updateCenters(const MatrixXd& X,
    const VectorXi& labels) {
  const int n_samples = X.rows();
  const int n_features = X.cols();

  MatrixXd centroids = Eigen::MatrixXd::Zero(m_n_clusters, n_features);
  VectorXi count = Eigen::VectorXi::Zero(m_n_clusters);

  for (int i = 0; i < n_samples; ++i) {
    centroids.row(labels[i]) += X.row(i);
    count(labels[i]) += 1;
  }

  for (int i = 0; i < m_n_clusters; ++i) {
    if (count(i) > 0)
      centroids.row(i) /= count(i);
  }

  return centroids;
}

template<class T>
T& KMeansBase<T>::fit(const MatrixXd& X) {
  auto centers = this->initCentroids(X);

  if (this->m_verbosity >= 2)
    std::cout << "KMeansBase::fit - init centers = " << std::endl << centers << std::endl;

  VectorXd distances;

  double best_inertia = FLT_MAX / 2.0;
  VectorXi best_labels;
  MatrixXd best_centers;

  for (int i = 0; i < m_max_iter; ++i) {
    auto old_centers = centers;
    double inertia = 0.0;
    auto labels = this->labelsInertia(X, old_centers, &distances, &inertia);
    centers = this->updateCenters(X, labels);

    if (this->m_verbosity >= 1)
      std::cout << "KMeansBase::fit - iter " << i << " inertia = " << inertia << std::endl;

    if (inertia < best_inertia) {
      best_inertia = inertia;
      best_labels = labels;
      best_centers = centers;
    }

    auto shift = (old_centers - centers).norm();

    if (shift * shift < this->m_tol) {
      if (this->m_verbosity >= 1)
        std::cout << "KMeansBase::fit - Converged at iteration " << i << std::endl;
      break;
    }
  }

  this->m_cluster_centers = best_centers;
  this->m_labels = this->labelsInertia(X, best_centers, &distances,
      &best_inertia);
  this->m_inertia = best_inertia;

  return static_cast<T&>(*this);
}

///////////////////////////////////////////////////////////////////////////////
// KMeans
///////////////////////////////////////////////////////////////////////////////
KMeans::KMeans(int n_clusters, int n_init, int max_iter, double tol) :
    KMeansBase<KMeans>(n_clusters, n_init, max_iter, tol) {

}

KMeans::~KMeans() {
  //TODO
}

///////////////////////////////////////////////////////////////////////////////
// SpectralClustering
///////////////////////////////////////////////////////////////////////////////
SpectralClustering::SpectralClustering(int n_clusters, double gamma,
    AffinityType affinity_type) :
    KMeansBase<SpectralClustering>(n_clusters), m_affinity_type(affinity_type), m_gamma(
        gamma) {
//TODO
}

SpectralClustering::~SpectralClustering() {
  //TODO
}

MatrixXd SpectralClustering::constructAffinityMatrix(const MatrixXd& X,
    AffinityType affinity_type) {

  const int n_samples = X.rows();

  MatrixXd W(n_samples, n_samples);

  switch (affinity_type) {
  case AffinityType::RBF:

    // compute pairwise distance
    for (int i = 0; i < n_samples; ++i) {
      W(i, i) = 1.0;
      for (int j = i + 1; j < n_samples; ++j) {
        double dist = (X.row(i) - X.row(j)).norm();
        W(i, j) = W(j, i) = exp(-(this->m_gamma) * dist * dist);
      }
    }

    break;
  default:
    std::cerr << "Unsupported affinity type " << (int) affinity_type
        << std::endl;
    break;
  }

  return W;
}

SpectralClustering& SpectralClustering::fit(const MatrixXd& X) {
  const int n_samples = X.rows();
  const int n_features = X.cols();

  // Affinity matrix
  MatrixXd W;
  TIMING(W = this->constructAffinityMatrix(X, this->m_affinity_type), 1,
      "SpectralClustering::fit - construct affinity matrix");

  // Degree matrix
  MatrixXd D = W.rowwise().sum().asDiagonal();

  // Laplacian matrix
  MatrixXd L = D - W;

  Eigen::SelfAdjointEigenSolver<MatrixXd> es(n_samples);
  TIMING(es.compute(L), 1, "SpectralClustering::fit - compute eigen vectors");

  const MatrixXd& evs = es.eigenvectors();

  // embedded matrix (n_samples * k)
  MatrixXd embed(n_samples, m_n_clusters);

  for (int i = 0; i < m_n_clusters; ++i)
    embed.col(i) = evs.col(i);

  // run kmeans clustering on embed
  TIMING(KMeansBase<SpectralClustering>::fit(embed), 1,
      "SpectralClustering::fit - kmeans clustering");

  return *this;
}

} /* namespace clustering */
} /* namespace masc */


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
Solutions for testdata were generated with Scilab line:

M=fscanfMat('nsm1.example');e=spec(M);e=gsort(e);rr=real(e);ii=imag(e);e=cat(1, rr, ii); s=strcat(string(e), ' ');write('tmp', s);
*/

#ifndef NDEBUG
  #define NDEBUG
#endif

//#define VIENNACL_DEBUG_ALL
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <vector>

#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/qr-method.hpp"

#include <examples/benchmarks/benchmark-utils.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace ublas = boost::numeric::ublas;

typedef float ScalarType;

const ScalarType EPS = 0.0001f;

void read_matrix_size(std::fstream& f, std::size_t& sz)
{
    if(!f.is_open())
    {
        throw std::invalid_argument("File is not opened");
    }

    f >> sz;
}

template <typename MatrixLayout>
void read_matrix_body(std::fstream& f, viennacl::matrix<ScalarType, MatrixLayout>& A)
{
    if(!f.is_open())
    {
        throw std::invalid_argument("File is not opened");
    }

    boost::numeric::ublas::matrix<ScalarType> h_A(A.size1(), A.size2());

    for(std::size_t i = 0; i < h_A.size1(); i++) {
        for(std::size_t j = 0; j < h_A.size2(); j++) {
            ScalarType val = 0.0;
            f >> val;
            h_A(i, j) = val;
        }
    }

    viennacl::copy(h_A, A);
}

void read_vector_body(std::fstream& f, std::vector<ScalarType>& v) {
    if(!f.is_open())
        throw std::invalid_argument("File is not opened");

    for(std::size_t i = 0; i < v.size(); i++)
    {
            ScalarType val = 0.0;
            f >> val;
            v[i] = val;
    }
}

template <typename MatrixLayout>
bool check_tridiag(viennacl::matrix<ScalarType, MatrixLayout>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);

    for (unsigned int i = 0; i < A.size1(); i++) {
        for (unsigned int j = 0; j < A.size2(); j++) {
            if ((std::abs(A(i, j)) > EPS) && ((i - 1) != j) && (i != j) && ((i + 1) != j))
            {
                // std::cout << "Failed at " << i << " " << j << " " << A(i, j) << "\n";
                return false;
            }
        }
    }
    return true;
}

template <typename MatrixLayout>
bool check_hessenberg(viennacl::matrix<ScalarType, MatrixLayout>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);

    for (std::size_t i = 0; i < A.size1(); i++) {
        for (std::size_t j = 0; j < A.size2(); j++) {
            if ((std::abs(A(i, j)) > EPS) && (i > (j + 1)))
            {
                // std::cout << "Failed at " << i << " " << j << " " << A(i, j) << "\n";
                return false;
            }
        }
    }
    return true;
}

ScalarType matrix_compare(ublas::matrix<ScalarType>& res,
                            ublas::matrix<ScalarType>& ref)
{
    ScalarType diff = 0.0;
    ScalarType mx = 0.0;

    for(std::size_t i = 0; i < res.size1(); i++)
    {
        for(std::size_t j = 0; j < res.size2(); j++)
        {
            diff = std::max(diff, std::abs(res(i, j) - ref(i, j)));
            mx = std::max(mx, res(i, j));
        }
    }

    return diff / mx;
}

ScalarType vector_compare(std::vector<ScalarType> & res,
                          std::vector<ScalarType> & ref)
{
    std::sort(ref.begin(), ref.end());
    std::sort(res.begin(), res.end());

    ScalarType diff = 0.0;
    ScalarType mx = 0.0;
    for(size_t i = 0; i < res.size(); i++)
    {
        diff = std::max(diff, std::abs(res[i] - ref[i]));
        mx = std::max(mx, res[i]);
    }

    return diff / mx;
}

template <typename MatrixLayout>
void matrix_print(viennacl::matrix<ScalarType, MatrixLayout>& A)
{
    for (unsigned int i = 0; i < A.size1(); i++) {
        for (unsigned int j = 0; j < A.size2(); j++)
           std::cout << std::fixed << A(i, j) << "\t";
        std::cout << "\n";
    }
}

template <typename MatrixLayout>
void test_eigen(const std::string& fn, bool is_symm)
{
    std::cout << "Reading..." << "\n";
    std::size_t sz;
    // read file
    std::fstream f(fn.c_str(), std::fstream::in);
    //read size of input matrix
    read_matrix_size(f, sz);

    if (viennacl::is_row_major<MatrixLayout>::value)
      std::cout << "Testing row-major matrix of size " << sz << "-by-" << sz << std::endl;
    else
      std::cout << "Testing column-major matrix of size " << sz << "-by-" << sz << std::endl;

    viennacl::matrix<ScalarType> A_input(sz, sz), A_ref(sz, sz), Q(sz, sz);
    // reference vector with reference values from file
    std::vector<ScalarType> eigen_ref_re(sz);
    // calculated real eigenvalues
    std::vector<ScalarType> eigen_re(sz);
    // calculated im. eigenvalues
    std::vector<ScalarType> eigen_im(sz);

    // read input matrix from file
    read_matrix_body(f, A_input);
    // read reference eigenvalues from file
    read_vector_body(f, eigen_ref_re);

    f.close();

    A_ref = A_input;

    std::cout << "Calculation..." << "\n";

    Timer timer;
    timer.start();
    // Start the calculation
    if(is_symm)
        viennacl::linalg::qr_method_sym(A_input, Q, eigen_re);
    else
        viennacl::linalg::qr_method_nsm(A_input, Q, eigen_re, eigen_im);
/*

    std::cout << "\n\n Matrix A: \n\n";
    matrix_print(A_input);
    std::cout << "\n\n";

    std::cout << "\n\n Matrix Q: \n\n";
    matrix_print(Q);
    std::cout << "\n\n";
*/

    double time_spend = timer.get();

    std::cout << "Verification..." << "\n";

    bool is_hessenberg = check_hessenberg(A_input);
    bool is_tridiag = check_tridiag(A_input);

    ublas::matrix<ScalarType> A_ref_ublas(sz, sz), A_input_ublas(sz, sz), Q_ublas(sz, sz), result1(sz, sz), result2(sz, sz);
    viennacl::copy(A_ref, A_ref_ublas);
    viennacl::copy(A_input, A_input_ublas);
    viennacl::copy(Q, Q_ublas);

    // compute result1 = ublas::prod(Q_ublas, A_input_ublas);   (terribly slow when using ublas directly)
    for (std::size_t i=0; i<result1.size1(); ++i)
      for (std::size_t j=0; j<result1.size2(); ++j)
      {
        ScalarType value = 0;
        for (std::size_t k=0; k<Q_ublas.size2(); ++k)
          value += Q_ublas(i, k) * A_input_ublas(k, j);
        result1(i,j) = value;
      }
    // compute result2 = ublas::prod(A_ref_ublas, Q_ublas);   (terribly slow when using ublas directly)
    for (std::size_t i=0; i<result2.size1(); ++i)
      for (std::size_t j=0; j<result2.size2(); ++j)
      {
        ScalarType value = 0;
        for (std::size_t k=0; k<A_ref_ublas.size2(); ++k)
          value += A_ref_ublas(i, k) * Q_ublas(k, j);
        result2(i,j) = value;
      }

    ScalarType prods_diff = matrix_compare(result1, result2);
    ScalarType eigen_diff = vector_compare(eigen_re, eigen_ref_re);

    bool is_ok = is_hessenberg;

    if(is_symm)
        is_ok = is_ok && is_tridiag;

    is_ok = is_ok && (eigen_diff < EPS);
    is_ok = is_ok && (prods_diff < EPS);

    // std::cout << A_ref << "\n";
    // std::cout << A_input << "\n";
    // std::cout << Q << "\n";
    // std::cout << eigen_re << "\n";
    // std::cout << eigen_im << "\n";
    // std::cout << eigen_ref_re << "\n";
    // std::cout << eigen_ref_im << "\n";

    // std::cout << result1 << "\n";
    // std::cout << result2 << "\n";
    // std::cout << eigen_ref << "\n";
    // std::cout << eigen << "\n";

    printf("%6s [%dx%d] %40s time = %.4f\n", is_ok?"[[OK]]":"[FAIL]", (int)A_ref.size1(), (int)A_ref.size2(), fn.c_str(), time_spend);
    printf("tridiagonal = %d, hessenberg = %d prod-diff = %f eigen-diff = %f\n", is_tridiag, is_hessenberg, prods_diff, eigen_diff);
    std::cout << std::endl << std::endl;

    if (!is_ok)
      exit(EXIT_FAILURE);

}

int main()
{

  test_eigen<viennacl::row_major>("../../examples/testdata/eigen/symm5.example", true);
 // test_eigen<viennacl::row_major>("../../examples/testdata/eigen/symm3.example", true);  // Computation of this matrix takes very long

  test_eigen<viennacl::column_major>("../../examples/testdata/eigen/symm5.example", true);
//  test_eigen<viennacl::column_major>("../../examples/testdata/eigen/symm3.example", true);

#ifdef VIENNACL_WITH_OPENCL
  test_eigen<viennacl::row_major>("../../examples/testdata/eigen/nsm2.example", false);
#endif
  //test_eigen<viennacl::row_major>("../../examples/testdata/eigen/nsm2.example", false);
  //test_eigen("../../examples/testdata/eigen/nsm3.example", false);
  //test_eigen("../../examples/testdata/eigen/nsm4.example", false); //Note: This test suffers from round-off errors in single precision, hence disabled

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

  return EXIT_SUCCESS;
}


#ifndef __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_DISTRIBUTION_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_DISTRIBUTION_IMPL_HPP__

#include "asymmetric_distribution.hpp"

#include "const_slice.hpp"
#include "slice.hpp"

#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <class Dist, class D, class W, class T>
  AsymmetricDistribution<Dist,D,W,T>::AsymmetricDistribution(T p, T mu, T eps,
      T tol):
    fixed_mu_(false),
    fixed_p_(false),
    mu_(mu),
    eps_(eps),
    tol_(tol) {
      set_mu(mu);
      set_p(p);
    }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::init() {
    set_p(p_);
  }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::set_p(T p) {
    assert(p > 0);
    assert(p < 1);
    p_ = p;
    static_cast<Dist*>(this)->updated_p();
  }

  template <class Dist, class D, class W, class T>
  template <class RNG>
  void AsymmetricDistribution<Dist,D,W,T>::sample(MA::Array<D>& samples,
      size_t n_samples, RNG& rng) const {
    MA::Size::SizeType size(2);
    size[0] = n_samples;
    size[1] = 1;
    samples.resize(size);

    boost::random::uniform_real_distribution<T> dist(0, 1);
    auto gamma_plus = static_cast<Dist const*>(this)->create_gamma_plus();
    auto gamma_minus = static_cast<Dist const*>(this)->create_gamma_minus();

    D* ptr = samples.get_pointer();

    for (size_t j = 0; j < n_samples; j++) {
      if (dist(rng) < p_)
        ptr[j] = mu_ - gamma_minus(rng);
      else
        ptr[j] = mu_ + gamma_plus(rng);
    }
  }

  template <class Dist, class D, class W, class T>
  T AsymmetricDistribution<Dist,D,W,T>::log_likelihood(
      MA::ConstArray<D> const& data, MA::ConstArray<W> const& weight) const {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T ll = 0;
    T const_likelihood = static_cast<Dist const*>(this)->constant_likelihood() +
      (std::log(p_) + std::log(1-p_))/2;

    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      T local_likelihood = const_likelihood;
      if (s < mu_)
        local_likelihood += static_cast<Dist const*>(this)->negative_ll(s, mu_);
      else
        local_likelihood += static_cast<Dist const*>(this)->positive_ll(s, mu_);
      ll += w * local_likelihood;
    }

    return ll;
  }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::MLE(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes) {
    check_data_and_weight(data, weight);
    assert(data.size()[0] == indexes.size());

    static_cast<Dist*>(this)->init_MLE(data, weight, indexes);

    if (fixed_p_)
      static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
    else {
      T step  = eps_;

      T center_p = p_, left_p = center_p - step, right_p = center_p + step;

      set_p(center_p);
      static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
      T center_ll = log_likelihood(data, weight), left_ll, right_ll;

      if (left_p > 0) {
        set_p(left_p);
        static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
        left_ll = log_likelihood(data, weight);
      }
      else
        left_ll = -INFINITY;

      if (right_p < 1) {
        set_p(right_p);
        static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
        right_ll = log_likelihood(data, weight);
      }
        right_ll = -INFINITY;

      while (1) {
        if (std::abs(center_ll - left_ll)  < tol_ &&
            std::abs(right_ll  - left_ll)  < tol_ &&
            std::abs(center_ll - right_ll) < tol_)
          break;

        if (center_ll > left_ll && center_ll > right_ll) {
          if (step < tol_) {
            set_p(center_p);
            break;
          }

          step *= 1e-1;
          left_p = center_p - step;
          right_p = center_p + step;

          if (left_p > 0) {
            set_p(left_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            left_ll = log_likelihood(data, weight);
          }
          else
            left_ll = -INFINITY;

          if (right_p < 1) {
            set_p(right_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            right_ll = log_likelihood(data, weight);
          }
          else
            right_ll = -INFINITY;
        }
        else if (left_ll > right_ll) {
          right_p = center_p;
          center_p = left_p;
          right_ll = center_ll;
          center_ll = left_ll;

          left_p = center_p - step;

          if (left_p > 0) {
            set_p(left_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            left_ll = log_likelihood(data, weight);
          }
          else
            left_ll = -INFINITY;
        }
        else {
          left_p = center_p;
          center_p = right_p;
          left_ll = center_ll;
          center_ll = right_ll;

          right_p = center_p + step;

          if (right_p < 1) {
            set_p(right_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            right_ll = log_likelihood(data, weight);
          }
          else
            right_ll = -INFINITY;
        }
      }

      static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
    }

    static_cast<Dist*>(this)->end_MLE();
  }

  template <class Dist, class D, class W, class T>
  T AsymmetricDistribution<Dist,D,W,T>::fix_step(T p, T step) const {
    while (p_ + step <= 0 || p_ + step >= 1)
      step *= 0.99;
    return step;
  }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::check_data_and_weight(
      MA::ConstArray<D> const& data, MA::ConstArray<W> const& weight) const {
    assert(data.size().size() == 2);
    assert(data.size()[0] > 0);
    assert(data.size()[1] == 1);
    assert(weight.size().size() == 1);
    assert(weight.size()[0] == data.size()[0]);
  }
};

#endif


// Copyright (c) 2011 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include <Eigen/Geometry>

#include "libmv/multiview/affine.h"
#include "libmv/multiview/similarity.h"
#include "libmv/multiview/similarity_parameterization.h"

namespace libmv {

// Parametrization
// s*cos -s*sin  tx
// s*sin  s*cos  ty
// 0      0      1

// It gives the following system A x = B :
// |-Y1  Y1 1 0 | | s*sin |   | X2 |
// | X1  Y1 0 1 | | s*cos |   | Y2 |
//                | tx    | =
//                | ty    |
//
bool Similarity2DFromCorrespondencesLinear(const Mat &x1, const Mat &x2,
                                           Mat3 *M,
                                           double expected_precision) {
  assert(2 == x1.rows());
  assert(2 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());

  const int n = x1.cols();
  Mat A = Mat::Zero(2*n, 4);
  Mat b = Mat::Zero(2*n, 1);
  for (int i = 0; i < n; ++i) {
    const int j= i * 2;
    A(j,0) = -x1(1,i);
    A(j,1) =  x1(0,i);
    A(j,2) =  1.0;
    //A(j,3) =  0.0;

    A(j+1,0) = x1(0,i);
    A(j+1,1) = x1(1,i);
    //A(j+1,2) = 0.0;
    A(j+1,3) = 1.0;

    b(j,0)   = x2(0,i);
    b(j+1,0) = x2(1,i);
  }
  // Solve Ax=B
  Vec x = A.fullPivLu().solve(b);
  if ((A * x).isApprox(b, expected_precision))  {
    Similarity2DSCParameterization<double>::To(x, M);
    return true;
  } else {
    return false;
  }
}

bool Similarity3DFromCorrespondencesLinear(const Mat &x1,
                                          const Mat &x2,
                                          Mat4 *H,
                                          double expected_precision) {
   // TODO(julien) Compare to *H = umeyama (x1, x2, true);
   // and keep the best one (quality&speed)
  if (Affine3DFromCorrespondencesLinear(x1, x2, H, expected_precision)) {
    // Ensures that R is orthogonal (using SDV decomposition)
    Eigen::JacobiSVD<Mat> svd(H->block<3,3>(0, 0), Eigen::ComputeThinU |
                                                   Eigen::ComputeThinV);
    double scale = svd.singularValues()(0);
    Mat3 sI3 = scale * Mat3::Identity();
    H->block<3,3>(0, 0) = svd.matrixU() * sI3 * svd.matrixV().transpose();
    if (H->block<3,3>(0, 0).determinant() < 0)
      H->block<3,3>(0, 0) = -H->block<3,3>(0, 0);
    return true;
  }
  return false;
}

bool ExtractSimilarity2DCoefficients(const Mat3 &M,
                                     Vec2   *tr,
                                     double *angle,
                                     double *scale) {
  Vec4 p;
  Similarity2DSAParameterization<double>::From(M, &p);
  *scale = p(0);
  *angle = p(1);
  *tr << p(2), p(3);
  return true;
}
} // namespace libmv


/**
 * See https://gamedevelopment.tutsplus.com/tutorials/collision-detection-using-the-separating-axis-theorem--gamedev-169
 */

#include <cassert>
#include <limits>
#include <algorithm>
#include <set>

#include <boost/variant/static_visitor.hpp>

#include <common/Geometry.h>
#include <common/geometry/Circle.h>

#include <common/geometry/collision.h>

using namespace std;

namespace geometry {

bool collision(Circle const &c1, Circle const &c2) {
	return distance(c1.center, c2.center) <= (c1.radius + c2.radius);
}

MinMax minmaxProjection(Polygon const &polygon, Vector axis) {
	MinMax minmax = {numeric_limits<Scalar>::max(), numeric_limits<Scalar>::lowest()};
	for(Point const & point : polygon) {
		auto d = (point - Point{0,0}) * axis;
		minmax.min = min(minmax.min, d);
		minmax.max = max(minmax.max, d);
	}
	return minmax;
}

bool gapAlongAxis(Vector axis, Polygon const &p1, Polygon const &p2) {
	MinMax r1 = minmaxProjection(p1, axis);
	MinMax r2 = minmaxProjection(p2, axis);

	return (r1.max < r2.min) || (r2.max < r1.min);
}

void getNormals(std::set<Vector> &normals, Polygon const &polygon) {
	if (polygon.empty())
		return;

	Point prev = polygon.back();
	for(auto const & curr : polygon) {
		normals.insert(leftNormal(unit(curr - prev)));
		prev = curr;
	}
}

bool collision(Polygon const &p1, Polygon const &p2) {
	std::set<Vector> normals;
	getNormals(normals, p1);
	getNormals(normals, p2);

	for (auto const &n : normals) {
		if (gapAlongAxis(n, p1, p2))
			return false;
	}

	return true;
}

// TODO na zacatek nekam pridam kruh

// Does not detect full containing
bool collision(Polygon const &polygon, Circle const &circle) {

	// Near vertex
	for(Point const & vertex : polygon) {
		if (distance(vertex, circle.center) <= circle.radius)
			return true;
	}

	// Something is wrong down there
	assert(polygon.size() >= 3);
	Point const * lineFrom = &polygon.back();
	for (Point const & lineTo : polygon ) {

		Scalar const t = projection(circle.center, *lineFrom, lineTo);
		if (t >= 0 && t <= 1) {
			Point const projectedCenter = *lineFrom + t*(lineTo - *lineFrom);
			if (size(projectedCenter - circle.center) <= circle.radius)
					return true;
		}
		lineFrom = &lineTo;
	}

	return false;
}

bool collision(Circle const &circle, Polygon const &polygon) {
	return collision(polygon, circle);
}

namespace {

template < typename Object1 >
class CollisionVisitor1 : public boost::static_visitor<bool>{
	Object1 const & object1;
public:
	explicit CollisionVisitor1(Object1 const & object1) :
		object1(object1) {
	}

	template < typename Object2 >
	bool operator()(Object2 const & object2) {
		return collision(object1, object2);
	}
};

}

bool collision(Polygon const &polygon, Object2D const &object) {
	CollisionVisitor1<Polygon> visitor{polygon};
	return boost::apply_visitor(visitor, object);
}

bool collision(Circle const &circle, Object2D const &object) {
	CollisionVisitor1<Circle> visitor{circle};
	return boost::apply_visitor(visitor, object);
}

namespace {
class CollisionVisitor2 : public boost::static_visitor<bool>{
	Object2D const & object1;
public:
	explicit CollisionVisitor2(Object2D const & object1) :
		object1(object1) {
	}

	template < typename Object2 >
	bool operator()(Object2 const & object2) {
		CollisionVisitor1<Object2> visitor{object2};
		return boost::apply_visitor(visitor, object1);
	}
};
}

bool collision(Object2D const &object1, Object2D const &object2) {
	CollisionVisitor2 visitor{object1};
	return boost::apply_visitor(visitor, object2);
}

} // namespace geometry


#include "CEGO/CEGO.hpp"
#include <Eigen/Dense>
#include <atomic>

std::atomic_size_t Ncalls(0);

using CEGO::EArray;

/**
# Sadus, https://doi.org/10.1063/1.5041320, erratum: missing exponent of m
n is the repulsive exponent (the 12 of 12-6 LJ)
m is the attractive exponent (the 6 of 12-6 LJ)
*/
template <typename T>
auto B2_LennardJones(T Tstar, double n, double m){
    auto F = [&](auto y) {
        auto the_sum = 0.0;
        for (auto i = 0; i < 200; ++i) {
            auto my_factorial = [](auto k) { return tgamma(k + 1); };
            the_sum += tgamma((i * m - 3.0) / n) / my_factorial(i) * pow(y, i);
        }
        return pow(y, 3.0/(n - m)) * (tgamma((n - 3.0) / n) - 3.0/n*the_sum);
    };
    auto yn = pow(n/(n-m), n)*pow((n-m)/m,m)*pow(Tstar, -(n-m)); // y**n, Eq. 9
    auto y = pow(yn, 1.0/n);
    return 2*EIGEN_PI/3*F(y);
}

class FitClass {
public:
    EArray<double> m_T, B2, m_LHS, m_x;
    FitClass(const std::size_t Npts)
    {
        double Tmin = 0.1, Tmax = 10000;
        m_T = EArray<double>::LinSpaced(Npts, log10(Tmin), log10(Tmax)).exp();
        B2.resize(m_T.size());
        for (auto i = 0; i < m_T.size(); ++i) {
            B2(i) = B2_LennardJones(m_T(i), 12, 6);
        }
        // Set variables for fitting
        m_LHS = B2;
        m_x = 1/m_T;
    }
    template <typename TYPE>
    auto eval_RHS(const EArray<double>& x, const EArray<TYPE> &c)
    {
        std::decay_t<decltype(x)> val(x.size()); val.setZero();
        for (auto i = 0; i < c.size(); i += 2) {
            double a = c[i];
            double e = c[i + 1];
            val += a*x.pow(e);
        }
        return val.eval();
    }
    template <typename TYPE> TYPE objective(const EArray<TYPE>& c) {
        return ((eval_RHS(m_x, c) - m_LHS)).square().sum();
    }
    template <typename TYPE> EArray<TYPE> abs_rel_deviations(const EArray<TYPE>& c) {
        return (eval_RHS(m_x, c) - m_LHS).eval();
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const auto &c = dynamic_cast<const CEGO::NumericalIndividual<CEGO::numberish>*>(pind)->get_coeff_array<CEGO::numberish>();
        return objective(c);
    }
};

int do_one()
{
    //std::srand((unsigned int)time(0));
    std::size_t Nterms = 3;

    //EArray<CEGO::numberish> c1(3); c1 << 1.0, 2, 3;
    //EArray<CEGO::numberish> c2(3); c2 << 2.0, 3, 4;
    //EArray<CEGO::numberish> c3(3); c3 << 3.0, 4, 5;
    //auto oo = (c1 - c2).eval();
    //auto o2 = (c3 - c2).eval();
    //auto o3 = (o2*0.7).eval();

    // Construct the bounds
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < Nterms; ++i) {
        bounds.push_back(CEGO::Bound(std::make_pair(-1000.0, 1000.0)));
        bounds.push_back(CEGO::Bound(std::make_pair(0.0, 10.0)));
    }
    std::size_t Npts = 100;
    FitClass rp(Npts);

    auto Ncalls = 0;
    CEGO::CostFunction<CEGO::numberish> cost_wrapper = [&rp](const CEGO::AbstractIndividual*pind) {return rp.objective(pind); };
    auto Ntotal_individuals = 1000;
    auto Nlayers = 7;
    auto layers = CEGO::Layers<CEGO::numberish>(cost_wrapper, bounds.size(), Ntotal_individuals/Nlayers, Nlayers, 3);
    layers.parallel = false;
    layers.parallel_threads = 6;
    layers.set_bounds(bounds);
    layers.set_generation_mode(CEGO::GenerationOptions::LHS);
    layers.set_builtin_evolver(CEGO::BuiltinEvolvers::differential_evolution_best1bin);
    auto f = [&rp](const CEGO::EArray<double>& c) {return rp.objective<double>(c); };
    //auto f2 = [&rp](const CEGO::EArray<std::complex< double >>& c) {return rp.objective<std::complex<double>>(c); };
    //layers.add_gradient(f, f2);

    auto flags = layers.get_evolver_flags();
    flags["Nelite"] = 1;
    flags["Fmin"] = 0.1;
    flags["Fmax"] = 1.1;
    flags["CR"] = 0.9;
    layers.set_evolver_flags(flags);

    std::vector<double> best_costs;
    const double VTR = 2e-4;
    auto startTime = std::chrono::system_clock::now();
    bool success = false;
    for (auto counter = 0; counter < 15000; ++counter) {
        layers.do_generation();

        /*if (counter % 1000 == 0) {
            layers.gradient_minimizer();
        }*/

        auto [best_cost, best_coeffs] = layers.get_best();
        if (counter % 50 == 0) {
            std::cout << counter << ": best: " << best_cost << std::endl;
            //std::cout << counter << ": best coeffs: " << c << "||" << std::endl;
            //std::cout << counter << ": obj again: " << rp.objective(c) << "||" << std::endl;
        }
        if (best_cost < VTR) { success = true;  break; }
    }
    auto best_layer = layers.get_best();
    auto best_coeffs = std::get<1>(best_layer);
    //std::cout << rp.abs_rel_deviations(best_coeffs) << std::endl;
    auto endTime = std::chrono::system_clock::now();
    double elap = std::chrono::duration<double>(endTime - startTime).count();
    std::cout << "run:" << elap << " s\n";
    std::cout << "NFE:" << Ncalls << std::endl;
    return success;
}

int main() {
    int N = (CEGO::is_CI() ? 3 : 100);
    int good = 0;
    for (auto i = 0; i < N; ++i) {
        good += do_one();
    }
    std::cout << "success:" << good << "/" << N << std::endl;
}


#include "discretization.hpp"

#include <boost/numeric/odeint.hpp>

#include "eigenIntegration.hpp"
#include <eigen3/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h>

void eulerLinearDiscretization(Model &model,
                               double ts,
                               const Model::state_vector_t &x_eq,
                               const Model::input_vector_t &u_eq,
                               Model::state_matrix_t &A,
                               Model::control_matrix_t &B)
{
    Model::state_matrix_t A_c;
    Model::control_matrix_t B_c;
    model.computeJacobians(x_eq, u_eq, A_c, B_c);

    A = Model::state_matrix_t::Identity() + ts * A_c;
    B = ts * B_c;
}

void exactLinearDiscretization(Model &model,
                               double ts,
                               const Model::state_vector_t &x_eq,
                               const Model::input_vector_t &u_eq,
                               Model::state_matrix_t &A,
                               Model::control_matrix_t &B,
                               Model::state_vector_t &z)
{
    Model::state_matrix_t A_c;
    Model::control_matrix_t B_c;
    Model::state_vector_t f;
    model.computeJacobians(x_eq, u_eq, A_c, B_c);
    model.computef(x_eq, u_eq, f);

    Eigen::MatrixXd E;
    E.resize(Model::state_dim + Model::input_dim, Model::state_dim + Model::input_dim);
    E.setZero();
    E.topLeftCorner(Model::state_dim, Model::state_dim) << A_c;
    E.topRightCorner(Model::state_dim, Model::input_dim) << B_c;
    Eigen::MatrixXd expE = (E * ts).exp();

    A = expE.topLeftCorner(Model::state_dim, Model::state_dim);
    B = expE.topRightCorner(Model::state_dim, Model::input_dim);
    z = f - A * x_eq - B * u_eq;
}

class ODEMultipleShootingVariableTime
{
private:
    Model::input_vector_t u_t0, u_t1;
    double T, dt;
    Model &model;

public:
    using ode_matrix_t = Eigen::Matrix<double, Model::state_dim, 1 + Model::state_dim + 2 * Model::input_dim + 2>;

    ODEMultipleShootingVariableTime(
        const Model::input_vector_t &u_t0,
        const Model::input_vector_t &u_t1,
        const double &T,
        double dt,
        Model &model)
        : u_t0(u_t0), u_t1(u_t1), T(T), dt(dt), model(model) {}

    void operator()(const ode_matrix_t &V, ode_matrix_t &dVdt, const double t)
    {
        const Model::state_vector_t &x = V.col(0);
        const Model::input_vector_t u = u_t0 + t / dt * (u_t1 - u_t0);

        Model::state_vector_t f;
        Model::state_matrix_t A_bar;
        Model::control_matrix_t B_bar;
        model.computef(x, u, f);
        model.computeJacobians(x, u, A_bar, B_bar);
        A_bar *= T;
        B_bar *= T;

        const Model::state_matrix_t Phi_A_xi = V.block<Model::state_dim, Model::state_dim>(0, 1);
        const Model::state_matrix_t Phi_A_xi_inverse = Phi_A_xi.inverse();

        size_t cols = 0;

        // state
        dVdt.block<Model::state_dim, 1>(0, cols) = T * f;
        cols += 1;

        // A_bar
        dVdt.block<Model::state_dim, Model::state_dim>(0, cols) = A_bar * Phi_A_xi;
        cols += Model::state_dim;

        // B_bar
        const double alpha = (dt - t) / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * alpha;
        cols += Model::input_dim;

        // C_bar
        const double beta = t / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * beta;
        cols += Model::input_dim;

        // S_bar
        dVdt.block<Model::state_dim, 1>(0, cols) = Phi_A_xi_inverse * f;
        cols += 1;

        // z_bar
        dVdt.block<Model::state_dim, 1>(0, cols) = Phi_A_xi_inverse * (-A_bar * x - B_bar * u);

        assert(cols + 1 == size_t(dVdt.cols()));
    }
};

void multipleShootingVariableTime(
    Model &model,
    double T,
    const Eigen::MatrixXd &X,
    const Eigen::MatrixXd &U,
    Model::state_matrix_v_t &A_bar,
    Model::control_matrix_v_t &B_bar,
    Model::control_matrix_v_t &C_bar,
    Model::state_vector_v_t &S_bar,
    Model::state_vector_v_t &z_bar)
{
    const size_t K = X.cols();

    const double dt = 1. / double(K - 1);
    using namespace boost::numeric::odeint;
    runge_kutta4<ODEMultipleShootingVariableTime::ode_matrix_t, double, ODEMultipleShootingVariableTime::ode_matrix_t, double, vector_space_algebra> stepper;

    for (size_t k = 0; k < K - 1; k++)
    {
        ODEMultipleShootingVariableTime::ode_matrix_t V;
        V.setZero();
        V.col(0) = X.col(k);
        V.block<Model::state_dim, Model::state_dim>(0, 1).setIdentity();

        ODEMultipleShootingVariableTime odeMultipleShooting(U.col(k), U.col(k + 1), T, dt, model);

        integrate_adaptive(stepper, odeMultipleShooting, V, 0., dt, dt / 4.);

        size_t cols = 1;

        A_bar[k] = V.block<Model::state_dim, Model::state_dim>(0, cols);
        cols += Model::state_dim;

        B_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        C_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        S_bar[k] = A_bar[k] * V.block<Model::state_dim, 1>(0, cols);
        cols += 1;

        z_bar[k] = A_bar[k] * V.block<Model::state_dim, 1>(0, cols);
    }
}

class ODEMultipleShooting
{
private:
    Model::input_vector_t u_t0, u_t1;
    double dt;
    Model &model;

public:
    using ode_matrix_t = Eigen::Matrix<double, Model::state_dim, 1 + Model::state_dim + 2 * Model::input_dim + 1>;

    ODEMultipleShooting(
        const Model::input_vector_t &u_t0,
        const Model::input_vector_t &u_t1,
        double dt,
        Model &model)
        : u_t0(u_t0), u_t1(u_t1), dt(dt), model(model) {}

    void operator()(const ode_matrix_t &V, ode_matrix_t &dVdt, const double t)
    {
        const Model::state_vector_t &x = V.col(0);
        const Model::input_vector_t u = u_t0 + t / dt * (u_t1 - u_t0);

        Model::state_vector_t f;
        Model::state_matrix_t A_bar;
        Model::control_matrix_t B_bar;
        model.computef(x, u, f);
        model.computeJacobians(x, u, A_bar, B_bar);

        const Model::state_matrix_t Phi_A_xi = V.block<Model::state_dim, Model::state_dim>(0, 1);
        const Model::state_matrix_t Phi_A_xi_inverse = Phi_A_xi.inverse();

        size_t cols = 0;

        // state
        dVdt.block<Model::state_dim, 1>(0, cols) = f;
        cols += 1;

        // A_bar
        dVdt.block<Model::state_dim, Model::state_dim>(0, cols) = A_bar * Phi_A_xi;
        cols += Model::state_dim;

        // B_bar
        const double alpha = (dt - t) / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * alpha;
        cols += Model::input_dim;

        // C_bar
        const double beta = t / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * beta;
        cols += Model::input_dim;

        // z_bar
        dVdt.block<Model::state_dim, 1>(0, cols) = Phi_A_xi_inverse * (f - A_bar * x - B_bar * u);

        assert(cols + 1 == size_t(dVdt.cols()));
    }
};

void multipleShooting(
    Model &model,
    double T,
    const Eigen::MatrixXd &X,
    const Eigen::MatrixXd &U,
    Model::state_matrix_v_t &A_bar,
    Model::control_matrix_v_t &B_bar,
    Model::control_matrix_v_t &C_bar,
    Model::state_vector_v_t &z_bar)
{
    const size_t K = X.cols();

    const double dt = T / double(K - 1);
    using namespace boost::numeric::odeint;
    runge_kutta4<ODEMultipleShooting::ode_matrix_t, double, ODEMultipleShooting::ode_matrix_t, double, vector_space_algebra> stepper;

    for (size_t k = 0; k < K - 1; k++)
    {
        ODEMultipleShooting::ode_matrix_t V;
        V.setZero();
        V.col(0) = X.col(k);
        V.block<Model::state_dim, Model::state_dim>(0, 1).setIdentity();

        ODEMultipleShooting odeMultipleShooting(U.col(k), U.col(k + 1), dt, model);

        integrate_adaptive(stepper, odeMultipleShooting, V, 0., dt, dt / 4.);

        size_t cols = 1;

        A_bar[k] = V.block<Model::state_dim, Model::state_dim>(0, cols);
        cols += Model::state_dim;

        B_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        C_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        z_bar[k] = A_bar[k] * V.block<Model::state_dim, 1>(0, cols);
    }
}