#include "storm/utility/numerical.h"

#include <cmath>
#include <boost/math/constants/constants.hpp>

#include "storm/utility/macros.h"
#include "storm/utility/constants.h"
#include "storm/exceptions/InvalidArgumentException.h"
#include "storm/exceptions/PrecisionExceededException.h"

namespace storm {
    namespace utility {
        namespace numerical {

            template<typename ValueType>
            FoxGlynnResult<ValueType>::FoxGlynnResult() : left(0), right(0), totalWeight(storm::utility::zero<ValueType>()) {
                // Intentionally left empty.
            }

            /*!
             * The following implementation of Fox and Glynn's algorithm is taken from David Jansen's patched version
             * in MRMC, which is based on his paper:
             *
             * https://pms.cs.ru.nl/iris-diglib/src/getContent.php?id=2011-Jansen-UnderstandingFoxGlynn
             *
             * We have only adapted the code to match more of C++'s and our coding guidelines.
             */

            template<typename ValueType>
            FoxGlynnResult<ValueType> foxGlynnFinder(ValueType lambda, ValueType epsilon) {
                ValueType tau = std::numeric_limits<ValueType>::min();
                ValueType omega = std::numeric_limits<ValueType>::max();
                ValueType const sqrt_2_pi = boost::math::constants::root_two_pi<ValueType>();
                ValueType const log10_e = std::log10(boost::math::constants::e<ValueType>());

                uint64_t m = static_cast<uint64_t>(lambda);

                int64_t left = 0;
                int64_t right = 0;

                // tau is only used in underflow checks, which we are going to do in the logarithm domain.
                tau = log(tau);

                // In error bound comparisons, we always compare with epsilon*sqrt_2_pi.
                epsilon *= sqrt_2_pi;

                // Compute left truncation point.
                if (m < 25) {
                    // For lambda below 25 the exponential can be smaller than tau. If that is the case we expect
                    // underflows and warn the user.
                    if (-lambda <= tau) {
                        STORM_LOG_WARN("Fox-Glynn: 0 < lambda < 25, underflow near Poi(" << lambda << ", 0) = " << std::exp(-lambda) << ". The results are unreliable.");
                    }

                    // Zero is used as left truncation point for lambda <= 25.
                    left = 0;
                } else {
                    // Compute the left truncation point for lambda >= 25 (for lambda < 25 we use zero as left truncation point).

                    ValueType const bl = (1 + 1 / lambda) * std::exp((1/lambda) * 0.125);
                    ValueType const sqrt_lambda = std::sqrt(lambda);
                    int64_t k;

                    // Start looking for the left truncation point:
                    // * start search at k=4 (taken from original Fox-Glynn paper)
                    // * increase the left truncation point until we fulfil the error condition

                    for (k = 4;; ++k) {
                        ValueType max_err;

                        left = m - static_cast<int64_t>(std::ceil(k*sqrt_lambda + 0.5));

                        // For small lambda the above calculation can yield negative truncation points, crop them here.
                        if (left <= 0) {
                            left = 0;
                            break;
                        }

                        // Note that Propositions 2-4 in Fox--Glynn mix up notation: they write Phi where they mean
                        // 1 - Phi. (In Corollaries 1 and 2, phi is used correctly again.)
                        max_err = bl * exp(-0.5 * (k*k)) / k;
                        if (max_err * 2 <= epsilon) {
                            // If the error on the left hand side is smaller, we can be more lenient on the right hand
                            // side. To this end, we now set epsilon to the part of the error that has not yet been eaten
                            // up by the left-hand truncation.
                            epsilon -= max_err;
                            break;
                        }
                    }

                    // Finally the left truncation point is found.
                }

                // Compute right truncation point.
                {
                    ValueType lambda_max;
                    int64_t m_max, k;

                    // According to Fox-Glynn, if lambda < 400 we should take lambda = 400, otherwise use the original
                    // value. This is for computing the right truncation point.
                    if (m < 400) {
                        lambda_max = 400;
                        m_max = 400;
                        epsilon *= 0.662608824988162441697980;
                        /* i.e. al = (1+1/400) * exp(1/16) * sqrt_2; epsilon /= al; */
                    } else {
                        lambda_max = lambda;
                        m_max = m;
                        epsilon *= (1 - 1 / (lambda + 1)) * 0.664265347050632847802225;
                        /* i.e. al = (1+1/lambda) * exp(1/16) * sqrt_2; epsilon /= al; */
                    }

                    // Find right truncation point.

                    // This loop is a modification to the original Fox-Glynn paper.
                    // The search for the right truncation point is only terminated by  the error condition and not by
                    // the stop index from the FG paper. This can yield more accurate results if necessary.
                    for (k = 4;; ++k) {
                        // dkl_inv is between 1 - 1e-33 and 1 if lambda_max >= 400 and k >= 4; this will always be
                        // rounded to 1.0. We therefore leave the factor out.
                        // double dkl_inv=1 - exp(-266/401.0 * (k*sqrt(2*lambda_max) + 1.5));

                        // actually: "k * (dkl_inv*epsilon/al) >= exp(-0.5 * k^2)", but epsilon has been changed appropriately.
                        if (k * epsilon >= exp(-0.5*(k*k))) {
                            break;
                        }
                    }
                    right = m_max + static_cast<int64_t>(std::ceil(k * std::sqrt(2 * lambda_max) + 0.5));
                    if (right > m_max + static_cast<int64_t>(std::ceil((lambda_max + 1) * 0.5))) {
                        STORM_LOG_WARN("Fox-Glynn: right = " << right << " >> lambda = " << lambda_max << ", cannot bound the right tail. The results are unreliable.");
                    }
                }

                // Time to set the initial value for weights.
                FoxGlynnResult<ValueType> fgresult;
                fgresult.left = static_cast<uint64_t>(left);
                fgresult.right = static_cast<uint64_t>(right);
                fgresult.weights.resize(fgresult.right - fgresult.left + 1);

                fgresult.weights[m - left] = omega / (1.0e+10 * (right - left));

                if (m >= 25) {
                    // Perform underflow check.
                    ValueType result, log_c_m_inf;
                    int64_t i;

                    // we are going to compare with tau - log(w[m]).
                    tau -= std::log(fgresult.weights[m - left]);

                    // We take the c_m_inf = 0.14627 / sqrt( m ), as for lambda >= 25
                    // c_m = 1 / ( sqrt( 2.0 * pi * m ) ) * exp( m - lambda - 1 / ( 12.0 * m ) ) => c_m_inf.
                    // Note that m-lambda is in the interval (-1,0], and -1/(12*m) is in [-1/(12*25),0).
                    // So, exp(m-lambda - 1/(12*m)) is in (exp(-1-1/(12*25)),exp(0)).
                    // Therefore, we can improve the lower bound on c_m to exp(-1-1/(12*25)) / sqrt(2*pi) = ~0.14627.
                    // Its logarithm is -1 - 1/(12*25) - log(2*pi) * 0.5 = ~ -1.922272 (rounded towards -infinity).
                    log_c_m_inf = -1.922272 - log((double) m) * 0.5;

                    // We use FG's Proposition 6 directly (and not Corollary 4 i and ii), as k_prime may be too large
                    // if pFG->left == 0.
                    i = m - left;

                    // Equivalent to 2*i <= m, equivalent to i <= lambda/2.
                    if (i <= left) {
                        // Use Proposition 6 (i). Note that Fox--Glynn are off by one in the proof of this proposition;
                        // they sum up to i-1, but should have summed up to i. */
                        result = log_c_m_inf
                        - i * (i+1) * (0.5 + (2*i+1)/(6*lambda)) / lambda;
                    } else {
                        // Use Corollary 4 (iii). Note that k_prime <= sqrt(m+1)/m is a misprint for k_prime <= m/sqrt(m+1),
                        // which is equivalent to left >= 0, which holds trivially.
                        result = -lambda;
                        if (left != 0) {
                            // Also use Proposition 6 (ii).
                            double result_1 = log_c_m_inf + i * log(1 - i/(double) (m+1));

                            // Take the maximum.
                            if (result_1 > result) {
                                result = result_1;
                            }
                        }
                    }
                    if (result <= tau) {
                        int64_t const log10_result = static_cast<int64_t>(std::floor(result * log10_e));
                        STORM_LOG_WARN("Fox-Glynn: lambda >= 25, underflow near Poi(" << lambda << "," << left << ") <= " << std::exp(result - log10_result/log10_e) << log10_result << ". The results are unreliable.");
                    }

                    // We still have to perform an underflow check for the right truncation point when lambda >= 400.
                    if (m >= 400) {
                        // Use Proposition 5 of Fox--Glynn.
                        i = right - m;
                        result = log_c_m_inf - i * (i + 1) / (2 * lambda);
                        if (result <= tau) {
                            int64_t const log10_result = static_cast<int64_t>(std::floor(result * log10_e));
                            STORM_LOG_WARN("Fox-Glynn: lambda >= 25, underflow near Poi(" << lambda << "," << right << ") <= " << std::exp(result - log10_result/log10_e) << log10_result << ". The results are unreliable.");
                        }
                    }
                }

                return fgresult;
            }

            template<typename ValueType>
            FoxGlynnResult<ValueType> foxGlynnWeighter(ValueType lambda, ValueType epsilon) {
                ValueType tau = std::numeric_limits<ValueType>::min();

                // The magic m point.
                uint64_t m = static_cast<uint64_t>(lambda);
                int64_t j, t;

                FoxGlynnResult<ValueType> result = foxGlynnFinder(lambda, epsilon);

                // Fill the left side of the array.
                for (j = m - result.left; j > 0; --j) {
                    result.weights[j - 1] = (j + result.left) / lambda * result.weights[j];
                }

                t = result.right - result.left;

                // Fill the right side of the array, have two cases lambda < 400 & lambda >= 400.
                if (m < 400) {
                    // Perform the underflow check, according to Fox-Glynn.
                    STORM_LOG_ERROR_COND(result.right <= 600, "Fox-Glynn: " << result.right << " > 600, underflow is possible.");

                    // Compute weights.
                    for (j = m - result.left; j < t; ++j) {
                        ValueType q = lambda / (j + 1 + result.left);
                        if (result.weights[j] > tau / q) {
                            result.weights[j + 1] = q * result.weights[j];
                        } else {
                            t = j;
                            result.right = j + result.left;
                            result.weights.resize(result.right - result.left + 1);

                            // It's time to compute W.
                            break;
                        }
                    }
                } else {
                    // Compute weights.
                    for (j = m - result.left; j < t; ++j) {
                        result.weights[j + 1] = lambda / (j + 1 + result.left) * result.weights[j];
                    }
                }

                // It is time to compute the normalization weight W.
                result.totalWeight = storm::utility::zero<ValueType>();
                j = 0;

                // t was set above.
                while(j < t) {
                    if (result.weights[j] <= result.weights[t]) {
                        result.totalWeight += result.weights[j];
                        j++;
                    } else {
                        result.totalWeight += result.weights[t];
                        t--;
                    }
                }
                result.totalWeight += result.weights[j];

                STORM_LOG_TRACE("Fox-Glynn: ltp = " << result.left << ", rtp = " << result.right << ", w = " << result.totalWeight << ", " << result.weights.size() << " weights.");

                return result;
            }

            template<typename ValueType>
            FoxGlynnResult<ValueType> foxGlynn(ValueType lambda, ValueType epsilon) {
                STORM_LOG_THROW(lambda > 0, storm::exceptions::InvalidArgumentException, "Fox-Glynn requires positive lambda.");
                return foxGlynnWeighter(lambda, epsilon);
            }

            template FoxGlynnResult<double> foxGlynn(double lambda, double epsilon);

        }
    }
}


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

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <stdexcept>
#include <iostream>
#include <type_traits>
#include <boost/math/ccmath/round.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
constexpr void test()
{
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::isnan(boost::math::ccmath::round(std::numeric_limits<T>::quiet_NaN())), "If x is NaN, NaN is returned");
        static_assert(boost::math::ccmath::lround(std::numeric_limits<T>::quiet_NaN()) == T(0), "If x is NaN, 0 is returned");
        static_assert(boost::math::ccmath::llround(std::numeric_limits<T>::quiet_NaN()) == T(0), "If x is NaN, 0 is returned");
    }

    static_assert(boost::math::ccmath::round(T(0)) == T(0));
    static_assert(boost::math::ccmath::lround(T(0)) == 0l);
    static_assert(boost::math::ccmath::llround(T(0)) == 0ll);

    static_assert(boost::math::ccmath::round(T(-0)) == T(-0));
    static_assert(boost::math::ccmath::lround(T(-0)) == -0l);
    static_assert(boost::math::ccmath::llround(T(-0)) == -0ll);

    static_assert(boost::math::ccmath::isinf(boost::math::ccmath::round(std::numeric_limits<T>::infinity())));
    static_assert(boost::math::ccmath::lround(std::numeric_limits<T>::infinity()) == 0l);
    static_assert(boost::math::ccmath::llround(std::numeric_limits<T>::infinity()) == 0ll);

    static_assert(boost::math::ccmath::round(T(2.3)) == T(2));
    static_assert(boost::math::ccmath::round(T(2.5)) == T(3));
    static_assert(boost::math::ccmath::round(T(2.7)) == T(3));
    static_assert(boost::math::ccmath::round(T(-2.3)) == T(-2));
    static_assert(boost::math::ccmath::round(T(-2.5)) == T(-3));
    static_assert(boost::math::ccmath::round(T(-2.7)) == T(-3));

    static_assert(boost::math::ccmath::lround(T(2.3)) == 2l);
    static_assert(boost::math::ccmath::lround(T(2.5)) == 3l);
    static_assert(boost::math::ccmath::lround(T(2.7)) == 3l);
    static_assert(boost::math::ccmath::lround(T(-2.3)) == -2l);
    static_assert(boost::math::ccmath::lround(T(-2.5)) == -3l);
    static_assert(boost::math::ccmath::lround(T(-2.7)) == -3l);

    static_assert(boost::math::ccmath::llround(T(2.3)) == 2ll);
    static_assert(boost::math::ccmath::llround(T(2.5)) == 3ll);
    static_assert(boost::math::ccmath::llround(T(2.7)) == 3ll);
    static_assert(boost::math::ccmath::llround(T(-2.3)) == -2ll);
    static_assert(boost::math::ccmath::llround(T(-2.5)) == -3ll);
    static_assert(boost::math::ccmath::llround(T(-2.7)) == -3ll);
}

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #ifdef BOOST_HAS_FLOAT128
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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
//
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>

// Example for convolve_rows() and convolve_cols() in the numeric extension

using namespace boost::gil;

// Models a Unary Function
template <typename P> // Models PixelValueConcept
struct mandelbrot_fn {
  using point_t = boost::gil::point_t;
  using const_t = mandelbrot_fn;
  using value_type = P;
  using reference = value_type;
  using const_reference = value_type;
  using argument_type = point_t;
  using result_type = reference;
  static constexpr bool is_mutable = false;

  value_type _in_color, _out_color;
  point_t _img_size;
  static const int MAX_ITER = 100; // max number of iterations

  mandelbrot_fn() {}
  mandelbrot_fn(const point_t &sz, const value_type &in_color,
                const value_type &out_color)
      : _in_color(in_color), _out_color(out_color), _img_size(sz) {}

  result_type operator()(const point_t &p) const {
    // normalize the coords to (-2..1, -1.5..1.5)
    // (actually make y -1.0..2 so it is asymmetric, so we can verify some view
    // factory methods)
    double t = get_num_iter(
        point<double>(p.x / (double)_img_size.x * 3 - 2,
                      p.y / (double)_img_size.y * 3 - 1.0f)); // 1.5f));
    t = pow(t, 0.2);

    value_type ret;
    for (int k = 0; k < num_channels<P>::value; ++k)
      ret[k] = (typename channel_type<P>::type)(_in_color[k] * t +
                                                _out_color[k] * (1 - t));
    return ret;
  }

private:
  double get_num_iter(const point<double> &p) const {
    point<double> Z(0, 0);
    for (int i = 0; i < MAX_ITER; ++i) {
      Z = point<double>(Z.x * Z.x - Z.y * Z.y + p.x, 2 * Z.x * Z.y + p.y);
      if (Z.x * Z.x + Z.y * Z.y > 4)
        return i / (double)MAX_ITER;
    }
    return 0;
  }
};

int main() {
  using deref_t = mandelbrot_fn<rgb8_pixel_t>;
  using point_t = deref_t::point_t;
  using locator_t = virtual_2d_locator<deref_t, false>;
  using my_virt_view_t = image_view<locator_t>;

  boost::function_requires<PixelLocatorConcept<locator_t>>();
  gil_function_requires<StepIteratorConcept<locator_t::x_iterator>>();

  point_t dims(200, 200);
  my_virt_view_t mandel(dims, locator_t(point_t(0, 0), point_t(1, 1),
                                        deref_t(dims, rgb8_pixel_t(255, 0, 255),
                                                rgb8_pixel_t(0, 255, 0))));
  write_view("out-mandelbrot.jpg", mandel, jpeg_tag{});

  return 0;
}


//
// Created by jhwangbo on 17.04.17.
//

#ifndef RAI_DLCTRL_SIMPLEMLP_HPP
#define RAI_DLCTRL_SIMPLEMLP_HPP

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cstdlib>
#include "iostream"
#include <fstream>
#include <cmath>

namespace RAI {

template<int StateDim, int ActionDim>
class MLP_fullyconnected {

 public:
  MLP_fullyconnected(const std::vector<int>& hiddensizes)
  // :    act_(activation)
  {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n");

    layersizes.push_back(StateDim);
    layersizes.reserve(layersizes.size() + hiddensizes.size());
    layersizes.insert(layersizes.end(), hiddensizes.begin(), hiddensizes.end());
    layersizes.push_back(ActionDim);
    ///[input hidden output]

    params.resize(2 * (layersizes.size() - 1));
    Ws.resize(layersizes.size() - 1);
    bs.resize(layersizes.size() - 1);
    lo.resize(layersizes.size());
  }

  bool readPolicyFromFile(const std::string& fileName) {
    std::stringstream parameterFileName;
    std::ifstream indata;
    indata.open(fileName);
    std::string line;
    getline(indata, line);
    std::stringstream lineStream(line);
    std::string cell;

    ///assign parameters
    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      if (i % 2 == 0) ///W resize
      {
        Ws[i / 2].resize(layersizes[i / 2 + 1], layersizes[i / 2]);
        params[i].resize(layersizes[i / 2] * layersizes[i / 2 + 1]);
      }
      if (i % 2 == 1) ///b resize
      {
        bs[(i - 1) / 2].resize(layersizes[(i + 1) / 2]);
        params[i].resize(layersizes[(i + 1) / 2]);
      }

      while (std::getline(lineStream, cell, ',')) { ///Read param
        params[i](paramSize++) = std::stof(cell);
        if (paramSize == params[i].size()) break;
      }
      if (i % 2 == 0) ///W copy
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(double) * Ws[i / 2].size());
      if (i % 2 == 1) ///b copy
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(double) * bs[(i - 1) / 2].size());
    }

    return true;
  }

  Eigen::VectorXd forward(Eigen::VectorXd state) {
    lo[0] = state;
    for (int cnt = 0; cnt < Ws.size() - 1; cnt++) {
      lo[cnt + 1] = Ws[cnt] * lo[cnt] + bs[cnt];

      for (int i = 0; i < lo[cnt + 1].size(); i++) {
        // if (act_.compare("tanh") == 0) {
          lo[cnt + 1][i] = std::tanh(lo[cnt + 1][i]);
        // }
        // else if (act_.compare("relu") == 0) {
          // if (lo[cnt + 1][i] < 0) lo[cnt + 1][i] = 0;
        // }
        // else if (act_.compare("softsign") == 0) {
          // lo[cnt + 1][i] =  lo[cnt + 1][i] / (std::abs( lo[cnt + 1][i]) + 1.0);
        // }
      }
    }

    lo[lo.size() - 1] = Ws[Ws.size() - 1] * lo[lo.size() - 2] + bs[bs.size() - 1]; /// output layer

    return lo[lo.size() - 1];
  }
 private:

//Eigen::MatrixXd output_;
  std::vector<Eigen::VectorXd> params;
  std::vector<Eigen::MatrixXd> Ws;
  std::vector<Eigen::VectorXd> bs;
  std::vector<Eigen::VectorXd> lo;

  std::vector<int> layersizes;
};

}

#endif //RAI_SIMPLEMLP_HPP


#include <iostream>

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

#include "utils.hpp"

namespace ublas  = boost::numeric::ublas;

static const double TOL(1.0e-5); ///< Used for comparing two real numbers.
static const int n(10);           ///< defines the test matrix size

template<class mat, class vec>
double diff(const mat& A, const vec& x, const vec& b) {
  return ublas::norm_2(prod(A, x) - b);
}

// efficiently fill matrix depending on majority
template<class mat>
void fill_matrix(mat& A, ublas::column_major_tag) {
  for (int i=0; i<n; ++i) {
    if (i-1>=0) {
      A(i-1, i) = -1;
    }
    A(i, i) = 1;
    if (i+1<n) {
      A(i+1, i) = -2;
    }
  }
}
template<class mat>
void fill_matrix(mat& A, ublas::row_major_tag) {
  for (int i=0; i<n; ++i) {
    if (i-1>=0) {
      A(i, i-1) = -1;
    }
    A(i, i) = 1;
    if (i+1<n) {
      A(i, i+1) = -2;
    }
  }
}

template<class mat>
BOOST_UBLAS_TEST_DEF ( test_inplace_solve )
{
  mat A(n, n);
  A.clear();
  fill_matrix(A, typename mat::orientation_category());

  ublas::vector<double>  b(n, 1.0);

  // The test matrix is not triangular, but is interpreted that way by
  // inplace_solve using the lower_tag/upper_tags. For checking, the
  // triangular_adaptor makes A triangular for comparison.
  {
    ublas::vector<double>  x(b);
    ublas::inplace_solve(A, x, ublas::lower_tag());
    BOOST_UBLAS_TEST_CHECK(diff(ublas::triangular_adaptor<mat, ublas::lower>(A), x, b) < TOL);
  }
  {
    ublas::vector<double>  x(b);
    ublas::inplace_solve(A, x, ublas::upper_tag());
    BOOST_UBLAS_TEST_CHECK(diff (ublas::triangular_adaptor<mat, ublas::upper>(A), x, b) < TOL);
  }
  {
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, A, ublas::lower_tag());
    BOOST_UBLAS_TEST_CHECK(diff (trans(ublas::triangular_adaptor<mat, ublas::lower>(A)), x, b) < TOL);
  }
  {
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, A, ublas::upper_tag());
    BOOST_UBLAS_TEST_CHECK(diff (trans(ublas::triangular_adaptor<mat, ublas::upper>(A)), x , b) < TOL);
  }
}

int main() {

  // typedefs are needed as macros do not work with "," in template arguments
  typedef ublas::compressed_matrix<double, ublas::row_major>     commat_doub_rowmaj;
  typedef ublas::compressed_matrix<double, ublas::column_major>  commat_doub_colmaj;
  typedef ublas::matrix<double, ublas::row_major>                mat_doub_rowmaj;
  typedef ublas::matrix<double, ublas::column_major>             mat_doub_colmaj;
  typedef ublas::mapped_matrix<double, ublas::row_major>         mapmat_doub_rowmaj;
  typedef ublas::mapped_matrix<double, ublas::column_major>      mapmat_doub_colmaj;
  typedef ublas::coordinate_matrix<double, ublas::row_major>     cormat_doub_rowmaj;
  typedef ublas::coordinate_matrix<double, ublas::column_major>  cormat_doub_colmaj;
  typedef ublas::mapped_vector_of_mapped_vector<double, ublas::row_major> mvmv_doub_rowmaj;
  typedef ublas::mapped_vector_of_mapped_vector<double, ublas::column_major> mvmv_doub_colmaj;

  BOOST_UBLAS_TEST_BEGIN();

#ifdef USE_MATRIX
  BOOST_UBLAS_TEST_DO( test_inplace_solve<mat_doub_rowmaj> );
  BOOST_UBLAS_TEST_DO( test_inplace_solve<mat_doub_colmaj> );
#endif

#ifdef USE_COMPRESSED_MATRIX
  BOOST_UBLAS_TEST_DO( test_inplace_solve<commat_doub_rowmaj> );
  BOOST_UBLAS_TEST_DO( test_inplace_solve<commat_doub_colmaj> );
#endif

#ifdef USE_MAPPED_MATRIX
  BOOST_UBLAS_TEST_DO( test_inplace_solve<mapmat_doub_rowmaj> );
  BOOST_UBLAS_TEST_DO( test_inplace_solve<mapmat_doub_colmaj> );
#endif

#ifdef USE_COORDINATE_MATRIX
  BOOST_UBLAS_TEST_DO( test_inplace_solve<cormat_doub_rowmaj> );
  BOOST_UBLAS_TEST_DO( test_inplace_solve<cormat_doub_colmaj> );
#endif

#ifdef USE_MAPPED_VECTOR_OF_MAPPED_VECTOR
  BOOST_UBLAS_TEST_DO( test_inplace_solve<mvmv_doub_rowmaj> );
  BOOST_UBLAS_TEST_DO( test_inplace_solve<mvmv_doub_colmaj> );
#endif

  BOOST_UBLAS_TEST_END();
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008, 2019 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/functional.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/distributions/chisquaredistribution.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/termstructures/volatility/equityfx/localvoltermstructure.hpp>
#include <ql/methods/finitedifferences/meshers/fdmhestonvariancemesher.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <set>
#include <algorithm>

namespace QuantLib {

    namespace {
        struct interpolated_volatility {
            interpolated_volatility(const std::vector<Real>& pGrid,
                                    const std::vector<Real>& vGrid)
            : variance(pGrid.begin(), pGrid.end(), vGrid.begin()) {}
            Real operator()(Real x) const {
                return std::sqrt(variance(x, true));
            }
            LinearInterpolation variance;
        };
    }

    FdmHestonVarianceMesher::FdmHestonVarianceMesher(
        Size size,
        const ext::shared_ptr<HestonProcess> & process,
        Time maturity, Size tAvgSteps, Real epsilon,
        Real mixingFactor)
        : Fdm1dMesher(size) {

        std::vector<Real> vGrid(size, 0.0), pGrid(size, 0.0);
        const Real mixedSigma = process->sigma()*mixingFactor;
        const Real df  = 4*process->theta()*process->kappa()/squared(mixedSigma);
        try {
            std::multiset<std::pair<Real, Real> > grid;

            for (Size l=1; l<=tAvgSteps; ++l) {
                const Real t = (maturity*l)/tAvgSteps;
                const Real ncp = 4*process->kappa()*std::exp(-process->kappa()*t)/(squared(mixedSigma)
                    *(1-std::exp(-process->kappa()*t)))*process->v0();
                const Real k = squared(mixedSigma)
                    *(1-std::exp(-process->kappa()*t))/(4*process->kappa());

                const Real qMin = 0.0; // v_min = 0.0;
                const Real qMax = std::max(process->v0(),
                    k*InverseNonCentralCumulativeChiSquareDistribution(
                                            df, ncp, 100, 1e-8)(1-epsilon));

                const Real minVStep=(qMax-qMin)/(50*size);
                Real ps,p = 0.0;

                Real vTmp = qMin;
                grid.insert(std::pair<Real, Real>(qMin, epsilon));

                for (Size i=1; i < size; ++i) {
                    ps = (1 - epsilon - p)/(size-i);
                    p += ps;
                    const Real tmp = k*InverseNonCentralCumulativeChiSquareDistribution(
                        df, ncp, 100, 1e-8)(p);

                    const Real vx = std::max(vTmp+minVStep, tmp);
                    p = NonCentralCumulativeChiSquareDistribution(df, ncp)(vx/k);
                    vTmp=vx;
                    grid.insert(std::pair<Real, Real>(vx, p));
                }
            }
            QL_REQUIRE(grid.size() == size*tAvgSteps,
                       "something wrong with the grid size");

            const std::vector<std::pair<Real, Real> > tp(grid.begin(), grid.end());

            for (Size i=0; i < size; ++i) {
                const Size b = (i*tp.size())/size;
                const Size e = ((i+1)*tp.size())/size;
                for (Size j=b; j < e; ++j) {
                    vGrid[i]+=tp[j].first/(e-b);
                    pGrid[i]+=tp[j].second/(e-b);
                }
            }
        }
        catch (const Error&) {
            // use default mesh
            const Real vol = mixedSigma*
                std::sqrt(process->theta()/(2*process->kappa()));

            const Real mean = process->theta();
            const Real upperBound = std::max(process->v0()+4*vol, mean+4*vol);
            const Real lowerBound
                = std::max(0.0, std::min(process->v0()-4*vol, mean-4*vol));

            for (Size i=0; i < size; ++i) {
                pGrid[i] = i/(size-1.0);
                vGrid[i] = lowerBound + i*(upperBound-lowerBound)/(size-1.0);
            }
        }

        Real skewHint = ((process->kappa() != 0.0)
                ? std::max(1.0, mixedSigma/process->kappa()) : 1.0);

        std::sort(pGrid.begin(), pGrid.end());
        volaEstimate_ = GaussLobattoIntegral(100000, 1e-4)(
            interpolated_volatility(pGrid, vGrid),
                pGrid.front(), pGrid.back())*std::pow(skewHint, 1.5);

        const Real v0 = process->v0();
        for (Size i=1; i<vGrid.size(); ++i) {
            if (vGrid[i-1] <= v0 && vGrid[i] >= v0) {
                if (std::fabs(vGrid[i-1] - v0) < std::fabs(vGrid[i] - v0))
                    vGrid[i-1] = v0;
                else
                    vGrid[i] = v0;
            }
        }

        std::copy(vGrid.begin(), vGrid.end(), locations_.begin());

        for (Size i=0; i < size-1; ++i) {
            dminus_[i+1] = dplus_[i] = vGrid[i+1] - vGrid[i];
        }
        dplus_.back() = dminus_.front() = Null<Real>();
    }

    FdmHestonLocalVolatilityVarianceMesher::FdmHestonLocalVolatilityVarianceMesher(
        Size size,
        const ext::shared_ptr<HestonProcess>& process,
        const ext::shared_ptr<LocalVolTermStructure>& leverageFct,
        Time maturity, Size tAvgSteps, Real epsilon,
        Real mixingFactor)
     : Fdm1dMesher(size) {

        const FdmHestonVarianceMesher mesher(
            size, process, maturity, tAvgSteps, epsilon, mixingFactor);

        for (Size i=0; i < size; ++i) {
            dplus_[i] = mesher.dplus(i);
            dminus_[i] = mesher.dminus(i);
            locations_[i] = mesher.location(i);
        }

        volaEstimate_ = mesher.volaEstimate();

        if (leverageFct != nullptr) {
            typedef boost::accumulators::accumulator_set<
                Real, boost::accumulators::stats<
                    boost::accumulators::tag::mean> >
                accumulator_set;

            accumulator_set acc;

            const Real s0 = process->s0()->value();

            acc(leverageFct->localVol(0, s0, true));

            const Handle<YieldTermStructure> rTS = process->riskFreeRate();
            const Handle<YieldTermStructure> qTS = process->dividendYield();

            for (Size l=1; l <= tAvgSteps; ++l) {
                const Real t = (maturity*l)/tAvgSteps;
                const Real vol = volaEstimate_ * boost::accumulators::mean(acc);

                const Real fwd = s0*qTS->discount(t)/rTS->discount(t);

                const Size sAvgSteps = 50;

                std::vector<Real> u(sAvgSteps), sig(sAvgSteps);

                for (Size i=0; i < sAvgSteps; ++i) {
                    u[i] = epsilon + ((1-2*epsilon)/(sAvgSteps-1))*i;
                    const Real x = InverseCumulativeNormal()(u[i]);

                    const Real gf = x*vol*std::sqrt(t);
                    const Real f = fwd*std::exp(gf);

                    sig[i] = squared(leverageFct->localVol(t, f, true));
                }

                const Real leverageAvg =
                    GaussLobattoIntegral(10000, 1e-4)(
                        interpolated_volatility(u, sig), u.front(), u.back())
                    / (1-2*epsilon);

                acc(leverageAvg);
            }
            volaEstimate_ *= boost::accumulators::mean(acc);
        }
    }
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/DistanceFuncs.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidDataSet.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/FluidTensor.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <queue>
#include <string>

namespace fluid {
namespace algorithm {

class KMeans
{

public:
  void clear()
  {
    mMeans.setZero();
    mAssignments.setZero();
    mTrained = false;
  }

  bool initialized() const { return mTrained; }

  void train(const FluidDataSet<std::string, double, 1>& dataset, index k,
             index maxIter)
  {
    using namespace Eigen;
    using namespace _impl;
    assert(!mTrained || (dataset.pointSize() == mDims && mK == k));
    auto dataPoints = asEigen<Array>(dataset.getData());
    if (mTrained) { mAssignments = assignClusters(dataPoints); }
    else
    {
      mK = k;
      mDims = dataset.pointSize();
      mMeans = ArrayXXd::Zero(mK, mDims);
      mEmpty = std::vector<bool>(asUnsigned(mK), false);
      mAssignments =
          ((0.5 + (0.5 * ArrayXf::Random(dataPoints.rows()))) * (mK - 1))
              .round()
              .cast<int>();
    }

    while (maxIter-- > 0)
    {
      computeMeans(dataPoints);
      auto assignments = assignClusters(dataPoints);
      if (!changed(assignments)) { break; }
      else
      {
        mAssignments = assignments;
      }
    }
    mTrained = true;
  }

  index getClusterSize(index cluster) const
  {
    index count = 0;
    for (index i = 0; i < mAssignments.size(); i++)
    {
      if (mAssignments(i) == cluster) count++;
    }
    return count;
  }

  index vq(RealVectorView point) const
  {
    assert(point.size() == mDims);
    return assignPoint(_impl::asEigen<Eigen::Array>(point));
  }

  void getMeans(RealMatrixView out) const
  {
    if (mTrained) out <<= _impl::asFluid(mMeans);
  }

  void setMeans(RealMatrixView means)
  {
    mMeans = _impl::asEigen<Eigen::Array>(means);
    mDims = mMeans.cols();
    mK = mMeans.rows();
    mEmpty = std::vector<bool>(asUnsigned(mK), false);
    mTrained = true;
  }

  index dims() const { return mMeans.cols(); }
  index size() const { return mMeans.rows(); }
  index getK() const { return mMeans.rows(); }
  index nAssigned() const { return mAssignments.size(); }

  void getAssignments(FluidTensorView<index, 1> out) const
  {
    out <<= _impl::asFluid(mAssignments);
  }

  void transform(RealMatrixView data, RealMatrixView out) const
  {
    Eigen::ArrayXXd points = _impl::asEigen<Eigen::Array>(data);
    Eigen::ArrayXXd D = fluid::algorithm::DistanceMatrix(points, 2);
    Eigen::MatrixXd means = mMeans.matrix();
    D = fluid::algorithm::DistanceMatrix<Eigen::ArrayXXd>(points, mMeans, 2);
    out <<= _impl::asFluid(D);
  }

protected:
  double distance(const Eigen::ArrayXd&  v1, const Eigen::ArrayXd& v2) const
  {
    return (v1 - v2).matrix().norm();
  }

  index assignPoint(Eigen::ArrayXd point) const
  {
    double minDistance = std::numeric_limits<double>::infinity();
    index  minK;
    for (index k = 0; k < mK; k++)
    {
      double dist = distance(point, mMeans.row(k));
      if (dist < minDistance)
      {
        minK = k;
        minDistance = dist;
      }
    }
    return minK;
  }

  Eigen::VectorXi assignClusters(Eigen::ArrayXXd dataPoints) const
  {
    Eigen::VectorXi assignments = Eigen::VectorXi::Zero(dataPoints.rows());
    for (index i = 0; i < dataPoints.rows(); i++)
    { assignments(i) = static_cast<int>(assignPoint(dataPoints.row(i))); }
    return assignments;
  }

  void computeMeans(Eigen::ArrayXXd dataPoints)
  {
    using namespace Eigen;
    for (index k = 0; k < mK; k++)
    {
      if (mEmpty[asUnsigned(k)]) continue;
      std::vector<index> kAssignment;
      for (index i = 0; i < mAssignments.size(); i++)
      {
        if (mAssignments(i) == k) kAssignment.push_back(i);
      }
      if (kAssignment.size() == 0)
      {
        std::cout << "Warning: empty cluster" << std::endl;
        mEmpty[asUnsigned(k)] = true;
        return;
      }
      ArrayXXd clusterPoints =
          ArrayXXd::Zero(asSigned(kAssignment.size()), mDims);
      for (index i = 0; asUnsigned(i) < kAssignment.size(); i++)
      { clusterPoints.row(i) = dataPoints.row(kAssignment[asUnsigned(i)]); }
      ArrayXd mean = clusterPoints.colwise().mean();
      mMeans.row(k) = mean;
    }
  }

  bool changed(Eigen::VectorXi newAssignments) const
  {
    auto dif = (newAssignments - mAssignments).cwiseAbs().sum();
    return dif > 0;
  }

  index             mK{0};
  index             mDims{0};
  Eigen::ArrayXXd   mMeans;
  std::vector<bool> mEmpty;
  Eigen::VectorXi   mAssignments;
  bool              mTrained{false};
};
} // namespace algorithm
} // namespace fluid


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "Sampling.h"
#include "params.h"
#include "FFT.h"
#include "Random.h"
#include "Algebra.h"

using namespace std;
using namespace NTL;

const ZZX phi = Cyclo();

//==============================================================================
//Generates from parameters N and q :
// - a public key : polynomial h
// - a private key : polynomials f,g,F,G
//==============================================================================
void Keygen(ZZ_pX& PublicKey, ZZX* PrivateKey)
{
    ZZ SqNorm;
    ZZX f,g,F,G;

    SqNorm = conv<ZZ>(1.36*q0/2);

    GenerateBasis(f, g, F, G, SqNorm);
    PrivateKey[0] = f;
    PrivateKey[1] = g;
    PrivateKey[2] = F;
    PrivateKey[3] = G;

    for(unsigned int i=0; i<4; i++)
    {
            PrivateKey[i].SetLength(N0);
    }

    PublicKey = Quotient(f, g);
}

//==============================================================================
//Computes the private basis B from private key PrivateKey and parameter N
//==============================================================================
void CompletePrivateKey(mat_ZZ& B, const ZZX * const PrivateKey)
{
    ZZX f,g,F,G;
    f = PrivateKey[0];
    g = PrivateKey[1];
    F = PrivateKey[2];
    G = PrivateKey[3];

    f = -f;
    F = -F;

    B = BasisFromPolynomials(g, f, G, F);
}

void GPV(RR_t * v, const RR_t * const c, const RR_t s, const MSK_Data * const MSKD)
{

    int i;
    unsigned j;
    RR_t ci[2*N0], zi, cip, sip, aux;

    for(j=0; j<2*N0;j++)
    {
        ci[j] = c[j];
    }

//    for(j=0; j<2*N0; j++)
//    {
//
//    }

    for(i=2*N0-1; i>=0; i--)
    {
        aux = (MSKD->GS_Norms)[i];
        cip = DotProduct(ci, MSKD->Bstar[i])/(aux*aux);
        sip = s/aux;
        zi = Sample4(cip, sip*PiPrime);

        for(j=0; j<2*N0; j++)
        {
            ci[j] -= zi*(MSKD->B)[i][j];
        }
    }

    for(j=0; j<2*N0; j++)
    {
        v[j] = c[j] - ci[j];
    }

}

//==============================================================================
//==============================================================================
//                            MAIN PROGRAMS
//==============================================================================
//==============================================================================

void CompleteMSK(MSK_Data * MSKD, ZZX * MSK)
{
    unsigned int i, j;
    mat_ZZ B0;

    for(i=0; i<4; i++)
    {
        MSKD->PrK[i] = MSK[i];
        ZZXToFFT(MSKD->PrK_fft[i], MSK[i]);
    }

    CompletePrivateKey(B0, MSK);

    for(i=0; i<2*N0; i++)
    {
        for(j=0; j<2*N0; j++)
        {
            MSKD->B[i][j] = ( (RR_t) conv<double>(B0[i][j]) );
        }
    }

    for(i=0; i<1; i++)
    {
        FastMGS(MSKD->Bstar, MSKD->B);
    }

    for(i=0; i<2*N0; i++)
    {
        MSKD->GS_Norms[i] = sqrt( DotProduct(MSKD->Bstar[i], MSKD->Bstar[i]) );
    }

    MSKD->sigma = 2*MSKD->GS_Norms[0];

}

void CompleteMPK(MPK_Data * MPKD, ZZ_pX MPK)
{
    MPKD->h = MPK;
    ZZXToFFT(MPKD->h_FFT, conv<ZZX>(MPK));
}

void PEKS_Trapdoor(ZZX SK_tr[2], vec_ZZ kw, const MSK_Data * const MSKD)
{
    unsigned int i;
    RR_t c[2*N0], sk[2*N0], sigma;
    ZZX f,g,aux;

    f = MSKD -> PrK[0];
    g = MSKD -> PrK[1];
    sigma = MSKD->sigma;
    SK_tr[0].SetLength(N0);
    SK_tr[1].SetLength(N0);

    for(i=0;i<N0;i++)
    {
        c[i] = ((RR_t) conv<double>(kw[i])) ;
        c[i+N0] = 0;
    }

    GPV(sk, c, sigma, MSKD);

    for(i=0; i<N0; i++)
    {
        sk[i] = c[i] - sk[i];
        sk[i+N0] = - sk[i+N0];
    }

    for(i=0; i<N0; i++)
    {
        SK_tr[0][i] = sk[i];
        SK_tr[1][i] = sk[i+N0];
    }

}

unsigned long PEKS_Verify_Trapdoor(const ZZX SK_tr[2], const vec_ZZ kw, const MSK_Data * const MSKD)
{
    unsigned int i;
    ZZX f,g,t,aux;

    f = MSKD -> PrK[0];
    g = MSKD -> PrK[1];

    t = conv<ZZX>(kw);
    aux = ((SK_tr[0] - t)*f + g*SK_tr[1])%phi;

    for(i=0; i<N0; i++)
    {
        aux[i] %= q1;
    }

    if( IsZero(aux) != 0)
    {
        cout << "The signature (s1,s2) doesn't verify the required equality [ (s1 - t)*f + g*s2 = 0 ] !\nActually, (s1 - t)*f + g*s2 = " << aux << endl << endl;
    }
    return IsZero(aux);
}

void PEKS_Enc(long C[3][N0],  const long id0[N0],  const MPK_Data * const MPKD)
{

    unsigned long i;
    long r[N0], e1[N0], e2[N0];
    CC_t r_FFT[N0], t_FFT[N0], aux1_FFT[N0], aux2_FFT[N0];

    for(i=0; i<N0; i++)
    {
        e1[i] = (rand()%3) - 1;
        e2[i] = (rand()%3) - 1;
        r[i] = (rand()%3) - 1;
        C[2][i] = (rand()%2);
    }

    MyIntFFT(r_FFT, r);
    MyIntFFT(t_FFT, id0);

    for(i=0; i<N0; i++)
    {
        aux1_FFT[i] = r_FFT[i]*((MPKD->h_FFT)[i]);
        aux2_FFT[i] = r_FFT[i]*t_FFT[i];
    }

    MyIntReverseFFT(C[0], aux1_FFT);
    MyIntReverseFFT(C[1], aux2_FFT);

    for(i=0; i<N0; i++)
    {
        C[0][i] = (C[0][i] + e1[i]               + q0/2)%q0 - (q0/2);
        C[1][i] = (C[1][i] + e2[i] + (q0/2)*C[2][i] + q0/2)%q0 - (q0/2);
    }

}

bool PEKS_Test( const long C[3][N0], const CC_t * const SKtd_FFT)
{
    unsigned int i;
    CC_t c0_FFT[N0], aux_FFT[N0];
    bool fout = false ;
    long k[N0];
    MyIntFFT(c0_FFT, C[0]);

    for(i=0; i<N0; i++)
    {
        aux_FFT[i] = c0_FFT[i]*SKtd_FFT[i];
    }

    MyIntReverseFFT(k, aux_FFT);

    for(i=0; i<N0; i++)
    {
        k[i] = C[1][i] - k[i];
        k[i] = ((unsigned long)(k[i] ))%q0;
        k[i] = (k[i] + (q0>>2) )/(q0>>1);
        k[i] %= 2;
        if (C[2][i]== k[i])
            fout = true;

        else
            fout = false;
     }
   /* if (fout)
        cout << endl<< endl<<endl <<"  TEST algorithm has been succesful"<< endl; */
    return fout;

}

//==============================================================================
//==============================================================================
//                             BENCHES AND TESTS
//                   FOR EXTRACTION AND ENCRYPTION/DECRYPTION
//==============================================================================
//==============================================================================

void Trapdoor_Bench(const unsigned int nb_extr, MSK_Data * MSKD)
{
    clock_t t1, t2;
    float diff;
    unsigned int i;
    vec_ZZ kw;
    ZZX SK_tr[2];

    t1 = clock();

    cout << "0%" << flush;
    for(i=0; i<nb_extr; i++)
    {
        kw = RandomVector();

        PEKS_Trapdoor(SK_tr, kw, MSKD);
        if((i+1)%(nb_extr/10)==0)
        {
            cout << "..." << (i+1)/(nb_extr/10) << "0%" << flush;
        }
    }

    t2 = clock();
    diff = ((float)t2 - (float)t1)/1000000.0F;
    cout << "\n\nIt took " << diff << " seconds to create  " << nb_extr << " trapdoors." << endl;
    cout << "That's " << (diff/nb_extr)*1000 << " milliseconds per trapdoor." << endl << endl;
}

void Encrypt_Bench(const unsigned int nb_cryp, MPK_Data * MPKD, MSK_Data * MSKD)
{
    clock_t te1, te2, td1, td2;
    float diffe, diffd;
    unsigned int i,j;
    vec_ZZ kw;
    ZZX SK_tr[2], w;
    CC_t SKid_FFT[N0];
   // long int message[N0], decrypted[N0];
    long int keyword[N0], Ciphertext[3][N0];
	//bool flag = true;

    kw = RandomVector();
    PEKS_Trapdoor(SK_tr, kw, MSKD);
    PEKS_Verify_Trapdoor(SK_tr, kw, MSKD);
    ZZXToFFT(SKid_FFT, SK_tr[1]);

    for(i=0; i<N0; i++)
    {
        keyword[i] = conv<long int>(kw[i]);
    }

    cout << "0%" << flush ;
    for(i=0; i<nb_cryp; i++)
    {

   /*     for(j=0; j<N0; j++)
        {
            message[j] = (rand()%2);
        }*/
/*		for(j=0; j<N0; j++)
        {
             cout << " "<< j<<"th "<< message[j] << "\t"; ;
        }*/
	te1 = clock();
		PEKS_Enc(Ciphertext, keyword, MPKD);
	te2 = clock();

	td1 = clock();
		if (!PEKS_Test(Ciphertext, SKid_FFT)){
			 cout << "TEST FAILED --- Exiting..."<<endl;
			break;
			}

	td2 = clock();

        if((i+1)%(nb_cryp/10)==0)
        {
            cout << "..." << (i+1)/(nb_cryp/10) << "0%" << flush;
        }

	diffe += ((float)te2 - (float)te1)/1000000.0l;
	diffd += ((float)td2 - (float)td1)/1000000.0l;

    }

    cout << "\n\nIt took " << diffe << " seconds to do " << nb_cryp << " encryptions." << endl;
    cout << "That's " << (diffe/nb_cryp)*1000 << " milliseconds per PEKS generation." << endl;
    cout << "That's " << (diffe/nb_cryp)*1000*1024/N0 << " milliseconds per PEKS per Kilobit." << endl << endl;

    cout << "\n\nIt took " << diffd << " seconds to do " << nb_cryp << " Tests." << endl;
    cout << "That's " << (diffd/nb_cryp)*1000 << " milliseconds per Tests." << endl;
    cout << "That's " << (diffd/nb_cryp)*1000*1024/N0 << " milliseconds per Tests per Kilobit." << endl << endl;

}

void Trapdoor_Test(const unsigned int nb_extr, MSK_Data * MSKD)
{
    unsigned int i, rep;
    vec_ZZ kw;
    ZZX SK_kw[2];

    rep = 0;

    cout << "0%" << flush;
    for(i=0; i<nb_extr; i++)
    {
        kw = RandomVector();

        PEKS_Trapdoor(SK_kw, kw, MSKD);
        rep += PEKS_Verify_Trapdoor(SK_kw, kw, MSKD);
        if((i+1)%(nb_extr/10)==0)
        {
            cout << "..." << (i+1)/(nb_extr/10) << "0%" << flush;
        }
    }

    cout << endl;
    if(rep == 0)
    {    cout << endl << nb_extr << " Trapdoor successfully performed!" << endl << endl;    }
    else
    {    cout << endl << rep << " out of " << nb_extr << " extractions failed miserabily!" << endl << endl;    }
}

void Encrypt_Test(const unsigned int nb_cryp, MPK_Data * MPKD, MSK_Data * MSKD)
{
    unsigned int i, j, rep;
    vec_ZZ kw;
    ZZX SK_td[2], m;
    CC_t SKtd_FFT[N0];
    long int kw0[N0], Ciphertext[2][N0];
   // long int message[N0], decrypted[N0];

    kw = RandomVector();
    PEKS_Trapdoor(SK_td, kw, MSKD);
    PEKS_Verify_Trapdoor(SK_td, kw, MSKD);
    ZZXToFFT(SKtd_FFT, SK_td[1]);

    rep = 0;

    for(i=0; i<N0; i++)
    {
        kw0[i] = conv<long int>(kw[i]);

    }

    cout << "0%" << flush;
    for(i=0; i<nb_cryp; i++)
    {

       /* for(j=0; j<N0; j++)
        {
            message[j] = (rand()%2);
        }*/

		PEKS_Enc(Ciphertext, kw0, MPKD);
        PEKS_Test(Ciphertext, SKtd_FFT);

/*        for(j=0; j<N0; j++)
        {
            if(message[j] != decrypted[j])
            {
                cout << "ERROR : Dec(Enc(m)) != m " << endl;
                rep++;
                break;
            }
        }*/

        if((i+1)%(nb_cryp/10)==0)
        {
            cout << "..." << (i+1)/(nb_cryp/10) << "0%" << flush;
        }
    }

    cout << endl;
    if(rep == 0)
    {    cout << endl << nb_cryp << " PEKS+TEST successfully performed!" << endl << endl;    }
    else
    {    cout << endl << rep << " out of " << nb_cryp << " PEKS+TEST failed miserabily!" << endl << endl;    }
}


#include <Eigen/Dense>
#include "Common/Common.h"
#include <assert.h>

namespace Eigen
{
// https://stackoverflow.com/questions/25389480/how-to-write-read-an-eigen-matrix-from-binary-file
template <class Matrix>
void write_binary(const std::string filename, const Matrix &matrix)
{
    std::ofstream out(filename, std::ios::out | std::ios::binary | std::ios::trunc);
    typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
    out.write((char *)(&rows), sizeof(typename Matrix::Index));
    out.write((char *)(&cols), sizeof(typename Matrix::Index));
    out.write((char *)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
    out.close();
}
template <class Matrix>
void read_binary(const std::string filename, Matrix &matrix)
{
    std::ifstream in(filename, std::ios::in | std::ios::binary);
    typename Matrix::Index rows = 0, cols = 0;
    in.read((char *)(&rows), sizeof(typename Matrix::Index));
    in.read((char *)(&cols), sizeof(typename Matrix::Index));
    matrix.resize(rows, cols);
    in.read((char *)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
    in.close();
}
} // namespace Eigen

class TrajectoryData
{
public:
    TrajectoryData(std::vector<Eigen::Isometry3d> _transformations, Eigen::VectorXd _dt_inv)
    {
        transformations = _transformations;
        dt_inv = _dt_inv;
    }

    TrajectoryData(std::string path_prefix)
    {
        // Read trajectory from directory
        const int num_rows = 30;

        transformations.resize(num_rows);
        for (int i = 0; i < num_rows; i++)
        {
            // std::cout << i << std::endl;
            Eigen::read_binary(path_prefix + std::to_string(i) + ".dat", transformations[i].matrix());
            // std::cout<<transformations[i].translation()<<std::endl;
        }

        Eigen::read_binary(path_prefix + "dt.dat", dt_inv);
        // std::cout << dt_inv << std::endl;
        assert(dt_inv.cols() == transformations.size());
    }

    void step(Real currentTime)
    {
        idx++;
        Real dt = (idx >= dt_inv.size()) ? 1e8 : 1 / dt_inv[idx];
        nextUpdateTime += dt;

        std::cout << "STEP:" << idx << "/" << dt_inv.size() << std::endl;
        std::cout << "CUR:" << currentTime << " NEXT:" << nextUpdateTime << std::endl;
        std::cout << transformations[idx].translation() << std::endl
                  << std::endl;
    }

    bool needUpdate(Real currentTime)
    {
        return currentTime >= nextUpdateTime;
    }

    void transform(Vector3r &pos)
    {
        assert(idx < dt_inv.size());
        pos = transformations[idx] * pos;
    }

    void reset()
    {
        idx = 0;
        nextUpdateTime = 0;
    }

    std::vector<Eigen::Isometry3d> transformations;
    Eigen::VectorXd dt_inv;

private:
    int idx = 0;
    Real nextUpdateTime = 0;
};


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/log.hpp
 *
 * \brief Compute the natural logarithm to each element of a vector or matrix
 *  expression.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 *
 * <hr/>
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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_NUMERIC_UBLASX_OPERATION_LOG_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOG_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_log_functor_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef vector_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_log_functor_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef matrix_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

/// Auxiliary function used to replace ::std::log2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T log(T x)
{
    return ::std::log(x);
}

} // Namespace detail

/**
 * \brief Applies the \c std::log function to each element of a given vector
 *  expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::log to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_log_functor_traits<VectorExprT>::result_type log(vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_log_functor_traits<VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_log_functor_traits<VectorExprT>::signature_result_type signature_result_type;

    return expression_type(ve(), detail::log<signature_result_type>);
}

/**
 * \brief Applies the \c std::log function to each element of a given matrix
 *  expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::log to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_log_functor_traits<MatrixExprT>::result_type log(matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_log_functor_traits<MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_log_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

    return expression_type(me(), detail::log<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG_HPP


#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>

int main(int argc, char* argv[])
{
    typedef boost::geometry::model::d2::point_xy<double> point;

    boost::geometry::model::polygon<point> polygon;

    boost::geometry::read_wkt(
        "POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
        "(4.0 2.0, 4.2 1.4, 4.8 1.9, 4.4 2.2, 4.0 2.0))", polygon);

    boost::geometry::model::box<point> box;
    boost::geometry::envelope(polygon, box);

    std::cout << "envelope:" << boost::geometry::dsv(box) << std::endl;

	return 0;
}


//////////////////////////////////////////////////////////////////////////////////
// random::poisson_ex::poisson_devroye::detail::q_function::using_factorial.hpp //
//                                                                              //
//                                                                              //
//  (C) Copyright 2010 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_Q_FUNCTION_USING_FACTORIAL_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_Q_FUNCTION_USING_FACTORIAL_ER_2010
#include <cmath>
#include <boost/mpl/bool.hpp>
#include <boost/math/special_functions/factorials.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace q{

namespace factorial_method{
    template<typename T,typename Int,typename P,typename IntT>
    T fun(const Int& i_mean,const Int& i_y,const P& p,const IntT& converter);
}

    struct using_factorial{ using_factorial(){} };

    template<typename T,typename Int,typename P,typename IntT>
    T fun(const Int& i_mean,const Int& i_y,const P& p,
        const IntT& converter,using_factorial/*method*/)
    {
        return factorial_method::fun<T>(i_mean,i_y,p,converter);
    }

namespace factorial_method{

    template<typename T,typename Int,typename P,typename IntT>
    T fun(const Int& i_mean,const Int& i_y,const P& p,const IntT& converter)
    {
        T r = IntT::convert( 0 );
        if(i_y!=0){
            T m = IntT::convert( i_mean );
            T y = IntT::convert( i_y );
            T mp1 = IntT::convert( i_mean + 1 );
            r += first_half(m,y,p);
            if(i_y<0) r += second_half(m,i_y,p,boost::mpl::bool_<false>());
            if(i_y>0) r += second_half(mp1,i_y,p,boost::mpl::bool_<true>());
        }
        return r;
    }

    template<typename T,typename P>
    T first_half(const T& m,const T& y,const P& p){ return y * log( m ); }

    template<typename T,typename Int,typename P>
    T second_half(const T& mp1,const Int& i_y,const P& p,
        boost::mpl::bool_<true>/*y>0*/)
    {
        BOOST_ASSERT(i_y>0);
        using namespace boost::math;
        return -log( rising_factorial<T>(mp1,i_y,p) );
    }
    template<typename T,typename Int,typename P>
    T second_half(const T& m,const Int& i_y,const P& p,
         boost::mpl::bool_<false>/*y>0*/)
    {
        BOOST_ASSERT(i_y<0);
        using namespace boost::math;
        return log( falling_factorial<T>(m,-i_y,p) );
    }

}// factorial_method
}// q
}// devroye
}// poisson
}// random
}// boost

#endif


//
// Sprout C++ Library
//
// Copyright (c) 2013
// bolero-MURAKAMI : http://d.hatena.ne.jp/boleros/
// osyo-manga : http://d.hatena.ne.jp/osyo-manga/
//
// Readme:
// https://github.com/bolero-MURAKAMI/Sprout/blob/master/README
//
// License:
// Boost Software License - Version 1.0
// <http://www.boost.org/LICENSE_1_0.txt>
//
#include <sprout/random.hpp>
#include <iostream>
// #include <boost/mpl/print.hpp>

int
main(){
	static constexpr sprout::uniform_int_distribution<int> dist(100, 999);

	{
		static constexpr auto seed = 2013;
		static constexpr sprout::default_random_engine engine(seed);

		static_assert(engine() == 33832491, "");
		static_assert(dist(engine) == 114, "");
	}

	{
		static constexpr auto seed = 8379842;
		static constexpr sprout::default_random_engine engine(seed);

		static_assert(engine() == 1253567439, "");
		static_assert(dist(engine) == 625, "");
	}

	//
	// Compile time unique seed
	//
	{
		static constexpr auto seed = SPROUT_UNIQUE_SEED;
		std::cout << seed << std::endl;

		static constexpr sprout::default_random_engine engine(seed);
		std::cout << engine() << std::endl;
		std::cout << dist(engine) << std::endl;

		// compile time output
// 		typedef boost::mpl::print<boost::mpl::int_<seed>>::type unique_seed_type;
// 		typedef boost::mpl::print<boost::mpl::int_<engine()>>::type engine_type;
// 		typedef boost::mpl::print<boost::mpl::int_<dist(engine)>>::type dist_type;
	}

	return 0;
}


#include <mass.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(mass);

BOOST_AUTO_TEST_CASE(tetrahedron)
{
  double const rho  =  1.0;

  double const X1   = -1.0;
  double const Y1   =  0.0;
  double const Z1   = -1.0;
  double const X2   =  1.0;
  double const Y2   =  0.0;
  double const Z2   = -1.0;
  double const X3   =  0.0;
  double const Y3   = -1.0;
  double const Z3   =  1.0;
  double const X4   =  0.0;
  double const Y4   =  1.0;
  double const Z4   =  1.0;

  mass::Properties<double> P = mass::compute_tetrahedron(
                            rho
                            ,X1,Y1,Z1
                            ,X2,Y2,Z2
                            ,X3,Y3,Z3
                            ,X4,Y4,Z4
                            );

  BOOST_CHECK_CLOSE(P.m_x, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(P.m_y, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(P.m_z, 0.0, 0.01 );
  BOOST_CHECK(P.m_m  > 0.0);
  BOOST_CHECK(P.m_Ixx > 0.0);
  BOOST_CHECK(P.m_Iyy > 0.0);
  BOOST_CHECK(P.m_Izz > 0.0);
  BOOST_CHECK_CLOSE(P.m_Ixy, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(P.m_Ixz, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(P.m_Iyz, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(P.m_Ixx, P.m_Iyy, 0.01 );
  BOOST_CHECK(P.m_Izz < P.m_Ixx);

  BOOST_CHECK(   P.is_body_space() );
  BOOST_CHECK(  !P.is_model_space() );
}

BOOST_AUTO_TEST_SUITE_END();


/*
	File: StateSpace.hpp
	Description: A Class for creating state space calculations

	TODO: Finish full state
		  Write autonomous

*/
#ifndef STATE_SPACE_CONTROL_H
#define STATE_SPACE_CONTROL_H

#include <Eigen>
#include <iostream>

class StateSpace{
private:
	int states;
	int inputs;
	int outputs;
	int type;

    Eigen::MatrixXf systemMatrix;
    Eigen::MatrixXf inputMatrix;
    Eigen::MatrixXf outputMatrix;
    Eigen::MatrixXf transmissionMatrix;
    Eigen::MatrixXf controlInputs;
    Eigen::MatrixXf referenceInputs;
    Eigen::MatrixXf controlGain;
    Eigen::MatrixXf integralGain;
    Eigen::MatrixXf precompensator;
    Eigen::MatrixXf compensator;
	Eigen::MatrixXf estimatorOutput;

	//Set Actual State & Output
	Eigen::MatrixXf actual;
	Eigen::MatrixXf	realOutput;

public:
	StateSpace(int state, int input, int output, int type);
	void Initialise(Eigen::MatrixXf A, Eigen::MatrixXf B, Eigen::MatrixXf C, Eigen::MatrixXf K);  // Sets Values of matrices to values defined in the main
	Eigen::MatrixXf Calculate();	//Determines the output of the system and updates new values

	//Get Functions
	Eigen::MatrixXf getControlInputs();
	Eigen::MatrixXf getReferenceInputs();
	Eigen::MatrixXf getSystemMatrix();
	Eigen::MatrixXf getInputMatrix();
	Eigen::MatrixXf getOutputMatrix();
	Eigen::MatrixXf getTransmissionMatrix();
	Eigen::MatrixXf getControlGain();
	Eigen::MatrixXf getIntegralGain();
	Eigen::MatrixXf getPrecompensator();
	Eigen::MatrixXf getEstimatorOutput();
	Eigen::MatrixXf getActual();
	Eigen::MatrixXf getRealOutput();

	//Set Functions
	void setControlInputs(Eigen::MatrixXf X);
	void setReferenceInputs(Eigen::MatrixXf X);
	void setSystemMatrix(Eigen::MatrixXf X);
	void setInputMatrix(Eigen::MatrixXf X);
	void setOutputMatrix(Eigen::MatrixXf X);
	void setTransmissionMatrix(Eigen::MatrixXf X);
	void setControlGain(Eigen::MatrixXf X);
	void setIntegralGain(Eigen::MatrixXf X);
	void setPrecompensator(Eigen::MatrixXf X);
	void setEstimatorOutput(Eigen::MatrixXf X);
	void setActual(Eigen::MatrixXf X);
	void setRealOutput(Eigen::MatrixXf X);

};
#endif // STATE_SPACE_CONTROL_H


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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_GIL_EXTENSION_NUMERIC_AFFINE_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_AFFINE_HPP

#include <boost/gil/point.hpp>

namespace boost {
namespace gil {

////////////////////////////////////////////////////////////////////////////////////////
///
/// Simple matrix to do 2D affine transformations. It is actually 3x3 but the
/// last column is [0 0 1]
///
////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class matrix3x2 {
public:
  matrix3x2() : a(1), b(0), c(0), d(1), e(0), f(0) {}
  matrix3x2(T A, T B, T C, T D, T E, T F)
      : a(A), b(B), c(C), d(D), e(E), f(F) {}
  matrix3x2(const matrix3x2 &mat)
      : a(mat.a), b(mat.b), c(mat.c), d(mat.d), e(mat.e), f(mat.f) {}
  matrix3x2 &operator=(const matrix3x2 &m) {
    a = m.a;
    b = m.b;
    c = m.c;
    d = m.d;
    e = m.e;
    f = m.f;
    return *this;
  }

  matrix3x2 &operator*=(const matrix3x2 &m) {
    (*this) = (*this) * m;
    return *this;
  }

  static matrix3x2 get_rotate(T rads) {
    T c = std::cos(rads);
    T s = std::sin(rads);
    return matrix3x2(c, s, -s, c, 0, 0);
  }
  static matrix3x2 get_translate(point<T> const &t) {
    return matrix3x2(1, 0, 0, 1, t.x, t.y);
  }
  static matrix3x2 get_translate(T x, T y) {
    return matrix3x2(1, 0, 0, 1, x, y);
  }
  static matrix3x2 get_scale(point<T> const &s) {
    return matrix3x2(s.x, 0, 0, s.y, 0, 0);
  }
  static matrix3x2 get_scale(T x, T y) { return matrix3x2(x, 0, 0, y, 0, 0); }
  static matrix3x2 get_scale(T s) { return matrix3x2(s, 0, 0, s, 0, 0); }

  T a, b, c, d, e, f;
};

template <typename T>
BOOST_FORCEINLINE matrix3x2<T> operator*(const matrix3x2<T> &m1,
                                         const matrix3x2<T> &m2) {
  return matrix3x2<T>(m1.a * m2.a + m1.b * m2.c, m1.a * m2.b + m1.b * m2.d,
                      m1.c * m2.a + m1.d * m2.c, m1.c * m2.b + m1.d * m2.d,
                      m1.e * m2.a + m1.f * m2.c + m2.e,
                      m1.e * m2.b + m1.f * m2.d + m2.f);
}

template <typename T, typename F>
BOOST_FORCEINLINE point<F> operator*(point<T> const &p, matrix3x2<F> const &m) {
  return {m.a * p.x + m.c * p.y + m.e, m.b * p.x + m.d * p.y + m.f};
}

////////////////////////////////////////////////////////////////////////////////////////
/// Define affine mapping that transforms the source coordinates by the affine
/// transformation
////////////////////////////////////////////////////////////////////////////////////////
/*
template <typename MapFn>
concept MappingFunctionConcept {
    typename mapping_traits<MapFn>::result_type;   where
PointNDConcept<result_type>;

    template <typename Domain> { where PointNDConcept<Domain> }
    result_type transform(MapFn&, const Domain& src);
};
*/

template <typename T> struct mapping_traits;

template <typename F> struct mapping_traits<matrix3x2<F>> {
  using result_type = point<F>;
};

template <typename F, typename F2>
BOOST_FORCEINLINE point<F> transform(matrix3x2<F> const &mat,
                                     point<F2> const &src) {
  return src * mat;
}

} // namespace gil
} // namespace boost

#endif


#pragma once
#include <cstdint>
#include <cassert>
#include <algorithm>
#include <map>
#include <boost/dynamic_bitset.hpp>
//#include "BitOperations.h"
#include "../Basis/AbstractBasis1D.hpp"

template<typename UINT>
class TIXXXJ1J2
{
private:
	const edlib::AbstractBasis<UINT>& basis_;
	double J1_;
	double J2_;

	int sign_ = 1;

public:
	TIXXXJ1J2(const edlib::AbstractBasis<UINT>& basis, double J1, double J2,
			bool signRule = false)
		: basis_(basis), J1_(J1), J2_(J2)
	{
		if(signRule)
			sign_ = -1;
	}

	std::map<int,double> getCol(UINT n) const
	{
		int N = basis_.getN();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(N, a);

		std::map<int, double> m;
		for(unsigned int i = 0; i < N; i++)
		{
			//Nearest neighbors
			{
				unsigned int j = (i+1)%N;
				int sgn = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J1_*sgn;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J1_*(1-sgn)*sign_*coeff;
			}
			//Next-nearest neighbors
			{
				unsigned int j = (i+2)%N;
				int sgn = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J2_*sgn;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J2_*(1-sgn)*coeff;
			}
		}
		return m;
	}
};


#ifndef Jabc_HPP_
#define Jabc_HPP_

#include <armadillo>
using namespace arma;

void transform_xlogx_mat(cx_mat &X);
void transform_xlogx_vec(vec &s);
int log2_int(int n);
void apply_modular_op(int k, int n, cx_mat &psi);
void transform_ab(int a, int b, int c, cx_mat &psi);
void transform_bc(int a, int b, int c, cx_mat &psi);
double Jabc(int a, int b, int c, cx_mat &psi);

#endif // Jabc_HPP_


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_ARITHMETIC_FUNCTIONS_COMPLEX_GENERIC_FMA_HPP_INCLUDED
#define NT2_ARITHMETIC_FUNCTIONS_COMPLEX_GENERIC_FMA_HPP_INCLUDED

#include <nt2/arithmetic/functions/fma.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/if_zero_else.hpp>
#include <nt2/include/functions/if_allbits_else.hpp>
#include <nt2/include/functions/is_invalid.hpp>
#include <nt2/include/functions/is_real.hpp>
#include <nt2/include/functions/is_imag.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/plus.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/seladd.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <boost/simd/sdk/config.hpp>

// for optimize
#include <nt2/include/functions/fnma.hpp>

namespace boost { namespace simd { namespace ext
{

  //0 ccc
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_<complex_<arithmetic_<A0> > > )
                              (generic_<complex_<arithmetic_<A1> > > )
                              (generic_<complex_<arithmetic_<A2> > > )
    )
  {
    typedef A0 result_type;
    typedef typename nt2::meta::as_real<A0>::type r_type;
    NT2_FUNCTOR_CALL(3)
    {
      r_type r = fma(-nt2::imag(a1), nt2::imag(a0), fma(nt2::real(a0), nt2::real(a1), nt2::real(a2)));
      r_type i = fma(nt2::real(a1), nt2::imag(a0), fma(nt2::real(a0), nt2::imag(a1), nt2::imag(a2)));
      return checkr(a0, a1, a2, result_type(r, i));
    }

    static inline result_type checkr(const A0& a0,  const A1& a1,  const A2& a2, const result_type& res)
    {
#ifdef BOOST_SIMD_NO_INVALIDS
      return res;
#else
      if(nt2::any(is_invalid(res)))
      {
        result_type z = nt2::multiplies(a0, a1);
        z+=a2;
        return if_else(is_invalid(res), z, res);
      }
      return res;
#endif

    }
  };

  //8 acc
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_<arithmetic_<A0> > )
                              (generic_<complex_<arithmetic_<A1> > > )
                              (generic_<complex_<arithmetic_<A2> > > )
    )
  {
    typedef A1 result_type;
    typedef typename nt2::meta::as_real<A0>::type r_type;
    NT2_FUNCTOR_CALL(3)
    {
      r_type r = fma(a0, nt2::real(a1), nt2::real(a2));
      r_type i = fma(a0, nt2::imag(a1), nt2::imag(a2));
      return checkr(a0, a1, a2, r, i);
    }
    static inline result_type checkr(const A0& a0, const A1& a1, const A2& a2, r_type& r, r_type& i)
    {
#ifdef BOOST_SIMD_NO_INVALIDS
      return result_type(r, i);
#else
      r =  if_else(is_invalid(a0),
                   if_else(is_real(a1),
                           r,
                           if_else(nt2::is_imag(a1), nt2::real(a2),r)),
                   r
        );
      i =  if_else(is_invalid(a0),
                   if_else(is_real(a1), nt2::imag(a2), i),
                   i);
      return result_type(r, i);
#endif
    }
  };

  //9 cac
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_<complex_<arithmetic_<A0> > > )
                              (generic_<arithmetic_<A1> > )
                              (generic_<complex_<arithmetic_<A2> > > )
    )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(3)
    {
      return fma(a1, a0, a2);
    }
  };

  //10 cca
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_<complex_<arithmetic_<A0> > > )
                              (generic_<complex_<arithmetic_<A1> > > )
                              (generic_<arithmetic_<A2> > )
    )
  {
    typedef A0 result_type;
    typedef typename nt2::meta::as_real<A0>::type r_type;
    NT2_FUNCTOR_CALL(3)
    {
      r_type r =fma(-nt2::imag(a1), nt2::imag(a0), fma(nt2::real(a1), nt2::real(a0), a2));
      r_type i = fma(nt2::real(a1), nt2::imag(a0), nt2::real(a0)*nt2::imag(a1));
      return checkr(a0, a1, a2, r, i);

    }
    static inline result_type checkr(const A0& a0, const A1& a1, const A2& a2, const r_type& r, const r_type& i)
    {
#ifdef BOOST_SIMD_NO_INVALIDS
      return result_type(r, i);
#else
      BOOST_AUTO_TPL(test, nt2::logical_and(nt2::logical_and(nt2::is_real(a1), nt2::is_real(a0)), nt2::is_nez(i)));
      BOOST_AUTO_TPL(testr, nt2::logical_and(logical_or(nt2::is_eqz(a1), is_eqz(a0)),
                                             logical_and(logical_and(is_imag(a0),
                                                                     is_imag(a1)),
                                                         nt2::is_invalid(r))));
      return result_type(if_else(testr, a2, r), nt2::if_zero_else(test, i));
#endif

    }
  };

  //11 caa
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_<complex_<arithmetic_<A0> > > )
                              (generic_< arithmetic_<A1>  > )
                              (generic_< arithmetic_<A2>  > )
    )
  {
    typedef A0 result_type;
    typedef typename nt2::meta::as_real<A0>::type r_type;
    NT2_FUNCTOR_CALL(3)
    {
      r_type r = fma(nt2::real(a0), a1, a2);
      r_type i = nt2::imag(a0)*a1;
      return checkr(a0, r, i);
    }
    static inline result_type checkr(const A0& a0, const r_type& r, const r_type& i)
    {
#ifdef BOOST_SIMD_NO_INVALIDS
      return result_type(r, i);
#else
      BOOST_AUTO_TPL(test, nt2::logical_and(nt2::is_real(a0), nt2::is_nez(i)));
      return result_type(r, nt2::if_zero_else(test, i));
#endif
    }
  };

  //12 aca
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_< arithmetic_<A0>  > )
                              (generic_<complex_<arithmetic_<A1> > > )
                              (generic_< arithmetic_<A2>  > )
    )
  {
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(3)
    {
      return fma(a1, a0, a2);
    }
  };

  //13 aac
  BOOST_DISPATCH_IMPLEMENT  ( fma_, tag::cpu_, (A0)(A1)(A2)
                              , (generic_<arithmetic_<A0>  > )
                              (generic_<arithmetic_<A1>  > )
                              (generic_<complex_<arithmetic_<A2> > > )
    )
  {
    typedef A2 result_type;
    NT2_FUNCTOR_CALL(3)
    {
      return result_type( fma(a0, a1, nt2::real(a2))
                          , nt2::imag(a2)
        );
    }
  };

 } } }

#endif


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

#ifndef BOOST_MATH_COMPLEX_ATANH_INCLUDED
#define BOOST_MATH_COMPLEX_ATANH_INCLUDED

#ifndef BOOST_MATH_COMPLEX_DETAILS_INCLUDED
#  include <boost/math/complex/details.hpp>
#endif
#ifndef BOOST_MATH_LOG1P_INCLUDED
#  include <boost/math/special_functions/log1p.hpp>
#endif
#include <boost/assert.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; using ::fabs; using ::acos; using ::asin; using ::atan; using ::atan2; }
#endif

namespace boost{ namespace math{

template<class T>
std::complex<T> atanh(const std::complex<T>& z)
{
   //
   // References:
   //
   // Eric W. Weisstein. "Inverse Hyperbolic Tangent."
   // From MathWorld--A Wolfram Web Resource.
   // http://mathworld.wolfram.com/InverseHyperbolicTangent.html
   //
   // Also: The Wolfram Functions Site,
   // http://functions.wolfram.com/ElementaryFunctions/ArcTanh/
   //
   // Also "Abramowitz and Stegun. Handbook of Mathematical Functions."
   // at : http://jove.prohosting.com/~skripty/toc.htm
   //
   // See also: https://svn.boost.org/trac/boost/ticket/7291
   //

   static const T pi = boost::math::constants::pi<T>();
   static const T half_pi = pi / 2;
   static const T one = static_cast<T>(1.0L);
   static const T two = static_cast<T>(2.0L);
   static const T four = static_cast<T>(4.0L);
   static const T zero = static_cast<T>(0);
   static const T log_two = boost::math::constants::ln_two<T>();

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif

   T x = std::fabs(z.real());
   T y = std::fabs(z.imag());

   T real, imag;  // our results

   T safe_upper = detail::safe_max(two);
   T safe_lower = detail::safe_min(static_cast<T>(2));

   //
   // Begin by handling the special cases specified in C99:
   //
   if((boost::math::isnan)(x))
   {
      if((boost::math::isnan)(y))
         return std::complex<T>(x, x);
      else if((boost::math::isinf)(y))
         return std::complex<T>(0, ((boost::math::signbit)(z.imag()) ? -half_pi : half_pi));
      else
         return std::complex<T>(x, x);
   }
   else if((boost::math::isnan)(y))
   {
      if(x == 0)
         return std::complex<T>(x, y);
      if((boost::math::isinf)(x))
         return std::complex<T>(0, y);
      else
         return std::complex<T>(y, y);
   }
   else if((x > safe_lower) && (x < safe_upper) && (y > safe_lower) && (y < safe_upper))
   {

      T yy = y*y;
      T mxm1 = one - x;
      ///
      // The real part is given by:
      //
      // real(atanh(z)) == log1p(4*x / ((x-1)*(x-1) + y^2))
      //
      real = boost::math::log1p(four * x / (mxm1*mxm1 + yy));
      real /= four;
      if((boost::math::signbit)(z.real()))
         real = (boost::math::changesign)(real);

      imag = std::atan2((y * two), (mxm1*(one+x) - yy));
      imag /= two;
      if(z.imag() < 0)
         imag = (boost::math::changesign)(imag);
   }
   else
   {
      //
      // This section handles exception cases that would normally cause
      // underflow or overflow in the main formulas.
      //
      // Begin by working out the real part, we need to approximate
      //    real = boost::math::log1p(4x / ((x-1)^2 + y^2))
      // without either overflow or underflow in the squared terms.
      //
      T mxm1 = one - x;
      if(x >= safe_upper)
      {
         // x-1 = x to machine precision:
         if((boost::math::isinf)(x) || (boost::math::isinf)(y))
         {
            real = 0;
         }
         else if(y >= safe_upper)
         {
            // Big x and y: divide through by x*y:
            real = boost::math::log1p((four/y) / (x/y + y/x));
         }
         else if(y > one)
         {
            // Big x: divide through by x:
            real = boost::math::log1p(four / (x + y*y/x));
         }
         else
         {
            // Big x small y, as above but neglect y^2/x:
            real = boost::math::log1p(four/x);
         }
      }
      else if(y >= safe_upper)
      {
         if(x > one)
         {
            // Big y, medium x, divide through by y:
            real = boost::math::log1p((four*x/y) / (y + mxm1*mxm1/y));
         }
         else
         {
            // Small or medium x, large y:
            real = four*x/y/y;
         }
      }
      else if (x != one)
      {
         // y is small, calculate divisor carefully:
         T div = mxm1*mxm1;
         if(y > safe_lower)
            div += y*y;
         real = boost::math::log1p(four*x/div);
      }
      else
         real = boost::math::changesign(two * (std::log(y) - log_two));

      real /= four;
      if((boost::math::signbit)(z.real()))
         real = (boost::math::changesign)(real);

      //
      // Now handle imaginary part, this is much easier,
      // if x or y are large, then the formula:
      //    atan2(2y, (1-x)*(1+x) - y^2)
      // evaluates to +-(PI - theta) where theta is negligible compared to PI.
      //
      if((x >= safe_upper) || (y >= safe_upper))
      {
         imag = pi;
      }
      else if(x <= safe_lower)
      {
         //
         // If both x and y are small then atan(2y),
         // otherwise just x^2 is negligible in the divisor:
         //
         if(y <= safe_lower)
            imag = std::atan2(two*y, one);
         else
         {
            if((y == zero) && (x == zero))
               imag = 0;
            else
               imag = std::atan2(two*y, one - y*y);
         }
      }
      else
      {
         //
         // y^2 is negligible:
         //
         if((y == zero) && (x == one))
            imag = 0;
         else
            imag = std::atan2(two*y, mxm1*(one+x));
      }
      imag /= two;
      if((boost::math::signbit)(z.imag()))
         imag = (boost::math::changesign)(imag);
   }
   return std::complex<T>(real, imag);
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
}

} } // namespaces

#endif // BOOST_MATH_COMPLEX_ATANH_INCLUDED


//
// Created by a.kiryanenko on 3/26/20.
//

#include "../SpuUltraGraphAdapter.h"
#include "../SpuUltraGraphProperty.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include "GraphPerformanceTest.h"

using namespace SPU_GRAPH;
using namespace boost;

typedef boost::adjacency_list <
        boost::vecS, // как хранить вершины - в векторе
        boost::vecS, // как хранить ребра из каждой вершины - в векторе
        boost::directedS,
        no_property,
        property < edge_weight_t, int >
> AdjacencyListGraph;

template <class G>
pair<typename graph_traits<G>::edge_descriptor, bool>
add_weight_edge(typename graph_traits<G>::vertex_descriptor u, typename graph_traits<G>::vertex_descriptor v, G& g) {
    auto weight = rand() % 16;
    return add_edge(u, v, weight, g);
}

template <>
pair<typename graph_traits<SpuUltraGraph>::edge_descriptor, bool>
add_weight_edge(typename graph_traits<SpuUltraGraph>::vertex_descriptor u, typename graph_traits<SpuUltraGraph>::vertex_descriptor v, SpuUltraGraph& g) {
    auto weight = rand() % 16;
    return {g.add_edge(g.get_free_edge_descriptor(weight), u, v), true};
}

template <class G>
void dijkstra_test(G &g) {
    typedef typename graph_traits<G>::vertex_descriptor vertex_t;

    // Создаю свойство предшественник для вершин
    map<vertex_t, vertex_t> vertex_to_predecessor;
    associative_property_map<map<vertex_t, vertex_t>> predecessor_property_map(vertex_to_predecessor);
    // Создаю свойство расстояние для вершин
    map<vertex_t, size_t> vertex_to_distance;
    associative_property_map<map<vertex_t, size_t>> distance_property_map(vertex_to_distance);

    // Выполняю алгоритм дейкстра для подсчета расстояний от вершины #1 до остальных
    dijkstra_shortest_paths_no_color_map(g, 1, predecessor_map(predecessor_property_map).distance_map(distance_property_map));
}

int main()
{
    cout << "SpuUltraGraph performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<SpuUltraGraph> spu_graph_test(dijkstra_test, "dijkstra_test_SpuUltraGraph.csv");
    spu_graph_test.is_mutable_test = false;
    spu_graph_test.add_edge_func = add_weight_edge;
    spu_graph_test.start();

    cout << "adjacency_list performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<AdjacencyListGraph> adjacency_list_test(dijkstra_test, "dijkstra_test_adjacency_list.csv");
    adjacency_list_test.is_mutable_test = false;
    adjacency_list_test.add_edge_func = add_weight_edge;
    adjacency_list_test.start();

    cout << "adjacency_matrix performance test" << endl;
    cout << "==========================================" << endl;
    GraphPerformanceTest<AdjacencyMatrixGraph> adjacency_matrix_test(dijkstra_test, "dijkstra_test_adjacency_matrix.csv");
    adjacency_matrix_test.is_mutable_test = false;
    adjacency_matrix_test.add_edge_func = add_weight_edge;
    adjacency_matrix_test.end_vertices_cnt = 25000;
    adjacency_matrix_test.start();
    return 0;
}


/*
 [auto_generated]
 libs/numeric/odeint/examples/molecular_dynamics.cpp

 [begin_description]
 Molecular dynamics example.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

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

#include <boost/numeric/odeint.hpp>

#include <vector>
#include <iostream>
#include <random>

using namespace boost::numeric::odeint;

using namespace std;
#define tab "\t"

const size_t n1 = 16;
const size_t n2 = 16;

struct md_system
{
    static const size_t n = n1 * n2;
    typedef std::vector< double > vector_type;

    md_system( double a = 0.0 ,            // strength of harmonic oscillator
               double gamma = 0.0   ,       // friction
               double eps = 0.1 ,          // interaction strenght
               double sigma = 1.0 ,            // interaction radius
               double xmax = 150.0 , double ymax = 150.0 )
    : m_a( a ) , m_gamma( gamma )
    , m_eps( eps ) , m_sigma( sigma )
    , m_xmax( xmax ) , m_ymax( ymax )
    { }

    static void init_vector_type( vector_type &x ) { x.resize( 2 * n ); }

    void operator()( vector_type const& x , vector_type const& v , vector_type &a , double t ) const
    {
        for( size_t i=0 ; i<n ; ++i )
        {
            double diffx = x[i] - 0.5 * m_xmax , diffy = x[i+n] - 0.5 * m_ymax;
            double r2 = diffx * diffx + diffy * diffy ;
            double r = std::sqrt( r2 );
            a[     i ] = - m_a * r * diffx - m_gamma * v[     i ] ;
            a[ n + i ] = - m_a * r * diffy - m_gamma * v[ n + i ] ;
        }

        for( size_t i=0 ; i<n ; ++i )
        {
            double xi = x[i] , yi = x[n+i];
            xi = periodic_bc( xi , m_xmax );
            yi = periodic_bc( yi , m_ymax );
            for( size_t j=0 ; j<i ; ++j )
            {
                double xj = x[j] , yj = x[n+j];
                xj = periodic_bc( xj , m_xmax );
                yj = periodic_bc( yj , m_ymax );

                double diffx = ( xj - xi ) , diffy = ( yj - yi );
                double r = sqrt( diffx * diffx + diffy * diffy );
                double f = lennard_jones( r );
                a[     i ] += diffx / r * f;
                a[ n + i ] += diffy / r * f;
                a[     j ] -= diffx / r * f;
                a[ n + j ] -= diffy / r * f;
            }
        }
    }

    void bc( vector_type &x )
    {
        for( size_t i=0 ; i<n ; ++i )
        {
            x[ i     ] = periodic_bc( x[ i     ] , m_xmax );
            x[ i + n ] = periodic_bc( x[ i + n ] , m_ymax );
        }
    }

    inline double lennard_jones( double r ) const
    {
        double c = m_sigma / r;
        double c3 = c * c * c;
        double c6 = c3 * c3;
        return 4.0 * m_eps * ( -12.0 * c6 * c6 / r + 6.0 * c6 / r );
    }

    static inline double periodic_bc( double x , double xmax )
    {
        return ( x < 0.0 ) ? x + xmax : ( x > xmax ) ? x - xmax : x ;
    }

    double m_a;
    double m_gamma;
    double m_eps ;
    double m_sigma ;
    double m_xmax , m_ymax;
};

int main( int argc , char *argv[] )
{
    const size_t n = md_system::n;
    typedef md_system::vector_type vector_type;

    std::mt19937 rng;
    std::normal_distribution<> dist( 0.0 , 1.0 );

    vector_type x , v;
    md_system::init_vector_type( x );
    md_system::init_vector_type( v );

    for( size_t i=0 ; i<n1 ; ++i )
    {
        for( size_t j=0 ; j<n2 ; ++j )
        {
            x[i*n2+j  ] = 5.0 + i * 4.0 ;
            x[i*n2+j+n] = 5.0 + j * 4.0 ;
            v[i]   = dist( rng ) ;
            v[i+n] = dist( rng ) ;
        }
    }

    velocity_verlet< vector_type > stepper;
    const double dt = 0.025;
    double t = 0.0;
    md_system sys;
    for( size_t oi=0 ; oi<100000 ; ++oi )
    {
        for( size_t ii=0 ; ii<100 ; ++ii,t+=dt )
            stepper.do_step( sys , std::make_pair( std::ref( x ) , std::ref( v ) ) , t , dt );
        sys.bc( x );

        std::cout << "set size square" << "\n";
        std::cout << "unset key" << "\n";
        std::cout << "p [0:" << sys.m_xmax << "][0:" << sys.m_ymax << "] '-' pt 7 ps 0.5" << "\n";
        for( size_t i=0 ; i<n ; ++i )
            std::cout << x[i] << " " << x[i+n] << " " << v[i] << " " << v[i+n] << "\n";
        std::cout << "e" << std::endl;
    }

    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <rrt/StateSpace.hpp>

namespace RRT {

/**
 * @brief A 2d plane with continuous states and no obstacles.
 */
template <class POINT_CLASS = Eigen::Vector2d>
class PlaneStateSpace : public StateSpace<POINT_CLASS> {
public:
    PlaneStateSpace(double width, double height)
        : _width(width), _height(height) {}

    POINT_CLASS randomState() const {
        return POINT_CLASS(drand48() * width(), drand48() * height());
    }

    POINT_CLASS intermediateState(const POINT_CLASS& source,
                                  const POINT_CLASS& target,
                                  double stepSize) const {
        POINT_CLASS delta = target - source;
        delta = delta / delta.norm();  //  unit vector

        POINT_CLASS val = source + delta * stepSize;
        return val;
    }

    double distance(const POINT_CLASS& from, const POINT_CLASS& to) const {
        POINT_CLASS delta = from - to;
        return sqrtf(powf(delta.x(), 2) + powf(delta.y(), 2));
    }

    /**
     * Returns a boolean indicating whether the given point is within bounds.
     */
    bool stateValid(const POINT_CLASS& pt) const {
        return pt.x() >= 0 && pt.y() >= 0 && pt.x() < width() &&
               pt.y() < height();
    }

    double width() const { return _width; }
    double height() const { return _height; }

private:
    double _width, _height;
};

}  // namespace RRT


#ifndef CDTree_H
#define CDTree_H

#include <iostream>
#include <fstream>  //读取文件内容
#include <map>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <cmath>

using std::cout; using std::cin; using std::cerr; using std::endl;
using std::string; using std::ifstream; using std::istringstream;
using std::vector; using std::map; using std::ios;

using Eigen::MatrixXf;   //Eigen Matrix float 存储数据  训练数据
using Eigen::MatrixXi;  //Eigen Matrix int 存储数据  训练数据和标签
using Eigen::VectorXi;
using Eigen::VectorXf;

class CDTree
{
public:
    struct TreeNode
    {   //节点信息
        TreeNode* parents;  //当前节点的双亲节点
        int AttributeIndex;   //此节点对应的属性索引
        bool LeafNode; //判断是否为叶子节点
        vector<TreeNode*> children; //孩子节点的地址。
        int label;  //if it is a leaf node then this prameter is useful

    };

    struct IntervalTree
    {
        IntervalTree *leftTree;
        IntervalTree *rightTree;
        bool LeafNode;
        float cutValue;
        IntervalTree *parients;
    };

    vector<int> ColsIndex;  //所有数据的列
    TreeNode* root;
    int deepestTree; // deepest depth of the tree
    int max_bin;   // max number of interval on every  continous parameter's cut
    float thresholdInfoGain; //在计算
    int trainX_Dimension;

	CDTree(int deepestTree, int max_bin, float thresholdInfoGain);
    CDTree();
    int buildTree(const MatrixXf &trainX, const MatrixXi &trainY, const MatrixXf &validateX, const MatrixXi &validateY, string Algorithm);
    int buildTree(const MatrixXf &trainX, const MatrixXi &trainY, string Algorithm);
    MatrixXi predict(const MatrixXf &testX);
    ~CDTree();

private:
    vector<vector<std::pair<float, float>>> intervals;  // 每个属性的区间分割
    vector<vector<std::pair<float, float>>> continous2discrete(const MatrixXf &trainX, const MatrixXi &trainY);
    IntervalTree* MultiwayPartitionGain(IntervalTree *parientsNode, const vector<float> &continuousVec, const MatrixXf &trainX, const MatrixXi &trainY, int index, float thresholdInfoGain);
    int getIntervalsFromTreeStruct(IntervalTree *Intervalroot, vector<float> &intervals);
    int sortVectorXf(const VectorXf &vec, VectorXf &sorted_vec, VectorXi &ind);
    int cutBranches(const MatrixXf &validateX, const MatrixXi &validateY);
    int cutBranches(int deepthTree);
    TreeNode* AlgorithmID3(TreeNode *parients, const MatrixXf &trainX, const MatrixXi &trainY, vector<int> attrrbuteIndexOfCurrentData);
	TreeNode* AlgorithmC4_5(const MatrixXf &trainX, const MatrixXi &trainY);
	TreeNode* AlgorithmCART(const MatrixXf &trainX, const MatrixXi &trainY);
    std::pair<MatrixXf, MatrixXi> splitData(const MatrixXf &trainX, const MatrixXi &trainY, std::pair<float, float> smallInterval, int maxIndex, string flag);
    int FindMaxInformationGain(vector<float> s);
    vector<float> CalculateInfGain(const MatrixXf &trainX, const MatrixXi &trainY, vector<int> attributeIndexOfCurrentData);
    vector<float> getSampleProbability(const MatrixXi &trainY);
    vector<float> removeColRepetitionValue(const MatrixXf &Col);
    MatrixXi findDvlabel(const MatrixXf &trainX, const MatrixXi &trainY, int index, std::pair<float, float> smallInterval, string flag);
    float calculateEntropy(vector<float> probability);
    bool TheSameLabel(const MatrixXi &trainY);
    int chooseMostLabel(const MatrixXi &trainY);
    int findExistandDealwith(int label, vector<std::pair<int, int>> &labelAndNum);
    int destroyTree(TreeNode* root);
    int destroyIntervalTree(IntervalTree *root);

    template<typename Type>
    int KMeans(vector<Type> vec, vector<Type> &newvec, int index);
    int predictTree(TreeNode *node, const MatrixXf testX);

};

#endif