#include <ql/quantlib.hpp>

#include <boost/assign/std/vector.hpp>

#include <iostream>
#include <fstream>

using namespace QuantLib;
using namespace  boost::assign;

void bdkSmile() {

    std::ofstream out;
    out.open("bdk.dat");

    Real forward = 0.03;
    Real expiryTime = 20.0;

    std::vector<Real> sabrParams;
    sabrParams.push_back(0.15); // alpha
    sabrParams.push_back(0.8);    // beta
    sabrParams.push_back(0.50);   // nu
    sabrParams.push_back(-0.48);  // rho

    boost::shared_ptr<SabrSmileSection> sabr(
        new SabrSmileSection(expiryTime, forward, sabrParams));

    boost::shared_ptr<BdkSmileSection> bdk(new BdkSmileSection(sabr, 1.0, 1.0, 0.025, 0.06));

    Real strike = 0.0001;
    while (strike <= 0.10) {
        out << strike << " " << sabr->optionPrice(strike) << " "
            << bdk->optionPrice(strike) << " "
            << sabr->digitalOptionPrice(strike) << " "
            << bdk->digitalOptionPrice(strike) << " " << sabr->density(strike)
            << " " << bdk->density(strike) << " " << sabr->volatility(strike) << " " <<
            bdk->volatility(strike) << std::endl;
        strike += 0.0001;
    }

    out.close();

}

void zabrExamples() {

    Real forward = 0.03;
    Real expiryTime = 15;

    Real alpha = 0.10;
    Real beta = 0.7;
    Real nu = 0.20;
    Real rho = -0.4;
    Real gamma = 1.0;

    std::vector<Real> sabrParams, zabrParams;

    sabrParams += alpha, beta, nu, rho;
    zabrParams += alpha, beta, nu, rho, gamma;

    boost::shared_ptr<SabrSmileSection> sabr = boost::make_shared<SabrSmileSection>(expiryTime, forward, sabrParams);

    boost::shared_ptr<ZabrSmileSection> zabrln =
        boost::make_shared<ZabrSmileSection>(
            expiryTime, forward, zabrParams,
            ZabrSmileSection::ShortMaturityLognormal);

    boost::shared_ptr<ZabrSmileSection> zabrfull =
        boost::make_shared<ZabrSmileSection>(
            expiryTime, forward, zabrParams,
            ZabrSmileSection::FullFd, std::vector<Real>(), 2);

    boost::shared_ptr<ZabrSmileSection> zabrfd =
        boost::make_shared<ZabrSmileSection>(
            expiryTime, forward, zabrParams,
            ZabrSmileSection::LocalVolatility, std::vector<Real>(), 2);

    // only for debug
    boost::shared_ptr<ZabrModel> tmpZabrFull = boost::make_shared<ZabrModel>(
        expiryTime, forward, alpha, beta, nu, rho, gamma);

    std::ofstream out;
    out.open("smiles.dat");

    // debug full fd
    std::vector<Real> tmpFullFd(100);
    Size tmpIdx = 0;
#pragma omp parallel for
    for(Size i=0;i<100;++i)
        tmpFullFd[i] = 0.0;//tmpZabrFull->fullFdPrice(0.0001+i*0.0010);
    // end debug full fd

    Real strike = 0.0001;
    while (strike <= 1.00) {
        // 1 strike
        out << strike << " ";
        // 2, 3, 4, 5  sabr
        out << sabr->volatility(strike) << " "
                  << sabr->optionPrice(strike) << " "
                  << sabr->digitalOptionPrice(strike) << " "
                  << sabr->density(strike) << " ";
        // 6, 7, 8, 9  zabr (lognormal short mat)
        out << zabrln->volatility(strike) << " "
                  << zabrln->optionPrice(strike) << " "
                  << zabrln->digitalOptionPrice(strike) << " "
                  << zabrln->density(strike) << " ";
        // 10, 11, 12, 13  zabr (full fd)
        out <<zabrfull->volatility(strike) << " "
                  << zabrfull->optionPrice(strike) << " "
                  << zabrfull->digitalOptionPrice(strike) << " "
                  << zabrfull->density(strike) << " ";
        // 14, 15, 16, 17  zabr (fd)
        out <<zabrfd->volatility(strike) << " "
                  << zabrfd->optionPrice(strike) << " "
                  << zabrfd->digitalOptionPrice(strike) << " "
                  << zabrfd->density(strike) << " ";
        // 14 zabr price full fd (debug)
        out << tmpFullFd[tmpIdx++];
        strike += 0.0010;
        out << std::endl;
    }

    out.close();

}

void zabrPaper() {

    // compare different SABR formulas

    std::ofstream out;
    out.open("smiles.dat");

    // this is an example from the paper, forward is a guess (since not
    // mentioned there)

    Real forward = 0.0325;
    Real expiryTime = 10.0;

    std::vector<Real> sabrParams, zabrParams;

    sabrParams.push_back(0.0873); // alpha
    sabrParams.push_back(0.7);    // beta
    sabrParams.push_back(0.47);   // nu
    sabrParams.push_back(-0.48);  // rho

    // Black Scholes with 20%
    // sabrParams.push_back(0.20); // alpha
    // Sabrparams.push_back(1.0);    // beta
    // sabrParams.push_back(0.0001);   // nu
    // sabrParams.push_back(0.0);  // rho

    // BS beta = 0.7
    // sabrParams.push_back(0.08); // alpha
    // sabrParams.push_back(0.7);    // beta
    // sabrParams.push_back(0.0001);   // nu
    // sabrParams.push_back(0.0);  // rho

    // BScholes nu = 0.50
    // sabrParams.push_back(0.20); // alpha
    // sabrParams.push_back(1.0);    // beta
    // sabrParams.push_back(0.50);   // nu
    // sabrParams.push_back(0.0);  // rho

    zabrParams.push_back(sabrParams[0]);
    zabrParams.push_back(sabrParams[1]);
    zabrParams.push_back(sabrParams[2]);
    zabrParams.push_back(sabrParams[3]);
    zabrParams.push_back(1.0); // gamma

    std::vector<Real> zabrParamsg0(zabrParams);
    zabrParamsg0[4] = 0.0;
    std::vector<Real> zabrParamsg05(zabrParams);
    zabrParamsg05[4] = 0.5;
    std::vector<Real> zabrParamsg15(zabrParams);
    zabrParamsg15[4] = 1.5;
    std::vector<Real> zabrParamsg17(zabrParams);
    zabrParamsg17[4] = 1.7;

    boost::shared_ptr<SabrSmileSection> sabr(
        new SabrSmileSection(expiryTime, forward, sabrParams));

    boost::shared_ptr<ZabrSmileSection> zabrln(
        new ZabrSmileSection(expiryTime, forward, zabrParams,
                             ZabrSmileSection::ShortMaturityLognormal));

    boost::shared_ptr<ZabrSmileSection> zabrnm(
        new ZabrSmileSection(expiryTime, forward, zabrParams,
                             ZabrSmileSection::ShortMaturityNormal));

    boost::shared_ptr<ZabrSmileSection> zabrlv(new ZabrSmileSection(
        expiryTime, forward, zabrParams, ZabrSmileSection::LocalVolatility));

    boost::shared_ptr<ZabrSmileSection> zabrnmg0(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg0,
                             ZabrSmileSection::ShortMaturityNormal));
    boost::shared_ptr<ZabrSmileSection> zabrnmg05(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg05,
                             ZabrSmileSection::ShortMaturityNormal));
    boost::shared_ptr<ZabrSmileSection> zabrnmg15(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg15,
                             ZabrSmileSection::ShortMaturityNormal));
    boost::shared_ptr<ZabrSmileSection> zabrnmg17(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg17,
                             ZabrSmileSection::ShortMaturityNormal));

    // boost::shared_ptr<ZabrSmileSection> zabrfd(new ZabrSmileSection(
    //     expiryTime, forward, zabrParams, ZabrSmileSection::FullFd));

    boost::shared_ptr<ZabrModel> zabrmodel = zabrln->model();

    Real strike = 0.0001; // we start at 1bp ...

    while (strike <= 0.50) { // ... and go to 50% ...

        // debug: localvol function for sabr hardcoded
        Real ytmp = strike >= 0.0
                        ? 1.0 / (sabrParams[0] * (1.0 - sabrParams[1])) *
                              (pow(forward, 1.0 - sabrParams[1]) -
                               pow(strike, 1.0 - sabrParams[1]))
                        : 1.0 / (sabrParams[0] * (1.0 - sabrParams[1])) *
                              (pow(-strike, 1.0 - sabrParams[1]) +
                               pow(forward, 1.0 - sabrParams[1]));
        Real Jtmp = sqrt(1.0 + sabrParams[2] * sabrParams[2] * ytmp * ytmp -
                         2.0 * sabrParams[3] * sabrParams[2] * ytmp);
        Real localVol =
            Jtmp * pow(std::fabs(strike), sabrParams[1]) * sabrParams[0];
        // end debug

        out << strike << " "                           // 1  SABR hagan
            << sabr->volatility(strike) << " "         // 2
            << sabr->optionPrice(strike) << " "        // 3
            << sabr->digitalOptionPrice(strike) << " " // 4
            << sabr->density(strike) << " "            // 5
            << zabrln->volatility(strike)
            << " " // 6  ZABR short maturity lognormal
            << zabrln->optionPrice(strike) << " "        // 7
            << zabrln->digitalOptionPrice(strike) << " " // 8
            << zabrln->density(strike) << " "            // 9
            << zabrnm->volatility(strike)
            << " " // 10  ZABR short maturity normal
            << zabrnm->optionPrice(strike) << " "        // 11
            << zabrnm->digitalOptionPrice(strike) << " " // 12
            << zabrnm->density(strike) << " "            // 13
            << zabrmodel->localVolatility(strike)
            << " " // 14  *** Equivalent deterministic volatility
            << localVol
            << " " // 15  *** same as 14, but hardcoded for SABR (debug)
            << zabrlv->volatility(strike) << " "  // 16  ZABR local vol
            << zabrlv->optionPrice(strike) << " " // 17
            << zabrlv->digitalOptionPrice(strike, Option::Call, 1.0, 1E-4)
            << " "                                                // 18
            << zabrlv->density(strike, Option::Call, 1E-4) << " " // 19
            // << zabrfd->volatility(strike) << " "         // 20  ZABR fd
            // << zabrfd->optionPrice(strike) << " "        // 21
            // << zabrfd->digitalOptionPrice(strike) << " " // 22
            // << zabrfd->density(strike) << " "            // 23
            << zabrnmg0->volatility(strike)
            << " " // 20  ZABR short maturity normal gamma=0.0
            << zabrnmg05->volatility(strike)
            << " " // 21  ZABR short maturity normal gamma=0.5
            << zabrnmg15->volatility(strike)
            << " " // 22  ZABR short maturity normal gamma=1.5
            << zabrnmg17->volatility(strike)
            << " " // 23  ZABR short maturity normal gamma=1.7
            << std::endl;

        strike += 0.0001; // ... in steps of 5bp
    }

    out.close();
}

void splineSmiles() {

    std::ofstream out;
    out.open("smiles2.dat");

    // same example as in zabrPaper

    Real forward = 0.0325;
    Real expiryTime = 10.0;

    std::vector<Real> sabrParams;

    sabrParams.push_back(0.0873); // alphacd
    sabrParams.push_back(0.7);    // beta
    sabrParams.push_back(0.47);   // nu
    sabrParams.push_back(-0.48);  // rho

    std::vector<Real> money;
    for(Size i=0;i<40;i++) {
        money.push_back(i/40.0);
    }
    for(Size i=10;i<=100;i++) {
        money.push_back(i*0.1);
    }
    for(Size i=11;i<=100;i++) {
        money.push_back(i*1.0);
    }

    boost::shared_ptr<SabrSmileSection> sabr(
        new SabrSmileSection(expiryTime, forward, sabrParams));

    boost::shared_ptr<SmileSection> flatSmileSection(new FlatSmileSection(expiryTime,0.10,Actual365Fixed(),forward));

    boost::shared_ptr<SplineDensitySmileSection> spline(
        new SplineDensitySmileSection(flatSmileSection, forward, 0.0, 1.00,
                                      false, money));

    std::cout << "arbitrage free region is " << spline->leftCoreStrike()
              << " ... " << spline->rightCoreStrike() << std::endl;

    Real strike = 0.00001;

    while (strike <= 3.0) {

        Real vol = 0.10;
        Real mu = std::log(forward) + vol*vol*expiryTime/2.0;
        Real referencePrice = blackFormula(Option::Call,strike,forward,vol*sqrt(expiryTime));
        Real referenceDigital = blackFormulaCashItmProbability(Option::Call,strike,forward,vol*sqrt(expiryTime));
        NormalDistribution norm(mu,vol*sqrt(expiryTime));
        Real referenceDensity = norm(std::log(strike))/strike;

        out << strike << " "                                    // 1
            // << sabr->volatility(strike) << " "                  // 2
            // << sabr->optionPrice(strike) << " "                 // 3
            // << sabr->digitalOptionPrice(strike) << " "          // 4
            // << sabr->density(strike) << " "                     // 5
            << flatSmileSection->volatility(strike) << " "                  // 2
            << flatSmileSection->optionPrice(strike) << " "                 // 3
            << flatSmileSection->digitalOptionPrice(strike) << " "          // 4
            << flatSmileSection->density(strike) << " "                     // 5
            << spline->volatility(strike) << " "                // 6
            << spline->optionPrice(strike) << " "               // 7
            << spline->digitalOptionPrice(strike) << " "        // 8
            << spline->density(strike) << " "                   // 9
            // << referencePrice << " " // 5
            // << referenceDigital << " " //6
            // << referenceDensity << " "  //7
            // << referenceDigital - spline->digitalOptionPrice(strike) << " " // 8
            << std::endl;
        strike += 0.0010;
    }

    out.close();
}

void sbSmiles() {

    std::ofstream out;
    out.open("smiles3.dat");

    Real forward = 0.0325;
    Real expiryTime = 10.0;

    boost::shared_ptr<SbSmileSection> sb(new SbSmileSection(expiryTime,forward,
                                                            0.0,0.50,1.0,1.0));

    Real strike = -1.0;

    while (strike <= 1.0) {

        out << strike << " "                                    // 1
            //<< sb->volatility(strike) << " "                  // 2
            << sb->optionPrice(strike) << " "                 // 3
            << sb->digitalOptionPrice(strike) << " "          // 4
            << sb->density(strike) << " "                     // 5
            << std::endl;
        strike += 0.0010;
    }

    out.close();

}

void interpolation() {

    std::ofstream out;
    out.open("sample.dat");

    Real x[] = { 0.0, 0.5, 1.0, 1.5, 2.0 };
    Real y1[] = { 0.0, 0.5, 1.0, 0.5, 0.0 };
    Real y2[] = { 0.0, 0.5, 0.2, 0.5, 0.0 };

    std::vector<Real> xv(x,x+5);
    std::vector<Real> yv1(y1,y1+5);
    std::vector<Real> yv2(y2,y2+5);

    CubicInterpolation i1(xv.begin(), xv.end(), yv1.begin(), CubicInterpolation::Parabolic, false,
                         CubicInterpolation::FirstDerivative, 0.0, CubicInterpolation::FirstDerivative, 0.0);

    CubicInterpolation i2(xv.begin(), xv.end(), yv2.begin(), CubicInterpolation::Parabolic, false,
                         CubicInterpolation::FirstDerivative, 0.0, CubicInterpolation::FirstDerivative, 0.0);

    Real h = 0.0;
    while(h <= 2.01) {
        out << h << " " << i1(h) << " " << i2(h) << std::endl;
        h+=0.01;
    }

    out.close();

}

int main(int, char * []) {
    zabrExamples();
    // splineSmiles();
}


#include <boost/numeric/odeint.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>

using namespace boost::numeric::odeint;

using state_t = std::vector<double>;

int mod_subtract(int a, int b, int bound){
	//computes a - b mod bound
	int res = a - b;
	if(res < 0){
		while(res < 0){
			res += bound;
		}
	}
	else if(res >= bound){
		res -= bound;
	}
	return res;
}

void time_differentiate(const state_t& x, state_t& dxdt, const double /* type param */)
{
	auto J = x.size();

	//precondition: x and dx/dt have the same length
	assert(J == dxdt.size());

	auto bound = static_cast<long long>(J);

	double F = 8;

	for(int i = 0; i < bound; i++){
		dxdt[i] = ( x[(i+1) % bound] - x[mod_subtract(i, 2, bound)] ) * x[mod_subtract(i,1,bound)] - x[i] + F;
	}
}

void apply_initial_conditions(state_t& x){
	//apply initial conditions from the HW
	for(auto& val : x){
		val = 0;
	}
	x[1] = 1.0;
}

struct state_storage
{
	std::vector<state_t>& m_states;

	std::vector<double>& m_times;

	state_storage(std::vector<state_t>& states, std::vector<double>& times) :
																		m_states(states),
																		m_times(times) {};

	void operator()(const state_t& x, double t){
		m_states.push_back(x);
		m_times.push_back(t);
	}

};

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

	state_t x(10);

	std::vector<state_t> x_stored;
	std::vector<double> times;

	apply_initial_conditions(x);

	double time_length = 10.0;

	double init_step_size = 0.05;

	std::cout << "Integrating ODE over 10 second total time length.\n";

	//solve with runge_kutta54_cash_karp stepper
	size_t steps = integrate(time_differentiate, x, 0.0,
						   	 time_length, init_step_size,
							 state_storage(x_stored, times) );

	std::cout << "Solved ODE in " << steps << " timesteps.\n";

	std::ofstream filewriter;
	filewriter.open("hw1.csv");
	for(const auto& x : x_stored){
		for(const auto& val : x){
			filewriter << val << ", ";
		}
		filewriter << "\n";
	}

	filewriter.close();

	return 0;
}


// PythagoreanTriples.cpp
//
// Solving the classic Pythagorean triples problem with lazy Stream
//
#include <iostream>

#include <boost/timer/timer.hpp>

#include "Stream.h"
#include "Triple.h"

int main(int argc, char* argv[])
{
    std::cout << "--- triples(first 10)" << std::endl;
    std::cout << take(triples(), 10) << std::endl;
    std::cout << "--- pythagorean triples(within 20)" << std::endl;
    std::cout << triples(20, [](int x, int y, int z) { return (x*x + y*y == z*z); }) << std::endl;
    /// benchmark
    int count = argc > 1 ? std::atoi(argv[1]) : 50;
    std::cout << "--- benchmark (with lazy Stream)" << std::endl;
    int sum = 0;
    boost::timer::cpu_timer t;
    forEach(take(triples(INT_MAX-1, [&sum](int x, int y, int z) { return (x*x + y*y == z*z); }), count), [&sum](Triple t) { sum += (std::get<0>(t) + std::get<1>(t) + std::get<2>(t)); });
    std::cout << t.format() << std::endl;
    std::cout << "sum: " << sum << std::endl;
    /// nested loops
    std::cout << "--- benchmark (with nested loops)" << std::endl;
    sum = 0;
    int found = 0;
    bool done = false;
    boost::timer::cpu_timer t1;
    for (int z = 1; !done && z < INT_MAX; ++z) {
        for (int y = 1; !done && y < z; ++y) {
            for (int x = 1; !done && x <= y; ++x) {
                if (x*x + y*y == z*z) {
                    sum += (x + y + z);
                    if (++found == count)
                        done = true;
                }
            }
        }
    }
    std::cout << t1.format() << std::endl;
    std::cout << "sum: " << sum << std::endl;

    return 0;
}


/*
 * Copyright 2022 Sean McBane under the terms of the MIT license.
 * This file contains validation via method of manufactured solution for the
 * linear elasticity code. The analytical solution for displacement is given
 * by
 *
 * u_x = (-3.0/2.0*pow(x, 3) + (23.0/4.0)*pow(x, 2) - 5*x)*sin(M_PI*y) +
 *  ((3.0/2.0)*pow(x, 3) - 13.0/4.0*pow(x, 2) + 1)*cos(M_PI*y);
 *
 * u_y = -2*x*y + x + 2*y - 1;
 *
 * Young's modulus: E = 1 + sin(M_PI * x) * cos(M_PI * y) / 4;
 * Poisson's ratio: nu = 3 / 10;
 *
 * The functions below are all derived symbolically from these solutions.
 */

#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <cassert>
#include <chrono>
#include <cmath>

#include "convert_to_eigen.hpp"
#include "elasticity.hpp"
#include "get_triangles.hpp"
#include "mesh_tools.hpp"
#include "read_mesh.hpp"

using namespace Elasticity;
using namespace std::chrono;

double manufactured_ux(double x, double y)
{
    return (-3.0 / 2.0 * pow(x, 3) + (23.0 / 4.0) * pow(x, 2) - 5 * x) * sin(M_PI * y) +
           ((3.0 / 2.0) * pow(x, 3) - 13.0 / 4.0 * pow(x, 2) + 1) * cos(M_PI * y);
}

double manufactured_uy(double x, double y) { return -2 * x * y + x + 2 * y - 1; }

Eigen::Vector2d u_true(std::array<double, 2> coord)
{
    const auto [x, y] = coord;
    return Eigen::Vector2d(manufactured_ux(x, y), manufactured_uy(x, y));
}

Eigen::Vector2d u_right(std::array<double, 2> coord) { return u_true(coord); }

Eigen::Vector2d u_left(std::array<double, 2> coord) { return u_true(coord); }

Eigen::Vector2d u_upper(std::array<double, 2> coord) { return u_true(coord); }

Eigen::Vector2d u_lower(std::array<double, 2> coord) { return u_true(coord); }

/*
 * Definitions of the Lame parameters for the manufactured solution.
 */
double manufactured_lambda(std::array<double, 2> coord)
{
    auto [x, y] = coord;
    return (15.0 / 182.0) * sin(M_PI * x) * cos(M_PI * y) + 30.0 / 91.0;
}

double manufactured_mu(std::array<double, 2> coord)
{
    auto [x, y] = coord;
    return (5.0 / 52.0) * sin(M_PI * x) * cos(M_PI * y) + 5.0 / 13.0;
}

/*
 * Manufactured solution traction boundary condition for the lower boundary.
 */
double traction_lower_x(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 208.0) * (M_PI * x * (6 * pow(x, 2) - 23 * x + 20) - 4) * (sin(M_PI * x) + 4);
}

double traction_lower_y(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 364.0) * (sin(M_PI * x) + 4) * (-27 * pow(x, 2) + 79 * x - 40);
}

Eigen::Vector2d traction_lower(std::array<double, 2> coord) noexcept
{
    auto [x, y] = coord;
    return Eigen::Vector2d(traction_lower_x(x, y), traction_lower_y(x, y));
}

/*
 * Manufactured solution traction boundary condition for the upper boundary.
 */
double traction_upper_x(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 208.0) * (-M_PI * x * (6 * pow(x, 2) - 23 * x + 20) + 4) * (sin(M_PI * x) - 4);
}

double traction_upper_y(double x, [[maybe_unused]] double y) noexcept
{
    return (5.0 / 364.0) * (sin(M_PI * x) - 4) * (27 * pow(x, 2) + x - 40);
}

Eigen::Vector2d traction_upper(std::array<double, 2> coord) noexcept
{
    auto [x, y] = coord;
    return Eigen::Vector2d(traction_upper_x(x, y), traction_upper_y(x, y));
}

/*
 * Manufactured solution forcing term on \Omega
 */
double forcing_x(double x, double y) noexcept
{
    return (5.0 / 52.0) * ((13 - 18 * x) * cos(M_PI * y) + (18 * x - 23) * sin(M_PI * y)) *
               (sin(M_PI * x) * cos(M_PI * y) + 4) +
           (15.0 / 364.0) * (sin(M_PI * x) * cos(M_PI * y) + 4) *
               ((13 - 18 * x) * cos(M_PI * y) + (18 * x - 23) * sin(M_PI * y) + 4) +
           (5.0 / 208.0) * (sin(M_PI * x) * cos(M_PI * y) + 4) *
               (-pow(M_PI, 2) * x * (6 * pow(x, 2) - 23 * x + 20) * sin(M_PI * y) +
                pow(M_PI, 2) * (6 * pow(x, 3) - 13 * pow(x, 2) + 4) * cos(M_PI * y) + 8) -
           5.0 / 52.0 * M_PI *
               (x * (9 * x - 13) * cos(M_PI * y) +
                (-9 * pow(x, 2) + 23 * x - 10) * sin(M_PI * y)) *
               cos(M_PI * x) * cos(M_PI * y) +
           (15.0 / 364.0) * M_PI *
               (-x * (9 * x - 13) * cos(M_PI * y) + 4 * x +
                (9 * pow(x, 2) - 23 * x + 10) * sin(M_PI * y) - 4) *
               cos(M_PI * x) * cos(M_PI * y) -
           5.0 / 208.0 * M_PI *
               (M_PI * x * (6 * pow(x, 2) - 23 * x + 20) * cos(M_PI * y) + 8 * y +
                M_PI * (6 * pow(x, 3) - 13 * pow(x, 2) + 4) * sin(M_PI * y) - 4) *
               sin(M_PI * x) * sin(M_PI * y);
}

double forcing_y(double x, double y) noexcept
{
    return (5.0 / 1456.0) * M_PI *
           ((112 - 112 * x) * sin(M_PI * x) * sin(M_PI * y) +
            26 * (sin(M_PI * x) * cos(M_PI * y) + 4) *
                (x * (9 * x - 13) * sin(M_PI * y) +
                 (9 * pow(x, 2) - 23 * x + 10) * cos(M_PI * y)) -
            12 *
                (-x * (9 * x - 13) * cos(M_PI * y) + 4 * x +
                 (9 * pow(x, 2) - 23 * x + 10) * sin(M_PI * y) - 4) *
                sin(M_PI * x) * sin(M_PI * y) +
            7 *
                (M_PI * x * (6 * pow(x, 2) - 23 * x + 20) * cos(M_PI * y) + 8 * y +
                 M_PI * (6 * pow(x, 3) - 13 * pow(x, 2) + 4) * sin(M_PI * y) - 4) *
                cos(M_PI * x) * cos(M_PI * y));
}

Eigen::Vector2d forcing(std::array<double, 2> coord) noexcept
{
    auto [x, y] = coord;
    return Eigen::Vector2d(forcing_x(x, y), forcing_y(x, y));
}

auto compute_parameter_functions(const MeshVariant &mv)
{
    Eigen::VectorXd lambda = evaluate_on_mesh(mv, manufactured_lambda);
    Eigen::VectorXd mu = evaluate_on_mesh(mv, manufactured_mu);
    return std::pair(lambda, mu);
}

auto get_boundary_conditions(const MeshVariant &mv, size_t right_bound, size_t left_bound)
{
    Eigen::Matrix2Xd uright = evaluate_on_boundary(mv, right_bound, u_right);
    Eigen::Matrix2Xd uleft = evaluate_on_boundary(mv, left_bound, u_left);

    return std::pair(uright, uleft);
}

namespace
{

Eigen::Matrix2Xd solve(const MeshVariant &mv)
{
    size_t right_bound = find_boundary_with_tag(mv, "RIGHT").value();
    size_t left_bound = find_boundary_with_tag(mv, "LEFT").value();
    size_t upper_bound = find_boundary_with_tag(mv, "UPPER").value();
    size_t lower_bound = find_boundary_with_tag(mv, "LOWER").value();

    auto tp_start = steady_clock::now();
    Eigen::Matrix2Xd force_coefficients = evaluate_on_mesh(mv, forcing);
    Eigen::Matrix2Xd nodal_forcing = Elasticity::integrate_volume_force(mv, force_coefficients);

    Eigen::Matrix2Xd traction_upper_coefficients =
        evaluate_on_boundary(mv, upper_bound, traction_upper);
    Eigen::Matrix2Xd traction_lower_coefficients =
        evaluate_on_boundary(mv, lower_bound, traction_lower);

    Eigen::Matrix2Xd traction_upper =
        integrate_traction_force(mv, upper_bound, traction_upper_coefficients);
    Eigen::Matrix2Xd traction_lower =
        integrate_traction_force(mv, lower_bound, traction_lower_coefficients);

    add_traction_force(mv, nodal_forcing, upper_bound, traction_upper);
    add_traction_force(mv, nodal_forcing, lower_bound, traction_lower);

    auto tp_end = steady_clock::now();

    size_t us = duration_cast<microseconds>(tp_end - tp_start).count();
    printf("Assembling forcing took %ld us\n", us);

    Eigen::VectorXd lambda, mu;
    std::tie(lambda, mu) = compute_parameter_functions(mv);

    Eigen::Matrix2Xd uright, uleft;
    std::tie(uright, uleft) = get_boundary_conditions(mv, right_bound, left_bound);

    tp_start = steady_clock::now();
    auto K_unassembled = Elasticity::assemble_stiffness(mv, lambda, mu);

    auto flat_forcing = nodal_forcing.reshaped();
    Elasticity::impose_dirichlet_condition(mv, K_unassembled, flat_forcing, right_bound, uright);
    Elasticity::impose_dirichlet_condition(mv, K_unassembled, flat_forcing, left_bound, uleft);

    auto K = convert_to_eigen(K_unassembled);
    tp_end = steady_clock::now();
    us = duration_cast<microseconds>(tp_end - tp_start).count();
    printf("Assembling stiffness took %ld us\n", us);

    tp_start = steady_clock::now();
    Eigen::Matrix2Xd u(2, nodal_forcing.cols());
    u.reshaped() = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper>(K).solve(
        nodal_forcing.reshaped());
    tp_end = steady_clock::now();
    us = duration_cast<microseconds>(tp_end - tp_start).count();
    printf("Linear system solve took %ld us\n", us);

    return u;
}

} // namespace

int main(int argc, char *argv[])
{
    if (!(argc > 1))
    {
        fprintf(stderr, "Usage: ./manufactured [mesh order] [mesh files...]\n");
        return 1;
    }

    int order = atoi(argv[1]);
    if (!(order >= 1 && order <= 4))
    {
        fprintf(stderr, "Got mesh order %d; order should be 1-4\n", order);
        return 2;
    }

    for (int fi = 2; fi < argc; ++fi)
    {
        FILE *infile = fopen(argv[fi], "r");
        if (!infile)
        {
            fprintf(stderr, "Skipping mesh file %s (couldn't be opened)\n", argv[fi]);
            continue;
        }
        fclose(infile);
        auto mesh = read_mesh(argv[fi], order);
        FILE *outfile = fopen("mesh_elements.dat", "wb");
        if (!outfile)
        {
            fprintf(stderr, "Skipping output, couldn't open file\n");
        }
        else
        {
            get_triangle_info(mesh).serialize(outfile);
            fclose(outfile);
        }
        Eigen::Matrix2Xd u = solve(mesh);
        outfile = fopen("u.dat", "wb");
        int64_t dims[] = {u.rows(), u.cols()};
        fwrite(dims, sizeof(int64_t), 2, outfile);
        fwrite(u.data(), sizeof(double), u.size(), outfile);
        fclose(outfile);

        auto [l2_error, u_norm] = Elasticity::integrate_error(mesh, u, u_true);

        printf(
            "Error for input file %s:  %E (L^2 error)  %E (L^2 u)\n", argv[fi], l2_error, u_norm);
    }

    return 0;
}


#pragma once
#include <Eigen/Core>
#include <Eigen/StdVector>

using Vector2d = Eigen::Vector2d;
using Vector4d = Eigen::Vector4d;
using Matrix4d = Eigen::Matrix4d;
using Vectors2d = std::vector<Vector2d, Eigen::aligned_allocator<Vector2d>>;
using Vectors4d = std::vector<Vector4d, Eigen::aligned_allocator<Vector4d>>;


// 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 "math_unit_test.hpp"
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/mpfr.hpp>

using std::log;
using std::sin;
using std::abs;
using boost::math::quadrature::tanh_sinh;
using boost::multiprecision::mpfr_float;
using boost::math::constants::pi;
using boost::math::constants::zeta_three;

int main() {
    using Real = mpfr_float;
    int p = 100;
    mpfr_float::default_precision(p);
    auto f = [](Real x)->Real { return x*log(sin(x)); };
    auto integrator = tanh_sinh<mpfr_float>();
    Real Q = integrator.integrate(f, Real(0), pi<Real>()/2);
    // Sanity check: NIntegrate[x*Log[Sin[x]],{x,0,Pi/2}] = -0.329236
    Real expected = (7*zeta_three<Real>() - pi<Real>()*pi<Real>()*log(static_cast<Real>(4)))/16;
    CHECK_ULP_CLOSE(expected, Q, 3);
    return boost::math::test::report_errors();
}


#include "clustering/spectral_cluster.h"
#include "clustering/kmeans.h"

#include <algorithm>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <glog/logging.h>

#include "util/timer.h"

namespace GraphSfM {

std::unordered_map<int, int> SpectralCluster::ComputeCluster(
        const std::vector<std::pair<int, int>>& edges,
        const std::vector<int>& weights,
        const int num_partitions)
{
    if (num_partitions == 1) {
        for (auto node : nodes_) {
            labels_[node] = 0;
        }
        return labels_;
    }

    colmap::Timer timer;
    std::vector<Eigen::Triplet<double>> s_triplets;
    std::unordered_map<int, int> degrees;
    const int k = num_partitions;
    cluster_num_ = num_partitions;

    for (uint i = 0; i < edges.size(); i++) {
        auto edge = edges[i];
        nodes_.push_back(edge.first);
        nodes_.push_back(edge.second);
    }

    std::sort(nodes_.begin(), nodes_.end());
    nodes_.erase(std::unique(nodes_.begin(), nodes_.end()), nodes_.end());

    // 1. Compute similarity graph.
    for (uint i = 0; i < nodes_.size(); i++) {
        node_mapper_[nodes_[i]] = i;
    }

    timer.Start();
    const int N = nodes_.size();
    Eigen::SparseMatrix<double> S(N, N);
    for (uint i = 0; i < edges.size(); i++) {
        int src = edges[i].first, dst = edges[i].second;
        s_triplets.push_back(Eigen::Triplet<double>(src, dst, weights[i]));
        s_triplets.push_back(Eigen::Triplet<double>(dst, src, weights[i]));
        degrees[src] += 2;
    }

    S.setFromTriplets(s_triplets.begin(), s_triplets.end());
    S.makeCompressed();
    timer.Pause();
    LOG(INFO) << "1. Similarity Graph Computation Time: " << timer.ElapsedSeconds();

    // 2. Compute Laplacian matrix.
    timer.Start();
    Eigen::SparseMatrix<double> L = ComputeLaplacian(S, degrees);
    L.makeCompressed();
    timer.Pause();
    LOG(INFO) << "2. Laplacian Matrix Computation Time: " << timer.ElapsedSeconds();

    // 3. Compute the top-k smallest eigen values and corresponding eigen vectors.
    timer.Start();
    Spectra::SparseSymMatProd<double> op(L);
    Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE,
                          Spectra::SparseSymMatProd<double>> eigs(&op,
                                                                  k,
                                                                  std::min(2 * k, N));
    eigs.init();
    int nconv = eigs.compute();

    Eigen::VectorXd eigen_values;
    Eigen::MatrixXd eigen_vectors;
    if (eigs.info() == Spectra::SUCCESSFUL) {
        eigen_values = eigs.eigenvalues();
        eigen_vectors = eigs.eigenvectors();
    }

    timer.Pause();
    LOG(INFO) << "3. EigenValue Computation Time: " << timer.ElapsedSeconds();

    // 4. Reverse original eigen vectors(as it stored in descending order)
    timer.Start();
    uint i = 0, j = k - 1;
    while (i < j) {
        const Eigen::VectorXd tmp = eigen_vectors.col(i);
        eigen_vectors.col(i) = eigen_vectors.col(j);
        eigen_vectors.col(j) = tmp;
        i++; j--;
    }

    std::vector<Eigen::VectorXd> source_data;
    source_data.reserve(eigen_vectors.rows());
    for (uint i = 0; i < eigen_vectors.rows(); i++) {
        source_data.push_back(eigen_vectors.row(i));
    }
    timer.Pause();
    LOG(INFO) << "4. Eigen Vectors Reverse Time: " << timer.ElapsedSeconds();

    // 5. Invert K-Means for clustering.
    timer.Start();
    std::vector<uint32_t> cluster_assignment;
    std::vector<Eigen::VectorXd> centers;
    KMeans(source_data, cluster_assignment, centers, k);
    timer.Pause();
    LOG(INFO) << "5. KMeans Time: " << timer.ElapsedSeconds();

    for (uint i = 0; i < cluster_assignment.size(); i++) {
        labels_[nodes_[i]] = cluster_assignment[i];
    }
    return labels_;
}

Eigen::SparseMatrix<double> SpectralCluster::ComputeLaplacian(
                const Eigen::SparseMatrix<double>& S,
                const std::unordered_map<int, int>& degrees) const
{
    // Compute degree matrix.
    const int N = degrees.size();
    Eigen::SparseMatrix<double> D(N, N);
    // Eigen::SparseMatrix<double> D_inv(N, N);
    // Eigen::SparseMatrix<double> D_sqrt(N, N);
    for (auto it = degrees.begin(); it != degrees.end(); ++it) {
        int id = it->first;
        D.insert(node_mapper_.at(id), node_mapper_.at(id)) = it->second;
        // D_inv.insert(node_mapper_.at(id), node_mapper_.at(id)) = 1.0 / it->second;
        // D_sqrt.insert(node_mapper_.at(id), node_mapper_.at(id)) = -1.0 / sqrt(it->second);
    }
    D.makeCompressed();
    // D_inv.makeCompressed();
    // D_sqrt.makeCompressed();

    // Compute Laplacian matrix.
    Eigen::SparseMatrix<double> L = D - S;
    // Eigen::MatrixXd L_random = D_inv * L;
    // Eigen::MatrixXd L_sym = D_sqrt * L * D_sqrt;

    return L;
}

} // namespace GraphSfM


#include "stats/rmsd.h"
#include <xmd/utils/convert.h>
#include <Eigen/Core>
#include <Eigen/SVD>

namespace xmd {
    void compute_rmsd::operator()() const {
        using matrix_t = Eigen::Matrix<real, Eigen::Dynamic, 3>;
        matrix_t P = matrix_t::Zero(num_particles, 3);
        matrix_t Q = matrix_t::Zero(num_particles, 3);

        for (int idx = 0; idx < num_particles; ++idx) {
            P.row(idx) = convert<real>(r[idx]);
            Q.row(idx) = convert<real>(ref_r[idx]);
        }

        Eigen::Matrix<real, 3, 3> H = P.transpose() * Q;
        auto svd = Eigen::JacobiSVD<decltype(H)>(H,
            Eigen::ComputeFullU & Eigen::ComputeFullV);

        auto U = svd.matrixU();
        auto V = svd.matrixV();
        auto d = (V * U.transpose()).determinant() > 0 ? 1 : -1;
        auto R = V * Eigen::DiagonalMatrix<real, 3>(1, 1, d) * U.transpose();

        auto D = (P - Q) * R;
        real rmsd_ = 0.0;
        for (int idx = 0; idx < num_particles; ++idx) {
            rmsd_ += D.row(idx).squaredNorm();
        }
        *rmsd = sqrt(rmsd_ / num_particles);
    }
}


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

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

using namespace std;

template <typename MatrixA>
void test(MatrixA& A, unsigned dim1, unsigned dim2, const char* name)
{
    const unsigned max_print_size= 25;
    cout << "\n" << name << "\n";
    laplacian_setup(A, dim1, dim2);

    unsigned size= dim1 * dim2;
    mtl::dense_vector<double> v(size), b(size), r(size);

    for (unsigned i= 0; i < num_cols(A); i++)
	v[i]= A[12][i];
    b= 3.0;

    r= b + A * v;

    if (size <= max_print_size)
	cout << "A= \n" << A << "\n\nb + A * v = " << r << '\n';

    if (dim1 == 5 && dim2 == 5) {
	MTL_THROW_IF(abs(r[12] - 23.0) > 0.0001, mtl::runtime_error("r[12] should be 23.\n"));
    }

    r= b - A * v;

    if (size <= max_print_size)
	cout << "\n\nb - A * v = " << r << '\n';

    if (dim1 == 5 && dim2 == 5) {
	MTL_THROW_IF(abs(r[12] + 17.0) > 0.0001, mtl::runtime_error("r[12] should be -17.\n"));
    }

    // typedef mtl::dense_vector<double, mtl::parameters<mtl::row_major> >  vrt;
    // vrt rt, vt(trans(v));

    // rt= v * A;
}

int main(int argc, char* argv[])
{
    using namespace mtl;

    unsigned dim1= 5, dim2= 5;

    if (argc > 2) {dim1= atoi(argv[1]);dim2= atoi(argv[2]);}
    unsigned size= dim1 * dim2;

    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);

    test(cr, dim1, dim2, "Row-major sparse");
    test(cc, dim1, dim2, "Column-major sparse");

    test(dr, dim1, dim2, "Row-major dense");
    test(dc, dim1, dim2, "Column-major dense");

    return 0;
}


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include <fstream>
#include <iostream>
#include <iomanip>
#include <random>
#include <sstream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/SVD>

// -- Some typedefs
using TScalar = double;
using TMatrix = Eigen::Matrix< TScalar, Eigen::Dynamic, Eigen::Dynamic >;

// -- Helper functions
void read_file( TMatrix& X, const std::string& filename );

// -- Main function
int main( int argc, char** argv )
{
  // Check inputs and get them
  if( argc < 2 )
  {
    std::cerr
      << "Usage: " << argv[ 0 ] << " input.bin"
      << std::endl;
    return( 1 );
  } // end if
  std::string input = argv[ 1 ];

  // Read data
  TMatrix X;
  std::cout << "Read... " << std::flush;
  read_file( X, input );
  std::cout << "done! " << X.rows( ) << " " << X.cols( ) << std::endl;

  // Compute PCA
  std::cout << "Mean... " << std::flush;
  auto m = X.colwise( ).mean( );
  std::cout << "done! " << m.rows( ) << " " << m.cols( ) << std::endl;
  std::cout << "Centering... " << std::flush;
  auto c = X.rowwise( ) - m;
  std::cout << "done! " << c.rows( ) << " " << c.cols( ) << std::endl;
  std::cout << "Covariance... " << std::flush;
  auto S = ( c.transpose( ) * c ) / TScalar( X.cols( ) );
  std::cout << "done! " << S.rows( ) << " " << S.cols( ) << std::endl;
  std::cout << "Eigen analysis... " << std::flush;
  auto svd = S.bdcSvd( Eigen::ComputeFullU | Eigen::ComputeFullV );
  std::cout << "done!" << std::endl;
  TMatrix eR = svd.matrixU( );
  TMatrix ev = svd.singularValues( );
  TScalar sv = ev.sum( );

  unsigned long i = 0;
  double s = ev( i, 0 ) / sv;
  while( i < ev.rows( ) && s <= 0.95 )
  {
    i++;
    s += ev( i, 0 ) / sv;
  } // end while
  TMatrix Xp = ( X * eR ).block( 0, 0, X.rows( ), i );
  std::cout << i << " " << Xp.rows( ) << " " << Xp.cols( ) << std::endl;

  unsigned long xrows, xcols;
  xrows = Xp.rows( );
  xcols = Xp.cols( );

  std::ofstream out = std::ofstream( "out.bin", std::ios::binary );
  out.write( ( char* )( &xrows ), sizeof( unsigned long ) );
  out.write( ( char* )( &xcols ), sizeof( unsigned long ) );
  out.write( ( char* )( Xp.data( ) ), sizeof( TScalar ) * xrows * xcols );
  out.close( );

  /* TODO
     this->m_EigenValues.SetSize( ps );
     for( unsigned int d = 0; d < ps; ++d )
     this->m_EigenValues[ d ] = ev( d, 0 );
  */

  return( 0 );
}

// -------------------------------------------------------------------------
void read_file( TMatrix& X, const std::string& filename )
{
  std::ifstream Xreader = std::ifstream( filename, std::ios::binary );
  unsigned long xrows, xcols;
  Xreader.read( ( char* )( &xrows ), sizeof( unsigned long ) );
  Xreader.read( ( char* )( &xcols ), sizeof( unsigned long ) );
  X = TMatrix::Zero( xrows, xcols );
  Xreader.read( ( char* )( X.data( ) ), sizeof( TScalar ) * xrows * xcols );
  Xreader.close( );
}

// eof - pca.cxx


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

#include <app/benchmark/app_benchmark.h>
#include <app/benchmark/app_benchmark_detail.h>

#if(APP_BENCHMARK_TYPE == APP_BENCHMARK_TYPE_BOOST_MULTIPRECISION_CBRT)

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
#endif

//#define APP_BENCHMARK_TYPE_BOOST_BOOST_MULTIPRECISION_CBRT_USE_BIN_FLOAT

//TGT_INCLUDES  = -IC:/boost/modular_boost/boost/libs/multiprecision/include        \
//                -IC:/boost/modular_boost/boost/libs/math/include                  \
//                -IC:/boost/modular_boost/boost/libs/config/include

//LINKER_DEFINITION_FILE := $(PATH_TGT_MAKE)/stm32f446_with_stdlib.ld

//C:/boost/modular_boost/boost/libs/multiprecision/include;C:/boost/modular_boost/boost/libs/math/include;C:/boost/modular_boost/boost/libs/config/include;

#if !defined(BOOST_MP_STANDALONE)
#define BOOST_MP_STANDALONE
#endif

#if !defined(BOOST_MATH_STANDALONE)
#define BOOST_MATH_STANDALONE
#endif

#if !defined(BOOST_NO_RTTI)
#define BOOST_NO_RTTI
#endif

#if !defined(BOOST_DISABLE_THREADS)
#define BOOST_DISABLE_THREADS
#endif

#if !defined(BOOST_NO_EXCEPTIONS)
#define BOOST_NO_EXCEPTIONS
#endif

#if !defined(BOOST_MATH_DISABLE_ERROR_HANDLING)
#define BOOST_MATH_DISABLE_ERROR_HANDLING
#endif

#if !defined(BOOST_NO_CXX11_THREAD_LOCAL)
#define BOOST_NO_CXX11_THREAD_LOCAL
#endif

#include <boost/config.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#if defined(APP_BENCHMARK_TYPE_BOOST_BOOST_MULTIPRECISION_CBRT_USE_BIN_FLOAT)
#include <boost/multiprecision/cpp_bin_float.hpp>
#else
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif

bool app::benchmark::run_boost_multiprecision_cbrt()
{
  using big_float_backend_type =
  #if defined(APP_BENCHMARK_TYPE_BOOST_BOOST_MULTIPRECISION_CBRT_USE_BIN_FLOAT)
    boost::multiprecision::cpp_bin_float<101, boost::multiprecision::backends::digit_base_10, void, std::int32_t>;
  #else
    boost::multiprecision::cpp_dec_float<101, std::int32_t, void>;
  #endif

  using big_float_type =
    boost::multiprecision::number<big_float_backend_type, boost::multiprecision::et_off>;

  // Compute a square root.
  static const big_float_type
    big_float_arg
    {
      big_float_type(UINT32_C(123456)) / 100U
    };

  using std::cbrt;
  using boost::math::cbrt;

  const big_float_type big_float_result = cbrt(big_float_arg);

  // N[(123456/100)^(1/3), 111]
  // 10.7276369432283170454869317373527647801772956394047834686224956433128028534945259441672192774907629718402457465
  static const big_float_type
    control
    {
      "10.7276369432283170454869317373527647801772956394047834686224956433128028534945259441672192774907629718402457465"
    };

  // Compare the calculated result with the known control value.
  const bool app_benchmark_result_is_ok = detail::is_close_fraction(big_float_result, control);

  return app_benchmark_result_is_ok;
}

#if defined(APP_BENCHMARK_STANDALONE_MAIN)
int main()
{
  // g++ -Wall -O3 -march=native -I./ref_app/src/mcal/host -I./ref_app/src -DAPP_BENCHMARK_TYPE=APP_BENCHMARK_TYPE_BOOST_MULTIPRECISION_CBRT -DAPP_BENCHMARK_STANDALONE_MAIN ./ref_app/src/app/benchmark/app_benchmark_boost_math_cbrt_tgamma.cpp -o ./ref_app/bin/app_benchmark_boost_multiprecision_cbrt.exe

  bool result_is_ok = true;

  for(unsigned i = 0U; i < 64U; ++i)
  {
    result_is_ok &= app::benchmark::run_boost_multiprecision_cbrt();
  }

  return (result_is_ok ? 0 : -1);
}

#endif

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#endif

#endif // APP_BENCHMARK_TYPE_BOOST_MULTIPRECISION_CBRT


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
toeplitz(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& vec)
{
  index size = vec.size();

  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> mat(size, size);

  for (auto i = 0; i < size; i++)
  {
    for (auto j = 0; j < i; j++) mat(j, i) = vec(i - j);
    for (auto j = i; j < size; j++) mat(j, i) = vec(j - i);
  }

  return mat;
}

} // namespace algorithm
} // namespace fluid


/*
 * recon.cpp
 *
 *  Created on: Jul 25, 2019
 *      Author: dmarce1
 */
#include "../../octotiger/print.hpp"
#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <map>
#include <vector>
#include <math.h>
#include <iostream>
#include <complex>
#include <boost/program_options.hpp>

auto band_filter(double t, double Ps, double Pc) {
	const auto x = 2.0 * M_PI * t / Pc;
	const auto y = 2.0 * M_PI * (t / Ps + 0.5);
	const auto sinc = x == 0 ? 1.0 : sin(x) / x;
	constexpr auto a0 = 7938.0 / 18608.0;
	constexpr auto a1 = 9240.0 / 18608.0;
	constexpr auto a2 = 1430.0 / 18608.0;
	const auto blackman = (a0 - a1 * cos(y) + a2 * cos(2 * y));
	if (blackman > 0.0) {
		return sinc * blackman;
	} else {
		return 0.0;
	}
}

void output_spectrogram(double Ps, double Pc) {
	double pmin = Pc / 100.0;
	double pmax = 2.0 * Ps;
	double dp = (pmax - pmin) / 1000.0;
	const auto dt = Ps / 100000.0;
	FILE *fp = fopen("filter.dat", "wt");
	for (auto p = pmin; p <= pmax; p += dp) {
		double w = 0.0;
		const auto omega = 2.0 * M_PI / p;
		double sum = 0.0;
		constexpr std::complex<double> I(0, 1);
		for (double t = dt / 2.0; t < Ps / 2.0; t += dt) {
			const auto bf = band_filter(t, Ps, Pc);
			w += 2.0 * bf * dt;
			sum += 2.0 * bf * cos(omega * t) * dt;
		}
		sum /= w;
		fprintf(fp, "%.12e %.12e\n", p, sum);
	}
	fclose(fp);

}

auto derivative(const std::vector<std::vector<double>> &f, double omega) {
	std::vector<std::vector<double>> g;
	std::vector<double> u(f[0].size());
	const auto P = 2.0 * M_PI / omega;
	for (int n = 1; n < f.size() - 1; n++) {
		const auto nm = n - 1;
		const auto np = n + 1;
		const auto h1 = (f[n][0] - f[nm][0]);
		const auto h2 = (f[np][0] - f[n][0]);
		for (int i = 0; i < u.size(); i++) {
			u[i] = P * (f[np][i] * h1 * h1 + f[n][i] * (h2 * h2 - h1 * h1) - f[nm][i] * h2 * h2) / (f[0][i] * h1 * h2 * (h1 + h2));
		}
		u[0] = f[n][0];
		g.push_back(u);
	}

	return g;

}

auto filter(const std::vector<std::vector<double>> &f, double omega, double Pc, double Ps) {
	std::vector<std::vector<double>> g;
	std::vector<double> u(f[0].size());

	const auto tmin = f[0][0];
	const auto tmax = f[f.size() - 1][0];
	const auto P = 2.0 * M_PI / omega;
	double rt = tmin / P;
	Pc *= P;
	Ps *= P;
	for (int n = 1; n < f.size() - 1; n++) {
		double t0 = f[n][0];
		double dt;
		double weight = 0.0;

		if (Pc / 2.0 + tmin < t0 && t0 < tmax - Pc / 2.0) {
			std::fill(u.begin(), u.end(), 0.0);
			for (int m = 1; m < f.size() - 1; m++) {
				double t = f[m][0];
				if (t0 - Pc / 2.0 < t && t < t0 + Pc / 2.0) {
					dt = (f[m + 1][0] - f[m - 1][0]) / 2.0;
					const auto h1 = f[m][0] - f[m - 1][0];
					const auto h2 = f[m + 1][0] - f[m][0];
					const auto w1 = (h1 + h2) * (2 * h1 - h2) / h1 / 6.0;
					const auto w2 = std::pow(h1 + h2, 3) / h2 / h1 / 6.0;
					const auto w3 = (h1 + h2) * (2 * h2 - h1) / h2 / 6.0;
					double y1 = band_filter(f[m - 1][0] - t0, Ps, Pc);
					double y2 = band_filter(f[m][0] - t0, Ps, Pc);
					double y3 = band_filter(f[m + 1][0] - t0, Ps, Pc);
					for (int i = 0; i < u.size(); i++) {
						u[i] += w1 * f[m - 1][i] * y1 * dt;
						u[i] += w2 * f[m][i] * y2 * dt;
						u[i] += w3 * f[m + 1][i] * y3 * dt;
					}
					weight += w1 * y1 * dt;
					weight += w2 * y2 * dt;
					weight += w3 * y3 * dt;
				}
			}
			for (int i = 0; i < u.size(); i++) {
				u[i] /= weight;
			}
			u[0] = rt;
			g.push_back(u);
		}
		dt = (f[n + 1][0] - f[n - 1][0]) / 2.0;
		rt += dt / P;
	}
	return g;
}

struct options {
	double pmin;
	double pmax;
	bool help;
	bool normalize;
	std::string input;

	int read_options(int argc, char *argv[]) {
		namespace po = boost::program_options;

		po::options_description command_opts("options");

		command_opts.add_options() //
		("input", po::value<std::string>(&input)->default_value("binary.dat"), "input filename")           //
		("pmin", po::value<double>(&pmin)->default_value(1.25), "minimum period to allow through filter")           //
		("pmax", po::value<double>(&pmax)->default_value(5), "period where filter allows 100%")           //
		("normalize", po::value<bool>(&normalize)->default_value(true), "normalize averages to t=0 value")           //
		("help", po::value<bool>(&help)->default_value(false), "show the help page")           //
				;
		boost::program_options::variables_map vm;
		po::store(po::parse_command_line(argc, argv, command_opts), vm);
		po::notify(vm);

		if (help) {
			std::cout << command_opts << "\n";
			return -1;
		}
		FILE *fp = fopen(input.c_str(), "rb");
		if (fp == NULL) {
			print("Unable to open %s\n", input.c_str());
			return -1;
		} else {
			fclose(fp);
		}

		return 0;
	}
};
int main(int argc, char *argv[]) {
	options opts;
	if (opts.read_options(argc, argv) == 0) {

		FILE *fp = fopen(opts.input.c_str(), "rt");
		static char buffer[100000];
		std::map<double, std::vector<double>> values;

		while (!feof(fp)) {
			const char* b = fgets(buffer, 100000, fp);
			bool done = false;
			char *ptr = buffer;
			double t;
			int col = 0;
			std::vector<double> these_values;
			do {
				while (isspace(*ptr)) {
					if (*ptr == '\n') {
						done = true;
						break;
					}
					ptr++;
				}
				if (!done) {
					double number = atof(ptr);
					if (col == 0) {
						t = number;
					}
					these_values.push_back(number);
					col++;
				}
				while (!isspace(*ptr)) {
					ptr++;
				}
			} while (!done);
			if (values.find(t) == values.end()) {
				values.insert(std::make_pair(t, std::move(these_values)));
			}
		}

		std::vector<std::vector<double>> v1;
		for (auto &v : values) {
			v1.push_back(std::move(v.second));
		}

		auto Pc = 2.0 * opts.pmax * opts.pmin / (opts.pmax + opts.pmin);
		auto Ps = 4.0 * opts.pmax * opts.pmin / (opts.pmax - opts.pmin);
		print("sinc period      = %e\n", Pc);
		print("Blackmann period = %e\n", Ps);
		const auto P = 2.0 * M_PI / v1[0][2];

		print("Window is +/- %.12e orbits\n", Pc / 2.0);

		auto v2 = filter(v1, v1[0][2], Pc, Ps);
		if (v2.size() == 0) {
			print("Not enough data to produce output\n");
		} else {
			if (opts.normalize) {
				for (auto &line : v2) {
					for (int i = 1; i < line.size(); i++) {
						line[i] /= v1[0][i];
					}
				}
			}
			const auto v4 = derivative(v1, v1[0][2]);
			const auto v3 = filter(v4, v1[0][2], Pc, Ps);
			FILE *fp1 = fopen("avg.dat", "wt");
			FILE *fp2 = fopen("drv.dat", "wt");
			for (const auto &i : v2) {
				for (const auto &j : i) {
					fprintf(fp1, " %.12e ", j);
				}
				fprintf(fp1, "\n");
			}
			for (const auto &i : v3) {
				for (const auto &j : i) {
					fprintf(fp2, " %.12e ", j);
				}
				fprintf(fp2, "\n");
			}
			fclose(fp1);
			fclose(fp2);
			fclose(fp);
		}

		output_spectrogram(Ps, Pc);
	}
	return 0;
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MIQP_COMMON_MATH_HPP_
#define MIQP_COMMON_MATH_HPP_

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

namespace miqp {
namespace common {
namespace math {

inline void WrapRadiantTo2Pi(double& x) {
  x = fmod(x, 2 * boost::math::constants::pi<double>());
  if (x < 0) {
    x += 2 * boost::math::constants::pi<double>();
  }
  assert(x >= 0);
  assert(x <= 2 * boost::math::constants::pi<double>());
}

inline void SwapIfNeeded(float& maxval, float& minval) {
  if (maxval < minval) {
    float tmp = maxval;
    maxval = minval;
    minval = tmp;
  }
}

inline double InterpolateWithBounds(const double x0, const double y0,
                                    const double x1, const double y1,
                                    const double xInterp) {
  double yInterp;
  if (xInterp > x1) {
    yInterp = y1;
  } else if (xInterp < x0) {
    yInterp = y0;
  } else {
    yInterp = (y1 - y0) / (x1 - x0) * (xInterp - x0) + y0;
  }
  return yInterp;
};

}  // namespace math
}  // namespace common
}  // namespace miqp

#endif  // MIQP_COMMON_MATH_HPP_


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

#include "mylinearfeelementmatrix.h"

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

#include <Eigen/Core>

namespace ElementMatrixComputation {

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

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix

  auto lapl_mat = lf::uscalfe::LinearFELaplaceElementMatrix().Eval(cell);

  if (ref_el == lf::base::RefElType::kQuad) {
    Eigen::Matrix<double, 4, 4> elem_mat;
    elem_mat << 4,2,1,2,
                2,4,2,1,
                1,2,4,2,
                2,1,2,4;

    double x0 = vertices.row(0).minCoeff();
    double x1 = vertices.row(0).maxCoeff();
    double y0 = vertices.row(1).minCoeff();
    double y1 = vertices.row(1).maxCoeff();

    double area = (x1 - x0) * (y1 - y0);

    return area * elem_mat / 36. + lapl_mat;
  }

  else if(ref_el == lf::base::RefElType::kTria) {
      Eigen::Matrix<double, 4, 4> elem_mat;
      elem_mat << 2,1,1,0,
                  1,2,1,0,
                  1,1,2,0,
                  0,0,0,0;

      Eigen::Matrix2d side_lengths;

      side_lengths.col(0) = vertices.col(1) - vertices.col(0);
      side_lengths.col(1) = vertices.col(2) - vertices.col(0);

      double area = 0.5 * side_lengths.determinant();

      return area * elem_mat / 12. + lapl_mat;

  }
  else {
    assert(0 && "unsupported cell type");
  }

}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


//
// Created by TAADEJOM on 15.05.2021.
//

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

using namespace Eigen;

/**
 * Concatenates two matrices vertically. A at the top and B at the bottom
 */
MatrixXd vConcat(const MatrixXd& A, const MatrixXd& B) {
    MatrixXd D(A.rows()+B.rows(), A.cols()); // <-- D(A.rows() + B.rows(), ...)
    D << A, B;
    return D;
}

/**
 * See: https://gist.github.com/gocarlos/c91237b02c120c6319612e42fa196d77
 * Note: matrix index starts at 1 in matlab and at 0 in eigen -> never forget -1!
 */
QVector<double> DataSet::regression(int degree) const {
    QSharedPointer<QCPGraphDataContainer> data = graph->data();
    int m = data->size(); // rows
    int n = degree; // columns
    // xPoints = data(:,1);  %x -> A
    // yPoints = data(:,2); %y -> b
    MatrixXd xPoints(m, 1); // n rows, 1 column
    MatrixXd yPoints(m, 1); // n rows, 1 column
    for (int i = 0; i < m; ++i) {
        xPoints(i, 0) = data->at(i)->key;
        yPoints(i, 0) = data->at(i)->value;
    }

    // x0 = ones(m,1).*80.3e3;
    MatrixXd x1 = xPoints;
    MatrixXd x0 = Eigen::MatrixXd::Ones(m, 1) * 80300;

    MatrixXd z1 = (x1 - x0).array() / 50;

    // Set up vector A
    MatrixXd A(m, n);
    for (int i = 1; i <= n; ++i) {
        if (i == n) {
            // A(1:m, degree) = ones(m,1);
            A.col(i - 1) = Eigen::MatrixXd::Ones(m, 1);
        } else {
            // A(1:m, degree) = xPoints.^(n - degree);
            A.col(i - 1) = xPoints.array().pow(n - i);
        }
    }

    MatrixXd Im = Eigen::MatrixXd::Identity(m, m); // m*m identity matrix

    // QR-Zerlegung von A
    MatrixXd R = A;
    MatrixXd Q = Im;
    for (int k = 1; k <= n; ++k) {
        MatrixXd x = R.col(k - 1).bottomRows(R.rows() - k + 1); // we wanna show R.rows - k rows from the bottom // fixme: ErrorRowIndexOutOfBounds with 5 data points and degree 10

        // x is a matrix with 1 column and however many rows
        double lambda = x.coeff(k - 1, 0) < 0 ? x.norm() : -x.norm();

        MatrixXd e = Im.col(k - 1).bottomRows(R.rows() - k + 1);
        MatrixXd v = (x - lambda * e) / (x - lambda * e).norm(); // use normalize()
        MatrixXd z = Eigen::MatrixXd::Zero(k - 1, 1);

        // H = eye(m)-2.*[z;v]*[z;v]';
        MatrixXd H = Eigen::MatrixXd::Identity(m, m) - (2 * (vConcat(z, v) * vConcat(z, v).transpose()));

        Q = Q * H; // mXm * mXm matrix mul => VERY SLOW
        R = H * R;
    }
    // Rückwärtseinsetzen
    MatrixXd y = Q.transpose() * yPoints;

    MatrixXd x = Eigen::MatrixXd::Zero(1, n);
    // x(n) = y(n)/R(n,n);
    x.coeffRef(0, n - 1) = y.coeff(n  - 1, 0) / R.coeff(n - 1, n - 1);

    for (int i = (n - 1); i >= 1; --i) {
        double s = 0;
        for (int j = i + 1; j <= n; ++j) {
            // s = s + x(j)*R(i,j);
            s += x.coeff(0, j - 1) * R.coeff(i - 1, j - 1);
        }
        // x(i) = (y(i) - s)/R(i,i);
        x.coeffRef(0, i - 1) = (y.coeff(i - 1, 0) - s) / R.coeff(i - 1, i - 1);
    }

    return QVector<double>(x.data(), x.data() + x.rows() * x.cols());
}

/**
 * Makes a function string out of a list of coeffs
 */
QString DataSet::getFunctionString(const QVector<double>& coeffs) {
    QString regression = "";
    for (int i = 0; i < coeffs.size(); ++i) {
        regression += QString("%1*x^%2+").arg(QString::number(coeffs.at(i), 'E', 16)).arg(coeffs.length() - i - 1);
    }
    regression.truncate(regression.lastIndexOf('+'));
    return regression;
}

QDataStream &operator<<(QDataStream &out, const DataSet &dataSet) {
    out << dataSet.displayName << dataSet.functionString << dataSet.color << quint32(dataSet.graphWidth) << quint32(dataSet.pointDensity);
    return out;
}

QDataStream &operator>>(QDataStream &in, DataSet &dataSet) {
    QString displayName;
    QString functionString;
    QColor color;
    quint32 graphWidth;
    quint32 pointDensity;
    in >> displayName >> functionString >> color >> graphWidth >> pointDensity;
    dataSet.displayName = displayName;
    dataSet.functionString = functionString;
    dataSet.color = color;
    dataSet.graphWidth = graphWidth;
    dataSet.pointDensity = pointDensity;
    return in;
}


#pragma once

#include <Discregrid/common.hpp>
#include <Eigen/Dense>

class CubicKernel
{
public:
	 Discregrid::Real getRadius() { return m_radius; }
	 void setRadius(Discregrid::Real val)
	{
		m_radius = val;
		const Discregrid::Real pi = static_cast<Discregrid::Real>(M_PI);

		const Discregrid::Real h3 = m_radius*m_radius*m_radius;
		m_k = 8.0 / (pi*h3);
		m_l = 48.0 / (pi*h3);
		m_W_zero = W(Discregrid::Vector3r::Zero());
	}

public:
	Discregrid::Real W(Discregrid::Vector3r const& r)
	{
		Discregrid::Real res = 0.0;
		const Discregrid::Real rl = r.norm();
		const Discregrid::Real q = rl/m_radius;
		if (q <= 1.0)
		{
			if (q <= 0.5)
			{
				const Discregrid::Real q2 = q*q;
				const Discregrid::Real q3 = q2*q;
				res = m_k * (6.0*q3-6.0*q2+1.0);
			}
			else
			{
				auto _1mq = 1.0 - q;
				res = m_k * (2.0*_1mq*_1mq*_1mq);
			}
		}
		return res;
	}

	Discregrid::Vector3r gradW(const Discregrid::Vector3r &r)
	{
		using namespace Eigen;
		Discregrid::Vector3r res;
		const Discregrid::Real rl = r.norm();
		const Discregrid::Real q = rl / m_radius;
		if (q <= 1.0)
		{
			if (rl > 1.0e-6)
			{
				const Discregrid::Vector3r gradq = r * ((Discregrid::Real) 1.0 / (rl*m_radius));
				if (q <= 0.5)
				{
					res = m_l*q*((Discregrid::Real) 3.0*q - (Discregrid::Real) 2.0)*gradq;
				}
				else
				{
					const Discregrid::Real factor = 1.0 - q;
					res = m_l*(-factor*factor)*gradq;
				}
			}
		}
		else
			res.setZero();

		return res;
	}

	Discregrid::Real W_zero()
	{
		return m_W_zero;
	}

private:
	Discregrid::Real m_radius;
	Discregrid::Real m_k;
	Discregrid::Real m_l;
	Discregrid::Real m_W_zero;
};


#include "IntrinsicGeometry.h"
#include "MeshConnectivity.h"
#include <Eigen/Dense>

IntrinsicGeometry::IntrinsicGeometry(const MeshConnectivity &mesh, const std::vector<Eigen::Matrix2d> &abars) : abars(abars)
{
    int nedges = mesh.nEdges();
    int nfaces = mesh.nFaces();

    Js.resize(2 * nfaces, 2);
    for (int i = 0; i < nfaces; i++)
    {
        Eigen::Matrix2d Jeuclid;
        Jeuclid << 0, -1,
            1, 0;
        Js.block<2, 2>(2 * i, 0) = std::sqrt(abars[i].determinant()) * abars[i].inverse() * Jeuclid;
    }

    Ts.resize(2 * nedges, 4);
    Ts.setZero();
    for (int i = 0; i < nedges; i++)
    {
        int face1 = mesh.edgeFace(i, 0);
        int face2 = mesh.edgeFace(i, 1);
        if (face1 == -1 || face2 == -1)
            continue;

        int vert1 = mesh.edgeVertex(i, 0);
        int vert2 = mesh.edgeVertex(i, 1);

        // write edge vert1->vert2 in barycentric coordinates on each face
        Eigen::Vector2d barys[3] = { {0,0}, {1,0}, {0,1} };
        Eigen::Vector2d face1e(0, 0);
        Eigen::Vector2d face2e(0, 0);
        for (int j = 0; j < 3; j++)
        {
            if (vert1 == mesh.faceVertex(face1, j))
                face1e -= barys[j];
            else if (vert2 == mesh.faceVertex(face1, j))
                face1e += barys[j];
            if (vert1 == mesh.faceVertex(face2, j))
                face2e -= barys[j];
            else if (vert2 == mesh.faceVertex(face2, j))
                face2e += barys[j];
        }

        Eigen::Vector2d face1eperp = Js.block<2, 2>(2 * face1, 0) * face1e;
        Eigen::Vector2d face2eperp = Js.block<2, 2>(2 * face2, 0) * face2e;
        Eigen::Matrix2d face1basis;
        face1basis.col(0) = face1e;
        face1basis.col(1) = face1eperp;
        Eigen::Matrix2d face2basis;
        face2basis.col(0) = face2e;
        face2basis.col(1) = face2eperp;
        Ts.block<2, 2>(2 * i, 0) = face2basis * face1basis.inverse();
        Ts.block<2, 2>(2 * i, 2) = face1basis * face2basis.inverse();
    }
}


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

using namespace Eigen;
using namespace std;

#ifndef PLANE3D_HPP
#define PLANE3D_HPP

class Plane3d {
    int id;
    Vector3d axisX;
    Vector3d axisY;
    Vector3d normal;
    Vector3d target;
    Matrix3d inverse;
  public:
    Plane3d(int _id, double alpha, double theta, Vector3d _target);
    bool intersectsEdge(Vector3d v0, Vector3d v1);
    bool containsPoint(Vector3d v0);
    array<double, 3> findIntersection(Vector3d v0, Vector3d v1);
    int Id();
    Vector2d Rotate(array<double, 3> _vec);
    Vector3d Get3dPoint(Vector3d pt2d);
};

class Shape3d {
    unsigned long int tetId;
    vector<array<double, 3>> vertices;
    int label;
    //tissueId
    //these should probably all be doubles?
    double weight;
  public:
    Shape3d(unsigned long int _tetId, vector<array<double, 3>> _vertices, double _weight, int _label);
    unsigned long int TetId();
    vector<array<double, 3>> Vertices();
    int Label();
    double Weight();
};

#endif


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd A = MatrixXd::Random(6,6);
cout << "Here is a random 6x6 matrix, A:" << endl << A << endl << endl;

RealSchur<MatrixXd> schur(A);
cout << "The orthogonal matrix U is:" << endl << schur.matrixU() << endl;
cout << "The quasi-triangular matrix T is:" << endl << schur.matrixT() << endl << endl;

MatrixXd U = schur.matrixU();
MatrixXd T = schur.matrixT();
cout << "U * T * U^T = " << endl << U * T * U.transpose() << endl;

  return 0;
}


/*
A "C" interface to libigl's harmonic coordinates.
*/

#include <cassert>

#define IGL_HEADER_ONLY
#include <igl/boundary_conditions.h>
#include <igl/normalize_row_sums.h>
#include <igl/harmonic.h>

#include <Eigen/Dense>

#include <iostream>

#define kVertexDimension 3

extern "C"
{

typedef double real_t;
typedef int index_t;

// Returns 0 for success, anything else is an error.
int harmonic(
    /// Input Parameters
    // 'vertices' is a pointer to num_vertices*kVertexDimension floating point values,
    // packed: x0, y0, z0, x1, y1, z1, ...
    // In other words, a num_vertices-by-kVertexDimension matrix packed row-major.
    int num_vertices, real_t* vertices,
    // 'faces' is a pointer to num_faces*3 integers,
    // where each face is three vertex indices: f0.v0, f0.v1, f0.v2, f1.v0, f1.v1, f1.v2, ...
    // Face i's vertices are: vertices[ faces[3*i]*2 ], vertices[ faces[3*i+1]*2 ], vertices[ faces[3*i+2]*2 ]
    // In other words, a num_faces-by-3 matrix packed row-major.
    int num_faces, index_t* faces,
    // 'boundary_indices' is a pointer to num_boundary_vertices integers,
    // where each element "i" in boundary_indices references the vertex whose data
    // is located at vertices[ boundary_indices[i]*kVertexDimension ].
    int num_boundary_indices, index_t* boundary_indices,
    // Power of the harmonic operation (1 is harmonic, 2 is bi-harmonic, etc ),
    int power,

    /// Output Parameters
    // 'Wout' is a pointer to num_vertices*num_boundary_indices values.
    // Upon return, W will be filled with each vertex in 'num_vertices' weight for
    // each boundary vertex in 'boundary_indices'.
    // The data layout is that all 'num_boundary_indices' weights for vertex 0
    // appear before all 'num_boundary_indices' weights for vertex 1, and so on.
    // In other words, a num_vertices-by-num_boundary_indices matrix packed row-major.
    real_t* Wout
    )
{
    using namespace std;
    using namespace igl;
    using namespace Eigen;

    assert( num_vertices > 0 );
    assert( vertices );
    assert( num_faces > 0 );
    assert( faces );
    assert( num_boundary_indices > 0 );
    assert( boundary_indices );

    assert( Wout );

    // #V by 2 list of mesh vertex positions
    // Make this an Eigen::map from 'vertices'
    MatrixXd V = Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, kVertexDimension, Eigen::RowMajor > >( vertices, num_vertices, kVertexDimension );
    // #F by 3 list of triangle indices
    // Make this an Eigen::map from 'faces'
    MatrixXi F = Eigen::Map< Eigen::Matrix< index_t, Eigen::Dynamic, 3, Eigen::RowMajor > >( faces, num_faces, 3 );

#define CALL_BOUNDARY_CONDITIONS 0
#if !CALL_BOUNDARY_CONDITIONS
    // Make this an Eigen::map from boundary_indices.
    VectorXi b = Eigen::Map< Eigen::Matrix< index_t, Eigen::Dynamic, 1 > >( boundary_indices, num_boundary_indices, 1 );
    MatrixXd bc;
    bc.setIdentity( num_boundary_indices, num_boundary_indices );

#else

    // Alec: If your mesh is (V,F) and your control cage is C then first build a #C by 2 MatrixXi of “edge indices” for your control cage. Easy, just:
    MatrixXd C(num_boundary_indices,kVertexDimension);
    MatrixXi CE(num_boundary_indices,2);
    VectorXi P(num_boundary_indices);
    for( int c = 0; c < num_boundary_indices; ++c )
    {
      C.row(c) = V.row( boundary_indices[c] );
      P(c) = c;
      CE(c,0) = c;
      CE(c,1) = (c+1)%C.rows();
    }
    // We don't use this, but we need an empty one to pass to boundary_conditions().
    MatrixXi BE;

    // Compute boundary conditions (aka fixed value constraints)
    // List of boundary indices (aka fixed value indices into VV)
    VectorXi b;
    // List of boundary conditions of each weight function
    MatrixXd bc;
    if(!boundary_conditions(V,F,C,P,BE,CE,b,bc))
    {
        return 1;
    }
#endif

    // compute harmonic coordinates
    // Weights matrix
    MatrixXd W;
    if(!harmonic(V,F,b,bc,power,W))
    {
        return 2;
    }

    // Normalize weights.
    normalize_row_sums(W,W);

    // Save output
    // Copy W to Wout.
    Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > >( Wout, num_vertices, num_boundary_indices ) = W;

    return 0;
}

}


#include <blitz/array.h>
#include <blitz/array/stencil-et.h>

BZ_USING_NAMESPACE(blitz)

void setupInitialConditions(Array<float,3>& P1, Array<float,3>& P2,
    Array<float,3>& P3, Array<float,3>& c, int N);

BZ_DECLARE_STENCIL4(acoustic3D, P1, P2, P3, c)
    P3 = 2 * P2 + c * Laplacian3D_stencilop(P2) - P1;
BZ_END_STENCIL

float acoustic3D_BlitzStencil(int N, int niters)
{
    Array<float,3> P1, P2, P3, c;
    allocateArrays(shape(N,N,N), P1, P2, P3, c);

    setupInitialConditions(P1, P2, P3, c, N);

    for (int iter=0; iter < niters; ++iter)
    {
        applyStencil(acoustic3D(), P1, P2, P3, c);
        cycleArrays(P1, P2, P3);
    }

    return P1(N/2,N/2,N/2);
}


#include "space_vector_modulation.h"
#include "global_debug.h"
#include "util/clarke_transform.h"
#include "util/conversions.h"
#include "util/math_constants.h"
#include <Eigen/Dense>

const std::array<std::complex<Scalar>, 6> kSvmVectors = []() {
    std::array<std::complex<Scalar>, 6> result;
    result[0] = {1, 0};
    for (int i = 0; i < 6; ++i) {
        result[i] = {std::cos(i * 2 * kPI / 6), std::sin(i * 2 * kPI / 6)};
    }
    return result;
}();

int get_sector(const std::complex<Scalar>& voltage_ab) {
    const std::complex<Scalar> rot_60_deg = std::conj(kSvmVectors[1]);

    std::complex<Scalar> curr = voltage_ab;
    std::complex<Scalar> next = voltage_ab * rot_60_deg;
    int sector;
    for (sector = 0; sector < 5; ++sector) {
        if (curr.imag() >= 0 && next.imag() < 0) {
            // found it
            break;
        }
        // rotate
        curr = next;
        next *= rot_60_deg;
    }

    return sector;
}

std::array<Scalar, 3> get_pwm_duties(const Scalar bus_voltage,
                                     const std::complex<Scalar>& voltage_ab) {
    const int sector_x = get_sector(voltage_ab);
    const int sector_y = (sector_x + 1) % 6;

    const std::complex<Scalar> boundary_x =
        kSvmVectors[sector_x] * kClarkeScale * bus_voltage;
    const std::complex<Scalar> boundary_y =
        kSvmVectors[sector_y] * kClarkeScale * bus_voltage;

    Eigen::Matrix<Scalar, 2, 2> boundaries;
    boundaries <<
        // clang-format off
	boundary_x.real(), boundary_y.real(),
	boundary_x.imag(), boundary_y.imag()
        // clang-format on
        ;
    Eigen::Matrix<Scalar, 2, 1> v_ab;
    v_ab << voltage_ab.real(), voltage_ab.imag();

    Eigen::Matrix<Scalar, 2, 1> coeffs = boundaries.inverse() * v_ab;
    Scalar cx = coeffs(0);
    Scalar cy = coeffs(1);
    Scalar cx_p_cy = cx + cy;
    Scalar c0 = 1.0 - cx_p_cy;
    if (cx_p_cy > 1) {
        c0 = 0;
        cx /= cx_p_cy;
        cy /= cx_p_cy;
    }

    const auto& gx = kSvmStates[sector_x];
    const auto& gy = kSvmStates[sector_y];

    std::array<Scalar, 3> duties;
    for (int i = 0; i < 3; ++i) {
        duties[i] = c0 / 2 + gx[i] * cx + gy[i] * cy;
    }
    return duties;
}

std::complex<Scalar> get_avg_voltage_ab(const Scalar bus_voltage,
                                        const std::array<Scalar, 3>& duties) {
    // optimal sort 3 elements
    int first_gate_on = 0;
    int second_gate_on = 1;
    int third_gate_on = 2;
    if (duties[second_gate_on] > duties[first_gate_on]) {
        std::swap(first_gate_on, second_gate_on);
    }
    if (duties[third_gate_on] > duties[second_gate_on]) {
        std::swap(third_gate_on, second_gate_on);
    }
    // after these two swaps, duties[third_gate_on] is minimal
    if (duties[second_gate_on] > duties[first_gate_on]) {
        std::swap(first_gate_on, second_gate_on);
    }
    // duties[first_gate_on] is maximum

    Eigen::Matrix<Scalar, 3, 1> pole_voltages_x =
        Eigen::Matrix<Scalar, 3, 1>::Zero();
    pole_voltages_x(first_gate_on) = bus_voltage;

    Eigen::Matrix<Scalar, 3, 1> pole_voltages_y = pole_voltages_x;
    pole_voltages_y(second_gate_on) = bus_voltage;

    Eigen::Matrix<Scalar, 3, 1> pole_voltages_avg =
        pole_voltages_x * (duties[first_gate_on] - duties[second_gate_on]) +
        pole_voltages_y * (duties[second_gate_on] - duties[third_gate_on]);

    return to_complex<Scalar>(kClarkeTransform2x3 * pole_voltages_avg);
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_ROTATION2D_HPP
#define RW_MATH_ROTATION2D_HPP

/**
 * @file Rotation2D.hpp
 */

#if !defined(SWIG)
#include "Vector2D.hpp"

#include <rw/common/Serializable.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    template< class T > class Rotation2DVector;

    /** @addtogroup math */
    /* @{*/

#if !defined(SWIGJAVA)

    /**
     * @brief A 2x2 rotation matrix \f$ \mathbf{R}\in SO(2) \f$
     *
     * @f$
     *  \mathbf{R}=
     *  \left[
     *  \begin{array}{cc}
     *  {}^A\hat{X}_B & {}^A\hat{Y}_B
     *  \end{array}
     *  \right]
     *  =
     *  \left[
     *  \begin{array}{cc}
     *  r_{11} & r_{12} \\
     *  r_{21} & r_{22}
     *  \end{array}
     *  \right]
     * @f$
     */
#endif
    template< class T = double > class Rotation2D
    {
      public:
        //! Value type.
        typedef T value_type;

        //! The type of the internal Boost matrix implementation.
        typedef Eigen::Matrix< T, 2, 2 > EigenMatrix2x2;

        /**
           @brief A rotation matrix with uninitialized storage.
         */
        Rotation2D () {}

#if !defined(SWIGJAVA)

        /**
         * @brief Constructs an initialized 2x2 rotation matrix
         *
         * @param r11 \f$ r_{11} \f$
         * @param r12 \f$ r_{12} \f$
         * @param r21 \f$ r_{21} \f$
         * @param r22 \f$ r_{22} \f$
         *
         * @f$
         *  \mathbf{R} =
         *  \left[
         *  \begin{array}{cc}
         *  r_{11} & r_{12} \\
         *  r_{21} & r_{22}
         *  \end{array}
         *  \right]
         * @f$
         */

#endif
        Rotation2D (T r11, T r12, T r21, T r22)
        {
            _m[0][0] = r11;
            _m[0][1] = r12;
            _m[1][0] = r21;
            _m[1][1] = r22;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 2x2 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{cc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param i @f$ \robabx{a}{b}{\mathbf{i}} @f$
         * @param j @f$ \robabx{a}{b}{\mathbf{j}} @f$
         */

#endif
        Rotation2D (const rw::math::Vector2D< T >& i, const rw::math::Vector2D< T >& j)
        {
            _m[0][0] = i[0];
            _m[0][1] = j[0];
            _m[1][0] = i[1];
            _m[1][1] = j[1];
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 2x2 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{cc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param theta
         */

#endif
        Rotation2D (const T theta)
        {
            _m[0][0] = cos (theta);
            _m[0][1] = -sin (theta);
            _m[1][0] = sin (theta);
            _m[1][1] = cos (theta);
        }

        /**
           @brief Construct an initialized 2x2 rotation matrix.

           The second of column of the matrix is deduced from the first column.

           @param i [in] The first column of the rotation matrix.
        */
        Rotation2D (const rw::math::Vector2D< T >& i)
        {
            _m[0][0] = i[0];
            _m[0][1] = -i[1];
            _m[1][0] = i[1];
            _m[1][1] = i[0];
        }

        /**
           @brief Construct a rotation matrix from an Eigen matrix.
         */
        template< class R > explicit Rotation2D (const EigenMatrix2x2& m)
        {
            _m[0][0] = m (0, 0);
            _m[0][1] = m (0, 1);
            _m[1][0] = m (1, 0);
            _m[1][1] = m (1, 1);
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs a 2x2 rotation matrix set to identity
         * @return a 2x2 identity rotation matrix
         *
         * @f$
         * \mathbf{R} =
         * \left[
         * \begin{array}{cc}
         * 1 & 0\\
         * 0 & 1
         * \end{array}
         * \right]
         * @f$
         */

#endif
        static const Rotation2D& identity ()
        {
            static Rotation2D id (1, 0, 0, 1);
            return id;
        }

#if !defined(SWIG)
        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        T& operator() (size_t row, size_t column) { return _m[row][column]; }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        const T& operator() (size_t row, size_t column) const { return _m[row][column]; }
#else
        MATRIXOPERATOR (T);
#endif
        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] Rotation2D to compare with
         * @return True if equal.
         */
        bool operator== (const Rotation2D< T >& rhs) const
        {
            for (int i = 0; i < 2; ++i) {
                for (int j = 0; j < 2; ++j) {
                    if (!(_m[i][j] == rhs (i, j))) {
                        return false;
                    }
                }
            }
            return true;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] Rotation2D to compare with
         * @return True if not equal.
         */
        bool operator!= (const Rotation2D< T >& rhs) const { return !(*this == rhs); }

        /**
         * @brief Returns a boost 2x2 matrix @f$ \mathbf{M}\in SO(2)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(2) @f$
         */
        EigenMatrix2x2 e ()
        {
            EigenMatrix2x2 matrix;
            matrix (0, 0) = _m[0][0];
            matrix (0, 1) = _m[0][1];
            matrix (1, 0) = _m[1][0];
            matrix (1, 1) = _m[1][1];
            return matrix;
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         *
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        const Rotation2D operator* (const Rotation2D& bRc) const
        {
            return Rotation2D ((*this) (0, 0) * bRc (0, 0) + (*this) (0, 1) * bRc (1, 0),
                               (*this) (0, 0) * bRc (0, 1) + (*this) (0, 1) * bRc (1, 1),
                               (*this) (1, 0) * bRc (0, 0) + (*this) (1, 1) * bRc (1, 0),
                               (*this) (1, 0) * bRc (0, 1) + (*this) (1, 1) * bRc (1, 1));
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        const rw::math::Vector2D< T > operator* (const rw::math::Vector2D< T >& bVc) const
        {
            return rw::math::Vector2D< T > ((*this) (0, 0) * bVc (0) + (*this) (0, 1) * bVc (1),
                                            (*this) (1, 0) * bVc (0) + (*this) (1, 1) * bVc (1));
        }
#if !defined(SWIG)
        /**
         * @brief Writes rotation matrix to stream
         * @param os [in/out] output stream to use
         * @param r [in] rotation matrix to print
         * @return the updated output stream
         */
        friend std::ostream& operator<< (std::ostream& os, const Rotation2D& r)
        {
            return os << "Rotation2D {" << r (0, 0) << ", " << r (0, 1) << ", " << r (1, 0) << ", "
                      << r (1, 1) << "}";
        }
#else
        TOSTRING (rw::math::Rotation2D< T >);
#endif

      private:
        T _m[2][2];
        // Base _atrix;
    };

    /**
     * @brief Casts Rotation2D<T> to Rotation2D<Q>
     * @param rot [in] Rotation2D with type T
     * @return Rotation2D with type R
     */
    template< class R, class T > const Rotation2D< R > cast (const Rotation2D< T >& rot)
    {
        Rotation2D< R > res (Rotation2D< R >::identity ());
        for (size_t i = 0; i < 2; i++)
            for (size_t j = 0; j < 2; j++)
                res (i, j) = static_cast< R > (rot (i, j));
        return res;
    }

    /**
     * @brief The inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$ of a rotation matrix
     *
     * @relates Rotation2D
     *
     * @param aRb [in] the rotation matrix @f$ \robabx{a}{b}{\mathbf{R}} @f$
     *
     * @return the matrix inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$
     *
     * @f$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} =
     * \robabx{a}{b}{\mathbf{R}}^T @f$
     */
    template< class T > const Rotation2D< T > inverse (const Rotation2D< T >& aRb)
    {
        return Rotation2D< T > (aRb (0, 0), aRb (1, 0), aRb (0, 1), aRb (1, 1));
    }

    /**
     * @brief Find the transpose of \b aRb.
     *
     * The transpose of a rotation matrix is the same as the inverse.
     */
    template< class T > const Rotation2D< T > transpose (const Rotation2D< T >& aRb)
    {
        return Rotation2D< T > (aRb (0, 0), aRb (1, 0), aRb (0, 1), aRb (1, 1));
    }
#if !defined(SWIG)
    extern template class rw::math::Rotation2D< double >;
    extern template class rw::math::Rotation2D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Rotation2Dd, rw::math::Rotation2D< double >);
    SWIG_DECLARE_TEMPLATE (Rotation2Df, rw::math::Rotation2D< float >);
#endif
    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void write (const rw::math::Rotation2D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void write (const rw::math::Rotation2D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void read (rw::math::Rotation2D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation2D
         */
        template<>
        void read (rw::math::Rotation2D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

    }    // namespace serialization
}}       // namespace rw::common

#endif    // end include guard


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

#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

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

// Generic functor
// See http://eigen.tuxfamily.org/index.php?title=Functors
// C++ version of a function pointer that stores meta-data about the function
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{

  // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
  typedef _Scalar Scalar;
  enum { // Required by numerical differentiation module
      InputsAtCompileTime = NX,
      ValuesAtCompileTime = NY
  };

  // Tell the caller the matrix sizes associated with the input, output, and jacobian
  typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

  // Local copy of the number of inputs
  int m_inputs, m_values;

  // Two constructors:
  Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
  Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

  // Get methods for users to determine function input and output dimensions
  int inputs() const { return m_inputs; }
  int values() const { return m_values; }

};

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

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Booth Function
// Implement f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2
struct BoothFunctor : Functor<double>
{
  // Simple constructor
  BoothFunctor(): Functor<double>(2,3) {}

  // Implementation of the objective function
  int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const {
    double a = z(0);
	  double b = z(1);
	  double c = z(2);
	/*
     * Evaluate the Booth function.
     * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
     * of squared terms. The algorithm takes this into account: do not do it yourself.
     * In other words: objFun = sum(fvec(i)^2)
     */
    fvec(0) = 3*a -cos(b*c) -0.5;
    fvec(1) = pow(a,2.0) -81*pow((b+0.1),2.0)+sin(c)+1.06;
	  fvec(2) = exp(-a*b)+20*c+(10*M_PI-3.0)/3.0;
    return 0;
  }
};

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

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Himmelblau's Function
// Implement f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2
struct HimmelblauFunctor : Functor<double>
{
  // Simple constructor
  HimmelblauFunctor(): Functor<double>(2,2) {}
  // Implementation of the objective function
  int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const {
    double x = z(0);   double y = z(1);
    /*
     * Evaluate Himmelblau's function.
     * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
     * of squared terms. The algorithm takes this into account: do not do it yourself.
     * In other words: objFun = sum(fvec(i)^2)
     */
    fvec(0) = x * x + y - 11;
    fvec(1) = x + y * y - 7;
    return 0;
  }
};

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

void testBoothFun() {
  std::cout << "Testing the Booth function..." << std::endl;
  Eigen::VectorXd zInit(3); zInit << 0.1, 0.1, -0.1;
  std::cout << "zInit: " << zInit.transpose() << std::endl;

  BoothFunctor functor;
  Eigen::NumericalDiff<BoothFunctor> numDiff(functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<BoothFunctor>,double> lm(numDiff);
  lm.parameters.maxfev = 1000;
  lm.parameters.xtol = 1.0e-10;
  std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
  std::cout << "x tol: " << lm.parameters.xtol << std::endl;

  Eigen::VectorXd z = zInit;
  int ret = lm.minimize(z);
  std::cout << "iter count: " << lm.iter << std::endl;
  std::cout << "return status: " << ret << std::endl;
  std::cout << "zSolver: " << z.transpose() << std::endl;
  std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

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

void testHimmelblauFun() {
  std::cout << "Testing the Himmelblau function..." << std::endl;
  // Eigen::VectorXd zInit(2); zInit << 0.0, 0.0;  // soln 1
  // Eigen::VectorXd zInit(2); zInit << -1, 1;  // soln 2
  // Eigen::VectorXd zInit(2); zInit << -1, -1;  // soln 3
  Eigen::VectorXd zInit(2); zInit << 1, -1;  // soln 4
  std::cout << "zInit: " << zInit.transpose() << std::endl;
  std::cout << "soln 1: [3.0, 2.0]" << std::endl;
  std::cout << "soln 2: [-2.805118, 3.131312]" << std::endl;
  std::cout << "soln 3: [-3.77931, -3.28316]" << std::endl;
  std::cout << "soln 4: [3.584428, -1.848126]" << std::endl;

  HimmelblauFunctor functor;
  Eigen::NumericalDiff<HimmelblauFunctor> numDiff(functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<HimmelblauFunctor>,double> lm(numDiff);
  lm.parameters.maxfev = 1000;
  lm.parameters.xtol = 1.0e-10;
  std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
  std::cout << "x tol: " << lm.parameters.xtol << std::endl;

  Eigen::VectorXd z = zInit;
  int ret = lm.minimize(z);
  std::cout << "iter count: " << lm.iter << std::endl;
  std::cout << "return status: " << ret << std::endl;
  std::cout << "zSolver: " << z.transpose() << std::endl;
  std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

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

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

	std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
	testBoothFun();
	testHimmelblauFun();
	return 0;
}


#ifndef RSVD_STANDARD_NORMAL_RANDOM_HPP_
#define RSVD_STANDARD_NORMAL_RANDOM_HPP_

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

namespace Rsvd {

namespace Internal {

namespace {
/// \brief Shortcut for the underlying \a real type used in the matrix.
///
/// \long E.g. if the matrix is of type \c std::complex<double>, this typedef equals to \c double.
template <typename MatrixType>
using RealType = typename Eigen::NumTraits<typename MatrixType::Scalar>::Real;
} // namespace

/// \brief Helper struct to do partial specialization in the matrix scalar type
///
/// \tparam MatrixType Eigen matrix type.
///
/// \tparam ScalarType Underlying scalar type used in \c MatrixType, can be complex or real.
///
/// \tparam RandomEngineType Type of the random engine, e.g. \c std::default_random_engine or \c
/// std::mt19937_64.
template <typename MatrixType, typename ScalarType, typename RandomEngineType>
struct StandardNormalRandomHelper {
  static inline MatrixType generate(Eigen::Index numRows, Eigen::Index numCols,
                                    RandomEngineType &engine);
};

/// \brief Partial specialization for real matrices.
template <typename MatrixType, typename RandomEngineType>
struct StandardNormalRandomHelper<MatrixType, RealType<MatrixType>, RandomEngineType> {
  static inline MatrixType generate(const Eigen::Index numRows, const Eigen::Index numCols,
                                    RandomEngineType &engine) {
    // Create a standard normal distribution with zero mean (mu = 0) and unity variance (sigma^2 =
    // 1)
    std::normal_distribution<RealType<MatrixType>> distribution{0, 1};
    const auto normal{[&](typename MatrixType::Scalar) { return distribution(engine); }};
    return MatrixType::NullaryExpr(numRows, numCols, normal);
  }
};

/// \brief Partial specialization for complex matrices.
template <typename MatrixType, typename RandomEngineType>
struct StandardNormalRandomHelper<MatrixType, std::complex<RealType<MatrixType>>,
                                  RandomEngineType> {
  static inline MatrixType generate(const Eigen::Index numRows, const Eigen::Index numCols,
                                    RandomEngineType &engine) {
    // A complex standard normal distribution has half of its variance in the real variable and
    // half in the complex. Hence, the each variance is 1/2 and each standard deviation is
    // 1/sqrt(2)
    constexpr RealType<MatrixType> stdDev{0.707106781186547};
    std::normal_distribution<RealType<MatrixType>> distribution{0, stdDev};
    const auto complexNormal{[&](typename MatrixType::Scalar) {
      return std::complex<RealType<MatrixType>>(distribution(engine), distribution(engine));
    }};
    return MatrixType::NullaryExpr(numRows, numCols, complexNormal);
  }
};

/// \brief Generate a matrix with iid elements from standard normal
/// distribution.
///
/// \long The matrix elements are drawn from the standard normal distribution (zero mean and unit
/// variance), i.e. \f$ [ \Omega ]_{i,j} \sim \mathcal{N}(0, 1) \f$ for the real case and \f$ [
/// \Omega ]_{i,j} \sim \mathcal{N}\left(0, \frac{1}{2} \right) + \mathcal{N}\left(0, \frac{1}{2}
/// \right) \cdot \mathrm{i}\f$ for the complex case.
///
/// \tparam MatrixType Eigen matrix type.
/// \tparam RandomEngineType Type of the random engine, e.g. \c std::default_random_engine or \c
/// std::mt19937_64.
///
/// \param numRows Number of matrix rows \f$ m \f$.
/// \param numCols Number of matrix columns \f$ n \f$.
/// \param engine Reference to the random engine.
///
/// \return Matrix \f$ \Omega \in \mathbb{F}^{m \times n} \f$ with \f$\mathbb{F} \in \{ \mathbb{R},
/// \mathbb{C} \} \f$ with normally distributed elements.
template <typename MatrixType, typename RandomEngineType>
inline MatrixType standardNormalRandom(const Eigen::Index numRows, const Eigen::Index numCols,
                                       RandomEngineType &engine) {
  return StandardNormalRandomHelper<MatrixType, typename MatrixType::Scalar,
                                    RandomEngineType>::generate(numRows, numCols, engine);
}

} // namespace Internal

} // namespace Rsvd

#endif


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(5,5);
MatrixXd A = X * X.transpose();
X = MatrixXd::Random(5,5);
MatrixXd B = X * X.transpose();

GeneralizedSelfAdjointEigenSolver<MatrixXd> es(A,B,EigenvaluesOnly);
cout << "The eigenvalues of the pencil (A,B) are:" << endl << es.eigenvalues() << endl;
es.compute(B,A,false);
cout << "The eigenvalues of the pencil (B,A) are:" << endl << es.eigenvalues() << endl;

  return 0;
}


#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

#include <map>
#include <vector>

using std::vector;
using std::map;

namespace mcl_cpp
{
	class mcl
	{
	public:
		//! @brief MCL ctor. Register a callback function to return the cluster results
		mcl(const Eigen::MatrixXd& Min, std::function< void(size_t cluster_j, size_t member_i) > f) : M(Min), ClusterResultCallback(f) {}

		/*! @brief Apply Markov clustering algorithm with specified parameters and return clusters
			For each cluster, returns the list of node-ids that belong to that cluster
			*/
		Eigen::MatrixXd cluster_mcl(double expand_factor = 2, double inflate_factor = 2, double max_loop = 10, double mult_factor = 1)
		{
			Eigen::MatrixXd M_selfloop = M + mult_factor * Eigen::MatrixXd::Identity(M.cols(), M.rows());
			Eigen::MatrixXd M_normalized = normalize(M_selfloop);
			int i;
			for (i = 0; i < max_loop && !stop(M_normalized, i); i++)
			{
				inflate(M_normalized, inflate_factor);
				expand(M_normalized, expand_factor);
			}
			for (auto row = 0; row<M_normalized.rows(); row++)
			{
				if (M_normalized(row, row) > 0)
				{
					for (auto col = 0; col < M_normalized.cols(); col++)
					{
						if (M_normalized.coeff(row, col) > 0)
							ClusterResultCallback(row, col);
					}
				}
			}
			return std::move(M_normalized);
		}

	private:
		bool stop(const Eigen::MatrixXd& in, int i)
		{
			Eigen::MatrixXd m = in*in;
			Eigen::MatrixXd diff = (m - in);
			double mx = diff.maxCoeff(), mn = diff.minCoeff();
			return (diff.maxCoeff() == diff.minCoeff());
		}

		Eigen::MatrixXd normalize(Eigen::MatrixXd& in)
		{
			Eigen::MatrixXd one_over_col_sum = in.colwise().sum().cwiseInverse();
			Eigen::MatrixXd M_normalized = in * one_over_col_sum.asDiagonal();
			return std::move(M_normalized);
		}

		void expand(Eigen::MatrixXd& in, double expand_factor)
		{
			Eigen::MatrixPower<Eigen::MatrixXd> Apow(in);
			in = Apow(expand_factor);
		}

		void inflate(Eigen::MatrixXd& in, double inflate_factor)
		{
			auto lam = [inflate_factor](double x) -> double { return std::pow(x, inflate_factor); };
			in = in.unaryExpr(lam);
			in = normalize(in);
		}

		const Eigen::MatrixXd & M;
		std::function< void(size_t cls_i, size_t mem_j) > ClusterResultCallback;
	};

}