/**
 * @file advectionfv2d.cc
 * @brief NPDE homework AdvectionFV2D code
 * @author Philipp Egg
 * @date 21.06.2020
 * @copyright Developed at ETH Zurich
 */

#include "advectionfv2d.h"

#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <algorithm>
#include <array>
#include <memory>
#include <stdexcept>
#include <vector>

namespace AdvectionFV2D {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 2, 3> gradbarycoordinates(
    const Eigen::Matrix<double, 2, 3> &triangle) {
  Eigen::Matrix3d X;

  // solve for the coefficients of the barycentric coordinate functions
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = triangle.transpose();
  return X.inverse().block<2, 3>(1, 0);
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::shared_ptr<
    lf::mesh::utils::CodimMeshDataSet<Eigen::Matrix<double, 2, Eigen::Dynamic>>>
computeCellNormals(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Appears only in mastersolution

  // Initialize datastruture for the result
  lf::mesh::utils::CodimMeshDataSet<Eigen::Matrix<double, 2, Eigen::Dynamic>>
      result(mesh_p, 0);

  // Compute normal vectors
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    const lf::geometry::Geometry *geo_p = cell->Geometry();
    const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

    if (corners.cols() == 3) {
      Eigen::Matrix<double, 2, Eigen::Dynamic> normal_vectors(2, 3);

      // Compute normal vectors
      Eigen::Matrix<double, 2, 3> bary_coord = gradbarycoordinates(corners);

      // Reorder computed normal vectors
      // normal_vectors[0] -> normal_vector of edge[0]
      normal_vectors.col(0) = -(bary_coord.col(2)).normalized();
      normal_vectors.col(1) = -(bary_coord.col(0)).normalized();
      normal_vectors.col(2) = -(bary_coord.col(1)).normalized();

      // Save the matrix in our datastructure
      result(*cell) = normal_vectors;
    } else {  // corners.cols() == 4
      Eigen::Matrix<double, 2, Eigen::Dynamic> normal_vectors(2, 4);

      // Split the quadrilateral into two triangles (1,2,3) and (1,3,4)
      Eigen::Matrix<double, 2, 3> tria_1;
      Eigen::Matrix<double, 2, 3> tria_2;
      tria_1 << corners.col(0), corners.col(1), corners.col(2);
      tria_2 << corners.col(0), corners.col(2), corners.col(3);

      // Compute normal vectors
      Eigen::Matrix<double, 2, 3> bary_coord_1 = gradbarycoordinates(tria_1);
      Eigen::Matrix<double, 2, 3> bary_coord_2 = gradbarycoordinates(tria_2);

      // Reorder computed normal vectors
      // normal_vectors[0] -> normal_vector of edge[0]
      normal_vectors.col(0) = -(bary_coord_1.col(2)).normalized();
      normal_vectors.col(1) = -(bary_coord_1.col(0)).normalized();
      normal_vectors.col(2) = -(bary_coord_2.col(0)).normalized();
      normal_vectors.col(3) = -(bary_coord_2.col(1)).normalized();

      // Save the matrix in our datastructure
      result(*cell) = normal_vectors;
    }
  }
  return std::make_shared<lf::mesh::utils::CodimMeshDataSet<
      Eigen::Matrix<double, 2, Eigen::Dynamic>>>(result);

}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
std::shared_ptr<
    lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 4>>>
getAdjacentCellPointers(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Initialize auxilary object
  lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 2>>
      aux_obj(mesh_p, 1, {nullptr, nullptr});

  // Iterate over every cell
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    auto cell_edges = cell->SubEntities(1);

    // Iterate over every edge of the cell
    for (const lf::mesh::Entity *edge : cell_edges) {
      // If aux_obj at index 0 was not set, save cell there
      // otherwise save at the second position
      // (The first position has to be set; the second might be set)
      if (aux_obj(*edge)[0] == nullptr) {
        aux_obj(*edge)[0] = cell;
      } else if (aux_obj(*edge)[1] == nullptr) {
        aux_obj(*edge)[1] = cell;
      } else {
        throw std::runtime_error("Error in aux_obj");
      }
    }
  }

  // Initialize datastructure for result
  lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 4>>
      result(mesh_p, 0, {nullptr, nullptr, nullptr, nullptr});

  // Collect the objects of the auxilary object
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    auto cell_edges = cell->SubEntities(1);
    int counter = 0;
    for (const lf::mesh::Entity *edge : cell_edges) {
      if (aux_obj(*edge)[0] != cell) {
        result(*cell)[counter] = aux_obj(*edge)[0];
      } else {
        result(*cell)[counter] = aux_obj(*edge)[1];
      }
      counter++;
    }
  }

  return std::make_shared<lf::mesh::utils::CodimMeshDataSet<
      std::array<const lf::mesh::Entity *, 4>>>(result);
}
/* SAM_LISTING_END_3 */

// Function returning the barycenter of TRIA or QUAD
/* SAM_LISTING_BEGIN_4 */
Eigen::Vector2d barycenter(const Eigen::MatrixXd corners) {
  Eigen::Vector2d midpoint;
  if (corners.cols() == 3) {
    midpoint = (corners.col(0) + corners.col(1) + corners.col(2)) / 3.0;
  } else if (corners.cols() == 4) {
    midpoint =
        (corners.col(0) + corners.col(1) + corners.col(2) + corners.col(3)) /
        4.0;
  } else {
    throw std::runtime_error("Wrong geometrie in barycenter()");
  }
  return midpoint;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
double computeHmin(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Vector to store the distances between cells
  std::vector<double> min_h;

  // Get Adjectent Cells
  std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
      std::array<const lf::mesh::Entity *, 4>>>
      adjacentCells = AdvectionFV2D::getAdjacentCellPointers(mesh_p);

  // Iterate over all cells
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    const lf::geometry::Geometry *geo_p = cell->Geometry();
    const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

    // Compute the barycenter of the cell
    Eigen::Vector2d cur_midpoint = barycenter(corners);

    // Iterate over all adjecent cells
    for (const lf::mesh::Entity *neighbour_cell : (*adjacentCells)(*cell)) {
      // Check that the neighbor exists
      if (neighbour_cell != nullptr) {
        const lf::geometry::Geometry *geo_p_neighbour =
            neighbour_cell->Geometry();
        const Eigen::MatrixXd neighbour_corners =
            lf::geometry::Corners(*geo_p_neighbour);

        // Compute barycenter of neighbour cell
        Eigen::Vector2d neighbour_midpoint = barycenter(neighbour_corners);

        // Compute distances between cells
        double distance = (cur_midpoint - neighbour_midpoint).norm();

        // Store value in vector
        min_h.push_back(distance);
      }
    }
  }

  // Find the minimum value in the vector and return it
  return *std::min_element(std::begin(min_h), std::end(min_h));
}
/* SAM_LISTING_END_5 */

}  // namespace AdvectionFV2D


<%
cfg['compiler_args'] = ['-std=c++11']
cfg['include_dirs'] = ['../notebooks/eigen3']
setup_pybind11(cfg)
%>

#include <cmath>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/LU>

namespace py = pybind11;
using namespace Eigen;

VectorXd logistic(VectorXd xs) {
    return 1 - 1 / (1 + exp(xs.array()));
}

MatrixXd gd(MatrixXd X, VectorXd y, VectorXd beta, double alpha, int niter) {

    int i = 0, n, p;
    MatrixXd Xt;
    VectorXd y_pred, epsilon, grad;

    n = X.rows();
    p = X.cols();
    Xt = X.transpose();
    y_pred = VectorXd::Zero(n);
    epsilon = VectorXd::Zero(n);
    grad = VectorXd::Zero(p);

    for(i = 0; i < niter; i++) {
        y_pred = logistic(X * beta);
        epsilon = y - y_pred;
        grad = Xt * epsilon / n;
        beta += alpha * grad;
    }

    return beta;
}

PYBIND11_MODULE(logistic_gd, m) {
    m.doc() = "auto-compiled c++ extension";
    m.def("logistic", &logistic);
    m.def("gd", &gd);
}


//#####################################################################
//  Copyright (c) 2011-2013 Nathan Mitchell, Eftychios Sifakis.
//  This file is covered by the FreeBSD license. Please refer to the
//  license.txt file for more information.
//#####################################################################

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

using namespace Eigen;

namespace{
template<class T_MATRIX>
void Print_Formatted(const T_MATRIX& A,std::ostream& output)
{
    for(int i=0;i<A.rows();i++){
        for(int j=0;j<A.cols();j++){
            output<<std::setw(12)<<A(i,j);
            if(j<A.cols()-1) output<<" ";}
        output<<std::endl;}
}
}

template<class T>
void Singular_Value_Decomposition_Reference(const T A[9], T U[9], T Sigma[3], T V[9])
{
    Map<const Matrix<T,3,3>> mA=Map<const Matrix<T,3,3>>(A);

    Map<Matrix<T,3,3>> mU=Map<Matrix<T,3,3>>(U);
    Map<Matrix<T,3,1>> mSigma=Map<Matrix<T,3,1>>(Sigma);
    Map<Matrix<T,3,3>> mV=Map<Matrix<T,3,3>>(V);

    JacobiSVD<Matrix<T,3,3>> svd(mA, ComputeFullU|ComputeFullV);
    mU=svd.matrixU();
    mSigma=svd.singularValues();
    mV=svd.matrixV();

    if(mU.determinant() < 0.) {
        mU.col(2) *= -1.;
        mSigma(2) *= -1.;
    }

    if(mV.determinant() < 0.) {
        mV.col(2) *= -1.;
        mSigma(2) *= -1.;
    }
}

template<class T>
bool Singular_Value_Decomposition_Compare(const T U[9], const T Sigma[3], const T V[9],
                                          const T U_reference[9], const T Sigma_reference[3], const T V_reference[9])
{

    Map<const Matrix<T,3,3>> mU=Map<const Matrix<T,3,3>>(U);
    Map<const Matrix<T,3,1>> mSigma=Map<const Matrix<T,3,1>>(Sigma);
    Map<const Matrix<T,3,3>> mV=Map<const Matrix<T,3,3>>(V);

    Map<const Matrix<T,3,3>> mU_reference=Map<const Matrix<T,3,3>>(U_reference);
    Map<const Matrix<T,3,1>> mSigma_reference=Map<const Matrix<T,3,1>>(Sigma_reference);
    Map<const Matrix<T,3,3>> mV_reference=Map<const Matrix<T,3,3>>(V_reference);

    Matrix<T,3,3> mU_adjusted = mU_reference;
    Matrix<T,3,1> mSigma_adjusted = mSigma_reference;
    Matrix<T,3,3> mV_adjusted = mV_reference;

    for (int i = 0; i < 2; i++) {

      if (mU.col(i).dot(mU_adjusted.col(i)) < 0.) {
          mU_adjusted.col(i) *= -1.;
          mSigma_adjusted(i) *= -1.;
          mU_adjusted.col(2) *= -1.;
          mSigma_adjusted(2) *= -1.;
      }

      if (mV.col(i).dot(mV_adjusted.col(i)) < 0.) {
          mV_adjusted.col(i) *= -1.;
          mSigma_adjusted(i) *= -1.;
          mV_adjusted.col(2) *= -1.;
          mSigma_adjusted(2) *= -1.;
      }

    }

    std::cout<<"Computed matrix U :"<<std::endl;Print_Formatted(mU,std::cout);
    std::cout<<"Reference matrix U :"<<std::endl;Print_Formatted(mU_reference,std::cout);
    std::cout<<"Adjusted matrix U :"<<std::endl;Print_Formatted(mU_adjusted,std::cout);
    std::cout<<"Difference = "<< (mU-mU_adjusted).norm()  <<std::endl;

    std::cout<<std::endl;
    std::cout<<"Computed matrix Sigma :"<<std::endl;Print_Formatted(mSigma,std::cout);
    std::cout<<"Reference matrix Sigma :"<<std::endl;Print_Formatted(mSigma_reference,std::cout);
    std::cout<<"Adjusted matrix Sigma :"<<std::endl;Print_Formatted(mSigma_adjusted,std::cout);
    std::cout<<"Difference = "<<  (mSigma-mSigma_adjusted).norm()  <<std::endl;

    std::cout<<std::endl;
    std::cout<<"Computed matrix V :"<<std::endl;Print_Formatted(mV,std::cout);
    std::cout<<"Reference matrix V :"<<std::endl;Print_Formatted(mV_reference,std::cout);
    std::cout<<"Adjusted matrix V :"<<std::endl;Print_Formatted(mV_adjusted,std::cout);
    std::cout<<"Difference = "<< (mV-mV_adjusted).norm()  <<std::endl;

    Matrix<T,3,3> mA=mU*mSigma.asDiagonal()*mV.transpose();
    Matrix<T,3,3> mA_reference=mU_reference*mSigma_reference.asDiagonal()*mV_reference.transpose();

    std::cout<<std::endl;
    std::cout<<"Computed matrix A=U*Sigma*V^T :"<<std::endl;Print_Formatted(mA,std::cout);
    std::cout<<"Reference matrix A=U*Sigma*V^T :"<<std::endl;Print_Formatted(mA_reference,std::cout);
    std::cout<<"Difference = "<< (mA-mA_reference).norm()  <<std::endl;

    if(!((mU-mU_adjusted).norm() < 0.00001) )
        return false;

    if(!((mSigma-mSigma_adjusted).norm() < 0.00001) )
        return false;

    if(!((mV-mV_adjusted).norm() < 0.00001) )
        return false;

    return true;
}

template void Singular_Value_Decomposition_Reference(const float A[9], float U[9], float Sigma[3], float V[9]);
template bool Singular_Value_Decomposition_Compare(const float U[9], const float Sigma[3], const float V[9],
                                                   const float U_reference[9], const float Sigma_reference[3], const float V_reference[9]);


/*
 * Copyright 2015 C. Brett Witherspoon
 */

#define BOOST_TEST_MODULE signum_tests
#include <boost/test/unit_test.hpp>

#include "signum/math.hpp"

BOOST_AUTO_TEST_CASE(abs_test)
{
  using signum::math::abs;

  BOOST_CHECK_EQUAL(abs(-5), 5);

  BOOST_CHECK_EQUAL(abs(12), 12);

  BOOST_CHECK_EQUAL(abs(-1.3), 1.3);

  BOOST_CHECK_EQUAL(abs(5.4), 5.4);
}

BOOST_AUTO_TEST_CASE(max_test)
{
  using signum::math::max;

  BOOST_CHECK_EQUAL(max(-7, 5), 5);

  BOOST_CHECK_EQUAL(max(7, 67), 67);

  BOOST_CHECK_EQUAL(max(-9.0, 5.7), 5.7);
}

BOOST_AUTO_TEST_CASE(gcd_test)
{
  using signum::math::gcd;

  BOOST_CHECK_EQUAL(gcd(6, 15), 3);
}

BOOST_AUTO_TEST_CASE(lcm_test)
{
  using signum::math::lcm;

  BOOST_CHECK_EQUAL(lcm(4, 10), 20);
}

BOOST_AUTO_TEST_CASE(nextpow2_test)
{
  using signum::math::nextpow2;

  BOOST_CHECK_EQUAL(nextpow2(600000), 1048576);
}


#include "tile/math/bignum.h"

#include <boost/math/common_factor_rt.hpp>

namespace vertexai {
namespace tile {
namespace math {

Integer Floor(const Rational& x) {
  if (x < 0) {
    return (numerator(x) - denominator(x) + 1) / denominator(x);
  } else {
    return numerator(x) / denominator(x);
  }
}

Integer Ceil(const Rational& x) { return Floor(Rational(numerator(x) - 1, denominator(x))) + 1; }

Rational FracPart(const Rational& x) { return x - Floor(x); }

Integer Abs(const Integer& x) {
  if (x < 0) {
    return -x;
  }
  return x;
}

Rational Abs(const Rational& x) {
  if (x < 0) {
    return -x;
  }
  return x;
}

Rational Reduce(const Rational& v, const Rational& m) { return v - Floor(v / m) * m; }

Integer XGCD(const Integer& a, const Integer& b, Integer& x, Integer& y) {  // NOLINT(runtime/references)
  if (b == 0) {
    x = 1;
    y = 0;
    return a;
  }

  Integer x1;
  Integer gcd = XGCD(b, a % b, x1, x);
  y = x1 - (a / b) * x;

  if (gcd < 0) {
    gcd *= -1;
    x *= -1;
    y *= -1;
  }

  return gcd;
}

Rational XGCD(const Rational& a, const Rational& b, Integer& x, Integer& y) {  // NOLINT(runtime/references)
  Integer m = boost::math::lcm(denominator(a), denominator(b));
  Rational o;
  o = Rational(XGCD(numerator(a * m), numerator(b * m), x, y), m);

  if (o < 0) {
    o *= -1;
    x *= -1;
    y *= -1;
  }
  return o;
}

Rational GCD(const Rational& a, const Rational& b) {
  Integer m = boost::math::lcm(denominator(a), denominator(b));
  Integer g = boost::math::gcd(numerator(a * m), numerator(b * m));
  return Rational(g, m);
}

Integer GCD(const Integer& a, const Integer& b) { return boost::math::gcd(a, b); }

Integer LCM(const Integer& a, const Integer& b) { return boost::math::lcm(a, b); }

Integer Min(const Integer& a, const Integer& b) {
  if (a < b)
    return a;
  else
    return b;
}

Rational Min(const Rational& a, const Rational& b) {
  if (a < b)
    return a;
  else
    return b;
}

Integer Max(const Integer& a, const Integer& b) {
  if (a < b)
    return b;
  else
    return a;
}

Rational Max(const Rational& a, const Rational& b) {
  if (a < b)
    return b;
  else
    return a;
}

Integer RatDiv(const Rational& a, const Rational& b, Rational& r) {  // NOLINT(runtime/references)
  Integer q = Floor(a / b);
  r = a - q * b;
  return q;
}

}  // namespace math
}  // namespace tile
}  // namespace vertexai


#ifndef Interpolators__2D_BilinearInterpolator_hpp
#define Interpolators__2D_BilinearInterpolator_hpp

/** @file BilinearInterpolator.hpp
  * @brief
  * @author C.D. Clark III
  * @date 12/27/16
  */

#include "InterpolatorBase.hpp"
#include <boost/range/algorithm/lower_bound.hpp>
#include <boost/range/adaptor/strided.hpp>

/** @class
  * @brief Linear interpolation for 2D functions.
  * @author C.D. Clark III, Aaron Hoffman
  */

namespace _2D {

template<class Real>
class BilinearInterpolator : public InterpolatorBase<BilinearInterpolator<Real>>
{
  public:
    using BASE = InterpolatorBase<BilinearInterpolator<Real>>;
    using VectorType = typename BASE::VectorType;
    using MapType = typename BASE::MapType;

    // types used to view data as 2D coordinates
    using MatrixType    = typename BASE::MatrixType;
    using _2DVectorView = typename BASE::_2DVectorView;
    using _2DMatrixView = typename BASE::_2DMatrixView;

    // types used for 2x2 matrix algebra
    using Matrix22 = Eigen::Matrix<Real,2,2 >;
    using Matrix22Array = Eigen::Array< Matrix22, Eigen::Dynamic, Eigen::Dynamic >;
    using ColVector2 = Eigen::Matrix<Real,2,1 >;
    using RowVector2 = Eigen::Matrix<Real,1,2 >;

  protected:

    using BASE::xView;
    using BASE::yView;
    using BASE::zView;
    using BASE::X;
    using BASE::Y;
    using BASE::Z;

    Matrix22Array Q; // naming convention used by wikipedia article (see Wikipedia https://en.wikipedia.org/wiki/Bilinear_interpolation)

  public:

    template<typename I>
    BilinearInterpolator( I n, Real *x, Real *y, Real *z ) {this->setData(n,x,y,z);}

    template<typename X, typename Y, typename Z>
    BilinearInterpolator( X &x, Y &y, Z &z ) {this->setData(x,y,z);}

    BilinearInterpolator():BASE(){}
    BilinearInterpolator(const BilinearInterpolator& rhs)
    :BASE(rhs)
    ,Q(rhs.Q)
    {}

    // copy-swap idiom
    friend void swap( BilinearInterpolator& lhs, BilinearInterpolator& rhs)
    {
      lhs.Q.swap(rhs.Q);
      swap( static_cast<BASE&>(lhs), static_cast<BASE&>(rhs) );
    }

    BilinearInterpolator& operator=(BilinearInterpolator rhs)
    {
      swap(*this,rhs);
      return *this;
    }

    Real operator()( Real x, Real y ) const;

  private:

    void setupInterpolator();
    friend BASE;

};

template<class Real>
void
BilinearInterpolator<Real>::setupInterpolator()
{

  // Interpolation will be done by multiplying the coordinates by coefficients.

  Q = Matrix22Array( X->size()-1, Y->size()-1 );

  // We are going to pre-compute the interpolation coefficients so
  // that we can interpolate quickly
  for(int i = 0; i < X->size() - 1; i++)
  {
    for( int j = 0; j < Y->size() - 1; j++)
    {
      Real tmp = ( ((*X)(i+1) - (*X)(i) )*( (*Y)(j+1) - (*Y)(j) ) );
      Q(i,j) = Z->block(i,j,2,2)/tmp;
    }
  }
}

template<class Real>
Real
BilinearInterpolator<Real>::operator()( Real x, Real y ) const
{
  BASE::checkData();

  // no extrapolation...
  if( x < (*X)(0)
   || x > (*X)(X->size()-1)
   || y < (*Y)(0)
   || y > (*Y)(Y->size()-1) )
  {
    return 0;
  }
  // find the x index that is just to the LEFT of x
  //int i  = Utils::index__last_lt( x, *X, X->size() );
  // NOTE: X data is strided.
  auto xrng = std::make_pair( X->data(), X->data()+X->size()*X->innerStride() ) | boost::adaptors::strided(X->innerStride());
  int i = boost::lower_bound( xrng, x) - boost::begin(xrng) - 1;
  if(i < 0)
    i = 0;

  // find the y index that is just BELOW y
  //int j  = Utils::index__last_lt( y, *Y, Y->size() );
  // NOTE: Y data is NOT strided
  auto yrng = std::make_pair( Y->data(), Y->data()+Y->size() );
  int j = boost::lower_bound( yrng, y) - boost::begin(yrng) - 1;
  if(j < 0)
    j = 0;

  // now, create the coordinate vectors (see Wikipedia https://en.wikipedia.org/wiki/Bilinear_interpolation)
  RowVector2 vx;
  ColVector2 vy;
  vx << ((*X)(i+1) - x), (x - (*X)(i));
  vy << ((*Y)(j+1) - y), (y - (*Y)(j));

  // interpolation is just x*Q*y

  return vx*Q(i,j)*vy;
}

}

#endif // include protector


#include <Eigen/Core>
#include <Eigen/QR>

#include "tools.hpp"

using namespace std;

// For converting back and forth between radians and degrees.
constexpr double pi() {
  return M_PI;
}

double deg2rad(double x) {
  return x * pi() / 180;
}

double rad2deg(double x) {
  return x * 180 / pi();
}

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.rfind("}]");
  if (found_null != string::npos) {
    return "";
  } else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 2);
  }
  return "";
}

// Evaluate a polynomial.
double polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); j++) {
    for (int i = 0; i < order; i++) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

// 2D affine transformation for a list of points
void Tranform2d(std::vector<double>       &dst_x,
                std::vector<double>       &dst_y,
                const std::vector<double> &src_x,
                const std::vector<double> &src_y,
                double translation_x, double translation_y, double psi) {

  // make sure destination vectors have same length as source vectors
  dst_x.resize(src_x.size());
  dst_y.resize(src_y.size());

  for (int idx = 0; idx < (int) src_x.size(); idx++) {
    double dx = src_x[idx] - translation_x;
    double dy = src_y[idx] - translation_y;
    dst_x[idx] = dx*cos(psi) - dy*sin(psi);
    dst_y[idx] = dx*sin(psi) + dy*cos(psi);
  }
}


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

#include <boost/config.hpp>
#define BOOST_TEST_MAIN
#include <boost/array.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/mpl/list.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <utility>
#include <boost/random.hpp>

//
// Naive implmentation, any fancy versions we create should always agree with this:
//
template <class T>
typename boost::enable_if_c<std::numeric_limits<T>::is_signed, T>::type unsigned_abs(T v) { return v < 0 ? -v : v; }
template <class T>
typename boost::disable_if_c<std::numeric_limits<T>::is_signed, T>::type unsigned_abs(T v) { return v; }

template <class T>
T euclid_textbook(T a, T b)
{
   using std::swap;
   if(a < b)
      swap(a, b);
   while(b)
   {
      T t = b;
      b = a % b;
      a = t;
   }
   return unsigned_abs(a);
}

typedef boost::mpl::list<boost::int32_t
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   , boost::int64_t, boost::multiprecision::cpp_int
#endif
> signed_integral_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_zero, T, signed_integral_test_types)
{
    T a = boost::math::gcd(static_cast<T>(2), static_cast<T>(0));
    BOOST_CHECK_EQUAL(a, static_cast<T>(2));
    a = boost::math::gcd(static_cast<T>(0), static_cast<T>(2));
    BOOST_CHECK_EQUAL(a, static_cast<T>(2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_signed, T, signed_integral_test_types)
{
    T a = boost::math::gcd(static_cast<T>(-40902), static_cast<T>(-24140));
    BOOST_CHECK_EQUAL(a, static_cast<T>(34));
    a = boost::math::gcd(static_cast<T>(40902), static_cast<T>(24140));
    BOOST_CHECK_EQUAL(a, static_cast<T>(34));
}

typedef boost::mpl::list<boost::uint32_t
#if !BOOST_WORKAROUND(BOOST_MSVC, <= 1500)
   , boost::uint64_t, boost::multiprecision::uint256_t
#endif
> unsigned_integral_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_unsigned, T, unsigned_integral_test_types)
{
    T a = boost::math::gcd(static_cast<T>(40902), static_cast<T>(24140));
    BOOST_CHECK_EQUAL(a, static_cast<T>(34));
    a = boost::math::gcd(static_cast<T>(1836311903), static_cast<T>(2971215073)); // 46th and 47th Fibonacci numbers. 47th is prime.
    BOOST_CHECK_EQUAL(a, static_cast<T>(1));
}

typedef boost::mpl::list<boost::int32_t, boost::int64_t> short_signed_integral_test_types;
typedef boost::mpl::list<boost::uint32_t, boost::uint64_t> short_unsigned_integral_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(signed_random_test, T, short_signed_integral_test_types)
{
   boost::random::mt19937 gen;
   boost::uniform_int<T> dist((std::numeric_limits<T>::min)(), (std::numeric_limits<T>::max)());
   for(unsigned i = 0; i < 100000; ++i)
   {
      T u = dist(gen);
      T v = dist(gen);
      BOOST_CHECK_EQUAL(boost::math::gcd(u, v), euclid_textbook(u, v));
   }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_random_unsigned, T, short_unsigned_integral_test_types)
{
   boost::random::mt19937 gen;
   boost::uniform_int<T> dist((std::numeric_limits<T>::min)(), (std::numeric_limits<T>::max)());
   for(unsigned i = 0; i < 100000; ++i)
   {
      T u = dist(gen);
      T v = dist(gen);
      BOOST_CHECK_EQUAL(boost::math::gcd(u, v), euclid_textbook(u, v));
   }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_gcd_range, T, unsigned_integral_test_types)
{
    std::vector<T> a;
    typedef typename std::vector<T>::iterator I;
    std::pair<T, I> d;
    a.push_back(40902);
    d = boost::math::gcd_range(a.begin(), a.end());
    BOOST_CHECK(d == std::make_pair(T(40902), a.end()));
    a.push_back(24140);
    d = boost::math::gcd_range(a.begin(), a.end());
    BOOST_CHECK(d == std::make_pair(T(34), a.end()));
    a.push_back(85);
    d = boost::math::gcd_range(a.begin(), a.end());
    BOOST_CHECK(d == std::make_pair(T(17), a.end()));
    a.push_back(23893);
    d = boost::math::gcd_range(a.begin(), a.end());
    BOOST_CHECK(d == std::make_pair(T(1), a.end()));
    a.push_back(1024);
    d = boost::math::gcd_range(a.begin(), a.end());
    BOOST_CHECK(d == std::make_pair(T(1), a.end() - 1));
}


/*****************************************************************************
 * array.cpp        Blitz++ Array stencilling example
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    int N = 64;

    // Create three-dimensional arrays of float
    Array<float,3> A(N,N,N), B(N,N,N);

    // Set up initial conditions: +30 C over an interior block,
    // and +22 C elsewhere
    A = 22.0;

    Range interior(N/4,3*N/4);
    A(interior,interior,interior) = 30.0;

    int numIters = 301;

    Range I(1,N-2), J(1,N-2), K(1,N-2);

#ifdef BZ_HAVE_STD
#ifdef BZ_ARRAY_SPACE_FILLING_TRAVERSAL
    generateFastTraversalOrder(TinyVector<int,2>(N-2,N-2));
#endif
#endif

    for (int i=0; i < numIters; ++i)
    {
        double c = 1/6.5;

        B(I,J,K) = c * (.5 * A(I,J,K) + A(I+1,J,K) + A(I-1,J,K)
            + A(I,J+1,K) + A(I,J-1,K) + A(I,J,K+1) + A(I,J,K-1));

        A(I,J,K) = c * (.5 * B(I,J,K) + B(I+1,J,K) + B(I-1,J,K)
            + B(I,J+1,K) + B(I,J-1,K) + B(I,J,K+1) + B(I,J,K-1));

        // Output the result along a line through the centre
        for (int j=0; j < 8; ++j)
            cout << setprecision(2) << A(N/2,N/2,j*N/8) << " ";

        cout << endl;
        cout.flush();
    }

    return 0;
}


#ifndef CTCD_DISTANCE_HPP
#define CTCD_DISTANCE_HPP 1

#include <Eigen/Geometry>
#include <algorithm>

// Source: Etienne Vouga
// https://github.com/evouga/collisiondetection

namespace mcl {
namespace ctcd
{

  // Efficiently calculates whether or not a point p is closer to than eta distance to the plane spanned by vertices q0, q1, and q2.
  // This method does not require any floating point divisions and so is significantly faster than computing the distance itself.
  static inline bool vertexPlaneDistanceLessThan(const Eigen::Vector3d &p,
					    const Eigen::Vector3d &q0, const Eigen::Vector3d &q1, const Eigen::Vector3d &q2, double eta)
  {
    Eigen::Vector3d c = (q1-q0).cross(q2-q0);
    return c.dot(p-q0)*c.dot(p-q0) < eta*eta*c.dot(c);
  }

  // Efficiently calculates whether or not the line spanned by vertices (p0, p1) is closer than eta distance to the line spanned by vertices (q0, q1).
  // This method does not require any floating point divisions and so is significantly faster than computing the distance itself.
  static inline bool lineLineDistanceLessThan(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1,
					  const Eigen::Vector3d &q0, const Eigen::Vector3d &q1, double eta)
  {
	Eigen::Vector3d c = (p1-p0).cross(q1-q0);
	return c.dot(q0-p0)*c.dot(q0-p0) < eta*eta*c.dot(c);
  }

  // Computes the vector between a point p and the closest point to p on the triangle (q0, q1, q2). Also returns the barycentric coordinates of this closest point on the triangle;
  // q0bary is the barycentric coordinate of q0, etc. (The distance from p to the triangle is the norm of this vector.)
  static inline Eigen::Vector3d vertexFaceDistance(const Eigen::Vector3d &p,
					    const Eigen::Vector3d &q0, const Eigen::Vector3d &q1, const Eigen::Vector3d &q2,
					    double &q0bary, double &q1bary, double &q2bary, bool clamp_barys = true)
  {
  Eigen::Vector3d ab = q1-q0;
  Eigen::Vector3d ac = q2-q0;
  Eigen::Vector3d ap = p-q0;

  double d1 = ab.dot(ap);
  double d2 = ac.dot(ap);

  // corner and edge cases

  if(d1 <= 0 && d2 <= 0)
    {
      q0bary = 1.0;
      q1bary = 0.0;
      q2bary = 0.0;
      return q0-p;
    }

  Eigen::Vector3d bp = p-q1;
  double d3 = ab.dot(bp);
  double d4 = ac.dot(bp);
  if(d3 >= 0 && d4 <= d3)
    {
      q0bary = 0.0;
      q1bary = 1.0;
      q2bary = 0.0;
      return q1-p;
    }

  double vc = d1*d4 - d3*d2;
  if((vc <= 0) && (d1 >= 0) && (d3 <= 0))
    {
      double v = d1 / (d1-d3);
      q0bary = 1.0 - v;
      q1bary = v;
      q2bary = 0;
      return (q0 + v*ab)-p;
    }

  Eigen::Vector3d cp = p-q2;
  double d5 = ab.dot(cp);
  double d6 = ac.dot(cp);
  if(d6 >= 0 && d5 <= d6)
    {
      q0bary = 0;
      q1bary = 0;
      q2bary = 1.0;
      return q2-p;
    }

  double vb = d5*d2 - d1*d6;
  if((vb <= 0) && (d2 >= 0) && (d6 <= 0))
    {
      double w = d2/(d2-d6);
      q0bary = 1-w;
      q1bary = 0;
      q2bary = w;
      return (q0 + w*ac)-p;
    }

  double va = d3*d6 - d5*d4;
  if((va <= 0) && (d4-d3 >= 0) && (d5-d6 >= 0))
    {
      double w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
      q0bary = 0;
      q1bary = 1.0 - w;
      q2bary = w;

      return (q1 + w*(q2-q1))-p;
    }

  // face case
  double denom = 1.0 / (va + vb + vc);
  double v = vb * denom;
  double w = vc * denom;
  double u = 1.0 - v - w;
  q0bary = u;
  q1bary = v;
  q2bary = w;

  if (clamp_barys)
  {
    q0bary = std::clamp(q0bary, 0.0, 1.0);
    q1bary = std::clamp(q1bary, 0.0, 1.0);
    q2bary = std::clamp(q2bary, 0.0, 1.0);
  }

  return (u*q0 + v*q1 + w*q2)-p;
  }

  // Computes the shotest vector between a segment (p0, p1) and segment (q0, q1). Also returns the barycentric coordinates of the closest points on both segments; p0bary is the barycentric
  // coordinate of p0, etc. (The distance between the segments is the norm of this vector).
  static inline Eigen::Vector3d edgeEdgeDistance(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1,
					  const Eigen::Vector3d &q0, const Eigen::Vector3d &q1,
					  double &p0bary, double &p1bary,
					  double &q0bary, double &q1bary,
						bool clamp_barys = true )
  {
  Eigen::Vector3d d1 = p1-p0;
  Eigen::Vector3d d2 = q1-q0;
  Eigen::Vector3d r = p0-q0;
  double a = d1.squaredNorm();
  double e = d2.squaredNorm();
  double f = d2.dot(r);

  auto myclamp = [&clamp_barys](double x)
  {
    if (!clamp_barys) { return x; }
    return std::clamp(x, 0.0, 1.0);
  };

  double s,t;

  double c = d1.dot(r);
  double b = d1.dot(d2);
  double denom = a*e-b*b;
  if(denom != 0.0)
    {
      s = myclamp( (b*f-c*e)/denom );
    }
  else
    {
      //parallel edges and/or degenerate edges; values of s doesn't matter
      s = 0;
    }
  double tnom = b*s + f;
  if(tnom < 0 || e == 0)
    {
      t = 0;
      if(a == 0)
	s = 0;
      else
	s = myclamp(-c/a);
    }
  else if(tnom > e)
    {
      t = 1.0;
      if(a == 0)
	s = 0;
      else
	s = myclamp( (b-c)/a );
    }
  else
    t = tnom/e;

  Eigen::Vector3d c1 = p0 + s*d1;
  Eigen::Vector3d c2 = q0 + t*d2;

  p0bary = 1.0-s;
  p1bary = s;
  q0bary = 1.0-t;
  q1bary = t;

  if (clamp_barys)
  {
    p0bary = std::clamp(p0bary, 0.0, 1.0);
    p1bary = std::clamp(p1bary, 0.0, 1.0);
    q0bary = std::clamp(q0bary, 0.0, 1.0);
    q1bary = std::clamp(q1bary, 0.0, 1.0);
  }

  return c2-c1;
  }

} // ns ctcd
} // ns mcl

#endif


#include <iostream>
using namespace std;

#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char **argv)
{
    Matrix<float, 2, 3> matrix_23;
    Vector3d v_3d;
    Matrix<float, 3, 1> vd_3d;
    Matrix3d matrix_33 = Matrix3d::Zero();
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;

    MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout<< "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

    // visit element via ()
    cout << "print matrix 2x3: " << endl;
    for(int i = 0; i < 2; ++i) {
        for(int j = 0; j < 3; ++j) cout << matrix_23(i, j) << "\t";
        cout << endl;
    }

    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;
    Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6]*[3,2,1]" << result.transpose() << endl;

    return 0;
}


#include "eigen_ext.hpp"
#include "integrate_func.hpp"
#include "parameters.hpp"
#include <cassert>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace pear {
double triangle_area(Vec &xp, Vec &yp) {
  double area;
  area = xp(1) * yp(2) + xp(0) * yp(1) + xp(2) * yp(0) - xp(1) * yp(0) -
         xp(0) * yp(2) - xp(2) * yp(1);
  return area;
}

Mat int_func_block(Vec &xp, Vec &yp) {
  double area = triangle_area(xp, yp);
  Mat int_func_block(3, 3);

  double b11 = 6 * xp(0) + 2 * xp(1) + 2 * xp(2);
  double b12 = 2 * xp(0) + 2 * xp(1) + xp(2);
  double b13 = 2 * xp(0) + xp(1) + 2 * xp(2);

  double b21 = 2 * xp(0) + 2 * xp(1) + xp(2);
  double b22 = 2 * xp(0) + 6 * xp(1) + 2 * xp(2);
  double b23 = xp(0) + 2 * xp(1) + 2 * xp(2);

  double b31 = 2 * xp(0) + xp(1) + 2 * xp(2);
  double b32 = xp(0) + 2 * xp(1) + 2 * xp(2);
  double b33 = 2 * xp(0) + 2 * xp(1) + 6 * xp(2);

  int_func_block << b11, b12, b13, b21, b22, b23, b31, b32, b33;
  int_func_block = int_func_block * area / 60;

  return int_func_block;
}

Mat int_func(Vec &xp, Vec &yp, MatI &t) {

  int np = xp.rows();
  int nt = t.rows();

  Mat int_F(np, np);
  Mat int_F_block(3, 3);

  VecI t_loc(3);
  Vec xp_loc(3);
  Vec yp_loc(3);

  for (int idxm = 0; idxm < nt; idxm++) {
    t_loc = t.row(idxm);
    xp_loc = pear::extract<Vec>(xp, t_loc);
    yp_loc = pear::extract<Vec>(yp, t_loc);

    int_F_block = int_func_block(xp_loc, yp_loc);
    for (int idx1 = 0; idx1 < 3; idx1++) {
      for (int idx2 = 0; idx2 < 3; idx2++) {
        int_F(t_loc(idx1), t_loc(idx2)) += int_F_block(idx1, idx2);
      }
    }
  }
  return int_F;
}

} // namespace pear


#include <Eigen/Core>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <mathtoolbox/numerical-optimization.hpp>
#include <optimization-test-functions.hpp>

int main()
{
    constexpr otf::FunctionType type       = otf::FunctionType::Rosenbrock;
    constexpr int               dimensions = 100;

    std::srand(static_cast<unsigned>(std::time(nullptr)));

    mathtoolbox::optimization::Setting setting;
    setting.algorithm          = mathtoolbox::optimization::Algorithm::Bfgs;
    setting.x_init             = Eigen::VectorXd::Random(dimensions);
    setting.f                  = [](const Eigen::VectorXd& x) { return otf::GetValue(x, type); };
    setting.g                  = [](const Eigen::VectorXd& x) { return otf::GetGrad(x, type); };
    setting.type               = mathtoolbox::optimization::Type::Min;
    setting.max_num_iterations = 1000;

    const mathtoolbox::optimization::Result result = mathtoolbox::optimization::RunOptimization(setting);

    const Eigen::VectorXd expected_solution = otf::GetSolution(dimensions, type);
    const double          expected_value    = otf::GetValue(expected_solution, type);
    const double          initial_value     = otf::GetValue(setting.x_init, type);
    const double          found_value       = otf::GetValue(result.x_star, type);

    std::cout << "#iterations: " << result.num_iterations << std::endl;
    std::cout << "Initial solution: " << setting.x_init.transpose() << " (" << initial_value << ")" << std::endl;
    std::cout << "Found solution: " << result.x_star.transpose() << " (" << found_value << ")" << std::endl;
    std::cout << "Expected solution: " << expected_solution.transpose() << " (" << expected_value << ")" << std::endl;

    return 0;
}


/*
 * find_crossing.cpp
 *
 * Finds the energy threshold crossing for a damped oscillator.
 * The algorithm uses a dense out stepper with find_if to first find an
 * interval containing the threshold crossing and the utilizes the dense out
 * functionality with a bisection to further refine the interval until some
 * desired precision is reached.
 *
 * Copyright 2015 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <utility>
#include <algorithm>
#include <array>

#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>
#include <boost/numeric/odeint/iterator/adaptive_iterator.hpp>

namespace odeint = boost::numeric::odeint;

typedef std::array<double, 2> state_type;

const double gam = 1.0;  // damping strength

void damped_osc(const state_type &x, state_type &dxdt, const double /*t*/)
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam * x[1];
}

struct energy_condition {

    // defines the threshold crossing in terms of a boolean functor

    double m_min_energy;

    energy_condition(const double min_energy)
        : m_min_energy(min_energy) { }

    double energy(const state_type &x) {
        return 0.5 * x[1] * x[1] + 0.5 * x[0] * x[0];
    }

    bool operator()(const state_type &x) {
        // becomes true if the energy becomes smaller than the threshold
        return energy(x) <= m_min_energy;
    }
};

template<class System, class Condition>
std::pair<double, state_type>
find_condition(state_type &x0, System sys, Condition cond,
               const double t_start, const double t_end, const double dt,
               const double precision = 1E-6) {

    // integrates an ODE until some threshold is crossed
    // returns time and state at the point of the threshold crossing
    // if no threshold crossing is found, some time > t_end is returned

    auto stepper = odeint::make_dense_output(1.0e-6, 1.0e-6,
                                             odeint::runge_kutta_dopri5<state_type>());

    auto ode_range = odeint::make_adaptive_range(std::ref(stepper), sys, x0,
                                                 t_start, t_end, dt);

    // find the step where the condition changes
    auto found_iter = std::find_if(ode_range.first, ode_range.second, cond);

    if(found_iter == ode_range.second)
    {
        // no threshold crossing -> return time after t_end and ic
        return std::make_pair(t_end + dt, x0);
    }

    // the dense out stepper now covers the interval where the condition changes
    // improve the solution by bisection
    double t0 = stepper.previous_time();
    double t1 = stepper.current_time();
    double t_m;
    state_type x_m;
    // use odeint's resizing functionality to allocate memory for x_m
    odeint::adjust_size_by_resizeability(x_m, x0,
                                         typename odeint::is_resizeable<state_type>::type());
    while(std::abs(t1 - t0) > precision) {
        t_m = 0.5 * (t0 + t1);  // get the mid point time
        stepper.calc_state(t_m, x_m); // obtain the corresponding state
        if (cond(x_m))
            t1 = t_m;  // condition changer lies before midpoint
        else
            t0 = t_m;  // condition changer lies after midpoint
    }
    // we found the interval of size eps, take it's midpoint as final guess
    t_m = 0.5 * (t0 + t1);
    stepper.calc_state(t_m, x_m);
    return std::make_pair(t_m, x_m);
}

int main(int argc, char **argv)
{
    state_type x0 = {{10.0, 0.0}};
    const double t_start = 0.0;
    const double t_end = 10.0;
    const double dt = 0.1;
    const double threshold = 0.1;

    energy_condition cond(threshold);
    state_type x_cond;
    double t_cond;
    std::tie(t_cond, x_cond) = find_condition(x0, damped_osc, cond,
                                              t_start, t_end, dt, 1E-6);
    if(t_cond > t_end)
    {
        // time after t_end -> no threshold crossing within [t_start, t_end]
        std::cout << "No threshold crossing found." << std::endl;
    } else
    {
        std::cout.precision(16);
        std::cout << "Time of energy threshold crossing: " << t_cond << std::endl;
        std::cout << "State: [" << x_cond[0] << " , " << x_cond[1] << "]" << std::endl;
        std::cout << "Energy: " << cond.energy(x_cond) << std::endl;
    }
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)
/*
    boost::math::fft example 06

    Fast Polynomial Multiplication with quadmath
*/

#include <boost/math/fft/bsl_backend.hpp>
#include <boost/multiprecision/complex128.hpp>
#include <iostream>
#include <vector>
#include <exception>

template<class Real, class Complex>
std::vector<Real> multiply_complex(
    const std::vector<Real>& A,
    const std::vector<Real>& B)
{
  const std::size_t N = A.size();
  std::vector< Complex > TA(N),TB(N);
  boost::math::fft::bsl_rdft<Real> P(N);
  P.real_to_complex(A.begin(),A.end(),TA.begin());
  P.real_to_complex(B.begin(),B.end(),TB.begin());

  std::vector<Real> C(N);

  for(unsigned int i=0;i<N;++i)
  {
    TA[i]*=TB[i];
  }

  P.complex_to_real(TA.begin(),TA.end(),C.begin());
  std::transform(C.begin(), C.end(), C.begin(),
                 [N](Real x) { return x / N; });

  return C;
}

template<class Real>
Real difference(const std::vector<Real>& A, const std::vector<Real>& B)
{
  using std::abs;
  Real diff{};
  if(A.size()!=B.size()) return -1;
  for(unsigned int i=0;i<A.size();++i)
  {
    diff += abs(A[i]-B[i]);
  }
  return diff;
}

template<typename Real, typename Complex>
void multiply() {
  using std::abs;
  std::vector<Real> A{1.,4.,-5.,1.,0.,0.,0.,0.};
  std::vector<Real> B{-1.,1.,2.,3.,0.,0.,0.,0.};
  std::vector<Real> C{-1,-3,11,5,3,-13,3,0};

  std::vector<Real> result;
  Real diff;

  result = multiply_complex<Real,Complex>(A,B);
  diff = difference(result,C);
  if(abs(diff)>1e-3)
    throw std::runtime_error("wrong result");
}

int main()
{
  multiply<
    ::boost::multiprecision::float128,
    ::boost::multiprecision::complex128>();
  return 0;
}


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

Eigen::MatrixXd sigmoid(Eigen::MatrixXd mat){
    int r = mat.rows() ; int c = mat.cols();
    for (int i=0; i<r; i++){
        for (int j=0; j<c; j++){
            mat(i,j) = 1/(1+exp(-mat(i,j)));
        }
    }
    return mat;
};

class NeuralNetwork{
    public:
    std::vector <Eigen::MatrixXd> biases;
    std::vector <Eigen::MatrixXd> weights;
    int num_layers;
    std::vector <int> sizes;

    NeuralNetwork(std::vector <int> Sizes){
        num_layers = Sizes.size();
        sizes = Sizes;

        // Initializing random biases
        // std::cout<< "Initializing bases"<< std::endl;
        for (int i=1; i<sizes.size(); i++){
            int y = sizes[i];
            auto m = Eigen::MatrixXd::Random(y,1);
            biases.push_back(m);
            // std::cout<< "Shape : "<<y << " "<< 1 << std::endl;
        }

        // Initializing random weights
        // std::cout<< "Initializing weights"<< std::endl;
        for (int i=0; i<sizes.size()-1; i++){
            int x = sizes[i] ; int y = sizes[i+1];
            auto m = Eigen::MatrixXd::Random(y,x);
            weights.push_back(m);
            // std::cout<< "Shape : "<<y<< " "<<x<< std::endl;
        }
    };

    Eigen::MatrixXd predict(Eigen::MatrixXd input){
        for (int i=0; i<biases.size(); i++){
            input = sigmoid((weights[i]*input) + biases[i]);
        }

        return input;
    };

};


#pragma once

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

namespace cvgl {

inline double clip(double v, double min, double max)
{
    return (v < min ? min : (v > max ? max : v));
}

inline Eigen::ArrayXd clip(const Eigen::ArrayXd& v, double min, double max)
{
    return v.unaryExpr([&](double x){return clip(x, min, max); });
}

inline double wrap(double x, double x_min, double x_max)
{
    return fmod( fmod((x - x_min), (x_max - x_min)) + (x_max - x_min), (x_max - x_min)) + x_min;
}

inline double scale(double v, double in_min, double in_max, double out_min, double out_max)
{
    const double in_range = in_max - in_min;
    return (( (v - in_min) / (in_range == 0 ? 1 : in_range) ) * (out_max - out_min)) + out_min;
}

inline Eigen::ArrayXd scale(const Eigen::ArrayXd& v, double in_min, double in_max, double out_min, double out_max)
{
    return v.unaryExpr([&](double x){ return scale(x, in_min, in_max, out_min, out_max); });
}

template <typename T>
inline std::vector<T> scale(const std::vector<T>& v, const T in_min, const T in_max, const T out_min, const T out_max)
{
    T in_range = in_max - in_min;
    T out_range = out_max - out_min;
    std::vector<T> ret( v.size() );
    for( auto& q : v )
    {
        ret.emplace_back( ( ((q - in_min) / in_range) * out_range) + out_min );
    }
    return ret;
}

inline double scale_clip(double v, double in_min, double in_max, double out_min, double out_max)
{
    double clip_min = out_min > out_max ? out_max : out_min;
    double clip_max = out_max < out_min ? out_min : out_max;

    return clip( scale( v, in_min, in_max, out_min, out_max), clip_min, clip_max);
}

inline Eigen::ArrayXd scale_clip(const Eigen::ArrayXd& v, double in_min, double in_max, double out_min, double out_max)
{
    double clip_min = out_min > out_max ? out_max : out_min;
    double clip_max = out_max < out_min ? out_min : out_max;

    return clip( scale( v, in_min, in_max, out_min, out_max), clip_min, clip_max);
}

template <typename T>
inline std::vector<T> scale_clip(const std::vector<T>& v, const T in_min, const T in_max, const T out_min, const T out_max)
{
    double clip_min = out_min > out_max ? out_max : out_min;
    double clip_max = out_max < out_min ? out_min : out_max;

    T in_range = in_max - in_min;
    T out_range = out_max - out_min;
    std::vector<T> ret( v.size() );
    for( auto& q : v )
    {
        T val = ( ((q - in_min) / in_range) * out_range) + out_min;
        val = (val < clip_min ? clip_min : (val > clip_max ? clip_max : val));
        ret.emplace_back(val);
    }
    return ret;
}

inline double sum( std::vector<double> &vec )
{
    double _sum = 0;
    for (auto& n : vec){
        _sum += n;
    }
    return _sum;
}

inline int32_t round(double x)
{
    return int32_t(x + 0.5);
}

inline std::vector<double> dur2x( std::vector<double> & vec)
{
    std::vector<double> seq_x{0};
    for( size_t i = 0; i < vec.size()-1; ++i)
    {
        seq_x.emplace_back( seq_x.back() + vec[i] );
    }
    return seq_x;
}

inline Eigen::ArrayXd dur2x( Eigen::ArrayXd & vec)
{
    Eigen::ArrayXd seq_x( vec.size()+1 );
    seq_x(0) = 0;

    for( size_t i = 1; i < vec.size()+1; i++)
    {
        seq_x(i) = seq_x(i-1) + vec[i-1];
    }

    return seq_x;
}

inline Eigen::ArrayXd mtof( const Eigen::ArrayXd & v, double a4 = 440.)
{
    return a4 * pow(2., (v - 69.) / 12.) ;
}

inline double mtof( double v, double a4 = 440.)
{
    return a4 * pow(2., (v - 69.) / 12.) ;
}

inline double ftom( double v, double a4 = 440. )
{
    return 69.0 + (12.0 * log2( v / a4 ));
}

/*
inline Eigen::ArrayXd ftom( const Eigen::ArrayXd & v, double a4 = 440. )
{
    return 69 + (12 * log2( v / a4 ));
}*/

inline double erb( double center_hz, double ratio)
{
    const double half_bandwidth = (24.7 * ((0.00437 * center_hz) + 1)) * 0.5;
    return center_hz + (half_bandwidth * ratio);
}

inline double tanhScale( double x, double in_min, double in_max, double curveScalar = 1 )
{
    const double curveTanh = tanh(curveScalar);
    const double scaled_x = scale( x, in_min, in_max, -1., 1.);
    return scale( tanh( scaled_x * curveTanh ), -curveTanh, curveTanh, 0., 1. );
}

inline double scaleInterval( long step, std::vector<double> scale )
{
    if( step < 0 ) {
        return scale[ long( step % scale.size() ) ] + ( long( ((step+1) / (long)scale.size() ) - 1 ) * 12 );
    }
    else
        return scale[ long( step % scale.size() ) ] + long( (step / scale.size() ) * 12 );
}

inline double dbtoa(double db){ return pow(10., (db / 20.)); }

double ntom( const std::string & note );

double lineLookup( double t, std::vector<double> x, std::vector<double> y );

Eigen::ArrayXd lineLookup( const Eigen::ArrayXd& t, const std::vector<double>& x, const std::vector<double>& y );

template <typename T>
std::vector<T> aseq(T from, T to, T step = 1)
{
    std::vector<T> ret;
    T incr = from;
    while( incr <= to )
    {
        ret.emplace_back(incr);
        incr += step;
    }

    return ret;
}

inline double easeInOutQuad(double x)
{
    return x < 0.5 ? 2 * x * x : 1 - pow(-2 * x + 2, 2) / 2;
}

inline double easeInOutSine(double x)
{
    return -(cos(M_PI * x) - 1) * 0.5;
}

inline double easeInSine(double x)
{
     return 1 - cos((x * M_PI) * 0.5);
}

inline double easeOutSine(double x)
{
     return sin((x * M_PI) * 0.5);
}

inline double easeInExpo(double x)
{
    return x == 0 ? 0 : pow(2, 10 * x - 10);
}

inline double easeOutExpo(double x)
{
    return x == 1 ? 1 : 1 - pow(2, -10 * x);

}

}


#pragma once

#include <Eigen/Dense>
#include <cstddef>

namespace icarus
{
    template<typename T, size_t N>
    struct VarianceEstimator
    {
        VarianceEstimator(size_t sampleSize) :
            mSampleSize(sampleSize),
            mSampleNumber(0)
        {
            mMean.setZero();
            mMoment.setZero();
        }

        void addSample(Eigen::Matrix<T, N, 1> const & sample)
        {
            ++mSampleNumber;
            auto oldMean = mMean;
            mMean += (sample - mMean) / T(mSampleNumber);
            mMoment += (sample - oldMean).cwiseProduct(sample - mMean);
        }

        Eigen::Matrix<T, N, 1> mean() const
        {
            return mMean;
        }

        Eigen::Matrix<T, N, 1> variance() const
        {
            return mMoment / mSampleSize;
        }
    private:
        size_t mSampleSize;
        size_t mSampleNumber;
        Eigen::Matrix<T, N, 1> mMean;
        Eigen::Matrix<T, N, 1> mMoment;
    };
}


#include <iostream>
#include <string>
#include <vector>
#include <iomanip>
#include <boost/algorithm/string/erase.hpp>
#include <boost/algorithm/string/case_conv.hpp>

inline void ignoreExtraInputs() {
	std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}

inline std::string decimalToBinary(int decInput) {

	int exponent = 0;
	int previousExponent = 0;
	std::string binaryNum = "";

	while (pow(2, exponent) <= decInput) {
		previousExponent = exponent;
		exponent++;
	}
	while (previousExponent >= 0) {
		if (decInput - pow(2, previousExponent) >= 0) {
			decInput -= (int) pow(2, previousExponent);
			binaryNum.push_back('1');
		}
		else {
			binaryNum.push_back('0');
		}
		previousExponent--;
	}
	return binaryNum;
}

/*inline std::vector<std::string> decimalToBinaryReturningArray(int decInput) {

	int exponent = 0;
	int previousExponent = 0;
	std::vector<std::string> binaryNum = {};

	while (pow(2, exponent) <= decInput) {
		previousExponent = exponent;
		exponent++;
	}
	while (previousExponent >= 0) {
		if (decInput - pow(2, previousExponent) >= 0) {
			decInput -= (int)pow(2, previousExponent);
			binaryNum.push_back("1");
		}
		else {
			binaryNum.push_back("0");
		}
		previousExponent--;
	}
	return binaryNum;
}*/

inline std::string nibbleGuaranteeer(std::string sub_h_binInput)
{
	int binaryLength = sub_h_binInput.length();
	std::string newBinarySequ = "";

	if (binaryLength % 4 != 0) {
		for (unsigned int i = 0; i < (unsigned)(4 - (binaryLength % 4)); i++) {
			newBinarySequ.push_back('0');
		}
		binaryLength += (4 - (binaryLength % 4));
		newBinarySequ += sub_h_binInput;
	}
	else {
		newBinarySequ = sub_h_binInput;
	}
	return newBinarySequ;
}

/*inline std::vector<std::string> nibbleGuaranteeer(std::vector<std::string> sub_h_binInput, int requiredSize)
{
	int binaryLength = requiredSize;
	std::vector<std::string> newBinarySequ = {};

	if (binaryLength % 4 != 0) {
		for (unsigned int i = 0; i < (unsigned)(4 - (binaryLength % 4)); i++) {
			newBinarySequ.push_back("0");
		}
		binaryLength += (4 - (binaryLength % 4));
		//newBinarySequ += sub_h_binInput;
		for (unsigned int i = 0; i < sub_h_binInput.size(); i++) {
			newBinarySequ.push_back(sub_h_binInput[i]);
		}
	}
	else {
		newBinarySequ = sub_h_binInput;
	}
	return newBinarySequ;
}*/

inline std::string nibbleSpaceAparter(std::string sub_binInput) {

	int indexModifier = 4;
	int whiteSpaceAddedModifier = 0;

	for (unsigned int i = 0; i < (unsigned)((sub_binInput.length() / 4) - 1); i++) {
		sub_binInput.insert(indexModifier + whiteSpaceAddedModifier, " ");
		indexModifier += 4;
		whiteSpaceAddedModifier++;
	}
	return sub_binInput;
}

inline std::string binaryToHexa(std::string h_binInput) {

	int binaryLength = 0;
	int nibbleIndexBeginModifier = 0;
	int nibbleIndexEndModifier = 4;
	int nibbleCalculatedDecimalValue = 0;
	int nibbleHighestExponentValue = 8;
	std::string hexaValue = "";
	std::vector<std::string> nibbles;

	h_binInput = nibbleGuaranteeer(h_binInput);
	binaryLength = h_binInput.length() / 4;

	for (unsigned int i = 0; i < (unsigned) binaryLength; i++) {
		nibbles.push_back(h_binInput.substr(nibbleIndexBeginModifier, nibbleIndexEndModifier));
		nibbleIndexBeginModifier += 4;
		nibbleIndexEndModifier += 4;
	}

	for (std::string &nibbleValue : nibbles) {
		nibbleCalculatedDecimalValue = 0;
		nibbleHighestExponentValue = 8;
		for (unsigned int i = 0; i < 4; i++) {
			if (nibbleValue[i] == '1') {
				nibbleCalculatedDecimalValue += nibbleHighestExponentValue;
			}
			nibbleHighestExponentValue = (int) ((nibbleHighestExponentValue / 2) + 0.5);
		}
		if (nibbleCalculatedDecimalValue >= 0 && nibbleCalculatedDecimalValue <= 9) {
			hexaValue.push_back(nibbleCalculatedDecimalValue + 48);
		}
		else if (nibbleCalculatedDecimalValue >= 10 && nibbleCalculatedDecimalValue <= 15) {
			hexaValue.push_back(nibbleCalculatedDecimalValue + 55);
		}
	}
	return hexaValue;
}

inline int binaryToDecimal(std::string d_binInput) {

	int nibbleHighestExponentValue = (int) pow(2, d_binInput.length() - 1);
	int nibbleCalculatedDecimalValue = 0;

	for (char &indexValue : d_binInput) {
		if (indexValue == '1') {
			nibbleCalculatedDecimalValue += nibbleHighestExponentValue;
		}
		nibbleHighestExponentValue = (int) ((nibbleHighestExponentValue / 2) + 0.5);
	}
	return nibbleCalculatedDecimalValue;
}

inline int hexaToDecimal(std::string hexaInput) {

	int hexaHighestExponentValue = (int) pow(16, hexaInput.length() - 1);
	int hexaToDecimalValue = 0;

	for (char &indexValue : hexaInput) {
		if (indexValue >= '0' && indexValue <= '9') {
			hexaToDecimalValue += (indexValue - 48) * hexaHighestExponentValue;
		}
		else if (indexValue >= 'A' && indexValue <= 'F') {
			hexaToDecimalValue += (indexValue - 55) * hexaHighestExponentValue;
		}
		else {
			std::cout << "\nSomething went wrong in the 'hexaToDecimal' function\n";
		}
		hexaHighestExponentValue = hexaHighestExponentValue / 16;
	}
	return hexaToDecimalValue;
}

/*inline std::string addition(std::string binaryInputOne, std::string binaryInputTwo) {

	std::vector<std::string> indexedBinaryInput_One = {}, indexedBinaryInput_Two = {};
	std::string resultantBinaryValue = "", remainder = "0";
	int binaryLength = -1;

	indexedBinaryInput_One = decimalToBinaryReturningArray(binaryToDecimal(binaryInputOne));
	indexedBinaryInput_Two = decimalToBinaryReturningArray(binaryToDecimal(binaryInputTwo));

	if (indexedBinaryInput_One.size() > indexedBinaryInput_Two.size()) {
		binaryLength = indexedBinaryInput_One.size();
	}
	else {
		binaryLength = indexedBinaryInput_Two.size();
	}
	indexedBinaryInput_One = nibbleGuaranteeer(indexedBinaryInput_One, binaryLength);
	indexedBinaryInput_Two = nibbleGuaranteeer(indexedBinaryInput_Two, binaryLength);

	for (int i = (binaryLength-1); i >= 0; i--) {
		if (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "0" && remainder == "0") {
			resultantBinaryValue.push_back('0');
			remainder = "0";
		}
		else if ((indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "0" && remainder == "0") ||
				 (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "1" && remainder == "0") ||
				 (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "0" && remainder == "1")) {
			resultantBinaryValue.push_back('1');
			remainder = "0";
		}
		else if ((indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "1" && remainder == "0") ||
				 (indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "0" && remainder == "1") ||
				 (indexedBinaryInput_One[i] == "0" && indexedBinaryInput_Two[i] == "1" && remainder == "1")) {
			resultantBinaryValue.push_back('0');
			remainder = "1";
		}
		else if (indexedBinaryInput_One[i] == "1" && indexedBinaryInput_Two[i] == "1" && remainder == "1") {
			resultantBinaryValue.push_back('1');
			remainder = "1";
		}
		else {
			std::cout << "\nThere happens to be something else occurring in the forloop in 'binaryAddition'\n";
		}
	}

	return resultantBinaryValue;
}*/


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef __TEST_FUNCTIONS_HPP__
#define __TEST_FUNCTIONS_HPP__

#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include "bayesopt/bayesopt.hpp"
#include "specialtypes.hpp"

class ExampleOneD: public bayesopt::ContinuousModel
{
public:
  ExampleOneD(bayesopt::Parameters par):
    ContinuousModel(1,par) {}

  double evaluateSample(const vectord& xin)
  {
    if (xin.size() != 1)
      {
	std::cout << "WARNING: This only works for 1D inputs." << std::endl
		  << "WARNING: Using only first component." << std::endl;
      }

    double x = xin(0);
    return (x-0.3)*(x-0.3) + sin(20*x)*0.2;
  };

  bool checkReachability(const vectord &query)
  {return true;};

  void printOptimal()
  {
    std::cout << "Optimal:" << 0.23719 << std::endl;
  }

};

class BraninNormalized: public bayesopt::ContinuousModel
{
public:
  BraninNormalized(bayesopt::Parameters par):
    ContinuousModel(2,par) {}

  double evaluateSample( const vectord& xin)
  {
     if (xin.size() != 2)
      {
	std::cout << "WARNING: This only works for 2D inputs." << std::endl
		  << "WARNING: Using only first two components." << std::endl;
      }

    double x = xin(0) * 15 - 5;
    double y = xin(1) * 15;

    return branin(x,y);
  }

  double branin(double x, double y)
  {
    const double pi = boost::math::constants::pi<double>();
    const double rpi = pi*pi;
    return sqr(y-(5.1/(4*rpi))*sqr(x)
	       +5*x/pi-6)+10*(1-1/(8*pi))*cos(x)+10;
  };

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(2);
    sv(0) = 0.1238938; sv(1) = 0.818333;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = 0.5427728; sv(1) = 0.151667;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = 0.961652; sv(1) = 0.1650;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

};

class ExampleCamelback: public bayesopt::ContinuousModel
{
public:
  ExampleCamelback(bayesopt::Parameters par):
    ContinuousModel(2,par) {}

  double evaluateSample( const vectord& x)
  {
     if (x.size() != 2)
      {
	std::cout << "WARNING: This only works for 2D inputs." << std::endl
		  << "WARNING: Using only first two components." << std::endl;
      }
     double x1_2 = x(0)*x(0);
     double x2_2 = x(1)*x(1);

     double tmp1 = (4 - 2.1 * x1_2 + (x1_2*x1_2)/3) * x1_2;
     double tmp2 = x(0)*x(1);
     double tmp3 = (-4 + 4 * x2_2) * x2_2;
     return tmp1 + tmp2 + tmp3;
  }

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(2);
    sv(0) = 0.0898; sv(1) = -0.7126;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = -0.0898; sv(1) = 0.7126;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

};

class ExampleHartmann6: public bayesopt::ContinuousModel
{
public:
  ExampleHartmann6(bayesopt::Parameters par):
    ContinuousModel(6,par), mA(4,6), mC(4), mP(4,6)
  {
    mA <<= 10.0,   3.0, 17.0,   3.5,  1.7,  8.0,
      0.05, 10.0, 17.0,   0.1,  8.0, 14.0,
      3.0,   3.5,  1.7,  10.0, 17.0,  8.0,
      17.0,   8.0,  0.05, 10.0,  0.1, 14.0;

    mC <<= 1.0, 1.2, 3.0, 3.2;

    mP <<= 0.1312, 0.1696, 0.5569, 0.0124, 0.8283, 0.5886,
      0.2329, 0.4135, 0.8307, 0.3736, 0.1004, 0.9991,
      0.2348, 0.1451, 0.3522, 0.2883, 0.3047, 0.6650,
      0.4047, 0.8828, 0.8732, 0.5743, 0.1091, 0.0381;
  }

  double evaluateSample( const vectord& xin)
  {
    double y = 0.0;
    for(size_t i=0; i<4; ++i)
      {
	double sum = 0.0;
	for(size_t j=0;j<6; ++j)
	  {
	    double val = xin(j)-mP(i,j);
	    sum -= mA(i,j)*val*val;
	  }
	y -= mC(i)*std::exp(sum);
      }
    return y;
  };

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(6);
    sv <<= 0.20169, 0.150011, 0.476874, 0.275332, 0.311652, 0.6573;

    std::cout << "Solution: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

private:
  matrixd mA;
  vectord mC;
  matrixd mP;
};

#endif


#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <boost/program_options.hpp>
#include <filesystem>
#include "CommonCurve.h" //std::left std::setw std::setfill

namespace fs = std::filesystem;
using namespace boost;
namespace po = boost::program_options;
// The vertex of the curve model, template parameters: optimization of variable dimensions and data types
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // Reset
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // Update
    {
        _estimate += Eigen::Vector3d(update);
    }
    // Save and read: leave blank
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
};

// Error model template parameters: observation dimension, type, connection vertex type
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // Calculate curve model error
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
public:
    double _x;  //x value, y value _measurement
};

int main( int argc, char** argv )
{

    std::string dataFile = "readDataG2O.txt";
    std::string parameter = "parametersG2O.txt";
    std::string path = "./result/";
    // create directory if not exist
    struct stat info;
    if( stat( path.c_str(), &info ) != 0 )fs::create_directories(path);
    std::string pathRealData = path + za::getCurrentTime() + dataFile;
    std::string pathParameter = path + za::getCurrentTime() + parameter;

    // True parameter value to be estimated
    double a, b, c;

    // Total number of data point
    int N, iterate ;

    // Noise Sigma value
    double w_sigma;
    // Cammand line parser
	try
		{
			po::options_description desc("Allowed options");
			desc.add_options()
				("help,h", "produce help message")
				("first,a", po::value<double>(),
					"first parameter")
                ("second,b", po::value<double>(),
					"second parameter")
				("third,c", po::value<double>(),
					"third parameter")
                ("number,n", po::value<int>(&N)->default_value(100)->implicit_value(500),
					"number of data")
                ("iteration,i", po::value<int>(&iterate)->default_value(100)->implicit_value(200),
					"number of iterations")
				("noise,s", po::value<double>(&w_sigma)->default_value(1)->implicit_value(5),
					"noise added");

			/* Point out that all unknown values should be converted to the value of the "input-file" option.
					* Also use the command_line_parser class instead of parse_command_line */
			po::variables_map vm;
			po::store(po::parse_command_line(argc, argv, desc), vm);
			po::notify(vm);

			if (vm.count("help"))
			{
				std::cout << "Usage: options_description [options]\n";
				std::cout << desc;
				return 0;
			}
 			if (vm.count("first") )
        	{

                a =  vm["first"].as<double>();
        	}
        	else
       		{
            	std::cout << "First parameter not set.\n";
				return -1;
        	}
 			if (vm.count("second") )
        	{

                b =  vm["second"].as<double>();
        	}
        	else
       		{
            	std::cout << "Second parameter not set.\n";
				return -1;
        	}
 			if (vm.count("third") )
        	{

                a =  vm["third"].as<double>();
        	}
        	else
       		{
            	std::cout << "Third parameter not set.\n";
				return -1;
        	}
        }// end of try

        catch(std::exception& e)
		{
			std::cout << e.what() << "\n";
			return 1;
		}

      // OpenCV random number generator
    cv::RNG rng;

    // abc Estimated value of the parameter
    double abc[3] = {0,0,0};

   // set of x and y data
    std::vector<double> x_data, y_data;
    std::ofstream myfile;
    const int nameWidth     = 6;
    const int numWidth      = 8;
    myfile.open (&pathRealData[0]);
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    za::printElement("|", 1, myfile);
    za::printElement("N", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("X", nameWidth, myfile);
    za::printElement("|", 1, myfile);
    za::printElement("Y", 10, myfile);
    za::printElement("|", 1, myfile);
    myfile <<"\n";
    for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
    myfile <<"\n";
    std::cout<<"Generating data: \n";

    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        za::printElement("|", 1, myfile);
        za::printElement(i+1, nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(x_data[i], nameWidth, myfile);
        za::printElement("|", 1, myfile);
        za::printElement(y_data[i], 10, myfile);
        za::printElement("|", 1, myfile);
        myfile <<"\n";
        for(int i = 0; i < 25; i++) za::printElement("-", 1, myfile);
        myfile <<"\n";
    }
     myfile.close();
    // Build graph optimization, first set g2o
    // The dimension of the optimized variable for each error term is 3, and the dimension of the error value is 1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
    //zouma
    // Linear equation solver
    std::unique_ptr<Block::LinearSolverType> linearSolver
				(new g2o::LinearSolverDense<Block::PoseMatrixType>());

    // Matrix block solver
    std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver)));

    // Gradient descent method, choose from GN, LM, DogLeg
    g2o::OptimizationAlgorithmGaussNewton * solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));
    //zouma

    g2o::SparseOptimizer optimizer;     // Graph model
    optimizer.setAlgorithm( solver );   // Set up the solver
    optimizer.setVerbose( true );       // Turn on debug output

    // Add vertices to the graph
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

    // Add edges to the graph
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // Set the connected vertices
        edge->setMeasurement( y_data[i] );      // Observed value
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrix: the inverse of the covariance matrix
        optimizer.addEdge( edge );
    }

    //Perform optimization
    std::cout<<"start optimization\n";
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(iterate);
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>( t2-t1 );
    std::cout<<"solve time cost = "<<time_used.count()<<" seconds. \n";

    // Output optimized value
    Eigen::Vector3d abc_estimate = v->estimate();
    std::ofstream mySol;
    mySol.open (&pathParameter[0]);
    mySol<<"estimated model a, b, c: "<<abc_estimate.transpose()<<"\n";
    mySol.close();
    return 0;
}


//
//  strassen.cpp
//  Jacobian
//
//  Created by David Freifeld
//  Copyright © 2020 David Freifeld. All rights reserved.
//
//  Description:
//  Experimental benchmarking of Strassen's Algorithm vs plain Eigen matrix multiply.
//  While naive multiplication is O(n^3), Strassen's Algorithm for matrix multiply is O(n^2.8074)
//  which means significant differences will begin to manifest for large matrices. All faster algorithms are galactic.
//

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

// Not remotely extensible, just initial test.
Eigen::MatrixXf strassen_mul(Eigen::MatrixXf x, Eigen::MatrixXf y)
{
  int power = 0;
  int largest;
  if (x.rows() >= x.cols() || x.rows() >= y.cols() || x.rows() >= y.rows()) largest = x.rows();
  else if (x.cols() >= x.rows() || x.cols() >= y.cols() || x.cols() >= y.rows()) largest = x.cols();
  else if (y.cols() >= x.rows() || y.cols() >= x.cols() || y.cols() >= y.rows()) largest = y.cols();
  else if (y.rows() >= x.rows() || y.rows() >= x.cols() || y.rows() >= y.cols()) largest = y.rows();
  for (; pow(2, power) < largest; power++) {
    std::cout << pow(2, power) << " " << power <<"\n";
  }
  std::cout << "POWER: " << power << " LARGEST: " << largest << "\n";
  Eigen::MatrixXf a ((int)pow(2,power), (int)pow(2,power));
  Eigen::MatrixXf b ((int)pow(2,power), (int)pow(2,power));
  a.block(0,0,x.rows(), x.cols()) = x;
  b.block(0,0,y.rows(), y.cols()) = y;
  std::cout << "\nINIT\n" << a << "\n\n" << b << "\n\n\n";
  std::cout << "\nEIGEN_VER\n" << a*b << "\n\n\n";
  //Eigen::MatrixXf a ()
  int block_len = largest/2;
  Eigen::MatrixXf result ((int)pow(2,power), (int)pow(2,power));

  Eigen::MatrixXf m1 = ((a.block(0,0, block_len, block_len)) + a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len)) * (b.block(0,0, block_len, block_len) + b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m2 = (a.block(a.rows()-block_len, 0, block_len, block_len) + a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len)) * (b.block(0,0, block_len, block_len));
  Eigen::MatrixXf m3 = a.block(0,0, block_len, block_len) * (b.block(0,b.cols()-block_len, block_len, block_len) - b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m4 = a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len) * (b.block(b.rows()-block_len,0, block_len, block_len) - b.block(0,0, block_len, block_len));
  Eigen::MatrixXf m5 = (a.block(0, 0, block_len, block_len) + a.block(0,a.cols()-block_len, block_len, block_len)) * (b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m6 = (a.block(a.rows()-block_len,0, block_len, block_len) - a.block(0,0, block_len, block_len)) * (b.block(0,0, block_len, block_len) + b.block(0,b.cols()-block_len, block_len, block_len));
  Eigen::MatrixXf m7 = (a.block(0,a.cols()-block_len, block_len, block_len) - a.block(a.rows()-block_len,a.cols()-block_len, block_len, block_len)) * (b.block(a.rows()-block_len,0, block_len, block_len) + b.block(b.rows()-block_len,b.cols()-block_len, block_len, block_len));

  //  std::cout << m1 + m4 - m5 + m7 << "\n\n" << m3+m5 << "\n\n" << m2+m4 << "\n\n" << m1-m2+m3+m6;

  result.block(0,0, block_len, block_len) =  m1 + m4 - m5 + m7;
  result.block(0,result.cols()-block_len, block_len, block_len) =  m3 + m5;
  result.block(result.rows()-block_len,0, block_len, block_len) =  m2 + m4;
  result.block(result.rows()-block_len,result.cols()-block_len, block_len, block_len) =  m1 -m2 + m3 + m6;
  std::cout << "\nSTRASSEN_VER\n" << result << "\n\n\n";
  return result;
}

int main()
{
  srand((unsigned int) time(0));
  int sz;
  std::cin >> sz;
  Eigen::MatrixXf a = Eigen::MatrixXf::Random(sz, 3);
  Eigen::MatrixXf b = Eigen::MatrixXf::Random(3, sz);

  auto eigen_begin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf product = a * b;
  auto eigen_end = std::chrono::high_resolution_clock::now();

  auto strassen_begin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf sproduct = strassen_mul(a, b);
  auto strassen_end = std::chrono::high_resolution_clock::now();

  /* std::cout << "EIGEN: " << std::chrono::duration_cast<std::chrono::nanoseconds>(eigen_end - eigen_begin).count() / pow(10,9) << " STRASSEN: " << std::chrono::duration_cast<std::chrono::nanoseconds>(strassen_end - strassen_begin).count() / pow(10,9) << "\n"; */
  /* std::cout << "A:\n" << a << "\nB:\n" << b << "\nEigen:\n" << product << "\nStrassen:\n" << sproduct << "\n"; */
}


#pragma once

#include <Eigen/Core>
using namespace Eigen;

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

namespace samson::so3
{
    auto so3(const Vector3dual &v) -> Matrix3dual
    {
        Matrix3dual X;
        X << 0.0, -v(2), v(1), v(2), 0.0, -v(0), -v(1), v(0), 0.0;
        return X;
    }

    auto so3(const Matrix3dual &X) -> Vector3dual
    {
        Vector3dual v;
        v << X(2, 1), X(0, 2), X(1, 0);
        return v;
    }

    auto to_axis_angle(const Vector3dual &v) -> std::pair<Vector3dual, dual>
    {
        dual theta = v.norm();
        Vector3dual k = v.normalized();
        return std::make_pair(k, theta);
    }

    auto exp(const Vector3dual &v) -> Matrix3dual
    {
        auto [k, th] = to_axis_angle(v);
        // std::cout << (th) << std::endl;;
        Matrix3dual K = so3(k);
        return Matrix3dual::Identity() + sin(th) * K + (1.0 - cos(th)) * K * K;
    }

    auto exp(const Matrix3dual &X) -> Matrix3dual
    {
        Vector3dual v = so3(X);
        return exp(v);
    }

    auto from_axis_angle(const Vector3dual &axis, dual angle)
    {
        Vector3dual x = axis * angle;
        Matrix3dual X = so3(x);
        return exp(X);
    }

} // namespace samson::so3


#include "SlamShuffler.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

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

namespace AdventOfCode
{
namespace Year2019
{
namespace Day22
{

SlamShuffler::SlamShuffler(std::vector<ShuffleInstruction> instructions, BigNumber deckSize, BigNumber numIterations)
    : m_instructions{std::move(instructions)}
    , m_deckSize{deckSize}
    , m_numIterations{numIterations}
{

}

BigNumber SlamShuffler::getCardAtPosition(BigNumber position)
{
    BigNumber iterOneResult = getWhereCardComesFrom(position);
    BigNumber iterTwoResult = getWhereCardComesFrom(iterOneResult);

    // Subtract the following equations:
    // slope * position + offset = iterOneResult
    // slope * iterOneResult + offset = iterTwoResult
    // -> slope = (iterOneResult - iterTwoResult) / (position - iterOneResult)
    BigNumber slope = (iterOneResult - iterTwoResult) * boost::integer::mod_inverse(position - iterOneResult + m_deckSize, m_deckSize) % m_deckSize;
    BigNumber offset = (iterOneResult - slope * position + m_deckSize * position * slope) % m_deckSize;

    // For 2 iterations:
    // result = slope * (slope * position + offset) + offset
    // For m_numIterations iterations:
    // (slope^m_numIterations) * position + (slope^(m_numIterations - 1))) * offset + (slope^(m_numIterations - 2))) * offset + ... + offset
    // (slope^m_numIterations) * position + (slope^m_numIterations - 1) / (slope - 1) * B
    BigNumber slopeToTheNumberOfIterations = boost::multiprecision::powm(slope, m_numIterations, m_deckSize);
    BigNumber firstTerm = slopeToTheNumberOfIterations * position;
    BigNumber secondTerm = (slopeToTheNumberOfIterations - 1) * boost::integer::mod_inverse(slope - 1 + m_deckSize, m_deckSize) * offset;

    return (firstTerm + secondTerm) % m_deckSize;
}

std::vector<unsigned> SlamShuffler::getAllCardsAfterSingleIteration()
{
    std::vector<unsigned> allCards;

    for (int i = 0; i < m_deckSize; ++i)
    {
        auto cardAtPosition = getWhereCardComesFrom(i);
        allCards.push_back(unsigned{cardAtPosition});
    }

    return allCards;
}

BigNumber SlamShuffler::getWhereCardComesFrom(BigNumber position) const
{
    for (auto instructionIter = m_instructions.crbegin(); instructionIter != m_instructions.crend(); ++instructionIter)
    {
        position = executeInReverse(*instructionIter, position);
    }

    return position;
}

BigNumber SlamShuffler::executeInReverse(const ShuffleInstruction& instruction, BigNumber position) const
{
    if (instruction.type == DEAL_INTO_NEW_STACK)
    {
        return m_deckSize - 1 - position;
    }
    else if (instruction.type == CUT)
    {
        return (position + instruction.arg + m_deckSize) % m_deckSize;
    }
    else
    {
        BigNumber inverse = boost::integer::mod_inverse(BigNumber{instruction.arg}, m_deckSize);
        return inverse * position % m_deckSize;
    }
}

}
}
}


//Link to Boost
 #define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
#include <boost/test/unit_test.hpp>

#include <cmath>
#include "test.h"
#include "../common.h"
#include "../gegenbauer_polynomial.hpp"

BOOST_DATA_TEST_CASE(GegenbauerDAlphaAt0_test, bdata::xrange(0, 20) ^ bdata::random(-10.0, 10.0), n, x)
{
    float_type actual = GegenbauerDAlphaAt0(n, x);

    float_type arr[1] = {x};
    float_type *v1 = gegenbauer_polynomial_value(n, 1, -inc, arr);
    float_type *v2 = gegenbauer_polynomial_value(n, 1, inc, arr);
    float_type ret1 = v1[n + 0 * (n + 1)];
    float_type ret2 = v2[n + 0 * (n + 1)];
    delete v1;
    delete v2;

    float_type expect = (ret2 - ret1) / (2 * inc);
    MY_FLOAT_EQUAL(actual, expect, tol);
}


#include <stan/math/prim/scal.hpp>
#include <stan/math/prim/arr/fun/log_sum_exp.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

void test_log_sum_exp(double a, double b) {
  using stan::math::log_sum_exp;
  using std::exp;
  using std::log;
  EXPECT_FLOAT_EQ(log(exp(a) + exp(b)), log_sum_exp(a, b));
}

void test_log_sum_exp(const std::vector<double>& as) {
  using stan::math::log_sum_exp;
  using std::exp;
  using std::log;
  double sum_exp = 0.0;
  for (size_t n = 0; n < as.size(); ++n)
    sum_exp += exp(as[n]);
  EXPECT_FLOAT_EQ(log(sum_exp), log_sum_exp(as));
}

TEST(MathFunctions, log_sum_exp) {
  using stan::math::log_sum_exp;
  std::vector<double> as;
  test_log_sum_exp(as);
  as.push_back(0.0);
  test_log_sum_exp(as);
  as.push_back(1.0);
  test_log_sum_exp(as);
  as.push_back(-1.0);
  test_log_sum_exp(as);
  as.push_back(-10000.0);
  test_log_sum_exp(as);

  as.push_back(10000.0);
  EXPECT_FLOAT_EQ(10000.0, log_sum_exp(as));
}

TEST(MathFunctions, log_sum_exp_2) {
  using stan::math::log_sum_exp;
  test_log_sum_exp(1.0, 2.0);
  test_log_sum_exp(1.0, 1.0);
  test_log_sum_exp(3.0, 2.0);
  test_log_sum_exp(-20.0, 12);
  test_log_sum_exp(-20.0, 12);

  // exp(10000.0) overflows
  EXPECT_FLOAT_EQ(10000.0, log_sum_exp(10000.0, 0.0));
  EXPECT_FLOAT_EQ(0.0, log_sum_exp(-10000.0, 0.0));
}

TEST(MathFunctions, log_sum_exp_2_inf) {
  using stan::math::log_sum_exp;
  double inf = std::numeric_limits<double>::infinity();
  test_log_sum_exp(1.0, -inf);
  test_log_sum_exp(-inf, 3.0);
  test_log_sum_exp(-inf, -inf);
  EXPECT_FLOAT_EQ(inf, log_sum_exp(inf, 3.0));
  EXPECT_FLOAT_EQ(inf, log_sum_exp(inf, inf));
}

TEST(MathFunctions, log_sum_exp_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log_sum_exp(1.0, nan));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log_sum_exp(nan, 1.0));

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log_sum_exp(nan, nan));
}


#include "gtest/gtest.h"

#include <math.h>

#include <Eigen/Core>

#include "pICP/utils.h"

namespace
{

TEST(utils, translate_2d)
{
    Eigen::Matrix< float, 2, 5 > coordinates;
    coordinates << 1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.;

    Eigen::Matrix< float, 2, 2 > rotation;
    rotation.setIdentity();

    Eigen::Matrix< float, 2, 1 > translation;
    translation << 10., 10;

    Eigen::Matrix< float, 2, 5 > translatedCoordinates = pICP::transformCoordinatesMatrix( coordinates, rotation, translation );

    Eigen::Matrix< float, 2, 5 > expectedCoordinates;
    expectedCoordinates << 11., 12., 13., 14., 15.,
                           11., 12., 13., 14., 15.;

    EXPECT_TRUE( translatedCoordinates == expectedCoordinates );
}

TEST(utils, rotate_2d)
{
    Eigen::Matrix< float, 2, 5 > coordinates;
    coordinates <<  1.,  2.,  3.,  4.,  5.,
                   11., 22., 33., 44., 55.;

    Eigen::Matrix< float, 2, 2 > rotation;
    rotation << std::cos( M_PI/2. ), -std::sin( M_PI/2. ),
                std::sin( M_PI/2.),   std::cos( M_PI/2. );

    Eigen::Matrix< float, 2, 1 > translation;
    translation << 0., 0;

    Eigen::Matrix< float, 2, 5 > translatedCoordinates = pICP::transformCoordinatesMatrix( coordinates, rotation, translation );

    Eigen::Matrix< float, 2, 5 > expectedCoordinates;
    expectedCoordinates << -11., -22., -33., -44., -55.,
                             1.,   2.,   3.,   4.,   5.;

    EXPECT_TRUE( translatedCoordinates == expectedCoordinates );
}

TEST(utils, transform_3d)
{
    Eigen::Matrix< float, 3, 5 > coordinates;
    coordinates << 1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.;

    Eigen::Matrix< float, 3, 3 > rotation;
    rotation << 1.,                  0.,                   0.,
                0., std::cos( M_PI/2. ), -std::sin( M_PI/2. ),
                0.,  std::sin( M_PI/2.),  std::cos( M_PI/2. );

    Eigen::Matrix< float, 3, 1 > translation;
    translation << 1., 2., 3;

    Eigen::Matrix< float, 3, 5 > translatedCoordinates = pICP::transformCoordinatesMatrix( coordinates, rotation, translation );

    Eigen::Matrix< float, 3, 5 > expectedCoordinates;
    expectedCoordinates <<  2., 3.,  4.,  5.,  6.,
                            1., 0., -1., -2., -3.,
                            4., 5.,  6.,  7.,  8.;

    EXPECT_TRUE( translatedCoordinates == expectedCoordinates );
}

TEST(utils, coordinatesDifferenceNorm_equality)
{
    Eigen::Matrix< float, 3, 5 > coordinates;
    coordinates << 1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.;

    Eigen::Matrix< float, 3, 5 > sameCoordinates = coordinates;

    EXPECT_EQ(pICP::coordinatesDifferenceNorm( coordinates, sameCoordinates ), 0. );
}

TEST(utils, coordinatesDifferenceNorm_inequality)
{
    Eigen::Matrix< float, 3, 5 > coordinates;
    coordinates << 1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.,
                   1., 2., 3., 4., 5.;

    Eigen::Matrix< float, 3, 5 > differentCoordinates = coordinates;
    //differentCoordinates( 1, 3 ) += 2.;
    differentCoordinates( 2, 4 ) += 3.;

    EXPECT_EQ(pICP::coordinatesDifferenceNorm( coordinates, differentCoordinates ), 3. );
}

}  // namespace


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

MatrixXcd bl_bicggr(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
// Block BiCGGR [Tadano etal 2009 JSIAM letters]
  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)
  MatrixXcd R= B-A*X;
  MatrixXcd P= R;
  MatrixXcd V= A*R;
  MatrixXcd W= V;
  MatrixXcd R0til= R; //MatrixXcd::Random(n,L);
  MatrixXcd R0til_H= R0til.adjoint();
  for(int k= 0; k < itermax; ++k){
    MatrixXcd alfa= (R0til_H*V).fullPivLu().solve(R0til_H*R);
    complex<double> qsi= (W.adjoint()*R).trace()/(W.adjoint()*W).trace();
    MatrixXcd S= P-qsi*V;
    MatrixXcd U= S*alfa;
    MatrixXcd Y= A*U;
    X= X+qsi*R+U;
    MatrixXcd Rnew= R-qsi*W-Y;
    double err= Rnew.norm()/Bnorm;
    cout << "bl_bicggr: " << "iter= " << k << " relative err= " << err << endl;
    if(err < tol) break;
    W= A*Rnew;
    MatrixXcd gamma = (R0til_H*R).fullPivLu().solve(R0til_H*Rnew/qsi);
    R=Rnew;
    P= R+U*gamma;
    V= W+Y*gamma;

  }
  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_bicggr did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include "rmsd_align.hpp"
#include <Eigen/Dense>

#include <iostream> // delete me

namespace timemachine {

/*
Optimally align x2 onto x1. In particular, x2 is shifted so that its centroid is placed
at the same position as the of x1's centroid. x2 is also rotated so that the RMSD
is minimized.
*/
void rmsd_align_cpu(const int N, const double *x1_raw, const double *x2_raw, double *x2_aligned_raw) {

    Eigen::MatrixXd x1(N, 3);
    Eigen::MatrixXd x2(N, 3);

    for (int i = 0; i < N; i++) {
        x1(i, 0) = x1_raw[i * 3 + 0];
        x1(i, 1) = x1_raw[i * 3 + 1];
        x1(i, 2) = x1_raw[i * 3 + 2];

        x2(i, 0) = x2_raw[i * 3 + 0];
        x2(i, 1) = x2_raw[i * 3 + 1];
        x2(i, 2) = x2_raw[i * 3 + 2];
    }

    Eigen::Vector3d x1_centroid = x1.colwise().mean();
    Eigen::Vector3d x2_centroid = x2.colwise().mean();
    Eigen::Vector3d translation = x2_centroid - x1_centroid;

    // shift to the center
    Eigen::MatrixXd x1_centered = x1.rowwise() - x1_centroid.transpose();
    Eigen::MatrixXd x2_centered = x2.rowwise() - x2_centroid.transpose();

    // compute correlations
    Eigen::MatrixXd c = x2_centered.transpose() * x1_centered;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(c, Eigen::ComputeFullU | Eigen::ComputeFullV);
    auto s = svd.singularValues();

    Eigen::MatrixXd u = svd.matrixU();
    Eigen::MatrixXd v = svd.matrixV();
    Eigen::MatrixXd v_t = v.transpose();

    bool is_reflection = u.determinant() * v_t.determinant() < 0.0;
    if (is_reflection) {
        for (int i = 0; i < 3; i++) {
            u(i, 2) = -u(i, 2);
        }
    }

    Eigen::MatrixXd rotation = u * v_t;

    // x2 is centered
    Eigen::MatrixXd x2_rot = x2_centered * rotation;
    Eigen::MatrixXd x2_aligned = x2_rot.rowwise() - (translation.transpose() - x2_centroid.transpose());

    for (int i = 0; i < N; i++) {
        x2_aligned_raw[i * 3 + 0] = x2_aligned(i, 0);
        x2_aligned_raw[i * 3 + 1] = x2_aligned(i, 1);
        x2_aligned_raw[i * 3 + 2] = x2_aligned(i, 2);
    }
}

} // namespace timemachine


#include <iostream>
#include <Eigen/Dense>
#include "../simple_lib/include/simple_layer.h"

using namespace Eigen;

int main(){
    using std::cout;
    using std::endl;
    using namespace MyDL;

    int batch_size = 2;
    int num_category = 3;
    MatrixXd X = MatrixXd::Zero(batch_size, num_category);
    MatrixXd t = MatrixXd::Zero(batch_size, num_category);
    double loss;
    MatrixXd dX;

    // X << 0.2, 0.3, 0.5,
    //      0.1, 0.7, 0.2;
    X << 1, 2, 3,
         1, 0, 0;
    t << 0, 0, 1,
         1, 0, 0;

    SoftmaxWithLoss loss_layer;

    loss = loss_layer.forward(X, t);
    cout << "loss: " << loss << endl;

    dX = loss_layer.backward();
    cout << "gradient: " << endl;
    cout << dX << endl;

    return 0;
}


//EuropeanOption.cpp

//Modification date: 6/17/15

#include "EuropeanOption.hpp"
#include "GlobalFunctions.hpp"
#include <cmath>
#include <iostream>
#include <boost/math/distributions/normal.hpp>
using namespace std;

//Operator Overloads
EuropeanOption& EuropeanOption::operator = (const EuropeanOption& source)
{
	if (this == &source)							//check if current object and argument object are same object
		return *this;

	Option::operator =(source);

	T = source.T;

	return *this;
}

EuropeanOption& EuropeanCallOption::operator = (const EuropeanOption& source)
{
	if (this == &source)							//check if current object and argument object are same object
		return *this;

	EuropeanOption::operator=(source);
	return *this;
}

EuropeanOption& EuropeanPutOption::operator = (const EuropeanOption& source)
{
	if (this == &source)							//check if current object and argument object are same object
		return *this;

	EuropeanOption::operator=(source);
	return *this;
}

void EuropeanCallOption::Print() const{
	cout << "Call Option data: T=" << T << ", K=" << K << ", sig=" << sig << ", r=" <<r << ", S=" << S << "\n";
	double C = CallPrice(S, K, T, r, sig, b); double P = PutPrice(S, K, T, r, sig, b);
	cout << "Black Scholes Formulae: C=" << C << ", P=" << P << "\n";
	cout << "Put Call Parity: C=" << CallPricePCP(P, S, K, T, r, b) << ", P=" << PutPricePCP(C, S, K, T, r, b) << "\n";
	cout << "Delta(Call)=" << CallDelta(S, K, T, r, sig, b) << "\n";
	cout << "Gamma=" << GammaGF(S, K, T, r, sig, b) << ", Vega=" << VegaGF(S, K, T, r, sig, b) << "\n";		//Gamma and Vega are the same for call and put option, given the parameters are.
	cout << "Theta(Call)=" << CallTheta(S, K, T, r, sig, b) << "\n";
	cout << "Rho(Call)=" << CallRho(S, K, T, r, sig, b) << endl;

	if (PutCallParity(C, P))						//calling on either type of option, as long as Option Data is the same.
		cout << "Put Call Parity is Satisfied\n";
	else
		cout << "Arbitrage oportunity is detected!!!!\n";

}

void EuropeanPutOption::Print()const {
	cout << "Put Option data: T=" << T << ", K=" << K << ", sig=" << sig << ", r=" << r << ", S=" << S << "\n";
	double C = CallPrice(S, K, T, r, sig, b); double P = PutPrice(S, K, T, r, sig, b);
	cout << "Black Scholes Formulae: C=" << C << ", P=" << P << "\n";
	cout << "Put Call Parity: C=" << CallPricePCP(P, S, K, T, r, b) << ", P=" << PutPricePCP(C, S, K, T, r, b) << "\n";
	cout << "Delta(Put)=" << PutDelta(S, K, T, r, sig, b) << "\n";
	cout << "Gamma=" << GammaGF(S, K, T, r, sig, b) << ", Vega=" << VegaGF(S, K, T, r, sig, b) << "\n";		//Gamma and Vega are the same for call and put option, given the parameters are.
	cout << "Theta(Put)=" << PutTheta(S, K, T, r, sig, b) << "\n";
	cout << "Rho(Put)=" << CallRho(S, K, T, r, sig, b) << endl;

	if (PutCallParity(C, P))						//calling on either type of option, as long as Option Data is the same.
		cout << "Put Call Parity is Satisfied\n";
	else
		cout << "Arbitrage oportunity is detected!!!!\n";

}

//Pricing option functions

std::vector<double> EuropeanCallOption::Price(double LowerLimit, double UpperLimit, int Num, FunctionPointer Ptr)
{
	vector<double> mesh = MeshArray(LowerLimit, UpperLimit, Num);

	vector<double> result;
	result.reserve(mesh.size());

	for (vector<double>::iterator it = mesh.begin(); it != mesh.end(); ++it)
	{
		result.push_back((this->*Ptr)(*it));		//Calculate result using function pointer. Current value of vector element is taken as an argument.
	}
	return result;
}

std::vector<double> EuropeanPutOption::Price(double LowerLimit, double UpperLimit, int Num, FunctionPointer Ptr)
{
	vector<double> mesh = MeshArray(LowerLimit, UpperLimit, Num);

	vector<double> result;
	result.reserve(mesh.size());

	for (vector<double>::iterator it = mesh.begin(); it != mesh.end(); ++it)
	{
		result.push_back((this->*Ptr)(*it));
	}
	return result;
}

bool EuropeanOption::PutCallParity(double C, double P)const
{
	static const double epsilon = 0.00001;

	if ((C + K*exp(-r*T) - P - S*exp((b - r)*T)) > epsilon)
	{
		return false;
	}
	else
	{
		return true;
	}
}


#define BOOST_TEST_MODULE "JosephusModule"
#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_parameters.hpp>

#include "Josephus.h"
#include <list>
#include <vector>

BOOST_AUTO_TEST_CASE(ERASE_VECTOR_AT_INDEX)
{
	boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_all_errors);
	BOOST_TEST_MESSAGE("Test the erasing vector at index of");

	std::vector<int> vec1{ 1,2,3,4,5,6 };
	eraseVectorAtIndex(vec1, 2); // removes the 3 at index 2

	BOOST_CHECK(vec1.at(0) == 1);
	BOOST_CHECK(vec1.at(1) == 2);
	BOOST_CHECK(vec1.at(2) == 4);
	BOOST_CHECK(vec1.at(3) == 5);
	BOOST_CHECK(vec1.at(4) == 6);
}

BOOST_AUTO_TEST_CASE( JOSEPHUS_TEST_N5K2 )
{
	boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_all_errors);
	int N = 5;
	int K = 2;
	BOOST_TEST_MESSAGE("Test the Josephus Algorithm with N=" << N << " and K= " << K << " !!!");

	std::vector<int> evadedVectorList;
	int remainder = Josephus(N, K, evadedVectorList);
	/// 1 2 3 4 5 => 1 3 4 5 => 1 3 5 (1. durchlauf vorbei) => 3 5 => 3
	// sol : 2 4 1 5 , rem=3
	// math.sol: remainder = 1001 => 00011 = 3

	BOOST_CHECK(evadedVectorList.at(0) == 2);
	BOOST_CHECK(evadedVectorList.at(1) == 4);
	BOOST_CHECK(evadedVectorList.at(2) == 1);
	BOOST_CHECK(evadedVectorList.at(3) == 5);

	BOOST_CHECK(remainder == 3);
}

BOOST_AUTO_TEST_CASE(JOSEPHUS_TEST_N5K11)
{
	boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_all_errors);
	int N = 5;
	int K = 11;
	BOOST_TEST_MESSAGE("Test the Josephus Algorithm with N=" << N << " and K= "<< K <<" !!!");

	std::vector<int> evadedVectorList;
	int remainder = Josephus(N, K, evadedVectorList);
	// sol : 1  4 2 3  rem 5
	// math.sol: remainder = 5^1 + 5^0 = 5 rem = 5

	BOOST_CHECK(evadedVectorList.at(0) == 1);
	BOOST_CHECK(evadedVectorList.at(1) == 4);
	BOOST_CHECK(evadedVectorList.at(2) == 2);
	BOOST_CHECK(evadedVectorList.at(3) == 3);

	BOOST_CHECK(remainder == 5);
}

BOOST_AUTO_TEST_CASE(JOSEPHUS_TEST_N13K2)
{
	boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_all_errors);
	int N = 13;
	int K = 2;
	BOOST_TEST_MESSAGE("Test the Josephus Algorithm with N=" << N << " and K= " << K << " !!!");

	std::vector<int> evadedVectorList;
	int remainder = Josephus(N, K, evadedVectorList);
	/// 11
	// sol : 2 4 6 8 10 12 (all even positions done) 1 5 9 13 7 3 rem 11
	// math.sol: remainder = 1101 => 1011 = 11

	BOOST_CHECK(evadedVectorList.at(0) == 2);
	BOOST_CHECK(evadedVectorList.at(1) == 4);
	BOOST_CHECK(evadedVectorList.at(2) == 6);
	BOOST_CHECK(evadedVectorList.at(3) == 8);
	BOOST_CHECK(evadedVectorList.at(4) == 10);
	BOOST_CHECK(evadedVectorList.at(5) == 12);
	BOOST_CHECK(evadedVectorList.at(6) == 1);
	BOOST_CHECK(evadedVectorList.at(7) == 5);
	BOOST_CHECK(evadedVectorList.at(8) == 9);
	BOOST_CHECK(evadedVectorList.at(9) == 13);
	BOOST_CHECK(evadedVectorList.at(10) == 7);
	BOOST_CHECK(evadedVectorList.at(11) == 3);

	BOOST_CHECK(remainder == 11);
}

BOOST_AUTO_TEST_CASE(JOSEPHUS_TEST_N13K3)
{
	boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_all_errors);
	int N = 13;
	int K = 3;
	BOOST_TEST_MESSAGE("Test the Josephus Algorithm with N=" << N << " and K= " << K << " !!!");

	std::vector<int> evadedVectorList;
	int remainder = Josephus(N, K, evadedVectorList);
	///
	// sol: 3 6 9 12 2  7  11  4 10 5  1  8 rem 13

	// math.sol: remainder = 3^2 + 3^1 + 3^0 = 13 so rem is 13

	BOOST_CHECK(evadedVectorList.at(0) == 3);
	BOOST_CHECK(evadedVectorList.at(1) == 6);
	BOOST_CHECK(evadedVectorList.at(2) == 9);
	BOOST_CHECK(evadedVectorList.at(3) == 12);
	BOOST_CHECK(evadedVectorList.at(4) == 2);
	BOOST_CHECK(evadedVectorList.at(5) == 7);
	BOOST_CHECK(evadedVectorList.at(6) == 11);
	BOOST_CHECK(evadedVectorList.at(7) == 4);
	BOOST_CHECK(evadedVectorList.at(8) == 10);
	BOOST_CHECK(evadedVectorList.at(9) == 5);
	BOOST_CHECK(evadedVectorList.at(10) == 1);
	BOOST_CHECK(evadedVectorList.at(11) == 8);

	BOOST_CHECK(remainder == 13);
}