// (C) 2014 Arek Olek

#pragma once

#include <chrono>
#include <random>
#include <tuple>

#include <boost/graph/graphml.hpp>

#include "algorithm.hpp"
#include "graph.hpp"

template <class Iterator>
void generate_seeds(Iterator begin, Iterator end, std::string seed) {
  std::seed_seq seq(seed.begin(), seed.end());
  seq.generate(begin, end);
}

double pr_within(double x) {
  return x <= 1
    ?  .5*x*x*x*x - 8./3*x*x*x + M_PI*x*x
    : -.5*x*x*x*x - 4*x*x*atan(sqrt(x*x-1)) + 4./3*(2*x*x+1)*sqrt(x*x-1) + (M_PI-2)*x*x + 1./3;
};

template <class Graph>
class test_suite {
public:
  unsigned size() const { return seeds.size() * degrees.size() * sizes.size(); }

  std::string type() const { return t; }

  std::tuple<Graph, unsigned, double, double> get(unsigned i) const {
    auto run = i % seeds.size();
    std::default_random_engine generator(seeds[run]);
    auto expected_degree = degrees[(i / seeds.size()) % degrees.size()];
    auto n = sizes[i / seeds.size() / degrees.size()];
    auto d = expected_degree;
    auto tree_degree = 2.*(n-1)/n;
    bool mst = found("mst", t);
    bool unite = !found("++", t);
    bool vanilla = !found("+", t);
    Graph G(n), G_shuffled(n);
    double parameter;

    // use d in [0,1] to mean density (since connected graphs have d > 1 anyway)
    if(unite && d <= 1) d *= n-1;

    do {
      G = Graph(n);

      if(found("path", t)) {
        add_spider(G, 1);
        // the overlap satisfies    y     = a    x     + b
        // full graph satisfies:    0     = a  (n-1)   + b
        // tree graph satisfies: 2(n-1)/n = a 2(n-1)/n + b
        // so we must subtract this:
        if(unite) d -= 2./(2.-n) * d + 1. + n/(n-2.);
      }

      if(found("rgg", t)) {
        if(mst && unite) d -= d<2 ? tree_degree : 1/sinh(d-sqrt(2.)); // approximate fit
        parameter = find_argument(d/(n-1), pr_within, 0, sqrt(2.));
        Geometric points(n, generator);
        points.add_random_geometric(G, parameter);
        if(mst) points.add_mst(G);
      }
      else if(found("gnp", t)) {
        if(mst && unite) d -= d<2 ? tree_degree : 1/(2*M_PI*sinh(d-M_PI/sqrt(3.))); // approximate fit
        parameter = d<0 ? 0 : d/(n-1);
        add_edges_uniform(G, parameter, generator, mst);
      }
    } while(vanilla && !is_connected(G));

    copy_edges_shuffled(G, G_shuffled, generator);
    return std::make_tuple(G_shuffled, run, expected_degree, parameter);
  }

  unsigned get_seed(unsigned i) {
    return seeds[i % seeds.size()];
  }

  template<class Sizes, class Degrees>
  test_suite(std::string t, unsigned z, Sizes ns, Degrees ds, std::string seed)
      : t(t), sizes(ns), degrees(ds) {
    seeds.resize(z);
    generate_seeds(seeds.begin(), seeds.end(), seed);
  }
private:
  std::string t;
  std::vector<unsigned> sizes;
  std::vector<double> degrees;
  std::vector<unsigned> seeds;
};

template <class Graph>
class file_suite {
public:
  std::tuple<Graph, unsigned, double, double> get(unsigned i) const {
    auto run = i % seeds.size();
    i /= seeds.size();
    std::default_random_engine generator(seeds[run]);
    Graph g(num_vertices(graphs[i]));
    copy_edges_shuffled(graphs[i], g, generator);
    return std::make_tuple(g, run, 0, 0);
  }

  unsigned get_seed(unsigned i) const {
    return seeds[i % seeds.size()];
  }

  unsigned size() const { return graphs.size() * seeds.size(); }

  file_suite(std::string f, unsigned size, std::string seed)
      : t(f.substr(f.rfind('/')+1, f.rfind('.')-f.rfind('/')-1)),
        seeds(size)  {
    std::ifstream file(f);
    if(!file.good()) {
      throw std::invalid_argument("File does not exist: " + f);
    }
    int z, n, m, s, t;
    file >> z;
    while(z--) {
      file >> n >> m;
      Graph G(n);
      for(int i = 0; i < m; ++i) {
        file >> s >> t;
        add_edge(s, t, G);
      }
      graphs.push_back(G);
    }
    file.close();
    generate_seeds(seeds.begin(), seeds.end(), seed);
  }
  std::string type() const {
    return t;
  }
private:
  std::string t;
  std::vector<Graph> graphs;
  std::vector<unsigned> seeds;
};

template <class Graph>
class real_suite {
public:
  unsigned size() const { return seeds.size(); }

  std::tuple<Graph, unsigned, double, double> get(unsigned i) const {
    std::default_random_engine generator(seeds[i]);
    Graph g(num_vertices(G));
    copy_edges_shuffled(G, g, generator);
    return std::make_tuple(g, i, 0, 0);
  }

  unsigned get_seed(unsigned i) const {
    return seeds[i];
  }

  real_suite(std::string f, unsigned size, std::string seed)
      : G(0),
        t(f.substr(f.rfind('/')+1, f.rfind('.')-f.rfind('/')-1)),
        seeds(size) {
    std::ifstream file(f);
    if (!file.good()) {
      throw std::invalid_argument("File does not exist: " + f);
    }
    boost::dynamic_properties dp;
    read_graphml(file, G, dp);
    generate_seeds(seeds.begin(), seeds.end(), seed);
  }
  std::string type() const {
    return t;
  }
private:
  Graph G;
  std::string t;
  std::vector<unsigned> seeds;
};


/*! \file	ut_rpn_evaluator.cpp
	\brief	RPN evaluator unit test.
	\author	Garth Santor
	\date	2021-10-29
	\copyright	Garth Santor, Trinh Han

=============================================================
Reverse-Polish Evaluator unit test for Expression Evaluator Project.

=============================================================
Revision History
-------------------------------------------------------------

Version 2021.11.01
	C++ 20 validated

Version 2019.11.05
	C++ 17 cleanup

Version 2015.11.05
	Visual Studio 2015
	TEST_xxxx macros

Version 2014.11.21
	Improved round() - reduced float point conversion errors.

Version 2014.10.31
	Visual Studio 2013
	Removed bit operations
	Added multiprecision.

Version 2012.11.16
	Added BitAnd, BitNot, BitOr, BitXor, BitShiftLeft, BitShiftRight

Version 2012.11.15
	Added BinaryInteger, Binary <function>

Version 2012.11.13
	C++ 11 cleanup

Version 2009.12.01
	Alpha release.

=============================================================

Copyright Garth Santor / Trinh Han

The copyright to the computer program(s) herein
is the property of Garth Santor / Trinh Han, Canada.
The program(s) may be used and /or copied only with
the written permission of Garth Santor / Trinh Han
or in accordance with the terms and conditions
stipulated in the agreement / contract under which
the program(s) have been supplied.
============================================================= */

// unit test library
#include <gats/TestApp.hpp>

#include <ee/RPNEvaluator.hpp>
#include <ee/boolean.hpp>
#include <ee/integer.hpp>
#include <ee/function.hpp>
#include <ee/operator.hpp>
#include <ee/real.hpp>
#include <ee/variable.hpp>

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

#include "ut_test_phases.hpp"

#if TEST_REAL
Real::value_type round(Real::value_type x) {
	auto exp = pow(Real::value_type("10.0"), Real::value_type("990"));
	x *= exp;
	x += 0.5;
	x = floor(x);
	x /= exp;
	return x;
}

Real::value_type round(Token::pointer_type const& v) {
	return round(value_of<Real>(v));
}
#endif

GATS_TEST_CASE(no_operand) {
	try {
		auto t = RPNEvaluator().evaluate(TokenList());
		GATS_FAIL("Failed to throw exception");
	}
	catch( std::exception& e ) {
		GATS_CHECK( strcmp( e.what(), "Error: insufficient operands" ) == 0 );
	}
}

#if TEST_INTEGER
	GATS_TEST_CASE(too_many_operand) {
		try {
			TokenList tl = { make<Integer>(3), make<Integer>(4) };
			auto t = RPNEvaluator().evaluate(tl);
			GATS_FAIL("Failed to throw exception");
		}
		catch( std::exception& e ) {
			GATS_CHECK( strcmp( e.what(), "Error: too many operands" ) == 0 );
		}
	}
#endif

#if TEST_VARIABLE
	GATS_TEST_CASE(unitialized_single_variable) {
		auto result = RPNEvaluator().evaluate({ make<Variable>() });

		GATS_CHECK(is<Variable>(result));
		Variable::pointer_type v = convert<Variable>(result);
		GATS_CHECK(v->value() == nullptr);
	}
	GATS_TEST_CASE(unitialized_variable_expression) {
		try {
			auto result = RPNEvaluator().evaluate({ make<Variable>(), make<Negation>() });
			GATS_FAIL("Failed to throw exception");
		}
		catch (std::exception& e) {
			GATS_CHECK(strcmp(e.what(), "Error: variable not initialized") == 0);
		}
	}
#endif

// literals
#if TEST_INTEGER
	GATS_TEST_CASE(Integer_3){
		auto result = RPNEvaluator().evaluate({ make<Integer>(3) });
		GATS_CHECK(value_of<Integer>(result) == Integer::value_type(3));
	}
#endif
#if TEST_REAL
	GATS_TEST_CASE(Real_3_14){
		auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("3.14")) });
		GATS_CHECK(value_of<Real>(result) == Real::value_type("3.14"));
	}
#endif

// Constants
#if TEST_BOOLEAN
	GATS_TEST_CASE(constant_true) {
		auto result = RPNEvaluator().evaluate({ make<True>() });
		GATS_CHECK(value_of<True>(result) == true);
	}
	GATS_TEST_CASE(constant_false) {
		auto result = RPNEvaluator().evaluate({ make<False>() });
		GATS_CHECK(value_of<False>(result) == false);
	}
#endif
#if TEST_REAL
	GATS_TEST_CASE(constant_Pi) {
		auto result = RPNEvaluator().evaluate({ make<Pi>() });
		GATS_CHECK(value_of<Real>(result) == boost::math::constants::pi<Real::value_type>());
	}
	GATS_TEST_CASE(constant_E) {
		auto result = RPNEvaluator().evaluate({ make<E>() });
		GATS_CHECK(value_of<Real>(result) == boost::math::constants::e<Real::value_type>());
	}
#endif

// Identity
#if TEST_UNARY_OPERATOR
	#if TEST_INTEGER
		GATS_TEST_CASE(identity_test_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Identity>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(3));
		}
	#endif
	#if TEST_REAL
		GATS_TEST_CASE(identity_test_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("3")), make<Identity>() });
			GATS_CHECK(value_of<Real>(result) == Real::value_type("3.0"));
		}
	#endif

	// Negation
	#if TEST_INTEGER
		GATS_TEST_CASE(negation_test_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Negation>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(-3));
		}
	#endif
	#if TEST_REAL
		GATS_TEST_CASE(negation_test_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(3), make<Negation>() });
			GATS_CHECK(value_of<Real>(result) == Real::value_type("-3.0"));
		}
	#endif

	// Factorial
	#if TEST_INTEGER
		GATS_TEST_CASE(factorial_test_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(5), make<Factorial>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(120));
		}
	#endif

	// Logical not
	#if TEST_BOOLEAN
		GATS_TEST_CASE(not_test_Boolean) {
			auto result = RPNEvaluator().evaluate({ make<Boolean>(true), make<Not>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<Boolean>(false), make<Not>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
		}
	#endif
#endif // TEST_UNARY_OPERATOR

#if TEST_BINARY_OPERATOR
	#if TEST_INTEGER
		GATS_TEST_CASE(test_multiply_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Integer>(4), make<Multiplication>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(12));
		}
		GATS_TEST_CASE(test_divide_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(45), make<Integer>(3), make<Division>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(15));
		}
		GATS_TEST_CASE(test_addition_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Integer>(4), make<Addition>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(7));
		}
		GATS_TEST_CASE(test_subtraction_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Integer>(4), make<Subtraction>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(-1));
		}
		GATS_TEST_CASE(test_modulus_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(12), make<Integer>(5), make<Modulus>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(2));
		}
		GATS_TEST_CASE(test_power_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Integer>(4), make<Power>() });
			GATS_CHECK(value_of<Integer>(result) == Integer::value_type(81));
		}
		#if TEST_VARIABLE
			GATS_TEST_CASE( assignment_test ) {
				auto result = RPNEvaluator().evaluate({ make<Variable>(), make<Integer>(4), make<Assignment>() });

				GATS_CHECK(is<Variable>(result));
				Variable::pointer_type v = convert<Variable>(result);
				GATS_CHECK( is<Integer>( v->value() ) );
				Integer::pointer_type i = convert<Integer>( v->value() );
				GATS_CHECK( i->value() == 4 );
			}
		#endif
	#endif  // TEST_INTEGER

	#if TEST_REAL
		GATS_TEST_CASE(test_multiply_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(3.0), make<Real>(4.0), make<Multiplication>() });
			GATS_CHECK(value_of<Real>(result) == Real::value_type("12.0"));
		}
		GATS_TEST_CASE(test_divide_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("45.0")), make<Real>(Real::value_type("3.0")), make<Division>() });
			GATS_CHECK(round(result) == round(Real::value_type("15.0")));
		}
		GATS_TEST_CASE(test_addition_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("3.0")), make<Real>(Real::value_type("4.0")), make<Addition>() });
			GATS_CHECK(round(result) == round(Real::value_type("7.0")));
		}
		GATS_TEST_CASE(test_subtraction_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("3.0")), make<Real>(Real::value_type("4.0")), make<Subtraction>() });
			GATS_CHECK(round(result) == round(Real::value_type("-1.0")));
		}
		GATS_TEST_CASE(test_power_Real) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("3.0")), make<Real>(Real::value_type("4.0")), make<Power>() });
			GATS_CHECK(round(result) == round(Real::value_type("81")));
		}
	#endif	// TEST_REAL

	#if TEST_MIXED
		GATS_TEST_CASE(test_multiply_Integer_Real) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Real>(4.0), make<Multiplication>() });
			GATS_CHECK(value_of<Real>(result) == Real::value_type("12.0"));
		}
		GATS_TEST_CASE(test_multiply_Real_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Real>(3.0), make<Integer>(4), make<Multiplication>() });
			GATS_CHECK(value_of<Real>(result) == Real::value_type("12.0"));
		}
		GATS_TEST_CASE(test_divide_Integer_Real) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Real>(6.0), make<Division>() });
			GATS_CHECK(round(result) == round(Real::value_type("0.5")));
		}
		GATS_TEST_CASE(test_divide_Real_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Real>(3.0), make<Integer>(6), make<Division>() });
			GATS_CHECK(round(result) == round(Real::value_type("0.5")));
		}
		GATS_TEST_CASE(test_addition_Integer_Real) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Real>(Real::value_type("4.2")), make<Addition>() });
			GATS_CHECK(round(result) == round(Real::value_type("7.2")));
		}
		GATS_TEST_CASE(test_addition_Real_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("3.3")), make<Integer>(4), make<Addition>() });
			GATS_CHECK(round(result) == round(Real::value_type("7.3")));
		}
		GATS_TEST_CASE(test_subtraction_Integer_Real) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(3), make<Real>(Real::value_type("0.5")), make<Subtraction>() });
			GATS_CHECK(round(result) == round(Real::value_type("2.5")));
		}
		GATS_TEST_CASE(test_subtraction_Real_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("4.25")), make<Integer>(4), make<Subtraction>() });
			GATS_CHECK(round(result) == round(Real::value_type("0.25")));
		}
		GATS_TEST_CASE(test_neg_power_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Integer>(-4), make<Power>() });
			GATS_CHECK(value_of<Real>(result) == Real::value_type("1.0") / Real::value_type("16.0"));
		}
		GATS_TEST_CASE(test_power_Integer_Real) {
			auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("3.0")), make<Power>() });
			GATS_CHECK(round(result) == round(Real::value_type("8.0")));
		}
		GATS_TEST_CASE(test_power_Real_Integer) {
			auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Integer>(3), make<Power>() });
			GATS_CHECK(round(result) == round(Real::value_type("8.0")));
		}

		#if TEST_VARIABLE
			GATS_TEST_CASE( assignment_to_constant_fail ) {
				try {
					auto t = RPNEvaluator().evaluate({ make<Pi>(), make<Integer>(4), make<Assignment>() });
					GATS_FAIL( "Failed to throw an 'assignment to a non-variable' exception." );
				}
				catch( std::exception const& ex ) {
					GATS_CHECK( std::string( ex.what() ) == std::string("Error: assignment to a non-variable.") );
				}
			}
		#endif // TEST_VARIABLE
	#endif	// TEST_MIXED

	#if TEST_BOOLEAN
		GATS_TEST_CASE(test_and) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<And>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<And>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<And>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<False>(), make<False>(), make<And>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		GATS_TEST_CASE(test_nand) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Nand>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<Nand>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Nand>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<False>(), make<Nand>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
		}
		GATS_TEST_CASE(test_nor) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Nor>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<Nor>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Nor>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<False>(), make<False>(), make<Nor>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
		}
		GATS_TEST_CASE(test_or) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Or>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<Or>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Or>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<False>(), make<Or>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		GATS_TEST_CASE(test_xor) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Xor>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<Xor>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Xor>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<False>(), make<Xor>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		GATS_TEST_CASE(test_equality_boolean) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Equality>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Equality>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		GATS_TEST_CASE(test_inequality_boolean) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Inequality>() });
			GATS_CHECK(value_of<Boolean>(result) != true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Inequality>() });
			GATS_CHECK(value_of<Boolean>(result) != false);
		}
		GATS_TEST_CASE(test_greater_boolean) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Greater>() });
			GATS_CHECK(value_of<Boolean>(result) != true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Greater>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		GATS_TEST_CASE(test_greater_equal_boolean) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<GreaterEqual>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<GreaterEqual>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<GreaterEqual>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
		}
		GATS_TEST_CASE(test_less_boolean) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<Less>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<Less>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<Less>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		GATS_TEST_CASE(test_less_equal_boolean) {
			auto result = RPNEvaluator().evaluate({ make<True>(), make<True>(), make<LessEqual>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<False>(), make<True>(), make<LessEqual>() });
			GATS_CHECK(value_of<Boolean>(result) == true);
			result = RPNEvaluator().evaluate({ make<True>(), make<False>(), make<LessEqual>() });
			GATS_CHECK(value_of<Boolean>(result) == false);
		}
		#if TEST_INTEGER
			// Equality
			GATS_TEST_CASE(test_equality_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(1), make<Equality>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Integer>(1), make<Equality>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_inequality_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(1), make<Inequality>() });
				GATS_CHECK(value_of<Boolean>(result) != true);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Integer>(1), make<Inequality>() });
				GATS_CHECK(value_of<Boolean>(result) != false);
			}
			GATS_TEST_CASE(test_greater_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(1), make<Greater>() });
				GATS_CHECK(value_of<Boolean>(result) != true);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Integer>(1), make<Greater>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_greater_equal_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(1), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Integer>(1), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(0), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
			}
			GATS_TEST_CASE(test_less_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(1), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Integer>(1), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(0), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_less_equal_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(1), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Integer>(1), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(0), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
		#endif
		#if TEST_REAL
			GATS_TEST_CASE(test_equality_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("1.0")), make<Equality>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Real>(Real::value_type("1.0")), make<Equality>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_inequality_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("1.0")), make<Inequality>() });
				GATS_CHECK(value_of<Boolean>(result) != true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Real>(Real::value_type("1.0")), make<Inequality>() });
				GATS_CHECK(value_of<Boolean>(result) != false);
			}
			GATS_TEST_CASE(test_greater_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("1.0")), make<Greater>() });
				GATS_CHECK(value_of<Boolean>(result) != true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Real>(Real::value_type("1.0")), make<Greater>() });
				GATS_CHECK(value_of<Boolean>(result) != false);
			}
			GATS_TEST_CASE(test_greater_equal_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("1.0")), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("1.0")), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("2.0")), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_less_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("1.0")), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Real>(Real::value_type("1.0")), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("0.1")), make<Real>(Real::value_type("1.0")), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
			}
			GATS_TEST_CASE(test_less_equal_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("1.0")), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("1.0")), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("2.0")), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
			}
		#endif

		#if TEST_MIXED
			GATS_TEST_CASE(test_equality_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("1.0")), make<Equality>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Integer>(1), make<Equality>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_inequality_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("1.0")), make<Inequality>() });
				GATS_CHECK(value_of<Boolean>(result) != true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Integer>(1), make<Inequality>() });
				GATS_CHECK(value_of<Boolean>(result) != false);
			}
			GATS_TEST_CASE(test_greater_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("1.0")), make<Greater>() });
				GATS_CHECK(value_of<Boolean>(result) != true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Integer>(1), make<Greater>() });
				GATS_CHECK(value_of<Boolean>(result) != false);
			}
			GATS_TEST_CASE(test_greater_equal_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(1), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Integer>(1), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(2), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("1.0")), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("1.0")), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("2.0")), make<GreaterEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
			}
			GATS_TEST_CASE(test_less_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("1.0")), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Integer>(0), make<Real>(Real::value_type("1.0")), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("1.0")), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(1), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Integer>(1), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("0.1")), make<Integer>(1), make<Less>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
			}
			GATS_TEST_CASE(test_less_equal_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(1), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.1")), make<Integer>(1), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(2), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("1.0")), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
				result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("1.0")), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == false);
				result = RPNEvaluator().evaluate({ make<Integer>(1), make<Real>(Real::value_type("2.0")), make<LessEqual>() });
				GATS_CHECK(value_of<Boolean>(result) == true);
			}
		#endif
	#endif // TEST_BOOLEAN
#endif // TEST_BINARY_OPERATOR

#if TEST_FUNCTION
	#if TEST_SINGLE_ARG
		#if TEST_INTEGER
			GATS_TEST_CASE(test_abs_Integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(4), make<Abs>() });
				GATS_CHECK(value_of<Integer>(result) == 4);
			}
			GATS_TEST_CASE(test_abs_Integer_neg) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(-4), make<Abs>() });
				GATS_CHECK(value_of<Integer>(result) == 4);
			}
		#endif
		#if TEST_REAL
			GATS_TEST_CASE(test_abs_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("4.0")), make<Abs>() });
				GATS_CHECK(round(result) == round(Real::value_type("4.0")));
			}
			GATS_TEST_CASE(test_abs_Real_neg) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("-4.0")), make<Abs>() });
				GATS_CHECK(round(result) == round(Real::value_type("4.0")));
			}
			GATS_TEST_CASE(test_acos_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Arccos>() });
				GATS_CHECK(round(result) == Real::value_type("0.0"));
			}
			GATS_TEST_CASE(test_asin_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Arcsin>() });
				GATS_CHECK(value_of<Real>(result) == boost::math::constants::half_pi<Real::value_type>());
			}
			GATS_TEST_CASE(test_atan_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("0.0")), make<Arctan>() });
				GATS_CHECK(round(result) == Real::value_type("0.0"));
			}
			GATS_TEST_CASE(test_ceil_high_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("4.3")), make<Ceil>() });
				GATS_CHECK(round(result) == round(Real::value_type("5.0")));
			}
			GATS_TEST_CASE(test_ceil_low_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("-4.3")), make<Ceil>() });
				GATS_CHECK(round(result) == round(Real::value_type("-4.0")));
			}
			GATS_TEST_CASE(test_cos_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("0.0")), make<Cos>() });
				GATS_CHECK(round(result) == round(Real::value_type("1.0")));
			}
			GATS_TEST_CASE(test_exp) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Exp>() });
				GATS_CHECK(round(result) == round(boost::math::constants::e<Real::value_type>()));
			}
			GATS_TEST_CASE(test_floor_positive_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("4.3")), make<Floor>() });
				GATS_CHECK(round(result) == round(Real::value_type("4.0")));
			}
			GATS_TEST_CASE(test_floor__negative_Real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("-4.3")), make<Floor>() });
				GATS_CHECK(round(result) == round(Real::value_type("-5.0")));
			}
			GATS_TEST_CASE(test_lb) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("8.0")), make<Lb>() });
				GATS_CHECK(round(value_of<Real>(result)) == round(Real::value_type("3.0")));
			}
			GATS_TEST_CASE(test_ln) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Ln>() });
				GATS_CHECK(round(value_of<Real>(result)) == boost::multiprecision::log(Real::value_type("1.0")));
			}
			GATS_TEST_CASE(test_sin) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("0.0")), make<Sin>() });
				GATS_CHECK(round(value_of<Real>(result)) == Real::value_type("0.0"));
			}
			GATS_TEST_CASE(test_sqrt) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("16.0")), make<Sqrt>() });
				GATS_CHECK(round(value_of<Real>(result)) == round(Real::value_type("4.0")));
			}
			GATS_TEST_CASE(test_tan) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Tan>() });
				GATS_CHECK(value_of<Real>(result) == tan(Real::value_type("1.0")));
			}
		#endif
	#endif //TEST_SINGLE_ARG
	#if TEST_MULTI_ARG
		#if TEST_INTEGER
			GATS_TEST_CASE(test_max_int_int_rhs) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(2), make<Max>() });
				GATS_CHECK(value_of<Integer>(result) == 2);
			}
			GATS_TEST_CASE(test_max_int_int_lhs) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Integer>(1), make<Max>() });
				GATS_CHECK(value_of<Integer>(result) == 2);
			}
			GATS_TEST_CASE(test_max_int_int_same) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Integer>(2), make<Max>() });
				GATS_CHECK(value_of<Integer>(result) == 2);
			}
			GATS_TEST_CASE(test_min_int_int_rhs) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(1), make<Integer>(2), make<Min>() });
				GATS_CHECK(value_of<Integer>(result) == 1);
			}
			GATS_TEST_CASE(test_min_int_int_lhs) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Integer>(1), make<Min>() });
				GATS_CHECK(value_of<Integer>(result) == 1);
			}
			GATS_TEST_CASE(test_min_int_int_same) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Integer>(2), make<Min>() });
				GATS_CHECK(value_of<Integer>(result) == 2);
			}
			GATS_TEST_CASE(test_pow_integer) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Integer>(3), make<Pow>() });
				GATS_CHECK(value_of<Integer>(result) == Integer::value_type(8));
			}
		#endif // TEST_INTEGER
		#if TEST_REAL
			GATS_TEST_CASE(test_arctan2) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("2.0")), make<Arctan2>() });
				GATS_CHECK(value_of<Real>(result) == atan2(Real::value_type("1.0"), Real::value_type("2.0")));
			}
			GATS_TEST_CASE(test_max_real_real_rhs) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("2.0")), make<Max>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_max_real_real_lhs) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("1.0")), make<Max>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_max_real_real_same) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("2.0")), make<Max>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_min_real_real_rhs) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Real>(Real::value_type("2.0")), make<Min>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("1.0"));
			}
			GATS_TEST_CASE(test_min_real_real_lhs) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("1.0")), make<Min>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("1.0"));
			}
			GATS_TEST_CASE(test_min_real_real_same) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("2.0")), make<Min>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_pow_real) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Real>(Real::value_type("3.0")), make<Pow>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("8.0"));
			}
		#endif // TEST_REAL
		#if TEST_MIXED
			GATS_TEST_CASE(test_max_real_int_rhs) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(2), make<Max>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_max_int_real_lhs) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("1.0")), make<Max>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_max_int_real_same) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Integer>(2), make<Max>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_min_real_int_rhs) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("1.0")), make<Integer>(2), make<Min>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("1.0"));
			}
			GATS_TEST_CASE(test_min_int_real_lhs) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("1.0")), make<Min>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("1.0"));
			}
			GATS_TEST_CASE(test_min_int_real_same) {
				auto result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Integer>(2), make<Min>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("2.0"));
			}
			GATS_TEST_CASE(test_pow_mixed) {
				auto result = RPNEvaluator().evaluate({ make<Integer>(2), make<Real>(Real::value_type("3.0")), make<Pow>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("8.0"));
				result = RPNEvaluator().evaluate({ make<Real>(Real::value_type("2.0")), make<Integer>(3), make<Pow>() });
				GATS_CHECK(value_of<Real>(result) == Real::value_type("8.0"));
			}
		#endif // TEST_MIXED
	#endif // TEST_MULTI_ARG
#endif // TEST_FUNCTION


// math functions

//local headers
#include "math.h"

//third party headers
#include <boost/math/special_functions/binomial.hpp>

//standard headers
#include <cmath>
#include <cstdint>

std::size_t bin_coeff_get_max_k(std::size_t size)
{
    // what is the largest value of (n/2) such that
    // n choose (n/2) will fit in an integral T{} without overflow?

    // note: results found by incrementing 'n' until failure
    switch (size)
    {
        case 1:
            return 10/2;
        case 2:
            return 18/2;
        case 4:
            return 34/2;
        case 8:
            return 67/2;
        case 16:
            return 131/2;
        case 32:
            return 260/2;
        case 64:
            return 516/2;
        case 128:
            return 1029/2;
        break;
    };

    // unknown size, approximate the answer
    return (10 * size);
}

std::vector<std::uint16_t> get_primes_up_to(const std::uint16_t n)
{
    // Eratosthenes sieve
    // get all primes in range [2, n]
    // '1' and '0' not considered primes
    std::vector<bool> sieve;
    std::vector<std::uint16_t> result;
    sieve.resize(n + 1, false);
    result.reserve(n/2);

    for (std::size_t i{2}; i <= n; ++i)
    {
        // If false, then it's a prime.
        if (sieve[i] == false)
        {
            // Set all multiples of 'i' to true (i.e. non-prime)
            if (i * i <= n)
            {
                for (std::size_t m{i * i}; m <= n; m += i)
                    sieve[m] = true;
            }

            // save result
            result.push_back(i);
        }
    }

    return result;
}

std::int32_t n_choose_k_bwrap(const std::uint32_t n, const std::uint32_t k)
{
    if (k > n)
        return 0;

    // note: result is 'double::max()' aka 'infinity' if any error occurs
    using namespace boost::math::policies;
    double fp_result = boost::math::binomial_coefficient<double>(n, k,
      make_policy(overflow_error<ignore_error>(),
        evaluation_error<ignore_error>(),
        domain_error<ignore_error>(),
        pole_error<ignore_error>()));

    if (fp_result < 0 || fp_result == std::numeric_limits<double>::infinity())
        return 0;

    if (fp_result > std::numeric_limits<std::int32_t>::max())
        return 0;

    return std::round(fp_result);
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern uncertainty normal distribution
#include <boost/test/unit_test.hpp>
#include <boost/random/mersenne_twister.hpp>
#include "fern/language/uncertainty/normal_distribution.h"

BOOST_AUTO_TEST_CASE(constructor)
{
    fern::NormalDistribution<double> distribution(5.0, 2.5);

    BOOST_CHECK_CLOSE(distribution.mean(), 5.0, 0.001);
    BOOST_CHECK_CLOSE(distribution.standard_deviation(), 2.5, 0.001);

    // boost::random::mt19937 random_number_generator;

    // for(size_t i = 0; i < 1000; ++i) {
    //     std::cout << i << " " << distribution(random_number_generator) << std::endl;
    // }

    // BOOST_CHECK(false);
}


// Boost.Geometry

// Copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP

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

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace detail
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/
template <typename CT>
class thomas_inverse
{
public:
    template <typename T1, typename T2, typename Spheroid>
    thomas_inverse(T1 const& lon1,
                   T1 const& lat1,
                   T2 const& lon2,
                   T2 const& lat2,
                   Spheroid const& spheroid)
        : m_a(get_radius<0>(spheroid))
        , m_f(detail::flattening<CT>(spheroid))
        , m_is_result_zero(false)
    {
        // coordinates in radians

        if ( math::equals(lon1, lon2)
          && math::equals(lat1, lat2) )
        {
            m_is_result_zero = true;
            return;
        }

        CT const one_minus_f = CT(1) - m_f;

//        CT const tan_theta1 = one_minus_f * tan(lat1);
//        CT const tan_theta2 = one_minus_f * tan(lat2);
//        CT const theta1 = atan(tan_theta1);
//        CT const theta2 = atan(tan_theta2);

        CT const pi_half = math::pi<CT>() / CT(2);
        CT const theta1 = math::equals(lat1, pi_half) ? lat1 :
                          math::equals(lat1, -pi_half) ? lat1 :
                          atan(one_minus_f * tan(lat1));
        CT const theta2 = math::equals(lat2, pi_half) ? lat2 :
                          math::equals(lat2, -pi_half) ? lat2 :
                          atan(one_minus_f * tan(lat2));

        CT const theta_m = (theta1 + theta2) / CT(2);
        CT const d_theta_m = (theta2 - theta1) / CT(2);
        m_d_lambda = lon2 - lon1;
        CT const d_lambda_m = m_d_lambda / CT(2);

        m_sin_theta_m = sin(theta_m);
        m_cos_theta_m = cos(theta_m);
        m_sin_d_theta_m = sin(d_theta_m);
        m_cos_d_theta_m = cos(d_theta_m);
        CT const sin2_theta_m = math::sqr(m_sin_theta_m);
        CT const cos2_theta_m = math::sqr(m_cos_theta_m);
        CT const sin2_d_theta_m = math::sqr(m_sin_d_theta_m);
        CT const cos2_d_theta_m = math::sqr(m_cos_d_theta_m);
        CT const sin_d_lambda_m = sin(d_lambda_m);
        CT const sin2_d_lambda_m = math::sqr(sin_d_lambda_m);

        CT const H = cos2_theta_m - sin2_d_theta_m;
        CT const L = sin2_d_theta_m + H * sin2_d_lambda_m;
        m_cos_d = CT(1) - CT(2) * L;
        CT const d = acos(m_cos_d);
        m_sin_d = sin(d);

        CT const one_minus_L = CT(1) - L;

        if ( math::equals(m_sin_d, CT(0))
          || math::equals(L, CT(0))
          || math::equals(one_minus_L, CT(0)) )
        {
            m_is_result_zero = true;
            return;
        }

        CT const U = CT(2) * sin2_theta_m * cos2_d_theta_m / one_minus_L;
        CT const V = CT(2) * sin2_d_theta_m * cos2_theta_m / L;
        m_X = U + V;
        m_Y = U - V;
        m_T = d / m_sin_d;
        //CT const D = CT(4) * math::sqr(T);
        //CT const E = CT(2) * cos_d;
        //CT const A = D * E;
        //CT const B = CT(2) * D;
        //CT const C = T - (A - E) / CT(2);
    }

    inline CT distance() const
    {
        if ( m_is_result_zero )
        {
            // TODO return some approximated value
            return CT(0);
        }

        //CT const n1 = X * (A + C*X);
        //CT const n2 = Y * (B + E*Y);
        //CT const n3 = D*X*Y;

        //CT const f_sqr = math::sqr(f);
        //CT const f_sqr_per_64 = f_sqr / CT(64);

        CT const delta1d = m_f * (m_T*m_X-m_Y) / CT(4);
        //CT const delta2d = f_sqr_per_64 * (n1 - n2 + n3);

        return m_a * m_sin_d * (m_T - delta1d);
        //double S2 = a * sin_d * (T - delta1d + delta2d);
    }

    inline CT azimuth() const
    {
        // NOTE: if both cos_latX == 0 then below we'd have 0 * INF
        // it's a situation when the endpoints are on the poles +-90 deg
        // in this case the azimuth could either be 0 or +-pi
        if ( m_is_result_zero )
        {
            return CT(0);
        }

        // may also be used to calculate distance21
        //CT const D = CT(4) * math::sqr(T);
        CT const E = CT(2) * m_cos_d;
        //CT const A = D * E;
        //CT const B = CT(2) * D;
        // may also be used to calculate distance21
        CT const f_sqr = math::sqr(m_f);
        CT const f_sqr_per_64 = f_sqr / CT(64);

        CT const F = CT(2)*m_Y-E*(CT(4)-m_X);
        //CT const M = CT(32)*T-(CT(20)*T-A)*X-(B+CT(4))*Y;
        CT const G = m_f*m_T/CT(2) + f_sqr_per_64;
        CT const tan_d_lambda = tan(m_d_lambda);
        CT const Q = -(F*G*tan_d_lambda) / CT(4);

        CT const d_lambda_p = (m_d_lambda + Q) / CT(2);
        CT const tan_d_lambda_p = tan(d_lambda_p);

        CT const v = atan2(m_cos_d_theta_m, m_sin_theta_m * tan_d_lambda_p);
        CT const u = atan2(-m_sin_d_theta_m, m_cos_theta_m * tan_d_lambda_p);

        CT const pi = math::pi<CT>();
        CT alpha1 = v + u;
        if ( alpha1 > pi )
        {
            alpha1 -= CT(2) * pi;
        }

        return alpha1;
    }

private:
    CT const m_a;
    CT const m_f;

    CT m_d_lambda;
    CT m_cos_d;
    CT m_sin_d;
    CT m_X;
    CT m_Y;
    CT m_T;
    CT m_sin_theta_m;
    CT m_cos_theta_m;
    CT m_sin_d_theta_m;
    CT m_cos_d_theta_m;

    bool m_is_result_zero;
};

}}} // namespace boost::geometry::detail

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP


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

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <vector>
#include <numeric>

constexpr auto pi = 3.141592653589793;

double log_marginal_probability_cpp(Eigen::VectorXd m, Eigen::MatrixXd S,
    double r, double v, Eigen::MatrixXd X) {

    int N = X.rows();
    int p = X.cols();
    Eigen::VectorXd xsum = X.colwise().sum().transpose();

    Eigen::MatrixXd Sprime = S + X.transpose() * X + r * N / (N + r) * (m * m.transpose()) - 1 / (N + r)
    * (xsum * xsum.transpose()) - r / (N + r) * (m * xsum.transpose() + xsum * m.transpose());

    double vprime = v + N;

    std::vector<double> gamma1(p, 0);
    std::vector<double> gamma2(p, 0);
    for (int i = 0; i < p; ++i) {
        gamma1[i] = lgamma((v - i) / 2);
        gamma2[i] = lgamma((vprime - i) / 2);
    }
    double gamma3 = std::accumulate(gamma1.begin(), gamma1.end(), 0.0);
	double gamma4 = std::accumulate(gamma2.begin(), gamma2.end(), 0.0);

	double log_prob = -N * p / 2 * log(2 * pi) + p / 2 * log(r / (N + r)) + v / 2 * log(S.determinant()) - vprime / 2
		* log(Sprime.determinant()) + vprime * p / 2 * log(2) + gamma4 - v * p / 2 * log(2) - gamma3;

	return log_prob;
}

PYBIND11_PLUGIN(log_marginal_prob) {
    pybind11::module m("log_marginal_prob", "auto-compiled c++ extension");
    m.def("log_marginal_probability_cpp", &log_marginal_probability_cpp);
    return m.ptr();
}


#include "FKDerivative.h"
#include <Eigen/Dense>
#include <totalmodel.h>
#include <cmath>
#include <cassert>
#include <chrono>

using namespace Eigen;

void AngleAxisToRotationMatrix_Derivative(const double* pose, double* dR_data, const int idj, const int numberColumns)
{
    Eigen::Map< Eigen::Matrix<double, 9, Eigen::Dynamic, Eigen::RowMajor> > dR(dR_data, 9, numberColumns);
    std::fill(dR_data, dR_data + 9 * numberColumns, 0.0);
    const double theta2 = pose[0] * pose[0] + pose[1] * pose[1] + pose[2] * pose[2];
    if (theta2 > std::numeric_limits<double>::epsilon())
    {
        const double theta = sqrt(theta2);
        const double s = sin(theta);
        const double c = cos(theta);
        const Eigen::Map< const Eigen::Matrix<double, 3, 1> > u(pose);
        Eigen::VectorXd e(3);
        e[0] = pose[0] / theta; e[1] = pose[1] / theta; e[2] = pose[2] / theta;

        // dR / dtheta
        Eigen::Matrix<double, 9, 1> dRdth(9, 1);
        Eigen::Map< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > dRdth_(dRdth.data());
        // skew symmetric
        dRdth_ << 0.0, -e[2], e[1],
                  e[2], 0.0, -e[0],
                  -e[1], e[0], 0.0;
        // dRdth_ = dRdth_ * c - Matrix<double, 3, 3>::Identity() * s + s * e * e.transpose();
        dRdth_ = - dRdth_ * c - Matrix<double, 3, 3>::Identity() * s + s * e * e.transpose();

        // dR / de
        Eigen::Matrix<double, 9, 3, RowMajor> dRde(9, 3);
        // d(ee^T) / de
        dRde <<
            2 * e[0], 0., 0.,
            e[1], e[0], 0.,
            e[2], 0., e[0],
            e[1], e[0], 0.,
            0., 2 * e[1], 0.,
            0., e[2], e[1],
            e[2], 0., e[0],
            0., e[2], e[1],
            0., 0., 2 * e[2];
        Eigen::Matrix<double, 9, 3, RowMajor> dexde(9, 3);
        dexde <<
            0, 0, 0,
            0, 0, -1,
            0, 1, 0,
            0, 0, 1,
            0, 0, 0,
            -1, 0, 0,
            0, -1, 0,
            1, 0, 0,
            0, 0, 0;
        // dRde = dRde * (1. - c) + c * dexde;
        dRde = dRde * (1. - c) - s * dexde;
        Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> dedu = Matrix<double, 3, 3>::Identity() / theta - u * u.transpose() / theta2 / theta;

        dR.block(0, 3 * idj, 9, 3) = dRdth * e.transpose() + dRde * dedu;
    }
    else
    {
        dR(1, 3 * idj + 2) = 1;
        dR(2, 3 * idj + 1) = -1;
        dR(3, 3 * idj + 2) = -1;
        dR(5, 3 * idj) = 1;
        dR(6, 3 * idj + 1) = 1;
        dR(7, 3 * idj) = -1;
    }
}

void EulerAnglesToRotationMatrix_Derivative(const double* pose, double* dR_data, const int idj, const int numberColumns)
{
    const double degrees_to_radians = 3.14159265358979323846 / 180.0;
    const double pitch(pose[0] * degrees_to_radians);
    const double roll(pose[1] * degrees_to_radians);
    const double yaw(pose[2] * degrees_to_radians);

    const double c1 = cos(yaw);
    const double s1 = sin(yaw);
    const double c2 = cos(roll);
    const double s2 = sin(roll);
    const double c3 = cos(pitch);
    const double s3 = sin(pitch);

    Eigen::Matrix<double, 9, 3, Eigen::RowMajor> dRdp(9, 3);
    // dRdp <<
    //  -s1 * c2, -s2 * c1, 0.,
    //  -c1 * c3 - s1 * s2 * s3, c1 * c2 * s3, s1 * s3 + c1 * s2 * c3,
    //  c1 * s3 - s1 * s2 * c3, c1 * c2 * c3, s1 * c3 - c1 * s2 * s3,
    //  c1 * c2, -s1 * s2, 0.,
    //  -s1 * c3 + c1 * s2 * s3, s1 * c2 * s3, -c1 * s3 + s1 * s2 * c3,
    //  s1 * s3 + c1 * s2 * c3, s1 * c2 * c3, -c1 * c3 - s1 * s2 * s3,
    //  0, -c2, 0,
    //  0, -s2 * s3, c2 * c3,
    //  0, -s2 * c3, -c2 * s3;

    dRdp <<
        0.,                       -s2 * c1,      -s1 * c2,
        s1 * s3 + c1 * s2 * c3,    c1 * c2 * s3, -c1 * c3 - s1 * s2 * s3,
        s1 * c3 - c1 * s2 * s3,    c1 * c2 * c3, c1 * s3 - s1 * s2 * c3,
        0.,                       -s1 * s2,      c1 * c2,
        -c1 * s3 + s1 * s2 * c3,   s1 * c2 * s3, -s1 * c3 + c1 * s2 * s3,
        -c1 * c3 - s1 * s2 * s3,   s1 * c2 * c3, s1 * s3 + c1 * s2 * c3,
        0,                        -c2,           0,
        c2 * c3,                  -s2 * s3,      0,
        -c2 * s3,                 -s2 * c3,      0;

    Eigen::Map< Eigen::Matrix<double, 9, Eigen::Dynamic, Eigen::RowMajor> > dR(dR_data, 9, numberColumns);
    std::fill(dR_data, dR_data + 9 * numberColumns, 0.0);
    dR.block(0, 3 * idj, 9, 3) = degrees_to_radians * dRdp;
}

void Product_Derivative(const double* const A_data, const double* const dA_data, const double* const B_data,
                        const double* const dB_data, double* dAB_data, const int B_col)
{
    assert(dA_data != NULL || dB_data != NULL);
    assert(B_col == 3 || B_col == 1);  // matrix multiplication or matrix-vector multiplication
    if (dA_data != NULL && dB_data != NULL)
    {
        const Eigen::Map<const Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dA(dA_data);
        if (B_col == 1)
        {
            // B_col == 1
            // d(AB) = AdB + (dA)B
            const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > A(A_data);
            const Eigen::Map<const Eigen::Matrix<double, 3, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dB(dB_data);
            Eigen::Map< Eigen::Matrix<double, 3, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dAB(dAB_data);
            for (int r = 0; r < 3; r++)
            {
                const int baseIndex = 3*r;
                dAB.row(r) = A(r, 0) * dB.row(0) + A(r, 1) * dB.row(1) + A(r, 2) * dB.row(2) +
                    B_data[0] * dA.row(baseIndex) + B_data[1] * dA.row(baseIndex + 1) + B_data[2] * dA.row(baseIndex + 2);
            }
        }
        else
        {
            // B_col == 3
            // d(AB) = AdB + (dA)B
            const Eigen::Map<const Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dB(dB_data);
            Eigen::Map< Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dAB(dAB_data);
            for (int r = 0; r < 3; r++)
            {
                const int baseIndex = 3*r;
                for (int c = 0; c < 3; c++)
                {
                    dAB.row(baseIndex + c) = A_data[baseIndex] * dB.row(c) + A_data[baseIndex+1] * dB.row(3 + c) + A_data[baseIndex+2] * dB.row(6 + c) +
                        B_data[c] * dA.row(baseIndex) + B_data[3 + c] * dA.row(baseIndex + 1) + B_data[6 + c] * dA.row(baseIndex + 2);
                }
            }
        }
    }
    else if (dA_data != NULL && dB_data == NULL)  // B is a constant matrix / vector, no derivative
    {
        const Eigen::Map<const Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dA(dA_data);
        if (B_col == 1)
        {
            // d(AB) = AdB + (dA)B
            Eigen::Map< Eigen::Matrix<double, 3, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dAB(dAB_data);
            // // Matrix form (slower)
            // for (int r = 0; r < 3; r++)
            //     dABAux.row(r) = B * dA.block<3, TotalModel::NUM_JOINTS * 3>(r, 0);
            // For loop form
            for (int r = 0; r < 3; r++)
            {
                const int baseIndex = 3*r;
                dAB.row(r) = B_data[0] * dA.row(baseIndex) + B_data[1] * dA.row(baseIndex + 1) + B_data[2] * dA.row(baseIndex + 2);
            }
        }
        else
        {
            // B_col == 3
            Eigen::Map< Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dAB(dAB_data);
            for (int r = 0; r < 3; r++)
                for (int c = 0; c < 3; c++)
                    dAB.row(3 * r + c) = B_data[c] * dA.row(3 * r) + B_data[3 + c] * dA.row(3 * r + 1) + B_data[6 + c] * dA.row(3 * r + 2);  // d(AB) = AdB + (dA)B
        }
    }
    else // A is a constant matrix, no derivative
    {
        const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > A(A_data);
        // dA_data == NULL && dB_data != NULL
        if (B_col == 1)
        {
            const Eigen::Map<const Eigen::Matrix<double, 3, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dB(dB_data);
            Eigen::Map< Eigen::Matrix<double, 3, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dAB(dAB_data);
            dAB.setZero();
            for (int r = 0; r < 3; r++)
                dAB.row(r) = A(r, 0) * dB.row(0) + A(r, 1) * dB.row(1) + A(r, 2) * dB.row(2);
        }
        else
        {
            // B_col == 3
            const Eigen::Map<const Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dB(dB_data);
            Eigen::Map< Eigen::Matrix<double, 9, TotalModel::NUM_JOINTS * 3, Eigen::RowMajor> > dAB(dAB_data);
            dAB.setZero();
            for (int r = 0; r < 3; r++)
                for (int c = 0; c < 3; c++)
                    dAB.row(3 * r + c) = A(r, 0) * dB.row(0 + c) + A(r, 1) * dB.row(3 + c) + A(r, 2) * dB.row(6 + c);
        }
    }
}

void SparseProductDerivative(const double* const A_data, const double* const dA_data, const double* const B_data,
                             const double* const dB_data, const int colIndex, const std::vector<int>& parentIndexes, double* dAB_data, const int numberColumns)
{
    // d(AB) = AdB + (dA)B
    Eigen::Map< Eigen::Matrix<double, 9, Eigen::Dynamic, Eigen::RowMajor> > dAB(dAB_data, 9, numberColumns);

    std::fill(dAB_data, dAB_data + 9 * numberColumns, 0.0);
    // // Dense dAB (sparse dB) version
    // const Eigen::Map<const Eigen::Matrix<double, 9, numberColumns, Eigen::RowMajor> > dA(dA_data);
    // const Eigen::Map<const Eigen::Matrix<double, 9, numberColumns, Eigen::RowMajor> > dB(dB_data);
    // dAB.row(baseIndex + c) = B_data[c] * dA.row(baseIndex) + B_data[3 + c] * dA.row(baseIndex + 1) + B_data[6 + c] * dA.row(baseIndex + 2);
    // dAB.block<1,3>(baseIndex + c, 3*colIndex) += A_data[baseIndex] * dB.block<1,3>(c, 3*colIndex)
    //                                            + A_data[baseIndex+1] * dB.block<1,3>(3+c, 3*colIndex)
    //                                            + A_data[baseIndex+2] * dB.block<1,3>(6+c, 3*colIndex);
    // Sparse sped up equivalent
    const auto colOffset = 3*colIndex;
    for (int r = 0; r < 3; r++)
    {
        const int baseIndex = 3*r;
        for (int c = 0; c < 3; c++)
        {
            // AdB
            for (int subIndex = 0; subIndex < 3; subIndex++)
            {
                const auto finalOffset = colOffset + subIndex;
                dAB_data[numberColumns*(baseIndex + c) + finalOffset] +=
                    A_data[baseIndex] * dB_data[numberColumns*c + finalOffset]
                    + A_data[baseIndex+1] * dB_data[numberColumns*(3+c) + finalOffset]
                    + A_data[baseIndex+2] * dB_data[numberColumns*(6+c) + finalOffset];
            }
            // // AdB - Slower equivalent
            // dAB.block<1,3>(baseIndex + c, colOffset) += A_data[baseIndex] * dB.block<1,3>(c, colOffset)
            //                                            + A_data[baseIndex+1] * dB.block<1,3>(3+c, colOffset)
            //                                            + A_data[baseIndex+2] * dB.block<1,3>(6+c, colOffset);
            // (dA)B
            for (const auto& parentIndex : parentIndexes)
            {
                const auto parentOffset = 3*parentIndex;
                for (int subIndex = 0; subIndex < 3; subIndex++)
                {
                    const auto finalOffset = parentOffset + subIndex;
                    dAB_data[numberColumns*(baseIndex + c) + finalOffset] +=
                        B_data[c] * dA_data[numberColumns*baseIndex + finalOffset]
                        + B_data[3 + c] * dA_data[numberColumns*(baseIndex+1) + finalOffset]
                        + B_data[6 + c] * dA_data[numberColumns*(baseIndex+2) + finalOffset];
                }
            }
            // // (dA)B - Slower equivalent
            // for (const auto& parentIndex : parentIndexes)
            // {
            //     const auto parentOffset = 3*parentIndex;
            //     dAB.block<1,3>(baseIndex + c, parentOffset) += B_data[c] * dA.block<1,3>(baseIndex, parentOffset)
            //                                                  + B_data[3 + c] * dA.block<1,3>(baseIndex+1, parentOffset)
            //                                                  + B_data[6 + c] * dA.block<1,3>(baseIndex+2, parentOffset);
            // }
        }
    }
}

void SparseProductDerivative(const double* const dA_data, const double* const B_data,
                             const std::vector<int>& parentIndexes, double* dAB_data, const int numberColumns)
{
    // d(AB) = AdB + (dA)B
    // Sparse for loop form
    std::fill(dAB_data, dAB_data + 3 * numberColumns, 0.0);
    for (int r = 0; r < 3; r++)
    {
        const int baseIndex = 3*r;
        for (const auto& parentIndex : parentIndexes)
        {
            const auto parentOffset = 3*parentIndex;
            for (int subIndex = 0; subIndex < 3; subIndex++)
            {
                const auto finalOffset = parentOffset + subIndex;
                dAB_data[numberColumns*r + finalOffset] +=
                    B_data[0] * dA_data[numberColumns*baseIndex + finalOffset]
                    + B_data[1] * dA_data[numberColumns*(baseIndex+1) + finalOffset]
                    + B_data[2] * dA_data[numberColumns*(baseIndex+2) + finalOffset];
            }
        }
    }
    // // Dense Matrix form (slower)
    // Eigen::Map< Eigen::Matrix<double, 3, numberColumns, Eigen::RowMajor> > dAB(dAB_data);
    // const Eigen::Map<const Eigen::Matrix<double, 9, numberColumns, Eigen::RowMajor> > dA(dA_data);
    // for (int r = 0; r < 3; r++)
    //     dABAux.row(r) = B * dA.block<3, numberColumns>(r, 0);
    // // Dense for loop form
    // for (int r = 0; r < 3; r++)
    // {
    //     const int baseIndex = 3*r;
    //     dAB.row(r) = B_data[0] * dA.row(baseIndex) + B_data[1] * dA.row(baseIndex + 1) + B_data[2] * dA.row(baseIndex + 2);
    // }
}

void SparseProductDerivativeConstA(const double* const A_data, const double* const dB_data,
                             const std::vector<int>& parentIndexes, double* dAB_data, const int numberColumns)
{
	// d(AB) = AdB (A is a constant.)
    // Sparse for loop form
    std::fill(dAB_data, dAB_data + 3 * numberColumns, 0.0);
    for (int r = 0; r < 3; r++)
    {
		for (const auto& parentIndex : parentIndexes)
	    {
	    	const auto parentOffset = 3*parentIndex;
	    	for (int subIndex = 0; subIndex < 3; subIndex++)
            {
            	const auto finalOffset = parentOffset + subIndex;
            	dAB_data[numberColumns * r + finalOffset] = A_data[3 * r + 0] * dB_data[finalOffset] + A_data[3 * r + 1] * dB_data[finalOffset + numberColumns] +
            		A_data[3 * r + 2] * dB_data[finalOffset + numberColumns + numberColumns];
            }
		}
	}
}

void SparseAdd(const double* const B_data, const std::vector<int>& parentIndexes, double* A_data, const int numberColumns)
{
    // d(AB) += d(AB)_parent
    Eigen::Map< Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor>> A(A_data, 3, numberColumns);
    const Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor>> B(B_data, 3, numberColumns);
    // Sparse for loop
    for (int r = 0; r < 3; r++)
    {
        for (const auto& parentIndex : parentIndexes)
        {
            const auto parentOffset = 3*parentIndex;
            for (int subIndex = 0; subIndex < 3; subIndex++)
            {
                const auto finalOffset = parentOffset + subIndex;
                A_data[numberColumns*r + finalOffset] += B_data[numberColumns*r + finalOffset];
            }
        }
    }
    // // Dense equivalent
    // dMtdPIdj += dJdP.block<3, numberColumns>(3 * ipar, 0);
    // A += B;
}

void SparseSubtract(const double* const B_data, const std::vector<int>& parentIndexes, double* A_data, const int numberColumns)
{
    // d(AB) += d(AB)_parent
    Eigen::Map< Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor>> A(A_data, 3, numberColumns);
    const Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor>> B(B_data, 3, numberColumns);
    // Sparse for loop
    for (int r = 0; r < 3; r++)
    {
        for (const auto& parentIndex : parentIndexes)
        {
            const auto parentOffset = 3*parentIndex;
            for (int subIndex = 0; subIndex < 3; subIndex++)
            {
                const auto finalOffset = parentOffset + subIndex;
                A_data[numberColumns*r + finalOffset] -= B_data[numberColumns*r + finalOffset];
            }
        }
    }
    // // Dense equivalent
    // dMtdPIdj -= dJdP.block<3, numberColumns>(3 * ipar, 0);
    // A -= B;
}

void projection_Derivative(double* dPdI_data, const double* dJdI_data, const int ncol, double* XYZ, const double* pK_, int offsetP, int offsetJ, float weight)
{
	// Dx/Dt = dx/dX * dX/dt + dx/dY * dY/dt + dx/dZ * dZ/dt
	const double X = XYZ[0], Y = XYZ[1], Z = XYZ[2];
	double* P_row0 = dPdI_data + offsetP * ncol;
	double* P_row1 = dPdI_data + (offsetP + 1) * ncol;
	const double* J_row0 = dJdI_data + offsetJ * ncol;
	const double* J_row1 = dJdI_data + (offsetJ + 1) * ncol;
	const double* J_row2 = dJdI_data + (offsetJ + 2) * ncol;
	for (int i = 0; i < ncol; i++)
		P_row0[i] = weight * ( pK_[0] * J_row0[i] + pK_[1] * J_row1[i] - (pK_[0] * X + pK_[1] * Y) * J_row2[i] / Z ) / Z;
	for (int i = 0; i < ncol; i++)
		P_row1[i] = weight * pK_[4] * ( J_row1[i] - Y / Z * J_row2[i] ) / Z;
	// equivalent to
	// dPdI.row(offsetP + 0) = weight * (pK_[0] / Z * dJdI.row(offsetJ + 0)
	// 					 + pK_[1] / Z * dJdI.row(offsetJ + 1)
	// 					 - (pK_[0] * X + pK_[1] * Y) / Z / Z * dJdI.row(offsetJ + 2));
	// dPdI.row(offsetP + 1) = weight * (pK_[4] / Z * dJdI.row(offsetJ + 1)
	// 					 - pK_[4] * Y / Z / Z * dJdI.row(offsetJ + 2));
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>

using namespace Eigen;

//Existe dans Eigen Blas
//Experimental et pas de decomposition uplow='U' dans eigen et seulement pour les matrices def pos
//info important car utilise dans Matrix.h

// Cholesky decomposition (replaces xpotf2_)
template <typename T>
void subCholeskyDecomposition(const char* job, int *n, T* a, int *info)
{
    Map<Matrix<T, Dynamic, Dynamic> > AMap (a,*n,*n);
    Eigen::LLT<Matrix<T, Dynamic, Dynamic> > llt (AMap);
    AMap = (*job == 'L') ? llt.matrixLLT() : llt.matrixU();
    *info = int(llt.info());
};

void choleskyDecomposition(const char* job, int *n, double* a, int *info) { subCholeskyDecomposition<double>(job, n, a, info); }
void choleskyDecomposition(const char* job, int *n, float* a, int *info) { subCholeskyDecomposition<float>(job, n, a, info); }

// Norms
template <typename T>
T subMatrixNorm(char* norm, int *m, int *n, T *a, int *lda)
{
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a,*m,*n,OuterStride<>(*lda));

    T to_return = 0;
    switch (*norm)
    {
        case 'M':
            to_return = AMap.template lpNorm<Infinity>();
            break;
        case ('1'):
            to_return = (AMap.colwise().sum()).maxCoeff();
            break;
        case ('I'):
            to_return = (AMap.rowwise().sum()).maxCoeff();
            break;
        case ('F'):
            to_return = AMap.norm();
            break;
        default: assert(false);
    }

    return to_return;
}

double matrixNorm(char* norm, int *m, int *n, double *a, int *lda) { return subMatrixNorm<double>(norm, m, n, a, lda); }
float matrixNorm(char* norm, int *m, int *n, float *a, int *lda) { return subMatrixNorm<float>(norm, m, n, a, lda); }

// System solving by LU decomposition (replaces xgetrs_)
template <typename T>
void subLUSystemSolve(int *n,T *a,int *ipiv, T *b,int *info)
{
  Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a,*n,*n,OuterStride<>(*n));
  Map<Matrix<T, Dynamic, 1>, 0, InnerStride<1> > BMap (b,*n);
  Matrix<T, Dynamic, Dynamic> B = BMap;
  T nb = B.norm();
  // Eigen::FullPivLU<Matrix<T, Dynamic, Dynamic> > lu(AMap);

  // BMap = lu.solve(B);
  //  BMap = AMap.fullPivLu().solve(B);
  BMap = AMap.colPivHouseholderQr().solve(B);

  /* // test
  std::cout << "-- LU system solve : ||AX - B||_2 = " << (AMap * BMap - B).norm() << std::endl;
  std::cout << "A " << std::endl;
  printM(a, *n, *n);
  std::cout << "solution" << std::endl;
  printV(b, *n);
  std::cout << "AB " << std::endl;
  T* c = new T[*n];
  affinity(*n, *n, 1, a, b, 0, c, 'N');
  printV(c, *n);
  std::cout << "-- LU system solve : ||AX - B||_2 = " << (AMap * BMap - B).norm() << std::endl;
  delete[] c;
  */

  *info = ((AMap * BMap - B).norm() <= nb * 1.0e-3f) ? 0 : 1;

}

void LUSystemSolve(int *n,double *a,int *ipiv,double *b,int *info) { subLUSystemSolve<double>(n, a, ipiv, b, info); }
void LUSystemSolve(int *n,float *a,int *ipiv,float *b,int *info) { subLUSystemSolve<float>(n, a, ipiv, b, info); }

// System solving by QR decomposition (replaces xgels_)
template <typename T>
void subQRSystemSolve(int *m,int *n,T *a,T *b,int *info)
{
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (a, *m, *n, OuterStride<>(*m));
    Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > BMap (b, std::max(*m,*n), 1, OuterStride<>(std::max(*m, *n)));
    *info = 0;
    Eigen::HouseholderQR<Matrix<T, Dynamic, Dynamic> > qr(AMap);
    Matrix<T, Dynamic, Dynamic> x = qr.solve(BMap.block(0, 0, *m, 1));
    if(*m <= *n) *info = 1 - (AMap * x).isApprox(BMap.block(0, 0, *m, 1));
    BMap.block(0, 0, *n, 1) = x;
}

void QRSystemSolve(int *m, int *n, double *a, double *b, int *info) { subQRSystemSolve<double>(m, n, a, b, info); }
void QRSystemSolve(int *m, int *n, float *a, float *b, int *info) { subQRSystemSolve<float>(m, n, a, b, info); }

// QR factorization
template <typename T>
void subQRFactorization(int *m,int *n,int *k,T *a,T *r,int *info)
{
    Map<Matrix<T, Dynamic, Dynamic> > AMap (a, *m, *k);
    Map<Matrix<T, Dynamic, Dynamic> > RMap (r, *n, *k);
    Eigen::HouseholderQR<Matrix<T, Dynamic, Dynamic> > qr(AMap);
    Matrix<T, Dynamic, Dynamic> R1 = Matrix<T, Dynamic, Dynamic>::Zero(*m, *k);
    R1= qr.matrixQR().template triangularView<Upper>();
#ifdef _WIN32
    for(int i=0; i<*n; i++)
        for(int j=0; j<*k; j++)
            RMap(i,j) = R1(i,j);
#else
    RMap= R1.block(0, 0, *n, *k);
#endif
    Matrix<T, Dynamic, Dynamic> q = qr.householderQ();
    Matrix<T, Dynamic, Dynamic> original = q.block(0, 0, *m, *n) * RMap;
    *info=AMap.isApprox(original);
    if (*n == *k)
#ifdef _WIN32
    for(int i=0; i<*m; i++)
        for(int j=0; j<*k; j++)
            AMap(i,j) = q(i,j);
#else
        AMap=q.block(0, 0, *m, *k);
#endif
    else
    {
        Map<Matrix<T, Dynamic, Dynamic> > QMap (a, *m, *m);
        QMap = q;
    }
}

void QRFactorization(int *m, int *n, int *k, double *a, double *r, int *info) { subQRFactorization<double>(m, n, k, a, r, info); }
void QRFactorization(int *m, int *n, int *k, float *a, float *r, int *info) { subQRFactorization<float>(m, n, k, a, r, info); }

// Reciprocal condition number of triangular matrix
template <typename T>
void subConditionNumber(int *n,T *a,int *lda,T *rcond,int *info)
{
    char x = '1';
    T l = (matrixNorm(&x, n, n, a, lda));
    if (l != 0)
    {
        *rcond = 1 / l;
        *info = 0;
    }
    else *info = 1;
}

void conditionNumber(int *n, double *a, int *lda, double *rcond, int *info) { subConditionNumber<double>(n, a, lda, rcond, info); }
void conditionNumber(int *n, float *a, int *lda, float *rcond, int *info) { subConditionNumber<float>(n, a, lda, rcond, info); }


/*
 * sharpqballthread.cpp
 *
 * Created on: 27.12.2012
 * @author Ralph Schurade
 */

#include "sharpqballthread.h"

#include "fmath.h"
#include "../data/datasets/datasetdwi.h"

#include "../gui/gl/glfunctions.h"

#include <qmath.h>

#include <boost/math/special_functions/spherical_harmonic.hpp>

#include "math.h"

SharpQBallThread::SharpQBallThread( DatasetDWI* ds, int order, int id ) :
    m_ds( ds ),
    m_order( order ),
    m_id( id )
{
}

SharpQBallThread::~SharpQBallThread()
{
    m_qBallVector.clear();
}

QVector<ColumnVector> SharpQBallThread::getQBallVector()
{
    return m_qBallVector;
}

void SharpQBallThread::run()
{
    QVector<QVector3D> bvecs = m_ds->getBvecs();

    Matrix gradients( bvecs.size(), 3 );
    for ( int i = 0; i < bvecs.size(); ++i )
    {
        gradients( i + 1, 1 ) = bvecs.at( i ).x();
        gradients( i + 1, 2 ) = bvecs.at( i ).y();
        gradients( i + 1, 3 ) = bvecs.at( i ).z();
    }

    QVector<ColumnVector>* data = m_ds->getData();
    QVector<float>* b0Data = m_ds->getB0Data();

    m_qBallVector.clear();

    // inverse direction matrix for calculation:
    //const matrixT A( pseudoinverse (sh_base( gradients, order ) ) );
    Matrix B = FMath::sh_base( gradients, m_order );
    Matrix A = ( B.t() * B ).i() * B.t();

    int numThreads = GLFunctions::idealThreadCount;

    int chunkSize = data->size() / numThreads;

    int begin = m_id * chunkSize;
    int end = m_id * chunkSize + chunkSize;

    if ( m_id == numThreads - 1 )
    {
        end = data->size();
    }

    // for all voxels:
    for ( int i = begin; i < end; ++i )
    {
        ColumnVector voxel( data->at( i ) / b0Data->at( i ) );

        // regularize data data:
        regularize_sqball( 0.15, 0.15, voxel );
        voxel = FMath::vlog( FMath::vlog( voxel ) * ( -1 ) );
        ColumnVector coeff = A * voxel;

        for ( int k( 2 ); k <= m_order; k += 2 )
        {
            double frt_val = 2.0 * M_PI * boost::math::legendre_p<double>( k, 0 );
            double lbt_val = -k * ( k + 1 );

            for ( int degree = -k; degree <= k; degree++ )
            {
                int l = k * ( k + 1 ) / 2 + degree + 1;
                coeff( l ) *= frt_val * lbt_val;
            }
        }

        if ( b0Data->at( i ) > 0.0 )
        {
            coeff( 1 ) = 1.0 / sqrt( 4. * M_PI );
        }

        m_qBallVector.push_back( coeff );
    }
}

void SharpQBallThread::regularize_sqball( const double par_1, const double par_2, ColumnVector& data )
{
    for ( int i = 1; i <= data.Nrows(); ++i )
    {
        if ( data( i ) < 0 )
        {
            data( i ) = 0.5 * par_1;
        }
        else if ( data( i ) < par_1 )
        {
            data( i ) = 0.5 * par_1 + 0.5 * FMath::pow2( data( i ) ) / par_1;
        }
        else if ( data( i ) < 1.0 - par_2 )
        {
            // do nothing with data
        }
        else if ( data( i ) < 1.0 )
        {
            data( i ) = 1.0 - 0.5 * par_2 - 0.5 * FMath::pow2( 1.0 - data( i ) ) / par_2;
        }
        else
        {
            data( i ) = 1.0 - 0.5 * par_2;
        }
    }
}


#ifndef YASMIC_BGL_KCORE
#define YASMIC_BGL_KCORE

/**
 * @file bgl_kcore.hpp
 * Implement the O(m) algorithm for computing the core number
 * of vertices in the graph.
 *
 * The algorithm comes from:
 *
 * Vladimir Batagelj and Matjaz Zaversnik, "An O(m) Algorithm for Cores
 * Decomposition of Networks."  Sept. 1 2002.
 */

/*
 * David Gleich
 * Stanford University
 * 14 February 2006
 */

#if _MSC_VER >= 1400
	// disable the warning for ifstream::read
    #pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400

#include <boost/graph/iteration_macros.hpp>
#include <boost/iterator/reverse_iterator.hpp>

namespace boost
{

	template <class Graph, class KCoreMap, class PositionMap>
	void core_numbers(const Graph& g, KCoreMap kcm, PositionMap pos)
	{
		typedef typename graph_traits<Graph>::vertex_descriptor vertex;
		typedef typename graph_traits<Graph>::degree_size_type size_type;

		BGL_FORALL_VERTICES_T(v,g,Graph)
		{
			kcm[v] = 0;
		}

		// compute the degree of all vertices
		BGL_FORALL_EDGES_T(e,g,Graph)
		{
			++kcm[source(e,g)];
		}

		size_type max_deg = 0;

		// compute the maximum degree
		BGL_FORALL_VERTICES_T(v,g,Graph)
		{
			if (kcm[v] > max_deg)
			{
				max_deg = kcm[v];
			}
		}

		// now we sort vertices into bins by their degree
		// (we buffer this vector by 2 extra spots to make
		// some of the computations easier.
		//   1.  because deg > 0, we need max_deg+1 to index w/ deg itself
		//   2.  because we want to make things really easy, we extend
		//       the array one past degree to make computing partial_sums
		//       trivial
		std::vector<size_type> bin(max_deg+2);

		// compute the size of each bin
		BGL_FORALL_VERTICES_T(v,g,Graph)
		{
			++bin[kcm[v]];
		}

		// this loop sets bin[d] to the starting position of vertices
		// with degree d in the vert array
		size_type cur_pos = 0;
		for (size_type cur_deg = 0; cur_deg < max_deg+2; ++cur_deg)
		{
			size_type tmp = bin[cur_deg];
			bin[cur_deg] = cur_pos;
			cur_pos += tmp;
		}

		// place the vertices
		std::vector<vertex> vert(num_vertices(g));

		BGL_FORALL_VERTICES_T(v,g,Graph)
		{
			pos[v] = bin[kcm[v]];
			vert[pos[v]] = v;

			++bin[kcm[v]];
		}

		// we ``abused'' bin while placing the vertices, now,
		// we need to restore it

		std::copy(boost::make_reverse_iterator(bin.end()-2),
			boost::make_reverse_iterator(bin.begin()+1),
			boost::make_reverse_iterator(bin.end()-1));

		for (size_type i=0; i < num_vertices(g); ++i)
		{
			vertex v = vert[i];

			// we are now going to remove vertex v from the graph,
			// but only implicitly.  That is, we will decrement
			// the degree of each of the neighbors of v, and
			// adjust the sorting of the arrays appropriately.

			BGL_FORALL_ADJ_T(v,u,g,Graph)
			{
				// if kcm[u] > kcm[v], then u is still in the graph,
				// if kvm[u] = kcm[v], then we'll remove u soon, and
				// it's core number is the same as v.
				// if kvm[u] < kcm[v], we've already removed u.
				if (kcm[u] > kcm[v])
				{
					size_type deg_u = kcm[u];
					size_type pos_u = pos[u];

					// w is the first vertex with the same degree as u
					// (this is the resort operation!)
					size_type pos_w = bin[deg_u];
					vertex w = vert[pos_w];

					if (u != w)
					{
						// swap u and w
						pos[u] = pos_w;
						pos[w] = pos_u;
						vert[pos_w] = u;
						vert[pos_u] = w;
					}

					// now, the vertices array is sorted assuming
					// we perform the following step

					// start the set of vertices with degree of u
					// one into the future (this now points at vertex
					// w which we swapped with u).
					++bin[deg_u];

					// we are removing v from the graph, so u's degree
					// decreases
					--kcm[u];

				}

			}
		}
	}

}

#if _MSC_VER >= 1400
	// disable the warning for ifstream::read
    #pragma warning( pop )
#endif // _MSC_VER >= 1400

#endif // YASMIC_BGL_KCORE


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <orient/axis.hpp>
#include <orient/detail/axis_traits.hpp>

#include <Eigen/Dense>

#include <type_traits>
#include <utility>

namespace orient {

// \brief Calculate angle axis from rotation matrix
// \param R Source rotation matrix
// \return The angle axis
template<typename Scalar>
Eigen::Matrix<Scalar,3,1> angleAxisFromRotationMatrix(Eigen::Matrix<Scalar,3,3> const& R);

// \brief Calculate angle axis with partial derivatives from rotation matrix
// \param R Source rotation matrix
// \return A pair containing first the angle axis then secondly the Jacobian matrix
template<typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,1>, Eigen::Matrix<Scalar, 3, 9>> angleAxisFromRotationMatrixWD(Eigen::Matrix<Scalar,3,3> const& R);

// \brief Calculate quaternion from rotation matrix
// \param R Source rotation matrix
// \return The quaternion
template<typename Scalar>
Eigen::Matrix<Scalar,4,1> quaternionFromRotationMatrix(Eigen::Matrix<Scalar,3,3> const& R);

// \brief Calculate quaternion with partial derivatives from rotation matrix
// \param R Source rotation matrix
// \return A pair containing first the quaternion then secondly the Jacobian matrix
template<typename Scalar>
std::pair<Eigen::Matrix<Scalar,4,1>, Eigen::Matrix<Scalar, 4, 9>> quaternionFromRotationMatrixWD(Eigen::Matrix<Scalar,3,3> const& R);

// \brief Calculate Euler angles from rotation matrix and a rotation order
//        The order of rotation is expressed in instrinsic rotations.
//        For example, the angles associated to a rotation sequence
//        Rz(yaw) * Ry(pitch) * Rx(roll) = R
//        is retrieve with
//        auto ypr = eulerFromRotationMatrix<Axis::z, Axis::y, Axis::x>(R)
//        where the angles in ypr are in the same order as the axes,
//        i.e. [yaw, pitch, roll] in this example
// \param R Source rotation matrix
// \template-params A1,A2,A3 the intrinsic rotation order
// \return A vector containing the angles in intrinsic order
template<Axis A1, Axis A2, Axis A3, typename Scalar, typename Dummy>
Eigen::Matrix<Scalar,3,1> eulerFromRotationMatrix(Eigen::Matrix<Scalar,3,3> const& R);

// \brief Calculate Euler angles and partial derivatives from rotation matrix and a rotation order
//        See above for more detail
// \param R Source rotation matrix
// \template-params A1,A2,A3 The intrinsic rotation order
// \return A pair containg first a vector with angles in intrinsic order and secondly the Jacobian matrix
template<Axis A1, Axis A2, Axis A3, typename Scalar, typename Dummy>
std::pair<Eigen::Matrix<Scalar,3,1>, Eigen::Matrix<Scalar, 3, 9>> eulerFromRotationMatrixWD(Eigen::Matrix<Scalar,3,3> const& R);

}

#include <orient/impl/aa_quat_from_rotation_matrix.hpp>
#include <orient/impl/euler_from_rotation_matrix.hpp>


/**
 *  @file eigen_map.cpp
 *  @author Maximilian Harr <maximilian.harr@daimler.com>
 *  @date 29.06.2017
 *
 *  @brief Eigen map computations
 *
 *
 *
 *          Coding Standard:
 *          wiki.ros.org/CppStyleGuide
 *          https://google.github.io/styleguide/cppguide.html
 *
 *
 *  @bug
 *
 *
 *  @todo
 *
 *
 */

// PRAGMA

// SYSTEM INCLUDES
#include <iostream>  /* Header that defines the standard input/output stream objects */
#include <cstdlib>  /* Header that defines several general purpose functions */
#include <string>
#include <Eigen/Dense>  /* template library for linear algebra http://eigen.tuxfamily.org */
#include <math.h> /* Defines M_PI for pi value */
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

// PROJECT INCLUDES

// LOCAL INCLUDES

// FORWARD REFERENCES

// FUNCTION PROTOTYPES

/** @brief Standard command line parameter processing.
 *  @param Pass command line parameters
 *  @return 0 if -h flag is set
 */
int cmd_check(int, char*[]);

// GLOBAL VARIABLES

using namespace Eigen;
using namespace std;

void outputAsMatrix(const Eigen::Quaterniond& q)
{
    std::cout << "R=" << std::endl << q.normalized().toRotationMatrix() << std::endl;
}

//// MAIN //////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
  /* Check command line options. Stop execution if -h is set. */
  if(!cmd_check(argc,argv)) return 0;

  /* Map array of doubles in Eigen::Matrix3d */
  double array[9];
  for(int i = 0; i < 9; ++i) array[i] = i;
  Eigen::Map< Eigen::Matrix3d> residual_0 = Eigen::Map<Eigen::Matrix3d>(array);
  Eigen::Map< Eigen::Matrix<double, 9, 1> > residual_1 = Eigen::Map< Eigen::Matrix<double, 9, 1> >(array);
  std::cout << "residual_0: " << std::endl << residual_0 << std::endl;
  std::cout << "res 0+0: " << std::endl << residual_0 + residual_0 << std::endl;
  std::cout << "residual_1: " << std::endl << residual_1 << std::endl;

	/* Map Eigen::Matrix3d to array of doubles */
	double array2[9];
	Eigen::Map<Matrix<double,3,3,RowMajor> >(array2,3,3) = residual_0;
	std::cout << "Array2:" << std::endl;
	for(int i = 0; i < 9; ++i){
		std::cout << array2[i] << " ";
	}
	std::cout << std::endl;

  /* Cast Eigen VectorXd to Matrix of float */
  Eigen::Vector3d vector_0;
  vector_0 << 0,1,2;
  Eigen::Matrix<float, 3, 1> p_m = vector_0.template cast<float>();
  std::cout << "p_m: " << std::endl << p_m << std::endl;

}

//// FUNCTION DEFINITIONS //////////////////////////////////////////////////////////////////////////
int cmd_check(int argc, char* argv[])
{
  int option;
  /* third argument of getopt specifies valid options and whether they need input(:) */
  while((option = getopt(argc,argv,"hp:"))>=0)
  {
    switch (option)
    {
      case 'h': std::cout
                << "Usage: <filename> [options] \n\n"
                << "<desription> \n\n"
                << "Options: \n"
                << " -h                    show this help message and exit \n"
                << " -p <PARAMETER>        <description> \n"
                << " \n";
                return 0; /* do not execute main with this option */
      case 'p': std::cout << "-p = " << optarg << "\n"; /* optarg is option argument */
                break;
    }
  }
  return 1;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/QR>

template<typename MatrixType> void qr(const MatrixType& m)
{
  /* this test covers the following files:
     QR.h
  */
  int rows = m.rows();
  int cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> SquareMatrixType;
  typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;

  MatrixType a = MatrixType::Random(rows,cols);
  QR<MatrixType> qrOfA(a);
  VERIFY_IS_APPROX(a, qrOfA.matrixQ() * qrOfA.matrixR());
  VERIFY_IS_NOT_APPROX(a+MatrixType::Identity(rows, cols), qrOfA.matrixQ() * qrOfA.matrixR());

  #if 0 // eigenvalues module not yet ready
  SquareMatrixType b = a.adjoint() * a;

  // check tridiagonalization
  Tridiagonalization<SquareMatrixType> tridiag(b);
  VERIFY_IS_APPROX(b, tridiag.matrixQ() * tridiag.matrixT() * tridiag.matrixQ().adjoint());

  // check hessenberg decomposition
  HessenbergDecomposition<SquareMatrixType> hess(b);
  VERIFY_IS_APPROX(b, hess.matrixQ() * hess.matrixH() * hess.matrixQ().adjoint());
  VERIFY_IS_APPROX(tridiag.matrixT(), hess.matrixH());
  b = SquareMatrixType::Random(cols,cols);
  hess.compute(b);
  VERIFY_IS_APPROX(b, hess.matrixQ() * hess.matrixH() * hess.matrixQ().adjoint());
  #endif
}

void test_eigen2_qr()
{
  for(int i = 0; i < 1; i++) {
    CALL_SUBTEST_1( qr(Matrix2f()) );
    CALL_SUBTEST_2( qr(Matrix4d()) );
    CALL_SUBTEST_3( qr(MatrixXf(12,8)) );
    CALL_SUBTEST_4( qr(MatrixXcd(5,5)) );
    CALL_SUBTEST_4( qr(MatrixXcd(7,3)) );
  }

#ifdef EIGEN_TEST_PART_5
  // small isFullRank test
  {
    Matrix3d mat;
    mat << 1, 45, 1, 2, 2, 2, 1, 2, 3;
    VERIFY(mat.qr().isFullRank());
    mat << 1, 1, 1, 2, 2, 2, 1, 2, 3;
    //always returns true in eigen2support
    //VERIFY(!mat.qr().isFullRank());
  }

#endif
}


/* Boost numeric test of the runge kutta steppers test file

 Copyright 2012 Karsten Ahnert
 Copyright 2012 Mario Mulansky

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE numeric_runge_kutta

#include <iostream>
#include <cmath>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double value_type;

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

// harmonic oscillator, analytic solution x[0] = sin( t )
struct osc
{
    void operator()( const state_type &x , state_type &dxdt , const double t ) const
    {
        dxdt[0] = x[1];
        dxdt[1] = -x[0];
    }
};

/* reset dispatcher */
template< class StepperCategory >
struct resetter
{
    template< class Stepper >
    static void reset( Stepper &stepper ) { }
};

template< >
struct resetter< explicit_error_stepper_fsal_tag >
{
    template< class Stepper >
    static void reset( Stepper &stepper )
    { stepper.reset(); }
};

BOOST_AUTO_TEST_SUITE( numeric_runge_kutta_test )

/* generic test for all runge kutta steppers */
template< class Stepper >
struct perform_runge_kutta_test
{
    void operator()( void )
    {

        Stepper stepper;
        const int o = stepper.order()+1; //order of the error is order of approximation + 1

        const state_type x0 = {{ 0.0 , 1.0 }};
        state_type x1;
        const double t = 0.0;
        /* do a first step with dt=0.1 to get an estimate on the prefactor of the error dx = f * dt^(order+1) */
        double dt = 0.5;
        stepper.do_step( osc() , x0 , t , x1 , dt );
        const double f = 2.0 * std::abs( sin(dt) - x1[0] ) / std::pow( dt , o ); // upper bound

        std::cout << o << " , " << f << std::endl;

        /* as long as we have errors above machine precision */
        while( f*std::pow( dt , o ) > 1E-16 )
        {
            // reset stepper which require resetting (fsal steppers)
            resetter< typename Stepper::stepper_category >::reset( stepper );

            stepper.do_step( osc() , x0 , t , x1 , dt );
            std::cout << "Testing dt=" << dt << std::endl;
            BOOST_CHECK_LT( std::abs( sin(dt) - x1[0] ) , f*std::pow( dt , o ) );
            dt *= 0.5;
        }
    }
};

/* generic error test for all runge kutta steppers */
template< class Stepper >
struct perform_runge_kutta_error_test
{
    void operator()( void )
    {
        Stepper stepper;
        const int o = stepper.error_order()+1; //order of the error is order of approximation + 1

        const state_type x0 = {{ 0.0 , 1.0 }};
        state_type x1 , x_err;
        const double t = 0.0;
        /* do a first step with dt=0.1 to get an estimate on the prefactor of the error dx = f * dt^(order+1) */
        double dt = 0.5;
        stepper.do_step( osc() , x0 , t , x1 , dt , x_err );
        const double f = 2.0 * std::abs( x_err[0] ) / std::pow( dt , o );

        std::cout << o << " , " << f << " , " << x0[0] << std::endl;

        /* as long as we have errors above machine precision */
        while( f*std::pow( dt , o ) > 1E-16 )
        {
            // reset stepper which require resetting (fsal steppers)
            resetter< typename Stepper::stepper_category >::reset( stepper );

            stepper.do_step( osc() , x0 , t , x1 , dt , x_err );
            std::cout << "Testing dt=" << dt << ": " << x_err[1] << std::endl;
            BOOST_CHECK_SMALL( std::abs( x_err[0] ) , f*std::pow( dt , o ) );
            dt *= 0.5;
        }
    }
};

typedef mpl::vector<
    euler< state_type > ,
    modified_midpoint< state_type > ,
    runge_kutta4< state_type > ,
    runge_kutta4_classic< state_type > ,
    runge_kutta_cash_karp54_classic< state_type > ,
    runge_kutta_cash_karp54< state_type > ,
    runge_kutta_dopri5< state_type > ,
    runge_kutta_fehlberg78< state_type >
    > runge_kutta_steppers;

BOOST_AUTO_TEST_CASE_TEMPLATE( runge_kutta_test , Stepper, runge_kutta_steppers )
{
    perform_runge_kutta_test< Stepper > tester;
    tester();
}

typedef mpl::vector<
    runge_kutta_cash_karp54_classic< state_type > ,
    runge_kutta_cash_karp54< state_type > ,
    runge_kutta_dopri5< state_type > ,
    runge_kutta_fehlberg78< state_type >
    > runge_kutta_error_steppers;

BOOST_AUTO_TEST_CASE_TEMPLATE( runge_kutta_error_test , Stepper, runge_kutta_error_steppers )
{
    perform_runge_kutta_error_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()