/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2016 Andres Hernandez

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/qldefines.hpp>
#ifdef BOOST_MSVC
#  include <ql/auto_link.hpp>
#endif
#include <ql/math/optimization/differentialevolution.hpp>
#include <ql/math/optimization/simulatedannealing.hpp>
#include <ql/experimental/math/fireflyalgorithm.hpp>
#include <ql/experimental/math/hybridsimulatedannealing.hpp>
#include <ql/experimental/math/particleswarmoptimization.hpp>
#include <ql/functional.hpp>

#include <boost/timer.hpp>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

unsigned long seed = 127;

/*
    Some benchmark functions taken from
    https://en.wikipedia.org/wiki/Test_functions_for_optimization

    Global optimizers have generally a lot of hyper-parameters, and one
    * usually requires some hyper-parameter optimization to find appropriate values
*/

Real ackley(const Array& x) {
    //Minimum is found at 0
    Real p1 = 0.0, p2 = 0.0;

    for (Size i = 0; i < x.size(); i++) {
        p1 += x[i] * x[i];
        p2 += std::cos(M_TWOPI*x[i]);
    }
    p1 = -0.2*std::sqrt(0.5*p1);
    p2 *= 0.5;
    return M_E + 20.0 - 20.0*std::exp(p1)-std::exp(p2);
}

Disposable<Array> ackleyValues(const Array& x) {
    Array y(x.size());
    for (Size i = 0; i < x.size(); i++) {
        Real p1 = x[i] * x[i];
        p1 = -0.2*std::sqrt(0.5*p1);
        Real p2 = 0.5*std::cos(M_TWOPI*x[i]);
        y[i] = M_E + 20.0 - 20.0*std::exp(p1)-std::exp(p2);
    }
    return y;
}

Real sphere(const Array& x) {
    //Minimum is found at 0
    return DotProduct(x, x);
}

Disposable<Array> sphereValues(const Array& x) {
    Array y(x.size());
    for (Size i = 0; i < x.size(); i++) {
        y[i] = x[i]*x[i];
    }
    return y;
}

Real rosenbrock(const Array& x) {
    //Minimum is found at f(1, 1, ...)
    QL_REQUIRE(x.size() > 1, "Input size needs to be higher than 1");
    Real result = 0.0;
    for (Size i = 0; i < x.size() - 1; i++) {
        Real temp = (x[i + 1] - x[i] * x[i]);
        result += (x[i] - 1.0)*(x[i] - 1.0) + 100.0*temp*temp;
    }
    return result;
}

Real easom(const Array& x) {
    //Minimum is found at f(\pi, \pi, ...)
    Real p1 = 1.0, p2 = 0.0;
    for (Size i = 0; i < x.size(); i++) {
        p1 *= std::cos(x[i]);
        p2 += (x[i] - M_PI)*(x[i] - M_PI);
    }
    return -p1*std::exp(-p2);
}

Disposable<Array> easomValues(const Array& x) {
    Array y(x.size());
    for (Size i = 0; i < x.size(); i++) {
        Real p1 = std::cos(x[i]);
        Real p2 = (x[i] - M_PI)*(x[i] - M_PI);
        y[i] = -p1*std::exp(-p2);
    }
    return y;
}

Real eggholder(const Array& x) {
    //Minimum is found at f(512, 404.2319)
    QL_REQUIRE(x.size() == 2, "Input size needs to be equal to 2");
    Real p = (x[1] + 47.0);
    return -p*std::sin(std::sqrt(std::abs(0.5*x[0] + p))) -
        x[0] * std::sin(std::sqrt(std::abs(x[0] - p)));
}

Real printFunction(Problem& p, const Array& x) {
    std::cout << " f(" << x[0];
    for (Size i = 1; i < x.size(); i++) {
        std::cout << ", " << x[i];
    }
    Real val = p.value(x);
    std::cout << ") = " << val << std::endl;
    return val;
}

class TestFunction : public CostFunction {
public:
    typedef ext::function<Real(const Array&)> RealFunc;
    typedef ext::function<Disposable<Array>(const Array&)> ArrayFunc;
    TestFunction(const RealFunc & f, const ArrayFunc & fs = ArrayFunc()) : f_(f), fs_(fs) {}
    TestFunction(Real(*f)(const Array&), Disposable<Array>(*fs)(const Array&) = NULL) : f_(f), fs_(fs) {}
    virtual ~TestFunction(){}
    virtual Real value(const Array& x) const {
        return f_(x);
    }
    virtual Disposable<Array> values(const Array& x) const {
        if(!fs_)
            throw std::runtime_error("Invalid function");
        return fs_(x);
    }
private:
    RealFunc f_;
    ArrayFunc fs_;
};

int test(OptimizationMethod& method, CostFunction& f, const EndCriteria& endCriteria,
          const Array& start, const Constraint& constraint = Constraint(),
          const Array& optimum = Array()) {
    QL_REQUIRE(start.size() > 0, "Input size needs to be at least 1");
    std::cout << "Starting point: ";
    Constraint c;
    if (!constraint.empty())
        c = constraint;
    Problem p(f, c, start);
    printFunction(p, start);
    method.minimize(p, endCriteria);
    std::cout << "End point: ";
    Real val = printFunction(p, p.currentValue());
    if(!optimum.empty())
    {
        std::cout << "Global optimium: ";
        Real optimVal = printFunction(p, optimum);
        if(std::abs(optimVal) < 1e-13)
            return std::abs(val-optimVal) < 1e-6;
        else
            return std::abs((val-optimVal)/optimVal) < 1e-6;
    }
    return 1;
}

void testFirefly() {
    /*
    The Eggholder function is only in 2 dimensions, it has a multitude
    * of local minima, and they are not symmetric necessarily
    */
    Size n = 2;
    NonhomogeneousBoundaryConstraint constraint(Array(n, -512.0), Array(n, 512.0));
    Array x(n, 0.0);
    Array optimum(n);
    optimum[0] = 512.0;
    optimum[1] = 404.2319;
    Size agents = 150;
    Real vola = 1.5;
    Real intense = 1.0;
    ext::shared_ptr<FireflyAlgorithm::Intensity> intensity =
        ext::make_shared<ExponentialIntensity>(10.0, 1e-8, intense);
    ext::shared_ptr<FireflyAlgorithm::RandomWalk> randomWalk =
        ext::make_shared<LevyFlightWalk>(vola, 0.5, 1.0, seed);
    std::cout << "Function eggholder, Agents: " << agents
            << ", Vola: " << vola << ", Intensity: " << intense << std::endl;
    TestFunction f(eggholder);
    FireflyAlgorithm fa(agents, intensity, randomWalk, 40);
    EndCriteria ec(5000, 1000, 1.0e-8, 1.0e-8, 1.0e-8);
    test(fa, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testSimulatedAnnealing(Size dimension, Size maxSteps, Size staticSteps){

    /*The ackley function has a large amount of local minima, but the
      structure is symmetric, so if one could simply just ignore the
      walls separating the local minima, it would look like almost
      like a parabola

      Andres Hernandez: I could not find a configuration that was able
      to fix the problem
    */

    //global minimum is at 0.0
    TestFunction f(ackley, ackleyValues);

    //Starting point
    Array x(dimension, 1.5);
    Array optimum(dimension, 0.0);

    //Constraint for local optimizer
    Array lower(dimension, -5.0);
    Array upper(dimension, 5.0);
    NonhomogeneousBoundaryConstraint constraint(lower, upper);

    Real lambda = 0.1;
    Real temperature = 350;
    Real epsilon = 0.99;
    Size ms = 1000;
    std::cout << "Function ackley, Lambda: " << lambda
            << ", Temperature: " << temperature
            << ", Epsilon: " << epsilon
            << ", Iterations: " << ms
            << std::endl;

    MersenneTwisterUniformRng rng(seed);
    SimulatedAnnealing<MersenneTwisterUniformRng> sa(lambda, temperature, epsilon, ms, rng);
    EndCriteria ec(maxSteps, staticSteps, 1.0e-8, 1.0e-8, 1.0e-8);
    test(sa, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testGaussianSA(Size dimension, Size maxSteps, Size staticSteps, Real initialTemp,
                    Real finalTemp,
                    GaussianSimulatedAnnealing::ResetScheme resetScheme = GaussianSimulatedAnnealing::ResetToBestPoint,
                    Size resetSteps = 150,
                    GaussianSimulatedAnnealing::LocalOptimizeScheme optimizeScheme = GaussianSimulatedAnnealing::EveryBestPoint,
                    ext::shared_ptr<OptimizationMethod> localOptimizer = ext::make_shared<LevenbergMarquardt>()){

    /*The ackley function has a large amount of local minima, but the
     * structure is symmetric, so if one could simply just ignore the
     * walls separating the local minima, it would look like almost like
     * a parabola*/

    //global minimum is at 0.0
    TestFunction f(ackley, ackleyValues);

    std::cout << "Function: ackley, Dimensions: " << dimension
              << ", Initial temp:" << initialTemp
              << ", Final temp:" << finalTemp
              << ", Reset scheme:" << resetScheme
              << ", Reset steps:" << resetSteps
              << std::endl;
    //Starting point
    Array x(dimension, 1.5);
    Array optimum(dimension, 0.0);

    //Constraint for local optimizer
    Array lower(dimension, -5.0);
    Array upper(dimension, 5.0);
    NonhomogeneousBoundaryConstraint constraint(lower, upper);

    //Simulated annealing setup
    SamplerGaussian sampler(seed);
    ProbabilityBoltzmannDownhill probability(seed);
    TemperatureExponential temperature(initialTemp, dimension);
    GaussianSimulatedAnnealing sa(sampler, probability, temperature, ReannealingTrivial(),
                                  initialTemp, finalTemp, 50, resetScheme,
                                  resetSteps, localOptimizer,
                                  optimizeScheme);

    EndCriteria ec(maxSteps, staticSteps, 1.0e-8, 1.0e-8, 1.0e-8);
    test(sa, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testPSO(Size n){
    /*The Rosenbrock function has a global minima at (1.0, ...) and a local minima at (-1.0, 1.0, ...)
    The difficulty lies in the weird shape of the function*/
    NonhomogeneousBoundaryConstraint constraint(Array(n, -1.0), Array(n, 4.0));
    Array x(n, 0.0);
    Array optimum(n, 1.0);
    Size agents = 100;
    Size kneighbor = 25;
    Size threshold = 500;
    std::cout << "Function: rosenbrock, Dimensions: " << n
            << ", Agents: " << agents << ", K-neighbors: " << kneighbor
            << ", Threshold: " << threshold << std::endl;
    ext::shared_ptr<ParticleSwarmOptimization::Topology> topology =
        ext::make_shared<KNeighbors>(kneighbor);
    ext::shared_ptr<ParticleSwarmOptimization::Inertia> inertia =
        ext::make_shared<LevyFlightInertia>(1.5, threshold, seed);
    TestFunction f(rosenbrock);
    ParticleSwarmOptimization pso(agents, topology, inertia, 2.05, 2.05, seed);
    EndCriteria ec(10000, 1000, 1.0e-8, 1.0e-8, 1.0e-8);
    test(pso, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void testDifferentialEvolution(Size n, Size agents){
    /*The Rosenbrock function has a global minima at (1.0, ...) and a local minima at (-1.0, 1.0, ...)
    The difficulty lies in the weird shape of the function*/
    NonhomogeneousBoundaryConstraint constraint(Array(n, -4.0), Array(n, 4.0));
    Array x(n, 0.0);
    Array optimum(n, 1.0);

    TestFunction f(rosenbrock);

    Real probability = 0.3;
    Real stepsizeWeight = 0.6;
    DifferentialEvolution::Strategy strategy = DifferentialEvolution::BestMemberWithJitter;

    std::cout << "Function: rosenbrock, Dimensions: " << n << ", Agents: " << agents
              << ", Probability: " << probability
              << ", StepsizeWeight: " << stepsizeWeight
              << ", Strategy: BestMemberWithJitter" << std::endl;
    DifferentialEvolution::Configuration config;
    config.withBounds(true)
          .withCrossoverProbability(probability)
          .withPopulationMembers(agents)
          .withStepsizeWeight(stepsizeWeight)
          .withStrategy(strategy)
          .withSeed(seed);

    DifferentialEvolution de(config);
    EndCriteria ec(5000, 1000, 1.0e-8, 1.0e-8, 1.0e-8);
    test(de, f, ec, x, constraint, optimum);
    std::cout << "================================================================" << std::endl;
}

void printTime(double seconds){
    Integer hours = int(seconds/3600);
    seconds -= hours * 3600;
    Integer minutes = int(seconds/60);
    seconds -= minutes * 60;
    std::cout << " \nRun completed in ";
    if (hours > 0)
        std::cout << hours << " h ";
    if (hours > 0 || minutes > 0)
        std::cout << minutes << " m ";
    std::cout << std::fixed << std::setprecision(0)
              << seconds << " s\n" << std::endl;
}

int main(int, char* []) {

    try {
        std::cout << std::endl;
        boost::timer timer;

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Firefly Algorithm Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testFirefly();

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Hybrid Simulated Annealing Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testGaussianSA(3, 500, 200, 100.0, 0.1, GaussianSimulatedAnnealing::ResetToBestPoint, 150, GaussianSimulatedAnnealing::EveryNewPoint);
        testGaussianSA(10, 500, 200, 100.0, 0.1, GaussianSimulatedAnnealing::ResetToBestPoint, 150, GaussianSimulatedAnnealing::EveryNewPoint);
        testGaussianSA(30, 500, 200, 100.0, 0.1, GaussianSimulatedAnnealing::ResetToBestPoint, 150, GaussianSimulatedAnnealing::EveryNewPoint);

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Particle Swarm Optimization Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testPSO(3);
        testPSO(10);
        testPSO(30);

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Simulated Annealing Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testSimulatedAnnealing(3, 10000, 4000);
        testSimulatedAnnealing(10, 10000, 4000);
        testSimulatedAnnealing(30, 10000, 4000);

        printTime(timer.elapsed());

        std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
        std::cout << "Differential Evolution Test" << std::endl;
        std::cout << "----------------------------------------------------------------" << std::endl;
        testDifferentialEvolution(3, 50);
        testDifferentialEvolution(10, 150);
        testDifferentialEvolution(30, 450);

        printTime(timer.elapsed());

        return 0;
    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <inttypes.h>
#include <stdint.h>
#include "dSFMText.hpp"
#include "dSFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace dsfmt;
using namespace NTL;
using namespace std;

void calc_minimal(GF2X& minimal, dSFMText& dsfmt, int bitpos)
{
    uint64_t mask[2];
    if (bitpos >= 52) {
	mask[0] = 0;
	mask[1] = UINT64_C(1) << (bitpos - 52);
    } else {
	mask[0] = UINT64_C(1) << bitpos;
	mask[1] = 0;
    }
    int maxdegree = dsfmt.get_mamaxdegree();
    vec_GF2 seq;
    seq.SetLength(2 * maxdegree);
    for (int i = 0; i < 2 * maxdegree; i++) {
	seq[i] = dsfmt.next(mask);
    }
    MinPolySeq(minimal, seq, maxdegree);
#if defined(DEBUG)
    cout << "deg(minimal) = " << dec << deg(minimal) << endl;
#endif
}

void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
    GF2X gcd;
    mul(lcm, x, y);
    GCD(gcd, x, y);
    lcm /= gcd;
}

void get_lcm(GF2X& lcmpoly, dSFMText& dsfmt) {
    GF2X minimal;
    GF2X tmp;
    int maxdegree = dsfmt.get_mamaxdegree();
    dsfmt.seeding(1234);
//    dsfmt.set_high()
//    dsfmt.seeding(1234, 1);
    for (int bitpos = 0; bitpos < 52 * 2; bitpos++) {
	calc_minimal(minimal, dsfmt, bitpos);
	LCM(tmp, lcmpoly, minimal);
	lcmpoly = tmp;
#if defined(DEBUG)
    cout << "deg(lcm) = " << dec << deg(lcmpoly) << endl;
#endif
#if 0
	if (deg(lcmpoly) == maxdegree) {
	    return;
	}
	if (deg(lcmpoly) > maxdegree) {
	    return;
	}
#endif
    }
#if defined(DEBUG)
    cout << "deg(lcm) = " << dec << deg(lcmpoly) << endl;
#endif
#if 1
    for(int i = 0; i < maxdegree; i++) {
	dsfmt.init_basis();
	for (int bitpos = 0; bitpos < 52 * 2; bitpos++) {
	    calc_minimal(minimal, dsfmt, bitpos);
	    LCM(tmp, lcmpoly, minimal);
	    lcmpoly = tmp;
#if 0
	    if (deg(lcmpoly) == maxdegree) {
		return;
	    }
#endif
	}
    }
#endif
#if 0
    cerr << "can't find lcm deg=" << dec << deg(lcmpoly) << endl;
    cerr << "maxdegree = " << dec << maxdegree << endl;
    throw new logic_error("can't find lcm");
#endif
}

static int has_large_irreducible(GF2X& fpoly, int degree) {
    static const GF2X t2(2, 1);
    static const GF2X t1(1, 1);
    GF2X t2m;
    GF2X t;
    GF2X alpha;
    int m;

    t2m = t2;
    if (deg(fpoly) < degree) {
	return 0;
    }
    t = t1;
    t += t2m;

    for (m = 1; deg(fpoly) > degree; m++) {
	for(;;) {
	    GCD(alpha, fpoly, t);
	    if (IsOne(alpha)) {
		break;
	    }
	    fpoly /= alpha;
	    if (deg(fpoly) < degree) {
		return 0;
	    }
	}
	t2m *= t2m;
	t2m %= fpoly;
	add(t, t2m, t1);
    }
    if (deg(fpoly) != degree) {
	return 0;
    }
    return IterIrredTest(fpoly);
}

void check_fix(dSFMText & fix)
{
    fix.setup_high();
    cout << "before:";
    fix.print(cout);
    fix.next();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next();
    cout << "after 3 step:";
    fix.print(cout);
}

void check_fix2(dSFMText & fix)
{
    cout << "before:";
    fix.print(cout);
    fix.next_add();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 3 step:";
    fix.print(cout);
}

void calc_fix(GF2X& lcm, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText fix(mexp, pos1, sl1, mask1, mask2);
    //fix.setup_high();
    //fix.next();
    //cout << "setup0:";
    //fix.print(cout);
    fix.setup_constants();
    cout << "setup:";
    fix.print(cout);
    dSFMText work(mexp, pos1, sl1, mask1, mask2);
    cout << "zero:";
    work.print(cout);
    for (int i = 0; i <= deg(lcm); i++) {
	if (IsOne(coeff(lcm, i))) {
	    work.add(fix);
#if defined(DEBUG) && 0
    cout << "work:";
    work.print(cout);
#endif
	}
	fix.next();
    }
    cout << "fix:";
    work.print(cout);
    check_fix2(work);
#if 0
    cout << "* (t+1)" << endl;
    GF2X t1(1,1);
    dSFMText work2(mexp, pos1, sl1, mask1, mask2);
    cout << "work2:";
    work2.print(cout);
    for (int i = 0; i <= deg(t1); i++) {
	if (IsOne(coeff(t1, i))) {
	    work2.add(work);
#if defined(DEBUG)
    cout << "work2:";
    work2.print(cout);
#endif
	}
	work.next();
    }
    cout << "work2:";
    work2.print(cout);
#endif
}

void check_const(GF2X& lcm, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText normal(mexp, pos1, sl1, mask1, mask2);
    dSFMText add(mexp, pos1, sl1, mask1, mask2);
    normal.seeding(123);
    add.seeding(123);

    normal.setup_high();
    normal.next();
    add.next_add();
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
    for (int i = 0; i < 5; i++) {
	normal.next();
	add.next_add();
    }
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
}

int main(int argc, char *argv[]) {
    if (argc < 6) {
	cout << argv[0] << " mexp pos1 sl1 mask1 mask2" << endl;
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    uint64_t mask[2];
    mask[0] = strtoull(argv[4], NULL, 16);
    mask[1] = strtoull(argv[5], NULL, 16);
#if defined(DEBUG)
    cout << "mexp:" << dec << mexp << endl;
    cout << "pos1:" << dec << pos1 << endl;
    cout << "sl1:" << dec << sl1 << endl;
    cout << "mask1:" << hex<< mask[0] << endl;
    cout << "mask2:" << hex << mask[1] << endl;
#endif
    dSFMText dsfmt(mexp, pos1, sl1, mask[0], mask[1]);
    GF2X characteristic(0,1);
    get_lcm(characteristic, dsfmt);
#if defined(DEBUG)
    cout << "degree:" << dec << deg(characteristic) << endl;
    cerr << "maxdegree = " << dec << dsfmt.get_mamaxdegree() << endl;
    cout << characteristic << endl;
#endif
    GF2X work;
    work = characteristic;
#if defined(DEBUG)
    cout << "degree:" << deg(characteristic) << endl;
    cout << characteristic << endl;
#endif
    if (!has_large_irreducible(characteristic, mexp)) {
        cout << "error?" << endl;
        return -1;
    }
#if 0
    GF2X remain = work / characteristic;
    vec_pair_GF2X_long factors;
    CanZass(factors, remain);
    cout << "=== factor of remain ===" << endl;
    for (int i = 0; i < factors.length(); i++) {
	cout << factors[i].a;
	cout << ":";
	cout << factors[i].b << endl;
    }
    cout << "=== factor of remain ===" << endl;
#endif
    GF2X d, s, inv;
    GF2X t1(1,1);
    XGCD(d, s, inv, work, t1);
// d = gcd(a,b), a s + b t = d
    MulMod(s, inv, t1, work);
    if (deg(s) != 0) {
	cout << "inv is not inv." << endl;
    }

    string x;
//    cout << "# deg = " << dec << deg(work) << endl;
    polytostring(x, work);
    cout << "#" << dec << mexp;
    cout << "," << dec << pos1;
    cout << "," << dec << sl1;
    cout << "," << hex << mask[0];
    cout << "," << hex << mask[1];
    cout << dec << endl;
    cout << x << endl;
    cout << dec << flush;
#if 0
    string y;
    polytostring(y, inv);
    if (deg(d) == 0) {
	cout << y << endl;
    } else {
	cout << "can't finf inv d=" << d << endl;
    }
    calc_fix(inv, mexp, pos1, sl1, mask[0], mask[1]);
    //check_const(inv, mexp, pos1, sl1, mask[0], mask[1]);
#endif
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <strstream>
#include <fstream>
#include <set>
#include <map>
using namespace Eigen;

MatrixXd V;
MatrixXi F;
std::vector<std::vector<int> > IrregularF;

void Upsampling()
{
	std::map<std::pair<int, int>, int> edgeid;
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			if (v1 > v2)
				std::swap(v1, v2);
			auto key = std::make_pair(v1, v2);
			int s = edgeid.size();
			if (edgeid.count(key) == 0)
				edgeid[key] = s;
		}
	}
	std::vector<Vector4i> faces(F.cols() * 4);
	std::vector<Vector3d> vertices(V.cols() + edgeid.size() + F.cols());
	for (int i = 0; i < V.cols(); ++i) {
		vertices[i] = V.col(i);
	}
	for (auto info : edgeid)
		vertices[V.cols() + info.second] = 0.5 * (V.col(info.first.first) + V.col(info.first.second));
	for (int i = 0; i < F.cols(); ++i) {
		Vector3d p(0, 0, 0);
		for (int j = 0; j < 4; ++j) {
			p += V.col(F(j, i));
		}
		p *= 0.25;
		vertices[V.cols() + edgeid.size() + i] = p;
	}
	for (int i = 0; i < F.cols(); ++i) {
		int eid[4];
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			if (v1 > v2)
				std::swap(v1, v2);
			auto key = std::make_pair(v1, v2);
			if (edgeid.count(key) == 0) {
				printf("OMG!\n");
			}
			eid[j] = edgeid[key];
		}
		for (int j = 0; j < 4; ++j) {
			faces[i * 4 + j] =
				Vector4i(F(j, i), eid[j] + V.cols(), V.cols() + edgeid.size() + i, eid[(j + 3) % 4] + V.cols());
		}
	}
	V.resize(3, vertices.size());
	memcpy(V.data(), vertices.data(), sizeof(double) * 3 * vertices.size());
	F.resize(4, faces.size());
	memcpy(F.data(), faces.data(), sizeof(int) * 4 * faces.size());
}
void Load(const char* filename) {
	std::vector<Vector3d> positions;
	std::vector<Vector4i> faces;
	std::ifstream is(filename);
	char buffer[2048];
	while (is.getline(buffer, 2048)) {
		std::strstream str;
		str << buffer;
		str >> buffer;
		if (strcmp(buffer, "v") == 0) {
			double x, y, z;
			str >> x >> y >> z;
			positions.push_back(Vector3d(x, y, z));
		}
		else if (strcmp(buffer, "f") == 0) {
			std::vector<int> face;
			while (str >> buffer) {
				std::strstream fstr;
				fstr << buffer;
				int f;
				fstr >> f;
				face.push_back(f-1);
			}
			if (face.size() == 4) {
				Vector4i f(face[0], face[1], face[2], face[3]);
				faces.push_back(f);
			} else {
				IrregularF.push_back(face);
			}
		}
	}
	V.resize(3, positions.size());
	F.resize(4, faces.size());
	memcpy(F.data(), faces.data(), sizeof(int) * 4 * faces.size());

	std::vector<std::set<int> > VF(positions.size());
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int x = F(j, i);
			int y = F((j + 1) % 4, i);
			VF[x].insert(y);
			VF[y].insert(x);
		}
	}
	std::vector<Vector3d> new_positions(positions.size());
	for (int i = 0; i < positions.size(); ++i) {
		new_positions[i] = Eigen::Vector3d(0, 0, 0);
	}
	for (int i = 0; i < VF.size(); ++i) {
		for (auto& ind : VF[i]) {
			new_positions[i] += positions[ind];
		}
		new_positions[i] /= VF[i].size();
	}

	memcpy(V.data(), new_positions.data(), sizeof(double) * 3 * positions.size());
}

void ReportFaceInfo()
{
	printf("Vertex: %d      Regular Faes: %d       Irregular faces %d\n", V.cols(), F.cols(), IrregularF.size());
}

void ReportManifold()
{
	std::set<std::pair<int, int> > dedges;
	bool isManifold = true;
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			auto key = std::make_pair(v1, v2);
			if (dedges.count(key)) {
				isManifold = false;
			} else {
				dedges.insert(key);
			}
		}
	}
	for (int i = 0; i < IrregularF.size(); ++i) {
		for (int j = 0; j < IrregularF[i].size(); ++j) {
			int v1 = IrregularF[i][j];
			int v2 = IrregularF[i][(j + 1) % IrregularF[i].size()];
			auto key = std::make_pair(v1, v2);
			if (dedges.count(key)) {
				isManifold = false;
			} else {
				dedges.insert(key);
			}
		}
	}
	if (isManifold) {
		printf("Is Manifold: True.\n");
	} else {
		printf("Is Manifold: False.\n");
	}
}

void ReportSingularity()
{
	std::vector<std::set<int> > links(V.cols());
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v1 = F(j, i);
			int v2 = F((j + 1) % 4, i);
			links[v1].insert(v2);
			links[v2].insert(v1);
		}
	}
	for (int i = 0; i < IrregularF.size(); ++i) {
		for (int j = 0; j < IrregularF[i].size(); ++j) {
			int v1 = IrregularF[i][j];
			int v2 = IrregularF[i][(j + 1) % IrregularF[i].size()];
			links[v1].insert(v2);
			links[v2].insert(v1);
		}
	}
	std::map<int, int> valences;
	for (int i = 0; i < links.size(); ++i) {
		int v = links[i].size();
		if (v != 0) {
			if (v > 5) {
				//printf("Weird %d\n", i);
			}
			if (valences.count(v)) {
				valences[v] += 1;
			} else {
				valences[v] = 1;
			}
		}
	}
	for (auto& p : valences) {
		printf("Valence %d: %d\n", p.first, p.second);
	}
}

void ReportAngleDifference()
{
	double e = 0, max_angle = 0, min_angle = 360;
	for (int i = 0; i < F.cols(); ++i) {
		for (int j = 0; j < 4; ++j) {
			int v0 = F(j, i);
			int v1 = F((j+1)%4,i);
			int v2 = F((j+3)%4,i);
			Vector3d d1 = V.col(v1) - V.col(v0);
			Vector3d d2 = V.col(v2) - V.col(v0);
			d1.normalize();
			d2.normalize();
			double angle = 180.0/3.141592654*atan2(d1.cross(d2).norm(), d1.dot(d2));
			e += (angle-90)*(angle-90);
			max_angle = std::max(angle, max_angle);
			min_angle = std::min(angle, min_angle);
		}
	}
	printf("min max angle: %lf %lf\n", min_angle, max_angle);
	printf("angle average error: %lf\n", sqrt(e / 4 / F.cols()));
	std::vector<double> len;
	double sum_area = 0;
	for (int i = 0; i < F.cols(); ++i) {
		Eigen::Vector3d a1 = V.col(F(1, i)) - V.col(F(0, i));
		Eigen::Vector3d a2 = V.col(F(3, i)) - V.col(F(0, i));
		Eigen::Vector3d a3 = V.col(F(1, i)) - V.col(F(2, i));
		Eigen::Vector3d a4 = V.col(F(3, i)) - V.col(F(2, i));
		double t1 = a1.cross(a2).norm();
		double t2 = a3.cross(a4).norm();
		len.push_back(t1 + t2);
		sum_area += t1 + t2;
	}
	double med_area = sum_area / F.cols();
	for (int i = 0; i < len.size(); ++i) {
		len[i] = (len[i] - med_area) / med_area;
	}
	double t = 0;
	for (int i = 0; i < len.size(); ++i) {
		t += len[i] * len[i];
	}
	printf("area %lf\n", sqrt(t / len.size()));
}

void Analyze()
{
	ReportFaceInfo();
	ReportManifold();
	ReportSingularity();
	ReportAngleDifference();
}

int main(int argc, char** argv) {
	if (argc < 2) {
		printf("./analyzer input.obj [scale] [output.txt]\n");
		return 0;
	}

	Load(argv[1]);

	int scale = 1;
	if (argc >= 3) {
		sscanf(argv[2], "%d", &scale);
	}
	for (int i = 1; i < scale; ++i) {
		Upsampling();
	}
	if (argc >= 4) {
		freopen(argv[3],"w",stdout);
	}

	Analyze();

	fclose(stdout);
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <iostream>

#include <armadillo>
#include <tuple>

#include "sys.h"
#include "grid.h"
#include "vtk_functions.h"

using namespace std;

typedef struct {
    double minMag;
    double maxMag;
    double range;

} SYS_Image;

// void draw_image(System *sys, Grid *grid) {
//     std::cout << "Started drawing image" << endl;

//     int w = sys->NGrid;
//     int h = sys->NGrid;

//     FILE *f;
//     unsigned char *img = NULL;
//     int filesize = 54 + 3 * w * h;
//     //w is your image width, h is image height, both int

//     img = (unsigned char *) malloc(3 * w * h);
//     memset(img, 0, sizeof(&img));

//     for (int i = 0; i < sys->NGrid; i++) {
//         for (int j = 0; j < sys->NGrid; j++) {
//             int x = i;
//             int y = j;

//             double r = sys->theImage[i][j].x;
//             double g = sys->theImage[i][j].y;
//             double b = sys->theImage[i][j].z;

//             if (r > 255) r = 255;
//             if (g > 255) g = 255;
//             if (b > 255) b = 255;

//             img[(x + y * w) * 3 + 2] = (unsigned char) (r);
//             img[(x + y * w) * 3 + 1] = (unsigned char) (g);
//             img[(x + y * w) * 3 + 0] = (unsigned char) (b);
//         }
//     }

//     unsigned char bmpfileheader[14] = {'B', 'M', 0, 0, 0, 0, 0, 0, 0, 0, 54, 0, 0, 0};
//     unsigned char bmpinfoheader[40] = {40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 24, 0};
//     unsigned char bmppad[3] = {0, 0, 0};

//     bmpfileheader[2] = (unsigned char) (filesize);
//     bmpfileheader[3] = (unsigned char) (filesize >> 8);
//     bmpfileheader[4] = (unsigned char) (filesize >> 16);
//     bmpfileheader[5] = (unsigned char) (filesize >> 24);

//     bmpinfoheader[4] = (unsigned char) (w);
//     bmpinfoheader[5] = (unsigned char) (w >> 8);
//     bmpinfoheader[6] = (unsigned char) (w >> 16);
//     bmpinfoheader[7] = (unsigned char) (w >> 24);
//     bmpinfoheader[8] = (unsigned char) (h);
//     bmpinfoheader[9] = (unsigned char) (h >> 8);
//     bmpinfoheader[10] = (unsigned char) (h >> 16);
//     bmpinfoheader[11] = (unsigned char) (h >> 24);

//     f = fopen("magix.bmp", "wb");

//     fwrite(bmpfileheader, 1, 14, f);
//     fwrite(bmpinfoheader, 1, 40, f);

//     for (int i = 0; i < h; i++) {
//         fwrite(img + (w * (h - i - 1) * 3), 3, w, f);
//         fwrite(bmppad, 1, (4 - (w * 3) % 4) % 4, f);
//     }

//     fclose(f);

//     std::cout << "Image drawn" << endl;
// }

// void set_min_max(System *sys, Grid *grid) {
//     int i, j;

//     // sys->theImage = (Node **) calloc(sys->NGrid, sizeof(Node *));

//     // for (i = 0; i < sys->NGrid; i++)
//     //     sys->theImage[i] = (Node *) calloc(sys->NGrid, sizeof(Node));

//     double *magSorted = (double *) malloc(sizeof(double) * sys->NGrid * sys->NGrid);

//     for (j = 0; j < sys->NGrid; j++) {
//         for (i = 0; i < sys->NGrid; i++) {
//             double mag = sqrtf(sys->BField[j][i].x * sys->BField[j][i].x + sys->BField[j][i].y * sys->BField[j][i].y + sys->BField[j][i].z * sys->BField[j][i].z);

//             if (mag > 0)
//                 magSorted[j * sys->NGrid + i] = log(mag);
//         }
//     }

//     mergesort(magSorted, sys->NGrid * sys->NGrid);

//     sys_image->minMag = DBL_MAX;
//     sys_image->maxMag = -DBL_MAX;

//     for (j = 0; j < sys->NGrid; j++) {
//         for (i = 0; i < sys->NGrid; i++) {
//             if (magSorted[j * sys->NGrid + i] != DBL_MAX) {
//                 if (sys_image->minMag > magSorted[j * sys->NGrid + i])
//                     sys_image->minMag = magSorted[j * sys->NGrid + i];
//                 if (sys_image->maxMag < magSorted[j * sys->NGrid + i])
//                     sys_image->maxMag = magSorted[j * sys->NGrid + i];
//             }
//         }
//     }

//     sys_image->range = sys_image->maxMag - sys_image->minMag;

//     if (sys->debug == 1) {
//       printf("\nminMag: %E\n", sys_image->minMag);
//       printf("maxMag: %E\n", sys_image->maxMag);
//       printf("minMag non log: %E\n", exp(sys_image->minMag));
//       printf("maxMag non log: %E\n", exp(sys_image->maxMag));
//     }
// }

void write_bmp(System *sys, Grid *grid) {
    double red = 0;
    double green = 0;
    double blue = 0;

    Node *x_box = (Node *) calloc(1, sizeof(Node));
    Node *y_box = (Node *) calloc(1, sizeof(Node));
    Node *point = (Node *) calloc(1, sizeof(Node));

    arma::field<arma::rowvec> image(sys->NGrid, sys->NGrid);
    arma::rowvec pixel(3);

    tuple <double, double, double> geek;

    for (int i = 0; i < sys->NGrid; i++) {
        for (int j = 0; j < sys->NGrid; j++) {
            double lic_val = grid->Hx(j,i) / 255;
            double val = 0;

            // if (sys->pImage[j][i].y > 0)
            //     val = ((get_dec_place(sys->pImage[j][i].y, 20)) - sys_image->minMag) / sys_image->range;

            if (val < 0.25) {
                red = 0;
                green = 255 * (val / 0.25);
                blue = 255;
            } else if ((val >= 0.25) && (val < 0.5)) {
                red = 0;
                green = 255;
                blue = 255 - 255 * (val - 0.25) / 0.25;
            } else if ((val >= 0.5) && (val < 0.75)) {
                red = 255 * (val - 0.5) / 0.25;
                green = 255;
                blue = 0;
            } else if ((val >= 0.75) && (val < 1.00)) {
                red = 255;
                green = 255 - 255 * (val - 0.75) / 0.25;
                blue = 0;
            } else {
                red = 255;
                green = 255 - 255 * (val - 0.75) / 0.25;
                blue = 0;
            }

            pixel(0) = red * grid->Hy(j,i) * lic_val;
            pixel(1) = green * grid->Hy(j,i) * lic_val;
            pixel(2) = blue * grid->Hy(j,i) * lic_val;

            image(i, j) = pixel;

            // if (sys->set_vtk == 1) {
            //     pixel(0) = red * grid->Hy(j,i) * lic_val;
            //     pixel(1) = green * grid->Hy(j,i) * lic_val;
            //     pixel(2) = blue * grid->Hy(j,i) * lic_val;

            //     image(i, j) = pixel;
            // } else {
            //     pixel(0) = red * lic_val;
            //     pixel(1) = green * lic_val;
            //     pixel(2) = blue * lic_val;

            //     image(i, j) = pixel;
            // }

            // if (sys->set_vtk == 1) {
            //     // sys->theImage[i][sys->NGrid - j].x = red * sys->pImage[j][i].y * lic_val;
            //     // sys->theImage[i][sys->NGrid - j].y = green * sys->pImage[j][i].y * lic_val;
            //     // sys->theImage[i][sys->NGrid - j].z = blue * sys->pImage[j][i].y * lic_val;
            // } else {
            //     // sys->theImages[i][sys->NGrid - j].x = red * lic_val;
            //     // sys->theImage[i][sys->NGrid - j].y = green * lic_val;
            //     // sys->theImage[i][sys->NGrid - j].z = blue * lic_val;
            // }

            // for (k = 0; k < (int) p.size(); k++) {
            //     sort(p, x_box, 0, k, (int) p[k].size());
            //     sort(p, y_box, 1, k, (int) p[k].size());

            //     point->x = obs_grid(i)(1); // y-coord
            //     point->y = obs_grid(j)(2); // z-coord

            //     if (point_in_box(point, x_box, y_box)) {
            //         sys->theImage[i][sys->NGrid - j].x = 0;
            //         sys->theImage[i][sys->NGrid - j].y = 0;
            //         sys->theImage[i][sys->NGrid - j].z = 0;
            //     }
            // }
        }
    }

    // image.save("image.field");

    // if (sys->set_vtk == 1)
    //     vtk_mag_field(sys, image, obs_grid);
    // else
    //     draw_image(sys);
}


#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/tuple/tuple.hpp>

typedef  boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
  boost::property<boost::edge_capacity_t, long,
    boost::property<boost::edge_residual_capacity_t, long,
      boost::property<boost::edge_reverse_t, traits::edge_descriptor> > > >  graph;
typedef  boost::graph_traits<graph>::edge_descriptor      edge_desc;
typedef  boost::graph_traits<graph>::out_edge_iterator      out_edge_it;

class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

// Modeling it as a flow problem.
// The source connects to all vertices with a positive balance, all other vertices connect to the sink.
// Since an unions total value needs to be strictly greater than zero, we know there needs to be at
// least one province that does not need its full balance to support it. I no such province exists,
// there can not be an union. If there is a province that does not need its full balance, the flow is
// less than the sum of positive balances
void solve() {
  int n, m;
  cin >> n >> m;

  // Set up the graph
  graph G(n);
  edge_adder adder(G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);

  auto source = boost::add_vertex(G);
  auto target = boost::add_vertex(G);

  // Connect all provinces to source/target depending on balance
  int sumPositive = 0;
  int b;
  for (int i = 0; i < n; ++i) {
    cin >> b;
    if (b > 0) {
      sumPositive += b;
      adder.add_edge(source, i, b);
    }
    else {
      adder.add_edge(i, target, -b);
    }
  }

  // Connect provinces
  int u, v, d;
  for (int i = 0; i < m; ++i) {
    cin >> u >> v >> d;
    adder.add_edge(u, v, d);
  }

  // Compute flow & output
  int flow = boost::push_relabel_max_flow(G, source, target);
  cout << (flow < sumPositive ? "yes" : "no") << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
  return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/core/exceptions.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/gui/CDisplayWindowGUI.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/Lie/SO.h>

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

static void AppRotationConverter()
{
	nanogui::init();

	// Create main window:
	mrpt::gui::CDisplayWindowGUI_Params cp;
	// cp.fullscreen = true;

	// Input variables (they are bound to the GUI controls):
	mrpt::math::CQuaternionDouble in_quat;
	mrpt::math::CMatrixFixed<double, 3, 3> in_rot;
	in_rot.setIdentity();
	std::array<double, 3> in_ypr = {0, 0, 0};
	mrpt::math::TVector3D in_axisangle_ax(0, 0, 0);
	double in_axisangle_ang = 0;
	auto in_lie_log = mrpt::math::CVectorFixed<double, 3>::Zero();
	bool units_radians = true;

	mrpt::opengl::CSetOfObjects::Ptr gl_corner_user =
		mrpt::opengl::stock_objects::CornerXYZ(1.0f);
	mrpt::opengl::CAxis::Ptr gl_corner_reference = mrpt::opengl::CAxis::Create(
		-1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.2f, 1.0f, true);
	gl_corner_reference->setTextScale(0.04);

	// In/out UI control declarations (declared here so we can use them in the
	// lambda below):
	nanogui::TabWidget* tabWidget = nullptr;
	nanogui::TextBox* ed_in_rot[3][3];
	nanogui::TextBox *edOutMatrix = nullptr, *edOutQuat = nullptr,
					 *edOutAxisAngle_Ax = nullptr, *edOutAxisAngle_An = nullptr,
					 *edOutLogSO3 = nullptr;
	nanogui::TextBox* ed_out_rot[3][3];
	nanogui::Slider* sl_in_ypr[3] = {nullptr, nullptr, nullptr};

	// The main function: update all calculations:
	auto lambdaRecalcAll = [&]() {
		mrpt::poses::CPose3D userPose;
		switch (tabWidget->activeTab())
		{
			// YPR
			case 0:
			{
				const double K = units_radians ? 1.0 : (M_PI / 180.0);
				userPose.setFromValues(
					0, 0, 0, in_ypr[0] * K, in_ypr[1] * K, in_ypr[2] * K);
			}
			break;
			// Rot matrix:
			case 1:
			{
				// Run a SVD to ensure we take the closest SO(3) matrix to the
				// user input:
				Eigen::Matrix3d M = in_rot.asEigen();
				Eigen::JacobiSVD<Eigen::Matrix3d> svd(
					M, Eigen::ComputeFullU | Eigen::ComputeFullV);

				auto R = mrpt::math::CMatrixDouble33(
					(svd.matrixU() * svd.matrixV().transpose()).eval());
				userPose.setRotationMatrix(R);
				for (int r = 0; r < 3; r++)
					for (int c = 0; c < 3; c++)
						ed_in_rot[r][c]->setValue(
							mrpt::format("%.05f", R(r, c)));
			}
			break;
			// Quaternion
			case 2:
			{
				userPose = mrpt::poses::CPose3D(in_quat, 0, 0, 0);
			}
			break;
			// axis+angle
			case 3:
			{
				const double K = units_radians ? 1.0 : (M_PI / 180.0);
				mrpt::math::TVector3D v =
					in_axisangle_ax * (in_axisangle_ang * K);
				mrpt::math::CVectorFixed<double, 3> vn;
				for (int i = 0; i < 3; i++)
					vn[i] = v[i];

				userPose.setRotationMatrix(mrpt::poses::Lie::SO<3>::exp(vn));
			}
			break;
			// log(SO(3))
			case 4:
			{
				userPose.setRotationMatrix(
					mrpt::poses::Lie::SO<3>::exp(in_lie_log));
			}
			break;
		};

		// Set 3D view corner:
		gl_corner_user->setPose(userPose);

		// Update text output:
		const mrpt::math::CMatrixDouble33 Rout = userPose.getRotationMatrix();

		// matrix:
		edOutMatrix->setValue(Rout.inMatlabFormat());
		for (int r = 0; r < 3; r++)
			for (int c = 0; c < 3; c++)
				ed_out_rot[r][c]->setValue(mrpt::format("%.05f", Rout(r, c)));

		// quat:
		mrpt::math::CQuaternionDouble q;
		userPose.getAsQuaternion(q);
		edOutQuat->setValue(q.asString());

		// SO(3) log:
		const auto log_R = mrpt::poses::Lie::SO<3>::log(Rout);
		edOutLogSO3->setValue(
			mrpt::format("[%.05f %.05f %.05f]", log_R[0], log_R[1], log_R[2]));

		// axis-angle:
		mrpt::math::TVector3D axis(log_R[0], log_R[1], log_R[2]);
		if (axis.norm() > 1e-20) axis = axis.unitarize();

		edOutAxisAngle_Ax->setValue(
			mrpt::format("[%.05f %.05f %.05f]", axis[0], axis[1], axis[2]));
		edOutAxisAngle_An->setValue(mrpt::format(
			"%.05f %s", log_R.norm() * (units_radians ? 1.0 : 180.0 / M_PI),
			(units_radians ? "rad" : "deg")));
	};

	// Create GUI:
	mrpt::gui::CDisplayWindowGUI win("3D rotation converter", 900, 700, cp);

	// Add INPUT window:
	// -----------------------------
	nanogui::Window* winInput = new nanogui::Window(&win, "Rotation input");
	winInput->setPosition(nanogui::Vector2i(10, 50));
	winInput->setLayout(new nanogui::GroupLayout());
	winInput->setFixedWidth(350);

	tabWidget = winInput->add<nanogui::TabWidget>();

	{  // Yaw/pitch/roll  tab:
		nanogui::Widget* layer = tabWidget->createTab("Yaw-pitch-roll");
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		const char* lb[3] = {"yaw (Z)=", "pitch (Y)=", "roll (x)="};

		for (int i = 0; i < 3; i++)
		{
			layer->add<nanogui::Label>(lb[i]);
			nanogui::TextBox* ed =
				layer->add<nanogui::TextBox>(mrpt::format("%.2f", in_quat[i]));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([&in_ypr, i, lambdaRecalcAll, &sl_in_ypr,
							 &units_radians](const std::string& s) {
				in_ypr[i] = std::stod(s);
				sl_in_ypr[i]->setValue(
					in_ypr[i] / (units_radians ? M_PIf : 180.0f));
				lambdaRecalcAll();
				return true;
			});
			nanogui::Slider* sl = layer->add<nanogui::Slider>();
			sl_in_ypr[i] = sl;
			sl->setRange({-1.0f, 1.0f});
			sl->setCallback([&, i, ed](float val) {
				val *= units_radians ? M_PIf : 180.0f;
				if (i == 1) val *= 0.5f;  // Pitch
				ed->setValue(mrpt::format("%.03f", val));
				ed->callback()(ed->value());
			});
		}
	}

	{  // rotation matrix tab:
		nanogui::Widget* layer = tabWidget->createTab("SO(3) matrix");

		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		for (int r = 0; r < 3; r++)
		{
			for (int c = 0; c < 3; c++)
			{
				nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
					mrpt::format("%.04f", in_rot(r, c)));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([&in_rot, r, c](const std::string& s) {
					in_rot(r, c) = std::stod(s);
					return true;
				});
				ed_in_rot[r][c] = ed;
			}
		}
	}

	{  // Quaternion tab:
		nanogui::Widget* layer = tabWidget->createTab("Quaternion");
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		const char* lb[4] = {"w (real)=", "x=", "y=", "z="};

		for (int i = 0; i < 4; i++)
		{
			layer->add<nanogui::Label>(lb[i]);

			nanogui::TextBox* ed =
				layer->add<nanogui::TextBox>(mrpt::format("%.2f", in_quat[i]));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([&in_quat, i](const std::string& s) {
				in_quat[i] = std::stod(s);
				return true;
			});
		}
	}

	{  // Axis-angle tab:
		nanogui::Widget* layer = tabWidget->createTab("Axis-angle");
		layer->setLayout(new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5,
			0));
		layer->add<nanogui::Label>("Axis:");

		{
			nanogui::Widget* panel = layer->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));

			for (int r = 0; r < 3; r++)
			{
				nanogui::TextBox* ed = panel->add<nanogui::TextBox>(
					mrpt::format("%.04f", in_axisangle_ax[r]));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([&in_axisangle_ax, r](const std::string& s) {
					in_axisangle_ax[r] = std::stod(s);
					return true;
				});
			}
		}
		{
			layer->add<nanogui::Label>("Angle:");
			nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
				mrpt::format("%.04f", in_axisangle_ang));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([&in_axisangle_ang](const std::string& s) {
				in_axisangle_ang = std::stod(s);
				return true;
			});
		}
	}

	{  // axis with magnitude = log(R) in Lie group
		nanogui::Widget* layer = tabWidget->createTab("log(SO(3))");

		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		layer->add<nanogui::Label>("Axis with magnitude", "sans-bold");
		layer->add<nanogui::Label>("(i.e. vee(log(R)) in SO(3))");
		{
			nanogui::Widget* panel = layer->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));

			for (int r = 0; r < 3; r++)
			{
				nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
					mrpt::format("%.04f", in_lie_log[r]));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([&in_lie_log, r](const std::string& s) {
					in_lie_log[r] = std::stod(s);
					return true;
				});
			}
		}
	}

	tabWidget->setActiveTab(0);

	// Apply button:
	winInput->add<nanogui::Button>("Apply", ENTYPO_ICON_CALCULATOR)
		->setCallback(lambdaRecalcAll);

	// Add top menu subwindow:
	// -----------------------------
	{
		nanogui::ref<nanogui::Window> winMenu = new nanogui::Window(&win, "");
		winMenu->setPosition(nanogui::Vector2i(0, 0));
		winMenu->setLayout(new nanogui::BoxLayout(
			nanogui::Orientation::Horizontal, nanogui::Alignment::Middle, 5));
		nanogui::Theme* modTheme =
			new nanogui::Theme(win.screen()->nvgContext());
		modTheme->mWindowHeaderHeight = 1;
		winMenu->setTheme(modTheme);

		winMenu->add<nanogui::Button>("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
			->setCallback([&win]() { win.setVisible(false); });

		winMenu->add<nanogui::Label>("      ");	 // separator

		winMenu
			->add<nanogui::CheckBox>(
				"Show reference frame",
				[&](bool b) { gl_corner_reference->setVisibility(b); })
			->setChecked(true);

		winMenu
			->add<nanogui::CheckBox>(
				"Show rotated frame",
				[&](bool b) { gl_corner_user->setVisibility(b); })
			->setChecked(true);
		winMenu
			->add<nanogui::CheckBox>(
				"Ortho. view",
				[&](bool b) { win.camera().setCameraProjective(!b); })
			->setChecked(false);

		winMenu->add<nanogui::Label>("Units:");
		winMenu
			->add<nanogui::ComboBox>(
				std::vector<std::string>({"radians", "degrees"}))
			->setCallback([&](int index) {
				// On units update:
				units_radians = (index == 0);
				lambdaRecalcAll();
			});
	}

	// Add OUTPUT window:
	// -----------------------------
	{
		nanogui::Window* winOutput =
			new nanogui::Window(&win, "Rotation output");
		winOutput->setPosition(nanogui::Vector2i(10, 320));
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5,
			0);
		winOutput->setLayout(layout);
		winOutput->setFixedWidth(350);

		winOutput->add<nanogui::Label>("SO(3) rotation matrix", "sans-bold");
		{
			nanogui::Widget* panel = winOutput->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));
			for (int r = 0; r < 3; r++)
				for (int c = 0; c < 3; c++)
					ed_out_rot[r][c] = panel->add<nanogui::TextBox>("");
		}

		winOutput->add<nanogui::Label>("In MATLAB-like notation:");
		edOutMatrix = winOutput->add<nanogui::TextBox>("");
		edOutMatrix->setEditable(true);

		winOutput->add<nanogui::Label>("Quaternion (r,x,y,z)", "sans-bold");
		edOutQuat = winOutput->add<nanogui::TextBox>("");
		edOutQuat->setEditable(true);

		winOutput->add<nanogui::Label>("Axis-angle (r,x,y,z)", "sans-bold");
		edOutAxisAngle_Ax = winOutput->add<nanogui::TextBox>("");
		edOutAxisAngle_Ax->setEditable(true);
		edOutAxisAngle_An = winOutput->add<nanogui::TextBox>("");
		edOutAxisAngle_An->setEditable(true);

		winOutput->add<nanogui::Label>("Axis with angle (log(R))", "sans-bold");
		edOutLogSO3 = winOutput->add<nanogui::TextBox>("");
		edOutLogSO3->setEditable(true);
	}

	// Add a background scene:
	// -----------------------------
	{
		auto scene = mrpt::opengl::COpenGLScene::Create();
		scene->insert(mrpt::opengl::CGridPlaneXY::Create());

		scene->insert(gl_corner_user);
		scene->insert(gl_corner_reference);

		auto lck = mrpt::lockHelper(win.background_scene_mtx);
		win.background_scene = std::move(scene);
	}

	win.performLayout();

	win.camera().setZoomDistance(5.0f);

	// Update view and process events:
	win.drawAll();
	win.setVisible(true);
	nanogui::mainloop();

	nanogui::shutdown();
}

int main()
{
	try
	{
		AppRotationConverter();
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
}


/* boost random/cauchy_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP
#define BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <istream>
#include <boost/limits.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
namespace random {

// Cauchy distribution:

/**
 * The cauchy distribution is a continuous distribution with two
 * parameters, median and sigma.
 *
 * It has \f$\displaystyle p(x) = \frac{\sigma}{\pi(\sigma^2 + (x-m)^2)}\f$
 */
template<class RealType = double>
class cauchy_distribution
{
public:
    typedef RealType input_type;
    typedef RealType result_type;

    class param_type
    {
    public:

        typedef cauchy_distribution distribution_type;

        /** Constructs the parameters of the cauchy distribution. */
        explicit param_type(RealType median_arg = RealType(0.0),
                            RealType sigma_arg = RealType(1.0))
          : _median(median_arg), _sigma(sigma_arg) {}

        // backwards compatibility for Boost.Random

        /** Returns the median of the distribution. */
        RealType median() const { return _median; }
        /** Returns the sigma parameter of the distribution. */
        RealType sigma() const { return _sigma; }

        // The new names in C++0x.

        /** Returns the median of the distribution. */
        RealType a() const { return _median; }
        /** Returns the sigma parameter of the distribution. */
        RealType b() const { return _sigma; }

        /** Writes the parameters to a std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._median << " " << parm._sigma;
            return os;
        }

        /** Reads the parameters from a std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._median >> std::ws >> parm._sigma;
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._median == rhs._median && lhs._sigma == rhs._sigma; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _median;
        RealType _sigma;
    };

    /**
     * Constructs a \cauchy_distribution with the paramters @c median
     * and @c sigma.
     */
    explicit cauchy_distribution(RealType median_arg = RealType(0.0),
                                 RealType sigma_arg = RealType(1.0))
      : _median(median_arg), _sigma(sigma_arg) { }

    /**
     * Constructs a \cauchy_distribution from it's parameters.
     */
    explicit cauchy_distribution(const param_type& parm)
      : _median(parm.median()), _sigma(parm.sigma()) { }

    // compiler-generated copy ctor and assignment operator are fine

    // backwards compatibility for Boost.Random

    /** Returns: the "median" parameter of the distribution */
    RealType median() const { return _median; }
    /** Returns: the "sigma" parameter of the distribution */
    RealType sigma() const { return _sigma; }

    // The new names in C++0x

    /** Returns: the "median" parameter of the distribution */
    RealType a() const { return _median; }
    /** Returns: the "sigma" parameter of the distribution */
    RealType b() const { return _sigma; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return -(std::numeric_limits<RealType>::infinity)(); }

    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return (std::numeric_limits<RealType>::infinity)(); }

    param_type param() const { return param_type(_median, _sigma); }

    void param(const param_type& parm)
    {
        _median = parm.median();
        _sigma = parm.sigma();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /**
     * Returns: A random variate distributed according to the
     * cauchy distribution.
     */
    template<class Engine>
    result_type operator()(Engine& eng)
    {
        // Can we have a boost::mathconst please?
        const result_type pi = result_type(3.14159265358979323846);
        using std::tan;
        RealType val = uniform_01<RealType>()(eng)-result_type(0.5);
        return _median + _sigma * tan(pi*val);
    }

    /**
     * Returns: A random variate distributed according to the
     * cauchy distribution with parameters specified by param.
     */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm)
    {
        return cauchy_distribution(parm)(eng);
    }

    /**
     * Writes the distribution to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, cauchy_distribution, cd)
    {
        os << cd._median << " " << cd._sigma;
        return os;
    }

    /**
     * Reads the distribution from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, cauchy_distribution, cd)
    {
        is >> cd._median >> std::ws >> cd._sigma;
        return is;
    }

    /**
     * Returns true if the two distributions will produce
     * identical sequences of values, given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(cauchy_distribution, lhs, rhs)
    { return lhs._median == rhs._median && lhs._sigma == rhs._sigma; }

    /**
     * Returns true if the two distributions may produce
     * different sequences of values, given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(cauchy_distribution)

private:
    RealType _median;
    RealType _sigma;
};

} // namespace random

using random::cauchy_distribution;

} // namespace boost

#endif // BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP


#include <gtest/gtest.h>

#include "mfem.hpp"
using namespace mfem;

#include <iostream>
#include <fstream>
#include <chrono>
#include <random>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "../src/core/config.hpp"
#include "../src/mymfem/utilities.hpp"

using namespace mymfem;

/** @brief Unit test for point locator algorithm
 * Test the point location algorithm implemented in class PointLocator
 * for meshes with local refinement, generated by _serial_ code
 */
TEST(MfemUtil, pointLocator1)
{
    std::string input_dir
            = "../tests/input/gammaShapedBr/";

    int lx1 = 1;
    const std::string mesh_file1
            = input_dir+"mesh_lx"+std::to_string(lx1);

    int lx2 = 6;
    const std::string mesh_file2
            = input_dir+"mesh_lx"+std::to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    const IntegrationRule *ir = nullptr;
    ir = &IntRules.Get(2, 3);

    std::random_device rd;     // initialise (seed) engine
    std::mt19937 rng(rd());    // random-number engine used
    std::uniform_int_distribution<int> uni(0,mesh1.GetNE()-1);

    auto elId1 = uni(rng);
    //std::cout << "Generate in element:" << elId1 << std::endl;
    //elId1 = 3;
    ElementTransformation *trans1 = mesh1.GetElementTransformation(elId1);
    DenseMatrix true_x(mesh1.Dimension(), ir->GetNPoints());
    Vector xk;
    for (int k=0; k < true_x.NumCols(); k++) {
        true_x.GetColumnReference(k,xk);
        trans1->Transform(ir->IntPoint(k), xk);
    }

    Array <int> elIds1(true_x.NumCols());
    Array <IntegrationPoint> ips1(true_x.NumCols());

    Array <int> elIds2(true_x.NumCols());
    Array <IntegrationPoint> ips2(true_x.NumCols());

    auto start1 = std::chrono::high_resolution_clock::now();
    mesh2.FindPoints(true_x, elIds1, ips1);
    auto end1 = std::chrono::high_resolution_clock::now();

    auto start2 = std::chrono::high_resolution_clock::now();
    PointLocator point_locator(&mesh2);
    int init_elId = 0;
    for (int k=0; k < true_x.NumCols(); k++) {
        Vector xk;
        true_x.GetColumn(k, xk);
        std::tie (elIds2[k],ips2[k]) = point_locator(xk, init_elId);
        init_elId = elIds2[k];
    }
    auto end2 = std::chrono::high_resolution_clock::now();

    ElementTransformation *trans2 = nullptr;
    Vector x1, x2, x;
    double TOL = 1E-8;
    for (int k=0; k < true_x.NumCols(); k++)
    {
        trans2 = mesh2.GetElementTransformation(elIds1[k]);
        trans2->Transform(ips1[k], x1);

        trans2 = mesh2.GetElementTransformation(elIds2[k]);
        trans2->Transform(ips2[k], x2);

        true_x.GetColumn(k,x);

        Vector xmx1(x.Size()), xmx2(x.Size());
        subtract(x, x1, xmx1);
        subtract(x, x2, xmx2);

        ASSERT_LE(xmx1.Norml1(), TOL);
        ASSERT_LE(xmx2.Norml1(), TOL);
    }

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Run times: "
              << "\t slow  " << duration1.count()
              << "\t fast  " << duration2.count() << std::endl;
}

/** @brief Unit test for point locator algorithm
 * Test the point location algorithm implemented in class PointLocator
 * for quasi-uniform meshes, generated by _serial_ code.
 * The shared vertices table is generated, but expected to be empty.
 */
TEST(MfemUtil, pointLocator2)
{
    std::string input_dir
            = "../tests/input/unitSquareQu/";

    int lx1 = 1;
    const std::string mesh_file1 = input_dir+"mesh_lx"+std::to_string(lx1);

    int lx2 = 6;
    const std::string mesh_file2 = input_dir+"mesh_lx"+std::to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    const IntegrationRule *ir = nullptr;
    ir = &IntRules.Get(2, 3);

    std::random_device rd;     // initialise (seed) engine
    std::mt19937 rng(rd());    // random-number engine used
    std::uniform_int_distribution<int> uni(0,mesh1.GetNE()-1);

    auto elId1 = uni(rng);
    ElementTransformation *trans1 = mesh1.GetElementTransformation(elId1);
    DenseMatrix true_x(mesh1.Dimension(), ir->GetNPoints());
    Vector xk;
    for (int k=0; k < true_x.NumCols(); k++) {
        true_x.GetColumnReference(k,xk);
        trans1->Transform(ir->IntPoint(k), xk);
    }

    Array <int> elIds1(true_x.NumCols());
    Array <IntegrationPoint> ips1(true_x.NumCols());

    Array <int> elIds2(true_x.NumCols());
    Array <IntegrationPoint> ips2(true_x.NumCols());

    auto start1 = std::chrono::high_resolution_clock::now();
    mesh2.FindPoints(true_x, elIds1, ips1);
    auto end1 = std::chrono::high_resolution_clock::now();

    bool has_shared_vertices = true;
    auto start2 = std::chrono::high_resolution_clock::now();
    PointLocator point_locator(&mesh2, has_shared_vertices);
    int init_elId = 0;
    std::tie (elIds2, ips2) = point_locator(true_x,
                                            init_elId);
    auto end2 = std::chrono::high_resolution_clock::now();

    ElementTransformation *trans2 = nullptr;
    Vector x1, x2, x;
    double TOL = 1E-8;
    for (int k=0; k < true_x.NumCols(); k++)
    {
        trans2 = mesh2.GetElementTransformation(elIds1[k]);
        trans2->Transform(ips1[k], x1);

        trans2 = mesh2.GetElementTransformation(elIds2[k]);
        trans2->Transform(ips2[k], x2);

        true_x.GetColumn(k,x);

        Vector xmx1(x.Size()), xmx2(x.Size());
        subtract(x, x1, xmx1);
        subtract(x, x2, xmx2);

        ASSERT_LE(xmx1.Norml1(), TOL);
        ASSERT_LE(xmx2.Norml1(), TOL);
    }

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Run times: "
              << "\t slow  " << duration1.count()
              << "\t fast  " << duration2.count() << std::endl;
}


#pragma once

// -*- coding: utf-8 -*-
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <py2cpp/nx2bgl.hpp>
#include <utility>  // for std::pair

using graph_t = boost::adjacency_list<
    boost::listS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_weight_t, int, boost::property<boost::edge_index_t, int>>>;
using Vertex = boost::graph_traits<graph_t>::vertex_descriptor;
using Edge_it = boost::graph_traits<graph_t>::edge_iterator;

template <typename Container> inline auto create_test_case1(const Container& weights)
    -> py::grAdaptor<graph_t> {
    using Edge = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes { A, B, C, D, E };
    static Edge edge_array[] = {Edge{A, B}, Edge{B, C}, Edge{C, D}, Edge{D, E}, Edge{E, A}};
    // int weights[] = {-5, 1, 1, 1, 1};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g = graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return py::grAdaptor<graph_t>{std::move(g)};
}

template <typename Container> inline auto create_test_case_timing(const Container& weights)
    -> py::grAdaptor<graph_t> {
    using Edge = std::pair<int, int>;
    constexpr auto num_nodes = 3;
    enum nodes { A, B, C };
    static Edge edge_array[] = {Edge{A, B}, Edge{B, A}, Edge{B, C}, Edge{C, B},
                                Edge{B, C}, Edge{C, B}, Edge{C, A}, Edge{A, C}};
    // int weights[] = {7, 0, 3, 1, 6, 4, 2, 5};
    constexpr int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g = graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return py::grAdaptor<graph_t>{std::move(g)};
}


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

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

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/degree_centrality.hpp>

#include "helper.hpp"

using namespace std;
using namespace boost;

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

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

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

// Declare a container type for degree centralities and its
// corresponding property map.
typedef exterior_vertex_property< Graph, unsigned > CentralityProperty;
typedef CentralityProperty::container_type CentralityContainer;
typedef CentralityProperty::map_type CentralityMap;

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

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

    // Compute the degree centrality for graph.
    CentralityContainer cents(num_vertices(g));
    CentralityMap cm(cents, g);
    all_degree_centralities(g, cm);

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

    return 0;
}
//]


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of cyl_bessel_yn"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;
  TTS_EXPR_IS( eve::cyl_bessel_yn(T(), T())  ,  T);
  TTS_EXPR_IS( eve::cyl_bessel_yn(v_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_yn(i_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_yn(I_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_yn(i_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_yn(I_t(),v_t()), T);
};

//==================================================================================================
//== integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_yn on wide with integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::ramp(0), eve::test::randoms(0.0, 2000.0))
        )
  <typename T>(T n, T a0)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_yn =  [](auto n, auto x) { return eve::cyl_bessel_yn(n, x); };
  auto std__cyl_bessel_yn =  [](auto n, auto x)->v_t { return boost::math::cyl_neumann(n, x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(0, eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
  }
  //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, v_t(1500)), std__cyl_bessel_yn(3, v_t(1500)), 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, v_t(500)), std__cyl_bessel_yn(2, v_t(500)), 3.0);
  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(0, v_t(10)), std__cyl_bessel_yn(0, v_t(10))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(1, v_t(5)),  std__cyl_bessel_yn(1, v_t(5))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, v_t(10)), std__cyl_bessel_yn(2, v_t(10))  , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, v_t(5)),  std__cyl_bessel_yn(3, v_t(5))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(-1, v_t(5)),  std__cyl_bessel_yn(-1, v_t(5))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(-2, v_t(10)), std__cyl_bessel_yn(-2, v_t(10))  , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(-3, v_t(5)),  std__cyl_bessel_yn(-3, v_t(5))   , 35.0);
  //scalar small
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(0, v_t(0.1)), std__cyl_bessel_yn(0, v_t(0.1))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(1, v_t(0.2)),  std__cyl_bessel_yn(1, v_t(0.2))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, v_t(0.1)), std__cyl_bessel_yn(2, v_t(0.1))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, v_t(0.2)),  std__cyl_bessel_yn(3, v_t(0.2))   , 3.0);
  //scalar besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(10, v_t(8)), std__cyl_bessel_yn(10, v_t(8))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(10, v_t(8)),  std__cyl_bessel_yn(10, v_t(8))   , 3.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(0, eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  //simd large x
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, T(1500)),  T(std__cyl_bessel_yn(3, v_t(1500))),  3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, T(500)),   T(std__cyl_bessel_yn(2, v_t(500))),   3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(-3, T(1500)), T(std__cyl_bessel_yn(-3, v_t(1500))), 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(-2, T(500)),  T(std__cyl_bessel_yn(-2, v_t(500))),  3.0);
  //simd forward
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, T(10)),    T(std__cyl_bessel_yn(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, T(5)),     T(std__cyl_bessel_yn(3, v_t(5)))    , 35.0);
  //simd small
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(0, T(0.1)),   T(std__cyl_bessel_yn(0, v_t(0.1)))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(1, T(0.2)),   T(std__cyl_bessel_yn(1, v_t(0.2)))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(2, T(0.1)),   T(std__cyl_bessel_yn(2, v_t(0.1)))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(3, T(0.2)),   T(std__cyl_bessel_yn(3, v_t(0.2)))  , 3.0);
  //simd besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(10, T(8)),   T(std__cyl_bessel_yn(10, v_t(8)))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(10, T(8)),   T(std__cyl_bessel_yn(10, v_t(8)))   , 3.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(0), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3), T(1500)),  T(std__cyl_bessel_yn(3, v_t(1500))),  3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2), T(500)),   T(std__cyl_bessel_yn(2, v_t(500))),   3.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2), T(10)),    T(std__cyl_bessel_yn(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3), T(5)),     T(std__cyl_bessel_yn(3, v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2), T(0.1)),   T(std__cyl_bessel_yn(2, v_t(0.1)))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3), T(0.2)),   T(std__cyl_bessel_yn(3, v_t(0.2)))  , 3.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(10), T(8)),   T(std__cyl_bessel_yn(10, v_t(8)))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(10), T(8)),   T(std__cyl_bessel_yn(10, v_t(8)))   , 3.0);

  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(0), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(2), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(3), T(1500)),  T(std__cyl_bessel_yn(3, v_t(1500))),  3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(2), T(500)),   T(std__cyl_bessel_yn(2, v_t(500))),   3.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(2), T(10)),    T(std__cyl_bessel_yn(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(3), T(5)),     T(std__cyl_bessel_yn(3, v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(2), T(0.1)),   T(std__cyl_bessel_yn(2, v_t(0.1)))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(3), T(0.2)),   T(std__cyl_bessel_yn(3, v_t(0.2)))  , 3.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(10), T(8)),   T(std__cyl_bessel_yn(10, v_t(8)))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(I_t(10), T(8)),   T(std__cyl_bessel_yn(10, v_t(8)))   , 3.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_yn(n, a0),   map(std__cyl_bessel_yn, n, a0)   , 20.0);
  TTS_ULP_EQUAL(map(eve__cyl_bessel_yn, n, a0),   map(std__cyl_bessel_yn, n, a0)   , 20.0);

};

//==================================================================================================
//== non integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_yn on wide with non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
        , eve::test::randoms(0.0, 2000.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_yn =  [](auto n, auto x) { return eve::cyl_bessel_yn(n, x); };
  auto std__cyl_bessel_yn =  [](auto n, auto x)->v_t { return boost::math::cyl_neumann(n, x); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2.5), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3.5), T(1500)),  T(std__cyl_bessel_yn(v_t(3.5), v_t(1500))),  3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2.5), T(500)),   T(std__cyl_bessel_yn(v_t(2.5), v_t(500))),   3.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2.5), T(10)),    T(std__cyl_bessel_yn(v_t(2.5), v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3.5), T(5)),     T(std__cyl_bessel_yn(v_t(3.5), v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(2.5), T(0.1)),   T(std__cyl_bessel_yn(v_t(2.5), v_t(0.1)))  , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(3.5), T(0.2)),   T(std__cyl_bessel_yn(v_t(3.5), v_t(0.2)))  , 2.5);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(10.5), T(8)),   T(std__cyl_bessel_yn(v_t(10.5), v_t(8)))   , 3.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_yn(T(10.5), T(8)),   T(std__cyl_bessel_yn(v_t(10.5), v_t(8)))   , 3.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_yn(n, a0),   map(std__cyl_bessel_yn, n, a0)   , 0.001);
};

EVE_TEST( "Check behavior of cyl_bessel_yn on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_yn =  [](auto n, auto x) { return eve::diff(eve::cyl_bessel_yn)(n, x); };
  auto std__diff_bessel_yn =  [](auto n, auto x)->v_t { return boost::math::cyl_neumann_prime(n, x); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_yn(n, a0),   map(std__diff_bessel_yn, n, a0)   , 1.0e-2);
};


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric
    This function object returns the atan2pi function : atan2 in pi multiples.

    @par Header <boost/simd/function/atan2pi.hpp>

    @par Note

      For every parameters of same floating type `atan2pi(y, x)`
      is similar  to: `atan2(y, x)/Pi`

    @see atan2,  atan2pi

    @par Example:

      @snippet atan2pi.cpp atan2pi

    @par Possible output:

      @snippet atan2pi.txt atan2pi

  **/
  IEEEValue atan2pi(IEEEValue const& y, const IEEEValue& x);
} }
#endif

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

#endif


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

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

using namespace std;

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

    using cblock= mtl::vector<double, dim<2> >;
    using rblock= mtl::vector<double, dim<2>, row_major >;

    using mtype= matrix<rblock, sparse>;
    using vtype= mtl::vector<cblock>;

    mtype A(2, 3);
    {
	mat::inserter<mtype> ins(A);

	ins[0][0] << rblock{1, 3};
	ins[1][1] << rblock{4, 9};
    }
    cout << "A = \n" << A;

    vtype x{cblock{1, 3}, cblock{1, 2}, cblock{9, 3}};
    cout << "x = " << x << endl;

    mtl::vector<double> y( A * x );
    cout << "y= " << y << endl;

    return 0;
}


// permutation_iterator.hpp
//
// Generates all permutations of the {0,...n-1} element universe.
// The generation algorithm used is from the standard library.

#ifndef PERMUTATION_ITERATOR_HPP
#define PERMUTATION_ITERATOR_HPP

#include <cstdint>
#include <numeric>
#include <algorithm>
#include <type_traits>
#include <vector>
#include <cassert>

#include <boost/iterator/iterator_facade.hpp>

template <typename T>
class permutation_iterator
	: public boost::iterator_facade <
	permutation_iterator<T>,
	const std::vector<T>&,
	boost::forward_traversal_tag
	>
{
private:
	static_assert(std::is_integral<T>::value, "T must be integral");

public:
	permutation_iterator() : end_(true), perm_() { }

	explicit permutation_iterator(int n) : end_(false), perm_(n)
	{
		std::iota(perm_.begin(), perm_.end(), 0);
	}

private:
	friend class boost::iterator_core_access;

	void increment()
	{
		end_ = !std::next_permutation(perm_.begin(), perm_.end());
	}

	bool equal(const permutation_iterator& other) const
	{
		return end_ == other.end_;
	}

	const std::vector<T>& dereference() const
	{
		return perm_;
	}

	bool end_;
	std::vector<T> perm_;
};

#endif


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

// Class JPS Manager
#include "ros/ros.h"
// Convex Decomposition includes
#include <decomp_ros_utils/data_ros_utils.h>
#include <decomp_util/ellipsoid_decomp.h>
#include <decomp_util/seed_decomp.h>

#include "read_map.hpp"
#include <jps_basis/data_utils.h>
#include <jps_planner/jps_planner/jps_planner.h>

#include <Eigen/Dense>

#include "utils.hpp"

#include <mutex>

class JPS_Manager
{
public:
  JPS_Manager();

  std::mutex mtx_jps_map_util;  // mutex for map_util_ and planner_ptr_

  std::shared_ptr<JPS::VoxelMapUtil> map_util_;
  std::unique_ptr<JPSPlanner3D> planner_ptr_;

  vec_Vec3f vec_o_;   // Vector that contains the occupied points
  vec_Vec3f vec_uo_;  // Vector that contains the unkown and occupied points

  // JPS
  void updateJPSMap(pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr, Eigen::Vector3d& center);
  vec_Vecf<3> solveJPS3D(Vec3f& start, Vec3f& goal, bool* solved, int i);
  void setNumCells(int cells_x, int cells_y, int cells_z);

  // Convex Decomposition
  void cvxEllipsoidDecomp(vec_Vecf<3>& path, int type_space, std::vector<LinearConstraint3D>& l_constraints,
                          vec_E<Polyhedron<3>>& poly_out);

  void setResolution(double res);
  void setFactorJPS(double factor_jps);
  void setInflationJPS(double inflation_jps);

  void setZGroundAndZMax(double z_ground, double z_max);
  void setVisual(bool visual);
  void setDroneRadius(double inflation_jps);

private:
  double factor_jps_, res_, inflation_jps_, z_ground_, z_max_, drone_radius_;
  int cells_x_, cells_y_, cells_z_;
  bool visual_;
  EllipsoidDecomp3D ellip_decomp_util_;
};


#include "hello.h"

#include <Eigen/Dense>

namespace voltlbx
{

    int add(int i, int j)
    {
        return i + j;
    }

    using Matrix = Eigen::MatrixXd;
    using Vector = Eigen::VectorXd;
    void test_eigen()
    {
        Matrix m1 = Matrix::Zero(2, 2);
        m1(0, 0) = 3.0;
        m1(1, 1) = 50.0;

        Vector v(2);
        v(0) = 1.0;
        v(1) = 1.0;

        auto r = m1 * v;
    }

}


#include <gtest/gtest.h>
#include "../include/model.h"
#include <math.h>
#include "../include/Eigen.h"
#include <vector>
#include <stdexcept>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/operators.hpp>

#include <iostream>

//Testing continuousSolverRK4<double> and continuousModel<double>
//---tests to make sure continuousSolverRK4<T>.solve(double t) does not
//accept values with t < 0 (can't solve back in time)
//---tests to make sure that RK4 method can solve basic heat equation PDE
//correctly according to exact solution while testing that the solved
//state and updated current time are properly stored
TEST(continuousSolverRK4, RK4Test){

	double exponent = -0.1;
	class testModel: public continuousModel<double>{
		public:
			double function(const double & val, const double time) const override{
				double rhs = -0.1*val;
				return rhs;
			}
	};
	testModel instance;

	double initialCondition = 100;
	double initialTime = 0;
	continuousSolverRK4<double> testModelSolver(&instance,initialTime,initialCondition,0.001);

	double times[9] = {0,1,1,1,1,1,1,1,1};
	int length = 9;
	//assert time can't be negative
	double rollingSum = 0;
	for(int i=0; i<length; i++){
		rollingSum+=times[i];
		testModelSolver.solve(times[i]);
		ASSERT_NEAR(rollingSum,testModelSolver.getCurrentTime(),1e-6);
		ASSERT_NEAR(exp(exponent*rollingSum)*initialCondition,testModelSolver.getCurrentState(),1e-6);
		if(i!=length-1){continue;}
	}
	ASSERT_THROW(testModelSolver.solve(-1),std::out_of_range);
}

//CUSTOM VECTOR CLASS
class customVector{
	private:
		std::vector<double> entries;
	public:
		int getSize() const{
			return entries.size();
		}

		double getValueAtIndex(int i) const{
			if(i < 0 || i >= getSize()){
				throw std::out_of_range("");
			}
			return entries[i];
		}
		void setValueAtIndex(int i, double value){
			if(i < 0 || i >= getSize()){
				throw std::out_of_range("");
			}
			entries[i] = value;
		}

		customVector(const std::vector<double> & vec){
			if(vec.size() == 0){
				throw std::out_of_range("");
			}
			entries = vec;
		}

		customVector(int size){
			if(size <= 0){
				throw std::out_of_range("");
			}
			entries = std::vector<double>(size);
		}
};

customVector operator+(const customVector &lhs,const customVector &rhs){
	if(lhs.getSize() != rhs.getSize()){
		throw std::range_error("");
	}
	customVector res(rhs.getSize());
	for(int i = 0; i < lhs.getSize(); i++){
		res.setValueAtIndex(i,lhs.getValueAtIndex(i)+rhs.getValueAtIndex(i));
	}
	return res;
};

customVector operator*(const double &lhs, const customVector &rhs){
	customVector res(rhs.getSize());
	for(int i = 0; i < rhs.getSize(); i++){
		res.setValueAtIndex(i,rhs.getValueAtIndex(i)*lhs);
	}
	return res;
}

TEST(continuousSolverRK4,differentLibraryTest){
	class testModel: public continuousModel<customVector>{
		public:
		customVector function(const customVector & val, const double time) const{
			customVector result(2);
			result.setValueAtIndex(0,-0.1*val.getValueAtIndex(0));
			result.setValueAtIndex(1,-0.2*val.getValueAtIndex(1));
			return result;
		}
	};
	testModel instance;
	std::vector<double> t;
	t.push_back(100);t.push_back(100);
	customVector initialCondition(t);
	double initialTime = 0;
	continuousSolverRK4<customVector> testModelSolver(&instance,initialTime,initialCondition,1);

	class testModel2: public continuousModel<Eigen::VectorXd>{
		private:
			Eigen::MatrixXd modelMatrix;
		public:
			testModel2(){
				Eigen::MatrixXd tmp(2,2);
				tmp<<-0.1,0,0,-0.2;
				modelMatrix = tmp;
			}
			Eigen::VectorXd function(const Eigen::VectorXd & val, const double time) const{
				Eigen::VectorXd gg = modelMatrix*val;
				return modelMatrix*val;
			}
	};

	testModel2 instance2;;
	Eigen::VectorXd initialCondition2(2); initialCondition2<<100,100;
	instance2.function(initialCondition2,0.0);
	continuousSolverRK4<Eigen::VectorXd> testModelSolver2(&instance2,initialTime,initialCondition2,1);

/*
	class testModel3: public continuousModel<boost::numeric::ublas::vector<double>>{
		private:
			boost::numeric::ublas::matrix<double> modelMatrix;
		public:
			testModel3(){
				boost::numeric::ublas::matrix<double> tmp(2,2);
				tmp(0,0) = -0.1;
				tmp(0,1) = 0;
				tmp(1,0) = 0;
				tmp(1,1) = -0.2;
				modelMatrix = tmp;
			}
			boost::numeric::ublas::vector<double>function(boost::numeric::ublas::vector<double> & val,const double time)const{
				//modelMatrix*val;
				return prod(modelMatrix,val);
			}
	};

	testModel3 instance3 = testModel3();
	boost::numeric::ublas::vector<double> initialCondition3(2); initialCondition3(0) = 100; initialCondition3(1) = 100;
	continuousSolverRK4<boost::numeric::ublas::vector<double>> testModelSolver3(&instance3,initialTime,initialCondition3,0.001);
*/

	double times[9] = {0,1,1,1,1,1,1,1,1};
	int length = 9;
	double rollingSum = 0;
	for(int i=0; i<length; i++){
		rollingSum+=times[i];
		testModelSolver.solve(times[i]);
		testModelSolver2.solve(times[i]);
		Eigen::VectorXd tmp2 = testModelSolver2.getCurrentState();
		double tmp3 = tmp2[0];
		double tmp4 = tmp2[1];
		ASSERT_NEAR(testModelSolver.getCurrentState().getValueAtIndex(0),tmp3,1e-9);
		ASSERT_NEAR(testModelSolver.getCurrentState().getValueAtIndex(1),tmp4,1e-9);
	}
	ASSERT_THROW(testModelSolver.solve(-1),std::out_of_range);
}

int main(int argc, char **argv){
	testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}


#include <armadillo>
#include "electronic.hpp"

/*
  From NR 17.1.3 and A&S 25.5.10. For systems of the form $i \hbar
  \dot{c} = H c$, advances c using a 4th order Runge-Kutta integrator
*/
// FIXME: Returns different results than exact propagation
void Electronic::advance_rk4(const arma::cx_mat &H, double dt){
  const std::complex<double> midt(0,-dt);

  k1 = midt * H * amplitudes;
  k2 = midt * H * (amplitudes + (k1 / 2));
  k3 = midt * H * (amplitudes - (k1 / 3) + k2);
  k4 = midt * H * (amplitudes + (k1    ) - k2 + k3);

  amplitudes = amplitudes + (k1 / 6) + (k2 / 3) + (k3 / 3) + (k4 / 6);
}

void Electronic::advance_exact(const arma::cx_mat &H, double dt){
  const std::complex<double> midt(0,-dt);
  amplitudes = arma::expmat(midt * H) * amplitudes;
}

void Electronic::reserve(void){
  auto s = arma::size(amplitudes);
  k1.set_size(s);
  k2.set_size(s);
  k3.set_size(s);
  k4.set_size(s);
}

/*
  Replace U with the closest unitary matrix according to the
  transformation U' = (UU^T)^(-1/2)U.

  For a thorough discussion, see:
  https://en.wikipedia.org/wiki/Kabsch_algorithm
  https://en.wikipedia.org/wiki/Polar_decomposition
*/
void Electronic::unitarize(arma::mat &U){
  // arma::cx_mat R; arma::cx_vec s;
  // arma::eig_gen(s, R, U*U.t());
  // U = arma::real(R * diagmat(arma::pow(s, -0.5)) * R.t()) * U;

  /*
    These algorithms are equivalent but the below has better numerical
    stability properties; namely the above returns 2*I for the
    identity (I) rather than I itself.
  */

  arma::mat W,V; arma::vec s;
  arma::svd(W,s,V,U);
  U = W*V;
}

/*
  Implements Zeyu Zhou et al. JCTC 2020, 16, 835--846

  Approximately minimizes Tr[log(U)^2] via jacobi sweeps while
  enforcing det(U) == 1

  Results agree with the matricies in the paper's Appendix D
*/
void Electronic::phase_match(arma::mat &U, bool do_unitarize){
  // Step 1: det(U) == 1
  if (do_unitarize){
    unitarize(U);
  }
  if (arma::det(U) < 0){
    U.col(0) *= -1.0;
  }

  // Step 2: Jacobi sweeps
  double deljk, Ujj, Ukk;

  const arma::uword N = U.n_rows;
  bool change;
  do{
    change = false;
    for (arma::uword j = 0; j < N; j++){
      for (arma::uword k = j + 1; k < N; k++){
	Ujj = U(j,j);
	Ukk = U(k,k);
	// Eq. 34 with the sum implemented as a dot product
	deljk =
            3*(Ujj*Ujj + Ukk*Ukk)
	  + 6*(U(j,k)*U(k,j))
	  + 8*(Ujj + Ukk)
	  - 3*(arma::as_scalar(U.row(j)*U.col(j) + U.row(k)*U.col(k)));
	if (deljk < 0){
	  U.col(j) *= -1.0;
	  U.col(k) *= -1.0;
          change = true;
	}
      }
    }
  }while(change);
}

void Electronic::phase_match(arma::cx_mat &U, bool do_unitarize){
  (void) U;
  (void) do_unitarize;
  throw std::runtime_error("complex phase matching not implemented");
}


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

TEST(MathFunctions, gamma_p) {
  using stan::math::gamma_p;

  EXPECT_FLOAT_EQ(0.63212055, gamma_p(1.0, 1.0));
  EXPECT_FLOAT_EQ(0.82755178, gamma_p(0.1, 0.1));
  EXPECT_FLOAT_EQ(0.76189667, gamma_p(3.0, 4.0));
  EXPECT_FLOAT_EQ(0.35276812, gamma_p(4.0, 3.0));
  EXPECT_THROW(gamma_p(-4.0, 3.0), std::domain_error);
  EXPECT_THROW(gamma_p(4.0, -3.0), std::domain_error);
}

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

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

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

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


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!
  @ingroup group-euler
    Function object implementing erfcx capabilities

   Computes the  underflow-compensating (scaled) error function:
   \f$\displaystyle e^{x^2}\frac{2}{\sqrt\pi}\int_0^{x} e^{-t^2}\mbox{d}t\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = erfcx(x);
    @endcode

    is similar to:

    @code
    T r = exp(sqr(x))*erfc(x);
    @endcode

    But avoid underflow as much as possible.

    @see erfc, erf

  **/
  const boost::dispatch::functor<tag::erfcx_> erfcx = {};
} }
#endif

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

#endif