/*
this file should cover the following functions in hmm.h
- hmm_vec2 HMM_MultiplyVec2(hmm_vec2 Left, hmm_vec2 Right);
- hmm_vec2 HMM_MultiplyVec2f(hmm_vec2 Left, float Right);
- hmm_vec3 HMM_MultiplyVec3(hmm_vec3 Left, hmm_vec3 Right);
- hmm_vec3 HMM_MultiplyVec3f(hmm_vec3 Left, float Right);
- hmm_vec4 HMM_MultiplyVec4(hmm_vec4 Left, hmm_vec4 Right);
- hmm_vec4 HMM_MultiplyVec4f(hmm_vec4 Left, float Right);
- hmm_mat4 HMM_MultiplyMat4(hmm_mat4 Left, hmm_mat4 Right);
- hmm_mat4 HMM_MultiplyMat4f(hmm_mat4 Matrix, float Scalar);
- hmm_vec4 HMM_MultiplyMat4ByVec4(hmm_mat4 Matrix, hmm_vec4 Vector);

take care of operator overloading also for these functions:
- hmm_vec2 operator*(hmm_vec2 Left, hmm_vec2 Right);
- hmm_vec3 operator*(hmm_vec3 Left, hmm_vec3 Right);
- hmm_vec4 operator*(hmm_vec4 Left, hmm_vec4 Right);
- hmm_mat4 operator*(hmm_mat4 Left, hmm_mat4 Right);
- hmm_vec2 operator*(hmm_vec2 Left, float Right);
- hmm_vec3 operator*(hmm_vec3 Left, float Right);
- hmm_vec4 operator*(hmm_vec4 Left, float Right);
- hmm_mat4 operator*(hmm_mat4 Left, float Right);
- hmm_vec2 operator*(float Left, hmm_vec2 Right);
- hmm_vec3 operator*(float Left, hmm_vec3 Right);
- hmm_vec4 operator*(float Left, hmm_vec4 Right);
- hmm_mat4 operator*(float Left, hmm_mat4 Right);
- hmm_vec4 operator*(hmm_mat4 Matrix, hmm_vec4 Vector);
- hmm_vec2 &operator*=(hmm_vec2 &Left, hmm_vec2 Right);
- hmm_vec3 &operator*=(hmm_vec3 &Left, hmm_vec3 Right);
- hmm_vec4 &operator*=(hmm_vec4 &Left, hmm_vec4 Right);
- hmm_vec2 &operator*=(hmm_vec2 &Left, float Right);
- hmm_vec3 &operator*=(hmm_vec3 &Left, float Right);
- hmm_vec4 &operator*=(hmm_vec4 &Left, float Right);
- hmm_mat4 &operator*=(hmm_mat4 &Left, float Right);
*/
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include "HMM.h"
#include "test_helpers.h"
namespace utf = boost::unit_test;
namespace tt = boost::test_tools;
using namespace std;

BOOST_AUTO_TEST_SUITE(multiplication_test_suite)

BOOST_AUTO_TEST_SUITE(multiplication_vec2_by_int, *utf::label("trivial"))

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyVec2_int_BothVecsZeros, *utf::tolerance(0.00001)) {
    //Arrange
    auto zeros = HMM_Vec2i(0, 0);
    //Act
    auto res = HMM_MultiplyVec2(zeros, zeros);
    //Assert
    float expectedRes[2] = {0, 0};
    BOOST_TEST(res.Elements == expectedRes);
}

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyVec2_int_V1_zeros, *utf::tolerance(0.00001)) {
    //Arrange
    auto zeros = HMM_Vec2i(0, 0);
    auto right = HMM_Vec2i(5, -10);
    //Act
    auto res = HMM_MultiplyVec2(zeros, right);
    //Assert
    float expectedRes[2] = {0, 0};
    BOOST_TEST(res.Elements == expectedRes);
}

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyVec2_int_postives, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2i(1, 5);
    auto right = HMM_Vec2i(3, 4);
    //Act
    auto res = HMM_MultiplyVec2(left, right);
    //Assert
    float expectedRes[2] = {left.Elements[0] * right.Elements[0], left.Elements[1] * right.Elements[1]};
    BOOST_TEST(res.Elements == expectedRes);
}

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyVec2_int_pos_neg, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2i(1, 5);
    auto right = HMM_Vec2i(-3, -4);
    //Act
    auto res = HMM_MultiplyVec2(left, right);
    //Assert
    float expectedRes[2] = {left.Elements[0] * right.Elements[0], left.Elements[1] * right.Elements[1]};
    BOOST_TEST(vector<float>(res.Elements, res.Elements + 2) == vector<float>({left.Elements[0] * right.Elements[0], left.Elements[1] * right.Elements[1]}));
}

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyVec2_float_pos_neg, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2(.5, .2);
    auto right = HMM_Vec2(-2, -10.);
    //Act
    auto res = HMM_MultiplyVec2(left, right);
    //Assert
    float expectedRes[2] = {left.Elements[0] * right.Elements[0], left.Elements[1] * right.Elements[1]};
    BOOST_TEST(res.Elements == expectedRes);
}

BOOST_AUTO_TEST_SUITE_END()
//--------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(multiplication_vec2_by_float)

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyVec2f_vec_x_posfloat, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2(.5, .2);
    float right = 10.;
    //Act
    auto res = HMM_MultiplyVec2f(left, right);
    //Assert
    float expectedRes[2] = {left.Elements[0] * right, left.Elements[1] * right};
    BOOST_TEST(res.Elements == expectedRes);
}

BOOST_AUTO_TEST_SUITE_END()
//--------------------------------------------------------------
BOOST_AUTO_TEST_SUITE(mul_vec2_using_operators)

BOOST_AUTO_TEST_CASE(test_mul_vec2_by_vec2_using_mul_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2(10., 1.);
    auto right = HMM_Vec2(-2, -5.);
    //Act
    auto res = left * right;
    //Assert
    float expectedRes[2] = {left.Elements[0] * right.Elements[0], left.Elements[1] * right.Elements[1]};
    BOOST_TEST(res.Elements == expectedRes);
}

BOOST_AUTO_TEST_CASE(test_mul_vec2_by_float_using_mul_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2(.5, .2);
    float right = 10.;
    //Act
    auto res = left * right;
    //Assert
    BOOST_TEST(vector<float>(res.Elements, res.Elements + 2) == vector<float>({left.Elements[0] * right, left.Elements[1] * right}));
}

BOOST_AUTO_TEST_CASE(test_mul_vec2_by_vec2_using_mul_equal_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2(.5, .2);
    auto right = HMM_Vec2(-2, -5.);
    //Act
    auto res = left;
    res *= right;
    //Assert
    BOOST_TEST(vector<float>(res.Elements, res.Elements + 2) == vector<float>({left.Elements[0] * right.Elements[0], left.Elements[1] * right.Elements[1]}));
}

BOOST_AUTO_TEST_CASE(test_mul_vec2_by_float_using_mul_equal_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = HMM_Vec2(.5, .2);
    float right = 10.;
    //Act
    auto res = left;
    res *= right;
    //Assert
    BOOST_TEST(vector<float>(res.Elements, res.Elements + 2) == vector<float>({left.Elements[0] * right, left.Elements[1] * right}));
}

BOOST_AUTO_TEST_SUITE_END()

//--------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(multiplication_mat4_by_mat4)

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyMat4, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = hmm_mat4();
    float arr[4][4] = {{1, 2, 3, 4},
                       {5, -6, 7, 8},
                       {9, 10, 11, 12},
                       {0, 15, 0, 0}};
    copy(&arr[0][0], &arr[0][0] + 4 * 4, &left.Elements[0][0]);

    //Act
    hmm_mat4 res = HMM_MultiplyMat4(left, left);

    //Assert
    float expMat[4][4] = {{38, 80, 50, 56},
                          {38, 236, 50, 56},
                          {158, 248, 218, 248},
                          {75, -90, 105, 120}};

    BOOST_TEST(res.Elements == expMat);
}

BOOST_AUTO_TEST_SUITE_END()

//--------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(multiplication_mat4_by_floatscalar)

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyMat4f, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = hmm_mat4();
    float arr[4][4] = {{1, 2, 3, 4},
                       {5, -6, 7, 8},
                       {9, 10, 11, 12},
                       {0, 15, 0, 0}};
    copy(&arr[0][0], &arr[0][0] + 4 * 4, &left.Elements[0][0]);

    float right = .5;
    //Act
    hmm_mat4 res = HMM_MultiplyMat4f(left, right);

    //Assert
    float expMat[4][4] = {{arr[0][0] * right, arr[0][1] * right, arr[0][2] * right, arr[0][3] * right},
                          {arr[1][0] * right, arr[1][1] * right, arr[1][2] * right, arr[1][3] * right},
                          {arr[2][0] * right, arr[2][1] * right, arr[2][2] * right, arr[2][3] * right},
                          {arr[3][0] * right, arr[3][1] * right, arr[3][2] * right, arr[3][3] * right}};

    BOOST_TEST(res.Elements == expMat);
}

BOOST_AUTO_TEST_SUITE_END()

//--------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(multiplication_mat4_by_vec4)

BOOST_AUTO_TEST_CASE(test_HMM_MultiplyMat4ByVec4, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = hmm_mat4();
    float arr[4][4] = {{1, 2, 3, 4},
                       {5, -6, 7, 8},
                       {9, 10, 11, 12},
                       {0, 15, 0, 0}};
    copy(&arr[0][0], &arr[0][0] + 4 * 4, &left.Elements[0][0]);

    hmm_vec4 right = HMM_Vec4(.5, 1., 0., -3.);
    //Act
    hmm_vec4 res = HMM_MultiplyMat4ByVec4(left, right);
    //Assert
    float expRes[] = {5.5, -50, 8.5, 10};
    BOOST_TEST(res.Elements == expRes);
}

BOOST_AUTO_TEST_SUITE_END()
//--------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(multiplication_using_operators)

BOOST_AUTO_TEST_CASE(test_mul_mat4_by_mat4_using_mul_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = hmm_mat4();
    float arr[4][4] = {{1, 2, 3, 4},
                       {5, -6, 7, 8},
                       {9, 10, 11, 12},
                       {0, 15, 0, 0}};
    copy(&arr[0][0], &arr[0][0] + 4 * 4, &left.Elements[0][0]);

    //Act
    hmm_mat4 res = left * left;

    //Assert
    float expMat[4][4] = {{38, 80, 50, 56},
                          {38, 236, 50, 56},
                          {158, 248, 218, 248},
                          {75, -90, 105, 120}};

    BOOST_TEST(res.Elements == expMat);
}

BOOST_AUTO_TEST_CASE(test_mul_mat4_by_floatscalar_using_mul_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = hmm_mat4();
    float arr[4][4] = {{1, 2, 3, 4},
                       {5, -6, 7, 8},
                       {9, 10, 11, 12},
                       {0, 15, 0, 0}};
    copy(&arr[0][0], &arr[0][0] + 4 * 4, &left.Elements[0][0]);

    float right = .5;
    //Act
    hmm_mat4 res = left * right;
    //Assert
    float expMat[4][4] = {{arr[0][0] * right, arr[0][1] * right, arr[0][2] * right, arr[0][3] * right},
                          {arr[1][0] * right, arr[1][1] * right, arr[1][2] * right, arr[1][3] * right},
                          {arr[2][0] * right, arr[2][1] * right, arr[2][2] * right, arr[2][3] * right},
                          {arr[3][0] * right, arr[3][1] * right, arr[3][2] * right, arr[3][3] * right}};

    BOOST_TEST(res.Elements == expMat);
}

BOOST_AUTO_TEST_CASE(test_mul_mat4_by_vec4_using_mul_operator, *utf::tolerance(0.00001)) {
    //Arrange
    auto left = hmm_mat4();
    float arr[4][4] = {{1, 2, 3, 4},
                       {5, -6, 7, 8},
                       {9, 10, 11, 12},
                       {0, 15, 0, 0}};
    copy(&arr[0][0], &arr[0][0] + 4 * 4, &left.Elements[0][0]);

    hmm_vec4 right = HMM_Vec4(.5, 1., 0., -3.);
    //Act
    hmm_vec4 res = left * right;
    //Assert
    float epxRes[] = {5.5, -50, 8.5, 10};
    BOOST_TEST(res.Elements == epxRes);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/** \file student_t_distribution.hpp
    \brief Student's t probability distribution */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef __STUDENT_T_DISTRIBUTION_HPP__
#define __STUDENT_T_DISTRIBUTION_HPP__

// for student t distribution
#include <boost/math/distributions/students_t.hpp>
#include "prob_distribution.hpp"

namespace bayesopt
{

  class StudentTDistribution: public ProbabilityDistribution
  {
  public:
    StudentTDistribution(randEngine& eng);
    virtual ~StudentTDistribution();

    /**
     * \brief Sets the mean and std of the distribution
     */
    void setMeanAndStd(double mean, double std)
    { mean_ = mean; std_ = std; };

    /**
     * \brief Sets the degrees of freedom (dof) the distribution
     */
    void setDof(size_t dof)
    {
      dof_ = dof;
      boost::math::students_t new_d(dof);
      d_ = new_d;
    };

    /**
     * \brief Probability density function
     * @param x query point
     * @return probability
     */
    double pdf(double x)
    {
      x = (x - mean_) / std_;
      return boost::math::pdf(d_,x);
    };

    /**
     * \brief Expected Improvement algorithm for minimization
     * @param min  minimum value found
     * @param g exponent (used for annealing)
     *
     * @return negative value of the expected improvement
     */
    double negativeExpectedImprovement(double min, size_t g);

    /**
     * \brief Lower confindence bound. Can be seen as the inverse of the Upper
     * confidence bound
     * @param beta std coefficient (used for annealing)
     * @return value of the lower confidence bound
     */
    double lowerConfidenceBound(double beta);

    /**
     * Probability of improvement algorithm for minimization
     * @param min  minimum value found
     * @param epsilon minimum improvement margin
     *
     * @return negative value of the probability of improvement
     */
    double negativeProbabilityOfImprovement(double min,
					    double epsilon);

    /**
     * Sample outcome acording to the marginal distribution at the query point.
     * @return outcome
     */
    double sample_query();

    double getMean() { return mean_; };
    double getStd()  { return std_; };

  private:
    boost::math::students_t d_;
    double mean_;
    double std_;
    size_t dof_;
  };

} //namespace bayesopt

#endif


#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <iostream>

using namespace boost::geometry;
using namespace std;

int main()
{
	typedef model::d2::point_xy<double> pointtype;
	pointtype point1(3, 3);
	pointtype point2(2, 12.1);

	model::polygon<pointtype> poly;
	pointtype points[] = {pointtype(0, 0), pointtype(0, 10), pointtype(5, 15),
						  pointtype(10, 10), pointtype(10, 0), pointtype(0, 0)};
	append(poly, points);

	cout << wkt(point1) << " is "
		 << (within(point1, poly) ? "inside" : "outside of") << " the "
		 << wkt(poly) << endl;
	cout << wkt(point2) << " is "
		 << (within(point2, poly) ? "inside" : "outside of") << " the "
		 << wkt(poly) << endl;
	return 0;
}


#include <catch2/catch.hpp>

#include <Eigen/Geometry>
#include <finitediff.hpp>
#include <igl/PI.h>

#include <ipc/distance/edge_edge.hpp>

#include <autodiff/autodiff_types.hpp>
#include <geometry/distance.hpp>
#include <logger.hpp>
#include <utils/not_implemented_error.hpp>

using namespace ipc::rigid;

//-----------------------------------------------------------------------------
// Unsigned Distances
//-----------------------------------------------------------------------------

TEST_CASE("Edge-edge distance gradient", "[distance][gradient]")
{
    using namespace ipc::rigid;
    typedef AutodiffType<12> Diff;
    Diff::activate();

    // Generate a geometric space of
    double angle = 0;
    SECTION("Almost parallel")
    {
        double exponent = GENERATE(range(-6, 3));
        angle = pow(10, exponent) * igl::PI / 180.0;
    }
    // SECTION("Parallel") { angle = 0; }

    Diff::D2Vector3d ea0 = Diff::d2vars(0, Eigen::Vector3d(-1.0, 0, 0));
    Diff::D2Vector3d ea1 = Diff::d2vars(3, Eigen::Vector3d(+1.0, 0, 0));
    Diff::D2Vector3d eb0 =
        Diff::d2vars(6, Eigen::Vector3d(cos(angle), 1, sin(angle)));
    Diff::D2Vector3d eb1 = Diff::d2vars(
        9, Eigen::Vector3d(cos(angle + igl::PI), 1, sin(angle + igl::PI)));

    Diff::DDouble2 distance = ipc::edge_edge_distance(ea0, ea1, eb0, eb1);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(12);
    x.segment<3>(0) = Diff::get_value(ea0);
    x.segment<3>(3) = Diff::get_value(ea1);
    x.segment<3>(6) = Diff::get_value(eb0);
    x.segment<3>(9) = Diff::get_value(eb1);
    auto f = [](const Eigen::VectorXd& x) {
        return ipc::edge_edge_distance(
            Eigen::Vector3d(x.segment<3>(0)), Eigen::Vector3d(x.segment<3>(3)),
            Eigen::Vector3d(x.segment<3>(6)), Eigen::Vector3d(x.segment<3>(9)));
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CAPTURE(angle, distance.getGradient().transpose(), fgrad.transpose());
    CHECK(distance.getValue() == Approx(1.0));
    CHECK(fd::compare_gradient(distance.getGradient(), fgrad));
    CHECK(distance.getHessian().squaredNorm() != 0.0);
}

//-----------------------------------------------------------------------------
// Signed Distances
//-----------------------------------------------------------------------------

template <typename T> int sign(T val) { return (T(0) < val) - (val < T(0)); }

TEST_CASE("Point-line signed distance", "[distance]")
{
    double expected_distance = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    Eigen::Vector2d p = Eigen::Vector2d::Random();
    p.y() = expected_distance;
    Eigen::Vector2d s0(-10, 0);
    Eigen::Vector2d s1(-9, 0);

    double distance = point_line_signed_distance(p, s0, s1);
    CAPTURE(distance, expected_distance);
    CHECK(sign(distance) == sign(expected_distance));
}

TEST_CASE("Line-line signed distance", "[distance]")
{
    double expected_distance = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    Eigen::Vector3d line0_point0(-9.9, expected_distance, 0);
    Eigen::Vector3d line0_point1(-10, expected_distance, 0);
    Eigen::Vector3d line1_point0(0, 0, -10);
    Eigen::Vector3d line1_point1(0, 0, -9.9);

    double distance = line_line_signed_distance(
        line0_point0, line0_point1, line1_point0, line1_point1);
    CAPTURE(distance, expected_distance);
    CHECK(sign(distance) == sign(expected_distance));
}

TEST_CASE("Point-plane signed distance", "[distance]")
{
    double expected_distance = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    Eigen::Vector3d p = Eigen::Vector3d::Random();
    p.y() = expected_distance;
    Eigen::Vector3d p0 = Eigen::Vector3d::Random();
    Eigen::Vector3d p1 = Eigen::Vector3d::Random();
    Eigen::Vector3d p2 = Eigen::Vector3d::Random();
    p0.y() = p1.y() = p2.y() = 0;
    if (Eigen::Vector3d::UnitY().dot((p1 - p0).cross(p2 - p0)) < 0) {
        std::swap(p1, p2);
    }

    double distance = point_plane_signed_distance(p, p0, p1, p2);
    CAPTURE(distance, expected_distance);
    CHECK(sign(distance) == sign(expected_distance));
}


#include <itp/core>
#include <fstream>
#include <Eigen/Dense>

double safe_acos(double value)
{
	if (value <= -1.0)
	{
		return itp::pi;
	}
	else if (value >= 1.0)
	{
		return 0;
	}
	else
	{
		return std::acos(value);
	}
}

double calcVolume(double RMOF, double R, double L)
{
	double ret = 0;
	if (R <= RMOF - L)
	{
		ret = 4 * itp::pi * R * R * R / 3;
	}
	else if (R > RMOF + L)
	{
		double hmax = std::sqrt(R * R - (RMOF - L) * (RMOF - L));
		double hmin = std::sqrt(R * R - (RMOF + L) * (RMOF + L));
		double dh = 1e-5;
		double r, alpha, belta, cosAlpha, cosBelta;
		for (double h = hmin; h <= hmax; h += dh)
		{
			r = std::sqrt(R * R - h * h);
			cosAlpha = (r * r + L * L - RMOF * RMOF) / (2.0 * r * L);
			cosBelta = (L * L + RMOF * RMOF - r * r) / (2.0 * L * RMOF);
			alpha = safe_acos(cosAlpha);
			belta = safe_acos(cosBelta);
			ret += dh * (alpha * r * r + belta * RMOF * RMOF - L * RMOF * std::sin(belta));
		}
		ret += itp::pi / 3 * (3 * R - (R - hmax)) * (R - hmax) * (R - hmax);
		ret += hmin * itp::pi * RMOF * RMOF;
		ret *= 2;
	}
	else
	{
		double hmax = std::sqrt(R * R - (RMOF - L) * (RMOF - L));
		double dh = 1e-5;
		double r, alpha, belta, cosAlpha, cosBelta;
		for (double h = 0; h <= hmax; h += dh)
		{
			r = std::sqrt(R * R - h * h);
			cosAlpha = (r * r + L * L - RMOF * RMOF) / (2.0 * r * L);
			cosBelta = (L * L + RMOF * RMOF - r * r) / (2.0 * L * RMOF);
			alpha = safe_acos(cosAlpha);

			belta = safe_acos(cosBelta);

			ret += dh * (alpha * r * r + belta * RMOF * RMOF - L * RMOF * std::sin(belta));
		}
		ret += itp::pi / 3 * (3 * R - (R - hmax)) * (R - hmax) * (R - hmax);
		ret *= 2;
	}
	return ret;
}

int main()
{
	double upPos = 32.927;
	double lowPos = 27.073;

	double RMOF = 0.655;
	double L = 0.4;
	double dL = 0.001;

	int Lbin = RMOF / dL;
	double LMOF = upPos - lowPos;
	double Rmax = LMOF / 2;
	double dR = 0.002; // nm
	int Rbin = Rmax / dR;

	Eigen::ArrayXXd totVolume(Lbin, Rbin);
	totVolume.fill(0);
	fmt::print("{}\n", calcVolume(RMOF, 0.656, 0.001));

}


/*
 * CubicInterpolation.hpp
 *
 *  Created on: Jan 21, 2020
 *      Author: Edo Jelavic
 *      Institute: ETH Zurich, Robotic Systems Lab
 */

#pragma once

#include <Eigen/Core>
#include <vector>
#include <map>
#include "grid_map_core/TypeDefs.hpp"

/*
 * For difference between bicubic convolution interpolation (piecewise cubic)
 * and bicubic interpolation see:
 *
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 *
 * R. Keys (1981). "Cubic convolution interpolation for digital image processing".
 * IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
 *
*   https://web.archive.org/web/20051024202307/
*   http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
 */

namespace grid_map {

class GridMap;

/*
 * Data structure (matrix) that contains data
 * necessary for interpolation. These are either 16
 * function values in the case of bicubic convolution interpolation
 * or function values and their derivatives for the case
 * of standard bicubic inteprolation.
 */
using FunctionValueMatrix = Eigen::Matrix4d;

/*!
 * Takes the id requested, performs checks and returns
 * an id that it is within the specified bounds.
 * @param[in] idReq - input index .
 * @param[in] nElem - number of elements in the container
 * @return index that is within [0, nElem-1].
 */
unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem);

/*!
 * Extract the value of the specific layer at the
 * row and column requested. If row and column are out
 * of bounds they will be bound to range.
 * @param[in] layerMat - matrix of the layer from where
 *                       the data is extracted
 * @param[in] rowReq - row requested
 * @param[in] colReq - column requested
 * @return - value of the layer at rowReq and colReq
 */
double getLayerValue(const Matrix &layerMat, int rowReq, int colReq);

namespace bicubic_conv {

/*!
 * Matrix for cubic interpolation via convolution. Taken from:
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 */
static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix {
    (Eigen::Matrix4d() << 0.0,  2.0,  0.0,  0.0,
                         -1.0,  0.0,  1.0,  0.0,
                          2.0, -5.0,  4.0, -1.0,
                         -1.0,  3.0, -3.0,  1.0).finished() };

/*
 * Index of the middle knot for bicubic inteprolation. This is
 * the function value with subscripts (0,0), i.e. f00 in
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 * In the grid map it corresponds to the grid map point closest to the
 * queried point (in terms of Euclidean distance). Queried point has
 * coordinates (x,y) for at which the interpolation is requested.
 * @param[in]  gridMap - grid map with the data
 * @param[in]  queriedPosition - position for which the interpolated data is requested
 * @param[out] index - indices of the middle knot for the interpolation
 * @return - true if success
 */
bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index);

/*
 * Coordinates used for interpolation need to be shifted and scaled,
 * since the original algorithm operates around the origin and with unit
 * resolution
 * @param[in]  gridMap - grid map with the data
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] position - normalized coordinates of the point for which the interpolation is requested
 * @return - true if success
 */
bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition,
                              Position *position);

/*
 * Queries the grid map for function values at the coordiantes which are neccesary for
 * performing the interpolation. The right function values are then assembled
 * in a matrix.
 * @param[in]  gridMap - grid map with the data
 * @param[in]  layer - name of the layer that we are interpolating
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] data - 4x4 matrix with 16 function values used for interpolation, see
 *           R. Keys (1981). "Cubic convolution interpolation for digital image processing".
 *           IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
 *           for the details.
 * @return - true if success
 */
bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer,
                                 const Position &queriedPosition, FunctionValueMatrix *data);

/*
 * Performs convolution in 1D. the function requires 4 function values
 * to compute the convolution. The result is interpolated data in 1D.
 * @param[in]  t - normalized coordinate (x or y)
 * @param[in]  functionValues - vector of 4 function values neccessary to perform
 *                            interpolation in 1 dimension.
 * @return - interpolated value at normalized coordinate t
 */
double convolve1D(double t, const Eigen::Vector4d &functionValues);

/*
 * Performs convolution in 1D. the function requires 4 function values
 * to compute the convolution. The result is interpolated data in 1D.
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  layer - name of the layer for which we want to perform interpolation
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] interpolatedValue - interpolated value at queried point
 * @return - true if success
 */
bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer,
                                             const Position &queriedPosition,
                                             double *interpolatedValue);

} /* namespace bicubic_conv */

namespace bicubic {

/*
 * Enum for the derivatives direction
 * to perform interpolation one needs
 * derivatives w.r.t. to x and y dimension.
 */
enum class Dim2D: int {
    X,
    Y
};

/*!
 * Matrix for cubic interpolation. Taken from:
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 */
static const Eigen::Matrix4d bicubicInterpolationMatrix {
    (Eigen::Matrix4d() << 1.0,  0.0,  0.0,  0.0,
                          0.0,  0.0,  1.0,  0.0,
                         -3.0,  3.0, -2.0, -1.0,
                          2.0, -2.0,  1.0,  1.0).finished() };

/*
 * Data matrix that can hold function values
 * these can be either function values at requested
 * positions or their derivatives.
 */
struct DataMatrix
{
  double topLeft_ = 0.0;
  double topRight_ = 0.0;
  double bottomLeft_ = 0.0;
  double bottomRight_ = 0.0;
};

/*
 * Interpolation is performed on a unit square.
 * Hence we need to compute 4 corners of that unit square,
 * and find their indices in the grid map. IndicesMatrix
 * is a container that stores those indices. Each index
 * contains two numbers (row number, column number) in the
 * grid map.
 */
struct IndicesMatrix
{
  Index topLeft_ { 0, 0 };
  Index topRight_ { 0, 0 };
  Index bottomLeft_ { 0, 0 };
  Index bottomRight_ { 0, 0 };
};

/*
 * Makes sure that all indices in side the
 * data structure IndicesMatrix are within the
 * range of the grid map.
 * @param[in] gridMap - input grid map with discrete function values
 * @param[in/out] indices - indices that are bound to range, i.e.
 *                          rows and columns are with ranges
 */
void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices);

/*
 * Performs bicubic interpolation at requested position.
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  layer - name of the layer for which we want to perform interpolation
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] interpolatedValue - interpolated value at queried point
 * @return - true if success
 */
bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer,
                                  const Position &queriedPosition, double *interpolatedValue);

/*
 * Deduces which points in the grid map close a unit square around the
 * queried point and returns their indices (row and column number)
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] indicesMatrix - data structure with indices forming a unit square
 *                            around the queried point
 * @return - true if success
 */
bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition,
                                IndicesMatrix *indicesMatrix);

/*
 * Get index (row and column number) of a point in grid map, which
 * is closest to the queried position.
 * @param[in]  gridMap - grid map with discrete function values
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] index - indices of the closest point in grid_map
 * @return - true if success
 */
bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index);

/*
 * Retrieve function values from the grid map at requested indices.
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  indices - indices (row and column numbers) for which function values are requested
 * @param[out] data - requested function values
 * @return - true if success
 */
bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data);

/*
 * Retrieve function derivative values from the grid map at requested indices. Function
 * derivatives are approximated using central difference.
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  indices - indices (row and column numbers) for which function derivative
 *                       values are requested
 * @param[in]  dim - dimension along which we want to evaluate partial derivatives (X or Y)
 * @param[in]  resolution - resolution of the grid map
 * @param[out] derivatives - values of derivatives at requested indices
 * @return - true if success
 */
bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
                              double resolution, DataMatrix *derivatives);

/*
 * Retrieve second order function derivative values from the grid map at requested indices.
 * Function derivatives are approximated using central difference. We compute partial derivative
 * w.r.t to one coordinate and then the other. Note that the order of differentiation
 * does not matter.
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  indices - indices (row and column numbers) for which function derivative
 *                       values are requested
 * @param[in]  resolution - resolution of the grid map
 * @param[out] derivatives - values of second order mixed derivatives at requested indices
 * @return - true if success
 */
bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices,
                                    double resolution, DataMatrix *derivatives);

/*
 * First order derivative for a specific point determined by index.
 * Approximated by central difference.
 * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
 * for details
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  index - index (row and column number) for which function derivative
 *                       value is requested
 * @param[in]  dim - dimension along which we want to evaluate partial derivative (X or Y)
 * @param[in]  resolution - resolution of the grid map
 * @return - value of the derivative at requested index
 */
double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
                              double resolution);

/*
 * Second order mixed derivative for a specific point determined by index.
 * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
 * for details
 * @param[in]  layerData - layer of a grid map with function values
 * @param[in]  index - index (row and column number) for which function derivative
 *                       value is requested
 * @param[in]  resolution - resolution of the grid map
 * @return - value of the second order mixed derivative at requested index
 */
double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution);

/*
 * Evaluate polynomial at requested coordinates. the function will compute the polynomial
 * coefficients and then evaluate it. See
 * https://en.wikipedia.org/wiki/Bicubic_interpolation
 * for details.
 * @param[in]  functionValues - function values and derivatives required to
 *                              compute polynomial coefficients
 * @param[in]  tx - normalized x coordinate for which the interpolation should be computed
 * @param[in]  ty - normalized y coordinate for which the interpolation should be computed
 * @return - interpolated value at requested normalized coordinates.
 */
double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty);

/*
 * Assemble function value matrix from small submatrices containing function values
 * or derivative values at the corners of the unit square.
 * See https://en.wikipedia.org/wiki/Bicubic_interpolation for details.
 *
 * @param[in]  f - Function values at the corners of the unit square
 * @param[in]  dfx - Partial derivative w.r.t to x at the corners of the unit square
 * @param[in]  dfy - Partial derivative w.r.t to y at the corners of the unit square
 * @param[in]  ddfxy - Second order partial derivative w.r.t to x and y at the corners of the unit square
 * @param[out]  functionValues - function values and derivatives required to
 *                              compute polynomial coefficients
 */
void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy,
                                 const DataMatrix &ddfxy, FunctionValueMatrix *functionValues);

/*
 * Coordinates used for interpolation need to be shifter and scaled,
 * since the original algorithm operates on a unit square around the origin.
 * @param[in]  gridMap - grid map with the data
 * @param[in]  originIndex - index of a bottom left corner if the unit square in the grid map
 *                            this corner is the origin for the normalized coordinates.
 * @param[in]  queriedPosition - position for which the interpolation is requested
 * @param[out] position - normalized coordinates of the point for which the interpolation is requested
 * @return - true if success
 */
bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
                                  const Position &queriedPosition, Position *normalizedCoordinates);

} /* namespace bicubic */

} /* namespace grid_map*/


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric
    This function object returns the quadrant aware `atan2` function.

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

    @par Notes

    - For any real arguments @c x and @c y not both equal to zero, <tt>atan2(y, x)</tt>
    (be aware of the parameter order) is the angle in radians between the positive
    x-axis of a plane and the point  given by the coordinates  <tt>(x, y)</tt>.

    - It is also the angle in \f$[-\pi,\pi[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    - Following IEEE norms,  we should have:
     -  If y is \f$\pm0\f$ and x is negative or -0,\f$\pm\pi\f$ is returned
     -  If y is \f$\pm0\f$ and x is positive or +0, \f$\pm0\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is finite, \f$\pm\pi/2\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$-\infty\f$,\f$\pm3\pi/4\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$+\infty\f$, \f$\pm\pi/4\f$ is returned
     -  If x is \f$\pm0\f$ and y is negative, \f$-\pi/2\f$ is returned
     -  If x is \f$\pm0\f$ and y is positive, \f$+\pi/2\f$  is returned
     -  If x is \f$-\infty\f$ and y is finite and positive, \f$+\pi\f$ is returned
     -  If x is \f$-\infty\f$ and y is finite and negative, \f$-\pi\f$ is returned
     -  If x is \f$+\infty\f$ and y is finite and positive, +0 is returned
     -  If x is \f$+\infty\f$ and y is finite and negative, -0 is returned
     -  If either x is Nan or y is Nan, Nan is returned

     The pedantic_ decorator ensures all these conditions, but the regular version
     (no decorator) will return a NaN if x and y are both either null or infinite,
     result which in fact is not more absurd than the IEEE choices.
     It will be conforming in all other cases.

    @par Decorators

    - std_  provides access to std::atan2

    - pedantic_ ensures the respect of all IEEE limits

    @see atan, atand, atanpi

    @par Example:

      @snippet atan2.cpp atan2

    @par Possible output:

      @snippet atan2.txt atan2

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

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

#endif


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

#ifndef RW_MATH_PERSPECTIVETRANSFORM2D_HPP
#define RW_MATH_PERSPECTIVETRANSFORM2D_HPP

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

#include <rw/common/Serializable.hpp>
#include <rw/core/macros.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /**
     * @brief The PerspectiveTransform2D is a perspective transform in 2D.
     *
     * The homographic transform can be used to map one arbitrary 2D
     * quadrilateral into another.
     */
    template< class T = double > class PerspectiveTransform2D
    {
      private:
        //! Eigen 3x3 matrix used as internal data structure.
        typedef Eigen::Matrix< T, 3, 3 > EigenMatrix3x3;

      public:
        /**
         * @brief constructor
         */
        PerspectiveTransform2D ()    //: _matrix(3,3)
        {
            _matrix (0, 0) = 1;
            _matrix (0, 1) = 0;
            _matrix (0, 2) = 0;
            _matrix (1, 0) = 0;
            _matrix (1, 1) = 1;
            _matrix (1, 2) = 0;
            _matrix (2, 0) = 0;
            _matrix (2, 1) = 0;
            _matrix (2, 2) = 1;
        }

        /**
         * @brief constructor
         */
        PerspectiveTransform2D (T r11, T r12, T r13, T r21, T r22, T r23, T r31, T r32,
                                T r33)    //: _matrix(3,3)
        {
            _matrix (0, 0) = r11;
            _matrix (0, 1) = r12;
            _matrix (0, 2) = r13;
            _matrix (1, 0) = r21;
            _matrix (1, 1) = r22;
            _matrix (1, 2) = r23;
            _matrix (2, 0) = r31;
            _matrix (2, 1) = r32;
            _matrix (2, 2) = r33;
        }

        /**
         * @brief constructor
         * @param r
         * @return
         */
        template< class R > explicit PerspectiveTransform2D (const Eigen::Matrix< R, 3, 3 >& r)
        //: _matrix(r)
        {
            for (size_t i = 0; i < 3; i++)
                for (size_t j = 0; j < 3; j++)
                    _matrix (i, j) = r (i, j);
        }

        /**
         * @brief constructor
         * @param r
         * @return
         */
        template< class R > explicit PerspectiveTransform2D (const Eigen::MatrixBase< R >& r)
        //: _matrix(r)
        {
            RW_ASSERT (r.rows () == 3);
            RW_ASSERT (r.cols () == 3);
            for (size_t i = 0; i < 3; i++)
                for (size_t j = 0; j < 3; j++)
                    _matrix (i, j) = r.row (i) (j);
        }

        /**
         * @brief calculates a PerspectiveTransform2D that maps points from point
         * set pts1 to point set pts2
         * @param pts1 [in] point set one
         * @param pts2 [in] point set two
         */
        static PerspectiveTransform2D< T >
        calcTransform (std::vector< rw::math::Vector2D< T > > pts1,
                       std::vector< rw::math::Vector2D< T > > pts2);

        /**
         * @brief Returns the inverse of the PerspectiveTransform
         */
        PerspectiveTransform2D< T > inverse () const
        {
            return PerspectiveTransform2D< T > (_matrix.transpose ());
        }
#if !defined(SWIG)
        /**
         * @brief Returns matrix element reference
         * @param row [in] row, row must be @f$ < 3 @f$
         * @param col [in] col, col must be @f$ < 3 @f$
         * @return reference to matrix element
         */
        T& operator() (std::size_t row, std::size_t col)
        {
            assert (row < 3);
            assert (col < 3);
            return _matrix (row, col);
        }

        /**
         * @brief Returns const matrix element reference
         * @param row [in] row, row must be @f$ < 3 @f$
         * @param col [in] col, col must be @f$ < 3 @f$
         * @return const reference to matrix element
         */
        const T& operator() (std::size_t row, std::size_t col) const
        {
            assert (row < 3);
            assert (col < 3);
            return _matrix (row, col);
        }
#else
        MATRIXOPERATOR (T);
#endif

        /**
         * @brief transform a point using this perspective transform
         */
        rw::math::Vector2D< T > operator* (const rw::math::Vector2D< T >& v) const
        {
            const T x = v (0);
            const T y = v (1);

            const T g      = (*this) (2, 0);
            const T h      = (*this) (2, 1);
            const T one    = static_cast< T > (1);
            const T lenInv = one / (g * x + h * y + one);

            const T a = (*this) (0, 0);
            const T b = (*this) (0, 1);
            const T c = (*this) (0, 2);

            const T d = (*this) (1, 0);
            const T e = (*this) (1, 1);
            const T f = (*this) (1, 2);

            return rw::math::Vector2D< T > ((a * x + b * y + c) * lenInv,
                                            (d * x + e * y + f) * lenInv);
        }

        /**
         * @brief transform a 2d point into a 3d point with this
         * perspective transform
         * @param hT
         * @param v
         * @return
         */
        rw::math::Vector3D< T > calc3dVec (const PerspectiveTransform2D< T >& hT,
                                           const rw::math::Vector2D< T >& v)
        {
            const T x = v (0);
            const T y = v (1);

            const T g   = hT (2, 0);
            const T h   = hT (2, 1);
            const T one = static_cast< T > (1);
            // const T lenInv = one / (g * x + h * y + one);
            const T len = (g * x + h * y + one);

            const T a = hT (0, 0);
            const T b = hT (0, 1);
            const T c = hT (0, 2);

            const T d = hT (1, 0);
            const T e = hT (1, 1);
            const T f = hT (1, 2);

            return rw::math::Vector3D< T > ((a * x + b * y + c), (d * x + e * y + f), len);
        }

        /**
         * @brief Returns reference to the 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        const Eigen::Matrix< T, 3, 3 >& e () const { return _matrix; }

        /**
         * @brief Returns reference to the 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        Eigen::Matrix< T, 3, 3 >& e () { return _matrix; }

      private:
        EigenMatrix3x3 _matrix;
    };

    /**
     * @brief Take the inverse of a PerspectiveTransform2D.
     * @param aRb [in] a PerspectiveTransform2D.
     * @return the inverse of \b aRb .
     * @relates rw::math::PerspectiveTransform2D
     */
    template< class T > PerspectiveTransform2D< T > inverse (const PerspectiveTransform2D< T >& aRb)
    {
        return aRb.inverse ();
        // return PerspectiveTransform2D<T>(trans(aRb.m()));
    }
#if !defined(SWIG)
    extern template class rw::math::PerspectiveTransform2D< double >;
    extern template class rw::math::PerspectiveTransform2D< float >;
#else
    SWIG_DECLARE_TEMPLATE (PerspectiveTransform2Dd, rw::math::PerspectiveTransform2D< double >);
    SWIG_DECLARE_TEMPLATE (PerspectiveTransform2Df, rw::math::PerspectiveTransform2D< float >);
#endif

    using PerspectiveTransform2Dd = PerspectiveTransform2D< double >;
    using PerspectiveTransform2Df = PerspectiveTransform2D< float >;

}}    // namespace rw::math

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

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

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

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

#endif    // end include guard


/*
 * Copyright 2017 Maeve Automation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */
#include <gtest/gtest.h>

#include <Eigen/Core>

#include <cmath>
#include <tuple>

#include "open_maeve/maeve_geometry/tau.h"

namespace open_maeve {
namespace {
constexpr auto EPS = 1e-4;
constexpr auto INF = std::numeric_limits<double>::infinity();

//------------------------------------------------------------------------------

double extent(const double Z, const Eigen::Vector2d& p1,
              const Eigen::Vector2d& p2) {
  const auto d = (p1 - p2).norm();
  return d / Z;
}

//------------------------------------------------------------------------------

}  // namespace

//------------------------------------------------------------------------------

TEST(Tau, tau1) {
  {
    const auto range = 0.0;
    const auto relative_speed = 0.0;
    const auto expected_tau = 0.0;
    const auto computed_tau = tau(range, relative_speed, EPS);
    EXPECT_EQ(computed_tau, expected_tau);
  }

  {
    const auto range = 1.0;
    const auto relative_speed = 1.0;
    const auto expected_tau = 1.0;
    const auto computed_tau = tau(range, relative_speed, EPS);
    EXPECT_EQ(computed_tau, expected_tau);
  }

  {
    const auto range = 1.0;
    const auto relative_speed = -1.0;
    const auto expected_tau = -1.0;
    const auto computed_tau = tau(range, relative_speed, EPS);
    EXPECT_EQ(computed_tau, expected_tau);
  }

  {
    const auto range = 1.0;
    const auto relative_speed = 0.0;
    const auto expected_tau = INF;
    const auto computed_tau = tau(range, relative_speed, EPS);
    EXPECT_EQ(computed_tau, expected_tau);
  }

  {
    const auto range = 1.0;
    const auto relative_speed = (0.5 * EPS);
    const auto expected_tau = INF;
    const auto computed_tau = tau(range, relative_speed, EPS);
    EXPECT_EQ(computed_tau, expected_tau);
  }

  {
    const auto range = 1.0;
    const auto relative_speed = (2.0 * EPS);
    const auto computed_tau = tau(range, relative_speed, EPS);
    EXPECT_TRUE(std::isfinite(computed_tau));
  }
}

//------------------------------------------------------------------------------

TEST(Tau, verifyScaling) {
  const auto Z = 17.0;
  const auto Z_dot = -1.2;
  const Eigen::Vector2d P1(2.3, 3.13);
  const Eigen::Vector2d P2(5.67, -1.32);

  auto t_delta = 10.37;
  auto tau = -(Z + Z_dot * t_delta) / Z_dot;
  const auto e1 = extent(Z, P1, P2);
  const auto e2 = extent(Z + Z_dot * t_delta, P1, P2);
  const auto e_dot = (e2 - e1) / t_delta;
  const auto tau_estimated = tauFromDiscreteScaleDt(e2, e_dot, t_delta, EPS);
  EXPECT_NEAR(tau_estimated, tau, 0.0001)
      << "e1: " << e1 << ", e2: " << e2 << ", e_dot: " << e_dot;
}

//------------------------------------------------------------------------------

}  // namespace open_maeve


#include "Math.h"
#include "../utils/GapsAssert.h"

#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/normal.hpp>

#define GAPS_SQ(x) ((x) * (x))

#define Q_GAMMA_THRESHOLD 0.000001f
#define Q_GAMMA_MIN_VALUE 0.f

float gaps::min(float a, float b)
{
    return a < b ? a : b;
}

unsigned gaps::min(unsigned a, unsigned b)
{
    return a < b ? a : b;
}

uint64_t gaps::min(uint64_t a, uint64_t b)
{
    return a < b ? a : b;
}

float gaps::max(float a, float b)
{
    return a < b ? b : a;
}

unsigned gaps::max(unsigned a, unsigned b)
{
    return a < b ? b : a;
}

uint64_t gaps::max(uint64_t a, uint64_t b)
{
    return a < b ? b : a;
}

float gaps::d_gamma(float d, float shape, float scale)
{
    boost::math::gamma_distribution<> gam(shape, scale);
    return pdf(gam, d);
}

float gaps::p_gamma(float p, float shape, float scale)
{
    boost::math::gamma_distribution<> gam(shape, scale);
    return cdf(gam, p);
}

float gaps::q_gamma(float q, float shape, float scale)
{
    if (q < Q_GAMMA_THRESHOLD)
    {
        return Q_GAMMA_MIN_VALUE;
    }
    boost::math::gamma_distribution<> gam(shape, scale);
    return quantile(gam, q);
}

float gaps::d_norm(float d, float mean, float sd)
{
    boost::math::normal_distribution<> norm(mean, sd);
    return pdf(norm, d);
}

float gaps::q_norm(float q, float mean, float sd)
{
    boost::math::normal_distribution<> norm(mean, sd);
    return quantile(norm, q);
}

float gaps::p_norm(float p, float mean, float sd)
{
    boost::math::normal_distribution<> norm(mean, sd);
    return cdf(norm, p);
}

double gaps::lgamma(double x)
{
    return boost::math::lgamma(x); // NOLINT
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

//[point_order
//` Examine the expected point order of a polygon type

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type, false> polygon_type;

    boost::geometry::order_selector order = boost::geometry::point_order<polygon_type>::value;

    std::cout << "order: " << order << std::endl
        << "(clockwise = " << boost::geometry::clockwise
        << ", counterclockwise = " << boost::geometry::counterclockwise
        << ") "<< std::endl;

    return 0;
}

//]

//[point_order_output
/*`
Output:
[pre
order: 2
(clockwise = 1, counterclockwise = 2)
]
*/
//]


#define BOOST_TEST_MODULE "test_LUDecomposition"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "test_Defs.hpp"
using ax::test::tolerance;
using ax::test::seed;

#include <random>

#include "../src/LUDecomposition.hpp"

BOOST_AUTO_TEST_CASE(LUDecomposition)
{
    ax::Matrix<double, 4,4> mat;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    std::array<std::array<double, 4>, 4> rand1;
    for(std::size_t i=0; i<4; ++i)
        for(std::size_t j=0; j<4; ++j)
            mat(i,j) = rand1[i][j] = randreal(mt);

    const auto LUpair = ax::LUdecompose<ax::Doolittle>(mat);

    const ax::Matrix<double, 4,4> L = LUpair.first;
    const ax::Matrix<double, 4,4> U = LUpair.second;
    const ax::Matrix<double, 4,4> A = L * U;

    // ============= test for L =============
    for(std::size_t i = 0; i<4; ++i)
        BOOST_CHECK_EQUAL(L(i,i), 1e0);

    for(std::size_t i = 0; i<4; ++i)
        for(std::size_t j = i+1; j<4; ++j)
        {
            BOOST_CHECK_EQUAL(L(i,j), 0e0);
        }

    // ============= test for U =============
    for(std::size_t i = 0; i<4; ++i)
        for(std::size_t j = 0; j<i; ++j)
        {
            BOOST_CHECK_EQUAL(U(i,j), 0e0);
        }

    for(std::size_t i=0; i<4; ++i)
        for(std::size_t j=0; j<4; ++j)
            BOOST_CHECK_CLOSE(A(i,j), mat(i,j), tolerance);
}


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

TEST(MathFunctions, cbrt) {
  using stan::math::cbrt;
  EXPECT_FLOAT_EQ(-2.0, cbrt(-8.0));
  EXPECT_FLOAT_EQ(-1.392476650083834, cbrt(-2.7));
  EXPECT_FLOAT_EQ(0, cbrt(0));
  EXPECT_FLOAT_EQ(2.0, cbrt(8.0));

}

TEST(MathFunctions, cbrt_inf_return) {
  EXPECT_EQ(-std::numeric_limits<double>::infinity(),
            stan::math::cbrt(-std::numeric_limits<double>::infinity()));
  EXPECT_EQ(std::numeric_limits<double>::infinity(),
            stan::math::cbrt(std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, cbrt_nan) {
  using stan::math::cbrt;
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::cbrt(std::numeric_limits<double>::quiet_NaN()));
}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <vector>
#include <array>
#include <map>
#include <random>
#include <cassert>
#include <fstream>
#include <Eigen/Dense>
#include "StrategyN2M3.hpp"

// memory-1 species in strategy space discretized with `D`
// memory-1 strategy is characterized by tuple (p_{cc}, p_{cd}, p_{dc}, p_{dd}),
// where each of which denotes cooperation probability conditioned by the last Alice's action and the other players' defectors.
// Each of which can take discrete values [0,1/D,2/D,...,D/D].
// ID of a species is given by an integer with base-(D+1).
// ID can take values [0, (D+1)^4-1]
// ID=0 : AllD, ID=ID_MAX-1 : AllC
class Cprobs {
 public:
  Cprobs(size_t id, size_t DIS) {
    double dinv = 1.0 / DIS;
    const size_t B = DIS+1;
    cc = (id % B) * dinv;
    cd = ((id/B) % B) * dinv; // A: c, B: d
    dc = ((id/B/B) % B) * dinv; // A: d, B: c
    dd = ((id/B/B/B) % B) * dinv;
  }
  double cc, cd, dc, dd;
};

class Mem1Species {
 public:
  static size_t N_M1_Species(size_t DIS) { return (DIS+1)*(DIS+1)*(DIS+1)*(DIS+1); }
  Mem1Species(size_t _id, size_t _DIS) : id(_id), DIS(_DIS), prob(_id, _DIS) {
    assert(id <= N_M1_Species(DIS));
  }
  size_t id;
  size_t DIS;
  Cprobs prob;

  std::string ToString() const {
    const size_t B = DIS+1;
    size_t cc = (id % B);
    size_t cd = ((id/B) % B);
    size_t dc = ((id/B/B) % B);
    size_t dd = ((id/B/B/B) % B);
    std::ostringstream oss;
    oss << cc << '-' << cd << '-' << dc << '-' << dd << '/' << DIS;
    if (IsDefensible()) { oss << "_D"; }
    if (IsEfficient()) { oss << "_E"; }
    return oss.str();
  }

  std::vector<double> StationaryState(const Mem1Species& Bstr, double error = 0.0) const {
    Eigen::Matrix<double,4,4> A;

    // state 0: cc, state 1: cd (lower bit is A's history), ... 3: dd
    // calculate transition probability from j to i

    for (size_t j = 0; j < 4; j++) {
      Action last_a = (j & 1ul) ? D : C;
      Action last_b = ((j>>1ul) & 1ul) ? D : C;

      auto cooperation_prob = [](Action a, Action b, const Mem1Species& str) -> double {
        if (a == C) {
          if (b == C) { return str.prob.cc; }
          else { return str.prob.cd; }
        }
        else {
          if (b == C) { return str.prob.dc; }
          else { return str.prob.dd; }
        }
      };
      double c_A = cooperation_prob(last_a, last_b, *this);
      double c_B = cooperation_prob(last_b, last_a, Bstr);

      c_A = (1.0 - error) * c_A + error * (1.0 - c_A);
      c_B = (1.0 - error) * c_B + error * (1.0 - c_B);
      A(0,j) = c_A * c_B;
      A(1,j) = (1.0-c_A) * c_B;
      A(2,j) = c_A * (1.0-c_B);
      A(3,j) = (1.0-c_A) * (1.0-c_B);
    }

    for(int i=0; i<4; i++) {
      A(i,i) -= 1.0;
    }
    for(int i=0; i<4; i++) {
      A(4-1,i) += 1.0;  // normalization condition
    }
    Eigen::VectorXd b(4);
    for(int i=0; i<4; i++) { b(i) = 0.0;}
    b(4-1) = 1.0;
    Eigen::VectorXd x = A.colPivHouseholderQr().solve(b);
    std::vector<double> ans(4, 0.0);
    for(int i=0; i<ans.size(); i++) {
      ans[i] = x(i);
    }

    return ans;
  }

  double CooperationProb(StateN2M3 s) const {
    if (s.a_1 == C) {
      if (s.b_1 == C) { return prob.cc; }
      else { return prob.cd; }
    }
    else {
      if (s.b_1 == C) { return prob.dc; }
      else { return prob.dd; }
    }
  }

  // returns false for mixed strategy
  bool IsDefensible() const {
    double tolerance = 1.0e-8;
    if (prob.cc > tolerance && prob.cc < 1.0 - tolerance) { return false; }
    if (prob.cd > tolerance && prob.cd < 1.0 - tolerance) { return false; }
    if (prob.dc > tolerance && prob.dc < 1.0 - tolerance) { return false; }
    if (prob.dd > tolerance && prob.dd < 1.0 - tolerance) { return false; }

    // N = 2^{2}
    const size_t N = 4;
    typedef std::array<std::array<int, N>, N> d_matrix_t;
    d_matrix_t d;

    // construct adjacency matrix
    const int INF = N; // N is large enough since the path length is between -N/4 to N/4.
    for (size_t i = 0; i < N; i++) {
      for (size_t j = 0; j < N; j++) {
        d[i][j] = INF;
      }
    }

    for (size_t i = 0; i < N; i++) {
      Action a0 = (i & 1) ? D : C; // current state
      Action b0 = (i & 2) ? D : C;
      Action act_a = C;
      if (a0 == C) {
        if (b0 == C) { act_a = (prob.cc > 0.5 ? C : D); }
        else { act_a = (prob.cd > 0.5 ? C : D); }
      }
      else {
        if (b0 == C) { act_a = (prob.dc > 0.5 ? C : D); }
        else { act_a = (prob.dd > 0.5 ? C : D); }
      }

      // Get possible next states
      std::array<Action, 2> act_bs = {C, D};
      for (auto act_b: act_bs) {
        size_t j = 0;
        if (act_a == D) { j += 1; }
        if (act_b == D) { j += 2; }
        if (j == 0 || j == 3) d[i][j] = 0;
        else if (j == 1) d[i][j] = 1;
        else if (j == 2) d[i][j] = -1;
      }
      if (d[i][i] < 0) { return false; }
    }

    for (size_t k = 0; k < N; k++) {
      for (size_t i = 0; i < N; i++) {
        for (size_t j = 0; j < N; j++) {
          d[i][j] = std::min(d[i][j], d[i][k] + d[k][j]);
        }
        if (d[i][i] < 0) { return false; }
      }
    }
    return true;
  }

  bool IsEfficient() const {
    auto ss = StationaryState(*this, 0.0001);
    return (ss[0] > 0.99);
  }
};

class Species { // either Mem1Species or StrategyN3M5
 public:
  Species(size_t ID, size_t DIS) : m1(Mem1Species(0, DIS)), m5(0ull){
    const size_t N_M1 = Mem1Species::N_M1_Species(DIS);
    if (ID < N_M1) {
      is_m1 = true;
      m1 = Mem1Species(ID, DIS);
      m5 = StrategyN2M3(0ull);
      name = m1.ToString();
    }
    else if (ID == N_M1) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::CAPRI2();
      name = "CAPRI2";
    }
    else if (ID == N_M1 + 1) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::CAPRI();
      name = "CAPRI";
    }
    else if (ID == N_M1 + 2) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::TFT_ATFT();
      name = "TFT_ATFT";
    }
    else if (ID == N_M1 + 3) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::AON(2);
      name = "AON2";
    }
    else if (ID == N_M1 + 4) {
      is_m1 = false;
      m1 = Mem1Species(0, DIS);
      m5 = StrategyN2M3::AON(3);
      name = "AON3";
    }
    else {
      throw std::runtime_error("must not happen");
    }
  };
  bool is_m1;
  Mem1Species m1;
  StrategyN2M3 m5;
  std::string name;
  std::string ToString() const { return name; }
  std::vector<double> StationaryState(const Species &sb, double error) const {
    if (is_m1 && sb.is_m1) {
      return m1.StationaryState(sb.m1, error);
    }
    std::cerr << "calculating stationary state: " << name << ' ' << sb.name << std::endl;

    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletVec;

    for (size_t j = 0; j < 64; j++) {
      // calculate transition probability from j to i
      const StateN2M3 sj(j);
      double c_a = _CooperationProb(sj);
      double c_b = sb._CooperationProb(sj.SwapAB());
      // cooperation probability taking noise into account
      c_a = (1.0 - error) * c_a + error * (1.0 - c_a);
      c_b = (1.0 - error) * c_b + error * (1.0 - c_b);

      for (size_t t = 0; t < 4; t++) {
        Action act_a = (t & 1ul) ? D : C;
        Action act_b = (t & 2ul) ? D : C;
        size_t i = sj.NextState(act_a, act_b).ID();
        double p_a = (act_a == C) ? c_a : (1.0-c_a);
        double p_b = (act_b == C) ? c_b : (1.0-c_b);
        tripletVec.emplace_back(i, j, p_a * p_b);
      }
    }

    const size_t S = 64;
    Eigen::SparseMatrix<double> A(S, S);
    A.setFromTriplets(tripletVec.cbegin(), tripletVec.cend());

    // subtract unit matrix & normalization condition
    std::vector<T> iVec;
    for (int i = 0; i < S-1; i++) { iVec.emplace_back(i, i, -1.0); }
    for (int i = 0; i < S-1; i++) { iVec.emplace_back(S-1, i, 1.0); }
    Eigen::SparseMatrix<double> I(S, S);
    I.setFromTriplets(iVec.cbegin(), iVec.cend());
    A = A + I;
    // std::cerr << "  transition matrix has been created" << std::endl;

    Eigen::VectorXd b = Eigen::VectorXd::Zero(S);
    b(S-1) = 1.0;

    Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double> > solver;
    solver.compute(A);
    Eigen::VectorXd x = solver.solve(b);

    // std::cerr << "#iterations:     " << solver.iterations() << std::endl;
    // std::cerr << "estimated error: " << solver.error() << std::endl;

    std::vector<double> ans(S, 0.0);
    for (int i = 0; i < S; i++) { ans[i] = x[i]; }
    return ans;
  }
 private:
  double _CooperationProb(const StateN2M3 &s) const {
    if (is_m1) { return m1.CooperationProb(s); }
    else { return m5.ActionAt(s) == C ? 1.0 : 0.0; }
  }

 public:
  static std::vector<Species> Memory1Species(size_t discrete_level) {
    std::vector<Species> ans;
    size_t n = Mem1Species::N_M1_Species(discrete_level);
    for (size_t i = 0; i < n; i++) {
      ans.emplace_back(i, discrete_level);
    }
    return std::move(ans);
  }
  static std::vector<Species> ReactiveMem1Species(size_t discrete_level) {
    std::vector<Species> ans;
    size_t B = discrete_level + 1;
    size_t n = Mem1Species::N_M1_Species(discrete_level);
    size_t B2 = B * B;
    for (size_t i = 0; i < n/B2; i++) {
      size_t id = i * B2 + i;
      ans.emplace_back(id, discrete_level);
    }
    return std::move(ans);
  }
};

class Ecosystem {
 public:
  Ecosystem(const std::vector<Species> &species_pool, double error) :pool(species_pool), N_SPECIES(species_pool.size()), e(error) {
    CalculateSSCache();
  };
  size_t N_SPECIES;
  std::vector<Species> pool;
  const double e;
  typedef std::vector<double> ss_cache_t;
  std::vector<std::vector<ss_cache_t> > ss_cache;
  // ss_cache[i][i] stores the stationary state when PG game is played by (i,i)
  // ss_cache[i][j] stores the stationary state when PG game is played by (i,j)

  void CalculateSSCache() {
    ss_cache.resize(N_SPECIES);
    for (size_t i = 0; i < N_SPECIES; i++) {
      ss_cache[i].resize(N_SPECIES);
    }

#pragma omp parallel for schedule(dynamic,1)
    for (size_t I=0; I < N_SPECIES * N_SPECIES; I++) {
      size_t i = I / N_SPECIES;
      size_t j = I % N_SPECIES;
      ss_cache[i][j] = pool[i].StationaryState(pool[j], e);
    }
  }

  // payoff of species i and j when the game is played by (i,j)
  std::array<double,2> PayoffVersus(size_t i, size_t j, double benefit, double cost) const {
    return Payoffs(ss_cache[i][j], benefit, cost);
  }

  std::array<double,2> Payoffs(const ss_cache_t &ss, double benefit, double cost) const {
    std::array<double, 2> ans = {0.0, 0.0};
    if (ss.size() == 4) {
      for (size_t i = 0; i < 4; i++) {
        double pa = 0.0, pb = 0.0;
        if ((i & 1ul) == 0) {
          pa -= cost;
          pb += benefit;
        }
        if ((i & 2ul) == 0) {
          pb -= cost;
          pa += benefit;
        }
        ans[0] += ss[i] * pa;
        ans[1] += ss[i] * pb;
      }
    }
    else {
      assert(ss.size() == 64);
      for (size_t i = 0; i < 64; i++) {
        StateN2M3 s(i);
        double pa = 0.0, pb = 0.0;
        if (s.a_1 == C) {
          pa -= cost;
          pb += benefit;
        }
        if (s.b_1 == C) {
          pb -= cost;
          pa += benefit;
        }
        ans[0] += ss[i] * pa;
        ans[1] += ss[i] * pb;
      }
    }
    return ans;
  }

  // calculate the equilibrium distribution exactly by linear algebra
  std::vector<double> CalculateEquilibrium(double benefit, double cost, uint64_t N, double sigma) const {
    Eigen::MatrixXd A(N_SPECIES, N_SPECIES);
    #pragma omp parallel for
    for (size_t ii = 0; ii < N_SPECIES * N_SPECIES; ii++) {
      size_t i = ii / N_SPECIES;
      size_t j = ii % N_SPECIES;
      if (i == j) { A(i, j) = 0.0; continue; }
      double p = FixationProb(benefit, cost, N, sigma, i, j);
      // std::cerr << "Fixation prob of mutant (mutant,resident): " << p << " (" << pool[i].ToString() << ", " << pool[j].ToString() << ")" << std::endl;
      A(i, j) = p * (1.0 / N_SPECIES);
    }

    for (size_t j = 0; j < N_SPECIES; j++) {
      double p_sum = 0.0;
      for (size_t i = 0; i < N_SPECIES; i++) {
        p_sum += A(i, j);
      }
      assert(p_sum <= 1.0);
      A(j, j) = 1.0 - p_sum; // probability that the state doesn't change
    }

    size_t n_row = A.rows();

    // subtract Ax = x => (A-I)x = 0
    for (size_t i = 0; i < A.rows(); i++) {
      A(i, i) -= 1.0;
    }
    // normalization condition
    for (size_t i = 0; i < A.rows(); i++) {
      A(A.rows()-1, i) += 1.0;
    }

    Eigen::VectorXd b(A.rows());
    for(int i=0; i<A.rows()-1; i++) { b(i) = 0.0;}
    b(A.rows()-1) = 1.0;
    Eigen::VectorXd x = A.householderQr().solve(b);
    std::vector<double> ans(A.rows());
    double prob_total = 0.0;
    for(int i=0; i<ans.size(); i++) {
      ans[i] = x(i);
      prob_total += x(i);
      assert(x(i) > -0.000001);
    }
    assert(std::abs(prob_total - 1.0) < 0.00001);
    return ans;
  }

  double FixationProb(double benefit, double cost, uint64_t N, double sigma, size_t mutant_idx, size_t resident_idx) const {
    // \frac{1}{\rho} = \sum_{i=0}^{N-1} \exp\left( \sigma \sum_{j=1}^{i} \left[(N-j-1)s_{yy} + js_{yx} - (N-j)s_{xy} - (j-1)s_{xx} \right] \right) \\
    //                = \sum_{i=0}^{N-1} \exp\left( \frac{\sigma i}{2} \left[(-i+2N-3)s_{yy} + (i+1)s_{yx} - (-i+2N-1)s_{xy} - (i-1)s_{xx} \right] \right)

    double s_xx = PayoffVersus(mutant_idx, mutant_idx, benefit, cost)[0];
    double s_yy = PayoffVersus(resident_idx, resident_idx, benefit, cost)[0];
    auto xy = PayoffVersus(mutant_idx, resident_idx, benefit, cost);
    double s_xy = xy[0];
    double s_yx = xy[1];

    double num_games = (N-1);
    s_xx /= num_games;
    s_yy /= num_games;
    s_xy /= num_games;
    s_yx /= num_games;
    double rho_inv = 0.0;
    for (int i=0; i < N; i++) {
      double x = sigma * i * 0.5 * (
          (2*N-3-i) * s_yy
          + (i+1) * s_yx
          - (2*N-1-i) * s_xy
          - (i-1) * s_xx
      );
      rho_inv += std::exp(x);
    }
    return 1.0 / rho_inv;
  }

  std::vector<std::string> SpeciesNames() const {
    std::vector<std::string> ans;
    for(auto s: pool) {
      ans.emplace_back( s.ToString() );
    }
    return std::move(ans);
  }
  double CooperationLevelSpecies(size_t i) const {
    const ss_cache_t &ss = ss_cache[i][i];
    if (ss.size() == 4) {
      double level = 0.0;
      for (size_t s = 0; s < 4; s++) {
        size_t num_c = 2 - std::bitset<2>(s).count();
        level += ss[s] * (num_c / 2.0);
      };
      return level;
    }
    else {
      double level = 0.0;
      for (size_t s = 0; s < 64; s++) {
        StateN2M3 state(s);
        size_t num_c = 2ul;
        if (state.a_1 == D) num_c -= 1;
        if (state.b_1 == D) num_c -= 1;
        level += ss[s] * (num_c / 2.0);
      }
      return level;
    }
  }
  double CooperationLevel(const std::vector<double> &eq_rate) const {
    assert(eq_rate.size() == N_SPECIES);
    double ans = 0.0;
    for (size_t i = 0; i < N_SPECIES; i++) {
      double c_lev = CooperationLevelSpecies(i);
      ans += eq_rate[i] * c_lev;
    }
    return ans;
  }
};

int main(int argc, char *argv[]) {
  Eigen::initParallel();
  if( argc != 11 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <Nmax> <sigma> <error rate> <discrete_level> <0:reactive/1:full memory1> <1:capri2> <1:capri> <1:aon2> <1:aon3> <1:tft_atft>" << std::endl;
    return 1;
  }

  if (false)  // debugging
  {
    Species wsls(0b1001, 1);
    Species cccd(0b0111, 1);
    Species capri2(0b10000, 1);
    std::cerr << wsls.ToString() << std::endl;
    std::cerr << capri2.ToString() << std::endl;
    auto x = capri2.StationaryState(wsls, 0.0001);
    std::cerr << x[0] << std::endl;
    // std::vector<Species> pool = Species::Memory1Species(1);
    std::vector<Species> pool;
    pool.push_back(wsls);
    pool.push_back(capri2);
    pool.push_back(cccd);
    Ecosystem eco(pool, 0.0001);
    double p01 = eco.FixationProb(4.0, 1.0, 64, 1.0, 0, 1);
    double p10 = eco.FixationProb(4.0, 1.0, 64, 1.0, 1, 0);
    double p02 = eco.FixationProb(4.0, 1.0, 64, 1.0, 0, 2);
    double p20 = eco.FixationProb(4.0, 1.0, 64, 1.0, 2, 0);
    double p12 = eco.FixationProb(4.0, 1.0, 64, 1.0, 1, 2);
    double p21 = eco.FixationProb(4.0, 1.0, 64, 1.0, 2, 1);
    auto eq = eco.CalculateEquilibrium(4.0, 1.0, 64, 1.0);
    std::cerr << cccd.ToString() << std::endl;
    std::cerr << ' ' << p02 << std::endl;
  }

  double cost = 1.0;
  uint64_t Nmax = std::strtoull(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  uint64_t discrete_level = std::strtoull(argv[4], nullptr,0);

  unsigned long full_or_reactive = std::strtoul(argv[5], nullptr, 0);
  unsigned long add_capri2 = std::strtoul(argv[6], nullptr, 0);
  unsigned long add_capri = std::strtoul(argv[7], nullptr, 0);
  unsigned long add_aon2 = std::strtoul(argv[8], nullptr, 0);
  unsigned long add_aon3 = std::strtoul(argv[9], nullptr, 0);
  unsigned long add_tftatft = std::strtoul(argv[10], nullptr, 0);

  std::vector<Species> pool = (full_or_reactive == 1 ? Species::Memory1Species(discrete_level) : Species::ReactiveMem1Species(discrete_level));
  size_t N_M1 = Mem1Species::N_M1_Species(discrete_level);
  if (add_capri2) {
    pool.emplace_back(N_M1+0, discrete_level);
  }
  if (add_capri) {
    pool.emplace_back(N_M1+1, discrete_level);
  }
  if (add_aon2) {
    pool.emplace_back(N_M1+3, discrete_level);
  }
  if (add_aon3) {
    pool.emplace_back(N_M1+4, discrete_level);
  }
  if (add_tftatft) {
    pool.emplace_back(N_M1+2, discrete_level);
  }
  Ecosystem eco(pool, e);

  {
    std::ofstream namout("species.txt");
    for(auto name: eco.SpeciesNames()) {
      namout << name << std::endl;
    }
    namout << std::endl;
  }

  auto SweepOverBeta = [&eco,cost,sigma](size_t N)->std::vector<std::pair<double,double>> {
    char fname1[100];
    sprintf(fname1, "abundance_%zu.dat", N);
    std::ofstream eqout(fname1);
    std::vector<std::pair<double,double>> c_levels;
    for (int i = 5; i <= 300; i+=5) {
      double benefit = 1.0 + i / 100.0;
      auto eq = eco.CalculateEquilibrium(benefit, cost, N, sigma);
      eqout << benefit << ' ';
      for (double x: eq) { eqout << x << ' '; }
      eqout << std::endl;
      double c_lev = eco.CooperationLevel(eq);
      c_levels.push_back(std::make_pair(benefit, c_lev));
    }
    return c_levels;
  };

  std::vector<std::vector<std::pair<double,double>>> ans;
  for (int N = 2; N <= Nmax; N++) {
    auto a = SweepOverBeta(N);
    ans.push_back(a);
  }

  std::ofstream fout("cooperation_level.dat");
  for (size_t i = 0; i < ans[0].size(); i++) {
    fout << ans[0][i].first;
    for (size_t j = 0; j < ans.size(); j++) {
      fout << ' ' << ans[j][i].second;
    }
    fout << "\n";
  }
  fout.close();

  return 0;
}


/*
 * Copyright 2019 GridGain Systems, Inc. and Contributors.
 *
 * Licensed under the GridGain Community Edition License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     https://www.gridgain.com/products/software/community-edition/gridgain-community-edition-license
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#define _USE_MATH_DEFINES

#ifdef _WIN32
#   include <windows.h>
#endif

#include <sql.h>
#include <sqlext.h>

#include <cmath>

#include <vector>
#include <string>

#include <boost/test/unit_test.hpp>

#include "ignite/ignite.h"
#include "ignite/ignition.h"
#include "ignite/impl/binary/binary_utils.h"

#include "test_type.h"
#include "test_utils.h"
#include "sql_test_suite_fixture.h"

using namespace ignite;
using namespace ignite::cache;
using namespace ignite::cache::query;
using namespace ignite::common;
using namespace ignite_test;

using namespace boost::unit_test;

using ignite::impl::binary::BinaryUtils;

BOOST_FIXTURE_TEST_SUITE(SqlNumericFunctionTestSuite, ignite::SqlTestSuiteFixture)

BOOST_AUTO_TEST_CASE(TestNumericFunctionAbs)
{
    TestType in;

    in.i32Field = -42;

    testCache.Put(1, in);

    CheckSingleResult<SQLINTEGER>("SELECT {fn ABS(i32Field)} FROM TestType", std::abs(in.i32Field));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionAcos)
{
    TestType in;

    in.doubleField = 0.32;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn ACOS(doubleField)} FROM TestType", std::acos(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionAsin)
{
    TestType in;

    in.doubleField = 0.12;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn ASIN(doubleField)} FROM TestType", std::asin(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionAtan)
{
    TestType in;

    in.doubleField = 0.14;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn ATAN(doubleField)} FROM TestType", std::atan(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionAtan2)
{
    TestType in;

    in.doubleField = 0.24;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn ATAN2(doubleField, 0.2)} FROM TestType", std::atan2(in.doubleField, 0.2));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionCeiling)
{
    TestType in;

    in.doubleField = 7.31;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn CEILING(doubleField)} FROM TestType", std::ceil(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionCos)
{
    TestType in;

    in.doubleField = 2.31;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn COS(doubleField)} FROM TestType", std::cos(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionCot)
{
    TestType in;

    in.doubleField = 2.31;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn COT(doubleField)} FROM TestType", 1 / std::tan(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionDegrees)
{
    TestType in;

    in.doubleField = 2.31;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn DEGREES(doubleField)} FROM TestType", in.doubleField * M_1_PI * 180);
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionExp)
{
    TestType in;

    in.doubleField = 1.23;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn EXP(doubleField)} FROM TestType", std::exp(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionFloor)
{
    TestType in;

    in.doubleField = 5.29;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn FLOOR(doubleField)} FROM TestType", std::floor(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionLog)
{
    TestType in;

    in.doubleField = 15.3;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn LOG(doubleField)} FROM TestType", std::log(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionLog10)
{
    TestType in;

    in.doubleField = 15.3;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn LOG10(doubleField)} FROM TestType", std::log10(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionMod)
{
    TestType in;

    in.i64Field = 26;

    testCache.Put(1, in);

    CheckSingleResult<SQLBIGINT>("SELECT {fn MOD(i64Field, 3)} FROM TestType", in.i64Field % 3);
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionPi)
{
    CheckSingleResult<double>("SELECT {fn PI()}", M_PI);
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionPower)
{
    TestType in;

    in.doubleField = 1.81;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn POWER(doubleField, 2.5)} FROM TestType", std::pow(in.doubleField, 2.5));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionRadians)
{
    TestType in;

    in.doubleField = 161;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn RADIANS(doubleField)} FROM TestType", in.doubleField * M_PI / 180.0);
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionRand)
{
    CheckSingleResult<double>("SELECT {fn RAND()} * 0", 0);
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionRound)
{
    TestType in;

    in.doubleField = 5.29;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn ROUND(doubleField)} FROM TestType", std::floor(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionSign)
{
    TestType in;

    in.doubleField = -1.39;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn SIGN(doubleField)} FROM TestType", in.doubleField < 0 ? -1 : in.doubleField == 0 ? 0 : 1);
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionSin)
{
    TestType in;

    in.doubleField = 1.01;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn SIN(doubleField)} FROM TestType", std::sin(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionSqrt)
{
    TestType in;

    in.doubleField = 2.56;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn SQRT(doubleField)} FROM TestType", std::sqrt(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionTan)
{
    TestType in;

    in.doubleField = 0.56;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn TAN(doubleField)} FROM TestType", std::tan(in.doubleField));
}

BOOST_AUTO_TEST_CASE(TestNumericFunctionTruncate)
{
    TestType in;

    in.doubleField = 4.17133;

    testCache.Put(1, in);

    CheckSingleResult<double>("SELECT {fn TRUNCATE(doubleField, 3)} FROM TestType", 4.171);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/math/bindings/rr.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/test.hpp>
#include <fstream>

#include <boost/math/tools/test_data.hpp>

using namespace boost::math::tools;
using namespace std;

struct asinh_data_generator
{
   boost::math::ntl::RR operator()(boost::math::ntl::RR z)
   {
      std::cout << z << " ";
      boost::math::ntl::RR result = log(z + sqrt(z * z + 1));
      std::cout << result << std::endl;
      return result;
   }
};

struct acosh_data_generator
{
   boost::math::ntl::RR operator()(boost::math::ntl::RR z)
   {
      std::cout << z << " ";
      boost::math::ntl::RR result = log(z + sqrt(z * z - 1));
      std::cout << result << std::endl;
      return result;
   }
};

struct atanh_data_generator
{
   boost::math::ntl::RR operator()(boost::math::ntl::RR z)
   {
      std::cout << z << " ";
      boost::math::ntl::RR result = log((z + 1) / (1 - z)) / 2;
      std::cout << result << std::endl;
      return result;
   }
};

int test_main(int argc, char*argv [])
{
   boost::math::ntl::RR::SetPrecision(500);
   boost::math::ntl::RR::SetOutputPrecision(40);

   parameter_info<boost::math::ntl::RR> arg1;
   test_data<boost::math::ntl::RR> data;
   std::ofstream ofs;

   bool cont;
   std::string line;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse hyperbolic sin function:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(asinh_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=asinh_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "asinh_data.ipp";
   ofs.open(line.c_str());
   write_code(ofs, data, "asinh_data");
   data.clear();

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse hyperbolic cos function:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(acosh_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=acosh_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "acosh_data.ipp";
   ofs.close();
   ofs.open(line.c_str());
   write_code(ofs, data, "acosh_data");
   data.clear();

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse hyperbolic tan function:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(atanh_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=atanh_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "atanh_data.ipp";
   ofs.close();
   ofs.open(line.c_str());
   write_code(ofs, data, "atanh_data");

   return 0;
}