// Copyright 2019, Collabora, Ltd.
// Copyright 2016, Sensics, Inc.
// SPDX-License-Identifier: Apache-2.0
/*!
 * @file
 * @brief  Base implementations for math library.
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_math
 *
 * Based in part on inc/osvr/Util/EigenQuatExponentialMap.h in OSVR-Core
 */

#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <assert.h>

// anonymous namespace for internal types
namespace {
template <typename Scalar> struct FourthRootMachineEps;
template <> struct FourthRootMachineEps<double>
{
	/// machine epsilon is 1e-53, so fourth root is roughly 1e-13
	static double
	get()
	{
		return 1.e-13;
	}
};
template <> struct FourthRootMachineEps<float>
{
	/// machine epsilon is 1e-24, so fourth root is 1e-6
	static float
	get()
	{
		return 1.e-6f;
	}
};
/// Computes the "historical" (un-normalized) sinc(Theta)
/// (sine(theta)/theta for theta != 0, defined as the limit value of 0
/// at theta = 0)
template <typename Scalar>
inline Scalar
sinc(Scalar theta)
{
	/// fourth root of machine epsilon is recommended cutoff for taylor
	/// series expansion vs. direct computation per
	/// Grassia, F. S. (1998). Practical Parameterization of Rotations
	/// Using the Exponential Map. Journal of Graphics Tools, 3(3),
	/// 29-48. http://doi.org/10.1080/10867651.1998.10487493
	Scalar ret;
	if (theta < FourthRootMachineEps<Scalar>::get()) {
		// taylor series expansion.
		ret = Scalar(1.f) - theta * theta / Scalar(6.f);
		return ret;
	}
	// direct computation.
	ret = std::sin(theta) / theta;
	return ret;
}

/// fully-templated free function for quaternion expontiation
template <typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar>
quat_exp(Eigen::MatrixBase<Derived> const &vec)
{
	EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
	using Scalar = typename Derived::Scalar;
	/// Implementation inspired by
	/// Grassia, F. S. (1998). Practical Parameterization of Rotations
	/// Using the Exponential Map. Journal of Graphics Tools, 3(3),
	/// 29–48. http://doi.org/10.1080/10867651.1998.10487493
	///
	/// However, that work introduced a factor of 1/2 which I could not
	/// derive from the definition of quaternion exponentiation and
	/// whose absence thus distinguishes this implementation. Without
	/// that factor of 1/2, the exp and ln functions successfully
	/// round-trip and match other implementations.
	Scalar theta = vec.norm();
	Scalar vecscale = sinc(theta);
	Eigen::Quaternion<Scalar> ret;
	ret.vec() = vecscale * vec;
	ret.w() = std::cos(theta);
	return ret.normalized();
}

/// Taylor series expansion of theta over sin(theta), aka cosecant, for
/// use near 0 when you want continuity and validity at 0.
template <typename Scalar>
inline Scalar
cscTaylorExpansion(Scalar theta)
{
	return Scalar(1) +
	       // theta ^ 2 / 6
	       (theta * theta) / Scalar(6) +
	       // 7 theta^4 / 360
	       (Scalar(7) * theta * theta * theta * theta) / Scalar(360) +
	       // 31 theta^6/15120
	       (Scalar(31) * theta * theta * theta * theta * theta * theta) /
	           Scalar(15120);
}

/// fully-templated free function for quaternion log map.
///
/// Assumes a unit quaternion.
template <typename Scalar>
inline Eigen::Matrix<Scalar, 3, 1>
quat_ln(Eigen::Quaternion<Scalar> const &quat)
{
	// ln q = ( (phi)/(norm of vec) vec, ln(norm of quat))
	// When we assume a unit quaternion, ln(norm of quat) = 0
	// so then we just scale the vector part by phi/sin(phi) to get the
	// result (i.e., ln(qv, qw) = (phi/sin(phi)) * qv )
	Scalar vecnorm = quat.vec().norm();

	// "best for numerical stability" vs asin or acos
	Scalar phi = std::atan2(vecnorm, quat.w());

	// Here is where we compute the coefficient to scale the vector part
	// by, which is nominally phi / std::sin(phi).
	// When the angle approaches zero, we compute the coefficient
	// differently, since it gets a bit like sinc in that we want it
	// continuous but 0 is undefined.
	Scalar phiOverSin = vecnorm < 1e-4 ? cscTaylorExpansion<Scalar>(phi)
	                                   : (phi / std::sin(phi));
	return quat.vec() * phiOverSin;
}

} // namespace

extern "C" void
math_quat_integrate_velocity(const struct xrt_quat *quat,
                             const struct xrt_vec3 *ang_vel,
                             const float dt,
                             struct xrt_quat *result)
{
	assert(quat != NULL);
	assert(ang_vel != NULL);
	assert(result != NULL);
	assert(dt != 0);

	Eigen::Quaternionf q = map_quat(*quat);
	Eigen::Quaternionf incremental_rotation =
	    quat_exp(map_vec3(*ang_vel) * dt * 0.5f).normalized();
	map_quat(*result) = q * incremental_rotation;
}

extern "C" void
math_quat_finite_difference(const struct xrt_quat *quat0,
                            const struct xrt_quat *quat1,
                            const float dt,
                            struct xrt_vec3 *out_ang_vel)
{
	assert(quat0 != NULL);
	assert(quat1 != NULL);
	assert(out_ang_vel != NULL);
	assert(dt != 0);

	Eigen::Quaternionf inc_quat =
	    map_quat(*quat1) * map_quat(*quat0).conjugate();
	map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat);
}


#include <iostream>
#include <cassert>
#include <cmath>

#include <Eigen/Dense>

#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

/*!
 * There are exactly ten ways of selecting three from five, 12345:
 *
 *   123, 124, 125, 134, 135, 145, 234, 245, 345
 *
 * In combinatorics, we use the notation, 5c3 = 10.
 *
 * In generall, ncr = n! / r!*(n - r)!, where r <= n.
 *
 * It is not until n = 23 that a value exceeds one-million: 23c10 = 1144066.
 *
 * How many, not necessarily distinct, values of ncr for 1 <= n <= 100, are
 * greater than one-million.
 */

unsigned int compute_num_combinations_over_million() {
  unsigned int num_greater = 0;
  MatrixXd pascal = MatrixXd::Zero(101, 101);
  pascal.col(0) = VectorXd::Ones(101);

  for (unsigned int n = 1; n <= 100; ++n) {
    for (unsigned int r = 1; r <= n; ++r) {
      pascal(n, r) = pascal(n - 1, r) + pascal(n - 1, r -1);

      if (pascal(n, r) > 1000000) {
        pascal(n, r) = 1000000;
        ++num_greater;
      }
    }
  }

  return num_greater;
}

int main(int argc, char **argv) {
  std::cout << compute_num_combinations_over_million() << std::endl;
}


// combination_iterator.hpp
//
// Produces all k-combinations of the universe {0,...,n-1} in
// lexicographical order. The algorithm is a tuned-up version
// of a simple generation method ("Algorithm T") described in:
//
//   Knuth, Donald E.
//   The Art of Computer Programming, Volume 4A: Combinatorial Algorithms,
//   Part 1. Pearson Education Inc., 2011.

#ifndef COMBINATION_ITERATOR_HPP
#define COMBINATION_ITERATOR_HPP

// This code was produced by Juho Lauri (euler314).

#include <cassert>
#include <cstdint>
#include <numeric>
#include <vector>

#include <boost/iterator/iterator_facade.hpp>

template <typename T>
class combination_iterator
    : public boost::iterator_facade<combination_iterator<T>, const std::vector<T>&, boost::forward_traversal_tag>
{
public:
    combination_iterator() : comb_() {}

    explicit combination_iterator(T n, T k)
        : end_(false), n_(n), k_(k), comb_(k)
    {
        assert(k != 0 && n_ > k);
        std::iota(comb_.begin(), comb_.end(), 0);
        assert(!end_);
    }

private:
    friend class boost::iterator_core_access;

    void increment()
    {
        std::int64_t j = k_ - 1;

        for (const T end = n_ - k_; j >= 0 && comb_[j] >= end + j; --j) {}

        if (j < 0)
        {
            assert(comb_.front() == n_ - k_);
            end_ = true;
            return;
        }

        ++comb_[j];

        for (const std::int64_t end = k_ - 1; j < end; ++j)
        {
            comb_[j + 1] = comb_[j] + 1;
        }
    }

    bool equal(const combination_iterator& other) const
    {
        return end_ == other.end_;
    }

    const std::vector<T>& dereference() const { return comb_; }

    bool end_{true};
    const int n_{0};
    const int k_{0};
    std::vector<T> comb_;
};

template <typename T>
class combination_iterator_minimax_order
    : public boost::iterator_facade<combination_iterator_minimax_order<T>,
                                    const std::vector<T>&,
                                    boost::forward_traversal_tag>
{
public:
    combination_iterator_minimax_order() : comb_() {}

    explicit combination_iterator_minimax_order(T n, T k)
        : end_(false), n_(n), k_(k), hint_(k), comb_(k)
    {
        assert(k != 0 && n_ > k);
        std::iota(comb_.begin(), comb_.end(), 0);
        assert(!end_);
    }

private:
    friend class boost::iterator_core_access;

    void increment()
    {
        // The following code was copied from the discreture library:
        // http://github.com/mraggi/discreture
        if (k_ == 0)
            return;

        if (hint_ > 0)
        {
            --hint_;
            ++comb_[hint_];
            return;
        }

        T i = 0;
        for (const T last = comb_.size() - 1;
             (i < last) && (comb_[i] + 1 == comb_[i + 1]);
             ++i)
        {
            comb_[i] = i;
        }

        ++comb_[i];

        if (comb_[i] == n_)
            end_ = true;
        else
            hint_ = i;
    }

    bool equal(const combination_iterator_minimax_order& other) const
    {
        return end_ == other.end_;
    }

    const std::vector<T>& dereference() const { return comb_; }

    bool end_{true};
    const int n_{0};
    const int k_{0};
    int hint_{};
    std::vector<T> comb_;
};

#endif


#include <Eigen/Core>
#include <cmath>
#include <fstream>
#include <iostream>
#include <mathtoolbox/gaussian-process-regression.hpp>
#include <random>
#include <string>
#include <timer.hpp>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
    std::random_device                     seed;
    std::default_random_engine             engine(seed());
    std::uniform_real_distribution<double> uniform_dist(0.0, 1.0);
    std::normal_distribution<double>       normal_dist(0.0, 1.0);

    double CalculateFunction(double x) { return x * std::sin(10.0 * x); }
} // namespace

int main(int argc, char** argv)
{
    // Set a output directory path
    const std::string output_directory_path = (argc < 2) ? "." : argv[1];

    // Define the scene setting
    constexpr int    number_of_samples = 20;
    constexpr double noise_intensity   = 0.010;

    // Generate (and export) scattered data
    std::ofstream scattered_data_stream(output_directory_path + "/scattered_data.csv");
    scattered_data_stream << "x,y" << std::endl;
    MatrixXd X(1, number_of_samples);
    VectorXd y(number_of_samples);
    for (int i = 0; i < number_of_samples; ++i)
    {
        X(0, i) = uniform_dist(engine);
        y(i)    = CalculateFunction(X(0, i)) + noise_intensity * normal_dist(engine);

        scattered_data_stream << X(0, i) << "," << y(i) << std::endl;
    }
    scattered_data_stream.close();

    // Define the kernel type
    const auto kernel_type = mathtoolbox::GaussianProcessRegressor::KernelType::ArdMatern52;

    // Instantiate the interpolation object
    mathtoolbox::GaussianProcessRegressor regressor(X, y, kernel_type);

    // Perform hyperparameter estimation
    const Eigen::Vector2d default_kernel_hyperparams{0.50, 0.50};
    {
        timer::Timer t("maximum likelihood estimation");
        regressor.PerformMaximumLikelihood(default_kernel_hyperparams, 0.010);
    }

    // Define constants for export
    constexpr int    resolution       = 200;
    constexpr double percentile_point = 1.95996398454005423552;

    // Calculate (and export) predictive distribution
    std::ofstream estimated_data_stream(output_directory_path + "/estimated_data.csv");
    estimated_data_stream << "x,mean,standard deviation,95-percent upper,95-percent lower" << std::endl;
    for (int i = 0; i <= resolution; ++i)
    {
        const double x = (1.0 / static_cast<double>(resolution)) * i;
        const double y = regressor.PredictMean(VectorXd::Constant(1, x));
        const double s = regressor.PredictStdev(VectorXd::Constant(1, x));

        estimated_data_stream << x << "," << y << "," << s << "," << y + percentile_point * s << ","
                              << y - percentile_point * s << std::endl;
    }
    estimated_data_stream.close();

    return 0;
}


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

#ifndef BOOST_HYPERGEOMETRIC_1F1_RECURRENCE_HPP_
#define BOOST_HYPERGEOMETRIC_1F1_RECURRENCE_HPP_

#include <boost/math/special_functions/modf.hpp>
#include <boost/math/special_functions/next.hpp>

#include <boost/math/tools/recurrence.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>

  namespace boost { namespace math { namespace detail {

  // forward declaration for initial values
  template <class T, class Policy>
  inline T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol);

  template <class T, class Policy>
  inline T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling);

  template <class T>
  struct hypergeometric_1F1_recurrence_a_coefficients
  {
    typedef boost::math::tuple<T, T, T> result_type;

    hypergeometric_1F1_recurrence_a_coefficients(const T& a, const T& b, const T& z):
    a(a), b(b), z(z)
    {
    }

    result_type operator()(boost::intmax_t i) const
    {
      const T ai = a + i;

      const T an = b - ai;
      const T bn = (2 * ai - b + z);
      const T cn = -ai;

      return boost::math::make_tuple(an, bn, cn);
    }

  private:
    const T a, b, z;
    hypergeometric_1F1_recurrence_a_coefficients operator=(const hypergeometric_1F1_recurrence_a_coefficients&);
  };

  template <class T>
  struct hypergeometric_1F1_recurrence_b_coefficients
  {
    typedef boost::math::tuple<T, T, T> result_type;

    hypergeometric_1F1_recurrence_b_coefficients(const T& a, const T& b, const T& z):
    a(a), b(b), z(z)
    {
    }

    result_type operator()(boost::intmax_t i) const
    {
      const T bi = b + i;

      const T an = bi * (bi - 1);
      const T bn = bi * (1 - bi - z);
      const T cn = z * (bi - a);

      return boost::math::make_tuple(an, bn, cn);
    }

  private:
    const T a, b, z;
    hypergeometric_1F1_recurrence_b_coefficients& operator=(const hypergeometric_1F1_recurrence_b_coefficients&);
  };
  //
  // for use when we're recursing to a small b:
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_small_b_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_small_b_coefficients(const T& a, const T& b, const T& z, int N) :
        a(a), b(b), z(z), N(N)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        const T bi = b + (i + N);
        const T bi_minus_1 = b + (i + N - 1);

        const T an = bi * bi_minus_1;
        const T bn = bi * (-bi_minus_1 - z);
        const T cn = z * (bi - a);

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     hypergeometric_1F1_recurrence_small_b_coefficients operator=(const hypergeometric_1F1_recurrence_small_b_coefficients&);
     const T a, b, z;
     int N;
  };

  template <class T>
  struct hypergeometric_1F1_recurrence_a_and_b_coefficients
  {
    typedef boost::math::tuple<T, T, T> result_type;

    hypergeometric_1F1_recurrence_a_and_b_coefficients(const T& a, const T& b, const T& z, int offset = 0):
    a(a), b(b), z(z), offset(offset)
    {
    }

    result_type operator()(boost::intmax_t i) const
    {
      const T ai = a + (offset + i);
      const T bi = b + (offset + i);

      const T an = bi * (b + (offset + i - 1));
      const T bn = bi * (z - (b + (offset + i - 1)));
      const T cn = -ai * z;

      return boost::math::make_tuple(an, bn, cn);
    }

  private:
    const T a, b, z;
    int offset;
    hypergeometric_1F1_recurrence_a_and_b_coefficients operator=(const hypergeometric_1F1_recurrence_a_and_b_coefficients&);
  };
#if 0
  //
  // These next few recurrence relations are archived for future reference, some of them are novel, though all
  // are trivially derived from the existing well known relations:
  //
  // Recurrence relation for double-stepping on both a and b:
  // - b(b-1)(b-2) / (2-b+z) M(a-2,b-2,z) + [b(a-1)z / (2-b+z) + b(1-b+z) + abz(b+1) /(b+1)(z-b)] M(a,b,z) - a(a+1)z^2 / (b+1)(z-b) M(a+2,b+2,z)
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_2a_and_2b_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_2a_and_2b_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        i *= 2;
        const T ai = a + (offset + i);
        const T bi = b + (offset + i);

        const T an = -bi * (b + (offset + i - 1)) * (b + (offset + i - 2)) / (-(b + (offset + i - 2)) + z);
        const T bn = bi * (a + (offset + i - 1)) * z / (z - (b + (offset + i - 2)))
           + bi * (z - (b + (offset + i - 1)))
           + ai * bi * z * (b + (offset + i + 1)) / ((b + (offset + i + 1)) * (z - bi));
        const T cn = -ai * (a + (offset + i + 1)) * z * z / ((b + (offset + i + 1)) * (z - bi));

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_2a_and_2b_coefficients operator=(const hypergeometric_1F1_recurrence_2a_and_2b_coefficients&);
  };

  //
  // Recurrence relation for double-stepping on a:
  // -(b-a)(1 + b - a)/(2a-2-b+z)M(a-2,b,z)  + [(b-a)(a-1)/(2a-2-b+z) + (2a-b+z) + a(b-a-1)/(2a+2-b+z)]M(a,b,z)   -a(a+1)/(2a+2-b+z)M(a+2,b,z)
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_2a_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_2a_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        i *= 2;
        const T ai = a + (offset + i);
        // -(b-a)(1 + b - a)/(2a-2-b+z)
        const T an = -(b - ai) * (b - (a + (offset + i - 1))) / (2 * (a + (offset + i - 1)) - b + z);
        const T bn = (b - ai) * (a + (offset + i - 1)) / (2 * (a + (offset + i - 1)) - b + z) + (2 * ai - b + z) + ai * (b - (a + (offset + i + 1))) / (2 * (a + (offset + i + 1)) - b + z);
        const T cn = -ai * (a + (offset + i + 1)) / (2 * (a + (offset + i + 1)) - b + z);

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_2a_coefficients operator=(const hypergeometric_1F1_recurrence_2a_coefficients&);
  };

  //
  // Recurrence relation for double-stepping on b:
  // b(b-1)^2(b-2)/((1-b)(2-b-z)) M(a,b-2,z)  + [zb(b-1)(b-1-a)/((1-b)(2-b-z)) + b(1-b-z) + z(b-a)(b+1)b/((b+1)(b+z)) ] M(a,b,z) + z^2(b-a)(b+1-a)/((b+1)(b+z)) M(a,b+2,z)
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_2b_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_2b_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        i *= 2;
        const T bi = b + (offset + i);
        const T bi_m1 = b + (offset + i - 1);
        const T bi_p1 = b + (offset + i + 1);
        const T bi_m2 = b + (offset + i - 2);

        const T an = bi * (bi_m1) * (bi_m1) * (bi_m2) / (-bi_m1 * (-bi_m2 - z));
        const T bn = z * bi * bi_m1 * (bi_m1 - a) / (-bi_m1 * (-bi_m2 - z)) + bi * (-bi_m1 - z) + z * (bi - a) * bi_p1 * bi / (bi_p1 * (bi + z));
        const T cn = z * z * (bi - a) * (bi_p1 - a) / (bi_p1 * (bi + z));

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_2b_coefficients operator=(const hypergeometric_1F1_recurrence_2b_coefficients&);
  };

  //
  // Recurrence relation for a+ b-:
  // -z(b-a)(a-1-b)/(b(a-1+z)) M(a-1,b+1,z) + [(b-a)(a-1)b/(b(a-1+z)) + (2a-b+z) + a(b-a-1)/(a+z)] M(a,b,z) + a(1-b)/(a+z) M(a+1,b-1,z)
  //
  // This is potentially the most useful of these novel recurrences.
  //              -                                      -                  +        -                           +
  template <class T>
  struct hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        const T ai = a + (offset + i);
        const T bi = b - (offset + i);

        const T an = -z * (bi - ai) * (ai - 1 - bi) / (bi * (ai - 1 + z));
        const T bn = z * ((-1 / (ai + z) - 1 / (ai + z - 1)) * (bi + z - 1) + 3) + bi - 1;
        const T cn = ai * (1 - bi) / (ai + z);

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients operator=(const hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients&);
  };
#endif

  template <class T, class Policy>
  inline T hypergeometric_1F1_backward_recurrence_for_negative_a(const T& a, const T& b, const T& z, const Policy& pol, const char* function, int& log_scaling)
  {
    BOOST_MATH_STD_USING // modf, frexp, fabs, pow

    boost::intmax_t integer_part = 0;
    T ak = modf(a, &integer_part);
    //
    // We need ak-1 positive to avoid infinite recursion below:
    //
    if (0 != ak)
    {
       ak += 2;
       integer_part -= 2;
    }

    if (-integer_part > static_cast<boost::intmax_t>(policies::get_max_series_iterations<Policy>()))
       return policies::raise_evaluation_error<T>(function, "1F1 arguments sit in a range with a so negative that we have no evaluation method, got a = %1%", std::numeric_limits<T>::quiet_NaN(), pol);

    T first, second;
    if(ak == 0)
    {
       first = 1;
       ak -= 1;
       second = 1 - z / b;
    }
    else
    {
       int scaling1(0), scaling2(0);
       first = detail::hypergeometric_1F1_imp(ak, b, z, pol, scaling1);
       ak -= 1;
       second = detail::hypergeometric_1F1_imp(ak, b, z, pol, scaling2);
       if (scaling1 != scaling2)
       {
          second *= exp(T(scaling2 - scaling1));
       }
       log_scaling += scaling1;
    }
    ++integer_part;

    detail::hypergeometric_1F1_recurrence_a_coefficients<T> s(ak, b, z);

    return tools::apply_recurrence_relation_backward(s,
                                                     static_cast<unsigned int>(std::abs(integer_part)),
                                                     first,
                                                     second, &log_scaling);
  }

  template <class T, class Policy>
  T hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(const T& a, const T& b, const T& z, const Policy& pol, const char*, int& log_scaling)
  {
     using std::swap;
     BOOST_MATH_STD_USING // modf, frexp, fabs, pow
     //
     // We compute
     //
     // M[a + a_shift, b + b_shift; z]
     //
     // and recurse backwards on a and b down to
     //
     // M[a, b, z]
     //
     // With a + a_shift > 1 and b + b_shift > z
     //
     // There are 3 distinct regions to ensure stability during the recursions:
     //
     // a > 0         :  stable for backwards on a
     // a < 0, b > 0  :  stable for backwards on a and b
     // a < 0, b < 0  :  stable for backwards on b (as long as |b| is small).
     //
     // We could simplify things by ignoring the middle region, but it's more efficient
     // to recurse on a and b together when we can.
     //

     BOOST_ASSERT(a < -1); // Not tested nor taken for -1 < a < 0

     int b_shift = itrunc(z - b) + 2;

     int a_shift = itrunc(-a);
     if (a + a_shift != 0)
     {
        a_shift += 2;
     }
     //
     // If the shifts are so large that we would throw an evaluation_error, try the series instead,
     // even though this will almost certainly throw as well:
     //
     if (b_shift > static_cast<boost::intmax_t>(boost::math::policies::get_max_series_iterations<Policy>()))
        return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);

     if (a_shift > static_cast<boost::intmax_t>(boost::math::policies::get_max_series_iterations<Policy>()))
        return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);

     int a_b_shift = b < 0 ? itrunc(b + b_shift) : b_shift;   // The max we can shift on a and b together
     int leading_a_shift = (std::min)(3, a_shift);        // Just enough to make a negative
     if (a_b_shift > a_shift - 3)
     {
        a_b_shift = a_shift < 3 ? 0 : a_shift - 3;
     }
     else
     {
        // Need to ensure that leading_a_shift is large enough that a will reach it's target
        // after the first 2 phases (-,0) and (-,-) are over:
        leading_a_shift = a_shift - a_b_shift;
     }
     int trailing_b_shift = b_shift - a_b_shift;
     if (a_b_shift < 5)
     {
        // Might as well do things in two steps rather than 3:
        if (a_b_shift > 0)
        {
           leading_a_shift += a_b_shift;
           trailing_b_shift += a_b_shift;
        }
        a_b_shift = 0;
        --leading_a_shift;
     }

     BOOST_ASSERT(leading_a_shift > 1);
     BOOST_ASSERT(a_b_shift + leading_a_shift + (a_b_shift == 0 ? 1 : 0) == a_shift);
     BOOST_ASSERT(a_b_shift + trailing_b_shift == b_shift);

     if ((trailing_b_shift == 0) && (fabs(b) < 0.5) && a_b_shift)
     {
        // Better to have the final recursion on b alone, otherwise we lose precision when b is very small:
        int diff = (std::min)(a_b_shift, 3);
        a_b_shift -= diff;
        leading_a_shift += diff;
        trailing_b_shift += diff;
     }

     T first, second;
     int scale1(0), scale2(0);
     first = boost::math::detail::hypergeometric_1F1_imp(T(a + a_shift), T(b + b_shift), z, pol, scale1);
     //
     // It would be good to compute "second" from first and the ratio - unfortunately we are right on the cusp
     // recursion on a switching from stable backwards to stable forwards behaviour and so this is not possible here.
     //
     second = boost::math::detail::hypergeometric_1F1_imp(T(a + a_shift - 1), T(b + b_shift), z, pol, scale2);
     if (scale1 != scale2)
        second *= exp(T(scale2 - scale1));
     log_scaling += scale1;

     //
     // Now we have [a + a_shift, b + b_shift, z] and [a + a_shift - 1, b + b_shift, z]
     // and want to recurse until [a + a_shift - leading_a_shift, b + b_shift, z] and [a + a_shift - leadng_a_shift - 1, b + b_shift, z]
     // which is leading_a_shift -1 steps.
     //
     second = boost::math::tools::apply_recurrence_relation_backward(
        hypergeometric_1F1_recurrence_a_coefficients<T>(a + a_shift - 1, b + b_shift, z),
        leading_a_shift, first, second, &log_scaling, &first);

     if (a_b_shift)
     {
        //
        // Now we need to switch to an a+b shift so that we have:
        // [a + a_shift - leading_a_shift, b + b_shift, z] and [a + a_shift - leadng_a_shift - 1, b + b_shift - 1, z]
        // A&S 13.4.3 gives us what we need:
        //
        {
           // local a's and b's:
           T la = a + a_shift - leading_a_shift - 1;
           T lb = b + b_shift;
           second = ((1 + la - lb) * second - la * first) / (1 - lb);
        }
        //
        // Now apply a_b_shift - 1 recursions to get down to
        // [a + 1, b + trailing_b_shift + 1, z] and [a, b + trailing_b_shift, z]
        //
        second = boost::math::tools::apply_recurrence_relation_backward(
           hypergeometric_1F1_recurrence_a_and_b_coefficients<T>(a, b + b_shift - a_b_shift, z, a_b_shift - 1),
           a_b_shift - 1, first, second, &log_scaling, &first);
        //
        // Now we need to switch to a b shift, a different application of A&S 13.4.3
        // will get us there, we leave "second" where it is, and move "first" sideways:
        //
        {
           T lb = b + trailing_b_shift + 1;
           first = (second * (lb - 1) - a * first) / -(1 + a - lb);
        }
     }
     else
     {
        //
        // We have M[a+1, b+b_shift, z] and M[a, b+b_shift, z] and need M[a, b+b_shift-1, z] for
        // recursion on b: A&S 13.4.3 gives us what we need.
        //
        T third = -(second * (1 + a - b - b_shift) - first * a) / (b + b_shift - 1);
        swap(first, second);
        swap(second, third);
        --trailing_b_shift;
     }
     //
     // Finish off by applying trailing_b_shift recursions:
     //
     if (trailing_b_shift)
     {
        second = boost::math::tools::apply_recurrence_relation_backward(
           hypergeometric_1F1_recurrence_small_b_coefficients<T>(a, b, z, trailing_b_shift),
           trailing_b_shift, first, second, &log_scaling);
     }
     return second;
  }

  } } } // namespaces

#endif // BOOST_HYPERGEOMETRIC_1F1_RECURRENCE_HPP_


#include <armadillo>
#include "GEFE_utility.h"

// Demonstration.
int main() {
    const arma::mat H("-0.2414 0.3160; 0.3160 -0.8649");
    const arma::vec ii("0 1");
    const arma::vec gefe_eigvecs = getEigenvectorFromEigenvalues(H, ii, /*column j=*/0);
    printf("\ngefe:\n");
    gefe_eigvecs.print(); //    0.1488
                          //    0.8512

    arma::vec arma_eigvals;
    arma::mat arma_eigvecs;
    arma::eig_sym(arma_eigvals, arma_eigvecs, H);

    printf("\narma::eig_sym:\n");
    arma_eigvecs = arma::square(arma_eigvecs);
    arma_eigvecs.col(0).print(); //    0.1488
                                         //    0.8512
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_on_sphere_2.h>
#include <CGAL/Projection_on_sphere_traits_3.h>

#include <boost/iterator/transform_iterator.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel          K;

typedef CGAL::Projection_on_sphere_traits_3<K>                       Traits;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Traits>             DToS2;

typedef Traits::Point_3                                              Point_3;

int main(int, char**)
{
  std::vector<Point_3> points;
  points.emplace_back( 3,  1,  1);
  points.emplace_back(-8,  1,  1);
  points.emplace_back( 1,  2,  1);
  points.emplace_back( 1, -2,  1);
  points.emplace_back( 1,  1, 10);

  Traits traits(Point_3(1,1,1)); // radius is 1 by default
  DToS2 dtos(traits);

  Traits::Construct_point_on_sphere_2 cst = traits.construct_point_on_sphere_2_object();

  for(const auto& pt : points)
  {
    std::cout << "----- Inserting (" << pt
              << ") at squared distance " << CGAL::squared_distance(pt, traits.center())
              << " from the center of the sphere" << std::endl;
    dtos.insert(cst(pt));

    std::cout << "The triangulation now has dimension: " << dtos.dimension() << " and\n";
    std::cout << dtos.number_of_vertices() << " vertices" << std::endl;
    std::cout << dtos.number_of_edges() << " edges" << std::endl;
    std::cout << dtos.number_of_faces() << " solid faces" << std::endl;
    std::cout << dtos.number_of_ghost_faces() << " ghost faces" << std::endl;
  }

  CGAL::IO::write_OFF("result.off", dtos, CGAL::parameters::stream_precision(17));

  return EXIT_SUCCESS;
}


#include <pa/stages/ftle_map_generator.hpp>

#include <Eigen/Dense>
#include <tbb/tbb.h>

namespace pa
{
ftle_map_generator::ftle_map_generator(partitioner* partitioner, mode mode) : partitioner_(partitioner), mode_(mode)
{

}

void                          ftle_map_generator::set_flow_map(vector_field* flow_map   )
{
  flow_map_ = flow_map;
}
void                          ftle_map_generator::set_time    (scalar        time       )
{
  time_ = time;
}

std::unique_ptr<scalar_field> ftle_map_generator::generate    ()
{
  auto ftle_map = std::make_unique<scalar_field>();
  ftle_map->data.resize(boost::extents
   [flow_map_->data.shape()[0]]
   [flow_map_->data.shape()[1]]
   [flow_map_->data.shape()[2]]);
  ftle_map->offset  = flow_map_->offset ;
  ftle_map->size    = flow_map_->size   ;
  ftle_map->spacing = flow_map_->spacing;

  auto gradient = flow_map_->gradient();
  tbb::parallel_for(tbb::blocked_range3d<std::size_t>(0, gradient->data.shape()[0], 0, gradient->data.shape()[1], 0, gradient->data.shape()[2]),
    [&] (const tbb::blocked_range3d<std::size_t>& index) {
    for (auto x = index.pages().begin(), x_end = index.pages().end(); x < x_end; ++x) {
    for (auto y = index.rows ().begin(), y_end = index.rows ().end(); y < y_end; ++y) {
    for (auto z = index.cols ().begin(), z_end = index.cols ().end(); z < z_end; ++z) {
      // Compute the spectral norm (Left Cauchy-Green tensor).
      matrix3 spectral_norm = gradient->data[x][y][z].transpose().eval() * gradient->data[x][y][z];

      // Compute eigenvalues and eigenvectors.
      Eigen::SelfAdjointEigenSolver<matrix3> solver(spectral_norm);
      auto eigenvalues  = solver.eigenvalues ();
      auto eigenvectors = solver.eigenvectors();

      // Compute FTLE.
      pa::scalar value;
      if (mode_ == mode::regular)
        value = std::log(std::sqrt(eigenvalues.maxCoeff())) / std::abs(time_);
      else if (mode_ == mode::fractional_anisotropy)
        value = std::sqrt(0.5) *
                std::sqrt(std::pow(eigenvalues[0] - eigenvalues[1], 2) + std::pow(eigenvalues[1] - eigenvalues[2], 2) + std::pow(eigenvalues[2] - eigenvalues[0], 2)) /
                std::sqrt(std::pow(eigenvalues[0], 2) + std::pow(eigenvalues[1], 2) + std::pow(eigenvalues[2], 2));

      ftle_map->data[x][y][z] = value;
  }}}});

  return ftle_map;
}
}


/*
    mvee.cpp
    Desc: minimum volume ellipsoid method
    @author Chris Larson, Cornell University
    @date (2017)
    @version 1.0
*/

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <algorithm>
#include <exception>
#include <string>
#include <map>
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "boost/chrono.hpp"
#include "mvee.hpp"
#include "utils.hpp"

using namespace std;
using namespace Eigen;
using namespace mvee;

Mvee::Mvee() {}

void Mvee::decompose(MatrixXd& A, VectorXd& s, MatrixXd& U, MatrixXd& V)
{
    JacobiSVD<MatrixXd> SVD(A, ComputeThinU | ComputeThinV);
    s = SVD.singularValues();
    V = SVD.matrixV();
    U = SVD.matrixU();
}

/*
    Khachiyan's ellipsoid method
    @param X: matrix of data (n x d)
    @param eps: approximation error (default: 0.001)
    @param lmcoeff: Levenberg-Marquardt coefficient
                    used for inverting ill-condditioned
                    matrices. (default: 1e-8)
    Ref: Khachiyan (1979)
*/
void Mvee::khachiyan(MatrixXd& data, double eps, double lmcoeff)
{
    // Khachiyan's algorithm
    auto t0 = chrono::system_clock::now();
    iters = 0;
    long double err = INFINITY;
    double alpha;
    int n = data.rows();
    int d = data.cols();
    MatrixXd X = data.transpose();
    MatrixXd Q(X.rows() + 1, X.cols());
    Q.row(0) = X.row(0);
    Q.row(1) = X.row(1);
    Q.row(2).setOnes();
    VectorXd u = (1 / (double) n) * VectorXd::Ones(n);
    VectorXd uhat = u;
    MatrixXd G(d + 1, d + 1);
    MatrixXd noiseye = MatrixXd::Identity(G.rows(), G.cols()) * lmcoeff;
    VectorXd g(n);
    double m; int i;
    while (err > eps)
    {
        G = Q * u.asDiagonal() * Q.transpose() + noiseye;
        g = (Q.transpose() * G.inverse() * Q).diagonal();
        m = g.maxCoeff();
        i = findIdx(g, m);
        alpha = (m - d - 1) / ((d + 1) * (m - 1));
        uhat = (1 - alpha) * u;
        uhat(i) += alpha;
        err = (uhat - u).norm();
        u = uhat;
        iters++;
    }
    time = chrono::duration<double>(
        chrono::system_clock::now() - t0
    ).count();

    // Decompose US^2U^T
    MatrixXd E = (1 / (double) d) * (
        X * u.asDiagonal() * X.transpose()
        - (X * u) * (X * u).transpose()
    ).inverse();
    VectorXd x = X * u;
    VectorXd s(d);
    MatrixXd V(d, d);
    MatrixXd U(d, d);
    decompose(E, s, U, V);

    // Cache result
    _centroid = toStdVec(x, [](double z) { return z; });
    _radii    = toStdVec(s, [](double z) { return 1 / sqrt(z); });
    _pose     = toStdMat(V, [](double z) { return z; });
}

/*
    Computes minimum volume ellipse enclosing data in file (path/to/file.csv)
    @param file:string path to .csv file containing data
    @param delim:char delimiter character
    @param eps:double approximation error (default: 0.001)
*/
void Mvee::compute(string file, char delim, double eps, double lmcoeff)
{
    MatrixXd D = readCSV(file, delim);
    khachiyan(D, eps, lmcoeff);
}

/*
    Computes minimum volume ellipse enclosing data
    @param data:vector<vector<double>> data (n x d)
    @param delim:char delimiter character
    @param eps:double approximation error (default: 0.001)
*/
void Mvee::compute(vector<vector<double>>& data, double eps, double lmcoeff)
{
    int rows = data.size();
    int cols = data[0].size();
    MatrixXd D(rows, cols);
    for (int i=0; i<rows; i++)
    {
        for (int j=0; j<cols; j++)
        {
            D(i, j) = data[i][j];
        }
    }
    khachiyan(D, eps, lmcoeff);
}

/*
    Computes minimum volume ellipse enclosing data in file
    @param file:Eigen::MatrixXd data (n x d)
    @param delim:char delimiter character
    @param eps:double approximation error (default: 0.001)
*/
void Mvee::compute(MatrixXd& data, double eps, double lmcoeff)
{
    khachiyan(data, eps, lmcoeff);
}

/*
    Ellipse centroid
    @return centroid:vector<double> (d x 1)
    Note: Invoke mveeInstance.compute() prior to retrieving
            the ellipse parameters.
*/
vector<double> Mvee::centroid()
{
    return _centroid;
}

/*
    Ellipse radii
    @return radii:vector<double> (d x 1)
    Note: Invoke mveeInstance.compute() prior to retrieving
            the ellipse parameters.
*/
vector<double> Mvee::radii()
{
    return _radii;
}

/*
    Ellipse pose (rotation)
    @return pose:vector<vector<double>> (d x d)
    Note: Invoke mveeInstance.compute() prior to retrieving
            the ellipse parameters.
*/
vector<vector<double>> Mvee::pose()
{
    return _pose;
}

Mvee::~Mvee() {}


/**
 * \file boost/numeric/ublasx/operation/logspace.hpp
 *
 * \brief Logarithmically spaced vector
 *
 * Inspired by MATLAB's logspace function.
 *
 * <hr/>
 *
 * Copyright (c) 2015, 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)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LOGSPACE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOGSPACE_HPP

#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/element_pow.hpp>
#include <boost/numeric/ublasx/operation/linspace.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

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

/**
 * \brief Generates a logarithmically spaced vector.
 *
 * Generates \a n values logarithmically equally spaced between `pow(base,a)`
 * and `pow(base,b)`.
 * Note, in case \f$a<b\f$, generates a decreasing sequence.
 *
 * Inspired by MATLAB's logspace function.
 *
 * \param a The starting value of the logarithmically spaced sequence.
 * \param b The final value of the logarithmically spaced sequence.
 * \param n The number of values to generate
 * \param base The base of the logarithm
 * \return A vector of logarithmically spaced values in
 *  \f$[\mathrm{base}^a,\mathrm{base}^b]\f$; if `n=1`, returns `pow(base,b)`.
 *
 * The call `logspace(a,b,n)` is equivalent to the call:
 * ```
 *    base .^ linspace(a,b,n)
 * ```
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename ValueT>
BOOST_UBLAS_INLINE
vector<ValueT> logspace(ValueT a, ValueT b, std::size_t n = 100, ValueT base = 10)
{
	// pre: n > 0
	BOOST_UBLAS_CHECK( n > 0,
					   bad_argument() );
	// pre: base > 0
	BOOST_UBLAS_CHECK( base > 0,
					   bad_argument() );

	return element_pow(base, linspace(a, b, n));
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOGSPACE_HPP


//
//  main.cpp
//  NE - Normal Equations (for regression problem)
//
//  Created by Zhalgas Baibatyr on 1/15/16.
//  Copyright © 2016 Zhalgas Baibatyr. All rights reserved.
//

/*

    DEGREE ----- the degree of a polynomial in regression function
    N_SAMPLES -- number of training samples
    N_FEATURES - number of given input features
    features --- input features, including intercept term (xₒ = 1)
    target ----- target (output) values
    params ----- parameters (weights) with initial values (zeros)
    hypothesis - hypothesis function;

*/

#include <armadillo>

using namespace arma;

int main()
{
    /* Loading initial data: */
    mat initData;
    initData.load("../data/regression1", arma_ascii);

    /* Initializing constants: */
    const uword DEGREE = 1;
    const uword N_SAMPLES  = initData.n_rows;
    const uword N_FEATURES = initData.n_cols - 1;

    /* Transforming input data using polynomial formula: */
    mat features(N_SAMPLES, DEGREE * N_FEATURES + 1);
    features.col(0) = ones<vec>(N_SAMPLES);
    for (int i = 0; i < N_FEATURES; ++i)
    {
        for (int j = 1; j <= DEGREE; ++j)
        {
            features.col(i * DEGREE + j) = pow(initData.col(i), j);
        }
    }

    /* Preparing other data: */
    vec target = initData.col(initData.n_cols - 1);
    vec params = zeros<vec>(features.n_cols);
    vec hypothesis;

    wall_clock timer;
    double elapsedTime;
    timer.tic();

    /* Normal Equations is solved here: */
    params = (features.t() * features).i() * features.t() * target;
    hypothesis = features * params;

    /* Measuring the performance of the algorithm: */
    elapsedTime = timer.toc();
    printf("Elapsed time: %f sec.\n\n", elapsedTime);

    mat outputData = join_rows(initData.cols(0, initData.n_cols - 2), hypothesis);
    outputData.save("outputData", arma_ascii);
    hypothesis.save("hypothesis", arma_ascii);
    params.save("params", arma_ascii);

    return EXIT_SUCCESS;
}


//////////////////////////////////////////////////////////////////////////
// Author       :  Ngo Tien Dat
// Email        :  dat.ngo@epfl.ch
// Organization :  EPFL
// Purpose      :  Linear algebra library
// Date         :  15 March 2012
//////////////////////////////////////////////////////////////////////////

#include "LinearAlgebraUtils.h"
#include <armadillo>

using namespace arma;

mat LinearAlgebraUtils::SolveWithConstraints (const mat& A, const mat& B, const mat& Ac, const mat& Bc)
{
  mat P, Q;
  makeLinearParam(Ac, Bc, P, Q); // Given Ac * X = Bc, we represent X = PY + Q

  // Solution of the least square problem
  return P * solve(A*P, B-A*Q) + Q;
}

void LinearAlgebraUtils::makeLinearParam(const mat& A, const mat& B, mat& P, mat& Q)
{
  int m = A.n_rows;
  int n = A.n_cols;

  Q = solve(A, B);

  // P is an orthonormal basis for the null space of A
  mat U, V;
  vec s;
  svd(U, s, V, A);

  double tolerance = std::max(m, n) * max(s) * math::eps();

  uvec c = s > tolerance;
  int nR = sum(c);

  if (nR <= n-1)
    P = V.cols(nR, n-1);
  else
    P.set_size(V.n_rows, 0);
}

mat LinearAlgebraUtils::PseudoInverse(const mat& A, double lambda)
{
  int m = A.n_rows;
  int n = A.n_cols;

  if (m > n)
  {
    if (lambda > 0)
    {
      return solve(A.t()*A + lambda*eye(n,n), A.t());    // solve(A, B) = A \ B
    }
    else
    {
      return solve(A.t()*A, A.t());
    }
  }
  else
  {
    if (lambda > 0)
    {
      // We want to compute this: A' / ( A*A' + lambda*eye(n,n) )
      // Using relation: A / B = (B' \ A')'
      mat B = A*A.t() + lambda*eye(m,m);    // B' = B, since B is symmetric
      mat C = solve(B, A);

      return C.t();
    }
    else
    {
      // We want to compute: A' / (A*A')
      return solve(A*A.t(), A);
    }
  }
}

vec LinearAlgebraUtils::LeastSquareSolve(const mat& A, const vec& b, double lambda)
{
  if (lambda > 0)
  {
    int m = A.n_rows;
    int n = A.n_cols;

    if (m > n)      // Over-constrained system
    {
      mat AtA = A.t() * A + lambda * eye(n, n);
      mat Atb = A.t() * b;

      return solve(AtA, Atb);
    }
    else if (m < n)    // Under-constrained system
    {
      mat AAt = A * A.t() + lambda * eye(m, m);
      vec y  = solve(AAt, b);

      return A.t() * y;
    }
    else        // As many unknowns as equations
    {
      mat AlamdaI = A + lambda * eye(m, m);
      return solve(AlamdaI, b);
    }
  }
  else
  {
    return solve(A, b);
  }
}


#include <Eigen/Dense>

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/loss_function.h>
#include <ceres/autodiff_cost_function.h>

#include <sphericalsfm/so3.h>

#include <sphericalsfm/rotation_averaging.h>

namespace sphericalsfm {

    struct RotationError
    {
        RotationError( const Eigen::Matrix3d &_meas ) : meas(_meas) { }

        template <typename T>
        bool operator()(const T* const r0,
                        const T* const r1,
                        T* residuals) const
        {
            Eigen::Matrix<T,3,3> R, R0, R1;
            for ( int i = 0; i < 3; i++ )
                for ( int j = 0; j < 3; j++ )
                    R(i,j) = T(meas(i,j));
            ceres::AngleAxisToRotationMatrix( r0, R0.data() );
            ceres::AngleAxisToRotationMatrix( r1, R1.data() );

            Eigen::Matrix<T,3,3> cycle = (R1 * R0.transpose()) * R.transpose();
            ceres::RotationMatrixToAngleAxis( cycle.data(), residuals );

            return true;
        }

        Eigen::Matrix3d meas;
    };

    void optimize_rotations( std::vector<Eigen::Matrix3d> &rotations, const std::vector<RelativeRotation> &relative_rotations )
    {
        std::vector<Eigen::Vector3d> data(rotations.size());
        for ( int i = 0; i < rotations.size(); i++ ) data[i] = so3ln(rotations[i]);

        ceres::Problem problem;
        ceres::LossFunction* loss_function = new ceres::SoftLOneLoss(0.03);
        for ( int i = 0; i < relative_rotations.size(); i++ )
        {
            std::cout << relative_rotations[i].index0 << "\n";
            std::cout << relative_rotations[i].index1 << "\n";
            std::cout << relative_rotations[i].R << "\n";
            RotationError *error = new RotationError(relative_rotations[i].R);
            ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<RotationError, 3, 3, 3>(error);
            problem.AddResidualBlock(cost_function,
                loss_function,
                data[relative_rotations[i].index0].data(),
                data[relative_rotations[i].index1].data()
            );
        }

        ceres::Solver::Options options;
        options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
        options.minimizer_progress_to_stdout = true;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        std::cout << summary.FullReport() << "\n";
        if ( summary.termination_type == ceres::FAILURE )
        {
            std::cout << "error: ceres failed.\n";
            exit(1);
        }

        for ( int i = 0; i < rotations.size(); i++ ) rotations[i] = so3exp(data[i]);
    }

}


#include <boost/random.hpp>
#include <cmath>
#include <iostream>

#define mean 0.0
#define sigma 1.0
#define EPS 0.01
#define SAMPLES 10000000

using namespace boost::random;
using namespace std;

int main()
{
	mt19937 rng(static_cast<unsigned int>(time(0)));
	normal_distribution<double> dist(mean, sigma);

	cout << "Generating numbers..." << endl;
	int meanpoints = 0;
	int epoints = 0;
	for (int i = 0; i < SAMPLES; i++)
	{
		double rnd = dist(rng);
		if (fabs(rnd - mean) < EPS)
			meanpoints++;
		if ((fabs(rnd - mean - sqrt(2) * sigma) < EPS) ||
			(fabs(rnd - mean + sqrt(2) * sigma) < EPS))
			epoints++;
	}

	cout << "Expected f(mean)/f(mean+sqrt(2)*sigma): " << M_E << endl;
	cout << "Computed: " << meanpoints * 2.0 / epoints << endl;

	return 0;
}


#ifndef STAN_MATH_PRIM_FUN_CONSTANTS_HPP
#define STAN_MATH_PRIM_FUN_CONSTANTS_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/inv.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

// TODO(anyone) Use constexpr when moving to C++17

/**
 * Return the base of the natural logarithm.
 *
 * @return Base of natural logarithm.
 */
inline double e() { return boost::math::constants::e<double>(); }

/**
 * Return the value of pi.
 *
 * @return Pi.
 */
inline double pi() { return boost::math::constants::pi<double>(); }

/**
 * Smallest positive value.
 */
const double EPSILON = std::numeric_limits<double>::epsilon();

/**
 * Positive infinity.
 */
const double INFTY = std::numeric_limits<double>::infinity();

/**
 * Negative infinity.
 */
const double NEGATIVE_INFTY = -INFTY;

/**
 * (Quiet) not-a-number value.
 */
const double NOT_A_NUMBER = std::numeric_limits<double>::quiet_NaN();

/**
 * Twice the value of \f$ \pi \f$,
 * \f$ 2\pi \f$.
 */
const double TWO_PI = 2.0 * pi();

/**
 * The natural logarithm of 0,
 * \f$ \log 0 \f$.
 */
const double LOG_ZERO = std::log(0.0);

/**
 * The natural logarithm of machine precision \f$ \epsilon \f$,
 * \f$ \log \epsilon \f$.
 */
const double LOG_EPSILON = std::log(EPSILON);

/**
 * The natural logarithm of \f$ \pi \f$,
 * \f$ \log \pi \f$.
 */
const double LOG_PI = std::log(pi());

/**
 * The natural logarithm of 0.5,
 * \f$ \log 0.5 \f$.
 */
const double LOG_HALF = std::log(0.5);

/**
 * The natural logarithm of 2,
 * \f$ \log 2 \f$.
 */
const double LOG_TWO = std::log(2.0);

/**
 * The natural logarithm of 2 plus the natural logarithm of \f$ \pi \f$,
 * \f$ \log(2\pi) \f$.
 */
const double LOG_TWO_PI = LOG_TWO + LOG_PI;

/**
 * The value of one quarter the natural logarithm of \f$ \pi \f$,
 * \f$ \log(\pi) / 4 \f$.
 */
const double LOG_PI_OVER_FOUR = 0.25 * LOG_PI;

/**
 * The natural logarithm of the square root of \f$ \pi \f$,
 * \f$ \log(sqrt{\pi}) \f$.
 */
const double LOG_SQRT_PI = std::log(std::sqrt(pi()));

/**
 * The natural logarithm of 10,
 * \f$ \log 10 \f$.
 */
const double LOG_TEN = std::log(10.0);

/**
 * The value of the square root of 2,
 * \f$ \sqrt{2} \f$.
 */
const double SQRT_TWO = std::sqrt(2.0);

/**
 * The value of the square root of \f$ \pi \f$,
 * \f$ \sqrt{\pi} \f$.
 */
const double SQRT_PI = std::sqrt(pi());

/**
 * The value of the square root of \f$ 2\pi \f$,
 * \f$ \sqrt{2\pi} \f$.
 */
const double SQRT_TWO_PI = std::sqrt(TWO_PI);

/**
 * The square root of 2 divided by the square root of \f$ \pi \f$,
 * \f$ \sqrt{2} / \sqrt{\pi} \f$.
 */
const double SQRT_TWO_OVER_SQRT_PI = SQRT_TWO / SQRT_PI;

/**
 * The value of 1 over the square root of 2,
 * \f$ 1 / \sqrt{2} \f$.
 */
const double INV_SQRT_TWO = inv(SQRT_TWO);

/**
 * The value of 1 over the square root of \f$ \pi \f$,
 * \f$ 1 / \sqrt{\pi} \f$.
 */
const double INV_SQRT_PI = inv(SQRT_PI);

/**
 * The value of 1 over the square root of \f$ 2\pi \f$,
 * \f$ 1 / \sqrt{2\pi} \f$.
 */
const double INV_SQRT_TWO_PI = inv(SQRT_TWO_PI);

/**
 * The value of 2 over the square root of \f$ \pi \f$,
 * \f$ 2 / \sqrt{\pi} \f$.
 */
const double TWO_OVER_SQRT_PI = 2.0 / SQRT_PI;

/**
 * The value of half the natural logarithm 2,
 * \f$ \log(2) / 2 \f$.
 */
const double HALF_LOG_TWO = 0.5 * LOG_TWO;

/**
 * The value of half the natural logarithm \f$ 2\pi \f$,
 * \f$ \log(2\pi) / 2 \f$.
 */
const double HALF_LOG_TWO_PI = 0.5 * LOG_TWO_PI;

/**
 * The value of minus the natural logarithm of the square root of \f$ 2\pi \f$,
 * \f$ -\log(\sqrt{2\pi}) \f$.
 */
const double NEG_LOG_SQRT_TWO_PI = -std::log(SQRT_TWO_PI);

/**
 * Largest rate parameter allowed in Poisson RNG
 */
const double POISSON_MAX_RATE = std::pow(2.0, 30);

/**
 * Return positive infinity.
 *
 * @return Positive infinity.
 */
inline double positive_infinity() { return INFTY; }

/**
 * Return negative infinity.
 *
 * @return Negative infinity.
 */
inline double negative_infinity() { return NEGATIVE_INFTY; }

/**
 * Return (quiet) not-a-number.
 *
 * @return Quiet not-a-number.
 */
inline double not_a_number() { return NOT_A_NUMBER; }

/**
 * Returns the difference between 1.0 and the next value
 * representable.
 *
 * @return Minimum positive number.
 */
inline double machine_precision() { return EPSILON; }

/**
 * Returns the natural logarithm of ten.
 *
 * @return Natural logarithm of ten.
 */
inline double log10() { return LOG_TEN; }

/**
 * Returns the square root of two.
 *
 * @return Square root of two.
 */
inline double sqrt2() { return SQRT_TWO; }

}  // namespace math
}  // namespace stan

#endif


/*!
 * @file matrix_coeff.hpp
 * @brief Contains implementation of diffusion coefficient.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_MATRIX_COEFF_HPP_
#define INCLUDE_MATRIX_COEFF_HPP_

// Deal.ii
#include <deal.II/base/tensor_function.h>

// STL
#include <cmath>
#include <fstream>

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class MatrixCoeff
 * @brief Diffusion coefficient.
 *
 * Class implements a matrix valued diffusion coefficient.
 * This coefficient must be positive definite.
 */
template <int dim>
class MatrixCoeff : public TensorFunction<2,dim>
{
public:
	MatrixCoeff ();

	virtual Tensor<2, dim> value(const Point<dim> &point) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
			std::vector<Tensor<2,dim>>  &values) const override;

private:
	const int k = 21;
	const double scale_factor = 0.9999999;

	const double alpha = PI_D/3,
				beta = PI_D/6,
				gamma = PI_D/4;
	Tensor<2,dim> rot;
};

template <>
MatrixCoeff<2>::MatrixCoeff ()
:
TensorFunction<2,2> ()
{
	rot[0][0] = cos(alpha);
	rot[0][1] = sin(alpha);
	rot[1][0] = -sin(alpha);
	rot[1][1] = cos(alpha);
}

template <>
MatrixCoeff<3>::MatrixCoeff ()
:
TensorFunction<2,3> ()
{
	rot[0][0] = cos(alpha)*cos(gamma) - sin(alpha)*cos(beta)*sin(gamma);
	rot[0][1] = -cos(alpha)*sin(gamma) - sin(alpha)*cos(beta)*cos(gamma);
	rot[0][2] = sin(alpha)*sin(beta);
	rot[1][0] = sin(alpha)*cos(gamma) + cos(alpha)*cos(beta)*sin(gamma);
	rot[1][1] = -sin(alpha)*sin(gamma) + cos(alpha)*cos(beta)*cos(gamma);
	rot[1][2] = -cos(alpha)*sin(beta);
	rot[2][0] = sin(beta)*sin(gamma);
	rot[2][1] = sin(beta)*cos(gamma);
	rot[2][2] = cos(beta);
}

template <int dim>
Tensor<2, dim>
MatrixCoeff<dim>::value(const Point<dim> &p) const
{
	Tensor<2, dim> value;
	value.clear();

	for (unsigned int d=0; d<dim; ++d)
	{
		value[d][d] = 1.0 * (1.0 - scale_factor*(
				0.5*sin(2*PI_D*k*p(0))
				+ 0.5*sin(2*PI_D*k*p(1))
				) ); /* Must be positive definite. */
	}

	value = rot * value * transpose (rot);

	return value;
}

template <int dim>
void
MatrixCoeff<dim>::value_list(const std::vector<Point<dim>> &points,
		std::vector<Tensor<2,dim>>  &values) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p].clear();

		for (unsigned int d=0; d<dim; ++d)
		{
			values[p][d][d] = 1.0 * (1.0 - scale_factor*(
					0.5*sin(2*PI_D*k*points[p](0))
					+ 0.5*sin(2*PI_D*k*points[p](1))
					) ); /* Must be positive definite. */
		}

		values[p] = rot * values[p] * transpose (rot);
	}
}

} // end namespace Coefficients

#endif /* INCLUDE_MATRIX_COEFF_HPP_ */


#include <Eigen/Dense>
#include "simple_activation.h"

namespace MyDL{
    using namespace Eigen;

    // 実装がめんどくさいので直接Eigen::MatrixXdを引数にとるよう実装
    MatrixXd softmax(MatrixXd x)
    {
        VectorXd max_coeff_vec, rowwise_sum;
        max_coeff_vec = x.rowwise().maxCoeff();

        x = x.colwise() - max_coeff_vec; // expのオーバーフロー回避 → 各バッチベクトルごとに最大の要素を抽出
        x = x.array().exp();             // expを各要素に実行
        rowwise_sum = x.rowwise().sum(); // バッチベクトルごとに総和を計算

        x.array().colwise() /= rowwise_sum.array(); // 出力の総和が1になるよう調整
        return x;
    }

    MatrixXd sigmoid(MatrixXd x)
    {
        x = x.unaryExpr([](double p) { return 1 / (1 + exp(-p)); });
        return x;
    }
}


#include "k_points_util.h"

#include <boost/functional/hash.hpp>
#include "../array_math.h"

size_t KPointsUtil::get_n_k_points(const double rcut) {
  size_t count = 0;
  const int8_t n_max = floor(rcut);
  for (int8_t i = -n_max; i <= n_max; i++) {
    for (int8_t j = -n_max; j <= n_max; j++) {
      for (int8_t k = -n_max; k <= n_max; k++) {
        if (i * i + j * j + k * k > pow(rcut, 2)) continue;
        count++;
      }
    }
  }
  return count;
}

std::vector<std::array<int8_t, 3>> KPointsUtil::generate_k_points(const double rcut) {
  std::vector<std::array<int8_t, 3>> k_points;
  const int8_t n_max = floor(rcut);
  for (int8_t i = -n_max; i <= n_max; i++) {
    for (int8_t j = -n_max; j <= n_max; j++) {
      for (int8_t k = -n_max; k <= n_max; k++) {
        if (i * i + j * j + k * k > pow(rcut, 2)) continue;
        k_points.push_back(std::array<int8_t, 3>({i, j, k}));
      }
    }
  }
  std::stable_sort(
      k_points.begin(),
      k_points.end(),
      [](const std::array<int8_t, 3>& a, const std::array<int8_t, 3>& b) -> bool {
        return squared_norm(a) < squared_norm(b);
      });
  return k_points;
}

std::vector<std::array<int8_t, 3>> KPointsUtil::get_k_diffs(
    const std::vector<std::array<int8_t, 3>>& k_points) {
  // Generate all possible differences between two different k points.
  std::unordered_set<std::array<int8_t, 3>, boost::hash<std::array<int8_t, 3>>> k_diffs_set;
  std::vector<std::array<int8_t, 3>> k_diffs;
  const size_t n_orbs = k_points.size();
  for (size_t p = 0; p < n_orbs; p++) {
    for (size_t q = 0; q < n_orbs; q++) {
      if (p == q) continue;
      const auto& diff_pq = k_points[q] - k_points[p];
      if (k_diffs_set.count(diff_pq) == 1) continue;
      k_diffs.push_back(diff_pq);
      k_diffs_set.insert(diff_pq);
    }
  }

  // Sort k_diffs into ascending order so that later sorting hci queue will be faster.
  std::stable_sort(
      k_diffs.begin(),
      k_diffs.end(),
      [](const std::array<int8_t, 3>& a, const std::array<int8_t, 3>& b) -> bool {
        return squared_norm(a) < squared_norm(b);
      });

  return k_diffs;
}


#include <fmt/core.h>
#include <yaml-cpp/yaml.h>

#include <Eigen/Dense>
#include <iostream>
#include <random>

#include "Metropolis.hpp"
#include "NumpySaver.hpp"
#include "Timer.hpp"

using namespace Eigen;
using std::cout, std::endl;

using Array1d = Array<double, 1, 1>;

constexpr double pow2(double x) { return x * x; };
constexpr double pow3(double x) { return x * x * x; };

double pdf_1d(const Array1d& x, double alpha) {
  return std::exp(-2 * alpha * x[0] * x[0]);
}

double energy_1d(const Array1d& x, double alpha) {
  return alpha + x[0] * x[0] * (1. / 2. - 2 * alpha * alpha);
}

/**
 * @brief
 *
 * @param x = {x0, y0, x1, y1}
 * @param alpha
 * @param lambda
 * @return double
 */
double pdf_wavefunction_2d(const Array4d& x, double alpha, double lambda) {
  double r = std::sqrt(pow2(x[0] - x[2]) + pow2(x[1] - x[3]));
  return std::exp(-pow2(x[0]) - pow2(x[1]) - pow2(x[2]) - pow2(x[3]) +
                  2. * lambda * r / (1. + alpha * r));
}

constexpr double derivative_factor(double r, double x, double x_other,
                                   double alpha, double lambda) {
  using std::pow;
  return -1. + lambda / pow2(1 + alpha * r) / r -
         lambda * pow2(x - x_other) / pow3((1. + alpha * r) * r) *
             (1. + 3. * alpha * r) +
         pow2(-x + lambda * (x - x_other) / pow2(1. + alpha * r) / r);
}

double energy_2d(const Array4d& x, double alpha, double lambda) {
  double r = std::sqrt(pow2(x[0] - x[2]) + pow2(x[1] - x[3]));

  double dx1 = derivative_factor(r, x[0], x[2], alpha, lambda);
  double dx2 = derivative_factor(r, x[2], x[0], alpha, lambda);
  double dy1 = derivative_factor(r, x[1], x[3], alpha, lambda);
  double dy2 = derivative_factor(r, x[3], x[1], alpha, lambda);

  return (-dx1 + pow2(x[0]) - dx2 + pow2(x[2]) - dy1 + pow2(x[1]) - dy2 +
          pow2(x[3])) /
             2. +
         lambda / r;
}

std::tuple<double, double> golden_search_simple(std::function<double(double)> f,
                                                double xmin, double xmax,
                                                double tol) {
  constexpr double golden_ratio = 1.618033988749895;
  double x1, x2;
  if (xmin >= xmax)
    throw std::invalid_argument("xmin must be smaller than xmax");

  while (xmax - xmin > tol) {
    x1 = xmax - (xmax - xmin) / golden_ratio;
    x2 = xmin + (xmax - xmin) / golden_ratio;

    if (f(x1) < f(x2))
      xmax = x2;
    else
      xmin = x1;
  }

  return {xmin, xmax};
}

std::tuple<double, double> golden_search(std::function<double(double)> f,
                                         double xmin, double xmax, double tol) {
  constexpr double golden_ratio = 1.618033988749895;
  double x1 = xmax - (xmax - xmin) / golden_ratio;
  double x2 = xmin + (xmax - xmin) / golden_ratio;
  double f1 = f(x1);
  double f2 = f(x2);

  if (xmin >= xmax)
    throw std::invalid_argument("xmin must be smaller than xmax");

  while (xmax - xmin > tol) {
    if (f1 < f2) {
      xmax = x2;
      x2 = x1;
      f2 = f1;
      x1 = xmax - (xmax - xmin) / golden_ratio;
      f1 = f(x1);
    } else {
      xmin = x1;
      x1 = x2;
      f1 = f2;
      x2 = xmin + (xmax - xmin) / golden_ratio;
      f2 = f(x2);
    }
  }

  return {xmin, xmax};
}

std::tuple<double, double> adaptive_golden_search(
    std::function<std::tuple<double, double>(double, Index sample)> f,
    double xmin, double xmax, double tol, Index start_sample = 1000) {
  constexpr double golden_ratio = 1.618033988749895;
  Index sample = start_sample;

  double x1 = xmax - (xmax - xmin) / golden_ratio;
  double x2 = xmin + (xmax - xmin) / golden_ratio;
  auto [f1, f1_err] = f(x1, sample);
  auto [f2, f2_err] = f(x2, sample);

  std::tuple<double, double> buf;

  if (xmin >= xmax)
    throw std::invalid_argument("xmin must be smaller than xmax");

  while (xmax - xmin > tol) {
    if (f1 < f2) {
      xmax = x2;
      x2 = x1;
      f2 = f1;
      x1 = xmax - (xmax - xmin) / golden_ratio;
      std::tie(f1, f1_err) = f(x1, sample);
    } else {
      xmin = x1;
      x1 = x2;
      f1 = f2;
      x2 = xmin + (xmax - xmin) / golden_ratio;
      std::tie(f2, f2_err) = f(x2, sample);
    }
    if (std::abs(f1 - f2) < f1_err + f2_err) {
      sample += (Index)pow2((f1_err + f2_err) / std::abs(f1 - f2));
    }
  }

  return {xmin, xmax};
}

void test_1D_metropolis() {
  Array1d argstart;
  argstart << 0;

  std::uniform_real_distribution<> step(-2, 2);
  std::normal_distribution<> step_gauss(0, 1);

  using namespace std::placeholders;
  auto pdf_normal = std::bind(pdf_1d, _1, 1. / 4.);

  MetropolisAlgorithm<1> malg_uniform(pdf_normal, argstart, step);
  MetropolisAlgorithm<1, std::normal_distribution<> > malg_normal(
      pdf_normal, argstart, step_gauss);

  auto test_1 = malg_uniform.get_sample(100000);
  auto test_2 = malg_normal.get_sample(100000);

  auto [mean, std] =
      Timer::measure_time([&]() { malg_uniform.get_sample(100000); }, .1);
  cout << "Generating 100000 events with uniform step took " << mean << "±"
       << std << "ms\n";

  auto [mean_n, std_n] =
      Timer::measure_time([&]() { malg_normal.get_sample(100000); }, .1);
  cout << "Generating 100000 events with normal step took " << mean_n << "±"
       << std_n << "ms\n";
  NumpySaver("build/output/test_normal_distribution.npy") << test_1 << test_2;
}

double optimal_alpha_2d(double lambda, Index sample = 100000) {
  using namespace std::placeholders;

  Array4d argstart;
  argstart << 0, 1, 0, -1;
  std::normal_distribution<> step_gauss(0, 1);

  std::function<double(double)> f = [&](double alpha) {
    auto pdf = std::bind(pdf_wavefunction_2d, _1, alpha, lambda);
    auto energy = std::bind(energy_2d, _1, alpha, lambda);
    MetropolisAlgorithm<4, std::normal_distribution<> > malg(pdf, argstart,
                                                             step_gauss);
    auto [mean, std, var] = malg.average(energy, sample);
    return mean;
  };

  auto [xmin, xmax] = golden_search(f, .1, 1, 1e-6);
  return (xmin + xmax) / 2;
}

void test_2D_metropolis(double lambda = 1) {
  Array4d argstart;
  argstart << 0, 1, 0, -1;

  std::uniform_real_distribution<> step(-2, 2);
  std::normal_distribution<> step_gauss(0, 1);

  using namespace std::placeholders;
  double alpha = optimal_alpha_2d(lambda);
  auto pdf_2d = std::bind(pdf_wavefunction_2d, _1, alpha, lambda);

  MetropolisAlgorithm<4, std::normal_distribution<> > malg_normal(
      pdf_2d, argstart, step_gauss);

  auto test_1 = malg_normal.get_sample(100000);

  auto [mean, std] =
      Timer::measure_time([&]() { malg_normal.get_sample(100000); }, .1);
  cout << "Generating 100000 2D events with uniform step took " << mean << "±"
       << std << "ms\n";

  NumpySaver("build/output/test_2D_distribution.npy")
      << test_1.col(0) << test_1.col(1) << test_1.col(2) << test_1.col(3);
}

void test_1D_energy(Index n = 300) {
  using namespace std::placeholders;

  Array1d argstart;
  argstart << 0;
  std::normal_distribution<> step_gauss(0, 1);

  ArrayXd alphas = ArrayXd::LinSpaced(n, 0.1, 1);
  ArrayXd means(n);
  ArrayXd stds(n);
  ArrayXd vars(n);

  for (Index i = 0; i < n; i++) {
    auto pdf_normal = std::bind(pdf_1d, _1, alphas[i]);
    auto energy = std::bind(energy_1d, _1, alphas[i]);
    MetropolisAlgorithm<1, std::normal_distribution<> > malg_normal(
        pdf_normal, argstart, step_gauss);
    auto [mean, std, var] = malg_normal.average(energy, 100000);
    means[i] = mean;
    stds[i] = std;
    vars[i] = var;
  }

  NumpySaver("build/output/test_1d_energy.npy")
      << alphas << means << stds << vars;
}

void test_2D_energy(Index n = 300, double lambda = 1) {
  using namespace std::placeholders;

  Array4d argstart;
  argstart << 0, 1, 0, -1;
  std::normal_distribution<> step_gauss(0, 1);

  ArrayXd alphas = ArrayXd::LinSpaced(n, 0.1, 1);
  ArrayXd means(n);
  ArrayXd stds(n);
  ArrayXd vars(n);

  for (Index i = 0; i < n; i++) {
    auto pdf = std::bind(pdf_wavefunction_2d, _1, alphas[i], lambda);
    auto energy = std::bind(energy_2d, _1, alphas[i], lambda);
    MetropolisAlgorithm<4, std::normal_distribution<> > malg(pdf, argstart,
                                                             step_gauss);
    auto [mean, std, var] = malg.average(energy, 100000);
    means[i] = mean;
    stds[i] = std;
    vars[i] = var;
  }

  NumpySaver(fmt::format("build/output/test_2d_energy_{}.npy", lambda))
      << alphas << means << stds << vars;
}

void find_1d_alpha(Index sample = 1000000) {
  using namespace std::placeholders;

  Array1d argstart;
  argstart << 0;
  std::normal_distribution<> step_gauss(0, 1);

  std::function<double(double)> f = [&](double alpha) {
    auto pdf = std::bind(pdf_1d, _1, alpha);
    auto energy = std::bind(energy_1d, _1, alpha);
    MetropolisAlgorithm<1, std::normal_distribution<> > malg(pdf, argstart,
                                                             step_gauss);
    auto [mean, std, var] = malg.average(energy, sample);
    return mean;
  };

  auto [xmin, xmax] = golden_search(f, .1, 1, 1e-6);
  cout << "Optimal α for 1D case is between " << xmin << " and " << xmax << " ("
       << f(xmin) << " < E <" << f(xmax) << ")" << endl;
}

void find_2d_alpha(double lambda = 1, Index sample = 1000000) {
  using namespace std::placeholders;

  Array4d argstart;
  argstart << 0, 1, 0, -1;
  std::normal_distribution<> step_gauss(0, 1);

  std::function<double(double)> f = [&](double alpha) {
    auto pdf = std::bind(pdf_wavefunction_2d, _1, alpha, lambda);
    auto energy = std::bind(energy_2d, _1, alpha, lambda);
    MetropolisAlgorithm<4, std::normal_distribution<> > malg(pdf, argstart,
                                                             step_gauss);
    auto [mean, std, var] = malg.average(energy, sample);
    return mean;
  };

  auto [xmin, xmax] = golden_search(f, .1, 1, 1e-6);
  cout << "Optimal α for 2D case (λ=" << lambda << ") is between " << xmin
       << " and " << xmax << " (" << f(xmin) << " < E <" << f(xmax) << ")"
       << endl;
}

void find_2d_alpha_adaptive(double lambda = 1) {
  using namespace std::placeholders;

  Array4d argstart;
  argstart << 0, 1, 0, -1;
  std::normal_distribution<> step_gauss(0, 1);

  std::function<std::tuple<double, double>(double, Index)> f =
      [&](double alpha, double sample) {
        auto pdf = std::bind(pdf_wavefunction_2d, _1, alpha, lambda);
        auto energy = std::bind(energy_2d, _1, alpha, lambda);
        MetropolisAlgorithm<4, std::normal_distribution<> > malg(pdf, argstart,
                                                                 step_gauss);
        auto [mean, std, var] = malg.average(energy, sample);
        return std::tuple<double, double>({mean, std});
      };

  auto [xmin, xmax] = adaptive_golden_search(f, .1, 1, 1e-6);
  cout << "Optimal (adaptive) α for 2D case (λ=" << lambda << ") is between "
       << xmin << " and " << xmax << endl;
}

void energy_function_test() {
  cout << "E_2d(1, 0, 2, 0|λ=1,α=1)=" << energy_2d({1, 0, 2, 0}, 1, 1) << endl;
  cout << "E_2d(1.1, 3.1, -5.6, -5|λ=2.1,α=1.1)="
       << energy_2d({1.1, 3.1, -5.6, -5}, 1.1, 2.1) << endl;
}

int main(int argc, char const* argv[]) {
  energy_function_test();
  test_1D_metropolis();
  test_2D_metropolis();
  test_1D_energy();
  test_2D_energy(300, 0);
  test_2D_energy(300, 1);
  test_2D_energy(300, 2);
  test_2D_energy(300, 8);
  find_1d_alpha();
  find_2d_alpha(0);
  find_2d_alpha(1);
  find_2d_alpha(2);
  find_2d_alpha(8);
  return 0;
}


#include "parameters.h"

#include <NTL/RR.h>

namespace {
    using namespace NTL;
}

namespace gnfs {
    long param_d(const ZZ& n) {
        RR ln = log(conv<RR>(n));
        RR base = (3 * ln) / log(ln);
        return conv<long>(pow(base, RR(1.0/3)));
    }

    long param_B(const ZZ& n) {
        RR ln = log(conv<RR>(n));
        RR base = 8.0 / 9 * ln * log(ln) * log(ln);
        return conv<long>(exp(pow(base, RR(1.0/3))));
    }

    std::pair<ZZ, ZZX> param_mf(const ZZ& n, long d) {
        ZZ m = conv<ZZ>(pow(conv<RR>(n), RR(1.0/d)));

        // Write n in base m
        ZZX f;
        ZZ cur = n;
        long i = 0;
        while (cur != 0) {
            SetCoeff(f, i, cur % m);
            cur /= m;
            i++;
        }

        return std::make_pair(m, f);
    }
}


/*
 * ex_main.cpp
 *
 *  Created on: 2017. 6. 6.
 *      Author: cho
 */

#include <random>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>
//#include "io.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/// number of data
const unsigned int N = 3;

int main () {
	// random number generator
	random_device rd;      // obtain seed
	mt19937_64 gen(rd());  // mersenne twister engine

	// vector
	cout << "============== vector" << endl;
	{
		ublas::vector<double> t (N); // time vector, sec
		for (unsigned i = 0; i < t.size(); ++i) t(i) = i;
		cout << "t("<<N<<") = " << t << endl;

		uniform_int_distribution<int> dis_y( 0, 10 );
		ublas::vector<double> y (N); // measured value
		for (unsigned i = 0; i < y.size(); ++i) y(i) = dis_y(gen);
		cout << "y("<<N<<") = " << y << endl;

		for (unsigned i = 0; i < N; ++i) {
			ublas::unit_vector<double> u(N, i);
			cout << "unit_vector("<<N<<","<<i<<") = " << u << endl;
		}

		ublas::zero_vector<double> z(N);
		cout << "zero_vector("<<N<<") = " << z << endl;

		ublas::scalar_vector<double> s(N, 3);
		cout << "scalar_vector("<<N<<",3) = " << s << endl;

		ublas::scalar_vector<double> one(N, 1);
		cout << "one("<<N<<",1) = " << one << endl;
	}

	// range
	cout << "============== range, slice" << endl;
	{
		ublas::range r1(0, N);
		for (unsigned i = 0; i < r1.size(); ++ i) {
			cout << "range(0,N) r("<<i<<") = " << r1(i) << endl;
		}

		ublas::range r2(3, N);
		for (unsigned i = 0; i < r2.size(); ++ i) {
			cout << "range(3,N) r("<<i<<") = " << r2(i) << endl;
		}

		ublas::slice s2(0, 2, N);
		for (unsigned i = 0; i < s2.size(); ++ i) {
			cout << "slice(0,2,N) s2("<<i<<") = " << s2(i) << endl;
		}

		ublas::slice s3(1, 3, N);
		for (unsigned i = 0; i < s3.size(); ++ i) {
			cout << "slice(1,3,N) s("<<i<<") = " << s3(i) << endl;
		}
	}

	// matrix
	cout << "============== matrix" << endl;
	{
		ublas::matrix<double> m;
		cout << "m()          = " << m << endl;

		m.resize( N, N );
		cout << "m.resize(N,N)= " << m << endl;

		m.clear();
		cout << "m.clear()    = " << m << endl;

		for (unsigned i = 0; i < m.size1 (); ++ i)
			for (unsigned j = 0; j < m.size2 (); ++ j)
				m (i, j) = i*10 + j;
		cout << "matrix       = " << m << endl;

		cout << "transpose    = " << ublas::trans(m) << endl;
		cout << "column(m, 1) = " << ublas::column (m, 1) << endl;
		cout << "column(m, 2) = " << ublas::column (m, 2) << endl;
		cout << "row(m, 1)    = " << ublas::row (m, 1) << endl;
		cout << "row(m, 2)    = " << ublas::row (m, 2) << endl;
		cout << "project(m, range(0,1), range(0,N))     = "
				<< ublas::project(m, ublas::range(0,1), ublas::range(0,N)) << endl;
		cout << "project(m, range(N-1,N), range(N-1,N)) = "
				<< ublas::project(m, ublas::range(N-1,N), ublas::range(N-1,N)) << endl;
		cout << "project(m, slice(1,2,2), slice(1,2,2)) = "
				<< ublas::project(m, ublas::slice(0,2,2), ublas::slice(0,2,2)) << endl;

		auto m_temp = m;
		for ( unsigned i = 0; i < m_temp.size2(); ++i ) {
			ublas::column( m_temp, i) = ublas::scalar_vector<double>(m_temp.size1(),1);
			cout << "m_temp       = " << m_temp << endl;
		}

		m_temp = m;
		for ( unsigned i = 0; i < m_temp.size1(); ++i ) {
			ublas::row( m_temp, i ) = ublas::scalar_vector<double>(m_temp.size2(),1);
			cout << "m_temp       = " << m_temp << endl;
		}

		ublas::identity_matrix<double> mi(N);
		cout << "identity(N)      = " << mi << endl;

		ublas::zero_matrix<double> zero(N);
		cout << "zero(N)          = " << zero << endl;

		ublas::scalar_matrix<double> ones(1, 1, 1);
		cout << "sizeof( scalar_matrix(1, 1, 1) ) = " << sizeof(ones) << endl;
		ublas::scalar_matrix<double> twos(1000, 2000000, 2);
		cout << "sizeof( scalar_matrix(1000, 2000000, 2) ) = " << sizeof(twos) << endl;

		ublas::matrix<double> ones1(1, 1, 1);
		cout << "sizeof( matrix(1, 1, 1) ) = " << sizeof(ones1) << endl;
		ublas::matrix<double> twos1(10000, 20000, 2);
		cout << "sizeof( matrix(10000, 20000, 2) ) = " << sizeof(twos1) << endl;
	}

	// matrix
	cout << "============== triangular_matrix" << endl;
	{
		ublas::triangular_matrix<double, ublas::lower> l(N,N);
		for ( unsigned i = 0; i < l.size1(); ++i )
			for ( unsigned j = 0; j <= i; ++j )
				l(i,j) = 10*i + j;
		cout << "l(N, N)           = " << l << endl;

		ublas::triangular_matrix<double, ublas::unit_lower> ul(N,N);
		for ( unsigned i = 0; i < ul.size1(); ++i )
			for ( unsigned j = 0; j < i; ++j )
				ul(i,j) = 10*i + j;
		cout << "ul(N, N)          = " << ul << endl;

		ublas::triangular_matrix<double, ublas::upper> u(N,N);
		for ( unsigned i = 0; i < u.size1(); ++i )
			for ( unsigned j = i; j < u.size2(); ++j )
				u(i,j) = 10*i + j+1;
		cout << "u(N, N)           = " << u << endl;

		ublas::triangular_matrix<double, ublas::unit_upper> uu(N,N);
		for ( unsigned i = 0; i < uu.size1(); ++i )
			for ( unsigned j = i+1; j < uu.size2(); ++j )
				uu(i,j) = 10*i + j+1;
		cout << "uu(N, N)          = " << uu << endl;
	}
}


/*
    This file is part of control-lib.

    Copyright (c) 2020, 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#include "control_lib/tools/math.hpp"

#include <Eigen/Dense>

namespace control_lib {
    namespace tools {
        inline Eigen::Vector3d eulerError(const Eigen::Vector3d& curr, const Eigen::Vector3d& ref)
        {
            Eigen::Vector3d error = curr - ref;

            for (size_t i = 0; i < 3; i++) {
                if (error(i) > M_PI)
                    error(i) -= 2 * M_PI;
                else if (error(i) < -M_PI)
                    error(i) += 2 * M_PI;
            }

            return error;
        }

        inline Eigen::Vector3d rotationError(const Eigen::Vector3d& curr, const Eigen::Vector3d& ref)
        {
            Eigen::Matrix3d R_current = Eigen::AngleAxisd(curr.norm(), curr.normalized()).toRotationMatrix(),
                            R_desired = Eigen::AngleAxisd(ref.norm(), ref.normalized()).toRotationMatrix();

            Eigen::AngleAxisd aa = Eigen::AngleAxisd(R_current.transpose() * R_desired);

            return aa.axis() * aa.angle();
        }

        inline Eigen::Vector4d quaternionError(const Eigen::Vector4d& curr, const Eigen::Vector4d& ref)
        {
            Eigen::Quaterniond q_current = Eigen::Quaterniond(curr), q_desired = Eigen::Quaterniond(ref);
            return (q_current.inverse() * q_desired).coeffs();
        }

        inline Eigen::MatrixXd kronecker(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
        {
            Eigen::MatrixXd C(A.rows() * B.rows(), A.cols() * B.cols());

            for (size_t i = 0; i < A.rows(); i++) {
                for (size_t j = 0; j < A.cols(); j++)
                    C.block(i * B.rows(), j * B.cols(), B.rows(), B.cols()) = A(i, j) * B;
            }

            return C;
        }

        Eigen::MatrixXd solveVectorized(const Eigen::MatrixXd& A, const Eigen::MatrixXd& W)
        {
            size_t dim = A.rows();

            return (kronecker(Eigen::MatrixXd::Identity(dim, dim), A) + kronecker(A, Eigen::MatrixXd::Identity(dim, dim)))
                .colPivHouseholderQr() // selfadjointView<Eigen::Upper>().llt()
                .solve(W.reshaped())
                .reshaped(dim, dim);
        }

        Eigen::MatrixXd bartelsStewart(const Eigen::MatrixXd& A, const Eigen::MatrixXd& W)
        {
            Eigen::RealSchur<Eigen::MatrixXd> schur(A);

            size_t dim = A.rows(), block_dim = (dim % 2) ? 1 : 2;

            Eigen::MatrixXd U = schur.matrixU(), T = schur.matrixT(), C = U.transpose() * W * U, Y = Eigen::MatrixXd::Zero(dim, dim);

            for (size_t i = dim / block_dim; i < 0; i--) {
                Eigen::MatrixXd C_11 = C.block(0, 0, (i - 1) * block_dim, (i - 1) * block_dim),
                                C_12 = C.block(0, (i - 1) * block_dim, (i - 1) * block_dim, block_dim),
                                C_21 = C.block((i - 1) * block_dim, 0, block_dim, (i - 1) * block_dim),
                                C_22 = C.block((i - 1) * block_dim, (i - 1) * block_dim, block_dim, block_dim),
                                R_11 = T.block(0, 0, (i - 1) * block_dim, (i - 1) * block_dim),
                                R_12 = T.block(0, (i - 1) * block_dim, (i - 1) * block_dim, block_dim),
                                R_22 = T.block((i - 1) * block_dim, (i - 1) * block_dim, block_dim, block_dim),
                                Z_12((i - 1) * block_dim, block_dim),
                                Z_21(block_dim, (i - 1) * block_dim),
                                Z_22 = solveVectorized(R_22, C_22),
                                Cbar_12 = C_12 - R_12 * Z_22,
                                Cbar_21 = C_21.transpose() - R_12 * Z_22.transpose();

                for (size_t j = 1 - 1; j < 0; j--) {
                    Eigen::MatrixXd Rcurr = R_11.block((j - 1) * block_dim, (j - 1) * block_dim, block_dim, block_dim);
                    Z_12.block((j - 1) * block_dim, block_dim, 0, block_dim) = solveVectorized(Rcurr, Cbar_12.block((j - 1) * block_dim, block_dim, 0, block_dim));
                    Z_21.block(0, block_dim, (j - 1) * block_dim, block_dim) = solveVectorized(Rcurr, Cbar_21.block((j - 1) * block_dim, block_dim, 0, block_dim));
                }

                Y.block((i - 1) * block_dim, (i - 1) * block_dim, block_dim, block_dim) = Z_22;
                Y.block(0, (i - 1) * block_dim, (i - 1) * block_dim, block_dim) = Z_12;
                Y.block((i - 1) * block_dim, 0, block_dim, (i - 1) * block_dim) = Z_21;

                C = C_11 - R_12 * Z_21 - Z_12 * R_12.transpose();
            }

            return U.transpose() * Y * U;
        }
    } // namespace tools
} // namespace control_lib


#include <cmath>
#include <ctime>
#include <cmath>
#include <bitset>
#include <string>
#include <fstream>
#include <cstdlib>
#include <cassert>
#include <utility>
#include <iostream>

#include <openssl/sha.h>

#pragma once

/* Arbitrary precision integers */
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/chrono/chrono.hpp>
#include <boost/multiprecision/random.hpp>
#include <boost/random/random_device.hpp>

using namespace boost::multiprecision;
using namespace boost::random;

typedef boost::multiprecision::cpp_int bigint;
typedef independent_bits_engine<mt19937, 256, bigint> generator_type;

random_device dev;
generator_type gen(dev);

// http://stackoverflow.com/a/10632725
std::string sha256(const std::string str)
{
    unsigned char hash[SHA256_DIGEST_LENGTH];
    SHA256_CTX sha256;
    SHA256_Init(&sha256);
    SHA256_Update(&sha256, str.c_str(), str.size());
    SHA256_Final(hash, &sha256);
    std::stringstream ss;
    for(int i = 0; i < SHA256_DIGEST_LENGTH; i++)
    {
        ss << std::hex << std::setw(2) << std::setfill('0') << (int)hash[i];
    }
    return ss.str();
}

std::string sha256_file(const std::string file_name) {
	std::ifstream ifs(file_name);
	std::string content( (std::istreambuf_iterator<char>(ifs) ),
						(std::istreambuf_iterator<char>()    ) );

	return sha256(content);
}

inline cpp_int modulo(cpp_int a, cpp_int b) {
	cpp_int m = a%b;
	return m>=0 ? m:m+b;
}

/* This is ugly, bad practice, yet extremely convenient.
	Boost is not allowing me to make an array
	cpp_int x[3]. So I just had to emulate an array
	by using the macro "correct_val" to select the wanted variable.
	To my knowledge, there is no better alternative.
*/
#define correct_val(x0,x1,x2,i) (((i)%3)?(((i)&1)?x1:x2):x0)

cpp_int modInverse(cpp_int a, cpp_int b) {

	cpp_int x0, x1, x2;
	cpp_int y0, y1, y2;
	cpp_int quotient  = abs(a / b);
	cpp_int remainder = modulo(a, b);

	x0 = 0;
	y0 = 1;
	x1 = 1;
	y1 = -quotient;

	cpp_int i = 2;
	for (; (b % (a%b)) != 0; i++) {
		a = b;
		b = remainder;
		quotient = abs(a / b);
		remainder = modulo(a, b);
		correct_val(x0,x1,x2,i) = (-quotient * correct_val(x0,x1,x2,i-1)) + correct_val(x0,x1,x2,i-2);
		correct_val(y0,y1,y2,i) = (-quotient * correct_val(y0,y1,y2,i-1)) + correct_val(y0,y1,y2,i-2);
	}

	return correct_val(x0,x1,x2,i-1);
}

typedef struct point {
	cpp_int x,y;

	inline bool operator==(const point& rhs){
		return (x == rhs.x && y == rhs.y);
	}

	inline bool operator!=(const point& rhs){
		return !(*this == rhs);
	}

} Point;

std::ostream& operator<<(std::ostream& out, const point& obj) {
   	return out << std::hex
   				<< obj.x << " " << obj.y;
}

std::istream& operator>>(std::istream& in, point& obj){
	std::string x, y;
	in >> x >> y;
	obj.x = bigint("0x" + x);
	obj.y = bigint("0x" + y);
	return in;
}

std::string string_to_binary(const std::string& input)
{
    std::ostringstream oss;
    for(auto c : input) {
        oss << std::bitset<8>(c);
    }
    return oss.str();
}

class EllipticCurve {

/* To do: change to private */
private:
/* p, a, b parameters of the curve y^2 = x^3 + ax + b */
	const cpp_int p; /* Modulus of the group */
	const cpp_int a;
	const cpp_int b;

	/* Generator */
	const Point G;

	/* Order of G */
	const cpp_int n;

	/* Cofactor */
	const cpp_int h;

	Point PointAdd(Point p1, Point p2){

		if( p1 == p2 ){
			return PointDouble(p1);
		}

		cpp_int xp = p1.x, yp = p1.y;
		cpp_int xq = p2.x, yq = p2.y;

		if( xp == xq ){
			return {0,0};
		}

		if( p1 == Point{0,0} ){
			return p2;
		}

		if( p2 == Point{0,0} ){
			return p1;
		}

		cpp_int inv = modInverse(xq-xp, p);
		cpp_int lambda = (yq-yp)*inv;
		cpp_int xr = modulo((pow(lambda, 2) - xp - xq), p);
		cpp_int yr = modulo((lambda * (xp - xr) - yp), p);

		if(p1.y == p2.y)
			std::cout << p1 << " + " << p2 << " = " << Point{xr,yr} << std::endl;
		return {xr,yr};
	}

	Point PointDouble(Point p1){

		cpp_int xp = p1.x, yp = p1.y;

		cpp_int inv = modInverse(2*yp, p);
		cpp_int lambda = (3*static_cast<cpp_int>(pow(xp,2))+a)*inv;
		cpp_int xr = modulo((static_cast<cpp_int>(pow(lambda, 2)) - 2*xp),p);
		cpp_int yr = modulo((lambda * (xp - xr) - yp),p);
		return {xr,yr};
	}

	Point PointMultiplication(cpp_int k, Point P) {

		if( k == 0 ) {
			return {0,0};
		}
		if( k == 1 ){ /* Base case for the recursion */
			return P;
		}
		if( !(k&1) ) { /* k is even */
			return PointDouble(PointMultiplication(k/2, P));
		}
		else { /* k is odd */
			return PointAdd(P, PointDouble(PointMultiplication((k-1)/2, P)));
		}
	}

	// wikipedia.org/wiki/Elliptic_Curve_Digital_Signature_Algorithm
	Point Signature(std::string msg_hash, cpp_int secret_key, Point public_point) {

		cpp_int r = 0;
		cpp_int s = 0;
		cpp_int z = bigint(string_to_binary(msg_hash));

		do {
			cpp_int k = getSecretKey(); /* This is just a random value, less than the order of the curve. */
			Point p1 = PointMultiplication(k, G);
			r = modulo(p1.x, n);
			s = modulo(modInverse(k,n) * (z + r * secret_key), n) ;
		} while(r == 0 || s == 0);

		return {r,s};
	}

	bool VerifySignature(std::string msg_hash, Point signature, Point public_key) {
		/* Checking parameters */
		if( public_key == Point{0,0}){
			return false;
		}
		if( !pointIsInEllipticCurve(public_key)){
			return false;
		}
		if( !(PointMultiplication(n, public_key) == Point{0,0})) {
			return false;
		}

		cpp_int r = signature.x;
		cpp_int s = signature.y;
		cpp_int z = bigint(string_to_binary(msg_hash));

		if( !(r > 0 && r < n && s > 0 && s < n) ) {
			return false;
		}

		cpp_int w  = modInverse(s, n);
		cpp_int u1 = modulo(z * w, n);
		cpp_int u2 = modulo(r * w, n);

		Point curve_point = PointAdd(PointMultiplication(u1, G),
									 PointMultiplication(u2, public_key));

		if( ! pointIsInEllipticCurve(curve_point)){
			return false;
		}

		return (r == curve_point.x);
	}

public:

	EllipticCurve(cpp_int p, cpp_int a, cpp_int b, Point generator, cpp_int n, cpp_int h):
	p(p), a(a), b(b), G(generator), n(n), h(h)
	{

	}

	cpp_int getSecretKey(){
		return gen()%(n-1) + 1;
	}

	Point getPublicValue(cpp_int secretKey) {
		return PointMultiplication(secretKey, G);
	}

	Point computeSharedSecret(cpp_int secretKey, Point publicPoint) {
		return PointMultiplication(secretKey, publicPoint);
	}

	bool pointIsInEllipticCurve(Point pt) {
		return modulo(pow(pt.y, 2), p) == modulo(pow(pt.x,3) + a*pt.x + b, p);
	}

	void Sign(const std::string fileName, cpp_int secretKey, Point publicPoint, bool printFileHash = false) {

		std::ofstream ofs(fileName + ".sig");
		std::string fileHash = sha256_file(fileName);
		if( printFileHash ){
			std::cout << "File hash: " << fileHash << std::endl;
		}

		Point signature = Signature(fileHash, secretKey, publicPoint);
		ofs << signature;
	}

	bool Validate(const std::string fileName, const std::string sigFileName, Point publicKey) {

		Point sig;
		std::string fileHash = sha256_file(fileName);
		std::ifstream ifs(sigFileName);

		ifs >> sig;
		return VerifySignature(fileHash, sig, publicKey);
	}
};


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;

int main()
{
  srand((unsigned int) time(0));
  Matrix3d m = Matrix3d::Random();
  cout << "Here is the matrix m:" << endl << m << endl;
  Matrix3d inverse;
  bool invertible;
  double determinant;
  m.computeInverseAndDetWithCheck(inverse,determinant, invertible);
  cout << "Its determinant is " << determinant << endl;
  if (invertible) {
    cout << "It is invertible, and its inverse is:" << endl << inverse << endl;
  }
  else {
    cout << "It is not invertible." << endl;
  }

  return 0;
}


/** MIT License

Copyright (c) 2018 Benjamin Bercovici and Jay McMahon

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
@file   main.cpp
@Author Benjamin Bercovici (bebe0705@colorado.edu)
@date   July, 2017
@brief  main.cpp Implementation of the dcm_transform example
*/

#include <RigidBodyKinematics.hpp>
#include <armadillo>
#include <iostream>

int main() {

	// Transforming a set of 313 euler angles sequence into a Direction Cosine Matrix
	arma::vec angles_313 = {0.1, 0.2, 0.3};
	arma::mat dcm = RBK::euler313_to_dcm(angles_313);

	// Converting the angles to degrees and recomputing the dcm
	arma::vec angles_313_deg = 180 / arma::datum::pi * angles_313;
	arma::mat dcm_deg = RBK::euler313d_to_dcm(angles_313_deg);

	// Ensuring that the two are the same
	std::cout << "Are the DCMs equal? ";
	if (arma::approx_equal(dcm, dcm_deg, "absdiff", 1e-6)) {
		std::cout << "Yes "  << std::endl;
	}
	else {
		std::cout << "No "  << std::endl;
	}

	// The dcm is then converted to a quaternion
	arma::vec quat = RBK::dcm_to_quat(dcm);

	// Is our quaternion of unit norm?
	std::cout << "Quaternion norm: " << arma::norm(quat) << std::endl;

	// The quaternion is then converted to a set of Modified Rodrigues Parameters
	arma::vec mrp = RBK::quat_to_mrp(quat);

	// The mrp is converted back to a set of 321 euler angles
	arma::vec angles_321 = RBK::mrp_to_euler321(mrp);

	// These angles are different from the 313 euler angles
	std::cout << "Are the 321 and 313 sequences the same? " << std::endl;
	std::cout << "321: " << angles_321.t();
	std::cout << "313: " << angles_313.t();

	// But these two sequences should be equivalent
	std::cout << "Are the 321 and 313 sequences equivalent? " ;

	if (arma::approx_equal(angles_313, RBK::mrp_to_euler313(RBK::euler321_to_mrp(angles_321)), "absdiff", 1e-6)) {
		std::cout << " Yes "  << std::endl;
	}
	else {
		std::cout << " No "  << std::endl;
	}

	return 0;

}