#include "stdafx.h"
#include "CppUnitTest.h"
#include <vector>
#include <algorithm>
#include <list>
#include "../../../iterators.hpp"
#include "../../../math/root.hpp"
#include "../../../math/axis.hpp"
#include "../../../math/multiaxis.hpp"
#include "../../../math/combinatorics.hpp"
#include <boost/math/distributions/normal.hpp>
using namespace Microsoft::VisualStudio::CppUnitTestFramework;

namespace hmLib {
	TEST_CLASS(test_math_root) {
	public:
		TEST_METHOD(root_finding_toms748) {
		auto f = [](double x) {return (x - 0.3)*(x - 0.4549)*(x - 0.8991); };

		std::vector<double> Ans;
		hmLib::math::stable_root_toms748(f, 0.0, 1.0, 0.001, 1e-5, hmLib::back_inserter(Ans));

		Assert::AreEqual(1u, Ans.size(), L"Ans Num Error");
//		Assert::AreEqual(0.3, Ans.at(0), 1e-5, L"Ans Num Error");
		Assert::AreEqual(0.4549, Ans.at(0), 1e-5, L"Ans Num Error");
//		Assert::AreEqual(0.8991, Ans.at(2), 1e-5, L"Ans Num Error");
		}
		TEST_METHOD(root_finding_bisect) {
		auto f = [](double x) {return (x - 0.3)*(x - 0.4549)*(x - 0.8991); };

		std::vector<double> Ans;
		hmLib::math::bisect_root_stepper<double> Stepper(0.001, 1e-5);
		hmLib::math::stable_root(Stepper, f, 0.0, 1.0, hmLib::back_inserter(Ans));

		Assert::AreEqual(1u, Ans.size(), L"Ans Num Error");
//		Assert::AreEqual(0.3, Ans.at(0), 1e-5, L"Ans Num Error");
		Assert::AreEqual(0.4549, Ans.at(0), 1e-5, L"Ans Num Error");
//		Assert::AreEqual(0.8991, Ans.at(2), 1e-5, L"Ans Num Error");
		}
	};
	TEST_CLASS(test_math_axis) {
		TEST_METHOD(make_range_axis_with_borders) {
			auto Axis1 = make_range_axis(0.0, 10.0, 11);
			Assert::AreEqual(11u, Axis1.size());
			Assert::AreEqual( 0.0, Axis1.lower());
			Assert::AreEqual(10.0, Axis1.upper());

			auto Axis2 = make_range_axis(0.0, 10.0, 11, math::range_axis_option::none);
			Assert::AreEqual(11u, Axis2.size());
			Assert::AreEqual(0.0, Axis2.lower());
			Assert::AreEqual(10.0, Axis2.upper());

			auto Axis3 = make_range_axis(0.0, 10.0, 9, math::range_axis_option::exclude_boundary);
			Assert::AreEqual(9u, Axis3.size());
			Assert::AreEqual(1.0, Axis3.lower());
			Assert::AreEqual(9.0, Axis3.upper());

			auto Axis4 = make_range_axis(0.0, 10.0, 10, math::range_axis_option::exclude_lower_boundary);
			Assert::AreEqual(10u, Axis4.size());
			Assert::AreEqual(1.0, Axis4.lower());
			Assert::AreEqual(10.0, Axis4.upper());

			auto Axis5 = make_range_axis(0.0, 10.0, 10, math::range_axis_option::exclude_upper_boundary);
			Assert::AreEqual(10u, Axis5.size());
			Assert::AreEqual(0.0, Axis5.lower());
			Assert::AreEqual(9.0, Axis5.upper());
		}
		TEST_METHOD(axis_grid_index) {
			{
				auto Axis = make_range_axis(0.0, 1.0, 11);
				Assert::AreEqual(0.1, Axis.interval());
				for(unsigned int i = 0; i<Axis.size(); ++i) {
					Assert::AreEqual<int>(i, Axis.index(0.1*i-0.04));
					Assert::AreEqual<int>(i, Axis.index(0.1*i+0.04));
				}
			}
			{
				auto Axis = make_range_axis(0.0, 1.0, 11, math::grid_adjuster<math::grid_policy::floor_grid_tag,-8>());
				Assert::AreEqual(0.1, Axis.interval());
				for(unsigned int i = 0; i<Axis.size(); ++i) {
					Assert::AreEqual<int>(i, Axis.index(0.1*i+0.04));
					Assert::AreEqual<int>(i, Axis.index(0.1*i+0.08));
				}
			}
			{
				auto Axis = make_range_axis(0.0, 1.0, 11, math::ceil_grid_adjuster<-8>());
				Assert::AreEqual(0.1, Axis.interval());
				for(unsigned int i = 0; i<Axis.size(); ++i) {
					Assert::AreEqual<int>(i, Axis.index(0.1*i-0.04));
					Assert::AreEqual<int>(i, Axis.index(0.1*i-0.08));
				}
			}
		}
		TEST_METHOD(axis_flort_index) {
			{
				auto Axis = make_range_axis(0.0, 1.0, 11);

				Assert::AreEqual(0.0, Axis.float_index(0.00), 1e-5);
				Assert::AreEqual(0.2, Axis.float_index(0.02), 1e-5);
				Assert::AreEqual(4.6, Axis.float_index(0.46), 1e-5);
				Assert::AreEqual(8.3, Axis.float_index(0.83), 1e-5);
			}
		}
		TEST_METHOD(axis_grid_round_range) {
			auto Axis = make_range_axis(0.0, 1.0, 11);

			Assert::AreEqual(0.1, Axis.interval());

			{
				auto Ans = Axis.weighted_index(0.30-0.04, 0.30+0.04);
				Assert::AreEqual(1u, Ans.size());
				Assert::AreEqual(0.8, Ans.volume(),1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(1.0, Ans.at(0).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(1.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30-0.04, 0.30+0.06);
				Assert::AreEqual(2u, Ans.size());
				Assert::AreEqual(1.0, Ans.volume(),1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9 , Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(0.1, Ans.at(1).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(0.1, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30-0.04, 0.30+0.26);
				Assert::AreEqual(4u, Ans.size());
				Assert::AreEqual(3.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9/3.0, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(1.0/3.0, Ans.at(1).second, 1e-5);
				Assert::AreEqual(5, Ans.at(2).first);
				Assert::AreEqual(1.0/3.0, Ans.at(2).second, 1e-5);
				Assert::AreEqual(6, Ans.at(3).first);
				Assert::AreEqual(0.1/3.0, Ans.at(3).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(5, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(6, (*Itr).first);
				Assert::AreEqual(0.1/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}
		}
		TEST_METHOD(axis_grid_floor_range) {
			auto Axis = make_range_axis(0.0, 1.0, 11,math::floor_grid_adjuster<-8>());

			Assert::AreEqual(0.1, Axis.interval());

			{
				auto Ans = Axis.weighted_index(0.30+0.04, 0.30+0.08);
				Assert::AreEqual(1u, Ans.size());
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(1.0, Ans.at(0).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(1.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30+0.01, 0.30+0.11);
				Assert::AreEqual(2u, Ans.size());
				Assert::AreEqual(1.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(0.1, Ans.at(1).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(0.1, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30+0.01, 0.30+0.31);
				Assert::AreEqual(4u, Ans.size());
				Assert::AreEqual(3.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9/3.0, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(1.0/3.0, Ans.at(1).second, 1e-5);
				Assert::AreEqual(5, Ans.at(2).first);
				Assert::AreEqual(1.0/3.0, Ans.at(2).second, 1e-5);
				Assert::AreEqual(6, Ans.at(3).first);
				Assert::AreEqual(0.1/3.0, Ans.at(3).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(5, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(6, (*Itr).first);
				Assert::AreEqual(0.1/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}
		}
		TEST_METHOD(axis_grid_ceil_range) {
			auto Axis = make_range_axis(0.0, 1.0, 11, math::ceil_grid_adjuster<-8>());

			Assert::AreEqual(0.1, Axis.interval());

			{
				auto Ans = Axis.weighted_index(0.20+0.04, 0.20+0.08);
				Assert::AreEqual(1u, Ans.size());
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(1.0, Ans.at(0).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(1.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.20+0.01, 0.20+0.11);
				Assert::AreEqual(2u, Ans.size());
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(0.1, Ans.at(1).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(0.1, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.20+0.01, 0.20+0.31);
				Assert::AreEqual(4u, Ans.size());
				Assert::AreEqual(3.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9/3.0, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(1.0/3.0, Ans.at(1).second, 1e-5);
				Assert::AreEqual(5, Ans.at(2).first);
				Assert::AreEqual(1.0/3.0, Ans.at(2).second, 1e-5);
				Assert::AreEqual(6, Ans.at(3).first);
				Assert::AreEqual(0.1/3.0, Ans.at(3).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(5, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(6, (*Itr).first);
				Assert::AreEqual(0.1/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}
		}
		TEST_METHOD(axis_mapping_floor_floor) {
			auto Axis1 = make_range_axis(0.2, 0.89, 4, math::floor_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02, 0.74, 7, math::floor_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_round_round) {
			auto Axis1 = make_range_axis(0.2+0.23/2, 0.89+0.23/2, 4, math::round_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02+0.12/2, 0.74+0.12/2, 7, math::round_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_ceil_round) {
			auto Axis1 = make_range_axis(0.2+0.23, 0.89+0.23, 4, math::ceil_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02+0.12/2, 0.74+0.12/2, 7, math::round_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_round_ceil) {
			auto Axis1 = make_range_axis(0.2+0.23/2, 0.89+0.23/2, 4, math::round_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02+0.12, 0.74+0.12, 7, math::ceil_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_norm) {
			auto Axis1 = hmLib::make_range_axis(1.0, 2.0, 101);
			auto Axis2 = hmLib::make_range_axis(0.5, 2.5, 101);

			auto Dist = boost::math::normal_distribution<double>(1.5, 0.1);

			std::vector<double> Vec1(Axis1.size(), 0.);

			for(unsigned int i = 0; i<Axis1.size(); ++i) {
				Vec1[i] = boost::math::pdf(Dist, Axis1[i])*Axis1.interval();
			}
			auto Mapper = Axis1.map_to(Axis2);

			std::vector<double> Vec2(Axis2.size(), 0.);
			for(unsigned int i = 0; i<Vec1.size(); ++i) {
				auto wi = Mapper.weighted_index(i);
				for(auto p:wi) {
					Vec2[p.first] += p.second*Vec1[i];
				}
			}

			auto Sum1 = std::accumulate(Vec1.begin(), Vec1.end(), 0.0);
			auto Sum2 = std::accumulate(Vec2.begin(), Vec2.end(), 0.0);

			Assert::AreEqual(1.0, Sum1, 1e-3);
			Assert::AreEqual(Sum1, Sum2, 1e-4);
		}
	};
	TEST_CLASS(test_math_multiaxis) {
		TEST_METHOD(test_map) {
			lattice_axis<double, 2> Axes1;
			Axes1.axis(0).assign(4.95, 5.05, 101);
			Axes1.axis(1).assign(0.80, 0.90, 101);

			lattice_axis<double, 2> Axes2;
			Axes2.axis(0).assign(4.90, 5.10, 101);
			Axes2.axis(1).assign(0.80, 0.90, 101);

			auto Dist1 = boost::math::normal_distribution<double>(5.00, 0.01);
			auto Dist2 = boost::math::normal_distribution<double>(0.85, 0.01);
			lattices::indexer<2> Indexer(lattices::extent_type<2>{101, 101});
			std::vector<double> Vec1(101*101, 0.0);
			for(unsigned int i = 0; i<Axes1.axis(0).size(); ++i) {
				for(unsigned int j = 0; j<Axes1.axis(1).size(); ++j) {
					Vec1[Indexer.index(lattices::point_type<2>{static_cast<int>(i), static_cast<int>(j)})] = boost::math::pdf(Dist1, Axes1.axis(0)[i])*Axes1.axis(0).interval()
						* boost::math::pdf(Dist2, Axes1.axis(1)[j])*Axes1.axis(1).interval();
				}
			}

			auto Mapper = Axes1.map_to(Axes2);
			std::vector<double> Vec2(Vec1.size(), 0.);
			for(unsigned int i = 0; i<Axes1.axis(0).size(); ++i) {
				for(unsigned int j = 0; j<Axes1.axis(1).size(); ++j) {
					auto wi = Mapper.weighted_point(lattices::point_type<2>{static_cast<int>(i), static_cast<int>(j)});
					for(auto p:wi) {
						Vec2[Indexer.index(p.first)] += p.second*Vec1[Indexer.index(lattices::point_type<2>{static_cast<int>(i), static_cast<int>(j)})];
					}
				}
			}

			auto Sum1 = std::accumulate(Vec1.begin(), Vec1.end(), 0.0);
			auto Sum2 = std::accumulate(Vec2.begin(), Vec2.end(), 0.0);

			Assert::AreEqual(1.0, Sum1, 1e-3);
			Assert::AreEqual(Sum1, Sum2, 1e-4);

		}
	};
	TEST_CLASS(test_math_combi) {
		TEST_METHOD(test_combination) {
			unsigned int N = 5;
			unsigned int R = 3;
			combination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			combination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			combination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp1) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			combination_indexer<> Indexer(N, R,Excp.begin(),Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			combination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			combination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination) {
			unsigned int N = 5;
			unsigned int R = 3;
			multicombination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			multicombination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(5, 3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(N-Excp.size(),R), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation) {
			unsigned int N = 5;
			unsigned int R = 3;
			permutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			permutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp1) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation) {
			unsigned int N = 5;
			unsigned int R = 3;
			multipermutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			multipermutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}

	};
}


/*++
    BDcpp -- Simple Bjontegaard Delta metric implementation for C++.

    MIT License

    Copyright (c) 2022 Tim Bruylants

    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 "bdcpp.h"

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

namespace bdcpp
{
namespace details
{
// Fit a polynomial of given order on the curve (minimizing squared distance).
std::vector<value_type> polyFit(const curve_data_type& curve, const size_t order)
{
    const size_t numCoefficients = order + 1;
    const size_t nCount = curve.size();

    Eigen::MatrixX<value_type> X(nCount, numCoefficients);
    Eigen::MatrixX<value_type> Y(nCount, 1);

    // fill X and Y matrices (X is a Vandermonde matrix)
    for (size_t row = 0; row < nCount; ++row)
    {
        Y(row, 0) = curve[row].second;
        value_type v = (value_type)1;
        for (size_t col = 0; col < numCoefficients; ++col)
        {
            X(row, col) = v;
            v *= curve[row].first;
        }
    }

    // Solve for the polynomial coefficients (one column) and return.
    const Eigen::VectorX<value_type> coefficients = X.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Y);
    return std::vector<value_type>(coefficients.data(), coefficients.data() + numCoefficients);
}

// Calculates Y(x), where the polynomial coefficients are given.
value_type polyVal(const std::vector<value_type>& coefficients, const value_type x)
{
    assert(!coefficients.empty());
    size_t c = coefficients.size();
    value_type r = coefficients[--c];
    while (c != 0)
    {
        r *= x;
        r += coefficients[--c];
    }
    return r;
}

std::vector<value_type> polyIntegrate(const std::vector<value_type>& coefficients, const value_type constant = 0)
{
    const size_t numCoefficients = coefficients.size();
    std::vector<value_type> ic(numCoefficients + 1);
    ic[0] = constant;
    for (size_t c = 0; c < numCoefficients; ++c)
    {
        ic[c + 1] = coefficients[c] / (c + 1);
    }
    return ic;
}

// The main Bjontegaard calculation to get the area surface difference between two curves.
value_type bdDiff(const curve_data_type& curveA, const curve_data_type& curveB, const int polyOrder)
{
    assert(polyOrder >= 3);
    // Take lowest and highest X values (assumes sorted curves and relevant range overlap).
    const auto lowX = std::max(curveA.front().first, curveB.front().first);
    const auto highX = std::min(curveA.back().first, curveB.back().first);

    // Fit curves as polynomials and integrate them.
    const auto iCoefficientsA = polyIntegrate(polyFit(curveA, (size_t)polyOrder));
    const auto iCoefficientsB = polyIntegrate(polyFit(curveB, (size_t)polyOrder));

    // Calculate the definite integrals.
    const auto intA = polyVal(iCoefficientsA, highX) - polyVal(iCoefficientsA, lowX);
    const auto intB = polyVal(iCoefficientsB, highX) - polyVal(iCoefficientsB, lowX);

    // Return the BD diff (as the area over range).
    return (intB - intA) / (highX - lowX);
}

// Prepare curve points for BD calculations.
template<bool TRANSPOSE>
curve_data_type prepareCurve(const curve_data_type& curve)
{
    assert(curve.size() >= 4);
    auto newCurve(curve);
    std::sort(newCurve.begin(), newCurve.end(), [](const curve_data_point_type& vA, const curve_data_point_type& vB)
        {
            return vA.first < vB.first;
        });
    std::for_each(newCurve.begin(), newCurve.end(), [](curve_data_point_type& v)
        {
            v.first = std::log(v.first);
            if constexpr (TRANSPOSE)
            {
                std::swap(v.first, v.second);
            }
        });
    return newCurve;
}
}

// Calculate the BD-SNR for the two given curves (returns a distortion improvement in dB).
value_type bdsnr(const curve_data_type& curveA, const curve_data_type& curveB, const int polyOrder)
{
    return details::bdDiff(details::prepareCurve<false>(curveA), details::prepareCurve<false>(curveB), polyOrder);
}

// Calculate the BD-BR for the two given curves (returns a rate improvement in %).
value_type bdbr(const curve_data_type& curveA, const curve_data_type& curveB, const int polyOrder)
{
    return (std::exp(details::bdDiff(details::prepareCurve<true>(curveA), details::prepareCurve<true>(curveB), polyOrder)) - 1) * 100;
}
}


/*
 * main.cpp
 *
 *  Created on: 3 Aug 2018
 *      Author: scsjd
 */

#include <chrono>
#include <iostream>
#include <random>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/vec_ZZ.h>
#include <NTL/vec_ZZ_p.h>
#include "DETEncrypter.h"
#include "DETDecrypter.h"
#include "SSEEncrypter.h"
#include "SSEDecrypter.h"
#include "GACDEncrypter.h"
#include "GACDDecrypter.h"
#include "PolyACDEncrypter.h"
#include "PolyACDDecrypter.h"
#include "HE1Encrypter.h"
#include "HE1Decrypter.h"
#include "HE1NEncrypter.h"
#include "HE1NDecrypter.h"
#include "HE2Encrypter.h"
#include "HE2Decrypter.h"
#include "HE2NEncrypter.h"
#include "HE2NDecrypter.h"
#include "HE2Ciphertext.h"
#include "PolyCiphertext.h"
#include "SSECiphertext.h"

NTL::ZZX createRandomPoly(std::mt19937& rng, int mu, int n) {
	NTL::ZZX p;
	//Generate random int for degree of poly
	unsigned int rnd = (rng() % n) + 1;
	//Generate random coefficients for poly
	p.SetLength(rnd+1);
	for (unsigned int i = 0; i < rnd + 1; i++) {
		NTL::ZZ tmp;
		RandomBits(tmp, mu);
		p[i] = tmp;
	}
	return p;
}

int main(){
	std::cout << "======= SSE Tests =======" << std::endl;

	std::string test = "this is a test";

	std::cout << "DET consistency test: ";
	DETEncrypter DETenc;
	std::string DETsecrets = DETenc.writeSecretsToJSON();
	std::string DETencrypted = DETenc.encryptToHex(test);

	DETDecrypter DETdec;
	DETdec.readSecretsFromJSON(DETsecrets);
	std::string DETdecrypted = DETdec.decryptFromHex(DETencrypted);
	if(test==DETdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "DET equality test: ";
	std::string encrypted1 = DETenc.encryptToHex(test);
	std::string encrypted2 = DETenc.encryptToHex(test);
	if(encrypted1==encrypted2) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "SSE consistency test: ";
	SSEEncrypter SSEenc;
	std::string SSEsecrets = SSEenc.writeSecretsToJSON();
	std::string SSEencrypted = SSEenc.encrypt(test);

	SSEDecrypter SSEdec;
	SSEdec.readSecretsFromJSON(SSEsecrets);
	std::string SSEdecrypted = SSEdec.decrypt(SSEencrypted);
	if(test==SSEdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "SSE match test 1: ";
	std::string searchKey = SSEenc.createHexEncodedSearchKey(test);
	SSECiphertext ssec(SSEencrypted);
	if(ssec.match(searchKey)) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "SSE match test 2: ";
	std::string test2 = "The quick brown fox jumped over the lazy dog";
	std::string SSEencrypted2 = SSEenc.encrypt(test2);
	std::string searchKey2 = SSEenc.createHexEncodedSearchKey(test2);
	SSECiphertext ssec2(SSEencrypted2);
	if(ssec2.match(searchKey2)) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "======= OPE Tests =======" << std::endl;
	//Data for GACD test
	int bits = 32;
	NTL::ZZ ope_m1 = NTL::RandomBits_ZZ(bits);
	NTL::ZZ ope_m2 = NTL::RandomBits_ZZ(bits);

	std::cout << "GACD consistency test: ";
	GACDEncrypter GACDenc(GACDEncrypter::getMinimumKeyLength(bits));
	std::string GACDsecrets = GACDenc.writeSecretsToJSON();
	NTL::ZZ GACDencrypted1 = GACDenc.encrypt(ope_m1);

	GACDDecrypter GACDdec;
	GACDdec.readSecretsFromJSON(GACDsecrets);
	NTL::ZZ GACDdecrypted = GACDdec.decrypt(GACDencrypted1);
	if(ope_m1==GACDdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "GACD order-preserving test: ";
	NTL::ZZ GACDencrypted2 = GACDenc.encrypt(ope_m2);
	if(ope_m1 < ope_m2){
		if(GACDencrypted1<GACDencrypted2) std::cout << "PASSED" << std::endl;
		else std::cout << "FAILED" << std::endl;
	}
	else{
		if (ope_m1>ope_m2){
			if(GACDencrypted1>GACDencrypted2) std::cout << "PASSED" << std::endl;
			else std::cout << "FAILED" << std::endl;
		}
		else{
			//random order
			std::cout << "PASSED" << std::endl;
		}
	}

	//Data for PolyACD tests
	int lambda = 80;
	int mu = 32;
	int cdegree = 10;
	int pdegree = 5;
	std::mt19937 rng;
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	rng.seed(seed);
	NTL::ZZX ope_p1 = createRandomPoly(rng,mu,pdegree);
	PolyCiphertext polyp1(ope_p1);
	NTL::ZZX ope_p2 = createRandomPoly(rng,mu,pdegree);
	PolyCiphertext polyp2(ope_p2);

	std::cout << "PolyACD consistency test: ";
	PolyACDEncrypter PolyACDenc(lambda,mu,cdegree);
	std::string PolyACDsecrets = PolyACDenc.writeSecretsToJSON();
	NTL::ZZX PolyACDencrypted1 = PolyACDenc.encrypt(ope_p1);

	PolyACDDecrypter PolyACDdec;
	PolyACDdec.readSecretsFromJSON(PolyACDsecrets);
	NTL::ZZX PolyACDdecrypted = PolyACDdec.decrypt(PolyACDencrypted1);
	if(ope_p1==PolyACDdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "PolyACD order-preserving test: ";
	NTL::ZZX PolyACDencrypted2 = PolyACDenc.encrypt(ope_p2);
	PolyCiphertext polyc1(PolyACDencrypted1);
	PolyCiphertext polyc2(PolyACDencrypted2);
	if(polyp1 < polyp2){
		if(polyc1<polyc2) std::cout << "PASSED" << std::endl;
		else std::cout << "FAILED" << std::endl;
	}
	else{
		if (polyp1>polyp2){
			if(polyc1>polyc2) std::cout << "PASSED" << std::endl;
			else std::cout << "FAILED" << std::endl;
		}
		else{
			//random order
			std::cout << "PASSED" << std::endl;
		}
	}

	std::cout << "======= Integer HE Tests =======" << std::endl;
	//Data for HEx tests
	int n = 1000;
	int d = 2;
	int rho1=8;
	int rho2=32;
	int rhoprime=32;

	NTL::ZZ he_rho1_m1 = NTL::RandomBits_ZZ(rho1);
	NTL::ZZ he_rho1_m2 = NTL::RandomBits_ZZ(rho1);
	NTL::ZZ he_rho2_m1 = NTL::RandomBits_ZZ(rho2);
	NTL::ZZ he_rho2_m2 = NTL::RandomBits_ZZ(rho2);
	NTL::ZZ rho1_sum = he_rho1_m1+he_rho1_m2;
	NTL::ZZ rho1_product = he_rho1_m1*he_rho1_m2;
	NTL::ZZ rho2_sum = he_rho2_m1+he_rho2_m2;
	NTL::ZZ rho2_product = he_rho2_m1*he_rho2_m2;

	std::cout << "HE1 consistency test: ";
	HE1Encrypter he1enc(n,d,rho2);
	he1enc.init();
	//NTL::ZZ he1key = he1enc.getKey();
	std::string he1secrets = he1enc.writeSecretsToJSON();
	NTL::ZZ_p he1_c1 = he1enc.encrypt(he_rho2_m1);

	HE1Decrypter he1dec;
	//he1dec.setKey(he1key);
	he1dec.readSecretsFromJSON(he1secrets);
	he1dec.init();
	NTL::ZZ he1_m1 = he1dec.decrypt(he1_c1);
	if(he1_m1==he_rho2_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE1 homomorphic tests:" << std::endl;
	NTL::ZZ_p he1_c2 = he1enc.encrypt(he_rho2_m2);
	NTL::ZZ_p he1_sum = he1_c1+he1_c2;
	NTL::ZZ_p he1_product = he1_c1*he1_c2;
	NTL::ZZ he1_sum_dec = he1dec.decrypt(he1_sum);
	NTL::ZZ he1_product_dec = he1dec.decrypt(he1_product);
	std::cout << "\t Sum: ";
	if(he1_sum_dec==rho2_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he1_product_dec==rho2_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE1N consistency test: ";
	HE1NEncrypter he1nenc(n,d,rho1,rhoprime);
	he1nenc.init();
	//NTL::vec_ZZ he1nkey = he1nenc.getKey();
	std::string he1nsecrets = he1nenc.writeSecretsToJSON();
	NTL::ZZ_p he1n_c1 = he1nenc.encrypt(he_rho1_m1);

	HE1NDecrypter he1ndec;
	//he1ndec.setKey(he1nkey);
	he1ndec.readSecretsFromJSON(he1nsecrets);
	NTL::ZZ he1n_m1 = he1ndec.decrypt(he1n_c1);
	if(he1n_m1==he_rho1_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE1N homomorphic tests:" << std::endl;
	NTL::ZZ_p he1n_c2 = he1nenc.encrypt(he_rho1_m2);
	NTL::ZZ_p he1n_sum = he1n_c1+he1n_c2;
	NTL::ZZ_p he1n_product = he1n_c1*he1n_c2;
	NTL::ZZ he1n_sum_dec = he1ndec.decrypt(he1n_sum);
	NTL::ZZ he1n_product_dec = he1ndec.decrypt(he1n_product);
	std::cout << "\t Sum: ";
	if(he1n_sum_dec==rho1_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he1n_product_dec==rho1_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2 consistency test: ";
	HE2Encrypter he2enc(n,d,rho2);
	he2enc.init();
	std::string he2secrets = he2enc.writeSecretsToJSON();
	std::string he2params = he2enc.writeParametersToJSON();
	NTL::vec_ZZ_p he2_c1 = he2enc.encrypt(he_rho2_m1);

	HE2Decrypter he2dec;
	he2dec.readSecretsFromJSON(he2secrets);
	NTL::ZZ he2_m1 = he2dec.decrypt(he2_c1);
	if(he2_m1==he_rho2_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2 homomorphic tests:" << std::endl;
	NTL::vec_ZZ_p he2_c2 = he2enc.encrypt(he_rho2_m2);
	HE2Ciphertext::setParameters(he2params);
	HE2Ciphertext he2c1(he2_c1);
	HE2Ciphertext he2c2(he2_c2);
	NTL::vec_ZZ_p he2_sum = (he2c1+he2c2).get_ciphertext();
	NTL::vec_ZZ_p he2_product = (he2c1*he2c2).get_ciphertext();
	NTL::ZZ he2_sum_dec = he2dec.decrypt(he2_sum);
	NTL::ZZ he2_product_dec = he2dec.decrypt(he2_product);
	std::cout << "\t Sum: ";
	if(he2_sum_dec==rho2_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he2_product_dec==rho2_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2N consistency test: ";
	HE2NEncrypter he2nenc(n,d,rho1,rhoprime);
	he2nenc.init();
	std::string he2nsecrets = he2nenc.writeSecretsToJSON();
	std::string he2nparams = he2nenc.writeParametersToJSON();
	NTL::vec_ZZ_p he2n_c1 = he2nenc.encrypt(he_rho1_m1);

	HE2NDecrypter he2ndec;
	he2ndec.readSecretsFromJSON(he2nsecrets);
	NTL::ZZ he2n_m1 = he2ndec.decrypt(he2n_c1);
	if(he2n_m1==he_rho1_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2N homomorphic tests:" << std::endl;
	NTL::vec_ZZ_p he2n_c2 = he2nenc.encrypt(he_rho1_m2);
	//Fails in setParameters when initialising R
	HE2Ciphertext::setParameters(he2nparams);
	HE2Ciphertext he2nc1(he2n_c1);
	HE2Ciphertext he2nc2(he2n_c2);
	HE2Ciphertext he2ncsum = he2nc1+he2nc2;
	HE2Ciphertext he2ncproduct = he2nc1*he2nc2;
	NTL::vec_ZZ_p he2n_sum = he2ncsum.get_ciphertext();
	NTL::vec_ZZ_p he2n_product = (he2nc1*he2nc2).get_ciphertext();
	NTL::ZZ he2n_sum_dec = he2ndec.decrypt(he2n_sum);
	NTL::ZZ he2n_product_dec = he2ndec.decrypt(he2n_product);
	std::cout << "\t Sum: ";
	if(he2n_sum_dec==rho1_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he2n_product_dec==rho1_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
}


#include <dlib/matrix.h>
#include <dlib/svm_threaded.h>
#include <plot.h>

#include <experimental/filesystem>
#include <iostream>
#include <map>

using namespace dlib;
namespace fs = std::experimental::filesystem;

const std::vector<std::string> data_names{"dataset0.csv", "dataset1.csv",
                                          "dataset2.csv", "dataset3.csv",
                                          "dataset4.csv"};

const std::vector<std::string> colors{"red", "green", "blue", "cyan", "black"};

using DataType = double;
using Coords = std::vector<DataType>;
using PointCoords = std::pair<Coords, Coords>;
using Classes = std::map<size_t, PointCoords>;

using SampleType = matrix<DataType, 2, 1>;
using Samples = std::vector<SampleType>;
using Labels = std::vector<DataType>;

void PlotClasses(const Classes& classes,
                 const std::string& name,
                 const std::string& file_name) {
  plotcpp::Plot plt(true);
  // plt.SetTerminal("qt");
  plt.SetTerminal("png");
  plt.SetOutput(file_name);
  plt.SetTitle(name);
  plt.SetXLabel("x");
  plt.SetYLabel("y");
  plt.SetAutoscale();
  plt.GnuplotCommand("set grid");

  auto draw_state = plt.StartDraw2D<Coords::const_iterator>();
  for (auto& cls : classes) {
    std::stringstream params;
    params << "lc rgb '" << colors[cls.first] << "' pt 7";
    plt.AddDrawing(
        draw_state,
        plotcpp::Points(cls.second.first.begin(), cls.second.first.end(),
                        cls.second.second.begin(),
                        std::to_string(cls.first) + " cls", params.str()));
  }

  plt.EndDraw2D(draw_state);
  plt.Flush();
}

void KRRClassification(const Samples& samples,
                       const Labels& labels,
                       const Samples& test_samples,
                       const Labels& test_labels,
                       const std::string& name) {
  using OVOtrainer = one_vs_one_trainer<any_trainer<SampleType>>;
  using KernelType = radial_basis_kernel<SampleType>;

  krr_trainer<KernelType> krr_trainer;
  krr_trainer.set_kernel(KernelType(0.1));

  OVOtrainer trainer;
  trainer.set_trainer(krr_trainer);

  one_vs_one_decision_function<OVOtrainer> df = trainer.train(samples, labels);

  Classes classes;
  DataType accuracy = 0;
  for (size_t i = 0; i != test_samples.size(); i++) {
    auto vec = test_samples[i];
    auto class_idx = static_cast<size_t>(df(vec));
    if (static_cast<size_t>(test_labels[i]) == class_idx)
      ++accuracy;
    classes[class_idx].first.push_back(vec(0, 0));
    classes[class_idx].second.push_back(vec(1, 0));
  }

  accuracy /= test_samples.size();

  PlotClasses(classes, "Kernel Ridge Regression " + std::to_string(accuracy),
              name + "-krr-dlib.png");
}

void SVMClassification(const Samples& samples,
                       const Labels& labels,
                       const Samples& test_samples,
                       const Labels& test_labels,
                       const std::string& name) {
  using OVOtrainer = one_vs_one_trainer<any_trainer<SampleType>>;
  using KernelType = radial_basis_kernel<SampleType>;

  svm_nu_trainer<KernelType> svm_trainer;
  svm_trainer.set_kernel(KernelType(0.1));

  OVOtrainer trainer;
  trainer.set_trainer(svm_trainer);

  one_vs_one_decision_function<OVOtrainer> df = trainer.train(samples, labels);

  Classes classes;
  DataType accuracy = 0;
  for (size_t i = 0; i != test_samples.size(); i++) {
    auto vec = test_samples[i];
    auto class_idx = static_cast<size_t>(df(vec));
    if (static_cast<size_t>(test_labels[i]) == class_idx)
      ++accuracy;
    classes[class_idx].first.push_back(vec(0, 0));
    classes[class_idx].second.push_back(vec(1, 0));
  }

  accuracy /= test_samples.size();

  PlotClasses(classes, "SVM " + std::to_string(accuracy),
              name + "-svm-dlib.png");
}

int main(int argc, char** argv) {
  if (argc > 1) {
    auto base_dir = fs::path(argv[1]);
    for (auto& dataset : data_names) {
      auto dataset_name = base_dir / dataset;
      if (fs::exists(dataset_name)) {
        std::ifstream file(dataset_name);
        matrix<DataType> data;
        file >> data;

        auto inputs = dlib::subm(data, 0, 1, data.nr(), 2);
        auto outputs = dlib::subm(data, 0, 3, data.nr(), 1);

        auto num_samples = inputs.nr();
        auto num_features = inputs.nc();
        std::size_t num_clusters =
            std::set<double>(outputs.begin(), outputs.end()).size();

        std::cout << dataset << "\n"
                  << "Num samples: " << num_samples
                  << " num features: " << num_features
                  << " num clusters: " << num_clusters << std::endl;

        // split data set to the train and test parts
        long test_num = 300;
        Samples test_samples;
        Labels test_labels;
        {
          for (long row = 0; row < test_num; ++row) {
            test_samples.emplace_back(dlib::reshape_to_column_vector(
                dlib::subm_clipped(inputs, row, 0, 1, data.nc())));

            test_labels.emplace_back(outputs(row, 0));
          }
        }

        std::vector<SampleType> samples;
        Labels labels;
        {
          for (long row = test_num; row < inputs.nr(); ++row) {
            samples.emplace_back(dlib::reshape_to_column_vector(
                dlib::subm_clipped(inputs, row, 0, 1, data.nc())));
            labels.emplace_back(outputs(row, 0));
          }
        }

        // SVMClassification(samples, labels, test_samples, test_labels,
        // dataset);
        KRRClassification(samples, labels, test_samples, test_labels, dataset);
      } else {
        std::cerr << "Dataset file " << dataset_name << " missed\n";
      }
    }
  } else {
    std::cerr << "Please provider path to the datasets folder\n";
  }

  return 0;
}


/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <boost/mpl/arithmetic.hpp>
#include <boost/mpl/comparison.hpp>
#include <gridtools/common/defs.hpp>
#include <gridtools/common/generic_metafunctions/mpl_tags.hpp>
#include <gtest/gtest.h>

TEST(integralconstant, comparison) {
    GT_STATIC_ASSERT(
        (boost::mpl::greater<std::integral_constant<int, 5>, std::integral_constant<int, 4>>::type::value), "");

    GT_STATIC_ASSERT(
        (boost::mpl::less<std::integral_constant<int, 4>, std::integral_constant<int, 5>>::type::value), "");

    GT_STATIC_ASSERT(
        (boost::mpl::greater_equal<std::integral_constant<int, 5>, std::integral_constant<int, 4>>::type::value), "");

    GT_STATIC_ASSERT(
        (boost::mpl::less_equal<std::integral_constant<int, 4>, std::integral_constant<int, 5>>::type::value), "");
}

TEST(integralconstant, arithmetic) {
    GT_STATIC_ASSERT(
        (boost::mpl::plus<std::integral_constant<int, 5>, std::integral_constant<int, 4>>::type::value == 9), "");
    GT_STATIC_ASSERT(
        (boost::mpl::minus<std::integral_constant<int, 5>, std::integral_constant<int, 4>>::type::value == 1), "");
}


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class BernoulliLogitTestRig : public VectorIntRNGTestRig {
 public:
  BernoulliLogitTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1},
                            {-5.7, -1.0, 0.0, 0.2, 1.0, 10.0}, {-3, -2, 0, 1},
                            {}, {}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& t, const T2&, const T3&, T_rng& rng) const {
    return stan::math::bernoulli_logit_rng(t, rng);
  }

  template <typename T1>
  double pmf(int y, T1 t, double, double) const {
    return std::exp(stan::math::bernoulli_logit_lpmf(y, t));
  }
};

TEST(ProbDistributionsBernoulliLogit, errorCheck) {
  check_dist_throws_all_types(BernoulliLogitTestRig());
}

TEST(ProbDistributionsBernoulliLogit, distributionCheck) {
  check_counts_real(BernoulliLogitTestRig());
}


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

// baseline
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// bilinear interpolation
inline float get(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

Eigen::Vector3d get_3D_point_from_depth(const Eigen::Vector2d& p, double depth)
{
    return Eigen::Vector3d(depth * (p.x() - cx) / fx,
                           depth * (p.y() - cy) / fy,
                           depth);
}

using namespace Sophus;
// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
class LocalParameterizationSE3 : public ceres::LocalParameterization {
 public:
  virtual ~LocalParameterizationSE3() {}

  // SE3 plus operation for Ceres
  //
  //  T * exp(x)
  //
  virtual bool Plus(double const* T_raw, double const* delta_raw,
                    double* T_plus_delta_raw) const {
    Eigen::Map<SE3d const> const T(T_raw);
    Eigen::Map<Vector6d const> const delta(delta_raw);
    Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * SE3d::exp(delta); ///// check if good left or righ multiply ??????????????????
    // std::cout << (SE3d::exp(delta) * T).matrix() << "\n";
    // std::cout << (T * SE3d::exp(delta)).matrix() << "\n\n";
    return true;
  }

  // Jacobian of SE3 plus operation for Ceres
  //
  // Dx T * exp(x)  with  x=0
  //
  virtual bool ComputeJacobian(double const* T_raw,
                               double* jacobian_raw) const {
    Eigen::Map<SE3d const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
        jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  virtual int GlobalSize() const { return SE3d::num_parameters; }

  virtual int LocalSize() const { return SE3d::DoF; }
};

struct PhotometricError: public ceres::SizedCostFunction<1, 7>
{
    PhotometricError(const cv::Mat& img1, const cv::Mat& img2, const Eigen::Vector2d& p1, const Eigen::Vector3d& P1, const Eigen::Matrix3d& K)
    : _img1(img1), _img2(img2), _p1(p1), _P1(P1), _K(K) {}

    virtual bool Evaluate(double const* const *params,
                          double *residuals,
                          double **jacobians) const {
        const Eigen::Map<const Sophus::SE3d> Rt(params[0]);

        Eigen::Vector3d P2 = Rt * _P1;
        Eigen::Vector3d p2 = _K * P2;
        p2 /= p2.z();

        double v1 = get(_img1, _p1.x(), _p1.y());
        double v2 = get(_img2, p2.x(), p2.y());
        double err = v1 - v2;
        residuals[0] = err;

        if (!jacobians) return true;
        if (!jacobians[0]) return true;

        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X2 = std::pow(P2.x(), 2);
        double Y2 = std::pow(P2.y(), 2);
        double Z2 = std::pow(P2.z(), 2);

        int xx = 0, yy = 0;
        double dx = 0.5 * (get(_img2, p2.x() + xx + 1, p2.y() + yy) - get(_img2, p2.x() + xx - 1, p2.y() + yy));
        double dy = 0.5 * (get(_img2, p2.x() + xx, p2.y() + yy + 1) - get(_img2, p2.x() + xx, p2.y() + yy - 1));
        Eigen::Vector2d dIdu(dx, dy);
        Eigen::Matrix<double, 2, 6> dudRt;
        dudRt << fx/P2.z(), 0.0, -fx*P2.x() / Z2,-fx * P2.x() * P2.y() / Z2, fx + fx * X2 / Z2, -fx * P2.y() / P2.z(),
                 0.0, fy / P2.z(), -fy*P2.y()/Z2, -fy-fy * Y2 / Z2, fy * P2.x() * P2.y() / Z2, fy * P2.x() / P2.z();
        Eigen::Matrix<double, 6, 1> J = -(dIdu.transpose() * dudRt).transpose();
        jacobians[0][0] = J(0, 0);
        jacobians[0][1] = J(1, 0);
        jacobians[0][2] = J(2, 0);
        jacobians[0][3] = J(3, 0);
        jacobians[0][4] = J(4, 0);
        jacobians[0][5] = J(5, 0);
        jacobians[0][6] = 0.0;

        return true;
    }

    private:
        cv::Mat _img1, _img2;
        Eigen::Vector2d _p1;
        Eigen::Vector3d _P1;
        Eigen::Matrix3d _K;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt // points from cam1 reference frame to cam2
)
{
    int nb_iters = 10;
    int half_w_size = 2;
    double prev_cost = 0.0;

    ceres::Problem problem;

    problem.AddParameterBlock(Rt.data(), 7, new LocalParameterizationSE3());
std::cout << "AFTER add parameters bloxk" << std::endl;
    for (int i = 0; i < px_ref.size(); ++i)
    {
        const auto& p1 = px_ref[i];
        Eigen::Vector3d P1 = get_3D_point_from_depth(p1, depth_ref[i]);
        problem.AddResidualBlock(
            new PhotometricError(img1, img2, p1, P1, K),
            nullptr,
            Rt.data()
        );
    }
std::cout << "AFTER ad dresiduals block" << std::endl;

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
    cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;

    std::cout << "translation: " << Rt.translation().transpose() << "\n";
    std::cout << "rotation: " << Rt.so3().unit_quaternion().toRotationMatrix() << "\n";

}

void DirectPoseEstimationPyramidal(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt)
{
    int nb_levels = 1;
    double factor = 2.0;
    double scale = 1.0 / std::pow(factor, nb_levels-1);

    for (int l = 0; l < nb_levels; ++l)
    {
        cv::Mat img1_r, img2_r;
        cv::resize(img1, img1_r, cv::Size(), scale, scale);
        cv::resize(img2, img2_r, cv::Size(), scale, scale);

        Eigen::Matrix3d K_r = K;
        K_r(0, 0) *= scale;
        K_r(1, 1) *= scale;
        K_r(0, 2) *= scale;
        K_r(1, 2) *= scale;
        auto p_r = px_ref;
        for (auto& p : p_r)
        {
            p *= scale;
        }

        DirectPoseEstimationSingleLayer(img1_r, img2_r, p_r, depth_ref, K_r, Rt);

        scale *= factor;
    }
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng(1994);
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d Rt;
    Eigen::Matrix3d K;
    K << fx, 0.0, cx,
         0.0, fy, cy,
         0.0, 0.0, 1.0;

    for (int i = 1; i < 2; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, K, Rt);
        // DirectPoseEstimationPyramidal(left_img, img, pixels_ref, depth_ref, K, Rt);

        // plot the projected pixels here
        cv::Mat img2_show;
        cv::cvtColor(img, img2_show, CV_GRAY2BGR);
        std::vector<Eigen::Vector2d> projections(pixels_ref.size());
        for (int i = 0; i < pixels_ref.size(); ++i)
        {
            Eigen::Vector3d P_ref = get_3D_point_from_depth(pixels_ref[i], depth_ref[i]);
            Eigen::Vector3d uv = K * (Rt * P_ref);
            projections[i] = uv.hnormalized();
        }

        for (size_t i = 0; i < pixels_ref.size(); ++i) {
            auto p_ref = pixels_ref[i];
            auto p_cur = projections[i];
            if (p_cur[0] > 0 && p_cur[1] > 0) {
                cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
                cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                        cv::Scalar(0, 250, 0));
            }
        }
        cv::imshow("current", img2_show);
        cv::waitKey();
        // cv::imwrite("img_"+std::to_string(i) + ".png", img2_show);

    }
    return 0;
}