#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>

#include <CGAL/subdivision_method_3.h>
#include <CGAL/Timer.h>

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <fstream>

typedef CGAL::Simple_cartesian<double>      Kernel;
typedef CGAL::Surface_mesh<Kernel::Point_3> PolygonMesh;
using namespace std;
using namespace CGAL;
namespace params = CGAL::parameters;

// ======================================================================
template <class Poly>
class WLoop_mask_3 {
  typedef Poly                                         PolygonMesh;

  typedef typename boost::graph_traits<PolygonMesh>::vertex_descriptor   vertex_descriptor;
  typedef typename boost::graph_traits<PolygonMesh>::halfedge_descriptor halfedge_descriptor;

  typedef typename boost::property_map<PolygonMesh, vertex_point_t>::type Vertex_pmap;
  typedef typename boost::property_traits<Vertex_pmap>::value_type Point;
  typedef typename boost::property_traits<Vertex_pmap>::reference Point_ref;

  PolygonMesh& pmesh;
  Vertex_pmap vpm;

public:
  WLoop_mask_3(PolygonMesh& pmesh)
    : pmesh(pmesh), vpm(get(CGAL::vertex_point, pmesh))
  {}

  void edge_node(halfedge_descriptor hd, Point& pt) {
    Point_ref p1 = get(vpm, target(hd,pmesh));
    Point_ref p2 = get(vpm, target(opposite(hd,pmesh),pmesh));
    Point_ref f1 = get(vpm, target(next(hd,pmesh),pmesh));
    Point_ref f2 = get(vpm, target(next(opposite(hd,pmesh),pmesh),pmesh));

    pt = Point((3*(p1[0]+p2[0])+f1[0]+f2[0])/8,
               (3*(p1[1]+p2[1])+f1[1]+f2[1])/8,
               (3*(p1[2]+p2[2])+f1[2]+f2[2])/8 );
  }
  void vertex_node(vertex_descriptor vd, Point& pt) {
    double R[] = {0.0, 0.0, 0.0};
    Point_ref S = get(vpm,vd);

    std::size_t n = 0;
    for(halfedge_descriptor hd : halfedges_around_target(vd, pmesh)){
      ++n;
      Point_ref p = get(vpm, target(opposite(hd,pmesh),pmesh));
      R[0] += p[0];         R[1] += p[1];         R[2] += p[2];
    }

    if (n == 6) {
      pt = Point((10*S[0]+R[0])/16, (10*S[1]+R[1])/16, (10*S[2]+R[2])/16);
    } else if (n == 3) {
      double B = (5.0/8.0 - std::sqrt(3+2*std::cos(6.283/n))/64.0)/n;
      double A = 1-n*B;
      pt = Point((A*S[0]+B*R[0]), (A*S[1]+B*R[1]), (A*S[2]+B*R[2]));
    } else {
      double B = 3.0/8.0/n;
      double A = 1-n*B;
      pt = Point((A*S[0]+B*R[0]), (A*S[1]+B*R[1]), (A*S[2]+B*R[2]));
    }
  }

  void border_node(halfedge_descriptor hd, Point& ept, Point& vpt) {
    Point_ref ep1 = get(vpm, target(hd,pmesh));
    Point_ref ep2 = get(vpm, target(opposite(hd,pmesh),pmesh));
    ept = Point((ep1[0]+ep2[0])/2, (ep1[1]+ep2[1])/2, (ep1[2]+ep2[2])/2);

    Halfedge_around_target_circulator<Poly> vcir(hd,pmesh);
    Point_ref vp1  = get(vpm, target(opposite(*vcir,pmesh),pmesh));
    Point_ref vp0  = get(vpm, target(*vcir,pmesh));
    --vcir;
    Point_ref vp_1 = get(vpm,target(opposite(*vcir,pmesh),pmesh));
    vpt = Point((vp_1[0] + 6*vp0[0] + vp1[0])/8,
                (vp_1[1] + 6*vp0[1] + vp1[1])/8,
                (vp_1[2] + 6*vp0[2] + vp1[2])/8 );
  }
};

int main(int argc, char **argv) {
  if (argc > 4) {
    cerr << "Usage: Customized_subdivision [d] [filename_in] [filename_out] \n";
    cerr << "         d -- the depth of the subdivision (default: 1) \n";
    cerr << "         filename_in -- the input mesh (.off) (default: data/quint_tris.off) \n";
    cerr << "         filename_out -- the output mesh (.off) (default: result.off)" << endl;
    return 1;
  }

  int d = (argc > 1) ? boost::lexical_cast<int>(argv[1]) : 1;
  const std::string in_file = (argc > 2) ? argv[2] : CGAL::data_file_path("meshes/quint_tris.off");
  const char* out_file = (argc > 3) ? argv[3] : "result.off";

  PolygonMesh pmesh;
  std::ifstream in(in_file);
  if(in.fail()) {
    std::cerr << "Could not open input file " << in_file << std::endl;
    return 1;
  }
  in >> pmesh;

  Timer t;
  t.start();
  Subdivision_method_3::PTQ(pmesh, WLoop_mask_3<PolygonMesh>(pmesh), params::number_of_iterations(d));
  std::cerr << "Done (" << t.time() << " s)" << std::endl;

  std::ofstream out(out_file);
  out << pmesh;

  return 0;
}


/**
 * @file sdirk.cc
 * @brief NPDE homework SDIRK code
 * @author Unknown, Oliver Rietmann
 * @date 31.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "sdirk.h"

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "../../../lecturecodes/helperfiles/polyfit.h"

namespace SDIRK {

/* SAM_LISTING_BEGIN_0 */
Eigen::Vector2d SdirkStep(const Eigen::Vector2d &z0, double h, double gamma) {
  Eigen::Vector2d res;
  // Compute one timestep of the SDIRK implicit RK-SSM for the linear ODE
#if SOLUTION
  // Matrix A for evaluation of f
  Eigen::Matrix2d A;
  A << 0., 1., -1., -1.;
  // Precompute and reuse factorization
  auto A_lu = (Eigen::Matrix2d::Identity() - h * gamma * A).partialPivLu();
  Eigen::Vector2d az = A * z0;

  // Increments according to \prbeqref{eq:ies}
  Eigen::Vector2d k1 = A_lu.solve(az);
  Eigen::Vector2d k2 = A_lu.solve(az + h * (1 - 2 * gamma) * A * k1);

  // Updated state
  res = z0 + h * 0.5 * (k1 + k2);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return res;
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
std::vector<Eigen::Vector2d> SdirkSolve(const Eigen::Vector2d &z0,
                                        unsigned int M, double T,
                                        double gamma) {
  // Solution vector
  std::vector<Eigen::Vector2d> res(M + 1);
  // Solve the ODE with uniform timesteps using the SDIRK method
#if SOLUTION
  // Equidistant step size
  const double h = T / M;
  // Push initial data
  res[0] = z0;
  // Main loop
  for (unsigned int i = 1; i <= M; ++i) {
    res[i] = SdirkStep(res[i - 1], h, gamma);
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return res;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double CvgSDIRK() {
  double conv_rate;
  // Study the convergence rate of the method.
#if SOLUTION
  // Initial data z0 = [y(0), y'(0)]
  Eigen::Vector2d z0;
  z0 << 1, 0;
  // Final time
  const double T = 10;
  // Parameter
  const double gamma = (3. + std::sqrt(3.)) / 6.;
  // Mesh sizes
  Eigen::ArrayXd err(10);
  Eigen::ArrayXd M(10);
  M << 20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240;

  // Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
  auto yex = [&z0](double t) {
    return 1. / 3. * std::exp(-t / 2.) *
           (3. * z0(0) * std::cos(std::sqrt(3.) * t / 2.) +
            std::sqrt(3.) * z0(0) * std::sin(std::sqrt(3.) * t / 2.) +
            2. * std::sqrt(3.) * z0(1) * std::sin(std::sqrt(3.) * t / 2.));
  };

  // Store old error for rate computation
  double errold = 0;
  std::cout << std::setw(15) << "m" << std::setw(15) << "maxerr"
            << std::setw(15) << "rate" << std::endl;
  // Loop over all meshes
  for (unsigned int i = 0; i < M.size(); ++i) {
    int m = M(i);
    // Get solution
    auto sol = SdirkSolve(z0, m, T, gamma);
    // Compute error
    err(i) = std::abs(sol.back()(0) - yex(T));

    // Print table
    std::cout << std::setw(15) << m << std::setw(15) << err(i);
    if (i > 0) std::cout << std::setw(15) << std::log2(errold / err(i));
    std::cout << std::endl;

    // Store old error
    errold = err(i);
  }

  Eigen::VectorXd coeffs = polyfit(M.log(), err.log(), 1);
  conv_rate = -coeffs(0);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return conv_rate;
}
/* SAM_LISTING_END_2 */

}  // namespace SDIRK


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FREXP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FREXP_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing frexp capabilities

    Computes a mantissa and an exponent pair for the input

    @par Semantic:

    For every parameter of floating type @c T

    @code
    std::tie(m, e)= frexp(x);
    @endcode

    is similar to:

    @code
    as_integer_t<T> e = exponent(x)+1;
    T m = copysign(mantissa(x)/2, x);
    @endcode

    The call

    @code
    std:pair<T,as_integer_t<T>> p = frexp(x);
    @endcode

    can also be used.

    @par Note:

    This function splits a floating point value @c v f in a signed mantissa @c m and
    an exponent @c e so that:  @f$v = m\times 2^e@f$,
    with absolute value of @c m \f$\in [1/2, 1[\f$

    @warninbox{Take care that these results differ from the returns of the functions @ref mantissa
    and @ref exponent}

    The decorators fast_ and std_ can be used.

    fast_ provides a speedier call, but special values as Nan or Inf are not handled properly.
    std_ transmit the call to std::frexp. That implies that simd is ever emulated.
    @see exponent, mantissa, copysign

  **/
  std::pair<T, as_integer_t<Value>> frexp(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/frexp.hpp>
#include <boost/simd/function/scalar/frexp.hpp>
#include <boost/simd/function/simd/frexp.hpp>

#endif


#pragma once
/*
  Types and definitions
  zeFresk
*/

#include <boost/multi_array.hpp>
#include <cstdint>
#include <complex>

#include "configuration.h"

// real and complex
#if defined(DOUBLE_FP) // no multiprecision supported -yet
	using real_t = double;
#elif defined(MPFR_FP)
	#include <boost/multiprecision/mpfr.hpp>
	using real_t = boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<PRECISION_DIGITS> >;
#elif defined(GMP_FP)
	#include <boost/multiprecision/gmp.hpp>
	using real_t = boost::multiprecision::number<boost::multiprecision::gmp_float<PRECISION_DIGITS> >;
#elif defined(BOOST_FP)
	#include <boost/multiprecision/cpp_bin_float.hpp>
	using real_t = boost::multiprecision::number<boost::multiprecision::cpp_bin_float<PRECISION_DIGITS> >;
#else
	using real_t = float;
#endif

#if defined(CUDA_BACKEND)
	#include <thrust/complex.h>
	using complex_t = thrust::complex<real_t>;
#else
	using complex_t = std::complex<real_t>;
#endif

// pixels array
#include "pixel_array.hpp"
using pixel_array = PixelArray;

// used to compute fractal
struct Parameters {
	complex_t center;
	real_t precision;
	size_t iterations;
};


#include <chrono>
#include <iostream>

#include <Eigen/Dense>

EIGEN_DONT_INLINE
double simple_function(Eigen::VectorXd &va, Eigen::VectorXd &vb) {
  // this simple function computes the dot product of two vectors
  // of course it could be expressed more compactly
  double d = va.dot(vb);
  return d;
}

int main() {
  int len = 1000000;
  int num_repetitions = 100;

  // generate two random vectors
  Eigen::VectorXd va = Eigen::VectorXd::Random(len);
  Eigen::VectorXd vb = Eigen::VectorXd::Random(len);

  double result;
  auto start = std::chrono::system_clock::now();
  for (auto i = 0; i < num_repetitions; i++) {
    result = simple_function(va, vb);
  }
  auto end = std::chrono::system_clock::now();
  auto elapsed_seconds = std::chrono::duration_cast<std::chrono::microseconds>(end-start);

  std::cout << "result: " << result << std::endl;
  std::cout << "elapsed seconds: " << elapsed_seconds.count()/1000000.0 << std::endl;
}


#ifndef IMGEN_FILTER_HPP_
#define IMGEN_FILTER_HPP_

#include "imgen/image.hpp"

#include <boost/gil/gil_all.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <format.h>

#include <functional>

namespace gil = boost::gil;
namespace ublas = boost::numeric::ublas;

namespace imgen {

/**
 * The filter type, for now this will just be a matrix of floats.
 *
 * TODO: look into using the pixel channel type? could have side effects if
 * channel type is scoped?
 */
typedef ublas::matrix<float> filter_t;

/**
 * Apply a function to each region of an image using a sliding neighbourhood.
 * This can be slow on large images. The neighbourhood is padded with 0s.
 *
 * img - the image to apply the function to
 * w - the width of the region
 * h - the height of the region
 * f - the function to apply to each region
 */
void nlfilter(image& img, std::size_t w, std::size_t h,
              std::function<image::pixel_t(const ublas::matrix<image::pixel_t>&)> f);

/**
 * Applies a filter to an image using sliding neighbourhood correlation. This
 * can be slow on large images. The neighbourhood is padded with 0s.
 *
 * TODO: Give a way to define the padding element.
 */
void filter(image& img, const filter_t& filter);

/**
 * Produces a square gaussian filter mask. The mask is calcualted using:
 *
 * g(x, y) = e ^ (-(x^2 + y^2) / (2 * σ^2))
 *
 * Where the center element of the matrix is the origin. The mask is then
 * normalized. σ defaults to 0.5.
 */
filter_t gaussian(filter_t::size_type size, filter_t::value_type sigma = 0.5);

} // namespace imgen

#endif // IMGEN_FILTER_HPP_


/*!
 * \file PlainGeometry.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date February 20, 2020: created
 */

#pragma once

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

namespace {
enum Orientation {
    OnLine,
    Clockwise,
    CntrClockwise
};

Orientation orientation(
    Eigen::Vector2d const& p0,
    Eigen::Vector2d const& p1,
    Eigen::Vector2d const& p2) noexcept;

//! Compute the convex hull.
//! \param A set of vertices.
//! \return A list of control points spanning the hull. They are stored in the counter-clockwise order.
std::vector<Eigen::Vector2d> getConvexHull(
    std::vector<Eigen::Vector2d> const& vs) noexcept;

//! Check if the convex hull of control points overlaps with that of another Bezier curve.
//! Here, so-called *The Separating Axis Theorem* is used.
bool hasHullIntersection(
    std::vector<Eigen::Vector2d> const& lhs,
    std::vector<Eigen::Vector2d> const& rhs) noexcept;

}


///////////////////////////////////////////////////////////////////
//  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 for non-complex types
  Use of DFT for Number Theoretical Transform.
*/
#include <boost/math/fft/bsl_backend.hpp>
namespace fft = boost::math::fft;

#include <iostream>
#include <vector>
#include "fft_test_helpers.hpp"

class Z337
{
public:
  typedef int integer;
  static constexpr integer mod{337}; // 337 = 2*2*2*2*3*7 + 1
};

int convolution()
/*
  product of two integer by means of the NTT,
  using the convolution theorem
*/
{
  int errors = 0;
  using M_int = fft::my_modulo_lib::mint<Z337>;
  // 85 is a primitive root of 337,
  // ie. the smallest number k for such that 85^k mod 337 = 1 is k=phi(337)=336
  const M_int w{85};
  const M_int inv_8{M_int{8}.inverse()};

  // Multiplying 1234 times 5678 = 7006652
  std::vector<M_int> A{4, 3, 2, 1, 0, 0, 0, 0};
  std::vector<M_int> B{8, 7, 6, 5, 0, 0, 0, 0};

  // forward FFT
  fft::bsl_algebraic_transform::forward(A.cbegin(),A.cend(),A.begin(), w);
  fft::bsl_algebraic_transform::forward(B.cbegin(),B.cend(),B.begin(), w);

  // convolution in Fourier space
  std::vector<M_int> AB;
  std::transform(A.begin(), A.end(), B.begin(),
                 std::back_inserter(AB),
                 [](M_int x, M_int y) { return x * y; });

  // backwards FFT
  fft::bsl_algebraic_transform::backward(AB.cbegin(),AB.cend(),AB.begin(),w);
  std::transform(AB.begin(), AB.end(), AB.begin(),
                 [&inv_8](M_int x) { return x * inv_8; });

  // carry the remainders in base 10
  std::vector<int> C;
  M_int r{0};
  for (auto x : AB)
  {
    auto y = x + r;
    C.emplace_back(int(y) % 10);
    r = M_int(int(y) / 10);
  }
  // yields 7006652
  if(static_cast<int>(C.size())!=8) errors++;
  if(C[0]!=2) errors++;
  if(C[1]!=5) errors++;
  if(C[2]!=6) errors++;
  if(C[3]!=6) errors++;
  if(C[4]!=0) errors++;
  if(C[5]!=0) errors++;
  if(C[6]!=7) errors++;
  if(C[7]!=0) errors++;
  return errors;
}
int main()
{
  return convolution();
}


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

using namespace Eigen;
using namespace std;

extern "C" void dggev_(const char* JOBVL, const char* JOBVR, const int* N,
        const double* A, const int* LDA, const double* B, const int* LDB,
        double* ALPHAR, double* ALPHAI, double* BETA,
        double* VL, const int* LDVL, double* VR, const int* LDVR,
        double* WORK, const int* LWORK, int* INFO);

// Generalised Eigen-Problem
// source adapted from https://eigen.tuxfamily.org/index.php?title=Lapack
// Solve:
// A * v(j) = lambda(j) * B * v(j).
//
// v are the eigenvectors and are stored in v.
// lambda are the eigenvalues and are stored in lambda.
// The eigenvalues are stored as: (lambda(:, 1) + lambda(:, 2)*i)./lambda(:, 3)
//
// returns true on success.
// A and B will be changed.
bool GEP(MatrixXd& A, MatrixXd& B, MatrixXd& v, MatrixXd& lambda)
{
    int N = A.cols(); // Number of columns of A and B. Number of rows of v.
    if (B.cols() != N  || A.rows()!=N || B.rows()!=N){
        cout << "Matrices A and B are not square and the same size, line " << __LINE__ <<  endl;
        return false;
    }

    v.resize(N,N);
    lambda.resize(N, 3);

    int LDA = A.outerStride();
    int LDB = B.outerStride();
    int LDV = v.outerStride();

    double WORKDUMMY;
    int LWORK = -1; // Request optimum work size.
    int INFO = 0;

//    double * alphar = const_cast<double*>(lambda.col(0).data());
//    double * alphai = const_cast<double*>(lambda.col(1).data());
//    double * beta   = const_cast<double*>(lambda.col(2).data());

    double * alphar = lambda.col(0).data();
    double * alphai = lambda.col(1).data();
    double * beta   = lambda.col(2).data();

    // Get the optimum work size.
    dggev_("N", "V", &N, A.data(), &LDA, B.data(), &LDB, alphar, alphai, beta, 0, &LDV, v.data(), &LDV, &WORKDUMMY, &LWORK, &INFO);

    LWORK = int(WORKDUMMY) + 32;
    VectorXd WORK(LWORK);

    dggev_("N", "V", &N, A.data(), &LDA, B.data(), &LDB, alphar, alphai, beta, 0, &LDV, v.data(), &LDV, WORK.data(), &LWORK, &INFO);

    return INFO==0;
}

int main(){

    MatrixXd A;
    MatrixXd B;
    MatrixXd v;
    MatrixXd lambda;

    A.setRandom(4, 4);
    B.setRandom(4, 4);

    //A(1, 1) = 2;

    cout << "Before calling the GEP function. " << endl;
    cout << "Matrix A " << endl << A << endl;
    cout << "Matrix B " << endl << B << endl;

    bool success = GEP(A, B, v, lambda);

    if (success){
        cout << "A " << endl << A << endl;
        cout << "B" << endl << B << endl;

        cout << "lambda " << endl << lambda << endl;
        cout << "v " << endl << v << endl;

    }   else {
        cout << "GEP failed." << endl;
    }

    return 0;
}


#include <Eigen/Core>
#include <unsupported/Eigen/SpecialFunctions>
#include <iostream>
using namespace Eigen;
int main()
{
  Array4d v(-0.5,2,0,-7);
  std::cout << v.erfc() << std::endl;
}


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

using namespace probreg;

KabschResult probreg::computeKabsch(const MatrixX3& model,
                                    const MatrixX3& target,
                                    const Vector& weight) {
    //Compute the center
    Vector3 model_center = Vector3::Zero();
    Vector3 target_center = Vector3::Zero();
    Float total_weight = 0.0f;
    for(auto i = 0; i < model.rows(); ++i) {
        const Float w_i = weight[i];
        total_weight += w_i;
        model_center.noalias() += w_i * model.row(i);
        target_center.noalias() += w_i * target.row(i);
    }
    if (total_weight == 0) {
        return std::make_pair(Matrix3::Identity(), Vector3::Zero());
    }
    const Float divided_by = 1.0f / total_weight;
    model_center *= divided_by;
    target_center *= divided_by;

    //Centralize them
    //Compute the H matrix
    Float h_weight = 0.0f;
    Matrix3 hh = Matrix3::Zero();
    for(auto k = 0; k < model.rows(); ++k) {
        const auto& model_k = model.row(k).transpose();
        auto centralized_model_k = model_k - model_center;
        const auto& target_k = target.row(k).transpose();
        auto centralized_target_k = target_k - target_center;
        const Float this_weight = weight[k];
        h_weight += this_weight * this_weight;
        hh.noalias() += (this_weight * this_weight) * centralized_model_k * centralized_target_k.transpose();
    }

    //Do svd
    hh /= h_weight;
    Eigen::JacobiSVD<Matrix3> svd(hh, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Vector3 ss = Vector3::Ones(3);
    ss[2] = (svd.matrixU() * svd.matrixV()).determinant();
    const Matrix3 r = svd.matrixV() * ss.asDiagonal() * svd.matrixU().transpose();

    //The translation
    Vector3 translation = target_center;
    translation.noalias() -= r * model_center;

    return std::make_pair(r, translation);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

// Alleged error reported by Dragan Vidovic

template<class ct>
mtl::dense2D<ct> inv2 ( const mtl::dense2D<ct> & M )
{
   mtl::dense2D<ct> N(2,2);
   ct d = M[0][0]*M[1][1]-M[0][1]*M[1][0];
   N[0][0] = M[1][1]/d;
   N[0][1] =-M[0][1]/d;
   N[1][0] =-M[1][0]/d;
   N[1][1] = M[0][0]/d; return N;
}

int main(int, char**)
{
    typedef double ct;
    mtl::dense2D<ct> tmp(2, 2);
    tmp= 3, 5,
	 8, 9;

    mtl::dense2D<ct> tmp1 = inv2(tmp), P(tmp * tmp1);
    cout << "tmp1 is\n" << tmp1 << "\ntmp * tmp1 is:\n" << P;

    return 0;
}


#ifndef _KERNEL_HPP_
#define _KERNEL_HPP_

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/SparseCore>
#include <cstdarg>
#include <mimkl/definitions.hpp>

using mimkl::definitions::Index;

namespace mimkl
{
namespace kernel
{

//! The linear kernel with the extended kernel K = X*Y_t
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed (unconjugated) right hand side KxM data-matrix.
\returns kernel_matrix the similarity NxK matrix.
\sa test/linear_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived>
KDerived linear_kernel(LhsDerived &lhs, RhsDerived &rhs)
{
    return lhs * rhs.adjoint();
}

//! The polynomial kernel with the extended kernel K =
//! (X*Y_t + c)^p
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed (unconjugated)  right hand side KxM data-matrix.
\param degree polynomial degree.
\param offset "free parameter trading off the influence of higher-order versus
lower-order terms in the polynomial".
\returns kernel_matrix the similarity NxK matrix.
\sa TODO
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived>
KDerived polynomial_kernel(LhsDerived &lhs,
                           RhsDerived &rhs,
                           const double degree,
                           const double offset)
{
    return (linear_kernel<KDerived>(lhs, rhs).array() + offset).pow(degree).matrix();
}

//! The gaussian kernel  k = exp(-
//! (x-y)_t*(x-y) / ( 2*s^2 )).
/*!  This squared pairwise euclidean distance cannot be expressed in a concise
matrix multiplication.
The squared distance of xi to yj = \f$(x_i^T*x_i) -(x_i^T*y_j)
-(y_j^T*x_i) + (y_j^T*y_j) \f$
This means next to \f$XY^T\f$ (the linear kernel) only the diagonal entries of
\f$XX^T\f$ and \f$YY^T\f$ are needed.
\param lhs left hand side NxM data-matrix.
\param rhs untransposed  (unconjugated) right hand side KxM data-matrix.
\param sigma_square variance of the bell curve.
\returns kernel_matrix the similarity NxK matrix.
\sa gaussian_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived>
KDerived
gaussian_kernel(LhsDerived &lhs, RhsDerived &rhs, const double sigma_square)
{

    return (-(
            /*! -2* lhs_inducer_rhs only in case of scalar matrices.
             with complex numbers we need: -lhs_rhs
             -lhs_rhs.adjoint
             imaginary parts cancel out! */
            ((-2 * (lhs * rhs.adjoint()).real()).colwise() +
             lhs.rowwise().squaredNorm())
            .rowwise() +
            lhs.rowwise().squaredNorm().transpose()) /
            (2 * sigma_square))
    .array()
    .exp()
    .matrix();
}

//! The sigmoidal kernel with the extended kernel k = tanh(a*
//! (x_t*y) +b).
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed  (unconjugated) right hand side KxM data-matrix.
\param a
\param b
\returns kernel_matrix the similarity NxK matrix.
\sa test/sigmoidal_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived>
KDerived
sigmoidal_kernel(LhsDerived &lhs, RhsDerived &rhs, const double a, const double b)
{

    return (a * linear_kernel<KDerived>(lhs, rhs).array() + b).tanh().matrix();
}

} // namespace kernel
} // namespace mimkl

#endif /*_KERNEL_HPP_*/


#include <Eigen/Dense>

#include <iostream>
#include <vector>

#include "timer.h"

//! \brief build A*x using array of ranges and of ones.
//! \param[in] x vector x for A*x = y
//! \param[out] y y = A*x
void multAminSlow(const Eigen::VectorXd & x, Eigen::VectorXd & y) {
    // TODO
}

//! \brief build A*x using a clever representation
//! \param[in] x vector x for A*x = y
//! \param[out] y y = A*x
void multAmin(const Eigen::VectorXd & x, Eigen::VectorXd & y) {
    // TODO
}

int main(void) {
    // Build Matrix B with 10x10 dimensions such that B = inv(A)
    unsigned int n = 10;
    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(n,n);
    for(unsigned int i = 0; i < n; ++i) {
        B(i,i) = 2;
        if(i < n-1) B(i+1,i) = -1;
        if(i > 0) B(i-1,i) = -1;
    }
    B(n-1,n-1) = 1;
    std::cout << "B = " << B << std::endl;

    // Check that B = inv(A) (up to machine precision)
    Eigen::VectorXd x = Eigen::VectorXd::Random(n), y;
    multAmin(B*x, y);
    std::cout << "|y-x| = " << (y - x).norm() << std::endl;
    multAminSlow(B*x, y);
    std::cout << "|y-x| = " << (y - x).norm() << std::endl;

    // Timing from 2^4 to 2^13 repeating nruns times
    timer<> tm_slow, tm_slow_loops, tm_fast;
    std::vector<int> times_slow, times_fast;
    unsigned int nruns = 10;
    for(unsigned int p = 4; p <= 13; ++p) {
        tm_slow.reset();
        tm_slow_loops.reset();
        tm_fast.reset();
        for(unsigned int r = 0; r < nruns; ++r) {
            x = Eigen::VectorXd::Random(pow(2,p));

            tm_slow.start();
            multAminSlow(x, y);
            tm_slow.stop();

            tm_fast.start();
            multAmin(x, y);
            tm_fast.stop();
        }
        times_slow.push_back( tm_slow.avg().count() );
        times_fast.push_back( tm_fast.avg().count() );
    }

    for(auto it = times_slow.begin(); it != times_slow.end(); ++it) {
        std::cout << *it << " ";
    }
    std::cout << std::endl;
    for(auto it = times_fast.begin(); it != times_fast.end(); ++it) {
        std::cout << *it << " ";
    }
    std::cout << std::endl;
}

Nummerical Methods for CSE/PS1/templates/multAmin.cpp


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#pragma once

#include <Eigen/Dense>

#include <iostream>

namespace sfcpp {
namespace math {

class VectorSubspace {
  Eigen::MatrixXd baseMat, kernelMat;
  size_t numCoordinates;
  static constexpr double myeps = 1e-8;
  static constexpr double mysqeps = myeps * myeps;

 public:
  VectorSubspace(size_t numCoordinates)
      : baseMat(Eigen::MatrixXd::Zero(numCoordinates, 1)),
        kernelMat(Eigen::MatrixXd::Identity(numCoordinates, numCoordinates)),
        numCoordinates(numCoordinates) {}

  void addVector(Eigen::VectorXd const &vec) {
    if (!containsVector(vec)) {
      addIndependentVector(vec);
    }
  }

  void addIndependentVector(Eigen::VectorXd const &vec) {
    baseMat.conservativeResize(baseMat.rows(), baseMat.cols() + 1);
    baseMat.col(baseMat.cols() - 1) = vec;

    kernelMat = baseMat.transpose().fullPivLu().kernel();
  }

  bool containsVector(Eigen::VectorXd const &vec) const {
    return (vec.transpose() * kernelMat).squaredNorm() <= mysqeps;
  }

  bool isOrthogonalTo(Eigen::VectorXd const &vec) const {
    return (vec.transpose() * baseMat).squaredNorm() <= mysqeps;
  }

  /**
   * @return a vector orthogonal to the subspace, which is nonzero iff the
   * subspace is not the whole space.
   */
  Eigen::VectorXd orthogonalVector() { return kernelMat.col(0); }
};

} /* namespace math */
} /* namespace sfcpp */


#pragma once
#include "coordinate_transform.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>

//! Computes the L^2 differences between
//! u1 (considered as coefficients for the shape functions)
//! and u2.
double computeL2Difference(const Eigen::MatrixXd &vertices,
                           const Eigen::MatrixXi &triangles,
                           const Eigen::VectorXd &u1,
                           const std::function<double(double, double)> &u2) {
	const int numberOfElements = triangles.rows();

	double error = 0;
	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = triangles.row(i);

		const int i0 = indexSet(0);
		const int i1 = indexSet(1);
		const int i2 = indexSet(2);

		const auto &a = vertices.row(i0);
		const auto &b = vertices.row(i1);
		const auto &c = vertices.row(i2);

		auto coordinateTransform = makeCoordinateTransform(b - a, c - a);
		auto volumeFactor        = std::abs(coordinateTransform.determinant());

		error += integrate([&](double x, double y) {
			Eigen::Vector2d z = coordinateTransform * Eigen::Vector2d(x, y) + Eigen::Vector2d(a(0), a(1));

			double approximateValue = u1(i0) * lambda(0, x, y) + u1(i1) * lambda(1, x, y) + u1(i2) * lambda(2, x, y);

			return std::pow(std::abs(u2(z(0), z(1)) - approximateValue), 2) * volumeFactor;
		});
	}

	return std::sqrt(error);
}


#include <random>
#include <algorithm>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <Polynomial.hh>

double discount(const double risk_free_rate, const int days){
    return std::exp(-risk_free_rate * (double(days) / 252.0));
}

void monte_carlo(std::vector<double> &data, std::vector<std::vector<double> > &sim_vec, int sim_len, const int iterations){
    double sigma;
    double mu;
    double drift;
    std::vector<double> log_returns;
    std::vector<double> sim;
    log_returns.reserve(data.size()-1);
    sim.reserve(sim_len);

    for(int i=1; i!=data.size(); ++i){
        log_returns.emplace_back(log(data[i] / data[i-1]));
    }

    // get standard deviation and mean
    using namespace boost::accumulators;
    accumulator_set<double, features<tag::mean, tag::variance>> acc;

    for(std::vector<double>::iterator it=log_returns.begin(); it!=log_returns.end(); ++it){
        acc(*it);
    }

    sigma = std::sqrt(variance(acc));
    mu = mean(acc);
    drift = mu - (0.5 * pow(sigma, 2.0));

    std::random_device rand;
    std::mt19937_64 gen(rand());
    std::normal_distribution<double> dist(0.0, sigma);

    // generate simulation
    for(int i=0; i!=iterations; ++i){
        sim.emplace_back(data[data.size()-1] * exp(drift + dist(gen)));
        for(int x=1; x!=sim_len; ++x){
            sim.emplace_back(sim.back() * exp(drift + dist(gen)));
        }
        sim_vec.emplace_back(sim);
        sim.clear();
    }
}

double monte_carlo_fixed_strike_arithmatic_avg_asian_call(std::vector<double> &data_underlying, const double strike, const double risk_free_rate, const int days_to_exp, const int iterations){
    std::vector<double> average_prices;
    std::vector<double> payout;
    std::vector<std::vector<double> > vec;
    double sum;

    // perform Monte Carlo on underlying asset
    monte_carlo(data_underlying, vec, days_to_exp, iterations);

    average_prices.reserve(iterations);
    payout.reserve(iterations);

    for(int i=0; i!=vec.size(); ++i){
        sum = 0.0;
        for(std::vector<double>::iterator it=vec[i].begin(); it!=vec[i].end(); ++it){
            sum += *it;
        }
        average_prices.emplace_back(sum / double(days_to_exp));
    }

    // determine average payout & discount back
    sum = 0.0;
    for(std::vector<double>::iterator it=average_prices.begin(); it!=average_prices.end(); ++it){
        sum += std::max(*it - strike, 0.0);
    }
    return (sum / double(iterations)) * std::exp(-risk_free_rate * (double(days_to_exp) / 252.0));
}

double monte_carlo_fixed_strike_arithmatic_avg_asian_put(std::vector<double> &data_underlying, const double strike, const double risk_free_rate, const int days_to_exp, const int iterations){
    std::vector<double> average_prices;
    std::vector<double> payout;
    std::vector<std::vector<double> > vec;
    double sum;

    // perform Monte Carlo on underlying asset
    monte_carlo(data_underlying, vec, days_to_exp, iterations);

    average_prices.reserve(iterations);
    payout.reserve(iterations);

    for(int i=0; i!=vec.size(); ++i){
        sum = 0.0;
        for(std::vector<double>::iterator it=vec[i].begin(); it!=vec[i].end(); ++it){
            sum += *it;
        }
        average_prices.emplace_back(sum / double(days_to_exp));
    }

    // determine average payout & discount back
    sum = 0.0;
    for(std::vector<double>::iterator it=average_prices.begin(); it!=average_prices.end(); ++it){
        sum += std::max(strike - *it, 0.0);
    }
    return (sum / double(iterations)) * std::exp(-risk_free_rate * (double(days_to_exp) / 252.0));
}

double monte_carlo_floating_strike_arithmatic_avg_asian_call(std::vector<double> &data_underlying, const double strike, const double risk_free_rate, const int days_to_exp, const int iterations){
    std::vector<double> average_prices;
    std::vector<double> payout;
    double maturity_price;
    std::vector<std::vector<double> > vec;
    double sum;

    // perform Monte Carlo on underlying asset
    monte_carlo(data_underlying, vec, days_to_exp, iterations);

    average_prices.reserve(iterations);
    payout.reserve(iterations);

    for(int i=0; i!=vec.size(); ++i){
        sum = 0.0;
        for(std::vector<double>::iterator it=vec[i].begin(); it!=vec[i].end(); ++it){
            sum += *it;
        }
        average_prices.emplace_back(sum / double(days_to_exp));
    }

    // get average price at maturity
    sum = 0.0;
    for(int i=0; i!=vec.size(); ++i){
        sum += vec[i].back();
    }
    maturity_price = sum / double(iterations);

    // determine average payout & discount back
    sum = 0.0;
    for(std::vector<double>::iterator it=average_prices.begin(); it!=average_prices.end(); ++it){
        sum += std::max(maturity_price - (*it * strike), 0.0);
    }
    return (sum / double(iterations)) * std::exp(-risk_free_rate * (double(days_to_exp) / 252.0));
}

double monte_carlo_floating_strike_arithmatic_avg_asian_put(std::vector<double> &data_underlying, const double strike, const double risk_free_rate, const int days_to_exp, const int iterations){
    std::vector<double> average_prices;
    std::vector<double> payout;
    double maturity_price;
    std::vector<std::vector<double> > vec;
    double sum;

    // perform Monte Carlo on underlying asset
    monte_carlo(data_underlying, vec, days_to_exp, iterations);

    average_prices.reserve(iterations);
    payout.reserve(iterations);

    for(int i=0; i!=vec.size(); ++i){
        sum = 0.0;
        for(std::vector<double>::iterator it=vec[i].begin(); it!=vec[i].end(); ++it){
            sum += *it;
        }
        average_prices.emplace_back(sum / double(days_to_exp));
    }

    // get average price at maturity
    sum = 0.0;
    for(int i=0; i!=vec.size(); ++i){
        sum += vec[i].back();
    }
    maturity_price = sum / double(iterations);

    // determine average payout & discount back
    sum = 0.0;
    for(std::vector<double>::iterator it=average_prices.begin(); it!=average_prices.end(); ++it){
        sum += std::max((*it * strike) - maturity_price, 0.0);
    }
    return (sum / double(iterations)) * std::exp(-risk_free_rate * (double(days_to_exp) / 252.0));
}

double american_put_longstaff_schwartz(std::vector<double> &data_underlying, const double strike, const double risk_free_rate, const int days_to_exp, const int iterations){
    std::vector<double> price;
    double iter_cont;  // continuation value for this particular iteration
    std::vector<std::vector<double> > sim_vec;
    std::vector<std::vector<double> > cfm; // cash flow matrix
    std::vector<double> day_cash; // cash flow for one day
    day_cash.reserve(iterations);

    double one_day_discount = discount(risk_free_rate, 1);

    // run Monte Carlo on underlying asset
    monte_carlo(data_underlying, sim_vec, days_to_exp, iterations);

    // work backwards, starting with maturity date
    for(std::vector<std::vector<double>>::iterator it=sim_vec.begin(); it!=sim_vec.end(); ++it){
        day_cash.emplace_back(std::max(it->back() - strike, 0.0));
    }
    cfm.emplace_back(day_cash);

    for(int i=days_to_exp-2; i!=-1; --i){
        // iterate backward
        for(int x=0; x!=iterations; ++x){
            day_cash[x] = std::max(sim_vec[x][i] - strike, 0.0);
        }

        std::vector<double> xs;
        std::vector<double> ys;
        std::vector<double> continuation;
        xs.reserve(iterations);
        ys.reserve(iterations);
        continuation.reserve(iterations);

        for(int x=0; x!=iterations; ++x){
            if(day_cash[x] > 0.0){
                xs.emplace_back(sim_vec[x][i]);
                ys.emplace_back(cfm[0][x] * one_day_discount);
            }
        }

        // get 2nd order polynomial
        Polynomial poly(xs, ys, 1, 2);

        // get continuation value
        for(int x=0; x!=iterations; ++x){
            if(day_cash[x] > 0.0){
                // option is in the money
                price = {sim_vec[x][i]};
                iter_cont = poly.eval(price) * one_day_discount;
                if(iter_cont > day_cash[x]){
                    // continuation value is greater than cash flow from exercising the option today
                    // set today's cash flow to zero
                    day_cash[x] = 0.0;
                }
                else{
                    // exercise option today!
                    // set future cash flows to zero
                    for(int q=0; q!=cfm.size(); ++q){
                        cfm[q][x] = 0.0;
                    }
                }
            }
            else{
                // option is out of the money, don't exercise today
                // set today's cash flow to zero
                day_cash[x] = 0.0;
            }
        }

        // exercise data to cash flow matrix
        std::vector<std::vector<double>>::iterator insert_front = cfm.begin();
        cfm.insert(insert_front, day_cash);
    }

    double sum = 0.0;
    int count = 0;
    for(int i=0; i!=iterations; ++i){
        for(int x=0; x!=days_to_exp; ++x){
            if(cfm[x][i] > 0.0){
                sum += cfm[x][i] * discount(risk_free_rate, x+1);
                ++count;
                break;
            }
        }
    }
    // average discounted cash flows
    return sum/double(count);
}

double american_call_longstaff_schwartz(std::vector<double> &data_underlying, const double strike, const double risk_free_rate, const int days_to_exp, const int iterations){
    std::vector<double> price;
    double iter_cont;  // continuation value for this particular iteration
    std::vector<std::vector<double> > sim_vec;
    std::vector<std::vector<double> > cfm; // cash flow matrix
    std::vector<double> day_cash; // cash flow for one day
    day_cash.reserve(iterations);

    double one_day_discount = discount(risk_free_rate, 1);

    // run Monte Carlo on underlying asset
    monte_carlo(data_underlying, sim_vec, days_to_exp, iterations);

    // work backwards, starting with maturity date
    for(std::vector<std::vector<double>>::iterator it=sim_vec.begin(); it!=sim_vec.end(); ++it){
        day_cash.emplace_back(std::max(strike - it->back(), 0.0));
    }
    cfm.emplace_back(day_cash);

    for(int i=days_to_exp-2; i!=-1; --i){
        // iterate backward
        for(int x=0; x!=iterations; ++x){
            day_cash[x] = std::max(strike - sim_vec[x][i], 0.0);
        }

        std::vector<double> xs;
        std::vector<double> ys;
        std::vector<double> continuation;
        xs.reserve(iterations);
        ys.reserve(iterations);
        continuation.reserve(iterations);

        for(int x=0; x!=iterations; ++x){
            if(day_cash[x] > 0.0){
                xs.emplace_back(sim_vec[x][i]);
                ys.emplace_back(cfm[0][x] * one_day_discount);
            }
        }

        // get 2nd order polynomial
        Polynomial poly(xs, ys, 1, 2);

        // get continuation value
        for(int x=0; x!=iterations; ++x){
            if(day_cash[x] > 0.0){
                // option is in the money
                price = {sim_vec[x][i]};
                iter_cont = poly.eval(price) * one_day_discount;
                if(iter_cont > day_cash[x]){
                    // continuation value is greater than cash flow from exercising the option today
                    // set today's cash flow to zero
                    day_cash[x] = 0.0;
                }
                else{
                    // exercise option today!
                    // set future cash flows to zero
                    for(int q=0; q!=cfm.size(); ++q){
                        cfm[q][x] = 0.0;
                    }
                }
            }
            else{
                // option is out of the money, don't exercise today
                // set today's cash flow to zero
                day_cash[x] = 0.0;
            }
        }

        // exercise data to cash flow matrix
        std::vector<std::vector<double>>::iterator insert_front = cfm.begin();
        cfm.insert(insert_front, day_cash);
    }

    double sum = 0.0;
    int count = 0;
    for(int i=0; i!=iterations; ++i){
        for(int x=0; x!=days_to_exp; ++x){
            if(cfm[x][i] > 0.0){
                sum += cfm[x][i] * discount(risk_free_rate, x+1);
                ++count;
                break;
            }
        }
    }
    // average discounted cash flows
    return sum/double(count);
}


#include <catch.hpp>
#include <typeclass/eq/list.h>
#include <typeclass/eq/optional.h>
#include <typeclass/eq/vector.h>
#include <typeclass/eq/scalar.h>
#include <typeclass/monoid/list.h>
#include <typeclass/monoid/vector.h>
#include <typeclass/monoid/scalar.h>
#include <boost/optional.hpp>

TEST_CASE("typeclass monoid") {
    using namespace funcpp::typeclass::monoid;
    using namespace funcpp::typeclass::monoid::operators;
    using namespace funcpp::typeclass::eq;
    using namespace funcpp::typeclass::eq::operators;

    GIVEN("two lists of ints") {
        std::list<int> a{1,2,3}, b{4,5,6,7};

        THEN("Addition equal concatenation") {
            REQUIRE((std::list<int>{1,2,3,4,5,6,7} == a + b));
            REQUIRE((std::list<int>{1,2,3,4,5,6,7} == mappend(a,b)));
        }
    }

    GIVEN("a list of ints") {
        std::list<int> a{1,2,3};

        THEN("Adding mempty() doesn't change anything") {
            REQUIRE((a == a + mempty<std::list<int>>()    ));
            REQUIRE((a ==     mempty<std::list<int>>() + a));

            REQUIRE( a == mappend(a, mempty<std::list<int>>()    ));
            REQUIRE( a == mappend(   mempty<std::list<int>>(),  a));
        }
    }

    GIVEN("two vectors of ints") {
        std::vector<int> a{1,2,3}, b{4,5,6,7};

        THEN("Addition equal concatenation") {
            REQUIRE((std::vector<int>{1,2,3,4,5,6,7} == a + b));
            REQUIRE((std::vector<int>{1,2,3,4,5,6,7} == mappend(a,b)));
        }
    }

    GIVEN("a vectors of ints") {
        std::vector<int> a{1,2,3};

        THEN("Adding mempty() doesn't change anything") {
            REQUIRE((a == a + mempty<std::vector<int>>()    ));
            REQUIRE((a ==     mempty<std::vector<int>>() + a));

            REQUIRE( a == mappend(a, mempty<std::vector<int>>()    ));
            REQUIRE( a == mappend(   mempty<std::vector<int>>(),  a));
        }
    }

    GIVEN("two ints") {
        int a = 7, b = 10;

        THEN("mappend(a,b) is just the sum a+b") {
            REQUIRE(a+b == mappend(a,b));
        }

        THEN("mappend(a,mempty()) is just a") {
            REQUIRE(a == mappend(a,mempty<int>()));
            REQUIRE(a == mappend(mempty<int>(),a));
        }

        THEN("Multiplication a*b is the same as mappend<int,std::multiplies<>>(a,b)") {
            REQUIRE(a*b == (mappend<int,std::multiplies<>>(a,b)));
        }
    }
}


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

namespace MyDL
{

    double cross_entropy_error(MatrixXd &y, MatrixXd &t)
    {
        int batch_size = y.rows();
        double loss = -(t.array() * y.array().log()).sum() / batch_size;
        return loss;
    }

}


#include <Eigen/Dense>

#include <iostream>

#include "refill/distributions/gaussian_distribution.h"
#include "refill/filters/extended_kalman_filter.h"
#include "refill/measurement_models/linear_measurement_model.h"
#include "refill/system_models/linear_system_model.h"

/*
 * This is an example program for using Refill to estimate the 3D position
 * using a constant position model and assuming measurements are 3D position
 * measurements of the real position.
 *
 * The system model can then be written as:
 *
 * x(k) = I * x(k-1) + v(k)
 *
 * with
 *
 * x(k) element of R^3
 *
 * I = Identity matrix element of R^3x3
 *
 * v(k) random variable distributed with N(0, dt * Q)
 *
 * where Q element of R^3x3 is the system model covariance.
 *
 * The measurement model can be written as:
 *
 * y(k) = I * x(k) + w(k)
 *
 * with
 *
 * w(k) ~ N(0, R)
 *
 * where R element of R^3x3 is the measurement model covariance.
 */

int main(int argc, char **argv) {
  /* initialize Q and R
   * Q = I * 2.0
   * R = I */
  Eigen::Matrix3d system_noise_cov = Eigen::Matrix3d::Identity() * 2.0;
  Eigen::Matrix3d measurement_noise_cov = Eigen::Matrix3d::Identity() * 1.0;

  /* initialize v(k) */
  refill::GaussianDistribution system_noise(Eigen::Vector3d::Zero(),
                                            system_noise_cov);
  /* initialize w(k) */
  refill::GaussianDistribution measurement_noise(
      Eigen::Vector3d::Zero(), measurement_noise_cov);

  /* initialize the system model */
  refill::LinearSystemModel system_model(Eigen::Matrix3d::Identity(),
                                         system_noise);
  /* initialize the measurement model */
  refill::LinearMeasurementModel measurement_model(Eigen::Matrix3d::Identity(),
                                                   measurement_noise);

  /* initialize the initial state distribution
   * Assumed to be at position [1, 1, 1]^T with cov[I * 5] */
  refill::GaussianDistribution initial_state(Eigen::Vector3d::Ones(),
                                             Eigen::Matrix3d::Identity() * 5.0);
  /* initialize the kf with the initial state */
  refill::ExtendedKalmanFilter ekf(initial_state);

  /* assume that 1.0 seconds has passed and we get a position measurement at
   * [1.5, 1.5, 1.5]^T
   * t = 1.0 */
  double dt = 1.0;
  Eigen::Vector3d measurement = Eigen::Vector3d::Constant(1.5);

  /* adapt the system model noise according to the time step */
  system_noise.setCov(system_noise_cov * dt);
  /* adapt the system model */
  system_model.setModelParameters(Eigen::Matrix3d::Identity(), system_noise);

  /* predict the kf to the current time */
  ekf.predict(system_model);
  /* update the kf with the measurement and the measurement model */
  ekf.update(measurement_model, measurement);

  /* print the current state */
  std::cout << "State at t = 1.0:\n";
  std::cout << "Mean:\n\n" << ekf.state().mean() << "\n\n";
  std::cout << "Covariance:\n\n" << ekf.state().cov() << "\n\n";

  /* Assume that another 0.5 seconds have passed and another measurement
   * is received
   * t = 1.5 */
  dt = 0.5;
  measurement = Eigen::Vector3d::Constant(1.5);

  // adapt the system noise according to the time step
  system_noise.setCov(system_noise_cov * dt);
  // adapt the system model
  system_model.setModelParameters(Eigen::Matrix3d::Identity(), system_noise);

  /* predict the kf to the current time */
  ekf.predict(system_model);
  /* update the kf with the measurement and the measurement model */
  ekf.update(measurement_model, measurement);

  /* print the current state */
  std::cout << "State at t = 1.5:\n";
  std::cout << "Mean:\n\n" << ekf.state().mean() << "\n\n";
  std::cout << "Covariance:\n\n" << ekf.state().cov() << "\n\n";

  return 0;
}


/*
 * Copyright (c) 2015, Jonathan Ventura
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef POLYNOMIAL_HPP
#define POLYNOMIAL_HPP

#include <Eigen/Core>
#include <unsupported/Eigen/Polynomials>
#include <vector>

#include <Polynomial/PolynomialInternal.hpp>

namespace polynomial
{
    /**
     * Polynomial
     *
     * Templated class for representing a polynomial expression.
     *
     * The template parameter indicates the degree of polynomial.  An n-th degree polynomial has n+1 coefficients.
     *
     * The coefficients are stored in this order:
     * c0 x^deg + c1 x^(deg-1) + ...
     *
     * A dynamically-sized version is available: Polynomial<Eigen::Dynamic>
     *
     * Note that the static and dynamic versions should not be mixed, i.e. do not add a dynamic polynomial to a static one.
     */
    template<int deg>
    class Polynomial
    {
        Eigen::Matrix<double,deg+1,1> coef;
    public:
        Polynomial()
        : coef( Eigen::Matrix<double,1,deg+1>::Zero() )
        {

        }

        Polynomial( const Eigen::Matrix<double,deg+1,1> &coefin )
        : coef( coefin )
        {

        }

        Polynomial( const Polynomial<deg> &polyin )
        : coef( polyin.coef )
        {

        }

        Polynomial( const double *coefin )
        : coef( Internal::vecmap<deg+1>( coefin ) )
        {

        }

        const Eigen::Matrix<double,deg+1,1> &coefficients() const
        {
            return coef;
        }

        Eigen::Matrix<double,deg+1,1> &coefficients()
        {
            return coef;
        }

        template<int degin>
        Polynomial<Internal::max<degin,deg>::value> operator+(const Polynomial<degin> &poly) const
        {
            Polynomial<Internal::max<degin,deg>::value> p;
            p.coefficients().tail(degin+1) = poly.coefficients();
            p.coefficients().tail(deg+1) += coef;
            return p;
        }

        template<int degin>
        Polynomial<Internal::max<degin,deg>::value> operator-(const Polynomial<degin> &poly) const
        {
            Polynomial<Internal::max<degin,deg>::value> p;
            p.coefficients().tail(deg+1) = coef;
            p.coefficients().tail(degin+1) -= poly.coefficients();
            return p;
        }

        template<int degin>
        Polynomial<degin+deg> operator*(const Polynomial<degin> &poly) const
        {
            Polynomial<degin+deg> p;
            Internal::PolyConv<deg,degin>::compute(p.coefficients(),coef,poly.coefficients());
            return p;
        }

        Polynomial<deg> operator*(const double c) const
        {
            return Polynomial<deg>(coef*c);
        }

        double eval(double x) const
        {
            return Internal::PolyVal<deg>::compute(coef,x);
        }

        void realRoots(std::vector<double> &roots) const
        {
            if ( coef[0] == 0 )
            {
                Internal::RootFinder<deg-1>::compute(coef.tail(deg),roots);
            } else {
                Internal::RootFinder<deg>::compute(coef,roots);
            }
        }

        void realRootsSturm(const double lb, const double ub, std::vector<double> &roots) const
        {
            if ( coef[0] == 0 )
            {
                Internal::SturmRootFinder<deg-1> sturm( coef.tail(deg) );
                sturm.realRoots( lb, ub, roots );
            } else {
                Internal::SturmRootFinder<deg> sturm( coef );
                sturm.realRoots( lb, ub, roots );
            }
        }

        void rootBounds( double &lb, double &ub )
        {
            Eigen::Matrix<double,deg,1> mycoef = coef.tail(deg).array().abs();
            mycoef /= fabs(coef(0));
            mycoef(0) += 1.;
            ub = mycoef.maxCoeff();
            lb = -ub;
        }
    };

    template <>
    class Polynomial<Eigen::Dynamic>
    {
        Eigen::VectorXd coef;
    public:
        Polynomial(const int deg)
        : coef( Eigen::VectorXd::Zero(deg+1) )
        {

        }

        Polynomial( const Eigen::VectorXd &coefin)
        : coef( coefin )
        {

        }

        Polynomial(const Polynomial<Eigen::Dynamic> &polyin)
        : coef( polyin.coef )
        {

        }

        const Eigen::VectorXd &coefficients() const
        {
            return coef;
        }

        Eigen::VectorXd &coefficients()
        {
            return coef;
        }

        Polynomial<Eigen::Dynamic> operator+(const Polynomial<Eigen::Dynamic> &poly) const
        {
            int deg = coef.rows()-1;
            int degin = poly.coef.rows()-1;
            Polynomial<Eigen::Dynamic> p( std::max(deg,degin) );
            p.coef.tail(degin+1) = poly.coef;
            p.coef.tail(deg+1) += coef;
            return p;
        }

        Polynomial<Eigen::Dynamic> operator-(const Polynomial<Eigen::Dynamic> &poly) const
        {
            int deg = coef.rows()-1;
            int degin = poly.coef.rows()-1;
            Polynomial<Eigen::Dynamic> p( std::max(deg,degin) );
            p.coef.tail(deg+1) = coef;
            p.coef.tail(degin+1) -= poly.coef;
            return p;
        }

        Polynomial<Eigen::Dynamic> operator*(const Polynomial<Eigen::Dynamic> &poly) const
        {
            int deg = coef.rows()-1;
            int degin = poly.coef.rows()-1;
            Polynomial<Eigen::Dynamic> p( deg+degin );
            Internal::PolyConv<Eigen::Dynamic,Eigen::Dynamic>::compute(p.coef,coef,poly.coef);
            return p;
        }

        Polynomial<Eigen::Dynamic> operator*(const double c) const
        {
            return Polynomial<Eigen::Dynamic>(coef*c);
        }

        double eval(double x) const
        {
            return Internal::PolyVal<Eigen::Dynamic>::compute(coef,x);
        }

        void realRoots(std::vector<double> &roots) const
        {
            if ( coef[0] == 0 )
            {
                int deg = coef.rows()-1;
                Internal::RootFinder<Eigen::Dynamic>::compute(coef.tail(deg),roots);
            } else {
                Internal::RootFinder<Eigen::Dynamic>::compute(coef,roots);
            }
        }

        void realRootsSturm(const double lb, const double ub, std::vector<double> &roots) const
        {
            if ( coef[0] == 0 )
            {
                int deg = coef.rows()-1;
                Internal::SturmRootFinder<Eigen::Dynamic> sturm( coef.tail(deg) );
                sturm.realRoots( lb, ub, roots );
            } else {
                Internal::SturmRootFinder<Eigen::Dynamic> sturm( coef );
                sturm.realRoots( lb, ub, roots );
            }
        }

        void rootBounds( double &lb, double &ub )
        {
            int deg = coef.rows()-1;
            Eigen::VectorXd mycoef = coef.tail(deg).array().abs()/fabs(coef(0));
            mycoef(0) += 1.;
            ub = mycoef.maxCoeff();
            lb = -ub;
        }
    };

} // end namespace Polynomial

#endif


#include <vector>
#include <Eigen/Dense>
#include <GraphMol/PeriodicTable.h>

#include "AdjMatrix.h"
#include "Mol.h"

double AtomDistance(Atom A, Atom B) {
    double dist = std::sqrt(
        (A.x - B.x)*(A.x - B.x) +
        (A.y - B.y)*(A.y - B.y) +
        (A.z - B.z)*(A.z - B.z)
    );
    return dist;
}

Eigen::MatrixXd DistanceMatrix(std::vector<Atom> atoms) {

    Eigen::MatrixXd distM(atoms.size(), atoms.size());

    for (int i = 0; i < atoms.size(); i++) {
        for (int j = i; j < atoms.size(); j++) {
            distM(j, i) = AtomDistance(atoms[i], atoms[j]);
            distM(i, j) = distM(j, i);
        }
    }
    return distM;
}

Eigen::MatrixXi AdjacencyMatrixDist(const std::vector<Atom> atoms, double covalentFactor) {

    RDKit::PeriodicTable *tbl = RDKit::PeriodicTable::getTable();

    Eigen::MatrixXi AC = Eigen::MatrixXi::Zero(atoms.size(), atoms.size());
    Eigen::MatrixXd distM = DistanceMatrix(atoms);

    for (int i = 0; i < atoms.size(); i++) {
        for (int j = i + 1; j < atoms.size(); j++) {
            double atomIRcov = tbl->getRcovalent(atoms[i].symbol) * covalentFactor;
            double atomJRcov = tbl->getRcovalent(atoms[j].symbol) * covalentFactor;

            if (distM(i, j) <= atomIRcov + atomJRcov) {
                AC(i, j) = 1;
                AC(j, i) = 1;
            }
        }
    }
    return AC;
}


//#include <Eigen/SVD>
#include <algorithm> // std::sort, std::stable_sort
#include <eigen3/Eigen/SVD>
#include <itkDataObject.h>
#include <itkImageRegionIterator.h>
#include <itkIndex.h>
#include <itkVariableLengthVector.h>
#include <itkVectorImage.h>
#include <m2CoreCommon.h>
#include <m2PcaImageFilter.h>
#include <m2Timer.h>
#include <mitkIOUtil.h>
#include <mitkImage.h>
#include <mitkImageAccessByItk.h>
#include <mitkImageCast.h>
#include <mitkImagePixelReadAccessor.h>
#include <numeric> // std::iota
#include <vnl/vnl_matrix.h>
#include <boost/progress.hpp>

void m2::PcaImageFilter::initMatrix()
{
  // this->GetValidIndices();
  auto input = this->GetIndexedInputs();
  auto mitkImage = dynamic_cast<mitk::Image *>(input.front().GetPointer());
  size_t pixels = 1;
  for (unsigned int i = 0; i < mitkImage->GetDimension(); ++i)
    pixels *= mitkImage->GetDimensions()[i];
  const unsigned long numberOfrow = pixels;
  const unsigned long numberOfcolumn = this->GetIndexedInputs().size();

  this->m_DataMatrix.resize(numberOfrow, numberOfcolumn);
  unsigned int c = 0;
  boost::progress_display p(numberOfcolumn);

  /*Fill matrix with image values one column includes values of one image*/
  for (auto it = input.begin(); it != input.end(); ++it, ++c)
  {
    mitkImage = dynamic_cast<mitk::Image *>(it->GetPointer());
    mitk::ImagePixelReadAccessor<m2::DisplayImagePixelType, 3> access(mitkImage);
    std::copy(access.GetData(), access.GetData() + pixels, m_DataMatrix.col(c).data());
    ++p;
  }

  // auto maxCoeffs = m_DataMatrix.colwise().maxCoeff();
  // auto minCoeffs = m_DataMatrix.colwise().minCoeff();
  // auto scalingFactors = maxCoeffs - minCoeffs;
  // m_DataMatrix = ((m_DataMatrix.rowwise() - minCoeffs).array().rowwise() / scalingFactors.array()).matrix();

  // auto means = m_DataMatrix.colwise().mean();
  // m_DataMatrix = m_DataMatrix.rowwise() - means;
  // m_DataMatrix /= (m_DataMatrix.rows() - 1);

  // for (unsigned int c = 0; c < m_DataMatrix.cols(); ++c)
  // m_DataMatrix.col(c) = m_DataMatrix.col(c) / stdDevs[c];
}

Eigen::MatrixXf m2::PcaImageFilter::GetEigenImageMatrix(){
  return m_EigenImageMatrix;
}

Eigen::VectorXf m2::PcaImageFilter::GetMeanImage(){
  return m_MeanImage;
}

void m2::PcaImageFilter::GenerateData()
{
  auto timer = m2::Timer("PCA - Generate data ...");
  this->initMatrix();

  // eigenionimages
  m_MeanImage = m_DataMatrix.rowwise().mean();
  Eigen::MatrixXf eigenionData = m_DataMatrix.colwise() - m_MeanImage;

  Eigen::JacobiSVD<Eigen::MatrixXf> svd(eigenionData, Eigen::ComputeThinU | Eigen::ComputeThinV);
  MITK_INFO << "S size: " << svd.singularValues().rows() << " " << svd.singularValues().cols();
  MITK_INFO << "U size: " << svd.matrixU().rows() << " " << svd.matrixU().cols();
  MITK_INFO << "V size: " << svd.matrixV().rows() << " " << svd.matrixV().cols();
  MITK_INFO << "m_DataMatrix: " << m_DataMatrix.rows() << " " << m_DataMatrix.cols();

  const Eigen::MatrixXf &U = svd.matrixU();
  m_EigenImageMatrix = U;

  auto eigenIonVectorImage = initializeItkVectorImage(m_NumberOfComponents);
  m2::DisplayImagePixelType *data;
  data = eigenIonVectorImage->GetBufferPointer();

  for (unsigned int c = 0; c < m_NumberOfComponents; ++c)
  {
    const auto &col = U.col(c);
    for (unsigned int p = 0; p < U.rows(); ++p)
    {
      data[p * m_NumberOfComponents + c] = col(p);
    }
  }

  // feature pca
  Eigen::MatrixXf pcaData = m_DataMatrix.rowwise() - m_DataMatrix.colwise().mean();
  Eigen::MatrixXf cov = (pcaData.transpose() * pcaData) / (pcaData.rows() - 1);

  Eigen::EigenSolver<Eigen::MatrixXf> solver(cov);
  Eigen::VectorXf values = solver.eigenvalues().real();
  Eigen::MatrixXf vectors = solver.eigenvectors().real();
  Eigen::MatrixXf vectorsSorted(vectors);
  // fill indices
  std::vector<unsigned int> indices(values.size());
  std::iota(indices.begin(), indices.end(), 0);
  // sort indices according to
  std::stable_sort(indices.begin(), indices.end(), [&values](auto i1, auto i2) { return values[i1] > values[i2]; });

  unsigned int i = 0;
  for (auto u : indices)
    vectorsSorted.col(i++) = vectors.col(u);

  Eigen::MatrixXf pc = pcaData * vectorsSorted;
  auto pcVectorImage = initializeItkVectorImage(m_NumberOfComponents);
  data = pcVectorImage->GetBufferPointer();

  for (unsigned int c = 0; c < m_NumberOfComponents; ++c)
  {
    const auto &col = pc.col(c);
    for (unsigned int p = 0; p < pc.rows(); ++p)
    {
      data[p * m_NumberOfComponents + c] = col(p);
    }
  }

  mitk::Image::Pointer eigenIonImage = this->GetOutput(0);
  mitk::CastToMitkImage(eigenIonVectorImage, eigenIonImage);
  eigenIonImage->SetSpacing(this->GetInput()->GetGeometry()->GetSpacing());
  eigenIonImage->SetOrigin(this->GetInput()->GetGeometry()->GetOrigin());

  mitk::Image::Pointer pcImage = this->GetOutput(1);
  mitk::CastToMitkImage(pcVectorImage, pcImage);
  pcImage->SetSpacing(this->GetInput()->GetGeometry()->GetSpacing());
  pcImage->SetOrigin(this->GetInput()->GetGeometry()->GetOrigin());

}


// test camera intrinsic calibration

#include <minisam/3rdparty/Catch2/catch.hpp>
#include <minisam/utils/testAssertions.h>
#include <minisam/nonlinear/numericalJacobian.h>

#include <minisam/geometry/CalibK.h>
#include <minisam/geometry/CalibKD.h>
#include <minisam/geometry/CalibBundler.h>

#include <Eigen/Dense>  // inverse

using namespace std;
using namespace Eigen;
using namespace minisam;

// wrapper for numerical jacobians
template<class CALIBRATION>
Vector2d wrapper_project(const CALIBRATION& K, const Vector2d& p) {
  return K.project(p);
}
template<class CALIBRATION>
Vector2d wrapper_unproject(const CALIBRATION& K, const Vector2d& p) {
  return K.unproject(p);
}

// static test vars
CalibK ucal1(1, 1, 0, 0);           // identity
CalibK ucal2(100, 100, 300, 200);   // non-identity
CalibK ucal3(37.4, 28.9, 107.1, -223.3);  // random non-identity

CalibKD dcal1(1, 1, 0, 0, 0, 0, 0 ,0);                // identity
CalibKD dcal2(100, 100, 300, 200, 0.1, 0.01, 0, 0);   // non-identity
CalibKD dcal3(37.4, 28.9, 107.1, -223.3, 0.089, 0.063, -0.03, 0.08);  // random non-identity
CalibKD dcal4(100, 100, 300, 200, -0.01, 0.001, 0, 0);   // non-identity

CalibBundler bcal1(1, 0 ,0);              // identity
CalibBundler bcal2(100, -0.01, 0.001);    // non-identity
CalibBundler bcal3(37.4, 0.089, 0.063);   // random non-identity

/* ************************************************************************** */
TEST_CASE("CalibKTraits", "[geometry]") {
  CHECK(has_traits<CalibK>::value);
  CHECK(is_manifold<CalibK>::value);
  CHECK_FALSE(is_lie_group<CalibK>::value);
}

/* ************************************************************************** */
TEST_CASE("CalibKConstructor", "[geometry]") {
  CHECK(assert_equal(CalibK((Vector4d() << 1, 2, 3, 4).finished()), CalibK(1, 2, 3, 4)));
}

/* ************************************************************************** */
TEST_CASE("CalibKMatrix", "[geometry]") {

  Matrix3d K2, K3;
  K2 << 100, 0, 300, 0, 100, 200, 0, 0, 1;
  K3 << 37.4, 0, 107.1, 0, 28.9, -223.3, 0, 0, 1;

  CHECK(assert_equal_matrix(Matrix3d::Identity(), ucal1.matrix()));
  CHECK(assert_equal_matrix(Matrix3d::Identity(), ucal1.inverse_matrix()));

  CHECK(assert_equal_matrix(K2, ucal2.matrix()));
  CHECK(assert_equal_matrix(K2.inverse(), ucal2.inverse_matrix()));

  CHECK(assert_equal_matrix(K3, ucal3.matrix()));
  CHECK(assert_equal_matrix(K3.inverse(), ucal3.inverse_matrix()));
}

/* ************************************************************************** */
TEST_CASE("CalibKc2i", "[geometry]") {

  Vector2d pc1 = Vector2d(0, 0);
  Vector2d pc3 = Vector2d(23.4, 16.2);

  // point
  CHECK(assert_equal(pc1, ucal1.project(pc1)));
  CHECK(assert_equal(pc3, ucal1.project(pc3)));

  CHECK(assert_equal(Vector2d(300, 200), ucal2.project(pc1)));
  CHECK(assert_equal(Vector2d(2640, 1820), ucal2.project(pc3)));

  // jacobians
  Eigen::Matrix<double, 2, 4> J_K_actual;
  Eigen::Matrix<double, 2, 2> J_p_actual;

  ucal1.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibK>, ucal1, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibK>, ucal1, pc1), J_p_actual));
  ucal1.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibK>, ucal1, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibK>, ucal1, pc3), J_p_actual));

  ucal2.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibK>, ucal2, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibK>, ucal2, pc1), J_p_actual));
  ucal2.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibK>, ucal2, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibK>, ucal2, pc3), J_p_actual));

  ucal3.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibK>, ucal3, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibK>, ucal3, pc1), J_p_actual));
  ucal3.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibK>, ucal3, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibK>, ucal3, pc3), J_p_actual));
}

/* ************************************************************************** */
TEST_CASE("CalibKi2c", "[geometry]") {

  Vector2d pi1 = Vector2d(300, 200);
  Vector2d pi3 = Vector2d(2640, 1820);

  // point
  CHECK(assert_equal(pi1, ucal1.unproject(pi1)));
  CHECK(assert_equal(pi3, ucal1.unproject(pi3)));

  CHECK(assert_equal(Vector2d(0, 0), ucal2.unproject(pi1)));
  CHECK(assert_equal(Vector2d(23.4, 16.2), ucal2.unproject(pi3)));

  // jacobians
  Eigen::Matrix<double, 2, 4> J_K_actual;
  Eigen::Matrix<double, 2, 2> J_p_actual;

  ucal1.unprojectJacobians(pi1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_unproject<CalibK>, ucal1, pi1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_unproject<CalibK>, ucal1, pi1), J_p_actual));
  ucal1.unprojectJacobians(pi3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_unproject<CalibK>, ucal1, pi3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_unproject<CalibK>, ucal1, pi3), J_p_actual));

  ucal2.unprojectJacobians(pi1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_unproject<CalibK>, ucal2, pi1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_unproject<CalibK>, ucal2, pi1), J_p_actual));
  ucal2.unprojectJacobians(pi3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_unproject<CalibK>, ucal2, pi3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_unproject<CalibK>, ucal2, pi3), J_p_actual));

  ucal3.unprojectJacobians(pi1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_unproject<CalibK>, ucal3, pi1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_unproject<CalibK>, ucal3, pi1), J_p_actual));
  ucal3.unprojectJacobians(pi3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_unproject<CalibK>, ucal3, pi3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_unproject<CalibK>, ucal3, pi3), J_p_actual));
}

/* ************************************************************************** */
TEST_CASE("CalibKDTraits", "[geometry]") {
  CHECK(has_traits<CalibKD>::value);
  CHECK(is_manifold<CalibKD>::value);
  CHECK_FALSE(is_lie_group<CalibKD>::value);
}

/* ************************************************************************** */
TEST_CASE("CalibKDConstructor", "[geometry]") {
  CHECK(assert_equal(CalibKD((VectorXd(8) << 1, 2, 3, 4, 5, 6, 7, 8).finished()),
      CalibKD(1, 2, 3, 4, 5, 6, 7, 8)));
}

/* ************************************************************************** */
TEST_CASE("CalibKDc2i", "[geometry]") {

  Vector2d pc1 = Vector2d(0, 0);
  Vector2d pc3 = Vector2d(2.4, 1.3);

  // point
  CHECK(assert_equal(pc1, dcal1.project(pc1)));
  CHECK(assert_equal(pc3, dcal1.project(pc3)));

  CHECK(assert_equal(Vector2d(300, 200), dcal2.project(pc1)));
  CHECK(assert_equal(Vector2d(852.006, 499.00325), dcal2.project(pc3)));

  CHECK(assert_equal(Vector2d(300, 200), dcal4.project(pc1)));
  CHECK(assert_equal(Vector2d(535.4406, 327.530325), dcal4.project(pc3)));

  // jacobians
  Eigen::Matrix<double, 2, 8> J_K_actual;
  Eigen::Matrix<double, 2, 2> J_p_actual;

  dcal1.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal1, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal1, pc1), J_p_actual));
  dcal1.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal1, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal1, pc3), J_p_actual));

  dcal2.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal2, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal2, pc1), J_p_actual));
  dcal2.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal2, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal2, pc3), J_p_actual));

  dcal3.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal3, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal3, pc1), J_p_actual));
  dcal3.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal3, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal3, pc3), J_p_actual));

  dcal4.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal4, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal4, pc1), J_p_actual));
  dcal4.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibKD>, dcal4, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibKD>, dcal4, pc3), J_p_actual));
}

/* ************************************************************************** */
TEST_CASE("CalibKDi2c", "[geometry]") {

  Vector2d pi1 = Vector2d(300, 200);
  Vector2d pi3 = Vector2d(535.4406, 327.530325);

  CHECK(assert_equal(pi1, dcal1.unproject(pi1), 1e-6));
  CHECK(assert_equal(pi3, dcal1.unproject(pi3), 1e-6));

  CHECK(assert_equal(Vector2d(0, 0), dcal4.unproject(pi1), 1e-6));
  CHECK(assert_equal(Vector2d(2.4, 1.3), dcal4.unproject(pi3), 1e-6));
}

/* ************************************************************************** */
TEST_CASE("CalibBundlerTraits", "[geometry]") {
  CHECK(has_traits<CalibBundler>::value);
  CHECK(is_manifold<CalibBundler>::value);
  CHECK_FALSE(is_lie_group<CalibBundler>::value);
}

/* ************************************************************************** */
TEST_CASE("CalibBundlerConstructor", "[geometry]") {
  CHECK(assert_equal(CalibBundler((VectorXd(3) << 1, 2, 3).finished()),
      CalibBundler(1, 2, 3)));
}

/* ************************************************************************** */
TEST_CASE("CalibBundlerc2i", "[geometry]") {

  Vector2d pc1 = Vector2d(0, 0);
  Vector2d pc3 = Vector2d(2.4, 1.3);

  // point
  CHECK(assert_equal(pc1, bcal1.project(pc1)));
  CHECK(assert_equal(pc3, bcal1.project(pc3)));

  CHECK(assert_equal(Vector2d(0, 0), bcal2.project(pc1)));
  CHECK(assert_equal(Vector2d(235.4406, 127.530325), bcal2.project(pc3)));

  // jacobians
  Eigen::Matrix<double, 2, 3> J_K_actual;
  Eigen::Matrix<double, 2, 2> J_p_actual;

  bcal1.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibBundler>, bcal1, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibBundler>, bcal1, pc1), J_p_actual));
  bcal1.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibBundler>, bcal1, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibBundler>, bcal1, pc3), J_p_actual));

  bcal2.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibBundler>, bcal2, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibBundler>, bcal2, pc1), J_p_actual));
  bcal2.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibBundler>, bcal2, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibBundler>, bcal2, pc3), J_p_actual));

  bcal3.projectJacobians(pc1, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibBundler>, bcal3, pc1), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibBundler>, bcal3, pc1), J_p_actual));
  bcal3.projectJacobians(pc3, J_K_actual, J_p_actual);
  CHECK(assert_equal_matrix(numericalJacobian21(wrapper_project<CalibBundler>, bcal3, pc3), J_K_actual));
  CHECK(assert_equal_matrix(numericalJacobian22(wrapper_project<CalibBundler>, bcal3, pc3), J_p_actual));
}

/* ************************************************************************** */
TEST_CASE("CalibBundleri2c", "[geometry]") {

  Vector2d pi1 = Vector2d(0, 0);
  Vector2d pi3 = Vector2d(235.4406, 127.530325);

  CHECK(assert_equal(pi1, bcal1.unproject(pi1), 1e-6));
  CHECK(assert_equal(pi3, bcal1.unproject(pi3), 1e-6));

  CHECK(assert_equal(Vector2d(0, 0), bcal2.unproject(pi1), 1e-6));
  CHECK(assert_equal(Vector2d(2.4, 1.3), bcal2.unproject(pi3), 1e-6));
}


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

void ExportMatrixToCsv(const std::string& file_path, const Eigen::MatrixXd& X)
{
    std::ofstream   file(file_path);
    Eigen::IOFormat format(Eigen::StreamPrecision, Eigen::DontAlignCols, ",");
    file << X.format(format);
}

int main()
{
    constexpr otf::FunctionType type       = otf::FunctionType::Sphere;
    constexpr int               num_dims   = 5;
    constexpr int               num_iters  = 15;
    constexpr int               num_trials = 2;

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

    const auto            objective_func = [&](const Eigen::VectorXd& x) { return -otf::GetValue(x, type); };
    const Eigen::VectorXd lower_bound    = Eigen::VectorXd::Constant(num_dims, -1.0);
    const Eigen::VectorXd upper_bound    = Eigen::VectorXd::Constant(num_dims, 1.0);

    Eigen::MatrixXd bo_result(num_iters, num_trials);

    for (int trial = 0; trial < num_trials; ++trial)
    {
        std::cout << "#trial: " << std::to_string(trial + 1) << std::endl;

        mathtoolbox::optimization::BayesianOptimizer optimizer(objective_func, lower_bound, upper_bound);

        for (int iter = 0; iter < num_iters; ++iter)
        {
            const auto new_point = optimizer.Step();

            const Eigen::VectorXd current_solution      = optimizer.GetCurrentOptimizer();
            const double          current_optimal_value = optimizer.EvaluatePoint(current_solution);

            std::cout << current_solution.transpose().format(Eigen::IOFormat(2));
            std::cout << " (" << current_optimal_value << ")" << std::endl;

            bo_result(iter, trial) = current_optimal_value;
        }
    }

    Eigen::MatrixXd rand_result(num_iters, num_trials);

    for (int trial = 0; trial < num_trials; ++trial)
    {
        std::cout << "#trial: " << std::to_string(trial + 1) << std::endl;

        Eigen::VectorXd current_solution;
        double          current_optimal_value;

        for (int iter = 0; iter < num_iters; ++iter)
        {
            const auto new_point = [&]() {
                const Eigen::VectorXd normalized_sample =
                    0.5 * (Eigen::VectorXd::Random(num_dims) + Eigen::VectorXd::Ones(num_dims));
                const Eigen::VectorXd sample =
                    (normalized_sample.array() * (upper_bound - lower_bound).array()).matrix() + lower_bound;

                return sample;
            }();

            const double new_value = objective_func(new_point);

            if (current_solution.size() == 0 || current_optimal_value < new_value)
            {
                current_solution      = new_point;
                current_optimal_value = new_value;
            }

            std::cout << current_solution.transpose().format(Eigen::IOFormat(2));
            std::cout << " (" << current_optimal_value << ")" << std::endl;

            rand_result(iter, trial) = current_optimal_value;
        }
    }

    const Eigen::VectorXd expected_solution = otf::GetSolution(num_dims, type);
    const double          expected_value    = objective_func(expected_solution);

    std::cout << "Expected solution: " << expected_solution.transpose() << " (" << expected_value << ")" << std::endl;

    ExportMatrixToCsv("./bo_result.csv", bo_result);
    ExportMatrixToCsv("./rand_result.csv", rand_result);

    return 0;
}


#pragma once

#include <array>
#include <Eigen/Core>

namespace Discregrid
{

enum class NearestEntity
{
	VN0, VN1, VN2, EN0, EN1, EN2, FN
};

double point_triangle_sqdistance(Eigen::Vector3d const& point,
	std::array<Eigen::Vector3d const*, 3> const& triangle,
	Eigen::Vector3d* nearest_point = nullptr,
	NearestEntity* ne = nullptr);

}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix
  Eigen::Matrix<double, 4, 4> elem_mat;
#if SOLUTION
  // Initialize matrix containing the mass part of the element matrix
  Eigen::Matrix<double, 4, 4> mass_elem_mat;
  // Retrieve laplace part of element matrix from LehrFEM's built-in laplace
  // element matrix builder
  lf::uscalfe::LinearFELaplaceElementMatrix laplace_elmat_builder;
  auto laplace_elem_mat = laplace_elmat_builder.Eval(cell);
  // Computations differ depending on the type of the cell
  switch (ref_el) {
    case lf::base::RefEl::kTria(): {
      double area = 0.5 * ((vertices(0, 1) - vertices(0, 0)) *
                               (vertices(1, 2) - vertices(1, 0)) -
                           (vertices(1, 1) - vertices(1, 0)) *
                               (vertices(0, 2) - vertices(0, 0)));
      // clang-format off
      mass_elem_mat << 2.0, 1.0, 1.0, 0.0,
	1.0, 2.0, 1.0, 0.0,
	1.0, 1.0, 2.0, 0.0,
	0.0, 0.0, 0.0, 0.0;
      // clang-format on
      mass_elem_mat *= area / 12.0;
      break;
    }
    case lf::base::RefEl::kQuad(): {
      double area =
          (vertices(0, 1) - vertices(0, 0)) * (vertices(1, 3) - vertices(1, 0));
      // clang-format off
      mass_elem_mat << 4.0, 2.0, 1.0, 2.0,
	2.0, 4.0, 2.0, 1.0,
	1.0, 2.0, 4.0, 2.0,
	2.0, 1.0, 2.0, 4.0;
      // clang-format on
      mass_elem_mat *= area / 36.0;
      break;
    }
    default: {
      LF_ASSERT_MSG(false, "Illegal cell type");
    }
  }  // end switch
  elem_mat = laplace_elem_mat + mass_elem_mat;
#else

  //====================
  // Your code goes here
  //====================

#endif
  return elem_mat;
}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


#pragma once

/* "THE BEER-WARE LICENSE" (Revision 42): Devin Lane wrote this file. As long as you retain
* this notice you can do whatever you want with this stuff. If we meet some day, and you
* think this stuff is worth it, you can buy me a beer in return.
https://shiftedbits.org/2011/01/30/cubic-spline-interpolation/

The code has been converted to use Armadillo data type for slabcc by https://github.com/MFTabriz

*/
#include <vector>
#include <iostream>
#include <armadillo>

template <typename X>
class Spline {
public:

	// A spline with x and y values
	Spline(const arma::Row<X>& x, const arma::Col<X>& y) {
		if (x.n_elem != y.n_elem) {
			std::cerr << "Spline: X and Y must be the same size!\n";
			return;
		}

		if (x.n_elem < 3) {
			std::cerr << "Spline: Must have at least three points for interpolation!\n";
			return;
		}

		const arma::uword n = y.n_elem - 1;

		arma::Row<X> b(n), d(n), a(n), c(n + 1), l(n + 1), u(n + 1), z(n + 1), h(n + 1);

		l(0) = 1;
		u(0) = 0; z(0) = 0;
		h(0) = x(1) - x(0);
		for (arma::uword i = 1; i < n; ++i) {
			h(i) = x(i + 1) - x(i);
			l(i) = 2 * (x(i + 1) - x(i - 1)) - h(i - 1) * u(i - 1);
			u(i) = h(i) / l(i);
			a(i) = (3 / h(i)) * (y(i + 1) - y(i)) - (3 / h(i - 1)) * (y(i) - y(i - 1));
			z(i) = (a(i) - h(i - 1) * z(i - 1)) / l(i);
		}
		l(n) = 1;
		z(n) = 0; c(n) = 0;
		for (arma::sword j = n - 1; j >= 0; --j) {
			c(j) = z(j) - u(j) * c(j + 1);
			b(j) = (y(j + 1) - y(j)) / h(j) - (h(j) * (c(j + 1) + 2 * c(j))) / 3;
			d(j) = (c(j + 1) - c(j)) / (3 * h(j));
		}
		for (arma::uword i = 0; i < n; ++i) {
			mElements.push_back(Element(x(i), y(i), b(i), c(i), d(i)));
		}
	}

	//return the value of the spline function for x
	X interpolate(const X&x) const {
		if (mElements.empty()) return X();

		auto it = std::lower_bound(mElements.begin(), mElements.end(), element_type(x));
		if (it != mElements.begin()) { --it; }
		return it->eval(x);
	}

	//return the value of the spline function for a Row
	arma::Col<X> interpolate(const arma::Row<X>& xx) const {
		if (mElements.empty()) return arma::Row<X>(xx.size());

		arma::Col<X> ys = arma::zeros<arma::Col<X>>(xx.n_elem);

		for (arma::uword it = 0; it < xx.n_elem; ++it) {
			ys(it) = interpolate(xx(it));
		}
		return ys;
	}

protected:

	class Element {
	public:

		X x = 0, a = 0, b = 0, c = 0, d = 0;

		Element(X _x) noexcept : x(_x) {}
		Element(X _x, X _a, X _b, X _c, X _d) noexcept
			: x(_x), a(_a), b(_b), c(_c), d(_d) {}

		X eval(const X& xx) const noexcept {
			X xix(xx - x);
			return a + b * xix + c * (xix * xix) + d * (xix * xix * xix);
		}

		bool operator<(const Element& e) const noexcept {
			return x < e.x;
		}
	};
	typedef Element element_type;
	std::vector<element_type> mElements;

};


#define _CRT_SECURE_NO_WARNINGS
#define _USE_MATH_DEFINES

#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <cmath>
#include <complex>
#include <utility>
#include <tuple>

#include "xtensor/xarray.hpp"
#include "xtensor/xio.hpp"
#include "xtensor/xview.hpp"
#include "xtensor/xmanipulation.hpp"
#include "xtensor-io/xnpz.hpp"
#include "xtensor-io/ximage.hpp"

#include <boost/program_options.hpp>

void mandelbrot(
    std::pair<long, long> resolution,
    std::pair<std::complex<double>,
    std::complex<double>> bounds,
    long max_iter,
    double r = 2.,
    int k = 2) {

    const double a0 = bounds.first.real();
    const double b0 = bounds.first.imag();
    const double a1 = bounds.second.real();
    const double b1 = bounds.second.imag();
    const long height = resolution.first;
    const long width = resolution.second;
    const long channels = 3;

    long n;
    double imag, real;
    auto I = std::complex<double>(0.0, 1.);

    std::complex<double> z0(0., 0.);
    std::complex<double> z(z0);
    std::complex<double> c;

    xt::xtensor<double, 1> xrange = xt::linspace<double>(a0, a1, width);
    xt::xtensor<double, 1> yrange = xt::linspace<double>(b0, b1, height);

    xt::xtensor<long, 2> counts = xt::zeros<long>({ height, width });
    xt::xtensor<long, 2> norms = xt::zeros<double>({ height, width });
    xt::xtensor<long, 2> args = xt::zeros<double>({ height, width });

    for (size_t i = 0; i < height; i++) {
        imag = yrange(i);

        std::cout << std::fixed << std::setprecision(2) << "\rRunning... " << 100.0 * (i + 1.0) / height << "%" << std::flush;

        for (size_t j = 0; j < width; j++) {
            real = xrange(j);

            n = 0;
            z = z0;
            c = std::complex<double>(real, imag);

            const bool period_one = std::abs(1. - std::sqrt(1. - 4. * c)) <= 1.;
            const bool period_two = std::abs(c + 1.) < 0.25;

            if (period_one || period_two) {
                n = max_iter;
            }
            else {
                while (n < max_iter && std::abs(z) < r) {
                    n++;
                    z = std::pow(z, k) + c;
                }
            }

            norms(i, j) = std::norm(z);
            args(i, j) = std::arg(z);
            counts(i, j) = n - std::log( std::log(std::norm(z))/std::log(r))/std::log(k);  // renormalized
        }
    }

    const double min_counts = xt::amin(counts)();
    const double max_counts = xt::amax(counts)();

    const double min_norms = xt::amin(norms)();
    const double max_norms = xt::amax(norms)();

    const double min_args = xt::amin(args)();
    const double max_args = xt::amax(args)();

    auto scaled_counts = (counts - min_counts) / (max_counts - min_counts);
    auto scaled_norms = (norms - min_norms) / (max_norms - min_norms);
    auto scaled_args = (args   - min_args  ) / (max_args   - min_args  );

    xt::dump_image("../../mandelbrot_counts.png", xt::cast<uint8_t>(255 * scaled_counts));
    xt::dump_image("../../mandelbrot_norms.png" , xt::cast<uint8_t>(255 * scaled_norms ));
    xt::dump_image("../../mandelbrot_args.png"  , xt::cast<uint8_t>(255 * scaled_args  ));

    xt::xtensor<uint8_t, 3> image = xt::zeros<uint8_t>({ height, width, channels });
    std::tuple<uint8_t, uint8_t, uint8_t> color;

    for (size_t i = 0; i < height; i++) {
        std::cout << std::fixed << std::setprecision(2) << "\rColoring... " << 100.0 * (i + 1.0) / height << "%" << std::flush;

        for (size_t j = 0; j < width; j++) {
            image(i, j, 0) = (uint8_t) 255 * scaled_counts(i, j);
            image(i, j, 1) = (uint8_t) 255 * scaled_args(i, j);
            image(i, j, 2) = (uint8_t) 255 * scaled_norms(i, j);
        }
    }

    xt::dump_image("../../mandelbrot_color.png", image);
}

namespace opt = boost::program_options;

int main(int argc, char** argv) {
    int height, width, max_iter;
    double xi, xf, yi, yf, r, p;

    opt::options_description params("Mandelbrot Set Parameters");

    params.add_options()
        ("help", "Show Usage")
        ("height,h", opt::value< int >(&height)->default_value(2160), "Image Height")  // 4320
        ("width,w", opt::value< int >(&width)->default_value(3840), "Image Width")  // 7680
        ("max_iter,m", opt::value< int >(&max_iter)->default_value(1000), "Maximum Number Of Iterations")
        ("xi", opt::value<double>(&xi)->default_value(-2.0), "Lower Real Bound")
        ("xf", opt::value<double>(&xf)->default_value(1.0), "Upper Real Bound")
        ("yi", opt::value<double>(&yi)->default_value(-1.5), "Lower Imaginary Bound")
        ("yf", opt::value<double>(&yf)->default_value(1.5), "Upper Imaginary Bound")
        ("radius,r", opt::value<double>(&r)->default_value(2.), "Bail-Out Radius")
        ("power,p",  opt::value<double>(&p)->default_value(2.), "Polynomial Power Of The Logistic Map (Classic Mandelbrot Set: p = 2)")
        ;

    opt::variables_map vm;
    opt::store(opt::parse_command_line(argc, argv, params), vm);

    if (vm.count("help")) {
        std::cout << params << std::endl;
        return 1;
    }
    else {
        opt::notify(vm);
        mandelbrot(std::make_pair(height, width), std::make_pair(std::complex<double>(xi, yi), std::complex<double>(xf, yf)), max_iter, r, p);
        return 0;
    }
}


#include <FindPeriod.hpp>
#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <vector>
#include <iomanip>
using namespace std;

/**********************************************************

Plots x_n vs. n. The main function is pretty simple. The
printFixedPoints function (after checking for sillyness)
Iterates the logistic function until FPVAL to remove the
error near the origin, then prints out p values, where p
is the period. Values at each point of the period are
the fixed points.

**********************************************************/

const int CUTOFF = 100;	// Cutoff value for graph
const long FPVAL = 10000; // Value to start finding fixed points
const double r = 3.5;	// Growth rate
const double x_0 = 0.2;	// Initial value

void plotStuff(vector<int> x, vector<double> y){
	Gnuplot gp;										// Define a gnuplot output stream (runs the gnuplot command behind the scenes)
	gp << setprecision(3);
	gp << "set xrange [0:" << CUTOFF << "]\n";		// Set the x and y range for the plot
	gp << "set yrange [0:1]\n";
	gp << "set term wxt font \"FreeSerif,12\"\n";
	gp << "set title \"Logistic Function with x_0 = " << x0 << " and r = " << r << "\"\n";
	gp << "set xlabel \"n\"\n";
	gp << "set ylabel \"x_n\"\n";
	gp << "plot '-' with linespoints lc rgb \"black\" notitle\n";	// Plot the data to be given as a line graph with points, title
	gp.send1d(boost::make_tuple(x,y));				// Send the data as a tuple
}

void printFixedPoints(){
	int p = getPeriod(r);	// Get period
	cout << "Period: " << p << "\n";

	if(p == 0){
		cout << "Chaotic!\n";	// Notify user if the value of r leads to chaotic behavior
	} else if (x_0 == 0 || x_0 == 1){
		cout << "Trivial!\n";	// Also notify if the initial value is bad
	} else {
		cout << "Fixed points:\n";

		double x = x_0;
		for(int i = 0; i < FPVAL+p; i++){
			x = r*x*(1-x);		// Perform the logistic map operation x_n+1 = r*x_n*(1-x_n)
			if(i >= FPVAL){
				cout << x << "\n";
			}
		}
	}
}

int main(){				// Main section
	vector<double> x_n;	// Stores values for x at generation n
	vector<int> n;		// Store generation n

	double x = x_0;		// Current value of x (population)

	for(int i = 0; i < CUTOFF; i++){	// Loop until the cutoff
		x_n.push_back(x);				// Push the x value onto the vector of values
		n.push_back(i);					// same with n
		x = r*x*(1-x);					// Perform the logistic map operation x_n+1 = r*x_n*(1-x_n)
	}

	for(int i = 0; i < x_n.size(); i++){		// Output loop for gnuplot!
		cout << n[i] << "\t" << x_n[i] << "\n";	// x_n vs n
	}

	plotStuff(n,x_n);		// Plot the data

	printFixedPoints();

}


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/differentiation/autodiff.hpp>
#include <iostream>
#include <stdexcept>

using namespace boost::math::constants;
using namespace boost::math::differentiation;

// Equations and function/variable names are from
// https://en.wikipedia.org/wiki/Greeks_(finance)#Formulas_for_European_option_Greeks

// Standard normal cumulative distribution function
template <typename X>
X Phi(X const& x) {
  return 0.5 * erfc(-one_div_root_two<X>() * x);
}

enum class CP { call, put };

// Assume zero annual dividend yield (q=0).
template <typename Price, typename Sigma, typename Tau, typename Rate>
promote<Price, Sigma, Tau, Rate> black_scholes_option_price(CP cp,
                                                            double K,
                                                            Price const& S,
                                                            Sigma const& sigma,
                                                            Tau const& tau,
                                                            Rate const& r) {
  using namespace std;
  auto const d1 = (log(S / K) + (r + sigma * sigma / 2) * tau) / (sigma * sqrt(tau));
  auto const d2 = (log(S / K) + (r - sigma * sigma / 2) * tau) / (sigma * sqrt(tau));
  switch (cp) {
    case CP::call:
      return S * Phi(d1) - exp(-r * tau) * K * Phi(d2);
    case CP::put:
      return exp(-r * tau) * K * Phi(-d2) - S * Phi(-d1);
    default:
      throw std::runtime_error("Invalid CP value.");
  }
}

int main() {
  double const K = 100.0;                    // Strike price.
  auto const S = make_fvar<double, 2>(105);  // Stock price.
  double const sigma = 5;                    // Volatility.
  double const tau = 30.0 / 365;             // Time to expiration in years. (30 days).
  double const r = 1.25 / 100;               // Interest rate.
  auto const call_price = black_scholes_option_price(CP::call, K, S, sigma, tau, r);
  auto const put_price = black_scholes_option_price(CP::put, K, S, sigma, tau, r);

  std::cout << "black-scholes call price = " << call_price.derivative(0) << '\n'
            << "black-scholes put  price = " << put_price.derivative(0) << '\n'
            << "call delta = " << call_price.derivative(1) << '\n'
            << "put  delta = " << put_price.derivative(1) << '\n'
            << "call gamma = " << call_price.derivative(2) << '\n'
            << "put  gamma = " << put_price.derivative(2) << '\n';
  return 0;
}
/*
Output:
black-scholes call price = 56.5136
black-scholes put  price = 51.4109
call delta = 0.773818
put  delta = -0.226182
call gamma = 0.00199852
put  gamma = 0.00199852
**/