#include <chrono>
#include <random>
#include <boost/program_options.hpp>
#include "myDist.hpp"
#include "matGen_lapack.hpp"

namespace po = boost::program_options;

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

  po::options_description desc("Artificial Matrices MT: Options");
  desc.add_options()
       ("help,h","show the help")
       ("N", po::value<std::size_t>()->default_value(10), "number of row and column of matrices to be generated.")
       ("dmax", po::value<double>()->default_value(21), "A scalar which scales the generated eigenvalues, this makes"
							 " the maximum absolute eigenvalue is abs(dmax)." )
       ("epsilon", po::value<double>()->default_value(0.1), "This value is epsilon." )

       ("myDist", po::value<std::size_t>()->default_value(0), "Specifies my externel setup distribution for generating eigenvalues:\n "
							      "0: Uniform eigenspectrum lambda_k = dmax * (epsilon + k * (1 - epsilon) / n for k = 0, ..., n-1)\n "
                                                              "1: Geometric eigenspectrum lambda_k =lambda_k = epsilon^[(n - k) / n] for k = 0, ..., n-1) \n"
							      "2: 1-2-1 matrix\n"
							      "3: Wilkinson matrix\n")
       ("mean", po::value<double>()->default_value(0.5), "Mean value of Normal distribution for the randomness." )
       ("stddev", po::value<double>()->default_value(1.0), "Standard deviation value of Normal distribution for the randomness." );

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 1;
  }

  //number of row and column of matrix to be generated
  std::size_t n = vm["N"].as<std::size_t>();
  double dmax = vm["dmax"].as<double>();
  double eps = vm["epsilon"].as<double>();
  std::size_t myDist = vm["myDist"].as<std::size_t>();
  //for the randomness with normal distribution
  double mean = vm["mean"].as<double>();
  double stddev = vm["stddev"].as<double>();

  //generating ...
  std::cout << "]> start generating ..." << std::endl;

  std::chrono::high_resolution_clock::time_point start, end;
  std::chrono::duration<double> elapsed;

  start = std::chrono::high_resolution_clock::now();

  double *A;

  //myDist = 0: Uniform eigenspectrum
  //myDist = 1: Geometric eigenspectrum
  //myDist = 2: 1-2-1 tridiagonal matrix
  //myDist = 3: Wilkinson tridiagonal matrix
  std::string mode;

  if(myDist == 0){
    mode= "Uniform";
  }else if(myDist == 1){
    mode = "Geometric";
  }else if(myDist == 2){
    mode = "1-2-1";
  }else if(myDist == 3){
    mode = "Wilkinson";
  }else{
    mode = "myLambda";
  }

  if(myDist == 0){
      A = matGen_lapack<double>(n, mean, stddev, myUniformDist<double>, n, eps, dmax);
  }else if(myDist == 1){
      A = matGen_lapack<double>(n, mean, stddev, myGeometricDist<double>, n, eps, dmax);
  }else if(myDist == 2){
      A = matGen_121<double>(n);
  }else if(myDist == 3){
      A = matGen_WilkinsonPlus<double>(n);
  }else{
    A = matGen_lapack<double>(n, mean, stddev, [](std::size_t n, int x){
                double *eigenv = new double[n];
                for(auto k = 0; k < n; k++){
                    eigenv[k] = k * (x + 1);
                }
                return eigenv;
            }, n, 1);
  }

  end = std::chrono::high_resolution_clock::now();

  elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(end - start);

  std::cout << "]> matrix generated in " << elapsed.count() << " seconds" << std::endl;

  std::ostringstream out_str;

  if(myDist == 0 || myDist == 1){
    out_str << std::scientific << "matgen_m_" << n << "_" << mode << "_eps_"<< eps
            << "_dmax_" << dmax << ".bin";
  }else{
    out_str << std::scientific << "matgen_m_" << n << "_"<< mode << ".bin";
  }

  //save matrix into binary file
  wrtMatIntoBinary<double>(A, out_str.str(), n * n);

  delete [] A;
  return 0;
}


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

// Test case for kurtosis.hpp

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/kurtosis.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    // double epsilon = 1;

    accumulator_set<double, stats<tag::kurtosis > > acc1;
    accumulator_set<int, stats<tag::kurtosis > > acc2;

    // two random number generators
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma(0,1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

    for (std::size_t i=0; i<100000; ++i)
    {
        acc1(normal());
    }

    // This check fails because epsilon is relative and not absolute
    // BOOST_CHECK_CLOSE( kurtosis(acc1), 0., epsilon );

    acc2(2);
    acc2(7);
    acc2(4);
    acc2(9);
    acc2(3);

    BOOST_CHECK_EQUAL( mean(acc2), 5 );
    BOOST_CHECK_EQUAL( accumulators::moment<2>(acc2), 159./5. );
    BOOST_CHECK_EQUAL( accumulators::moment<3>(acc2), 1171./5. );
    BOOST_CHECK_EQUAL( accumulators::moment<4>(acc2), 1863 );
    BOOST_CHECK_CLOSE( kurtosis(acc2), -1.39965397924, 1e-6 );
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("kurtosis test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include <iostream>
#include <stdio.h>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;

int main(int argc, char * argv[])
{
  if (argc != 3 || strcmp(argv[1], "-h") == 0)
  {
    cout << "Usage: ./eigDeterm N_MATRIX(<=1000) ITERATIONS(~1000)"
      << endl;
    return 1;
  }

  srand(time(NULL));
  int MAT_DIM = 6;
  int N_MATRIX = atoi(argv[1]);
  int ITERATIONS = atoi(argv[2]);
  int i, j, k, count;
  double start, end, t_time = 0.0;

  printf("N_MATRIX: %d, ITERATIONS: %d\n", N_MATRIX, ITERATIONS);

//  MatrixXd *m_list(6,6);

  MatrixXd m = MatrixXd::Random(MAT_DIM, MAT_DIM);

  cout << "Here is the matrix m:" << endl << m << endl;
  double determinant;
  cout << "Determinant is: " << m.determinant() << endl;

  return 0;
}


#include <bits/stdc++.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/multiprecision/cpp_int.hpp>
using mpi = boost::multiprecision::int128_t;
using namespace boost::numeric;
using imatrix = ublas::matrix<mpi>;

using namespace std;
typedef long long int ll;
typedef pair<int, int> P;
//typedef pair<ll, ll> Pll;
typedef vector<int> Vi;
//typedef tuple<int, int, int> T;
#define FOR(i,s,x) for(int i=s;i<(int)(x);i++)
#define REP(i,x) FOR(i,0,x)
#define ALL(c) c.begin(), c.end()
#define DUMP( x ) cerr << #x << " = " << ( x ) << endl
#define UNIQUE(c) sort(ALL(c)), c.erase(unique(ALL(c)), c.end())

const int dr[4] = {-1, 0, 1, 0};
const int dc[4] = {0, 1, 0, -1};
const ll mod = (int)1e9 + 7;

void modulo(imatrix &mat, mpi mod) {
  for (size_t i = 0; i < mat.size1(); i++) {
    for (size_t j = 0; j < mat.size2(); j++) {
      mat(i, j) %= mod;
    }
  }
}

int main() {
  // use scanf in CodeForces!

  cin.tie(0);
  ios_base::sync_with_stdio(false);
  int N; ll K; cin >> N >> K;
  imatrix mat(N, N);
  REP(i, N) REP(j, N) cin >> mat(i, j);

  imatrix pow2 = mat, ans(N, N);
  REP(i, N) REP(j, N) ans(i, j) = (ll)(i == j);
  while (K) {
    if (K & 1LL) {
      ans = ublas::prod(ans, pow2);
      modulo(ans, mod);
    }
    pow2 = ublas::prod(pow2, pow2);
    modulo(pow2, mod);
    K >>= 1;
  }

  mpi count = 0;
  REP(i, N) REP(j, N) count = (count + ans(i, j)) % mod;
  cout << count << endl;
  return 0;
}


/**
 * @file
 * @brief Implementations from make_quad_rule.h
 * @author Raffael Casagrande
 * @date   2018-08-19 06:35:16
 * @copyright MIT License
 */

#include "make_quad_rule.h"
#include <Eigen/KroneckerProduct>
#include "gauss_quadrature.h"

namespace lf::quad {
namespace detail {
template <base::RefElType REF_EL, int Order>
QuadRule HardcodedQuadRule();
}

QuadRule make_QuadRule(base::RefEl ref_el, unsigned char order) {
  if (ref_el == base::RefEl::kSegment()) {
    quadOrder_t n = order / 2 + 1;
    auto [points, weights] = GaussLegendre(n);
    return QuadRule(base::RefEl::kSegment(), points.transpose(),
                    std::move(weights), 2 * n - 1);
  }
  if (ref_el == base::RefEl::kQuad()) {
    quadOrder_t n = order / 2 + 1;
    auto [points1d, weights1d] = GaussLegendre(n);
    Eigen::MatrixXd points2d(2, n * n);
    points2d.row(0) = Eigen::kroneckerProduct(points1d.transpose(),
                                              Eigen::MatrixXd::Ones(1, n));
    points2d.row(1) = points1d.transpose().replicate(1, n);
    return QuadRule(base::RefEl::kQuad(), std::move(points2d),
                    Eigen::kroneckerProduct(weights1d, weights1d), 2 * n - 1);
  }
  if (ref_el == base::RefEl::kTria()) {
    switch (order) {
      case 1:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 1>();
      case 2:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 2>();
      case 3:  // user order 4 rule instead
      case 4:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 4>();
      case 5:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 5>();
      case 6:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 6>();
      case 7:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 7>();
      case 8:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 8>();
      case 9:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 9>();
      case 10:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 10>();
      case 11:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 11>();
      case 12:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 12>();
      case 13:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 13>();
      case 14:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 14>();
      case 15:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 15>();
      case 16:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 16>();
      case 17:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 17>();
      case 18:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 18>();
      case 19:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 19>();
      case 20:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 20>();
      case 21:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 21>();
      case 22:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 22>();
      case 23:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 23>();
      case 24:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 24>();
      case 25:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 25>();
      case 26:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 26>();
      case 27:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 27>();
      case 28:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 28>();
      case 29:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 29>();
      case 30:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 30>();
      case 31:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 31>();
      case 32:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 32>();
      case 33:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 33>();
      case 34:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 34>();
      case 35:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 35>();
      case 36:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 36>();
      case 37:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 37>();
      case 38:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 38>();
      case 39:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 39>();
      case 40:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 40>();
      case 41:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 41>();
      case 42:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 42>();
      case 43:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 43>();
      case 44:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 44>();
      case 45:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 45>();
      case 46:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 46>();
      case 47:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 47>();
      case 48:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 48>();
      case 49:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 49>();
      case 50:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 50>();
      default:
        // Create a quadrule using tensor product quadrature rule + duffy
        // transform
        quadOrder_t n = order / 2 + 1;
        auto [leg_p, leg_w] = GaussLegendre(n);
        auto [jac_p, jac_w] = GaussJacobi(n, 1, 0);
        jac_p.array() = (jac_p.array() + 1) / 2.;  // rescale to [0,1]
        jac_w.array() *= 0.25;
        Eigen::MatrixXd points2d(2, n * n);
        points2d.row(0) = Eigen::kroneckerProduct(
            leg_p.transpose(), (1 - jac_p.transpose().array()).matrix());
        points2d.row(1) = jac_p.transpose().replicate(1, n);
        return QuadRule(base::RefEl::kTria(), std::move(points2d),
                        Eigen::kroneckerProduct(leg_w, jac_w), 2 * n - 1);
    }
  }
  LF_VERIFY_MSG(
      false, "No Quadrature rules implemented for this reference element yet.");
}
}  // namespace lf::quad


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <iostream>

BOOST_DATA_TEST_CASE(
    test1,
    boost::unit_test::data::random(1, 17) ^ boost::unit_test::data::xrange(7),
    random_sample,
    index)
{
    std::cout << "test 1: " << random_sample << ", " << index << std::endl;
    BOOST_TEST((random_sample <= 17 && random_sample >= 1));
}

BOOST_DATA_TEST_CASE(
    test,
    boost::unit_test::data::random(
        (boost::unit_test::data::distribution =
            std::uniform_real_distribution<float>{ 1, 2 }))
    ^
    boost::unit_test::data::xrange(7),
    random_sample,
    index)
{
    std::cout << "test 2: " << random_sample << ", " << index << std::endl;
    BOOST_TEST(random_sample < 2.0);
}


//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include <iostream>
#include <boost/math/tools/ulps_plot.hpp>
#include <boost/core/demangle.hpp>
#include <boost/math/tools/agm.hpp>
#include <boost/multiprecision/float128.hpp>

using boost::math::tools::ulps_plot;
using boost::math::tools::agm;

int main() {
    using PreciseReal = boost::multiprecision::float128;
    using CoarseReal = float;

    auto agm_coarse = [](CoarseReal x) {
        return agm<CoarseReal>(x, CoarseReal(1));
    };
    auto agm_precise = [](PreciseReal x) {
        return agm<PreciseReal>(x, PreciseReal(1));
    };

    std::string filename = "agm_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    int samples = 2500;
    int width = 1100;
    PreciseReal clip = 100;
    auto plot = ulps_plot<decltype(agm_precise), PreciseReal, CoarseReal>(agm_precise, CoarseReal(0), CoarseReal(10000), samples);
    plot.clip(clip).width(width);
    std::string title = "AGM ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    //plot.title(title);
    plot.vertical_lines(10);
    plot.add_fn(agm_coarse);
    plot.write(filename);
}


/*
 * Copyright 2018-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <Eigen/Core>
#include <eigen-osqp/CSCMatrix.h>
#include <eigen-osqp/OSQP.h>
#include <iostream>

int main()
{
    int nrVar = 6;
    int nrConstr = 5;
    auto inf = std::numeric_limits<double>::infinity();

    Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(nrVar, nrVar);
    Eigen::MatrixXd A(nrConstr, nrVar);
    A << 1., -1., 1., 0., 3., 1.,
        -1., 0., -3., -4., 5., 6.,
        2., 5., 3., 0., 1., 0.,
        0., 1., 0., 1., 2., -1.,
        -1., 0., 2., 1., 1., 0.;

    Eigen::VectorXd c(nrVar);
    Eigen::VectorXd AL(nrConstr);
    Eigen::VectorXd AU(nrConstr);
    Eigen::VectorXd XL(nrVar);
    Eigen::VectorXd XU(nrVar);
    c << 1., 2., 3., 4., 5., 6.;
    AL << 1., 2., 3., -inf, -inf;
    AU << 1., 2., 3., -1., 2.5;
    XL << -1000., -10000., 0., -1000., -1000., -1000.;
    XU << 10000., 100., 1.5, 100., 100., 1000.;

    Eigen::OSQP qp;

    qp.problem(nrVar, nrConstr);
    bool success = qp.solve(Q, c, A, AL, AU, XL, XU);

    Eigen::VectorXd result = qp.result();

    std::cout << "Problem:"
              << "\n\tminimize 0.5*x'*Q*x + c'*x"
              << "\n\twith     AL <= A <= AU"
              << "\n\t         XL <= x <= XU"
              << "\n\nQ:\n"
              << Q
              << "\nc:\n"
              << c.transpose()
              << "\nA:\n"
              << A
              << "\nAL:\n"
              << AL.transpose()
              << "\nAU:\n"
              << AU.transpose()
              << "\nXL:\n"
              << XL.transpose()
              << "\nXU:\n"
              << XU.transpose()
              << "\n\n\nSolution:\n"
              << result.transpose() << std::endl;

    std::cout << "Press enter to quit" << std::endl;
    std::cin.get();
}


// Copyright (c) 2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <drake/math/discrete_algebraic_riccati_equation.h>

namespace frc3512 {

/**
 * Returns solution to the DARE.
 *
 * @param A System matrix.
 * @param B Input matrix.
 * @param Q State cost matrix.
 * @param R Input cost matrix.
 * @param N State-input cross-term cost matrix.
 */
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
    const Eigen::Matrix<double, States, States>& A,
    const Eigen::Matrix<double, States, Inputs>& B,
    const Eigen::Matrix<double, States, States>& Q,
    const Eigen::Matrix<double, Inputs, Inputs>& R,
    const Eigen::Matrix<double, States, Inputs>& N) {
    Eigen::Matrix<double, States, States> scrA =
        A - B * R.llt().solve(N.transpose());
    Eigen::Matrix<double, States, States> scrQ =
        Q - N * R.llt().solve(N.transpose());

    return drake::math::DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);
}

}  // namespace frc3512


#include <Eigen/Dense>
#include <tuple>
#include <iostream>
#include <fstream>
#include <string>
#include "NARMAL2Network.h"
#include "util.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::ArrayXd;
using narmal2::NARMAL2Network;
using narmal2::writeCSV;

/**
 * Generate data for the example system
 * @param n number of samples to be produced
 * @return X and y
 */
std::tuple<MatrixXd, VectorXd> generateData(const VectorXd& u)
{
	int n = u.size();
	double y0 = 0.3, y_1 = 0, u_1 = 0; // intial values
	MatrixXd X{ n, 3 };
	VectorXd y{ n + 1 };
	// compute y(1) manually: [y(k), y(k-1), u(k-1)] -> y(k+1)
	y(0) = y0;
	X.row(0) << y0, y_1, u_1;
	y(1) = 1.5 * y0 * y_1 / (1 + y0 * y_1 + y_1*y_1)
		+ std::sin(y0 + y_1) + 0.8 * u_1 + u(0) * std::cos(y0 - u_1);

	for (int k = 1; k < n; k++)
	{
		X.row(k) << y(k), y(k - 1), u(k - 1);
		y(k + 1) = 1.5 * y(k) * y(k - 1) / (1 + y(k) * y(k) + y(k - 1)*y(k - 1))
			+ std::sin(y(k) + y(k - 1)) + 0.8 * u(k - 1) + std::cos(y(k) - u(k-1)) * u(k);
	}
	return { X, y.bottomRows(n) };
}

/**
 * Generate the training or test dataset.
 * @param which "train" or "test"
 * @return X, u, and y
 */
std::tuple<MatrixXd, VectorXd, VectorXd> generateDataSet(const std::string& which)
{
	assert(which == "train" || which == "test");
	if (which == "train")
	{
		// generate data for training set
		ArrayXd k;
		int n = 5000;
		MatrixXd X;
		VectorXd y;
		k.setLinSpaced(n, 1, n);
		Eigen::VectorXd u = (1 * 3.14 / 3 * k).sin() + (2 * 3.14 / 24 * k).sin()
			+ ArrayXd::Random(n) * 0.2;
		std::tie(X, y) = generateData(u);
		return { X, u, y };
	}
	else
	{
		// generate data for test set
		int ntest = 200;
		ArrayXd k;
		k.setLinSpaced(ntest, 1, ntest); // [1, 2, ..., ntest]
		VectorXd utest = (1 * 3.14 / 5 * k).cos() + (2 * 3.14 / 25 * k).sin()
			+ ArrayXd::Random(ntest) * 0.1;
		MatrixXd Xtest;
		VectorXd ytest;
		std::tie(Xtest, ytest) = generateData(utest);
		return { Xtest, utest, ytest };
	}
}

int main()
{
	// get training set
	MatrixXd X;
	VectorXd u, y;
	std::tie(X, u, y) = generateDataSet("train");

	// train a network: 3 inputs,
	// the upper subnet has 30 hidden neurons and the lower 20 hidden neurons.
	NARMAL2Network net{ 3, {40}, {30}, "ReLU" };
	int batchSize = 500;
	int nIterations = 3000;
	auto stats = net.trainBatch(X, u, y, 1e-5, batchSize, nIterations, "momentum",
		{ {"learningRate", 1e-2}, {"gamma", 0.9}, {"decayRate", 0.98} });
	auto yp = net.predict(X, y);

	// get the test set and make predictions
	MatrixXd Xtest;
	VectorXd utest, ytest;
	std::tie(Xtest, utest, ytest) = generateDataSet("test");
	auto ytestp = net.predict(Xtest, utest);

	// write results CSV files for further analysis using for example MATLAB
	writeCSV("trainingLoss.csv", stats.trainLossHistory);
	Eigen::MatrixXd testResults{ytest.rows(), 2 };
	testResults.col(0) = ytest;
	testResults.col(1) = ytestp;
	writeCSV("testResults.csv", testResults);
	Eigen::MatrixXd trainingResults{ yp.rows(), 2 };
	trainingResults.col(0) = y;
	trainingResults.col(1) = yp;
	writeCSV("trainingResults.csv", trainingResults);

	std::getchar();
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/zerocurve.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/termstructures/interpolateddiscountcurve2.hpp>
#include <qle/termstructures/interpolateddiscountcurvelinearzero.hpp>

using namespace boost::unit_test_framework;
using namespace QuantLib;
using std::vector;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(DiscountCurveTest)

BOOST_AUTO_TEST_CASE(testDiscountCurve) {

    // FIXME: test curve1 or 2 or both
    BOOST_TEST_MESSAGE("Testing QuantExt::InteroplatedDiscountCurve2...");

    SavedSettings backup;
    Settings::instance().evaluationDate() = Date(1, Dec, 2015);
    Date today = Settings::instance().evaluationDate();

    vector<Date> dates;
    vector<Real> times;
    vector<DiscountFactor> dfs;
    vector<Rate> zeros;
    vector<Handle<Quote> > quotes;

    Size numYears = 30;
    int startYear = 2015;
    DayCounter dc = ActualActual();
    Calendar cal = NullCalendar();

    for (Size i = 0; i < numYears; i++) {

        // rate
        Real rate = 0.01 + i * 0.001;
        // 1 year apart
        dates.push_back(Date(1, Dec, startYear + i));
        Time t = dc.yearFraction(today, dates.back());
        times.push_back(t);

        // set up Quote of DiscountFactors
        DiscountFactor df = ::exp(-rate * t);
        Handle<Quote> q(boost::make_shared<SimpleQuote>(df));
        quotes.push_back(q);
        dfs.push_back(df);
        zeros.push_back(rate);
    }

    // Test against the QL curve
    boost::shared_ptr<YieldTermStructure> ytsBase;
    ytsBase =
        boost::shared_ptr<YieldTermStructure>(new QuantLib::InterpolatedDiscountCurve<LogLinear>(dates, dfs, dc, cal));
    ytsBase->enableExtrapolation();

    boost::shared_ptr<YieldTermStructure> ytsTest(new QuantExt::InterpolatedDiscountCurve2(times, quotes, dc));

    // now check that they give the same discount factors (including extrapolation)
    for (Time t = 0.1; t < numYears + 10.0; t += 0.1) {
        BOOST_CHECK_CLOSE(ytsBase->discount(t), ytsTest->discount(t), 1e-12);
    }

	BOOST_TEST_MESSAGE("Testing QuantExt::InterpolatedDiscountCurveLinearZero...");
	// Test linear interpolation in the zero rate against the QL curve
	// flat in the zero rate between t=0 and the first point
    zeros.at(0) = zeros.at(1);
    ytsBase = boost::make_shared<InterpolatedZeroCurve<Linear> >(dates, zeros, dc, cal);
    ytsBase->enableExtrapolation();

	ytsTest = boost::make_shared<QuantExt::InterpolatedDiscountCurveLinearZero>(times, quotes, dc);

    // now check that they give the same discount factors (including extrapolation)
    for (Time t = 0.1; t < numYears + 10.0; t += 0.1) {
        BOOST_CHECK_CLOSE(ytsBase->discount(t), ytsTest->discount(t), 1e-12);
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include "power.h"
#include <stopwatch.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <cmath>

using namespace boost::multiprecision;
using uint2048_t = number<cpp_int_backend<2048, 2048, unsigned_magnitude, unchecked, void>>;

auto mul = [](auto a, auto b) { return a * b; };

template<typename I>
I power_1(I a, I b)
{
    I r = 1;
    for (I i = 0; i < b; ++i) {
        r *= a;
    }
    return r;
}

template<typename I>
I power_2(I a, I b)
{
    return power(a, b, mul);
}

int main(int, char**)
{
    stop_watch_t t;

    t.start();
    uint64_t a = 12;
    uint64_t b = 27;
    auto r = std::pow(a, b);
    auto elapsed = t.stop();
    std::cout << "std::pow = " << static_cast<uint2048_t>(r) << " : " << elapsed << " " << t.period() << "\n";

    uint2048_t x = 12;
    uint2048_t y = 27;
    t.start();
    auto ra = power_1(x, y); // 429981696
    elapsed = t.stop();
    std::cout << "power_1  = " << ra << " : " << elapsed << " " << t.period() << "\n";

    t.start();
    ra = power_2(x, y);
    elapsed = t.stop();
    std::cout << "power_2  = " << ra << " : " << elapsed << " " << t.period() << "\n";
    return 0;
}


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

using namespace std;
int main(int, char**)
{
    typedef double value_type;
    typedef int    size_type;
    const int nb_elements= 6, nb_nodes= 12;

    value_type array[][4]= {{2, 3,   4,   5},
			    {4, 10, 13,  16},
			    {6, 25, 38,  46},
			    {8, 32, 77, 100}};
    mtl::dense2D<value_type>   E_mat(array);

    std::cout<<"E_mat=\n"<< E_mat <<"\n";

    mtl::mat::element_structure<value_type> A;

    typedef mtl::mat::element<value_type>	element_type;
    element_type* elements = new element_type[nb_elements];

    mtl::dense_vector<size_type>  index_a(4, 0),
				   index_b(4, 0),
				   index_c(4, 0),
				   index_d(4, 0),
				   index_e(4, 0),
				   index_f(4, 0);

    // construct nodes for every element
    index_a[0]= 0; index_a[1]= 1; index_a[2]= 4; index_a[3]= 5;
    index_b= index_a + 1;
    index_c= index_a + 2;
    index_d= index_a + 4;
    index_e= index_a + 5;
    index_f= index_a + 6;

    //construct the 6 elements from the example grid
    element_type a(0, index_a, E_mat);
    element_type b(1, index_b, E_mat);
    element_type c(2, index_c, E_mat);
    element_type d(3, index_d, E_mat);
    element_type e(4, index_e, E_mat);
    element_type f(5, index_f, E_mat);

    //construct neighborhood information for each element
    a.add_neighbors(&b, &d, &e);
    b.add_neighbors(&a, &c, &d, &e, &f);
    c.add_neighbors(&a, &b, &e);
    d.add_neighbors(&a, &b, &e);
    e.add_neighbors(&a, &b, &c, &d, &f);
    f.add_neighbors(&b, &c, &e);

    std::cout<< "a=" << a << "\n";

    //construct array of elements
    elements[0]=a;
    elements[1]=b;
    elements[2]=c;
    elements[3]=d;
    elements[4]=e;
    elements[5]=f;

    //construct element_structure from the 6 single elements
    A.consume(nb_elements, nb_nodes, elements);
    mtl::dense_vector<value_type> x(nb_nodes, 1.0), test(A * x);

    std::cout<< "test="<< test << "\n";

    return 0;
}


#ifndef JOINT_HPP
#define JOINT_HPP
#include <armadillo>
#include "Math.hpp"
#include "Body.hpp"

class Joint
{
public:
    Joint(unsigned int TypeIn, arma::vec piIn, arma::vec pjIn, arma::vec qiIn,
            arma::vec qjIn, Body *i_In, Body *j_In);
    ~Joint() {};
    void Build_C();
    void Build_Cq();
    void Build_GAMMA();
    void update();

    arma::mat get_Cq();
    arma::vec get_GAMMA();

private:
    unsigned int Type;
    arma::vec pi;
    arma::vec pj;
    arma::vec qi;
    arma::vec qj;
    arma::mat Cq;
    arma::vec GAMMA;
    arma::vec CONSTRAINT;
    arma::mat TBI_i;
    arma::mat TBI_j;
    arma::vec Pi;
    arma::vec Pj;
    arma::vec Qi;
    arma::vec Qj;
    arma::vec wi;
    arma::vec wj;
    arma::vec Si;
    arma::vec Sj;
    Body *body_i_ptr;
    Body *body_j_ptr;
};

#endif  //JOINT_HPP


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include "math/Bezier.hpp"
#include "math/Bezier2D.hpp"

TEST(Bezier, Linear2D)
{
    using VT = Eigen::Vector2d;

    auto bez = Bezier<VT,1>(VT(0.0, 0.0), VT(1.0, 2.0));
    EXPECT_TRUE(bez.get<0>() == VT(0.0, 0.0));
    EXPECT_TRUE(bez.get<1>() == VT(1.0, 2.0));
    EXPECT_LT( (bez.eval(0.2) - VT(0.2, 0.4)).norm(), 10e-14 );
    EXPECT_LT( (bez.eval(0.5) - VT(0.5, 1.0)).norm(), 10e-14 );
    EXPECT_LT( (bez.eval(0.7) - VT(0.7, 1.4)).norm(), 10e-14 );
}

TEST(Bezier, LinearDiv)
{
    using VT = Eigen::Vector2d;

    auto bez = Bezier<VT,1>(VT(0.0, 0.0), VT(1.0, 2.0));
    auto bezdiv = bez.divide();

    EXPECT_LT( (bezdiv.first.get<0>()-bez.eval(0.0)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.first.get<1>()-bez.eval(0.5)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.second.get<0>()-bez.eval(0.5)).norm(), 10e-14 )
        << bezdiv.second.get<0>() << std::endl;
    EXPECT_LT( (bezdiv.second.get<1>()-bez.eval(1.0)).norm(), 10e-14 )
        << bezdiv.second.get<1>() << std::endl;
}

TEST(Bezier, LinearClip)
{
    Bezier<Eigen::Vector2d,1> bez(
        Eigen::Vector2d(0.0, 0.0),
        Eigen::Vector2d(1.0, 2.0) );

    constexpr size_t smp = 10;
    constexpr double dur = 1.0/smp;
    for(size_t i = 0; i < smp; ++i) {
        for(size_t j = i+1; j < smp; ++j) {
            auto bezclip = bez.clip(i*dur, j*dur);
            EXPECT_LT( (bezclip.get<0>()-bez.eval(i*dur)).norm(), 10e-10);
            EXPECT_LT( (bezclip.get<1>()-bez.eval(j*dur)).norm(), 10e-10);        }
    }
}

TEST(Bezier, Quad2D)
{
    using VT = Eigen::Vector2d;

    auto bez = Bezier<VT,2>(VT(2.0, 0.0), VT(0.0, 0.0), VT(0.0, 2.0));

    EXPECT_TRUE(bez.get<0>() == VT(2.0, 0.0));
    EXPECT_TRUE(bez.get<1>() == VT(0.0, 0.0));
    EXPECT_TRUE(bez.get<2>() == VT(0.0, 2.0));
    EXPECT_LT( (bez.eval(0.2) - VT(1.28, 0.08)).norm(), 10e-14 );
    EXPECT_LT( (bez.eval(0.5) - VT(0.5, 0.5)).norm(), 10e-14 );
    EXPECT_LT( (bez.eval(0.8) - VT(0.08, 1.28)).norm(), 10e-14 );
}

TEST(Bezier, QuadDiv)
{
    using VT = Eigen::Vector2d;

    auto bez = Bezier<VT,2>(VT(2.0, 0.0), VT(0.0, 0.0), VT(0.0, 2.0));
    auto bezdiv = bez.divide();

    EXPECT_LT( (bezdiv.first.get<0>() - bez.eval(0.0)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.first.get<1>() - VT(1.0,0.0)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.first.get<2>() - bez.eval(0.5)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.second.get<0>() - bez.eval(0.5)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.second.get<1>() - VT(0.0,1.0)).norm(), 10e-14 );
    EXPECT_LT( (bezdiv.second.get<2>() - bez.eval(1.0)).norm(), 10e-14 );

    for(double t = 0.0; t < 1.0; t += 0.1) {
        EXPECT_LT( (bezdiv.first.eval(t) - bez.eval(t/2.0)).norm(), 10e-14 )
            << "different at " << t << std::endl
            << "bez(t/2) =" << bez.eval(t/2.0).adjoint() << std::endl
            << "bezdiv(t)=" << bezdiv.first.eval(t).adjoint() << std::endl;
        EXPECT_LT( (bezdiv.second.eval(t) - bez.eval(0.5+t/2.0)).norm(), 10e-14 )
            << "different at " << t << std::endl
            << "bez(t/2+0.5)=" << bez.eval(0.5+t/2.0).adjoint() << std::endl
            << "bezdiv2(t)  =" << bezdiv.second.eval(t).adjoint() << std::endl;
    }
}

TEST(Bezier, QuadClip)
{
    Bezier<Eigen::Vector2d,2> bez(
        Eigen::Vector2d(2.0, 0.0),
        Eigen::Vector2d(0.0, 0.0),
        Eigen::Vector2d(0.0, 2.0));

    constexpr size_t smp = 10;
    constexpr double dur = 1.0/smp;
    for(size_t i = 0; i < smp; ++i) {
        for(size_t j = i+1; j < smp; ++j) {
            auto bezclip = bez.clip(i*dur, j*dur);
            EXPECT_LT( (bezclip.get<0>()-bez.eval(i*dur)).norm(), 10e-10);
            EXPECT_LT( (bezclip.eval(0.5)-bez.eval(i*dur/2 + j*dur/2)).norm(), 10e-10);
            EXPECT_LT( (bezclip.get<2>()-bez.eval(j*dur)).norm(), 10e-10);        }
    }
}

TEST(Bezier, Range)
{
    Bezier<Eigen::Vector2d,3> bez = {
        Eigen::Vector2d(0.0, 0.0),
        Eigen::Vector2d(1.0, 0.0),
        Eigen::Vector2d(0.0, 1.0),
        Eigen::Vector2d(1.0, 1.0)
    };

    std::vector<Eigen::Vector2d> vec;

    for(auto& pt : bez) {
        vec.push_back(pt);
    }

    ASSERT_EQ(vec.size(), 4);
    EXPECT_EQ(vec[0], Eigen::Vector2d(0.0, 0.0));
    EXPECT_EQ(vec[1], Eigen::Vector2d(1.0, 0.0));
    EXPECT_EQ(vec[2], Eigen::Vector2d(0.0, 1.0));
    EXPECT_EQ(vec[3], Eigen::Vector2d(1.0, 1.0));

    Bezier<Eigen::Vector2d, 3> const cbez = bez;

    vec.clear();

    for(auto& pt : cbez) {
        vec.push_back(pt);
    }

    ASSERT_EQ(vec.size(), 4);
    EXPECT_EQ(vec[0], Eigen::Vector2d(0.0, 0.0));
    EXPECT_EQ(vec[1], Eigen::Vector2d(1.0, 0.0));
    EXPECT_EQ(vec[2], Eigen::Vector2d(0.0, 1.0));
    EXPECT_EQ(vec[3], Eigen::Vector2d(1.0, 1.0));
}

// Test class
struct TestVec2 {
    double arr[2];
    double norm() const noexcept {
        return std::sqrt(arr[0]*arr[0] + arr[1]*arr[1]);
    }
    constexpr double& operator[](size_t i) noexcept{ return arr[i]; }
    constexpr double const& operator[](size_t i) const noexcept { return arr[i]; }
    constexpr TestVec2& operator+=(TestVec2 const& rhs) {
        arr[0] += rhs.arr[0];
        arr[1] += rhs.arr[1];
        return *this;
    }
    constexpr TestVec2& operator-=(TestVec2 const& rhs) {
        arr[0] -= rhs.arr[0];
        arr[1] -= rhs.arr[1];
        return *this;
    }
};

constexpr TestVec2 operator+(TestVec2 lhs, TestVec2 const& rhs)
{
    lhs += rhs;
    return lhs;
}

constexpr TestVec2 operator-(TestVec2 lhs, TestVec2 const& rhs)
{
    lhs -= rhs;
    return lhs;
}

constexpr TestVec2 operator*(double a, TestVec2 const& rhs)
{
    return {rhs[0]*a, rhs[1]*a};
}

bool operator==(TestVec2 const& lhs, TestVec2 const& rhs)
{
    return (lhs-rhs).norm() <= (lhs.norm() + rhs.norm())*10e-10;
}

bool operator!=(TestVec2 const& lhs, TestVec2 const& rhs)
{
    return !(lhs==rhs);
}

TEST(Bezier, CubicWithMyVector)
{
    constexpr auto bez = Bezier<TestVec2,3>(
        TestVec2{0.0, 0.0},
        TestVec2{0.0, 1.0},
        TestVec2{1.0, 1.0},
        TestVec2{1.0, 0.0} );

    constexpr auto v = bez.eval(0.5);
    constexpr auto vshouldbe = TestVec2{1.0/8.0 + 3.0/8.0, 3.0/8.0 + 3.0/8.0};
    EXPECT_EQ(v, vshouldbe);

    constexpr auto bezdiv = bez.divide();
    constexpr size_t smp = 10;
    constexpr double dur = 1.0/smp;
    for(size_t i = 0; i < smp; ++i) {
        double t = i*dur;
        EXPECT_EQ(bezdiv.first.eval(t), bez.eval(t/2.0))
            << "different at " << t << std::endl;
        EXPECT_EQ(bezdiv.second.eval(t), bez.eval(0.5+t/2.0))
            << "different at " << t << std::endl;
    }

    constexpr auto bezclip = bez.clip(0.3, 0.7);
    for(size_t i = 0; i < smp; ++i) {
        double t = i*dur;
        EXPECT_EQ(bezclip.eval(t), bez.eval(0.3+t*0.4));
    }
}

TEST(Bezier, ArbitraryDivision)
{
    constexpr auto bez = Bezier<TestVec2,3>(
        TestVec2{0.0, 0.0},
        TestVec2{0.0, 1.0},
        TestVec2{1.0, 1.0},
        TestVec2{1.0, 0.0} );

    for(double t0 = 0.1; t0 < 1.0; t0 += 0.1) {
        auto bezdiv = bez.divide(t0);
        for(double t = 0.0; t < 1.0; t += 0.1) {
            EXPECT_EQ(bezdiv.first.eval(t), bez.eval(t*t0))
                << "different at " << t << std::endl;
            EXPECT_EQ(bezdiv.second.eval(t), bez.eval(t0+t*(1.0-t0)))
                << "different at " << t << std::endl;
        }
    }
}

TEST(Bezier, Convert)
{
    struct Rotate90 {
        constexpr TestVec2 operator()(TestVec2 src) const noexcept {
            return TestVec2{-src.arr[1], src.arr[0]};
        }
    };

    constexpr auto bez = Bezier<TestVec2,3>(
        TestVec2{0.0, 0.0},
        TestVec2{0.0, 1.0},
        TestVec2{1.0, 1.0},
        TestVec2{1.0, 0.0} );
    constexpr auto bez90 = bez.convert(Rotate90());
    constexpr size_t smp = 10;
    constexpr double dur = 1.0/smp;
    for(size_t i = 0; i < smp; ++i) {
        double t = i*dur;
        auto pt = bez.eval(t);
        auto pt90 = bez90.eval(t);
        auto pt90shouldbe = TestVec2{-pt.arr[1], pt.arr[0]};
        EXPECT_EQ(pt90, pt90shouldbe);
        if(i > 0)
            EXPECT_NE(pt90, pt);
    }
};

TEST(Bezier, Variant)
{
    constexpr Bezier<TestVec2,2> bez2(
        TestVec2{0.0, 0.0},
        TestVec2{1.0, 0.0},
        TestVec2{1.0, 1.0});
    constexpr BezierVariant<TestVec2,0,1,2,3> bezvar(std::integral_constant<size_t,2>(), bez2);

    std::vector<TestVec2> vs;
    for(auto pt : bezvar) {
        vs.push_back(pt);
    }
    ASSERT_EQ(vs.size(), 3);
    EXPECT_EQ(vs[0], (TestVec2{0.0, 0.0}));
    EXPECT_EQ(vs[1], (TestVec2{1.0, 0.0}));
    EXPECT_EQ(vs[2], (TestVec2{1.0, 1.0}));

    constexpr size_t smp = 10;
    constexpr double dur = 1.0/smp;

    // Evaluation test
    for(size_t i = 0; i <= smp; ++i) {
        double t = i*dur;
        EXPECT_EQ(bez2.eval(t),
                  bezvar.eval(t));
    }

    // Division test
    constexpr auto bez2div = bez2.divide();
    constexpr auto bezvardiv = bezvar.divide();
    for(size_t i = 0; i <= smp; ++i) {
        double t = i*dur;
        EXPECT_EQ(bez2div.first.eval(t),
                  bezvardiv.first.eval(t));
        EXPECT_EQ(bez2div.second.eval(t),
                  bezvardiv.second.eval(t));
    }

    // Clip test
    for(size_t i = 0; i < smp; ++i) {
        for(size_t j = i+1; j <= smp; ++j) {
            auto bez2clip = bez2.clip(i*dur, j*dur);
            auto bezvarclip = bezvar.clip(i*dur, j*dur);
            for(size_t k = 0; k <= smp; ++k) {
                double t = k*dur;
                EXPECT_EQ(bez2clip.eval(t), bezvarclip.eval(t));
            }
        }
    }

    // Convert test
    struct Rotate90 {
        constexpr TestVec2 operator()(TestVec2 src) const noexcept {
            return TestVec2{-src.arr[1], src.arr[0]};
        }
    };
    constexpr auto bez2_90 = bez2.convert(Rotate90());
    constexpr auto bezvar_90 = bezvar.convert(Rotate90());
    for(size_t i = 0; i <= smp; ++i) {
        double t = i*dur;
        EXPECT_EQ(bez2_90.eval(t), bezvar_90.eval(t));
    }

    // Assignment
    constexpr Bezier<TestVec2,3> bez3(
        TestVec2{0.0, 0.0},
        TestVec2{1.0, 0.0},
        TestVec2{0.0, 1.0},
        TestVec2{1.0, 1.0});
    auto bezvar3 = bezvar;
    bezvar3.assign(std::integral_constant<size_t,3>(), bez3);
    vs.clear();
    for(auto pt : bezvar3) {
        vs.push_back(pt);
    }
    ASSERT_EQ(vs.size(), 4);
    EXPECT_EQ(vs[0], (TestVec2{0.0, 0.0}));
    EXPECT_EQ(vs[1], (TestVec2{1.0, 0.0}));
    EXPECT_EQ(vs[2], (TestVec2{0.0, 1.0}));
    EXPECT_EQ(vs[3], (TestVec2{1.0, 1.0}));
}

TEST(Bezier2D, IntersectionLinLin)
{
    constexpr size_t max_smp = 20;
    for(size_t i = 0; i < max_smp; ++i) {
        double t = 2*i*M_PI/max_smp;
        Eigen::Matrix2d mat;
        mat << cos(t), -sin(t), sin(t), cos(t);

        auto bez1 = Bezier<Eigen::Vector2d,1>(
            mat*Eigen::Vector2d(-1.0, 0.0),
            mat*Eigen::Vector2d(1.0, 0.0) );
        auto bez2 = Bezier<Eigen::Vector2d,1>(
            mat*Eigen::Vector2d(0.8, 0.6),
            mat*Eigen::Vector2d(0.8, -0.6) );

        auto params = intersect<12,3>(bez1, bez2);
        ASSERT_GE(params.size(), 1) << "t=" << t;
        EXPECT_LT(std::abs(params[0].first - 0.9), 10e-10);
        EXPECT_LT(std::abs(params[0].second - 0.5), 10e-10);
        if (params.size() > 1) {
            ADD_FAILURE()
                << "i=" << i << ", t=" << t << std::endl
                << "(" << params[0].first << ", " << params[0].second << ")"
                << std::endl
                << "(" << params[1].first << ", " << params[1].second << ")"
                << std::endl;
        }
    }
}

TEST(Bezier2D, IntersectionLinLinTerminated)
{
    constexpr size_t max_smp = 20;
    for(size_t i = 0; i < max_smp; ++i) {
        double t = 2*i*M_PI/max_smp;
        Eigen::Matrix2d mat;
        mat << cos(t), -sin(t), sin(t), cos(t);

        auto bez1 = Bezier<Eigen::Vector2d,1>(
            mat*Eigen::Vector2d(-1.0, 0.0),
            mat*Eigen::Vector2d(1.0, 0.0) );
        auto bez2 = Bezier<Eigen::Vector2d,1>(
            mat*Eigen::Vector2d(0.8, 0.6),
            mat*Eigen::Vector2d(0.8, -0.6) );

        auto params = intersect<12,3>(bez1, bez2, std::false_type{});
        EXPECT_TRUE(params.empty());
    }
}

TEST(Bezier2D, IntersectionQQ)
{
    constexpr size_t max_smp = 20;
    for(size_t i = 0; i < max_smp; ++i) {
        double t = 2*i*M_PI/max_smp;
        Eigen::Matrix2d mat;
        mat << cos(t), -sin(t), sin(t), cos(t);

        auto bez1 = Bezier<Eigen::Vector2d,2>(
            mat*Eigen::Vector2d(-0.5, -1.0),
            mat*Eigen::Vector2d(1.0, 0.0),
            mat*Eigen::Vector2d(-0.5, 1.0) );
        auto bez2 = Bezier<Eigen::Vector2d,2>(
            mat*Eigen::Vector2d(0.5, -1.0),
            mat*Eigen::Vector2d(-1.0, 0.0),
            mat*Eigen::Vector2d(0.5, 1.0) );

        auto params = intersect<12,3>(bez1, bez2);
        ASSERT_GE(params.size(), 2) << "t=" << t;
        EXPECT_LT(
            std::abs((mat.adjoint()*bez1.eval(params[0].first))(0)),
            10e-10);
        EXPECT_LT(
            std::abs((mat.adjoint()*bez2.eval(params[1].first))(0)),
            10e-10);
        if (params.size() > 2) {
            ADD_FAILURE()
                << "i=" << i << ", t=" << t << std::endl
                << "(" << params[0].first << ", " << params[0].second << ")"
                << std::endl
                << "(" << params[1].first << ", " << params[1].second << ")"
                << std::endl;
        }
    }
}

TEST(Bezier2D, IntersectionVarQL)
{
    constexpr size_t max_smp = 20;
    for(size_t i = 0; i < max_smp; ++i) {
        double t = 2*i*M_PI/max_smp;
        Eigen::Matrix2d mat;
        mat << cos(t), -sin(t), sin(t), cos(t);

        auto bez1 = BezierVariant<Eigen::Vector2d,0,1,2>(
            std::integral_constant<size_t,2>(),
            mat*Eigen::Vector2d(-0.5, -1.0),
            mat*Eigen::Vector2d(1.0, 0.0),
            mat*Eigen::Vector2d(-0.5, 1.0) );
        auto bez2 = Bezier<Eigen::Vector2d,2>(
            mat*Eigen::Vector2d(0.5, -1.0),
            mat*Eigen::Vector2d(-1.0, 0.0),
            mat*Eigen::Vector2d(0.5, 1.0) );

        auto params = intersect<12,3>(bez1, bez2);
        ASSERT_GE(params.size(), 2) << "t=" << t;
        EXPECT_LT(
            std::abs((mat.adjoint()*bez1.eval(params[0].first))(0)),
            10e-10);
        EXPECT_LT(
            std::abs((mat.adjoint()*bez2.eval(params[1].first))(0)),
            10e-10);
        if (params.size() > 2) {
            ADD_FAILURE()
                << "i=" << i << ", t=" << t << std::endl
                << "(" << params[0].first << ", " << params[0].second << ")"
                << std::endl
                << "(" << params[1].first << ", " << params[1].second << ")"
                << std::endl;
        }
    }
}


 /*
 * functions.cpp
 *
 */
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <numeric>

#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/types.h"
#include "core/functions.h"

using namespace std;
using namespace yann;

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Activation functions implementation
//

// identity function:
//  f(x) = x
//  d f(x<i>) / d(x<j>) = 1 if i == j and 0 if i != j
string yann::IdentityFunction::get_info() const
{
  return "Identity";
}
void yann::IdentityFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  switch(mode) {
  case Operation_Assign:
    output.noalias() = input;
    break;
  case Operation_PlusEqual:
    output.noalias() += input;
    break;
  }
}
void yann::IdentityFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  output.setOnes();
}
unique_ptr<ActivationFunction> yann::IdentityFunction::copy() const
{
  return make_unique<IdentityFunction>();
}

// rectified linear unit function or leaky ReLU if a != 0:
//  f(x) = x for x > 0 ; a*x for x < 0
//  d f(x<i>) / d(x<j>) = 1 if x > 0; a if x < 0
string yann::ReluFunction::get_info() const
{
  return "Relu";
}
void yann::ReluFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  switch(mode) {
  case Operation_Assign:
    output.setZero();
    break;
  case Operation_PlusEqual:
    // do nothing
    break;
  }

  const auto batch_size = get_batch_size(output);
  const auto batch_item_size = get_batch_item_size(output);
  for(MatrixSize ii = 0 ; ii < batch_size; ++ii) {
    for(MatrixSize jj = 0 ; jj < batch_item_size; ++jj) {
      if(get_batch(input, ii)(jj) >= 0) {
        get_batch(output, ii)(jj) += get_batch(input, ii)(jj);
      } else {
        get_batch(output, ii)(jj) += _a * get_batch(input, ii)(jj);
      }
    }
  }
}
void yann::ReluFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  const auto batch_size = get_batch_size(output);
  const auto batch_item_size = get_batch_item_size(output);
  for(MatrixSize ii = 0 ; ii < batch_size; ++ii) {
    for(MatrixSize jj = 0 ; jj < batch_item_size; ++jj) {
      if(get_batch(input, ii)(jj) >= 0) {
        get_batch(output, ii)(jj) = 1;
      } else {
        get_batch(output, ii)(jj) = _a;
      }
    }
  }
}
unique_ptr<ActivationFunction> yann::ReluFunction::copy() const
{
  return make_unique<ReluFunction>(_a);
}

// sigmoid function:
//  f(x) = 1 / (1 + exp(-x))
string yann::SigmoidFunction::get_info() const
{
  return "Sigmoid";
}
Value yann::SigmoidFunction::sigmoid_scalar(const Value & x)
{
  return 1 / (1 + exp(-x));
}

Value yann::SigmoidFunction::sigmoid_derivative_scalar(const Value & x)
{
  Value s = sigmoid_scalar(x);
  return s * (1 - s);
}

void yann::SigmoidFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  switch(mode) {
  case Operation_Assign:
    output.array() = 1 / (1 + exp(-input.array()));
    break;
  case Operation_PlusEqual:
    output.array() += 1 / (1 + exp(-input.array()));
    break;
  }
}

void yann::SigmoidFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  this->f(input, output);
  output.array() =  output.array() * (1 -  output.array());
}
unique_ptr<ActivationFunction> yann::SigmoidFunction::copy() const
{
  return make_unique<SigmoidFunction>();
}

// Fast sigmoid function:
//  f(x) = 1 / (1 + exp(-x))
yann::FastSigmoidFunction::FastSigmoidFunction(const size_t & table_size, const Value & max_value) :
    _table(table_size + 1),
    _max_value(max_value)
{
  // populates _table with sigmoid(x) values for x in (-max_value, max_value)
  for(size_t ii = 0; ii <= table_size; ++ii) {
    _table[ii] = SigmoidFunction::sigmoid_scalar((2 * ii / (Value)table_size - 1) * max_value);
  }
}

yann::FastSigmoidFunction::FastSigmoidFunction(const SigmoidTable & table, const Value & max_value) :
    _table(table),
    _max_value(max_value)
{
}

string yann::FastSigmoidFunction::get_info() const
{
  ostringstream oss;
  oss << "FastSigmoidFunction["
      << "max_value=" << _max_value
      << ", table_size=" << _table.size()
      << "]"
      ;
  return oss.str();
}

void yann::FastSigmoidFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  auto sigmoid = [&](const Value & xx) -> Value {
    if(xx <= -_max_value) {
      return 0;
    } else if(xx >= _max_value) {
      return 1;
    }
    auto index = (size_t)((xx + _max_value) * (_table.size() - 1) / (2 * _max_value));
    YANN_SLOW_CHECK_GE(index, 0);
    YANN_SLOW_CHECK_LT(index, _table.size());
    return _table[index];
  };

  switch(mode) {
  case Operation_Assign:
    for(MatrixSize ii = 0; ii < input.rows(); ++ii) {
      for(MatrixSize jj = 0; jj < input.cols(); ++jj) {
        output(ii, jj) = sigmoid(input(ii, jj));
      }
    }
    break;
  case Operation_PlusEqual:
    for(MatrixSize ii = 0; ii < input.rows(); ++ii) {
      for(MatrixSize jj = 0; jj < input.cols(); ++jj) {
        output(ii, jj) += sigmoid(input(ii, jj));
      }
    }
    break;
  }
}

void yann::FastSigmoidFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  this->f(input, output);
  output.array() =  output.array() * (1 -  output.array());
}
unique_ptr<ActivationFunction> yann::FastSigmoidFunction::copy() const
{
  // can't use make_unique<> because this constructor is private
  auto res = new FastSigmoidFunction(_table, _max_value);
  return unique_ptr<ActivationFunction>(res);
}

// tanh function:
//  f(x) = A * tanh(S*x)
//  df/dx = A * S * (1−(tanh(S*x))^2)
string yann::TanhFunction::get_info() const
{
  ostringstream oss;
  oss << "Tanh["
      << "A=" << _AA
      << ", S=" << _SS
      << "]"
      ;
  return oss.str();
}

void yann::TanhFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  switch(mode) {
  case Operation_Assign:
    output.array() = _AA * tanh(input.array() * _SS);
    break;
  case Operation_PlusEqual:
    output.array() += _AA * tanh(input.array()* _SS);
    break;
  }
}

void yann::TanhFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  this->f(input, output);
  output.array() = (1 - tanh(input.array()* _SS).square()) * (_AA * _SS);
}
unique_ptr<ActivationFunction> yann::TanhFunction::copy() const
{
  return make_unique<TanhFunction>(_AA, _SS);
}

// quadratic cost function:
//  f(actual, expected) = sum((actual<i> - expected<i>)^2)
//  d f(actual, expected) / d (actual) = (actual<i> - expected<i>)
string yann::QuadraticCost::get_info() const
{
  return "Quadratic";
}
Value yann::QuadraticCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  return ((actual.array() - expected.array()).square()).sum();
}
void yann::QuadraticCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));
  output.noalias() = 2 * (actual - expected);
}
unique_ptr<CostFunction> yann::QuadraticCost::copy() const
{
  return make_unique<QuadraticCost>();
}

// Exponential cost:
//  f(actual, expected) = tau * exp(sum((actual<i> - expected<i>)^2) / tau)
//  d f(actual, expected) / d (actual) = 2 * f(actual, expected) * (actual<i> - expected<i>)
string yann::ExponentialCost::get_info() const
{
  ostringstream oss;
  oss << "Exponential["
      << "tau=" << _tau
      << "]"
      ;
  return oss.str();
}
Value yann::ExponentialCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  const auto val = (actual.array() - expected.array()).square().sum();
  return _tau * exp(val / _tau);
}
void yann::ExponentialCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));
  output.noalias() = (2 * f(actual, expected)) * (actual - expected);
}
unique_ptr<CostFunction> yann::ExponentialCost::copy() const
{
  return make_unique<ExponentialCost>(_tau);
}

// cross entropy function:
//  f(actual, expected) = sum(-(expected * ln(actual) + (1 - expected) * ln(1 - actual)))
//  d f(actual, expected) / d (actual) = (actual - expected) / ((1 - actual) * actual)
string yann::CrossEntropyCost::get_info() const
{
  ostringstream oss;
  oss << "CrossEntropy["
      << "epsilon=" << _epsilon
      << "]"
      ;
  return oss.str();
}

Value yann::CrossEntropyCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));

  // iterate over elements manually and use small _epsilon to avoid hitting nan
  Value res = 0;
  for(MatrixSize ii = 0; ii < actual.rows(); ++ii) {
    for(MatrixSize jj = 0; jj < actual.cols(); ++jj) {
      auto aa = actual(ii, jj);
      auto ee = expected(ii, jj);
      if(aa <= 0) {
        aa = _epsilon;
      } else if(aa >= 1) {
        aa = 1 - _epsilon;
      }
      res += (ee * log(aa) + (1 - ee) * log(1 - aa));
    }
  }
  return -(res);
}

void yann::CrossEntropyCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));

  // iterate over elements manually and use small _epsilon to avoid hitting nan
  for(MatrixSize ii = 0; ii < actual.rows(); ++ii) {
    for(MatrixSize jj = 0; jj < actual.cols(); ++jj) {
      auto aa = actual(ii, jj);
      auto ee = expected(ii, jj);
      if(fabs(aa - ee) > _epsilon) {
        if(aa <= 0) {
          aa = _epsilon;
        } else if(aa >= 1) {
          aa = 1 - _epsilon;
        }
        output(ii, jj) = (aa - ee) / ((1 - aa) * aa);
      } else {
        output(ii, jj) = 0;
      }
    }
  }
}

unique_ptr<CostFunction> yann::CrossEntropyCost::copy() const
{
  return make_unique<CrossEntropyCost>(_epsilon);
}

// Hellinger distance cost:
//  f(actual, expected) = sum(sqrt(actual) - sqrt(expected))^2
//  d f(actual, expected) / d (actual) =  (1 -  sqrt(expected) /  sqrt(actual))
string yann::HellingerDistanceCost::get_info() const
{
  ostringstream oss;
  oss << "HellingerDistance["
      << "epsilon=" << _epsilon
      << "]"
      ;
  return oss.str();
}

Value yann::HellingerDistanceCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  return (((actual.array() + _epsilon).sqrt() - expected.array().sqrt()).square()).sum();
}

void yann::HellingerDistanceCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));
  output.array() = (1 - expected.array().sqrt() / (actual.array() + _epsilon).sqrt());
}
unique_ptr<CostFunction> yann::HellingerDistanceCost::copy() const
{
  return make_unique<HellingerDistanceCost>(_epsilon);
}

// Squared Hinge Loss:
//  f(actual, expected) = (max(0, 1 - actual * expected))^2
//  d f(actual, expected) / d (actual(i)) =  if (actual * expected) > 1 then 0; otherwise -2 * (1 - actual * expected) * expected(i)
string yann::SquaredHingeLoss::get_info() const
{
  return "SquaredHingeLoss";
}
Value yann::SquaredHingeLoss::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));

  Value res = 0;
  for(MatrixSize ii = 0; ii < get_batch_size(actual); ++ii) {
    const auto aa = get_batch(actual, ii);
    const auto ee = get_batch(expected, ii);

    const Value vv = (aa.array() * ee.array()).sum();
    if(vv < 1.0) {
      res += (1.0 - vv) * (1.0 - vv);
    }
  }
  return res;
}

void yann::SquaredHingeLoss::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));

  for(MatrixSize ii = 0; ii < get_batch_size(actual); ++ii) {
    const auto aa = get_batch(actual, ii);
    const auto ee = get_batch(expected, ii);
    auto oo = get_batch(output, ii);

    const Value vv = (aa.array() * ee.array()).sum();
    if(vv < 1) {
      oo = - 2 * (1.0 - vv) * ee;
    } else {
      oo.setZero();
    }
  }
}

unique_ptr<CostFunction> yann::SquaredHingeLoss::copy() const
{
  return make_unique<SquaredHingeLoss>();
}


#include <iostream>

#include "KokkosCore/kokkosConfigCommon.h"
#include "KokkosCore/kokkosConfig.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include "../test_common.h"

using namespace Eigen;

using Matrix5d = Matrix<double, 5, 5>;

template <class C>
KOKKOS_INLINE_FUNCTION void printIt(C* m) {
#ifdef TEST_DEBUG
  printf("\nMatrix %dx%d\n", (int)m->rows(), (int)m->cols());
  for (u_int r = 0; r < m->rows(); ++r) {
    for (u_int c = 0; c < m->cols(); ++c) {
      printf("Matrix(%d,%d) = %f\n", r, c, (*m)(r, c));
    }
  }
#endif
}

KOKKOS_INLINE_FUNCTION void eigenValues(Matrix3d* m, Eigen::SelfAdjointEigenSolver<Matrix3d>::RealVectorType* ret) {
#if TEST_DEBUG
  printf("Matrix(0,0): %f\n", (*m)(0, 0));
  printf("Matrix(1,1): %f\n", (*m)(1, 1));
  printf("Matrix(2,2): %f\n", (*m)(2, 2));
#endif
  SelfAdjointEigenSolver<Matrix3d> es;
  es.computeDirect(*m);
  (*ret) = es.eigenvalues();
  return;
}

KOKKOS_INLINE_FUNCTION void kernel(
    Kokkos::View<Matrix3d, KokkosExecSpace> vm,
    Kokkos::View<Eigen::SelfAdjointEigenSolver<Matrix3d>::RealVectorType, KokkosExecSpace> vret) {
  eigenValues(vm.data(), vret.data());
}

KOKKOS_INLINE_FUNCTION void kernelInverse3x3(Kokkos::View<Matrix3d, KokkosExecSpace> vm,
                                             Kokkos::View<Matrix3d, KokkosExecSpace> vmret) {
  vmret() = vm().inverse();
}

KOKKOS_INLINE_FUNCTION void kernelInverse4x4(Kokkos::View<Matrix4d, KokkosExecSpace> vm,
                                             Kokkos::View<Matrix4d, KokkosExecSpace> vmret) {
  vmret() = vm().inverse();
}

KOKKOS_INLINE_FUNCTION void kernelInverse5x5(Kokkos::View<Matrix5d, KokkosExecSpace> vm,
                                             Kokkos::View<Matrix5d, KokkosExecSpace> vmret) {
  vmret() = vm().inverse();
}

template <typename M1, typename M2, typename M3>
KOKKOS_INLINE_FUNCTION void kernelMultiply(Kokkos::View<M1, KokkosExecSpace> d_j,
                                           Kokkos::View<M2, KokkosExecSpace> d_c,
                                           Kokkos::View<M3, KokkosExecSpace> d_result) {
//  Map<M3> res(result->data());
#if TEST_DEBUG
  printf("*** GPU IN ***\n");
#endif
  printIt(d_j.data());
  printIt(d_c.data());
  //  res.noalias() = (*J) * (*C);
  //  printIt(&res);
  d_result() = d_j() * d_c();
#if TEST_DEBUG
  printf("*** GPU OUT ***\n");
#endif
  return;
}

template <int row1, int col1, int row2, int col2>
void testMultiply() {
  std::cout << "TEST MULTIPLY" << std::endl;
  std::cout << "Product of type " << row1 << "x" << col1 << " * " << row2 << "x" << col2 << std::endl;

  Kokkos::View<Matrix<double, row1, col1>, KokkosExecSpace> d_j("d_j");
  Kokkos::View<Matrix<double, row2, col2>, KokkosExecSpace> d_c("d_c");
  Kokkos::View<Matrix<double, row1, col2>, KokkosExecSpace> d_multiply_result("d_multiply_result");

  auto h_j = Kokkos::create_mirror_view(d_j);
  auto h_c = Kokkos::create_mirror_view(d_c);
  auto h_multiply_result = Kokkos::create_mirror_view(d_multiply_result);

  fillMatrix(h_j());
  fillMatrix(h_c());
  h_multiply_result() = h_j() * h_c();
  auto multiply_result = h_multiply_result();

#if TEST_DEBUG
  std::cout << "Input J:" << std::endl;
  printIt(h_j.data());
  std::cout << "Input C:" << std::endl;
  printIt(h_c.data());
  std::cout << "Output:" << std::endl;
  printIt(&multiply_result);
#endif
  // GPU
  Eigen::Matrix<double, row1, col2>* multiply_resultGPUret = new Eigen::Matrix<double, row1, col2>();

  Kokkos::deep_copy(KokkosExecSpace(), d_j, h_j);
  Kokkos::deep_copy(KokkosExecSpace(), d_c, h_c);
  Kokkos::deep_copy(KokkosExecSpace(), d_multiply_result, h_multiply_result);

  auto policy = Kokkos::RangePolicy<KokkosExecSpace>(KokkosExecSpace(), 0, 1);
  Kokkos::parallel_for(
      "kernelMultiply", policy, KOKKOS_LAMBDA(const int& i) { kernelMultiply(d_j, d_c, d_multiply_result); });
  KokkosExecSpace().fence();

  Kokkos::deep_copy(KokkosExecSpace(), h_multiply_result, d_multiply_result);
  printIt(h_multiply_result.data());
  assert(isEqualFuzzy(multiply_result, h_multiply_result()));
}

void testInverse3x3() {
  std::cout << "TEST INVERSE 3x3" << std::endl;

  Kokkos::View<Matrix3d, KokkosExecSpace> d_m("d_m");
  Kokkos::View<Matrix3d, KokkosExecSpace> d_mret("d_mret");

  auto h_m = Kokkos::create_mirror_view(d_m);
  auto h_mret = Kokkos::create_mirror_view(d_mret);

  fillMatrix(h_m());
  h_m() += h_m().transpose().eval();

  Matrix3d m_inv = h_m().inverse();

#if TEST_DEBUG
  std::cout << "Here is the matrix m:" << std::endl << h_m() << std::endl;
  std::cout << "Its inverse is:" << std::endl << m_inv << std::endl;
#endif
  Kokkos::deep_copy(KokkosExecSpace(), d_m, h_m);

  auto policy = Kokkos::RangePolicy<KokkosExecSpace>(KokkosExecSpace(), 0, 1);
  Kokkos::parallel_for(
      "kernelInverse3x3", policy, KOKKOS_LAMBDA(const int& i) { kernelInverse3x3(d_m, d_mret); });
  Kokkos::deep_copy(KokkosExecSpace(), h_mret, d_mret);
  KokkosExecSpace().fence();

#if TEST_DEBUG
  std::cout << "Its GPU inverse is:" << std::endl << h_mret() << std::endl;
#endif
  assert(isEqualFuzzy(m_inv, h_mret()));
}

void testInverse4x4() {
  std::cout << "TEST INVERSE 4x4" << std::endl;

  Kokkos::View<Matrix4d, KokkosExecSpace> d_m("d_m");
  Kokkos::View<Matrix4d, KokkosExecSpace> d_mret("d_mret");

  auto h_m = Kokkos::create_mirror_view(d_m);
  auto h_mret = Kokkos::create_mirror_view(d_mret);

  fillMatrix(h_m());
  h_m() += h_m().transpose().eval();

  Matrix4d m_inv = h_m().inverse();

#if TEST_DEBUG
  std::cout << "Here is the matrix m:" << std::endl << h_m() << std::endl;
  std::cout << "Its inverse is:" << std::endl << m_inv << std::endl;
#endif
  Kokkos::deep_copy(KokkosExecSpace(), d_m, h_m);

  auto policy = Kokkos::RangePolicy<KokkosExecSpace>(KokkosExecSpace(), 0, 1);
  Kokkos::parallel_for(
      "kernelInverse4x4", policy, KOKKOS_LAMBDA(const int& i) { kernelInverse4x4(d_m, d_mret); });
  Kokkos::deep_copy(KokkosExecSpace(), h_mret, d_mret);
  KokkosExecSpace().fence();
#if TEST_DEBUG
  std::cout << "Its GPU inverse is:" << std::endl << h_mret() << std::endl;
#endif
  assert(isEqualFuzzy(m_inv, h_mret()));
}

void testInverse5x5() {
  std::cout << "TEST INVERSE 5x5" << std::endl;

  Kokkos::View<Matrix5d, KokkosExecSpace> d_m("d_m");
  Kokkos::View<Matrix5d, KokkosExecSpace> d_mret("d_mret");

  auto h_m = Kokkos::create_mirror_view(d_m);
  auto h_mret = Kokkos::create_mirror_view(d_mret);

  fillMatrix(h_m());
  h_m() += h_m().transpose().eval();

  Matrix5d m_inv = h_m().inverse();

#if TEST_DEBUG
  std::cout << "Here is the matrix m:" << std::endl << h_m() << std::endl;
  std::cout << "Its inverse is:" << std::endl << m_inv << std::endl;
#endif
  Kokkos::deep_copy(KokkosExecSpace(), d_m, h_m);

  auto policy = Kokkos::RangePolicy<KokkosExecSpace>(KokkosExecSpace(), 0, 1);
  Kokkos::parallel_for(
      "kernelInverse5x5", policy, KOKKOS_LAMBDA(const int& i) { kernelInverse5x5(d_m, d_mret); });
  Kokkos::deep_copy(KokkosExecSpace(), h_mret, d_mret);
  KokkosExecSpace().fence();
#if TEST_DEBUG
  std::cout << "Its GPU inverse is:" << std::endl << h_mret() << std::endl;
#endif
  assert(isEqualFuzzy(m_inv, h_mret()));
}

void testEigenvalues() {
  std::cout << "TEST EIGENVALUES" << std::endl;

  Kokkos::View<Matrix3d, KokkosExecSpace> d_m("d_m");
  Kokkos::View<Eigen::SelfAdjointEigenSolver<Matrix3d>::RealVectorType, KokkosExecSpace> d_ret("d_ret");

  auto h_m = Kokkos::create_mirror_view(d_m);
  auto h_ret = Kokkos::create_mirror_view(d_ret);

  fillMatrix(h_m());
  h_m() += h_m().transpose().eval();

  Eigen::SelfAdjointEigenSolver<Matrix3d>::RealVectorType* ret =
      new Eigen::SelfAdjointEigenSolver<Matrix3d>::RealVectorType;
  eigenValues(h_m.data(), ret);
#if TEST_DEBUG
  std::cout << "Generated Matrix M 3x3:\n" << h_m() << std::endl;
  std::cout << "The eigenvalues of M are:" << std::endl << (*ret) << std::endl;
  std::cout << "*************************\n\n" << std::endl;
#endif
  Kokkos::deep_copy(KokkosExecSpace(), d_m, h_m);

  auto policy = Kokkos::RangePolicy<KokkosExecSpace>(KokkosExecSpace(), 0, 1);
  Kokkos::parallel_for(
      "kernel", policy, KOKKOS_LAMBDA(const int& i) { kernel(d_m, d_ret); });
  Kokkos::deep_copy(KokkosExecSpace(), h_m, d_m);
  Kokkos::deep_copy(KokkosExecSpace(), h_ret, d_ret);
  KokkosExecSpace().fence();

#if TEST_DEBUG
  std::cout << "GPU Generated Matrix M 3x3:\n" << (h_m()) << std::endl;
  std::cout << "GPU The eigenvalues of M are:" << std::endl << (h_ret()) << std::endl;
  std::cout << "*************************\n\n" << std::endl;
#endif
  assert(isEqualFuzzy(*ret, h_ret()));
}

int main(int argc, char* argv[]) {
  kokkos_common::InitializeScopeGuard kokkosGuard({KokkosBackend<KokkosExecSpace>::value});
  testEigenvalues();
  testInverse3x3();
  testInverse4x4();
  // disable testInverse5x5 since it will result in runtime error: what():  cudaMemcpyAsync(dst, src, n, cudaMemcpyDefault, instance.cuda_stream()) error( cudaErrorLaunchFailure): unspecified launch failure
  // testInverse5x5();

  testMultiply<1, 2, 2, 1>();
  testMultiply<1, 2, 2, 2>();
  testMultiply<1, 2, 2, 3>();
  testMultiply<1, 2, 2, 4>();
  testMultiply<1, 2, 2, 5>();
  testMultiply<2, 1, 1, 2>();
  testMultiply<2, 1, 1, 3>();
  testMultiply<2, 1, 1, 4>();
  testMultiply<2, 1, 1, 5>();
  testMultiply<2, 2, 2, 2>();
  testMultiply<2, 3, 3, 1>();
  testMultiply<2, 3, 3, 2>();
  testMultiply<2, 3, 3, 4>();
  testMultiply<2, 3, 3, 5>();
  testMultiply<3, 2, 2, 3>();
  testMultiply<2, 3, 3, 3>();  // DOES NOT COMPILE W/O PATCHING EIGEN
  testMultiply<3, 3, 3, 3>();
  testMultiply<8, 8, 8, 8>();
  testMultiply<3, 4, 4, 3>();
  testMultiply<2, 4, 4, 2>();
  testMultiply<3, 4, 4, 2>();  // DOES NOT COMPILE W/O PATCHING EIGEN

  return 0;
}


#include <Eigen/Dense>
#include <string.h>
#include <fstream>
#include <iostream>
using namespace std;

#define MODEL_PATH_DISTANCE 100
#define POLYFIT_DEGREE 4

Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;

void poly_fit(float *in_pts, float *in_stds, float *out) {
  // References to inputs
  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > pts(in_pts, MODEL_PATH_DISTANCE);
  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > std(in_stds, MODEL_PATH_DISTANCE);
  Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE, 1> > p(out, POLYFIT_DEGREE);

  // Build Least Squares equations
  Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> lhs = vander.array().colwise() / std.array();
  Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();

  // Improve numerical stability
  Eigen::Matrix<float, POLYFIT_DEGREE, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
  lhs = lhs * scale.asDiagonal();

  // Solve inplace
  Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
  p = qr.solve(rhs);

  // Apply scale to output
  p = p.transpose() * scale.asDiagonal();
}

int main(int argc, char** argv)
{
	for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
    for(int j = 0; j < POLYFIT_DEGREE; j++) {
      vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
    }
  }
  float* points = new float[MODEL_PATH_DISTANCE]();
  float* stds = new float[MODEL_PATH_DISTANCE]();
  float* poly = new float[POLYFIT_DEGREE]();
  string filename = argv[1];
  string name = filename + "_pts";
  fstream file(name.c_str());
  float tp;
  int idx=0;
  while(file >> tp)
  {
    // cout << tp << " ";
    points[idx] = tp;
    idx++;
  }
  cout << endl;
  file.close();
  name = filename+"_stds";
  fstream file2(name.c_str());
  idx=0;
  while(file2 >> tp)
  {
    // cout << tp << " ";
    stds[idx] = tp;
    idx++;
  }

  poly_fit(points, stds, poly);
  cout << poly[0];
  for (int i=1;i<POLYFIT_DEGREE;i++)
  {
    cout << " " << poly[i];
  }
  cout << endl;
	return 0;
}


/*
 * Author: Mario Lüder
 * Date: Nov. 2018
 */

#ifndef MOTIONUKF_HPP
#define MOTIONUKF_HPP

#include "Ukf.hpp"
#include "tools.h"

#include <Eigen/Dense>
#include <sstream>

class ProcessModel
{
public:
   struct MotionParams
   {
      double p_x;
      double p_y;
      double v;
      double yaw;
      double yaw_dot;
      double std_a;
      double std_yaw_dot_dot;
   };

   static inline void normalize(const long colIdx, Eigen::MatrixXd &XSigmaPointsDiff)
   {
      double & yaw = XSigmaPointsDiff.col(colIdx)(3);

      // normalize phi to -/+ pi
      Tools::normalizeAngle(yaw);
   }

   static void predictSigmaPoints(const Eigen::MatrixXd & XSigmaPoints, const double d_t_s, Eigen::MatrixXd & XSigmaPointsPred);
   static inline void processModel(const MotionParams & x, const double d_t_s, MotionParams & x_pred);

   //set state dimension
   static constexpr long m_n_x = 5;

   //set augumented state dimension
   static constexpr long m_n_x_aug = 7;

   // constant 2 * PI
   static constexpr double TWO_PI = 2 * M_PI;

   static constexpr long m_nSigmaPoints = 1 + 2 * m_n_x_aug;

   // Process noise standard deviation longitudinal acceleration in m/s^2
   // static constexpr double m_std_a = 0.2;
   static constexpr double m_std_a = 1.5;

   // Process noise standard deviation yaw acceleration in rad/s^2
   // static constexpr double m_std_yawdd = 0.2;
   static constexpr double m_std_yawdd = 1;
};

class MotionUkf : public Ukf<ProcessModel>
{
public:
   MotionUkf()
      : Ukf<ProcessModel>()
   {}

   virtual ~MotionUkf();

   struct Measurement
   {
      Measurement(int32_t dim) : value(dim) {}
      Eigen::VectorXd value;
   };

   struct RadarMeasurement : public Measurement
   {
      RadarMeasurement() : Measurement(3) {}
      RadarMeasurement(std::istringstream & iss)  : Measurement(3)
      {
         double rho;
         double phi;
         double rho_dot;
         iss >> rho;
         iss >> phi;
         iss >> rho_dot;

         value << rho, phi, rho_dot;
      }

      RadarMeasurement & operator=(std::istringstream & iss)
      {
         double rho;
         double phi;
         double rho_dot;
         iss >> rho;
         iss >> phi;
         iss >> rho_dot;

         value << rho, phi, rho_dot;
         return  *this;
      }

      double & rho() {return value(0);}
      double & phi() {return value(1);}
      double & rho_dot() {return value(2);}

      struct Definition
      {
         double rho;
         double phi;
         double rho_dot;
      };

      static Definition * cast(void * data)
      {
         return reinterpret_cast<Definition*>(data);
      }
   };

   struct RadarMeasurementModel
   {
      using MEASUREMENT_T = RadarMeasurement;

      RadarMeasurementModel();

      static inline void normalize(const long colIdx, Eigen::MatrixXd &ZSigmaPointsDiff)
      {
         //double & phi = reinterpret_cast<RadarMeasurement*>(ZSigmaPointsDiff.col(colIdx).data())->phi;
         double & phi = RadarMeasurement::cast(ZSigmaPointsDiff.col(colIdx).data())->phi;

         // normalize phi to -/+ pi
         Tools::normalizeAngle(phi);
      }

      static inline void normalize(Eigen::MatrixXd &matrix)
      {
         const int cols = matrix.cols();

         for (int colIdx = 0; colIdx < cols; ++colIdx)
         {
            normalize(colIdx, matrix);
         }
      }

      static inline void normalize(Eigen::VectorXd &vector)
      {
         double & phi = RadarMeasurement::cast(vector.data())->phi;

         // normalize phi to -/+ pi
         Tools::normalizeAngle(phi);
      }

      void predictCovar(const Ukf<ProcessModel> & ukf, const Eigen::MatrixXd & XSigmaPoints, const Eigen::VectorXd & x_pred, Eigen::MatrixXd & P_pred) const;
      void predictMeasurement(const Eigen::MatrixXd & XSigmaPointPred, Eigen::MatrixXd & ZSigmaPointsRadarPred) const;

      static constexpr long nRadarValues = 3;

      //radar measurement noise standard deviation radius in m
      const double m_std_radr = 0.3;

      //radar measurement noise standard deviation angle in rad
      const double m_std_radphi = 0.03;

      //radar measurement noise standard deviation radius change in m/s
      const double m_std_radrd = 0.3;

      MEASUREMENT_T measurement;

      // NIS
      double m_nis = 0.0;

   private:
      Eigen::MatrixXd m_R;
   };

   struct LaserMeasurement : public Measurement
   {
      LaserMeasurement() : Measurement(2) {}
      LaserMeasurement(std::istringstream & iss) : Measurement(2)
      {
         double px;
         double py;
         iss >> px;
         iss >> py;
         value << px , py;
      }

      LaserMeasurement & operator=(std::istringstream & iss)
      {
         double px;
         double py;
         iss >> px;
         iss >> py;
         value << px, py;
         return *this;
      }

      double & px() {return value(0);}
      double & py() {return value(1);}

      struct Definition
      {
         double px;
         double py;
      };

      static Definition * cast(void * data)
      {
         return reinterpret_cast<Definition*>(data);
      }
   };

   struct LaserMeasurementModel
   {
      using MEASUREMENT_T = LaserMeasurement;

      LaserMeasurementModel();

      static inline void normalize(const long colIdx, Eigen::MatrixXd &ZSigmaPointsDiff)
      {
         (void)colIdx;
         (void)ZSigmaPointsDiff;
         // nothing to normalize
      }

      static inline void normalize(Eigen::MatrixXd &matrix)
      {
         (void)matrix;
      }

      static inline void normalize(Eigen::VectorXd &vector)
      {
         (void)vector;
      }

      void predictCovar(const Ukf<ProcessModel> & ukf, const Eigen::MatrixXd & XSigmaPoints, const Eigen::VectorXd & x_pred, Eigen::MatrixXd & P_pred) const;
      void predictMeasurement(const Eigen::MatrixXd & XSigmaPointPred, Eigen::MatrixXd & ZSigmaPointsLaserPred) const;

      // Process noise standard deviation longitudinal acceleration in m/s^2
      double m_std_a = 30;

      //laser measurement noise standard deviation (position) in m
      const double m_std_laser_pos = 0.15;

      static constexpr long nLaserValues = 2;

      MEASUREMENT_T measurement;

      // NIS
      double m_nis = 0.0;
   private:

      // Laser Measurement Covariance Matrix
      Eigen::MatrixXd m_L;
   };

   template<typename MEASUREMENTMODEL_T>
   void processMeasurement(MEASUREMENTMODEL_T & measurementModel, int64_t timestamp_us)
   {
      Ukf<ProcessModel>::processMeasurement<typename MEASUREMENTMODEL_T::MEASUREMENT_T, MEASUREMENTMODEL_T>(measurementModel.measurement, measurementModel, timestamp_us);
   }

   void init(const LaserMeasurementModel & laserMeasurementModel, int64_t timestamp_us)
   {
      Ukf<ProcessModel>::setNewTime(timestamp_us);
      m_x.setZero(ProcessModel::m_n_x);

      // set initial x pos
      m_x(0) = laserMeasurementModel.measurement.value(0);

      // set initial y pos
      m_x(1) = laserMeasurementModel.measurement.value(1);

      // initialize process noise
      m_std.setZero(2);
      m_std(0) = ProcessModel::m_std_a;
      m_std(1) = ProcessModel::m_std_yawdd;
   }

   RadarMeasurementModel radarMeasurementModel;
   LaserMeasurementModel laserMeasurementModel;
};

#endif // MOTIONUKF_HPP


#include <triumf/nmr/dipole_dipole.hpp>

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

#include <cmath>

double dipole_dipole(const double *x, const double *par) {
  double omega_d = std::abs(par[2] * par[3] * par[0] * par[0]);
  double factor = par[1] * (3.0 / 10.0);
  double normalization = factor / omega_d;
  return normalization * triumf::nmr::dipole_dipole::slr_rate<double>(
                             *x, par[0], par[1], par[2], par[3]);
}

double generic(const double *x, const double *par) {
  double B_d = par[0];
  double nu_c = par[1];
  double gamma_I = par[2];
  double omega = gamma_I * x[0];
  double omega_d = gamma_I * gamma_I * B_d * B_d;
  return triumf::nmr::dipole_dipole::j<double>(omega, nu_c) * par[1];
}

void plot_dipole_dipole() {

  const double B_min = 2e-5;
  const double B_max = 2e-1;

  const auto n_points = 200;

  const double B_d = 1e-5;
  const double nu_c = 1.0 / 23.8e-6;
  const double gamma_8Li = boost::math::constants::two_pi<double>() * 6.30221e6;
  const double gamma_93Nb =
      boost::math::constants::two_pi<double>() * 10.30221e6;

  TCanvas *canvas = new TCanvas();

  TF1 *f_dipole = new TF1("f_dipole", dipole_dipole, B_min, B_max, 4);
  f_dipole->SetTitle("");
  f_dipole->SetNpx(n_points);
  f_dipole->SetLineColor(kRed);
  f_dipole->SetParameter(0, B_d);
  f_dipole->SetParameter(1, nu_c);
  f_dipole->SetParameter(2, gamma_8Li);
  f_dipole->SetParameter(3, gamma_93Nb);

  f_dipole->GetHistogram()->GetXaxis()->SetTitle("B_{0} (T)");
  f_dipole->GetHistogram()->GetYaxis()->SetTitle(
      "[1/T_{1}(B_{0})] / [1/T_{1}(0)]");

  f_dipole->Draw();

  TF1 *f_generic = new TF1("f_generic", generic, B_min, B_max, 3);
  f_generic->SetTitle("");
  f_generic->SetLineColor(kBlue);
  f_generic->SetNpx(n_points);
  f_generic->SetParameter(0, B_d);
  f_generic->SetParameter(1, nu_c);
  f_generic->SetParameter(2, gamma_8Li);

  f_generic->GetHistogram()->GetXaxis()->SetTitle("B_{0} (T)");
  f_generic->GetHistogram()->GetYaxis()->SetTitle(
      "[1/T_{1}(B_{0})] / [1/T_{1}(0)]");

  f_generic->Draw("same");

  auto legend = new TLegend(0.15, 0.15, 0.60, 0.45);
  // option "C" allows to center the header
  legend->SetHeader("Models for dipole-dipole SLR 1/T_{1}", "C");
  legend->AddEntry(f_dipole, "Eq. (8.21) in Mehring (1983)", "l");
  legend->AddEntry(f_generic, "Generic BPP expression", "l");
  legend->Draw();

  // logarithmic scale
  gPad->SetLogx();
  gPad->SetLogy();

  // tick marks on all sides of the plot
  gPad->SetTickx();
  gPad->SetTicky();

  // grid lines
  gPad->SetGridx();
  gPad->SetGridy();

  canvas->Print("dipole-dipole.pdf", "EmbedFonts");
}


#include "geometrycentral/numerical/linear_solvers.h"
#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/simple_polygon_mesh.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include <emscripten/bind.h>
#include <emscripten/val.h>

#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/simple_polygon_mesh.h"

#include "interpolate_vectors.h"

using namespace emscripten;
using namespace geometrycentral;
using namespace geometrycentral::surface;

struct GeoMesh {
    std::unique_ptr<ManifoldSurfaceMesh> mesh;
    std::unique_ptr<VertexPositionGeometry> geom;
};

std::vector<Vector3> toList(ManifoldSurfaceMesh& mesh,
                            const VertexData<Vector3>& data) {
    std::vector<Vector3> vectorList;
    for (Vertex v : mesh.vertices()) {
        vectorList.push_back(data[v]);
    }
    return vectorList;
}

VertexData<Vector3> toData(ManifoldSurfaceMesh& mesh,
                           const std::vector<Vector3>& list) {
    VertexData<Vector3> data(mesh);
    for (size_t iV = 0; iV < mesh.nVertices(); iV++) {
        data[iV] = list[iV];
    }
    return data;
}

std::vector<Vector3> generateSmoothBoundaryField(GeoMesh& geo) {
    VertexData<Vector3> bField =
        generateSmoothBoundaryVectorField(*geo.mesh, *geo.geom);
    return toList(*geo.mesh, bField);
}

std::vector<Vector3> generateWavyBoundaryField(GeoMesh& geo, size_t frequency) {
    VertexData<Vector3> bField =
        generateWavyBoundaryVectorField(*geo.mesh, *geo.geom, frequency);
    return toList(*geo.mesh, bField);
}

std::vector<Vector3>
interpolateHarmonicFunction(GeoMesh& geo,
                            const std::vector<Vector3>& boundaryData) {
    return toList(*geo.mesh,
                  interpolateByHarmonicFunction(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData)));
}

std::vector<Vector3>
interpolateConnectionLaplacian(GeoMesh& geo,
                               const std::vector<Vector3>& boundaryData,
                               bool estimateNormalDirection) {
    return toList(*geo.mesh,
                  interpolateByConnectionLaplacian(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData),
                      estimateNormalDirection));
}

std::vector<Vector3>
interpolateStereographicProjection(GeoMesh& geo,
                                   const std::vector<Vector3>& boundaryData) {
    return toList(*geo.mesh,
                  interpolateByStereographicProjection(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData)));
}

std::vector<Vector3>
interpolateHarmonicMapToSphere(GeoMesh& geo,
                               const std::vector<Vector3>& boundaryData) {
    return toList(*geo.mesh,
                  interpolateByHarmonicMapToSphere(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData)));
}

// Stolen from Ricky Reusser https://observablehq.com/d/d0df0c04ce5c94FCC
template <typename T>
void copyToVector(const val& typedArray, std::vector<T>& vec) {
    unsigned int length = typedArray["length"].as<unsigned int>();
    val memory          = val::module_property("buffer");
    vec.reserve(length);
    val memoryView = typedArray["constructor"].new_(
        memory, reinterpret_cast<uintptr_t>(vec.data()), length);
    memoryView.call<void>("set", typedArray);
}

// Mostly stolen from Ricky Reusser https://observablehq.com/d/d0df0c04ce5c94fc
EMSCRIPTEN_BINDINGS(my_module) {
    value_array<Vector3>("Vector3")
        .element(&Vector3::x)
        .element(&Vector3::y)
        .element(&Vector3::z);
    value_array<Vector2>("Vector2").element(&Vector2::x).element(&Vector2::y);

    register_vector<Vector3>("VectorVector3");
    register_vector<size_t>("VectorSizeT");
    register_vector<std::vector<size_t>>("VectorVectorSizeT");

    class_<GeoMesh>("GCMesh")
        .function("polygons", optional_override([](GeoMesh& self) {
                      return self.mesh->getFaceVertexList();
                  }))
        .function("vertexCoordinates",
                  optional_override([](const GeoMesh& self) {
                      std::vector<Vector3> vCoords;
                      for (Vertex v : self.mesh->vertices())
                          vCoords.push_back(self.geom->inputVertexPositions[v]);
                      return vCoords;
                  }));

    function("readMesh",
             optional_override([](std::string str, std::string type = "") {
                 std::stringstream in;
                 in << str;

                 GeoMesh gMesh;
                 std::tie(gMesh.mesh, gMesh.geom) =
                     readManifoldSurfaceMesh(in, type);
                 return gMesh;
             }));

    function("generateSmoothBoundaryField", &generateSmoothBoundaryField);
    function("generateWavyBoundaryField", &generateWavyBoundaryField);

    function("interpolateHarmonicFunction", &interpolateHarmonicFunction);
    function("interpolateConnectionLaplacian", &interpolateConnectionLaplacian);
    function("interpolateStereographicProjection",
             &interpolateStereographicProjection);
    function("interpolateHarmonicMapToSphere", &interpolateHarmonicMapToSphere);
}


#pragma once

#include "../linesegment.hh"
#include "../../../util/Maybe.hh"

#include <Eigen/Core>

namespace bold
{
  template<typename T>
  class LineSegment2 : public LineSegment<T, 2>
  {
  public:
    typedef Eigen::Matrix<T,2,1> Point;

    LineSegment2(Point const& p1, Point const& p2)
    : LineSegment<T, 2>::LineSegment(p1, p2)
    {}

    LineSegment2(T x1, T y1, T x2, T y2)
    : LineSegment<T, 2>::LineSegment(Point(x1, y1), Point(x2, y2))
    {}

    using LineSegment<T, 2>::LineSegment;
    using LineSegment<T, 2>::operator=;

    Maybe<Point> tryIntersect(LineSegment2<T> const& other) const
    {
      double t;
      double u;
      return tryIntersect(other, t, u);
    }

    /**
    * Attempt to intersect two line segments.
    * Note that even if the line segments do not intersect, the t and u values will be set.
    * @param other the line to attempt intersection of this line with
    * @param t (output) the distance along this line at which intersection would occur, or NaN if lines are collinear/parallel
    * @param u (output) the distance along the other line at which intersection would occur, or NaN if lines are collinear/parallel
    * @return The point of intersection if within the line segments, or empty.
    */
    Maybe<Point> tryIntersect(LineSegment2<T> const& other, double& t, double& u) const
    {
      // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect

      Eigen::Vector2d pos1 = this->p1().template cast<double>();    // p
      Eigen::Vector2d pos2 = other.p1().template cast<double>();    // q
      Eigen::Vector2d dir1 = this->delta().template cast<double>(); // r
      Eigen::Vector2d dir2 = other.delta().template cast<double>(); // s

      // t = (q − p) × s / (r × s)
      // u = (q − p) × r / (r × s)

      double denom = fake2dCross(dir1, dir2);

      if (denom == 0)
      {
        // lines are collinear or parallel
        t = std::numeric_limits<double>::quiet_NaN();
        u = std::numeric_limits<double>::quiet_NaN();
        return Maybe<Point>::empty();
      }

      double t_numer = fake2dCross(pos2 - pos1, dir2);
      double u_numer = fake2dCross(pos2 - pos1, dir1);

      t = t_numer / denom;
      u = u_numer / denom;

      if (t < 0 || t > 1 || u < 0 || u > 1)
      {
        // line segments do not intersect within their ranges
        return Maybe<Point>::empty();
      }

      Eigen::Vector2d intersectionPoint = pos1 + dir1 * t;

      // If we are using integers, be sure to round the result before casting
      if (std::is_same<T,int>())
      {
        Eigen::Vector2d rounded = intersectionPoint.unaryExpr([](double v) { return round(v); });
        return Maybe<Point>(rounded.cast<T>());
      }

      return Maybe<Point>(intersectionPoint.cast<T>());
    }

  private:
    /**
     * Returns the magnitude of the vector that would result from a regular
     * 3D cross product of the input vectors, taking their Z values implicitly
     * as 0 (i.e. treating the 2D space as a plane in the 3D space)
     */
    inline static double fake2dCross(Eigen::Vector2d const& a, Eigen::Vector2d const& b)
    {
      return a.x()*b.y() - a.y()*b.x();
    }
  };

  typedef LineSegment2<double> LineSegment2d;
  typedef LineSegment2<float> LineSegment2f;
}


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

#include <vector>
#include <random>
#include <boost/multi_array.hpp>

#include <amgcl/detail/qr.hpp>
#include <amgcl/value_type/interface.hpp>
#include <amgcl/value_type/complex.hpp>
#include <amgcl/value_type/static_matrix.hpp>

template <class T>
struct make_random {
    static T get() {
        static std::mt19937 gen;
        static std::uniform_real_distribution<T> rnd;

        return rnd(gen);
    }
};

template <class T>
T random() {
    return make_random<T>::get();
}

template <class T>
struct make_random< std::complex<T> > {
    static std::complex<T> get() {
        return std::complex<T>( random<T>(), random<T>() );
    }
};

template <class T, int N, int M>
struct make_random< amgcl::static_matrix<T,N,M> > {
    typedef amgcl::static_matrix<T,N,M> matrix;
    static matrix get() {
        matrix A = amgcl::math::zero<matrix>();
        for(int i = 0; i < N; ++i)
            for(int j = 0; j < M; ++j)
                A(i,j) = make_random<T>::get();
        return A;
    }
};

template <class value_type, amgcl::detail::storage_order order>
void qr_factorize(int n, int m) {
    std::cout << "factorize " << n << " " << m << std::endl;
    typedef typename std::conditional<order == amgcl::detail::row_major,
            boost::c_storage_order,
            boost::fortran_storage_order
            >::type ma_storage_order;

    boost::multi_array<value_type, 2> A0(boost::extents[n][m], ma_storage_order());

    for(int i = 0; i < n; ++i)
        for(int j = 0; j < m; ++j)
            A0[i][j] = random<value_type>();

    boost::multi_array<value_type, 2> A = A0;

    amgcl::detail::QR<value_type> qr;

    qr.factorize(n, m, A.data(), order);

    // Check that A = QR
    int p = std::min(n, m);
    for(int i = 0; i < n; ++i) {
        for(int j = 0; j < m; ++j) {
            value_type sum = amgcl::math::zero<value_type>();

            for(int k = 0; k < p; ++k)
                sum += qr.Q(i,k) * qr.R(k,j);

            sum -= A0[i][j];

            BOOST_CHECK_SMALL(amgcl::math::norm(sum), 1e-8);
        }
    }
}

template <class value_type, amgcl::detail::storage_order order>
void qr_solve(int n, int m) {
    std::cout << "solve " << n << " " << m << std::endl;
    typedef typename std::conditional<order == amgcl::detail::row_major,
            boost::c_storage_order,
            boost::fortran_storage_order
            >::type ma_storage_order;

    typedef typename amgcl::math::rhs_of<value_type>::type rhs_type;

    boost::multi_array<value_type, 2> A0(boost::extents[n][m], ma_storage_order());

    for(int i = 0; i < n; ++i)
        for(int j = 0; j < m; ++j)
            A0[i][j] = random<value_type>();

    boost::multi_array<value_type, 2> A = A0;

    amgcl::detail::QR<value_type> qr;

    std::vector<rhs_type> f0(n, amgcl::math::constant<rhs_type>(1));
    std::vector<rhs_type> f = f0;

    std::vector<rhs_type> x(m);

    qr.solve(n, m, A.data(), f.data(), x.data(), order);

    std::vector<rhs_type> Ax(n);
    for(int i = 0; i < n; ++i) {
        rhs_type sum = amgcl::math::zero<rhs_type>();
        for(int j = 0; j < m; ++j)
            sum += A0[i][j] * x[j];

        Ax[i] = sum;

        if (n < m) {
            BOOST_CHECK_SMALL(amgcl::math::norm(sum - f0[i]), 1e-8);
        }
    }

    if (n >= m) {
        for(int i = 0; i < m; ++i) {
            rhs_type sumx = amgcl::math::zero<rhs_type>();
            rhs_type sumf = amgcl::math::zero<rhs_type>();

            for(int j = 0; j < n; ++j) {
                sumx += amgcl::math::adjoint(A0[j][i]) * Ax[j];
                sumf += amgcl::math::adjoint(A0[j][i]) * f0[j];
            }

            rhs_type delta = sumx - sumf;

            BOOST_CHECK_SMALL(amgcl::math::norm(delta), 1e-8);
        }
    }
}

BOOST_AUTO_TEST_SUITE( test_qr )

BOOST_AUTO_TEST_CASE( test_qr_factorize ) {
    const int shape[][2] = {
        {3, 3},
        {3, 5},
        {5, 3},
        {5, 5}
    };

    const int n = sizeof(shape) / sizeof(shape[0]);

    for(int i = 0; i < n; ++i) {
        qr_factorize<double,                             amgcl::detail::row_major>(shape[i][0], shape[i][1]);
        qr_factorize<double,                             amgcl::detail::col_major>(shape[i][0], shape[i][1]);
        qr_factorize<std::complex<double>,               amgcl::detail::row_major>(shape[i][0], shape[i][1]);
        qr_factorize<std::complex<double>,               amgcl::detail::col_major>(shape[i][0], shape[i][1]);
        qr_factorize<amgcl::static_matrix<double, 2, 2>, amgcl::detail::row_major>(shape[i][0], shape[i][1]);
        qr_factorize<amgcl::static_matrix<double, 2, 2>, amgcl::detail::col_major>(shape[i][0], shape[i][1]);
    }
}

BOOST_AUTO_TEST_CASE( test_qr_solve ) {
    const int shape[][2] = {
        {3, 3},
        {3, 5},
        {5, 3},
        {5, 5}
    };

    const int n = sizeof(shape) / sizeof(shape[0]);

    for(int i = 0; i < n; ++i) {
        qr_solve<double,                             amgcl::detail::row_major>(shape[i][0], shape[i][1]);
        qr_solve<double,                             amgcl::detail::col_major>(shape[i][0], shape[i][1]);
        qr_solve<std::complex<double>,               amgcl::detail::row_major>(shape[i][0], shape[i][1]);
        qr_solve<std::complex<double>,               amgcl::detail::col_major>(shape[i][0], shape[i][1]);
        qr_solve<amgcl::static_matrix<double, 2, 2>, amgcl::detail::row_major>(shape[i][0], shape[i][1]);
        qr_solve<amgcl::static_matrix<double, 2, 2>, amgcl::detail::col_major>(shape[i][0], shape[i][1]);
    }
}

BOOST_AUTO_TEST_CASE( qr_issue_39 ) {
    boost::multi_array<double, 2> A0(boost::extents[2][2]);
    A0[0][0] = 1e+0;
    A0[0][1] = 1e+0;
    A0[1][0] = 1e-8;
    A0[1][1] = 1e+0;

    boost::multi_array<double, 2> A = A0;

    amgcl::detail::QR<double> qr;

    qr.factorize(2, 2, A.data());

    // Check that A = QR
    for(int i = 0; i < 2; ++i) {
        for(int j = 0; j < 2; ++j) {
            double sum = 0;
            for(int k = 0; k < 2; ++k)
                sum += qr.Q(i,k) * qr.R(k,j);

            sum -= A0[i][j];

            BOOST_CHECK_SMALL(sum, 1e-8);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*****************************************************************************
 * random.cpp        Blitz++ random numbers example
 */

#include <random/uniform.h>
#include <blitz/numinquire.h>
#include <time.h>
#include <iostream>
#include <iomanip>

using namespace ranlib;
using namespace blitz;

// workaround for broken streams in Compaq cxx, can't handle long double
#if defined(__DECCXX)
#define LD_HACK(x) static_cast<double>(x)
#else
#define LD_HACK(x) x
#endif

template<typename T>
void printRandoms()
{
  Uniform<T> x;
  //x.seed((unsigned int)time(0));
  x.seed(5);
  int N=5;
  for (int i = 0; i < N; ++i)
    cout << setprecision(digits10(T())) << LD_HACK(x.random()) << endl;

  cout << endl;
}

int main()
{
// test get/set state interface
  Uniform<double> x;
  Uniform<double>::T_state S = x.getState();
  x.setState(S);
  std::string str = x.getStateString();
  x.setState(str);

  cout << "Some random float: " << endl;
  printRandoms<float>();

  cout << "Some random doubles: " << endl;
  printRandoms<double>();

  cout << "Some random long doubles: " << endl;
  printRandoms<long double>();

  return 0;
}


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

using namespace std;

int main()
{
  Eigen::Matrix4f m;
  m << 1, 2, 3, 4,
       5, 6, 7, 8,
       9, 10,11,12,
       13,14,15,16;
  cout << "m.leftCols(2) =" << endl << m.leftCols(2) << endl << endl;
  cout << "m.bottomRows<2>() =" << endl << m.bottomRows<2>() << endl << endl;
  m.topLeftCorner(1,3) = m.bottomRightCorner(3,1).transpose();
  cout << "After assignment, m = " << endl << m << endl;
}


/* Daniel R. Reynolds
   SMU Mathematics
   7 August 2020 */

// Inclusions
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <armadillo>
using namespace std;

// Gram-Schmidt process for orthonormalizing a set of vectors
int GramSchmidt(arma::mat& X) {

  // check that there is work to do
  if (X.n_cols < 1)  return 0;

  // get entry magnitude (for linear dependence check)
  double Xmax = arma::norm(X,"inf");

  // normalize first column
  double colnorm = arma::norm(X.col(0));
  if (colnorm < 1.e-13*Xmax) {
    cerr << "GramSchmidt error: vectors are linearly-dependent!\n";
    return 1;
  }
  X.col(0) *= (1.0/colnorm);

  // iterate over remaining vectors, performing Gram-Schmidt process
  for (int i=1; i<X.n_cols; i++) {

    // subtract off portions in directions of existing basis vectors
    for (int j=0; j<i; j++)
      X.col(i) -= (arma::dot(X.col(i), X.col(j)) * X.col(j));

    // normalize vector, checking for linear dependence
    colnorm = arma::norm(X.col(i));
    if (colnorm < 1.e-13*Xmax) {
      cerr << "GramSchmidt error: vectors are linearly-dependent!\n";
      return 1;
    }
    X.col(i) *= (1.0/colnorm);
  }

  // return success
  return 0;
}


/*
Copyright (c) 2020 Inverse Palindrome
ProceduralX - ECS/Utility/AngleConversions.hpp
https://inversepalindrome.com/
*/

#pragma once

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

namespace ECS::Utility
{
    template<typename T>
    T degreesToRadians(T degrees)
    {
        return degrees * boost::math::constants::pi<T>() / (T)180;
    }

    template<typename T>
    T radiansToDegrees(T radians)
    {
        return radians * (T)180 / boost::math::constants::pi<T>();
    }
}