#ifndef TRIUMF_MATH_PDF_HPP
#define TRIUMF_MATH_PDF_HPP

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

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace math {

// probability density function (PDF)
namespace pdf {

/// exponentially modified Gaussian distribution
template <typename T = double>
T exponentially_modified_gaussian(T x, T mu, T sigma, T lambda) {
  return boost::math::constants::half<T>() * lambda *
         std::exp(2.0 * mu + lambda * sigma * sigma - 2.0 * x) *
         std::erfc((mu + lambda * sigma * sigma - x) /
                   (boost::math::constants::root_two<T>() * sigma));
}

/// exponentially modified Gaussian distribution (ROOT interface)
template <typename T = double>
T exponentially_modified_gaussian(const T *x, const T *par) {
  return exponentially_modified_gaussian<T>(*x, par[0], par[1], par[2]);
}

/// skew-normal distribution
template <typename T = double>
T skew_normal_distribution(T x, T mu, T sigma, T alpha) {
  boost::math::skew_normal_distribution<T> distribution(mu, sigma, alpha);
  return boost::math::pdf(distribution, x);
}

/// skew-normal distribution (ROOT interface)
template <typename T = double>
T skew_normal_distribution(const T *x, const T *par) {
  return skew_normal_distribution<T>(*x, par[0], par[1], par[2]);
}

/// modified beta distribution - x in [0, x_max]
template <typename T = double> T modified_beta(T x, T alpha, T beta, T x_max) {
  if (x <= 0.0 or x >= x_max) {
    return 0.0;
  }
  T y = x / x_max;
  boost::math::beta_distribution<T> distribution(alpha, beta);
  return boost::math::pdf(distribution, y) / x_max;
}

/// modified beta distribution - x in [0, x_max] (ROOT interface)
template <typename T = double> T modified_beta(const T *x, const T *par) {
  T alpha = par[0];
  T beta = par[1];
  T x_max = par[2];
  return modified_beta<T>(*x, alpha, beta, x_max);
}

/// two modified beta distributions.
template <typename T = double>
T two_modified_beta(T x, T alpha_1, T beta_1, T x_max_1, T fraction_1,
                    T alpha_2, T beta_2, T x_max_2) {
  return fraction_1 * modified_beta<T>(x, alpha_1, beta_1, x_max_1) +
         (1.0 - fraction_1) * modified_beta<T>(x, alpha_2, beta_2, x_max_2);
}

/// modified non-central beta distribution - x in [0, x_max]
template <typename T = double>
T modified_non_central_beta(T x, T alpha, T beta, T lambda, T x_max) {
  if (x <= 0.0 or x >= x_max) {
    return 0.0;
  }
  T y = x / x_max;
  if (lambda == 0.0) {
      return modified_beta(x, alpha, beta, x_max);
  }
  boost::math::non_central_beta_distribution<T> distribution(alpha, beta,
                                                             lambda);
  return boost::math::pdf(distribution, y) / x_max;
}

/// modified non-central beta distribution - x in [0, x_max] (ROOT interface)
template <typename T = double>
T modified_non_central_beta(const T *x, const T *par) {
  return modified_non_central_beta<T>(*x, par[0], par[1], par[2], par[3]);
}

} // namespace pdf

} // namespace math

} // namespace triumf

#endif // TRIUMF_MATH_PDF_HPP


#include <boost/test/unit_test.hpp>

#include "combinatoire.h"

BOOST_AUTO_TEST_SUITE(test_combinatoire)

    BOOST_AUTO_TEST_CASE(coefficient_binomial) {
        BOOST_CHECK_EQUAL(35, combinatoire::coefficient_binomial(7, 3));
        BOOST_CHECK_EQUAL(70, combinatoire::coefficient_binomial(8, 4));
        BOOST_CHECK_EQUAL(1, combinatoire::coefficient_binomial(40, 0));
    }

    BOOST_AUTO_TEST_CASE(catalan) {
        BOOST_CHECK_EQUAL(1, combinatoire::catalan(0));
        BOOST_CHECK_EQUAL(1, combinatoire::catalan(1));
        BOOST_CHECK_EQUAL(16796, combinatoire::catalan<unsigned long long>(10));
    }

    BOOST_AUTO_TEST_CASE(factorielle) {
        BOOST_CHECK_EQUAL(120, combinatoire::factorielle(5));
        BOOST_CHECK_EQUAL(720, combinatoire::factorielle(6));
    }

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/LinearModel.h>

#include <cmath>
#include <iostream>
#include <utility>

#include <Eigen/Cholesky>

using namespace bfl;
using namespace Eigen;

LinearModel::LinearModel
(
    const LinearMatrixComponent& linear_matrix_component,
    const Ref<const MatrixXd>& noise_covariance_matrix,
    const unsigned int seed
) :
    LTIMeasurementModel(MatrixXd::Zero(linear_matrix_component.second.size(), linear_matrix_component.first), noise_covariance_matrix),
    generator_(std::mt19937_64(seed)),
    distribution_(std::normal_distribution<double>(0.0, 1.0)),
    gauss_rnd_sample_([&] { return (distribution_)(generator_); })
{
    for (std::size_t i = 0; i < linear_matrix_component.second.size(); ++i)
    {
        const std::size_t& component_index = linear_matrix_component.second[i];

        if (component_index < linear_matrix_component.first)
            H_(i, component_index) = 1.0;
        else
            throw std::runtime_error(std::string("ERROR::LINEARMODEL::CTOR\nERROR:\n\tIndex component out of bound.\nLOG:\n\tProvided: ") + std::to_string(component_index) + ". Index bound: " + std::to_string(linear_matrix_component.first) + ".");
    }

    LDLT<MatrixXd> chol_ldlt(R_);
    sqrt_R_ = (chol_ldlt.transpositionsP() * MatrixXd::Identity(R_.rows(), R_.cols())).transpose() * chol_ldlt.matrixL() * chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();
}

LinearModel::LinearModel(const LinearMatrixComponent& linear_matrix_component, const Ref<const MatrixXd>& noise_covariance_matrix) :
    LinearModel(linear_matrix_component, noise_covariance_matrix, 1)
{ }

std::pair<bool, MatrixXd> LinearModel::getNoiseSample(const int num) const
{
    MatrixXd rand_vectors(2, num);
    for (int i = 0; i < rand_vectors.size(); i++)
        *(rand_vectors.data() + i) = gauss_rnd_sample_();

    MatrixXd noise_sample = sqrt_R_ * rand_vectors;

    return std::make_pair(true, std::move(noise_sample));
}

std::pair<bool, MatrixXd> LinearModel::getNoiseCovarianceMatrix() const
{
    return std::make_pair(true, R_);
}

Eigen::MatrixXd LinearModel::getMeasurementMatrix() const
{
    return H_;
}


#pragma once

#include "perceive/foundation.hpp"
#include "perceive/utils/sdbm-hash.hpp"
#include "vector-3.hpp"
#include <Eigen/Core>

namespace perceive
{
// --------------------------------------------------------------------- Vector3

#pragma pack(push, 1)
template<typename T> class Vector4T
{
 public:
   using value_type = T;

   T x, y, z, w;

   Vector4T() noexcept
       : x(T(0.0))
       , y(T(0.0))
       , z(T(0.0))
       , w(T(0.0))
   {}
   Vector4T(T x_, T y_, T z_, T w_) noexcept
       : x(x_)
       , y(y_)
       , z(z_)
       , w(w_)
   {}
   Vector4T(const float p[4]) noexcept
   {
      x = p[0];
      y = p[1];
      z = p[2];
      w = p[3];
   }
   Vector4T(const double p[4]) noexcept
   {
      x = p[0];
      y = p[1];
      z = p[2];
      w = p[3];
   }
   Vector4T(const Vector3T<T>& p, T w_ = T(0.0)) noexcept
       : x(p.x)
       , y(p.y)
       , z(p.z)
       , w(w_)
   {}
   Vector4T(const Vector3T<T>& a,
            const Vector3T<T>& b,
            const Vector3T<T>& c) noexcept
   {
      *this = plane_from_3_points(a, b, c);
   }

   Vector4T& operator=(const Vector4T& v) = default;

   Vector4T& operator=(const Eigen::Vector4d& v) noexcept
   {
      for(int i = 0; i < 4; ++i) this->operator[](i) = v(i);
      return *this;
   }
   Vector4T& operator=(const Eigen::Vector4f& v) noexcept
   {
      for(int i = 0; i < 4; ++i) this->operator[](i) = v(i);
      return *this;
   }

   static Vector4T nan() noexcept
   {
      return Vector4T(T(NAN), T(NAN), T(NAN), T(NAN));
   }

   unsigned size() const noexcept { return 4; }

   Vector4T& normalise(T epsilon = 1e-9) noexcept
   {
      // Don't normalize if we don't have to
      T mag2 = quadrance();
      if(fabs(mag2 - T(1.0)) > epsilon) {
         T mag_inv = T(1.0) / sqrt(mag2);
         x *= mag_inv;
         y *= mag_inv;
         z *= mag_inv;
         w *= mag_inv;
      }
      return *this;
   }

   Vector4T& normalise_plane(T epsilon = 1e-9) noexcept
   {
      T mag2 = x * x + y * y + z * z;
      if(fabs(mag2 - T(1.0)) > epsilon) *this *= T(1.0) / sqrt(mag2);
      return *this;
   }

   Vector4T& normalise_point(T epsilon = 1e-9) noexcept
   {
      if(fabs(w - T(1.0)) > epsilon) *this *= T(1.0) / w;
      return *this;
   }

   Vector4T normalised(T epsilon = 1e-9) const noexcept
   {
      Vector4T res = *this;
      res.normalise(epsilon);
      return res;
   }
   Vector4T normalised_plane(T epsilon = 1e-9) const noexcept
   {
      Vector4T res = *this;
      res.normalise_plane(epsilon);
      return res;
   }
   Vector4T normalised_point(T epsilon = 1e-9) const noexcept
   {
      Vector4T res = *this;
      res.normalise_point(epsilon);
      return res;
   }

   Vector4T& normalize(T ep = 1e-9) noexcept { return normalise(ep); }
   Vector4T& normalize_plane(T ep = 1e-9) noexcept
   {
      return normalise_plane(ep);
   }
   Vector4T& normalize_point(T ep = 1e-9) noexcept
   {
      return normalise_point(ep);
   }

   Vector4T normalized(T epsilon = 1e-9) const noexcept
   {
      return normalised(epsilon);
   }
   Vector4T normalized_plane(T ep = 1e-9) const noexcept
   {
      return normalised_plane(ep);
   }
   Vector4T normalized_point(T ep = 1e-9) const noexcept
   {
      return normalised_point(ep);
   }

   T quadrance() const noexcept { return x * x + y * y + z * z + w * w; }
   T norm() const noexcept { return sqrt(quadrance()); }
   T dot(const Vector4T& rhs) const noexcept
   {
      return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
   }
   T distance(const Vector4T& rhs) const noexcept
   {
      return (*this - rhs).norm();
   }

   Vector3T<T>& xyz() noexcept
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
#endif
      return *(reinterpret_cast<Vector3T<T>*>(this));
   }
   const Vector3T<T>& xyz() const noexcept
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
#endif
      return *(reinterpret_cast<const Vector3T<T>*>(this));
   }
   T& d() noexcept { return w; } // for plane
   const T& d() const noexcept { return w; }

   Vector4T& set_to(const T& a, const T& b, const T& c, const T& d) noexcept
   {
      x = a;
      y = b;
      z = c;
      w = d;
      return *this;
   }
   Vector4T& set_to(T a[4]) noexcept
   {
      set_to(a[0], a[1], a[2], a[3]);
      return *this;
   }

   T* copy_to(T a[4]) const noexcept
   {
      a[0] = x;
      a[1] = y;
      a[2] = z;
      a[3] = w;
      return a;
   }

   T* ptr() noexcept
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
      assert(&w == reinterpret_cast<const T*>(this) + 3);
#endif
      return &x;
   }

   const T* ptr() const noexcept
   {
      return const_cast<Vector4T<T>*>(this)->ptr();
   }

   T& operator[](int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   const T& operator[](int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }
   const T& operator()(int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   Vector4T round() const noexcept
   {
      return Vector4T(floor(x + T(0.499)),
                      floor(y + T(0.499)),
                      floor(z + T(0.499)),
                      floor(w + T(0.499)));
   }

   Vector4T& operator*=(T scalar) noexcept
   {
      x *= scalar;
      y *= scalar;
      z *= scalar;
      w *= scalar;
      return *this;
   }
   Vector4T& operator/=(T scalar) noexcept
   {
      x /= scalar;
      y /= scalar;
      z /= scalar;
      w /= scalar;
      return *this;
   }
   Vector4T operator*(T scalar) const noexcept
   {
      Vector4T res(*this);
      res *= scalar;
      return res;
   }
   Vector4T operator/(T scalar) const noexcept
   {
      Vector4T res(*this);
      res /= scalar;
      return res;
   }

   Vector4T& operator+=(const Vector4T& rhs) noexcept
   {
      x += rhs.x;
      y += rhs.y;
      z += rhs.z;
      w += rhs.w;
      return *this;
   }
   Vector4T& operator-=(const Vector4T& rhs) noexcept
   {
      x -= rhs.x;
      y -= rhs.y;
      z -= rhs.z;
      w -= rhs.w;
      return *this;
   }
   Vector4T operator+(const Vector4T& rhs) const noexcept
   {
      Vector4T res(*this);
      res += rhs;
      return res;
   }
   Vector4T operator-(const Vector4T& rhs) const noexcept
   {
      Vector4T res(*this);
      res -= rhs;
      return res;
   }
   Vector4T operator-() const noexcept { return Vector4T(-x, -y, -z, -w); }

   bool operator==(const Vector4T& rhs) const noexcept
   {
      return x == rhs.x and y == rhs.y and z == rhs.z and w == rhs.w;
   }
   bool operator!=(const Vector4T& rhs) const noexcept
   {
      return !(*this == rhs);
   }

   std::string to_string(const char* fmt = "{{{}, {}, {}, {}}}") const noexcept
   {
      return format(fmt, x, y, z, w);
   }

   std::string to_str() const { return format("[{}, {}, {}, {}]", x, y, z, w); }

   bool is_nan() const noexcept
   {
      return std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w);
   }

   bool is_finite() const noexcept
   {
      return std::isfinite(x) && std::isfinite(y) && std::isfinite(z)
             && std::isfinite(w);
   }

   inline friend bool isfinite(const Vector4T& o) noexcept
   {
      return o.is_finite();
   }

   bool is_unit_vector(T epsilon = 1e-9) const noexcept
   {
      return is_finite() && fabs(quadrance() - 1.0) < epsilon;
   }

   void print(const char* msg = NULL, bool newline = true) const noexcept
   {
      printf("%s%s%s%s",
             (msg == NULL ? "" : msg),
             (msg == NULL ? "" : " "),
             to_string().c_str(),
             (newline ? "\n" : ""));
      fflush(stdout);
   }

   // Homogenous point
   bool pt_at_infinity(T epsilon = 1e-9) const noexcept
   {
      return fabs(w) < epsilon;
   }

   size_t hash() const noexcept { return sdbm_hash(ptr(), sizeof(T) * size()); }

   // Plane functions
   T side(const Vector3T<T>& o) const noexcept
   {
      return o.x * x + o.y * y + o.z * z + d();
   }

   // WARNING, must be normalised
   double point_plane_distance(const Vector3T<T>& point) const noexcept
   {
      assert(fabs(xyz().quadrance() - 1.0) < 1e-9);
      return fabs(side(point));
   }

   Vector3T<T> image(const Vector3T<T>& p) const noexcept
   {
      assert(fabs(xyz().quadrance() - 1.0) < 1e-9);
      return p - xyz() * side(p);
   }
   Vector3T<T> reflect(const Vector3T<T>& p) const noexcept
   {
      assert(fabs(xyz().quadrance() - 1.0) < 1e-9);
      return p - 2.0 * xyz() * side(p);
   }

   // Reflect a plane
   Vector4T<T> reflect(const Vector4T<T>& p) const noexcept
   {
      auto norm = (p.xyz() - 2.0 * p.xyz().dot(xyz()) * xyz()).normalised();
      auto C    = reflect(p.image(Vector3T<T>(0.0, 0.0, 0.0)));
      return Vector4T<T>(norm, -C.dot(norm));
   }

   friend std::string str(const Vector4T<T>& o) noexcept
   {
      return o.to_string();
   }
};
#pragma pack(pop)

template<typename T>
Vector4T<T> operator*(float a, const Vector4T<T>& v) noexcept
{
   return v * a;
}

template<typename T>
Vector4T<T> operator/(float a, const Vector4T<T>& v) noexcept
{
   return v / a;
}

template<typename T>
Vector4T<T> operator*(double a, const Vector4T<T>& v) noexcept
{
   return v * a;
}

template<typename T>
Vector4T<T> operator/(double a, const Vector4T<T>& v) noexcept
{
   return v / a;
}

template<typename T>
Vector4T<T> plane_from_3_points(const Vector3T<T>& a,
                                const Vector3T<T>& b,
                                const Vector3T<T>& c) noexcept
{
   Vector4T<T> ret;

   auto ab   = a - b;
   auto ac   = a - c;
   ret.xyz() = ab.cross(ac);
   ret.xyz().normalise();
   ret.d() = -1.0 * dot(a, ret.xyz());

   return ret;
}

template<typename T>
inline T ray_position_relative_to_plane_t(const Vector4T<T>& p3,
                                          const Vector3T<T>& a,
                                          const Vector3T<T>& b,
                                          const T side) noexcept
{
   return (side - p3.d() - dot(p3.xyz(), a)) / dot(p3.xyz(), b - a);
}

template<typename T>
inline Vector3T<T> ray_position_relative_to_plane(const Vector4T<T>& p3,
                                                  const Vector3T<T>& a,
                                                  const Vector3T<T>& b,
                                                  const T side) noexcept
{
   const auto t = ray_position_relative_to_plane_t(p3, a, b, side);
   return a + t * (b - a);
}

template<typename T>
inline T plane_ray_intersection_t(const Vector4T<T>& p,
                                  const Vector3T<T>& a,
                                  const Vector3T<T>& b) noexcept
{
   static_assert(std::is_floating_point<T>::value);
   return ray_position_relative_to_plane_t(p, a, b, T(0.0));
}

template<typename T>
inline Vector3T<T> plane_ray_intersection(const Vector4T<T>& p,
                                          const Vector3T<T>& a,
                                          const Vector3T<T>& b) noexcept
{
   static_assert(std::is_floating_point<T>::value);
   return a + plane_ray_intersection_t(p, a, b) * (b - a);
}

// String shim
template<typename T> std::string str(const Vector4T<T>& v) noexcept
{
   return v.to_string();
}
template<typename T>
std::ostream& operator<<(std::ostream& out, const Vector4T<T>& v) noexcept
{
   out << v.to_string();
   return out;
}

} // namespace perceive


#include <vector>
#include <Eigen/Dense>

# pragma once

using std::vector; using std::string;

typedef Eigen::MatrixXd matrix;

int atom(int ao_index, int orbitals_per_atom);

int orb_index(int ao_index, int orbitals_per_atom);

matrix calculate_fock_matrix(matrix hamiltonian_matrix, matrix interaction_matrix, matrix density_matrix, int orbitals_per_atom, double dipole);


#ifndef CANNON_CONTROL_LQR_H
#define CANNON_CONTROL_LQR_H

/*!
 * \file cannon/control/lqr.hpp
 * \brief File containing utility functions for computing LQR controllers in
 * discrete and continuous time.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace control {

    /*!
     * \brief Class representing an LQR controller for a continuous or
     * discrete-time system.
     */
    class LQRController {
      public:

        using LinearizationFunction =
            std::function<void(const Ref<const VectorXd> &, Ref<MatrixXd>,
                               Ref<MatrixXd>)>;

        LQRController() = delete;

        /*!
         * \brief Constructor taking state to stabilize around, control
         * dimension, whether the system to be controlled is continuous-time,
         * and linearization function for the system to be controlled.
         *
         * Initializes cost matrices to identity.
         */
        LQRController(const Ref<const VectorXd> &q0,
                      unsigned int control_dim,
                      LinearizationFunction f,
                      bool continuous = true);

        /*!
         * \brief Constructor taking state to stabilize around, state
         * cost matrix, control cost matrix, whether the system to be
         * controlled is continuous-time, and linearization function for the
         * system to be controlled.
         */
        LQRController(const Ref<const VectorXd> &q0,
                      const Ref<const MatrixXd> &Q,
                      const Ref<const MatrixXd> &R, LinearizationFunction f,
                      bool continuous = true);

        /*!
         * \brief Compute the control to apply at the input state.
         *
         * \param q The state to compute control for.
         *
         * \returns The computed control.
         */
        VectorXd compute_control(const Ref<const VectorXd>& q) const;

        /*!
         * \brief Get the linear portion of the control law represented by this
         * controller.
         *
         * \returns Gain matrix.
         */
        MatrixXd get_linear_gain() const;

        /*!
         * \brief Get the constant offset portion of the control law
         * represented by this controller.
         *
         * \returns Offset vector.
         */
        VectorXd get_control_offset() const;

        /*!
         * \brief Set the state that this controller attempts to stabilize to.
         *
         * \param q The new state to stabilize to.
         */
        void set_target(const Ref<const VectorXd>& q);

      private:

        /*!
         * \brief Compute LQR gain for controller represented by this object.
         */
        void compute_lqr_gain_();

        VectorXd q0_; //!< State to stabilize toward
        MatrixXd Q_; //!< State cost matrix
        MatrixXd K_; //!< Control gain matrix
        MatrixXd R_; //!< Control cost matrix
        LLT<MatrixXd> R_cholesky_; //!< Cholesky factorization of control cost matrix
        LinearizationFunction linearization_; //!< Linearization function for system to be controlled
        bool continuous_; //!< Whether the system to be controlled is continuous-time
    };

    // Free Functions
    /*!
     * \brief Solve the continuous-time algebraic riccati equation for
     * the input state and control derivative matrices A and B, and
     * return the resulting steady-state LQR gain.
     *
     * \param A Partial derivatives of ode with respect to state.
     * \param B Partial derivatives of ode with respect to control.
     * \param Q State cost matrix
     * \param R Control cost matrix Cholesky decomposition
     *
     * \returns CARE solution control gain matrix.
     */
  MatrixXd continuous_algebraic_riccati_equation(const Ref<const MatrixXd> &A,
                                                 const Ref<const MatrixXd> &B,
                                                 const Ref<const MatrixXd> &Q,
                                                 const LLT<MatrixXd> &R_cholesky);

  } // namespace control
} // namespace cannon

#endif /* ifndef CANNON_CONTROL_LQR_H */


//
//  Copyright Markus Rickert 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas/blas.hpp>
#include <boost/numeric/bindings/traits/c_array.hpp>
#include <boost/numeric/bindings/traits/c_array2.hpp>
#include <boost/numeric/bindings/traits/dense_traits.hpp>
#include <boost/numeric/bindings/traits/std_valarray.hpp>
#include <boost/numeric/bindings/traits/std_valarray2.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/bindings/traits/std_vector2.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

int
main(int argc, char** argv)
{
	{
		// a * b' = C ; a' * b = d

		boost::numeric::ublas::vector<double> a(3);
		for (std::size_t i = 0; i < a.size(); ++i) a(i) = i;
		std::cout << "a=" << a << std::endl;

		boost::numeric::ublas::vector<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b(i) = i;
		std::cout << "b=" << b << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "c=" << c << std::endl;

		boost::numeric::ublas::vector<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=" << d << std::endl;
	}

	std::cout << std::endl;

	{
		// a * b' = C ; a' * b = d

		std::vector<double> a(3);
		for (std::size_t i = 0; i < a.size(); ++i) a[i] = i;
		std::cout << "a=[" << a.size() << "](";
		for (std::size_t i = 0; i < a.size(); ++i) std::cout << (i > 0 ? "," : "") << a[i];
		std::cout << ")" << std::endl;

		std::valarray<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b[i] = i;
		std::cout << "b=[" << b.size() << "](";
		for (std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
		std::cout << ")" << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "c=" << c << std::endl;

		std::vector<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=[" << d.size() << "](";
		for (std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
		std::cout << ")" << std::endl;
	}

	std::cout << std::endl;

	{
		// a * b' = C ; a' * b = d

		double a[3];
		for (std::size_t i = 0; i < 3; ++i) a[i] = i;
		std::cout << "a=[" << 3 << "](";
		for (std::size_t i = 0; i < 3; ++i) std::cout << (i > 0 ? "," : "") << a[i];
		std::cout << ")" << std::endl;

		std::vector<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b[i] = i;
		std::cout << "b=[" << b.size() << "](";
		for (std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
		std::cout << ")" << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "c=" << c << std::endl;

		std::valarray<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=[" << d.size() << "](";
		for (std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
		std::cout << ")" << std::endl;
	}

	return 0;
}


#include <cmath>

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

#include <BingoCpp/explicit_regression.h>

#include "test_fixtures.h"

using namespace bingo;

namespace {

class TestExplicitRegression : public testing::Test {
 public:
  ExplicitTrainingData* training_data_;
  testutils::SumEquation sum_equation_;

  void SetUp() {
    training_data_ = init_sample_training_data();
    sum_equation_ = testutils::init_sum_equation();
  }

  void TearDown() {
    delete training_data_;
  }

 private:
  ExplicitTrainingData* init_sample_training_data() {
    const int num_points = 50;
    const int num_data_per_feature = 10;
    const int num_feature = 50 / num_data_per_feature;
    Eigen::ArrayXXd x = Eigen::ArrayXd::LinSpaced(num_points, 0, 0.98);
    x = x.reshaped(num_feature, num_data_per_feature);
    x.transposeInPlace();
    Eigen::Array<double, 10, 1> y = Eigen::ArrayXd::LinSpaced(10, 0.2, 4.7);
    return new ExplicitTrainingData(x, y);
  }
};

TEST_F(TestExplicitRegression, EvaluateIndividualFitness) {
  ExplicitRegression regressor(training_data_);
  double fitness = regressor.EvaluateIndividualFitness(sum_equation_);
  ASSERT_TRUE(fitness < 1e-10);
}

TEST_F(TestExplicitRegression, EvaluateIndividualFitnessWithNaN) {
  training_data_->x(0, 0) = std::numeric_limits<double>::quiet_NaN();
  ExplicitRegression regressor(training_data_);
  double fitness = regressor.EvaluateIndividualFitness(sum_equation_);
  ASSERT_TRUE(std::isnan(fitness));
}

TEST_F(TestExplicitRegression, GetSubsetOfTrainingData) {
  Eigen::ArrayXXd data_input = Eigen::ArrayXd::LinSpaced(5, 0, 4);
  ExplicitTrainingData* training_data = new ExplicitTrainingData(data_input, data_input);
  ExplicitTrainingData* subset_training_data = training_data->GetItem(std::vector<int>{0, 2, 3});

  Eigen::ArrayXXd expected_subset(3, 1);
  expected_subset << 0, 2, 3;
  ASSERT_TRUE(subset_training_data->x.isApprox(expected_subset));
  ASSERT_TRUE(subset_training_data->y.isApprox(expected_subset));
  delete training_data,
  delete subset_training_data;
}

TEST_F(TestExplicitRegression, CorrectTrainingDataSize) {
  for (int size : std::vector<int> {2, 5, 50}) {
    Eigen::ArrayXXd data_input = Eigen::ArrayXd::LinSpaced(size, 0, 10);
    ExplicitTrainingData* training_data = new ExplicitTrainingData(data_input, data_input);
    ASSERT_EQ(training_data->Size(), size);
    delete training_data;
  }
}
} // namespace


//test_duration_mc_var.cpp

//test Treasury portfolio VaR through MC simulation

#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>

#include<memory>

#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

#include "compute_returns_eigen.h"
#include "compute_var.h"
#include "path.h"
#include "ptf_var.h"
#include "rng.h"
#include "portfolio.h"

void readCSV(std::istream &input, std::vector< std::vector<std::string> > &output)
//https://www.gamedev.net/topic/444193-c-how-to-load-in-a-csv-file/
{
	std::string csvLine;
	// read every line from the stream
	while( std::getline(input, csvLine) )
	{
		std::istringstream csvStream(csvLine);
		std::vector<std::string> csvColumn;
		std::string csvElement;
		// read every element from the line that is seperated by commas
		// and put it into the vector or strings
		while( std::getline(csvStream, csvElement, ',') )
		{
			csvColumn.push_back(csvElement);
		}
		output.push_back(csvColumn);
	}
}

int main()
{

    try{

    // Read US Treasury Zero-Coupon Yield Curve
    // daily series obtained from US Federal Reserve Data Releases
	//https://www.quandl.com

	std::fstream file("/home/mrnoname/Documents/VaR/data/TermStructureData.csv", ios::in);
	if(!file.is_open())
	{
		std::cout << "File not found!\n";
		return 1;
	}
	// typedef to save typing for the following object
	typedef std::vector< std::vector<std::string> > csvVector;
	csvVector csvData;

	readCSV(file, csvData);

    //test
    for(size_t i = 0;i < 5; ++i){
        for(size_t j = 0;j < csvData[i].size();++j){
            cout << csvData[i][j] << '\t';

        }

        cout << endl;
    }
    cout << endl;

    // Remove lines with missing values

    size_t n(csvData.size() - 1);
    size_t m(csvData[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n-1062));

    for(size_t i = 1062;i < n;++i){
        for(size_t j = 1;j < csvData[i].size();++j){
            std::string tmp = csvData[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1062] = 99999.;
            }
            else{
                _prices[j-1][i-1062] = 1. - std::stod(tmp)/100.;
            }
        }
    }

    std::vector<std::string> indexNames(csvData[0].size() - 1);

    for(size_t i = 1;i < csvData[0].size();++i){
        indexNames[i-1] = csvData[0][i];
    }

	//Remove missing values to compute trailling returns
    //Asynchornous time series. Shift to the next value
    Mat prices;
    prices.resize(m,Vec(0));

	for(size_t i = 0;i < _prices.size();++i){
        for(size_t j = 0;j < _prices[i].size();++j){
            if(!((_prices[i][j] == 99999) || (_prices[i][j] == 0)))
                prices[i].push_back(_prices[i][j]);
        }
	}

    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,252,true));
	// 252 / 4 = 63 - 3 months
    // 4 * 252 = 1008 use 4 years of data to compute mean, and std dev

    //-------------------------------------------------------------------------
    // Compute Monte Carlo VaR

	// Simulate yield chge through brute force Monte-Carlo

	Path1x1 process;

	std::vector<Path1x1> processes(7);

	for(size_t i = 0;i < 7;++i) processes[i] = Path1x1();

	HistoricalVaR var1;

    // Portfolio with 7 durations across the yield curve

	double a = double(1./7.);

	std::vector<double> weights{a,a,a,a,a,a,a}; //initialization. Equi-weighted asset for mere convenience

	Ptf _ptf;

	_ptf.push_back(std::make_pair(0,shared_ptr<Instrument> (new FI(.960/100.)))); //1 yr
	_ptf.push_back(std::make_pair(1,shared_ptr<Instrument> (new FI(1.918/100.)))); //2 yrs
	_ptf.push_back(std::make_pair(2,shared_ptr<Instrument> (new FI(2.913/100.)))); //3 yrs
	_ptf.push_back(std::make_pair(4,shared_ptr<Instrument> (new FI(4.704/100.)))); //5 yrs
	_ptf.push_back(std::make_pair(6,shared_ptr<Instrument> (new FI(6.406/100.)))); // 7 yrs
	_ptf.push_back(std::make_pair(9,shared_ptr<Instrument> (new FI(8.874/100.)))); // 10  yrs
	_ptf.push_back(std::make_pair(29,shared_ptr<Instrument> (new FI(19.592/100.)))); // 30 yrs

    /*
    http://online.wsj.com/mdc/public/page/2_3022-bondmkt.html

    The Bond Market: Ryan Indexes
    Tuesday, December 20, 2016

    1 yr Treasury .960
    2 yr Treasury 1.918
    3 yr Treasury 2.913
    5 yr Treasury 4.704
    7 yr Treasury 6.406
    10 yr Treasury 8.874
    30 yr Treasury 19.592
    */

	shared_ptr<Portfolio> ptf(new Portfolio(_ptf, weights, cr, true, 1.e+07));

	rng _rng;

    //ptf value at risk

	VaRPtfMCCompute<HistoricalVaR, Path1x1> VaRMonteCarlo(ptf,var1, processes, _rng);

	cout << "Monte Carlo VaR: " << VaRMonteCarlo.computeVaR() << endl;

	//simulate portfolio rtn instead instead of component

	VaRMonteCarloCompute<Portfolio, HistoricalVaR, Path1x1> VaRMonteCarlo1(ptf,var1, process, _rng);

	cout << "Monte Carlo VaR - ptf rtn: " << VaRMonteCarlo1.computeVaR() << endl;

    return 0;

    } catch (const std::exception& e) { // caught by reference to base
        std::cout << " a standard exception was caught, with message '"
                  << e.what() << "'\n";
    }

}


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

// The original file belongs to MSCKF_VIO (https://github.com/KumarRobotics/msckf_vio/)
// Some changes have been made to use it in livox_slam_ware

#ifndef MATH_UTILS_HPP
#define MATH_UTILS_HPP

#include <cmath>
#include <Eigen/Dense>

namespace livox_slam_ware {

/*
 *  @brief Create a skew-symmetric matrix from a 3-element vector.
 *  @note Performs the operation:
 *  w   ->  [  0 -w3  w2]
 *          [ w3   0 -w1]
 *          [-w2  w1   0]
 */
inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) {
  Eigen::Matrix3d w_hat;
  w_hat(0, 0) = 0;
  w_hat(0, 1) = -w(2);
  w_hat(0, 2) = w(1);
  w_hat(1, 0) = w(2);
  w_hat(1, 1) = 0;
  w_hat(1, 2) = -w(0);
  w_hat(2, 0) = -w(1);
  w_hat(2, 1) = w(0);
  w_hat(2, 2) = 0;
  return w_hat;
}

/*
 * @brief Normalize the given quaternion to unit quaternion.
 */
inline void quaternionNormalize(Eigen::Vector4d& q) {
  double norm = q.norm();
  q = q / norm;
  return;
}

/*
 * @brief Perform q1 * q2.
 *
 *    Format of q1 and q2 is as [x,y,z,w]
 */
inline Eigen::Vector4d quaternionMultiplication(
    const Eigen::Vector4d& q1,
    const Eigen::Vector4d& q2) {
  Eigen::Matrix4d L;

  // QXC: Hamilton
  L(0, 0) =  q1(3); L(0, 1) = -q1(2); L(0, 2) =  q1(1); L(0, 3) =  q1(0);
  L(1, 0) =  q1(2); L(1, 1) =  q1(3); L(1, 2) = -q1(0); L(1, 3) =  q1(1);
  L(2, 0) = -q1(1); L(2, 1) =  q1(0); L(2, 2) =  q1(3); L(2, 3) =  q1(2);
  L(3, 0) = -q1(0); L(3, 1) = -q1(1); L(3, 2) = -q1(2); L(3, 3) =  q1(3);

  Eigen::Vector4d q = L * q2;
  quaternionNormalize(q);
  return q;
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Section 3.2 "Kalman Filter Update" in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Vector4d smallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Vector4d q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.head<3>() = dq;
    q(3) = std::sqrt(1-dq_square_norm);
  } else {
    q.head<3>() = dq;
    q(3) = 1;
    q = q / std::sqrt(1+dq_square_norm);
  }

  return q;
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Section 3.2 "Kalman Filter Update" in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Quaterniond getSmallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Quaterniond q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.x() = dq(0);
    q.y() = dq(1);
    q.z() = dq(2);
    q.w() = std::sqrt(1-dq_square_norm);
  } else {
    q.x() = dq(0);
    q.y() = dq(1);
    q.z() = dq(2);
    q.w() = 1;
    q.normalize();
  }

  return q;
}

/*
 * @brief Convert a quaternion to the corresponding rotation matrix
 * @note Pay attention to the convention used. The function follows the
 *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for Quaternion Algebra", Equation (78).
 *
 *    The input quaternion should be in the form
 *      [q1, q2, q3, q4(scalar)]^T
 */
inline Eigen::Matrix3d quaternionToRotation(
    const Eigen::Vector4d& q) {
  // QXC: Hamilton
  const double& qw = q(3);
  const double& qx = q(0);
  const double& qy = q(1);
  const double& qz = q(2);
  Eigen::Matrix3d R;
  R(0, 0) = 1-2*(qy*qy+qz*qz);  R(0, 1) =   2*(qx*qy-qw*qz);  R(0, 2) =   2*(qx*qz+qw*qy);
  R(1, 0) =   2*(qx*qy+qw*qz);  R(1, 1) = 1-2*(qx*qx+qz*qz);  R(1, 2) =   2*(qy*qz-qw*qx);
  R(2, 0) =   2*(qx*qz-qw*qy);  R(2, 1) =   2*(qy*qz+qw*qx);  R(2, 2) = 1-2*(qx*qx+qy*qy);

  return R;
}

/*
 * @brief Convert a rotation matrix to a quaternion.
 * @note Pay attention to the convention used. The function follows the
 *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for Quaternion Algebra", Equation (78).
 *
 *    The input quaternion should be in the form
 *      [q1, q2, q3, q4(scalar)]^T
 */
inline Eigen::Vector4d rotationToQuaternion(
    const Eigen::Matrix3d& R) {
  Eigen::Vector4d score;
  score(0) = R(0, 0);
  score(1) = R(1, 1);
  score(2) = R(2, 2);
  score(3) = R.trace();

  int max_row = 0, max_col = 0;
  score.maxCoeff(&max_row, &max_col);

  Eigen::Vector4d q = Eigen::Vector4d::Zero();

  // QXC: Hamilton
  if (max_row == 0) {
    q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;
    q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));
    q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));
    q(3) = (R(2, 1)-R(1, 2)) / (4*q(0));
  } else if (max_row == 1) {
    q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;
    q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));
    q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));
    q(3) = (R(0, 2)-R(2, 0)) / (4*q(1));
  } else if (max_row == 2) {
    q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;
    q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));
    q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));
    q(3) = (R(1, 0)-R(0, 1)) / (4*q(2));
  } else {
    q(3) = std::sqrt(1+R.trace()) / 2.0;
    q(0) = (R(2, 1)-R(1, 2)) / (4*q(3));
    q(1) = (R(0, 2)-R(2, 0)) / (4*q(3));
    q(2) = (R(1, 0)-R(0, 1)) / (4*q(3));
  }

  if (q(3) < 0) q = -q;
  quaternionNormalize(q);
  return q;
}

} // end namespace livox_slam_ware

#endif // MATH_UTILS_HPP


#include "SimpleIndex.h"
#include "cryptoTools/Crypto/PRNG.h"
#include <random>
#include "cryptoTools/Common/Log.h"
#include "cryptoTools/Common/CuckooIndex.h"
#include <numeric>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

namespace osuCrypto
{

    void SimpleIndex::print()
    {

        for (u64 i = 0; i < mBins.size(); ++i)
            //	for (u64 i = 0; i <1; ++i)
        {
            std::cout << "Bin #" << i << std::endl;

            std::cout << " contains " << mBinSizes[i] << " elements" << std::endl;

            for (u64 j = 0; j < mBinSizes[i]; ++j)
            {
                std::cout << "    idx=" << mBins(i, j).idx() << "  hIdx=" << mBins(i, j).hashIdx() << std::endl;
                //	std::cout << "    " << mBins[i].first[j] << "  " << mBins[i].second[j] << std::endl;

            }

            std::cout << std::endl;
        }

        std::cout << std::endl;
    }

    //template<unsigned int N = 16>
    double getBinOverflowProb(u64 numBins, u64 numBalls, u64 getBinSize, double epsilon = 0.0001)
    {
        if (numBalls <= getBinSize)
            return std::numeric_limits<double>::max();

        if (numBalls > std::numeric_limits<i32>::max())
        {
            auto msg = ("boost::math::binomial_coefficient(...) only supports " + std::to_string(sizeof(unsigned) * 8) + " bit inputs which was exceeded." LOCATION);
            std::cout << msg << std::endl;
            throw std::runtime_error(msg);
        }

        //std::cout << numBalls << " " << numBins << " " << binSize << std::endl;
        typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<16>> T;
        T sum = 0.0;
        T sec = 0.0;// minSec + 1;
        T diff = 1;
        u64 i = getBinSize + 1;

        while (diff > T(epsilon) && numBalls >= i /*&& sec > minSec*/)
        {
            sum += numBins * boost::math::binomial_coefficient<T>(i32(numBalls), i32(i))
                * boost::multiprecision::pow(T(1.0) / numBins, i) * boost::multiprecision::pow(1 - T(1.0) / numBins, numBalls - i);

            //std::cout << "sum[" << i << "] " << sum << std::endl;

            T sec2 = boost::multiprecision::log2(sum);
            diff = boost::multiprecision::abs(sec - sec2);
            //std::cout << diff << std::endl;
            sec = sec2;

            i++;
        }

        return std::max<double>(0, (double)-sec);
    }

    u64 SimpleIndex::get_bin_size(u64 numBins, u64 numBalls, u64 statSecParam)
    {

        auto B = std::max<u64>(1, numBalls / numBins);

        double currentProb = getBinOverflowProb(numBins, numBalls, B);
        u64 step = 1;

        bool doubling = true;

        while (currentProb < statSecParam || step > 1)
        {
            if (!step)
                throw std::runtime_error(LOCATION);

            if (statSecParam > currentProb)
            {
                if (doubling) step = std::max<u64>(1, step * 2);
                else          step = std::max<u64>(1, step / 2);

                B += step;
            }
            else
            {
                doubling = false;
                step = std::max<u64>(1, step / 2);
                B -= step;
            }
            currentProb = getBinOverflowProb(numBins, numBalls, B);
        }

        return B;
    }

    void SimpleIndex::init(u64 numBins, u64 numBalls, u64 statSecParam, u64 numHashFunction)
    {
        mNumHashFunctions = numHashFunction;
        mMaxBinSize = get_bin_size(numBins, numBalls * numHashFunction, statSecParam);
        mBins.resize(numBins, mMaxBinSize);
        mBinSizes.resize(numBins, 0);
        mItemToBinMap.resize(numBalls, numHashFunction);
        mNumBins = numBins;
    }

    void SimpleIndex::insertItems(span<block> items, block hashingSeed)
    {

        std::array<block, 8> hashs;
        AES hasher(hashingSeed);

        auto mainSteps = items.size() / hashs.size();
        auto remSteps = items.size() % hashs.size();
        u64 itemIdx = 0;
        if (mNumHashFunctions == 3 )
        {
            for (u64 i = 0; i < mainSteps; ++i, itemIdx += 8)
            {
                auto min = std::min<u64>(items.size() - itemIdx, hashs.size());

                hasher.ecbEncBlocks(items.data() + itemIdx, min, hashs.data());

                auto itemIdx0 = itemIdx + 0;
                auto itemIdx1 = itemIdx + 1;
                auto itemIdx2 = itemIdx + 2;
                auto itemIdx3 = itemIdx + 3;
                auto itemIdx4 = itemIdx + 4;
                auto itemIdx5 = itemIdx + 5;
                auto itemIdx6 = itemIdx + 6;
                auto itemIdx7 = itemIdx + 7;

                hashs[0] = hashs[0] ^ items[itemIdx0];
                hashs[1] = hashs[1] ^ items[itemIdx1];
                hashs[2] = hashs[2] ^ items[itemIdx2];
                hashs[3] = hashs[3] ^ items[itemIdx3];
                hashs[4] = hashs[4] ^ items[itemIdx4];
                hashs[5] = hashs[5] ^ items[itemIdx5];
                hashs[6] = hashs[6] ^ items[itemIdx6];
                hashs[7] = hashs[7] ^ items[itemIdx7];

                auto bIdx00 = CuckooIndex<>::getHash(hashs[0], 0, mNumBins);
                auto bIdx10 = CuckooIndex<>::getHash(hashs[1], 0, mNumBins);
                auto bIdx20 = CuckooIndex<>::getHash(hashs[2], 0, mNumBins);
                auto bIdx30 = CuckooIndex<>::getHash(hashs[3], 0, mNumBins);
                auto bIdx40 = CuckooIndex<>::getHash(hashs[4], 0, mNumBins);
                auto bIdx50 = CuckooIndex<>::getHash(hashs[5], 0, mNumBins);
                auto bIdx60 = CuckooIndex<>::getHash(hashs[6], 0, mNumBins);
                auto bIdx70 = CuckooIndex<>::getHash(hashs[7], 0, mNumBins);

                mBins(bIdx00, mBinSizes[bIdx00]++).set(itemIdx0, 0, false);
                mBins(bIdx10, mBinSizes[bIdx10]++).set(itemIdx1, 0, false);
                mBins(bIdx20, mBinSizes[bIdx20]++).set(itemIdx2, 0, false);
                mBins(bIdx30, mBinSizes[bIdx30]++).set(itemIdx3, 0, false);
                mBins(bIdx40, mBinSizes[bIdx40]++).set(itemIdx4, 0, false);
                mBins(bIdx50, mBinSizes[bIdx50]++).set(itemIdx5, 0, false);
                mBins(bIdx60, mBinSizes[bIdx60]++).set(itemIdx6, 0, false);
                mBins(bIdx70, mBinSizes[bIdx70]++).set(itemIdx7, 0, false);

                mItemToBinMap(itemIdx0, 0) = bIdx00;
                mItemToBinMap(itemIdx1, 0) = bIdx10;
                mItemToBinMap(itemIdx2, 0) = bIdx20;
                mItemToBinMap(itemIdx3, 0) = bIdx30;
                mItemToBinMap(itemIdx4, 0) = bIdx40;
                mItemToBinMap(itemIdx5, 0) = bIdx50;
                mItemToBinMap(itemIdx6, 0) = bIdx60;
                mItemToBinMap(itemIdx7, 0) = bIdx70;

                auto bIdx01 = CuckooIndex<>::getHash(hashs[0], 1, mNumBins);
                auto bIdx11 = CuckooIndex<>::getHash(hashs[1], 1, mNumBins);
                auto bIdx21 = CuckooIndex<>::getHash(hashs[2], 1, mNumBins);
                auto bIdx31 = CuckooIndex<>::getHash(hashs[3], 1, mNumBins);
                auto bIdx41 = CuckooIndex<>::getHash(hashs[4], 1, mNumBins);
                auto bIdx51 = CuckooIndex<>::getHash(hashs[5], 1, mNumBins);
                auto bIdx61 = CuckooIndex<>::getHash(hashs[6], 1, mNumBins);
                auto bIdx71 = CuckooIndex<>::getHash(hashs[7], 1, mNumBins);

                bool c01 = bIdx00 == bIdx01;
                bool c11 = bIdx10 == bIdx11;
                bool c21 = bIdx20 == bIdx21;
                bool c31 = bIdx30 == bIdx31;
                bool c41 = bIdx40 == bIdx41;
                bool c51 = bIdx50 == bIdx51;
                bool c61 = bIdx60 == bIdx61;
                bool c71 = bIdx70 == bIdx71;

                mBins(bIdx01, mBinSizes[bIdx01]++).set(itemIdx0, 1, c01);
                mBins(bIdx11, mBinSizes[bIdx11]++).set(itemIdx1, 1, c11);
                mBins(bIdx21, mBinSizes[bIdx21]++).set(itemIdx2, 1, c21);
                mBins(bIdx31, mBinSizes[bIdx31]++).set(itemIdx3, 1, c31);
                mBins(bIdx41, mBinSizes[bIdx41]++).set(itemIdx4, 1, c41);
                mBins(bIdx51, mBinSizes[bIdx51]++).set(itemIdx5, 1, c51);
                mBins(bIdx61, mBinSizes[bIdx61]++).set(itemIdx6, 1, c61);
                mBins(bIdx71, mBinSizes[bIdx71]++).set(itemIdx7, 1, c71);

                mItemToBinMap(itemIdx0, 1) = bIdx01 | ((u8)c01 & 1) * u64(-1);
                mItemToBinMap(itemIdx1, 1) = bIdx11 | ((u8)c11 & 1) * u64(-1);
                mItemToBinMap(itemIdx2, 1) = bIdx21 | ((u8)c21 & 1) * u64(-1);
                mItemToBinMap(itemIdx3, 1) = bIdx31 | ((u8)c31 & 1) * u64(-1);
                mItemToBinMap(itemIdx4, 1) = bIdx41 | ((u8)c41 & 1) * u64(-1);
                mItemToBinMap(itemIdx5, 1) = bIdx51 | ((u8)c51 & 1) * u64(-1);
                mItemToBinMap(itemIdx6, 1) = bIdx61 | ((u8)c61 & 1) * u64(-1);
                mItemToBinMap(itemIdx7, 1) = bIdx71 | ((u8)c71 & 1) * u64(-1);

                auto bIdx02 = CuckooIndex<>::getHash(hashs[0], 2, mNumBins);
                auto bIdx12 = CuckooIndex<>::getHash(hashs[1], 2, mNumBins);
                auto bIdx22 = CuckooIndex<>::getHash(hashs[2], 2, mNumBins);
                auto bIdx32 = CuckooIndex<>::getHash(hashs[3], 2, mNumBins);
                auto bIdx42 = CuckooIndex<>::getHash(hashs[4], 2, mNumBins);
                auto bIdx52 = CuckooIndex<>::getHash(hashs[5], 2, mNumBins);
                auto bIdx62 = CuckooIndex<>::getHash(hashs[6], 2, mNumBins);
                auto bIdx72 = CuckooIndex<>::getHash(hashs[7], 2, mNumBins);

                bool c02 = bIdx00 == bIdx02 || bIdx01 == bIdx02;
                bool c12 = bIdx10 == bIdx12 || bIdx11 == bIdx12;
                bool c22 = bIdx20 == bIdx22 || bIdx21 == bIdx22;
                bool c32 = bIdx30 == bIdx32 || bIdx31 == bIdx32;
                bool c42 = bIdx40 == bIdx42 || bIdx41 == bIdx42;
                bool c52 = bIdx50 == bIdx52 || bIdx51 == bIdx52;
                bool c62 = bIdx60 == bIdx62 || bIdx61 == bIdx62;
                bool c72 = bIdx70 == bIdx72 || bIdx71 == bIdx72;

                mBins(bIdx02, mBinSizes[bIdx02]++).set(itemIdx0, 2, c02);
                mBins(bIdx12, mBinSizes[bIdx12]++).set(itemIdx1, 2, c12);
                mBins(bIdx22, mBinSizes[bIdx22]++).set(itemIdx2, 2, c22);
                mBins(bIdx32, mBinSizes[bIdx32]++).set(itemIdx3, 2, c32);
                mBins(bIdx42, mBinSizes[bIdx42]++).set(itemIdx4, 2, c42);
                mBins(bIdx52, mBinSizes[bIdx52]++).set(itemIdx5, 2, c52);
                mBins(bIdx62, mBinSizes[bIdx62]++).set(itemIdx6, 2, c62);
                mBins(bIdx72, mBinSizes[bIdx72]++).set(itemIdx7, 2, c72);

                mItemToBinMap(itemIdx0, 2) = bIdx02 | ((u8)c02 & 1) * u64(-1);
                mItemToBinMap(itemIdx1, 2) = bIdx12 | ((u8)c12 & 1) * u64(-1);
                mItemToBinMap(itemIdx2, 2) = bIdx22 | ((u8)c22 & 1) * u64(-1);
                mItemToBinMap(itemIdx3, 2) = bIdx32 | ((u8)c32 & 1) * u64(-1);
                mItemToBinMap(itemIdx4, 2) = bIdx42 | ((u8)c42 & 1) * u64(-1);
                mItemToBinMap(itemIdx5, 2) = bIdx52 | ((u8)c52 & 1) * u64(-1);
                mItemToBinMap(itemIdx6, 2) = bIdx62 | ((u8)c62 & 1) * u64(-1);
                mItemToBinMap(itemIdx7, 2) = bIdx72 | ((u8)c72 & 1) * u64(-1);
            }

            hasher.ecbEncBlocks(items.data() + itemIdx, remSteps, hashs.data());
            for (u64 i = 0; i < remSteps; i += hashs.size())
            {
                hashs[i] = hashs[i] ^ items[itemIdx + i];

                std::vector<u64> bIdxs(mNumHashFunctions);
                for (u64 h = 0; h < mNumHashFunctions; ++h)
                {
                    auto bIdx = CuckooIndex<>::getHash(hashs[i], (u8)h, mNumBins);
                    bool collision = false;

                    bIdxs[h] = bIdx;
                    for (u64 hh = 0; hh < h; ++hh)
                        collision |= (bIdxs[hh] == bIdx);

                    mBins(bIdx, mBinSizes[bIdx]++).set(itemIdx, u8(h), collision);
                    mItemToBinMap(itemIdx + i, h) = bIdx | ((u8)collision & 1) * u64(-1);
                }
            }
        }
        else
        {
            std::vector<u64> bIdxs(mNumHashFunctions);
            for (u64 i = 0; i < u64(items.size()); i += u64(hashs.size()))
            {
                auto min = std::min<u64>(items.size() - i, hashs.size());

                hasher.ecbEncBlocks(items.data() + i, min, hashs.data());

                for (u64 j = 0, itemIdx = i; j < min; ++j, ++itemIdx)
                {
                    hashs[j] = hashs[j] ^ items[itemIdx];

                    for (u64 h = 0; h < mNumHashFunctions; ++h)
                    {
                        auto bIdx = CuckooIndex<>::getHash(hashs[j], (u8)h, mNumBins);
                        bool collision = false;

                        bIdxs[h] = bIdx;
                        for (u64 hh = 0; hh < h; ++hh)
                            collision |= (bIdxs[hh] == bIdx);

                        mBins(bIdx, mBinSizes[bIdx]++).set(itemIdx, u8(h), collision);
                        mItemToBinMap(itemIdx + i, h) = bIdx | ((u8)collision & 1) * u64(-1);

                    }
                }
            }
        }
    }

}


#ifndef POINT_TRIANGLE_HPP
#define POINT_TRIANGLE_HPP

#include <Eigen/Core>

template<class U>
class projector {
public:
  using real = U;
  using vec3 = Eigen::Matrix<real, 3, 1>;
private:
  vec3 p1, p2, p3, p1p2, p1p3;
  real distp1p2;
  real fa, fb, fc;
  real fdet;

  // singular case
  bool singular;
  real e0, e1, sign, ff;

  static constexpr real epsilon = 1e-10;
public:
  projector(vec3 p1, vec3 p2, vec3 p3):
      p1(p1),
      p2(p2),
      p3(p3),
      p1p2(p2 - p1),
      p1p3(p3 - p1),
      fa(p1p2.dot(p1p2)),
      fb(p1p2.dot(p1p3)),
      fc(p1p3.dot(p1p3)),
      fdet(fa * fc - fb * fb),
      singular(fdet < epsilon && fdet > -epsilon) {

  }

  static vec3 project_edge(vec3 origin, vec3 dir, vec3 p) {
    return origin + dir * dir.dot(p - origin) / dir.dot(dir);
  }

  vec3 operator()(vec3 p) const {
    if(singular) {
      if(fa + fc < epsilon) {
        // single point: easy peasy
        return p1;
      }

      if(fb > 0) {
        // both edges point in the same direction: project on the longest
        if(fa > fc) {
          return project_edge(p1, p1p2, p);
        } else {
          return project_edge(p1, p1p3, p);
        }
      } else {
        // edges pointing in opposite directions
        return project_edge(p2, p3 - p2, p);
      }
    }

    // non-singular case
    const vec3 pp1 = p1 - p;
    const real fd = p1p2.dot(pp1), fe = p1p3.dot(pp1);

    // minimize squared distance to source point
    real fs = fb * fe - fc * fd, ft = fb * fd - fa * fe;

    if(fs + ft <= fdet) {
      if(fs < 0) {
        if(ft < 0) {
          // region 4
          if(fd < 0) {
            ft = 0;
            if(-fd >= fa) {
              fs = 1;
            } else {
              fs = -fd / fa;
            }
          } else {
            fs = 0;
            if(fe >= 0) {
              ft = 0;
            } else if(-fe >= fc) {
              ft = 1;
            } else {
              ft = -fe / fc;
            }
          }
        } else {
          // region 3
          fs = 0;
          if(fe >= 0) {
            ft = 0;
          } else if(-fe >= fc) {
            ft = 1;
          } else {
            ft = -fe / fc;
          }
        }
      } else if(ft < 0) {
        // region 5
        ft = 0;
        if(fd >= 0) {
          fs = 0;
        } else if(-fd >= fa) {
          fs = 1;
        } else {
          fs = -fd / fa;
        }

      } else {
        // region 0
        // minimum at interior point
        fs /= fdet;
        ft /= fdet;
      }
    } else {
      real ftmp0, ftmp1, fNumer, fDenom;

      if(fs < 0) {
        // region 2
        ftmp0 = fb + fd;
        ftmp1 = fc + fe;
        if(ftmp1 > ftmp0) {
          fNumer = ftmp1 - ftmp0;
          fDenom = fa - 2 * fb + fc;
          if(fNumer >= fDenom) {
            fs = 1;
            ft = 0;
          } else {
            fs = fNumer / fDenom;
            ft = 1 - fs;
          }
        } else {
          fs = 0;
          if(ftmp1 <= 0) {
            ft = 1;
          } else if(fe >= 0) {
            ft = 0;
          } else {
            ft = -fe / fc;
          }
        }
      } else if(ft < 0) {
        // region 6
        ftmp0 = fb + fe;
        ftmp1 = fa + fd;
        if(ftmp1 > ftmp0) {
          fNumer = ftmp1 - ftmp0;
          fDenom = fa - 2 * fb + fc;
          if(fNumer >= fDenom) {
            ft = 1;
            fs = 0;
          } else {
            ft = fNumer / fDenom;
            fs = 1 - ft;
          }
        } else {
          ft = 0;
          if(ftmp1 <= 0) {
            fs = 1;
          } else if(fd >= 0) {
            fs = 0;
          } else {
            fs = -fd / fa;
          }
        }
      } else {
        // region 1
        fNumer = fc + fe - fb - fd;
        if(fNumer <= 0) {
          fs = 0;
          ft = 1;
        } else {
          fDenom = fa - 2 * fb + fc;
          if(fNumer >= fDenom) {
            fs = 1;
            ft = 0;
          } else {
            fs = fNumer / fDenom;
            ft = 1 - fs;
          }
        }
      }
    }

    return (1 - fs - ft) * p1 + fs * p2 + ft * p3;
  }
};

#endif


#include <dlib/optimization.h>
#include <fbxpstate.h>

namespace BezierFitter {
    using namespace dlib;

    // dlib reference fo the solver:
    // http://dlib.net/least_squares_ex.cpp.html

    typedef matrix< double, 3, 1 >                 BezierFitterInput;  // double t, P0, P3;
    typedef matrix< double, 2, 1 >                 BezierFitterParams; // double P1, P2;
    typedef std::pair< BezierFitterInput, double > BezierFitterSample; // double t, P0, P3 + Bt;

    inline double Squared( double v ) {
        return v * v;
    }

    inline double Cubed( double v ) {
        return v * v * v;
    }

    inline double BezierFitterModel( const BezierFitterInput input, const BezierFitterParams params ) {
        const double t  = input( 0 );
        const double P0 = input( 1 );
        const double P1 = params( 0 );
        const double P2 = params( 1 );
        const double P3 = input( 2 );
        return Cubed( 1.0 - t ) * P0 + 3 * Squared( 1.0 - t ) * t * P1 + 3 * ( 1.0 - t ) * Squared( t ) * P2 + Cubed( t ) * P3;
    }

    inline double BezierFitterResidual( const BezierFitterSample input, const BezierFitterParams params ) {
        const double Bt = input.second;
        return BezierFitterModel( input.first, params ) - Bt;
    }

    inline BezierFitterParams BezierFitterResidualDerivative( const BezierFitterSample input,
                                                              const BezierFitterParams params ) {
        const double       t = input.first( 0 );
        BezierFitterParams derivative;
        derivative( 0 ) = 3 * Squared( 1.0 - t ) * t;
        derivative( 1 ) = 3 * ( 1.0 - t ) * Squared( t );
        return derivative;
    }

    static BezierFitterParams SolveBezier( std::vector< BezierFitterSample > samples ) {
        BezierFitterParams bezierSolverParams;
        bezierSolverParams = 0;

        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm( objective_delta_stop_strategy( 1e-7 ),
                                BezierFitterResidual,
                                BezierFitterResidualDerivative,
                                samples,
                                bezierSolverParams );

        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        // solve_least_squares_lm( objective_delta_stop_strategy( 1e-7 ).be_verbose( ),
        //                         BezierFitterResidual,
        //                         derivative( BezierFitterResidual ),
        //                         samples,
        //                         x );

        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        // solve_least_squares( objective_delta_stop_strategy( 1e-7 ).be_verbose( ),
        // solve_least_squares( objective_delta_stop_strategy( 1e-7 ),
        //                      BezierFitterResidual,
        //                      BezierFitterResidualDerivative,
        //                      samples,
        //                      bezierSolverParams );

        return bezierSolverParams;
    }

    static void ExtractSamples( FbxAnimCurve*                      pAnimCurve,
                                const int                          /*startIndex*/,
                                const double                       P0X,
                                const double                       P0Y,
                                const double                       P3X,
                                const double                       P3Y,
                                std::vector< BezierFitterSample >& samples ) {

        // TODO: Scan only the relevan region, break the loop when the end time is reached.
        for ( int i = 0; i < pAnimCurve->KeyGetCount(); ++i ) {
            const double time = pAnimCurve->KeyGetTime( i ).GetSecondDouble( );

            if ( time >= P0X && time <= P3X ) {
                const double t  = ( time - P0X ) / ( P3X - P0X );
                const double Bt = pAnimCurve->KeyGetValue( i );

                BezierFitterSample sample;
                sample.first( 0 ) = t;
                sample.first( 1 ) = P0Y;
                sample.first( 2 ) = P3Y;
                sample.second     = Bt;

                samples.push_back( sample );
            }
        }
    }

} // namespace BezierFitter

bool BezierFitterFitSamples( FbxAnimCurve* pAnimCurve,
                             const int     startIndex,
                             const double  BezP0X,
                             const double  BezP0Y,
                             const double  BezP3X,
                             const double  BezP3Y,
                             double&       BezP1Y,
                             double&       BezP2Y ) {
    std::vector< BezierFitter::BezierFitterSample > samples;
    BezierFitter::ExtractSamples( pAnimCurve, startIndex, BezP0X, BezP0Y, BezP3X, BezP3Y, samples );

    auto & s = apemode::State::Get( );
    if ( !samples.empty( ) ) {
        auto params = BezierFitter::SolveBezier( std::move( samples ) );
        BezP1Y = params( 0 );
        BezP2Y = params( 1 );
        s.console->debug( "Samples taken: {}", samples.size( ) );
        s.console->debug( "Solved Bezier: {} {}", BezP1Y, BezP2Y );
        return true;
    }

    s.console->error("Failed to find the samples for fitting the Bezier control points.");
    return false;
}

void BezierFitterFitSamples( FbxAnimCurve* pAnimCurve, int keyIndex, double& OutFittedBezier1, double& OutFittedBezier2 ) {
    assert( pAnimCurve && ( keyIndex < ( pAnimCurve->KeyGetCount( ) - 1 ) ) );
    auto& s = apemode::State::Get( );

    const FbxString copiedCurveName = pAnimCurve->GetNameOnly() + " [FbxPipeline-Copy]";
    if ( FbxAnimCurve* pCopiedAnimCurve = FbxAnimCurve::Create( s.manager, copiedCurveName.Buffer() ) ) {
        pCopiedAnimCurve->CopyFrom( *pAnimCurve );

        auto resampleStartTime = pAnimCurve->KeyGet( keyIndex ).GetTime( );
        auto resampleStopTime  = pAnimCurve->KeyGet( keyIndex + 1 ).GetTime( );

        FbxTime resamplePeriodTime;
        resamplePeriodTime.SetMilliSeconds( ( FbxLongLong )( 1000.0f / 180.0f ) );

        FbxAnimCurveFilterResample animCurveFilterResample;
        animCurveFilterResample.SetPeriodTime( resamplePeriodTime );
        animCurveFilterResample.SetStartTime( resampleStartTime );
        animCurveFilterResample.SetStopTime( resampleStopTime );
        animCurveFilterResample.Apply( *pCopiedAnimCurve );

        BezierFitterFitSamples( pCopiedAnimCurve,
                                keyIndex,
                                pAnimCurve->KeyGet( keyIndex ).GetTime( ).GetSecondDouble( ),
                                pAnimCurve->KeyGet( keyIndex ).GetValue( ),
                                pAnimCurve->KeyGet( keyIndex + 1 ).GetTime( ).GetSecondDouble( ),
                                pAnimCurve->KeyGet( keyIndex + 1 ).GetValue( ),
                                OutFittedBezier1,
                                OutFittedBezier2 );

        pCopiedAnimCurve->Destroy( );
    }
}


#include "LineSegment.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/integer/common_factor.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2021
{
namespace Day05
{

LineSegment::LineSegment(Vector2D start, Vector2D end)
    : m_start{std::move(start)}
    , m_end{std::move(end)}
{

}

bool LineSegment::isAxisParallel() const
{
    return m_start.x() == m_end.x() || m_start.y() == m_end.y();
}

std::vector<Vector2D> LineSegment::getCoveredPoints() const
{
    const Vector2D differenceVector = m_end - m_start;
    const Vector2D differenceVectorAbs = differenceVector.cwiseAbs();

    const int numStepsFromStartToEnd = boost::integer::gcd(differenceVectorAbs[0], differenceVectorAbs[1]);

    const Vector2D stepVector = differenceVector / numStepsFromStartToEnd;

    std::vector<Vector2D> coveredPoints;
    for (Vector2D innerPointVector = m_start; ; innerPointVector += stepVector)
    {
        coveredPoints.push_back(innerPointVector);

        if (innerPointVector == m_end)
        {
            break;
        }
    }

    return coveredPoints;
}

}
}
}


// big_posit.cpp: Functionality tests for big posits
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#include "common.hpp"

#include <boost/multiprecision/cpp_bin_float.hpp>
// enable posit arithmetic exceptions
#define POSIT_THROW_ARITHMETIC_EXCEPTION 1
// to capture all the possible bits, use
#define POSIT_ROUNDING_ERROR_FREE_IO_FORMAT 1
#include <posit>
#include "../test_helpers.hpp"
#include "../posit_test_helpers.hpp"

/*
experiments with big posits
*/

// Sample the conversion space around 1 when number of fraction bits
// of a big posit is bigger than the number of bits in the input representation
template<typename Ty>
void Sample(Ty value) {
	using namespace std;
	using namespace sw::unum;

	cout << typeid(value).name() << endl;
	cout << posit<54, 3>(value) << " " << double(posit<54, 3>(value)) << endl;
	cout << posit<56, 3>(value) << " " << double(posit<56, 3>(value)) << endl;
	cout << posit<58, 3>(value) << " " << double(posit<58, 3>(value)) << endl;
	cout << posit<60, 3>(value) << " " << double(posit<60, 3>(value)) << endl;
	cout << posit<62, 3>(value) << " " << double(posit<62, 3>(value)) << endl;
	cout << posit<64, 3>(value) << " " << double(posit<64, 3>(value)) << endl;
	cout << posit<66, 3>(value) << " " << double(posit<66, 3>(value)) << endl;
	cout << posit<67, 3>(value) << " " << double(posit<67, 3>(value)) << endl;
	cout << posit<68, 3>(value) << " " << double(posit<68, 3>(value)) << endl;
	cout << posit<69, 3>(value) << " " << double(posit<69, 3>(value)) << endl;
	cout << posit<70, 3>(value) << " " << double(posit<70, 3>(value)) << endl;
	cout << posit<71, 3>(value) << " " << double(posit<71, 3>(value)) << endl;
	cout << posit<72, 3>(value) << " " << double(posit<72, 3>(value)) << endl;
	cout << posit<80, 3>(value) << " " << double(posit<80, 3>(value)) << endl;
}

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::unum;

	const size_t RND_TEST_CASES = 1000;

	const size_t nbits = 128;
	const size_t es = 4;

	int nrOfFailedTestCases = 0;
	bool bReportIndividualTestCases = false;
	std::string tag = " big posit conversion experiments";

	posit<80, 3> p;
	p = 1ull;
	p = 2ull;

	Sample(1);
	Sample(2);
	Sample(1ull);
	Sample(2ull);
	Sample(1.0f);

	return (nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS);
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_SUCCESS; //as we manually throwing the not supported yet it should not fall through the cracks     EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::runtime_error& err) {
	std::cerr << "Uncaught runtime exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[code_clustering_coefficient
#include <iostream>
#include <iomanip>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// The Actor type stores the name of each vertex in the graph.
struct Actor
{
    string name;
};

// Declare the graph type and its vertex and edge types.
typedef undirected_graph< Actor > Graph;
typedef graph_traits< Graph >::vertex_descriptor Vertex;
typedef graph_traits< Graph >::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map< Graph, string Actor::* >::type NameMap;

// The clustering property, container, and map define the containment
// and abstract accessor for the clustering coefficients of vertices.
typedef exterior_vertex_property< Graph, float > ClusteringProperty;
typedef ClusteringProperty::container_type ClusteringContainer;
typedef ClusteringProperty::map_type ClusteringMap;

int main(int argc, char* argv[])
{
    // Create the graph and a name map that provides access to
    // then actor names.
    Graph g;
    NameMap nm(get(&Actor::name, g));

    // Read the graph from standard input.
    read_graph(g, nm, cin);

    // Compute the clustering coefficients of each vertex in the graph
    // and the mean clustering coefficient which is returned from the
    // computation.
    ClusteringContainer coefs(num_vertices(g));
    ClusteringMap cm(coefs, g);
    float cc = all_clustering_coefficients(g, cm);

    // Print the clustering coefficient of each vertex.
    graph_traits< Graph >::vertex_iterator i, end;
    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        cout << setw(12) << setiosflags(ios::left) << g[*i].name << get(cm, *i)
             << endl;
    }
    cout << "mean clustering coefficient: " << cc << endl;

    return 0;
}
//]


#ifndef STAN_MATH_PRIM_FUN_FFT_HPP
#define STAN_MATH_PRIM_FUN_FFT_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <unsupported/Eigen/FFT>
#include <Eigen/Dense>
#include <complex>
#include <type_traits>
#include <vector>

namespace stan {
namespace math {

/**
 * Return the discrete Fourier transform of the specified complex
 * vector.
 *
 * Given an input complex vector `x[0:N-1]` of size `N`, the discrete
 * Fourier transform computes entries of the resulting complex
 * vector `y[0:N-1]` by
 *
 * ```
 * y[n] = SUM_{i < N} x[i] * exp(-n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 *
 * @tparam V type of complex vector argument
 * @param[in] x vector to transform
 * @return discrete Fourier transform of `x`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr>
inline Eigen::Matrix<scalar_type_t<V>, -1, 1> fft(const V& x) {
  // copy because fft() requires Eigen::Matrix type
  Eigen::Matrix<scalar_type_t<V>, -1, 1> xv = x;
  if (xv.size() <= 1)
    return xv;
  Eigen::FFT<base_type_t<V>> fft;
  return fft.fwd(xv);
}

/**
 * Return the inverse discrete Fourier transform of the specified
 * complex vector.
 *
 * Given an input complex vector `y[0:N-1]` of size `N`, the inverse
 * discrete Fourier transform computes entries of the resulting
 * complex vector `x[0:N-1]` by
 *
 * ```
 * x[n] = SUM_{i < N} y[i] * exp(n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 * The only difference between the discrete DFT and its inverse is
 * the sign of the exponent.
 *
 * @tparam V type of complex vector argument
 * @param[in] y vector to inverse transform
 * @return inverse discrete Fourier transform of `y`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr>
inline Eigen::Matrix<scalar_type_t<V>, -1, 1> inv_fft(const V& y) {
  // copy because fft() requires Eigen::Matrix type
  Eigen::Matrix<scalar_type_t<V>, -1, 1> yv = y;
  if (y.size() <= 1)
    return yv;
  Eigen::FFT<base_type_t<V>> fft;
  return fft.inv(yv);
}

/**
 * Return the two-dimensional discrete Fourier transform of the
 * specified complex matrix.  The 2D discrete Fourier transform first
 * runs the discrete Fourier transform on the each row, then on each
 * column of the result.
 *
 * @tparam M type of complex matrix argument
 * @param[in] x matrix to transform
 * @return discrete 2D Fourier transform of `x`
 */
template <typename M, require_eigen_dense_dynamic_vt<is_complex, M>* = nullptr>
inline Eigen::Matrix<scalar_type_t<M>, -1, -1> fft2(const M& x) {
  Eigen::Matrix<scalar_type_t<M>, -1, -1> y(x.rows(), x.cols());
  for (int i = 0; i < y.rows(); ++i)
    y.row(i) = fft(x.row(i));
  for (int j = 0; j < y.cols(); ++j)
    y.col(j) = fft(y.col(j));
  return y;
}

/**
 * Return the two-dimensional inverse discrete Fourier transform of
 * the specified complex matrix.  The 2D inverse discrete Fourier
 * transform first runs the 1D inverse Fourier transform on the
 * columns, and then on the resulting rows.  The composition of the
 * FFT and inverse FFT (or vice-versa) is the identity.
 *
 * @tparam M type of complex matrix argument
 * @param[in] y matrix to inverse trnasform
 * @return inverse discrete 2D Fourier transform of `y`
 */
template <typename M, require_eigen_dense_dynamic_vt<is_complex, M>* = nullptr>
inline Eigen::Matrix<scalar_type_t<M>, -1, -1> inv_fft2(const M& y) {
  Eigen::Matrix<scalar_type_t<M>, -1, -1> x(y.rows(), y.cols());
  for (int j = 0; j < x.cols(); ++j)
    x.col(j) = inv_fft(y.col(j));
  for (int i = 0; i < x.rows(); ++i)
    x.row(i) = inv_fft(x.row(i));
  return x;
}

}  // namespace math
}  // namespace stan

#endif


#include <igl/opengl/gl.h>
#include <igl/arap.h>
#include <igl/biharmonic_coordinates.h>
#include <igl/cat.h>
#include <igl/cotmatrix.h>
#include <igl/massmatrix.h>
#include <igl/matrix_to_list.h>
#include <igl/parula.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/readDMAT.h>
#include <igl/readMESH.h>
#include <igl/remove_unreferenced.h>
#include <igl/slice.h>
#include <igl/writeDMAT.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <iostream>
#include <queue>

#include "tutorial_shared_path.h"

struct Mesh
{
  Eigen::MatrixXd V,U;
  Eigen::MatrixXi T,F;
} low,high,scene;

Eigen::MatrixXd W;
igl::ARAPData arap_data;

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  if(!readMESH(TUTORIAL_SHARED_PATH "/octopus-low.mesh",low.V,low.T,low.F))
  {
    cout<<"failed to load mesh"<<endl;
  }
  if(!readMESH(TUTORIAL_SHARED_PATH "/octopus-high.mesh",high.V,high.T,high.F))
  {
    cout<<"failed to load mesh"<<endl;
  }

  // Precomputation
  {
    Eigen::VectorXi b;
    {
      Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(high.V.rows(),0,high.V.rows()-1);
      Eigen::VectorXd sqrD;
      Eigen::MatrixXd _2;
      cout<<"Finding closest points..."<<endl;
      igl::point_mesh_squared_distance(low.V,high.V,J,sqrD,b,_2);
      assert(sqrD.minCoeff() < 1e-7 && "low.V should exist in high.V");
    }
    // force perfect positioning, rather have popping in low-res than high-res.
    // The correct/elaborate thing to do is express original low.V in terms of
    // linear interpolation (or extrapolation) via elements in (high.V,high.F)
    igl::slice(high.V,b,1,low.V);
    // list of points --> list of singleton lists
    std::vector<std::vector<int> > S;
    igl::matrix_to_list(b,S);
    cout<<"Computing weights for "<<b.size()<<
      " handles at "<<high.V.rows()<<" vertices..."<<endl;
    // Technically k should equal 3 for smooth interpolation in 3d, but 2 is
    // faster and looks OK
    const int k = 2;
    igl::biharmonic_coordinates(high.V,high.T,S,k,W);
    cout<<"Reindexing..."<<endl;
    // Throw away interior tet-vertices, keep weights and indices of boundary
    VectorXi I,J;
    igl::remove_unreferenced(high.V.rows(),high.F,I,J);
    for_each(high.F.data(),high.F.data()+high.F.size(),[&I](int & a){a=I(a);});
    for_each(b.data(),b.data()+b.size(),[&I](int & a){a=I(a);});
    igl::slice(MatrixXd(high.V),J,1,high.V);
    igl::slice(MatrixXd(W),J,1,W);
  }

  // Resize low res (high res will also be resized by affine precision of W)
  low.V.rowwise() -= low.V.colwise().mean();
  low.V /= (low.V.maxCoeff()-low.V.minCoeff());
  low.V.rowwise() += RowVector3d(0,1,0);
  low.U = low.V;
  high.U = high.V;

  arap_data.with_dynamics = true;
  arap_data.max_iter = 10;
  arap_data.energy = ARAP_ENERGY_TYPE_DEFAULT;
  arap_data.h = 0.01;
  arap_data.ym = 0.001;
  if(!arap_precomputation(low.V,low.T,3,VectorXi(),arap_data))
  {
    cerr<<"arap_precomputation failed."<<endl;
    return EXIT_FAILURE;
  }
  // Constant gravitational force
  Eigen::SparseMatrix<double> M;
  igl::massmatrix(low.V,low.T,igl::MASSMATRIX_TYPE_DEFAULT,M);
  const size_t n = low.V.rows();
  arap_data.f_ext =  M * RowVector3d(0,-9.8,0).replicate(n,1);
  // Random initial velocities to wiggle things
  arap_data.vel = MatrixXd::Random(n,3);

  igl::opengl::glfw::Viewer viewer;
  // Create one huge mesh containing both meshes
  igl::cat(1,low.U,high.U,scene.U);
  igl::cat(1,low.F,MatrixXi(high.F.array()+low.V.rows()),scene.F);
  // Color each mesh
  viewer.data().set_mesh(scene.U,scene.F);
  MatrixXd C(scene.F.rows(),3);
  C<<
    RowVector3d(0.8,0.5,0.2).replicate(low.F.rows(),1),
    RowVector3d(0.3,0.4,1.0).replicate(high.F.rows(),1);
  viewer.data().set_colors(C);

  viewer.callback_key_pressed =
    [&](igl::opengl::glfw::Viewer & viewer,unsigned int key,int mods)->bool
  {
    switch(key)
    {
      default:
        return false;
      case ' ':
        viewer.core.is_animating = !viewer.core.is_animating;
        return true;
      case 'r':
        low.U = low.V;
        return true;
    }
  };
  viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer & viewer)->bool
  {
    glEnable(GL_CULL_FACE);
    if(viewer.core.is_animating)
    {
      arap_solve(MatrixXd(0,3),arap_data,low.U);
      for(int v = 0;v<low.U.rows();v++)
      {
        // collide with y=0 plane
        const int y = 1;
        if(low.U(v,y) < 0)
        {
          low.U(v,y) = -low.U(v,y);
          // ~ coefficient of restitution
          const double cr = 1.1;
          arap_data.vel(v,y) = - arap_data.vel(v,y) / cr;
        }
      }

      scene.U.block(0,0,low.U.rows(),low.U.cols()) = low.U;
      high.U = W * (low.U.rowwise() + RowVector3d(1,0,0));
      scene.U.block(low.U.rows(),0,high.U.rows(),high.U.cols()) = high.U;

      viewer.data().set_vertices(scene.U);
      viewer.data().compute_normals();
    }
    return false;
  };
  viewer.data().show_lines = false;
  viewer.core.is_animating = true;
  viewer.core.animation_max_fps = 30.;
  viewer.data().set_face_based(true);
  cout<<R"(
[space] to toggle animation
'r'     to reset positions
      )";
  viewer.core.rotation_type =
    igl::opengl::ViewerCore::ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP;
  viewer.launch();
}


//
// Copyright (c) 2020 INRIA
//

#include "pinocchio/math/multiprecision-mpfr.hpp"

#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <iostream>

#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/math/multiprecision.hpp"
#include "pinocchio/parsers/sample-models.hpp"

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_basic)
{
  using namespace boost::multiprecision;

  // Operations at fixed precision and full numeric_limits support:
  mpfr_float_100 b = 2;
  std::cout << std::numeric_limits<mpfr_float_100>::digits << std::endl;
  std::cout << std::numeric_limits<mpfr_float_100>::digits10 << std::endl;
  // We can use any C++ std lib function, lets print all the digits as well:
  std::cout << std::setprecision(
                   std::numeric_limits<mpfr_float_100>::max_digits10)
            << log(b)
            << std::endl;  // print log(2)
                           // We can also use any function from Boost.Math:
  std::cout << boost::math::tgamma(b) << std::endl;
  // These even work when the argument is an expression template:
  std::cout << boost::math::tgamma(b * b) << std::endl;
  // And since we have an extended exponent range we can generate some really
  // large numbers here (4.0238726007709377354370243e+2564):
  std::cout << boost::math::tgamma(mpfr_float_100(1000)) << std::endl;
}

BOOST_AUTO_TEST_CASE(test_sincos)
{
  using namespace boost::multiprecision;
  typedef mpfr_float_100 heap_float_100;
  typedef number<mpfr_float_backend<100, allocate_stack> > stack_float_100;
  {
    heap_float_100 x;
    heap_float_100 s;
    heap_float_100 c;
    x = 100;
    pinocchio::SINCOS(x, &s, &c);
    BOOST_CHECK(s == sin(x));
    BOOST_CHECK(c == cos(x));
  }
  {
    stack_float_100 x;
    stack_float_100 s;
    stack_float_100 c;
    x = 100;
    pinocchio::SINCOS(x, &s, &c);
    BOOST_CHECK(s == sin(x));
    BOOST_CHECK(c == cos(x));
  }
}

BOOST_AUTO_TEST_CASE(test_cast)
{
  typedef boost::multiprecision::mpfr_float_100 float_100;

  // Test Scalar cast
  double initial_value = boost::math::constants::pi<double>();
  float_100 value_100(initial_value);
  double value_cast = value_100.convert_to<double>();
  BOOST_CHECK(initial_value == value_cast);

  typedef Eigen::Matrix<float_100, Eigen::Dynamic, 1> VectorFloat100;
  static const Eigen::DenseIndex dim = 100;
  Eigen::VectorXd initial_vec = Eigen::VectorXd::Random(dim);
  VectorFloat100 vec_float_100 = initial_vec.cast<float_100>();
  Eigen::VectorXd vec = vec_float_100.cast<double>();

  BOOST_CHECK(vec == initial_vec);
}

#define BOOST_CHECK_IS_APPROX(double_field, multires_field, Scalar) \
  BOOST_CHECK(double_field.isApprox(multires_field.cast<Scalar>()))

BOOST_AUTO_TEST_CASE(test_mutliprecision)
{
  using namespace pinocchio;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);

  typedef boost::multiprecision::mpfr_float_100 float_100;
  typedef ModelTpl<float_100> ModelMulti;
  typedef DataTpl<float_100> DataMulti;

  ModelMulti model_multi = model.cast<float_100>();
  DataMulti data_multi(model_multi);

  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
  ModelMulti::TangentVectorType v_multi =
      ModelMulti::TangentVectorType::Random(model_multi.nv);
  ModelMulti::TangentVectorType a_multi =
      ModelMulti::TangentVectorType::Random(model_multi.nv);
  ModelMulti::TangentVectorType tau_multi =
      ModelMulti::TangentVectorType::Random(model_multi.nv);

  //  Model::ConfigVectorType q = randomConfiguration(model);
  //  Model::TangentVectorType v = Model::TangentVectorType::Random(model.nv);
  //  Model::TangentVectorType a = Model::TangentVectorType::Random(model.nv);
  //  Model::TangentVectorType tau = Model::TangentVectorType::Random(model.nv);

  Model::ConfigVectorType q = q_multi.cast<double>();
  Model::TangentVectorType v = v_multi.cast<double>();
  Model::TangentVectorType a = a_multi.cast<double>();
  Model::TangentVectorType tau = tau_multi.cast<double>();

  forwardKinematics(model_multi, data_multi, q_multi, v_multi, a_multi);
  forwardKinematics(model, data, q, v, a);

  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints;
       ++joint_id)
  {
    BOOST_CHECK_IS_APPROX(data.oMi[joint_id], data_multi.oMi[joint_id], double);
    BOOST_CHECK_IS_APPROX(data.v[joint_id], data_multi.v[joint_id], double);
    BOOST_CHECK_IS_APPROX(data.a[joint_id], data_multi.a[joint_id], double);
  }

  // Jacobians
  computeJointJacobians(model_multi, data_multi, q_multi);
  computeJointJacobians(model, data, q);

  BOOST_CHECK_IS_APPROX(data.J, data_multi.J, double);

  // Inverse Dynamics
  rnea(model_multi, data_multi, q_multi, v_multi, a_multi);
  rnea(model, data, q, v, a);

  BOOST_CHECK_IS_APPROX(data.tau, data_multi.tau, double);

  // Forward Dynamics
  aba(model_multi, data_multi, q_multi, v_multi, tau_multi);
  aba(model, data, q, v, tau);

  BOOST_CHECK_IS_APPROX(data.ddq, data_multi.ddq, double);

  // Mass matrix
  crba(model_multi, data_multi, q_multi);
  data_multi.M.triangularView<Eigen::StrictlyLower>() =
      data_multi.M.transpose().triangularView<Eigen::StrictlyLower>();

  crba(model, data, q);
  data.M.triangularView<Eigen::StrictlyLower>() =
      data.M.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK_IS_APPROX(data.M, data_multi.M, double);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef IEOMPP_CONSTANTS_HPP_
#define IEOMPP_CONSTANTS_HPP_

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

namespace ieompp
{
    template <typename Float>
    struct Pi {
        static const Float value;
    };

    template <typename Float>
    struct HalfPi {
        static const Float value;
    };

    template <typename Float>
    struct TwoPi {
        static const Float value;
    };

    template <typename Float>
    const Float Pi<Float>::value = boost::math::constants::pi<Float>();

    template <typename Float>
    const Float HalfPi<Float>::value = boost::math::constants::half_pi<Float>();

    template <typename Float>
    const Float TwoPi<Float>::value = boost::math::constants::two_pi<Float>();
} // namespace ieompp

#endif


//
// Compute jump ahead coefficients for Mersenne Twister RNG.
// Ken-Ichi Ishikawa [ishikawa[at]theo.phys.sci.hiroshima-u.ac.jp]
//
// See also:
//  H. Haramoto, M. Matsumoto, T. Nishimura, F. Panneton, and P. L'Ecuyer,
//   ``Efficient Jump Ahead for F_2-Linear Random Number Generators'',
//  GERAD Report G-2006-62. INFORMS Journal on Computing, 20, 3 (2008), 385-390.
//
// This routine uses;
//  Fast arithmetic in GF(2)[x], [http://wwwmaths.anu.edu.au/~brent/software.html]
//  NTL : A Library for doing Number Theory, [http://www.shoup.net/ntl/index.html]
//
//
// Copyright (c) 2010, Ken-Ichi Ishikawa [ishikawa[at]theo.phys.sci.hiroshima-u.ac.jp]
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer listed
//   in this license in the documentation and/or other materials
//   provided with the distribution.
//
// * Neither the name of the copyright holders nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//

#include <NTL/GF2X.h>
NTL_CLIENT

//#define _DEBUG_
#ifdef _DEBUG_
#define DEBUG_PRINTF(format, args...)  fprintf(stderr, format, ## args)
#else
#define DEBUG_PRINTF(format, args...)  ;
#endif

void print_hex(GF2X &f)
{
    int ww = 32;
    int nb = NumBits(f);
    int nn = (int)ceil((double)nb/(double)ww);
    unsigned int pp[nn];
    for(int i = 0; i < nn ; i++) pp[i] = 0;
    for(int i = 0; i < nb ; i++) {
      int iw = i / ww;
      int ib = i % ww;
      if (1 == coeff(f,i)) pp[iw] += (1 << ib);
    }
    if (0 != pp[nn-1]) printf("%X",pp[nn-1]);
    for(int i = nn-2; i > -1 ; i--) {
      printf("%8.8X",pp[i]);
    }
    printf("\n");
}

extern "C"
void get_coeff (const int& nn,     // MT param n
                const int& mm,     //          m
                const int& rr,     //          r
                const int& ww,     //          w => MT period_exp = n*w - r
                const int& avec,   //        aaa => companion matirx component vector a
                const int& nj,     // jump ahead step exponent   : id*(2^nj)
                const int& id,     // jump ahead step id=[0,...] : id*(2^nj)
                unsigned int *pp,  // jump ahead polynomial coefficients, pp[nn]
                int& np)           // jump ahead polynomial order(bit size)
{
  if (id <= 0) return;
  int period_exp = nn*ww-rr;
  int ns = (int)ceil(log((double)period_exp)/log(2.0));

  //
  // Compute MT characteristic polynomial f(x)
  // [see,
  //  M. Matsumoto and T. Nishimura,
  //  "Mersenne Twister: A 623-dimensionally equidistributed uniform
  //                                   pseudorandom number generator",
  //  ACM Trans. on Modeling and Computer Simulation Vol. 8, No. 1,
  //  January pp.3-30 (1998) DOI:10.1145/272991.272995 for explicit form]
  //
  // f(x) =         af^(w-r) * bf^r
  //      + a(0)  * af^(w-r) * bf^(r-1)
  //      + a(1)  * af^(w-r) * bf^(r-2)
  //      + ....
  //      + a(r-2)* af^(w-r) * bf^(1)
  //      + a(r-1)* af^(w-r)
  //      + a(r)  * af^(w-r-1)
  //      + a(r+1)* af^(w-r-2)
  //      + ...
  //      + a(w-2)* af^(1)
  //      + a(w-1)
  // where
  //   af = x^nn     + x^mm;
  //   bf = x^(nn-1) + x^(mm-1);
  //
  GF2X *af, *bf;
  af = new GF2X;
  bf = new GF2X;
  SetCoeff(*af,nn);
  SetCoeff(*af,mm);   // af = x^nn + x^mm;
  SetCoeff(*bf,nn-1);
  SetCoeff(*bf,mm-1); // bf = x^(nn-1) + x^(mm-1)
  GF2X *f;
  f = new GF2X;
  *f = power(*af,ww-rr) * power(*bf,rr);
  for (int i=0;i<rr;i++) {
    int ib = i % ww;
    int a = (avec >> ib) & 0x1;
    if (1 == a) *f += power(*af,ww-rr) * power(*bf,rr-1-i);
  }
  for (int i=rr;i<ww;i++) {
    int ib = i % ww;
    int a = (avec >> ib) & 0x1;
    if (1 == a) *f += power(*af,ww-1-i);
  }
  delete af;
  delete bf;

  //
  // compute r(x) = x^((2^nj)*id) mod f(x)
  //

  //
  // g(x) = x^(2^nj) mod f(x)
  //
  GF2X *g;
  g = new GF2X;
  if ( ns < nj ) {
    SetCoeff(*g,(1 << ns)); // g(x) = x^(2^ns)
    *g = *g % (*f);
    for (int i=ns;i < nj;i++) *g = power(*g,2) % (*f);

  } else {

    SetCoeff(*g,(1 << nj)); // g(x) = x^(2^nj)

  }

#ifdef _DEBUG_
  printf("%8d\n",ns);
  printf("%8d\n",nj);
  printf("@");
  print_hex(*f);
  printf("@");
  print_hex(*g);
#endif

  //
  // r(x) = g(x)^id mod f(x)
  //

  int id_bits = (int)floor(log(double(id))/log(2.0)) + 1; // bit size of id
  DEBUG_PRINTF("id=%d id_bits=%d\n",id,id_bits);
  GF2X *r;
  r = new GF2X;
  SetCoeff(*r,0);  // r(x) = 1
  for (int i=id_bits;i >=0; --i) {

    *r = power(*r,2) % (*f);

    if (1 == ((id >> i)& 1)) *r = ((*r) * (*g)) % (*f);

  }
  delete g;
  delete f;

#ifdef _DEBUG_
  printf("@");
  print_hex(*r);
#endif

  //
  // extract bit sequence (=pp[]) from r(x)
  //
  {
    int nb = NumBits(*r);
    for(int i = 0; i < nn ; i++) pp[i] = 0;
    for(int i = 0; i < nb ; i++) {
      int iw = i / ww;
      int ib = i % ww;
      if (1 == coeff(*r,i)) pp[iw] += (1 << ib);
    }
    np = nb;
  }
  delete r;

}


///////////////////////////////////////////////////////////////////////////////
// ars::point.hpp                                                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_ARS_POINT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_POINT_HPP_ER_2009
#include <ostream>
#include <boost/ars/constant.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// Given an unnormalized density f, this class is a representation for (x,y,dy),
// where y = log f(x).
template <typename T>
class point{
    typedef constant<T> const_;

    public:
    point(): x_(const_::zero_),y_(const_::zero_),dy_(const_::zero_){}
    point(const T& x,const T& y,const T& dy)
    : x_(x),y_(y),dy_(dy){}

    const T& x()const{ return x_; }
    const T& y()const{ return y_; }
    const T& dy()const{ return dy_; }

    private:
    //abscissa, ordinate, derivative of the log density
    T x_, y_, dy_;
};

template<typename T>
void dump(const point<T>& p,T& x,T& y,T& dy){
    x = p.x();
    y = p.y();
    dy = p.dy();
}

template <typename T>
bool operator<(
    const point<T> &a,
    const point<T> &b
){
    return (a.x() < b.x());
}

template <typename T>
std::ostream&
operator<<(std::ostream &out, const point<T>& p)
{
    out << '(' << p.x() << ',' << p.y() << ',' << p.dy() << ')';
    return out;
}

template<typename T,typename F>
point<T>
create_point(
    const T& x,
    const F& f
){
    typedef point<T> result_t;
    T y, dy;
    f(x,y,dy);
    return result_t(x,y,dy);
}

template<typename T>
T tangent(const point<T>& a, const T& x){
    return a.y() + a.dy() * (x-a.x());
}

template<typename T>
bool is_non_increasing_dy(const point<T>&a,const point<T>& b){
    return !(a.dy()<b.dy());
}

template<typename T>
bool is_concave(const point<T>&a,const point<T>& b){
    T t_b = tangent(a,b.x());
    T t_a = tangent(b,a.x());
    return !( (t_b < b.y()) || (t_a < a.y()) );
}

template<typename T>
T linearly_interpolate(
    const point<T>& a,
    const point<T>& b,
    const T& x
){
    T slope =  (b.y() - a.y()) / (b.x() - a.x());
    return a.y() + (x-a.x()) * slope;
}

}// ars
}// detail
}// statistics
}// boost

#endif // BOOST_STATISTICS_DETAIL_ARS_POINT_HPP_ER_2009


#include <boost/numeric/bindings/traits/ublas_banded.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>
#include <boost/numeric/bindings/lapack/gbsv.hpp>
#include <vector>
#include <stdexcept>

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

static const char NORMAL = 'N';
static const char TRANSPOSE = 'T';

// solves the equation Ax = B, and puts the solution in B
// A is mutated by this routine
template <typename MatrA, typename MatrB>
void InPlaceSolve(MatrA& a, MatrB& b)
{
  std::vector<integer_t> piv(a.size1());
  int ret = lapack::gbtrf(a, piv);
  if (ret < 0) {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRF had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRF had an illegal value");
  }
  if (ret > 0) {
    //CStdString err;
    //err.Format("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF", ret, ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF");
  }

  ret = lapack::gbtrs(NORMAL, a, piv, b);
  if (ret < 0) {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRS had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRS had an illegal value");
  }
}

template<typename T>
void do_typename()
{
  using namespace boost::numeric::ublas;
  // if the matrix has kl lower and ku upper diagonals, then we should
  // allocate kl lower and kl+ku upper diagonals
  size_t sz = 1000, kl = 1, ku = 1;
  ublas::banded_matrix<T> a(sz, sz, kl, kl+ku);
  ublas::vector<T> b(sz);
  // fill values in a and b
  for (size_t i = 0; i < sz; ++i) {
    a(i,i) = i;
    b(i) = i;
  }
  for (size_t i = 1; i < sz; ++i) {
    a(i,i-1) = i;
    a(i-1,i) = 1;
  }
  InPlaceSolve(a, b);
}

int main()
{
  do_typename<float>();
  do_typename<double>();
  do_typename<std::complex<float> >();
  do_typename<std::complex<double> >();
  return 0;
}


//
// Copyright (c) 2019 INRIA
//

#include <pinocchio/math/quaternion.hpp>
#include <pinocchio/spatial/se3.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_assignQuaternion)
{
  using namespace pinocchio;
  const int max_tests = 1e5;
  for(int k = 0; k < max_tests; ++k)
  {
    const SE3 M(SE3::Random());
    SE3::Quaternion quat_ref(M.rotation());

    SE3::Quaternion quat;
    quaternion::assignQuaternion(quat,M.rotation());

    BOOST_CHECK(quat.coeffs().isApprox(quat_ref.coeffs()));
  }
}

BOOST_AUTO_TEST_SUITE_END()