// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include "litCheckMacros.h"

#include "rttbDVH.h"
#include "rttbDvhBasedModels.h"
#include "rttbInvalidParameterException.h"

namespace rttb
{
	namespace testing
	{

		typedef core::DVH::DataDifferentialType DataDifferentialType;

		/*! @brief DvhBasedModelsTest.
		1) Test bed und lqed2
		*/
		int DvhBasedModelsTest(int argc, char* argv[])
		{
			PREPARE_DEFAULT_TEST_REPORTING;

			//1) test calcBEDDVH and calcLQED2DVH
			//generate artificial DVH and corresponding statistical values
			DoseTypeGy binSize = DoseTypeGy(0.1);
			DoseVoxelVolumeType voxelVolume = 8;
			const IDType structureID = "myStructure";
			const IDType doseID = "myDose";
			const IDType voxelizationID = "myVoxelization";
			DataDifferentialType aDataDifferential;
			std::vector<double> bedVector;
			std::vector<double> lqed2Vector;
			int numberOfFractions = 2;
			double alpha_beta = 10;

			for (int i = 0; i < 100; i++)
			{
				double volume = DoseCalcType((double(rand()) / RAND_MAX) * 1000);
				double dose = (i + 0.5) * binSize;
				aDataDifferential.push_back(volume);
				bedVector.push_back(dose * (1 + dose / (numberOfFractions * alpha_beta)));
				lqed2Vector.push_back(dose * ((alpha_beta + (dose / numberOfFractions)) / (alpha_beta + 2)));
			}

			core::DVH myDVH(aDataDifferential, binSize, voxelVolume, structureID, doseID, voxelizationID);
			core::DVH::Pointer dvhPtr = boost::make_shared<core::DVH>(myDVH);

			CHECK_THROW_EXPLICIT(rttb::models::calcBEDDVH(dvhPtr, 0, 10), core::InvalidParameterException);
			CHECK_THROW_EXPLICIT(rttb::models::calcBEDDVH(dvhPtr, 10, -1), core::InvalidParameterException);
			CHECK_NO_THROW(rttb::models::calcBEDDVH(dvhPtr, 10, 10));
			CHECK_EQUAL(rttb::models::calcBEDDVH(dvhPtr, 2, 10).size(), myDVH.getDataDifferential().size());
			rttb::models::BEDDVHType bedDVH = rttb::models::calcBEDDVH(dvhPtr, numberOfFractions, alpha_beta);

			CHECK_THROW_EXPLICIT(rttb::models::calcLQED2DVH(dvhPtr, 1, 10), core::InvalidParameterException);
			CHECK_THROW_EXPLICIT(rttb::models::calcLQED2DVH(dvhPtr, 10, -1), core::InvalidParameterException);
			CHECK_NO_THROW(rttb::models::calcLQED2DVH(dvhPtr, 10, 10, true));
			CHECK_EQUAL(rttb::models::calcLQED2DVH(dvhPtr, 2, 10).size(), myDVH.getDataDifferential().size());
			rttb::models::BEDDVHType lqed2DVH = rttb::models::calcLQED2DVH(dvhPtr, numberOfFractions,
			                                    alpha_beta);

			//check the calculation
			rttb::models::BEDDVHType::iterator itBED, itLQED2;
			std::vector<double>::iterator itBEDVec, itLQED2Vec;
			DataDifferentialType::iterator itDiff;

			for (itBED = bedDVH.begin(), itLQED2 = lqed2DVH.begin(), itBEDVec = bedVector.begin(),
			     itLQED2Vec = lqed2Vector.begin(), itDiff = aDataDifferential.begin();
			     itBED != bedDVH.end(), itLQED2 != lqed2DVH.end(), itBEDVec != bedVector.end(),
			     itLQED2Vec != lqed2Vector.end(), itDiff != aDataDifferential.end();
			     ++itBED, ++itLQED2, ++itBEDVec, ++itLQED2Vec, ++itDiff)
			{

				//check volume
				CHECK_EQUAL(*itDiff, (*itBED).second);
				CHECK_EQUAL((*itBED).second, (*itLQED2).second);

				//check bed
				CHECK_EQUAL(*itBEDVec, (*itBED).first);

				//check lqed2
				CHECK_EQUAL(*itLQED2Vec, (*itLQED2).first);
			}

			RETURN_AND_REPORT_TEST_SUCCESS;

		}

	}//testing
}//rttb


// BenchmarkFloatConv.cpp : Defines the entry point for the console application.
//

#include <vector>
#include <iostream>
#include <iomanip>
#include <string>
#include <cstring>
#include <cmath>
#include <cassert>
#include <sstream>
#include <cstdlib>
#include <chrono>
#include <boost/lexical_cast.hpp>
#include <boost/spirit/include/qi.hpp>
#include "google_double_conversion/double-conversion.h"
#include <charconv>

typedef std::pair<const std::string, const double> pair_type;
typedef std::vector< pair_type > vector_type;

#ifdef WIN32

#pragma optimize("", off)
template <class T>
void do_not_optimize_away(T&& datum) {
	datum = datum;
}
#pragma optimize("", on)

#else
static void do_not_optimize_away(void* p) {
    asm volatile("" : : "g"(p) : "memory");
}
#endif

void init(vector_type& vec);

#define MYASSERT(value, expected)

//#define MYASSERT(value, expected) if(value != expected) { std::cerr << value << " and expected:" << expected << " are different" << std::endl; }

class timer
{
public:
	timer() = default;
	void start(const std::string& text_)
	{
		text = text_;
		begin = std::chrono::high_resolution_clock::now();
	}
	void stop()
	{
		auto end = std::chrono::high_resolution_clock::now();
		auto dur = end - begin;
		auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(dur).count();
		std::cout << std::setw(19) << text << ":" << std::setw(5) << ms << "ms" << std::endl;
	}

private:
	std::string text;
	std::chrono::high_resolution_clock::time_point begin;
};

// Original crack_atof version is at http://crackprogramming.blogspot.sg/2012/10/implement-atof.html
// But it cannot convert floating point with high +/- exponent.
// The version below by Tian Bo fixes that problem and improves performance by 10%
// http://coliru.stacked-crooked.com/a/2e28f0d71f47ca5e
double pow10(int n)
{
	double ret = 1.0;
	double r = 10.0;
	if (n < 0) {
		n = -n;
		r = 0.1;
	}

	while (n) {
		if (n & 1) {
			ret *= r;
		}
		r *= r;
		n >>= 1;
	}
	return ret;
}

double crack_atof(const char* num)
{
	if (!num || !*num) {
		return 0;
	}

	int sign = 1;
	double integerPart = 0.0;
	double fractionPart = 0.0;
	bool hasFraction = false;
	bool hasExpo = false;

	// Take care of +/- sign
	if (*num == '-') {
		++num;
		sign = -1;
	}
	else if (*num == '+') {
		++num;
	}

	while (*num != '\0') {
		if (*num >= '0' && *num <= '9') {
			integerPart = integerPart * 10 + (*num - '0');
		}
		else if (*num == '.') {
			hasFraction = true;
			++num;
			break;
		}
		else if (*num == 'e') {
			hasExpo = true;
			++num;
			break;
		}
		else {
			return sign * integerPart;
		}
		++num;
	}

	if (hasFraction) {
		double fractionExpo = 0.1;

		while (*num != '\0') {
			if (*num >= '0' && *num <= '9') {
				fractionPart += fractionExpo * (*num - '0');
				fractionExpo *= 0.1;
			}
			else if (*num == 'e') {
				hasExpo = true;
				++num;
				break;
			}
			else {
				return sign * (integerPart + fractionPart);
			}
			++num;
		}
	}

	// parsing exponet part
	double expPart = 1.0;
	if (*num != '\0' && hasExpo) {
		int expSign = 1;
		if (*num == '-') {
			expSign = -1;
			++num;
		}
		else if (*num == '+') {
			++num;
		}

		int e = 0;
		while (*num != '\0' && *num >= '0' && *num <= '9') {
			e = e * 10 + *num - '0';
			++num;
		}

		expPart = pow10(expSign * e);
	}

	return sign * (integerPart + fractionPart) * expPart;
}

#define white_space(c) ((c) == ' ' || (c) == '\t')
#define valid_digit(c) ((c) >= '0' && (c) <= '9')
// http://www.leapsecond.com/tools/fast_atof.c
// Do not use this one because the converison is imprecise.
double fast_atof(const char *p)
{
	int frac;
	double sign, value, scale;

	// Skip leading white space, if any.

	while (white_space(*p)) {
		p += 1;
	}

	// Get sign, if any.

	sign = 1.0;
	if (*p == '-') {
		sign = -1.0;
		p += 1;

	}
	else if (*p == '+') {
		p += 1;
	}

	// Get digits before decimal point or exponent, if any.

	for (value = 0.0; valid_digit(*p); p += 1) {
		value = value * 10.0 + (*p - '0');
	}

	// Get digits after decimal point, if any.

	if (*p == '.') {
		double pow10 = 10.0;
		p += 1;
		while (valid_digit(*p)) {
			value += (*p - '0') / pow10;
			pow10 *= 10.0;
			p += 1;
		}
	}

	// Handle exponent, if any.

	frac = 0;
	scale = 1.0;
	if ((*p == 'e') || (*p == 'E')) {
		unsigned int expon;

		// Get sign of exponent, if any.

		p += 1;
		if (*p == '-') {
			frac = 1;
			p += 1;

		}
		else if (*p == '+') {
			p += 1;
		}

		// Get digits of exponent, if any.

		for (expon = 0; valid_digit(*p); p += 1) {
			expon = expon * 10 + (*p - '0');
		}
		if (expon > 308) expon = 308;

		// Calculate scaling factor.

		while (expon >= 50) { scale *= 1E50; expon -= 50; }
		while (expon >= 8) { scale *= 1E8;  expon -= 8; }
		while (expon > 0) { scale *= 10.0; expon -= 1; }
	}

	// Return signed and scaled floating point result.

	return sign * (frac ? (value / scale) : (value * scale));
}
int main(int argc, char *argv [])
{
	const size_t MAX_LOOP = (argc == 2) ? atoi(argv[1]) : 100000;

	vector_type vec;
	init(vec);
	timer stopwatch;
	double d = 0.0;

	stopwatch.start("atof");
	for (size_t k = 0; k<MAX_LOOP; ++k)
	{
		for (size_t j=0; j<vec.size(); ++j)
		{
			pair_type& pr = vec[j];
			d = std::atof(pr.first.c_str());
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("lexical_cast");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t j=0; j<vec.size(); ++j)
		{
			pair_type& pr = vec[j];
			d = boost::lexical_cast<double>(pr.first.c_str());
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("std::istringstream");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t i = 0; i<vec.size(); ++i)
		{
			pair_type& pr = vec[i];
			std::istringstream oss(pr.first);
			oss >> d;
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("std::stod");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t i = 0; i<vec.size(); ++i)
		{
			pair_type& pr = vec[i];
			d = std::stod(pr.first, nullptr);
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("std::strtod");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t i = 0; i < vec.size(); ++i)
		{
			pair_type& pr = vec[i];
			d = std::strtod(pr.first.c_str(), nullptr);
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("crack_atof");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t j=0; j<vec.size(); ++j)
		{
			pair_type& pr = vec[j];
			d = crack_atof(pr.first.c_str());
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("fast_atof");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t j=0; j<vec.size(); ++j)
		{
			pair_type& pr = vec[j];
			d = fast_atof(pr.first.c_str());
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	namespace qi = boost::spirit::qi;

	stopwatch.start("boost_spirit");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t j=0; j<vec.size(); ++j)
		{
			pair_type& pr = vec[j];
			bool success = qi::parse(pr.first.cbegin(), pr.first.cend(), qi::double_, d);
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("google_dconv");
	int processed_characters_count = 0;
	using namespace double_conversion;
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t j = 0; j < vec.size(); ++j)
		{
			pair_type& pr = vec[j];

			static StringToDoubleConverter conv(StringToDoubleConverter::NO_FLAGS, 0.0, NAN, "infinity", "nan");
			d = conv.StringToDouble(pr.first.c_str(), pr.first.size(), &processed_characters_count);

			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	stopwatch.start("std::from_chars");
	for (size_t k = 0; k < MAX_LOOP; ++k)
	{
		for (size_t j = 0; j < vec.size(); ++j)
		{
			pair_type& pr = vec[j];
			std::from_chars(pr.first.data(), pr.first.data() + pr.first.size(), d);
			do_not_optimize_away(&d);
			MYASSERT(d, pr.second);
		}
	}
	stopwatch.stop();

	std::cout << "Last float value: " << d << " <-- Ignore this" << std::endl;
	return 0;
}

void init(vector_type& vec)
{
	std::string float_str = "+12369";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "-25934";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "-47896.36";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "+532.102";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "4.5655";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "8.3658";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "-125.6900";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "-1236.2311";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "-5522.2389";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
	float_str = "-14.23";
	vec.push_back(std::make_pair(float_str, atof(float_str.c_str())));
}


#pragma once

#include <Eigen/Core>

namespace kt84 {

typedef Eigen::Matrix<double, 6, 1> PointNormal;
inline double pn_norm(const PointNormal& pn) { return pn.head(3).norm(); }
inline void   pn_normalize(PointNormal& pn) { pn.tail(3).normalize(); }
inline PointNormal pn_normalized(const PointNormal& pn) { auto temp = pn; temp.tail(3).normalize(); return temp; }

}


//
//  Copyright (c) 2020, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor/extents.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(test_static_strides)

using test_types = std::tuple<boost::numeric::ublas::layout::first_order,
                              boost::numeric::ublas::layout::last_order>;

template<std::size_t ... es>
using extents = boost::numeric::ublas::extents<es...>;

using first_order = boost::numeric::ublas::layout::first_order;
using last_order  = boost::numeric::ublas::layout::last_order;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_static_strides_ctor, value, test_types)
{
  namespace ublas = boost::numeric::ublas;

  constexpr auto s11  = ublas::to_strides_v<extents  <1,1>,first_order>;
  constexpr auto s12  = ublas::to_strides_v<extents  <1,2>,first_order>;
  constexpr auto s21  = ublas::to_strides_v<extents  <2,1>,first_order>;
  constexpr auto s23  = ublas::to_strides_v<extents  <2,3>,first_order>;
  constexpr auto s231 = ublas::to_strides_v<extents<2,3,1>,first_order>;
  constexpr auto s123 = ublas::to_strides_v<extents<1,2,3>,first_order>;
  constexpr auto s423 = ublas::to_strides_v<extents<4,2,3>,first_order>;

  BOOST_CHECK_EQUAL(s11.empty(), false);
  BOOST_CHECK_EQUAL(s12.empty(), false);
  BOOST_CHECK_EQUAL(s21.empty(), false);
  BOOST_CHECK_EQUAL(s23.empty(), false);
  BOOST_CHECK_EQUAL(s231.empty(), false);
  BOOST_CHECK_EQUAL(s123.empty(), false);
  BOOST_CHECK_EQUAL(s423.empty(), false);

  BOOST_CHECK_EQUAL(s11.size(), 2);
  BOOST_CHECK_EQUAL(s12.size(), 2);
  BOOST_CHECK_EQUAL(s21.size(), 2);
  BOOST_CHECK_EQUAL(s23.size(), 2);
  BOOST_CHECK_EQUAL(s231.size(), 3);
  BOOST_CHECK_EQUAL(s123.size(), 3);
  BOOST_CHECK_EQUAL(s423.size(), 3);
}

BOOST_AUTO_TEST_CASE(test_static_strides_ctor_access_first_order)
{
  namespace ublas = boost::numeric::ublas;

  constexpr auto s11  = ublas::to_strides_v<extents  <1,1>,first_order>;
  constexpr auto s12  = ublas::to_strides_v<extents  <1,2>,first_order>;
  constexpr auto s21  = ublas::to_strides_v<extents  <2,1>,first_order>;
  constexpr auto s23  = ublas::to_strides_v<extents  <2,3>,first_order>;
  constexpr auto s231 = ublas::to_strides_v<extents<2,3,1>,first_order>;
  constexpr auto s213 = ublas::to_strides_v<extents<2,1,3>,first_order>;
  constexpr auto s123 = ublas::to_strides_v<extents<1,2,3>,first_order>;
  constexpr auto s423 = ublas::to_strides_v<extents<4,2,3>,first_order>;

  BOOST_REQUIRE_EQUAL(s11.size(), 2);
  BOOST_REQUIRE_EQUAL(s12.size(), 2);
  BOOST_REQUIRE_EQUAL(s21.size(), 2);
  BOOST_REQUIRE_EQUAL(s23.size(), 2);
  BOOST_REQUIRE_EQUAL(s231.size(), 3);
  BOOST_REQUIRE_EQUAL(s213.size(), 3);
  BOOST_REQUIRE_EQUAL(s123.size(), 3);
  BOOST_REQUIRE_EQUAL(s423.size(), 3);

  BOOST_CHECK_EQUAL(s11[0], 1);
  BOOST_CHECK_EQUAL(s11[1], 1);

  BOOST_CHECK_EQUAL(s12[0], 1);
  BOOST_CHECK_EQUAL(s12[1], 1);

  BOOST_CHECK_EQUAL(s21[0], 1);
  BOOST_CHECK_EQUAL(s21[1], 2); // NOTE: is this the way we want to have it?

  BOOST_CHECK_EQUAL(s23[0], 1);
  BOOST_CHECK_EQUAL(s23[1], 2);

  BOOST_CHECK_EQUAL(s231[0], 1);
  BOOST_CHECK_EQUAL(s231[1], 2);
  BOOST_CHECK_EQUAL(s231[2], 6);

  BOOST_CHECK_EQUAL(s123[0], 1);
  BOOST_CHECK_EQUAL(s123[1], 1);
  BOOST_CHECK_EQUAL(s123[2], 2);

  BOOST_CHECK_EQUAL(s213[0], 1);
  BOOST_CHECK_EQUAL(s213[1], 2);
  BOOST_CHECK_EQUAL(s213[2], 2);

  BOOST_CHECK_EQUAL(s423[0], 1);
  BOOST_CHECK_EQUAL(s423[1], 4);
  BOOST_CHECK_EQUAL(s423[2], 8);
}

BOOST_AUTO_TEST_CASE(test_static_strides_ctor_access_last_order)
{
  namespace ublas = boost::numeric::ublas;

  constexpr auto s11  = ublas::to_strides_v<extents  <1,1>,last_order>;
  constexpr auto s12  = ublas::to_strides_v<extents  <1,2>,last_order>;
  constexpr auto s21  = ublas::to_strides_v<extents  <2,1>,last_order>;
  constexpr auto s23  = ublas::to_strides_v<extents  <2,3>,last_order>;
  constexpr auto s231 = ublas::to_strides_v<extents<2,3,1>,last_order>;
  constexpr auto s213 = ublas::to_strides_v<extents<2,1,3>,last_order>;
  constexpr auto s123 = ublas::to_strides_v<extents<1,2,3>,last_order>;
  constexpr auto s423 = ublas::to_strides_v<extents<4,2,3>,last_order>;

  BOOST_REQUIRE_EQUAL(s11.size(), 2);
  BOOST_REQUIRE_EQUAL(s12.size(), 2);
  BOOST_REQUIRE_EQUAL(s21.size(), 2);
  BOOST_REQUIRE_EQUAL(s23.size(), 2);
  BOOST_REQUIRE_EQUAL(s231.size(), 3);
  BOOST_REQUIRE_EQUAL(s213.size(), 3);
  BOOST_REQUIRE_EQUAL(s123.size(), 3);
  BOOST_REQUIRE_EQUAL(s423.size(), 3);

  BOOST_CHECK_EQUAL(s11[0], 1);
  BOOST_CHECK_EQUAL(s11[1], 1);

  BOOST_CHECK_EQUAL(s12[0], 2); //NOTE: is this the way we want the stride to be computed?
  BOOST_CHECK_EQUAL(s12[1], 1);

  BOOST_CHECK_EQUAL(s21[0], 1);
  BOOST_CHECK_EQUAL(s21[1], 1);

  BOOST_CHECK_EQUAL(s23[0], 3);
  BOOST_CHECK_EQUAL(s23[1], 1);

  BOOST_CHECK_EQUAL(s231[0], 3);
  BOOST_CHECK_EQUAL(s231[1], 1);
  BOOST_CHECK_EQUAL(s231[2], 1);

  BOOST_CHECK_EQUAL(s123[0], 6);
  BOOST_CHECK_EQUAL(s123[1], 3);
  BOOST_CHECK_EQUAL(s123[2], 1);

  BOOST_CHECK_EQUAL(s213[0], 3);
  BOOST_CHECK_EQUAL(s213[1], 3);
  BOOST_CHECK_EQUAL(s213[2], 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Dense>
#include "control.hpp"
#include "defines.hpp"
#include "fad.hpp"
#include "global_residual.hpp"
#include "J2_plane_strain.hpp"
#include "material_params.hpp"

namespace calibr8 {

static ParameterList get_valid_local_residual_params() {
  ParameterList p;
  p.set<std::string>("type", "J2_plane_strain");
  p.set<int>("nonlinear max iters", 0);
  p.set<double>("nonlinear absolute tol", 0.);
  p.set<double>("nonlinear relative tol", 0.);
  p.sublist("materials");
  return p;
}

static ParameterList get_valid_material_params() {
  ParameterList p;
  p.set<double>("E", 0.);
  p.set<double>("nu", 0.);
  p.set<double>("K", 0.);
  p.set<double>("Y", 0.);
  return p;
}

template <typename T>
J2_plane_strain<T>::J2_plane_strain(ParameterList const& inputs, int ndims) {

  this->m_params_list = inputs;
  this->m_params_list.validateParameters(get_valid_local_residual_params(), 0);

  int const num_residuals = 4;

  this->m_num_residuals = num_residuals;
  this->m_num_eqs.resize(num_residuals);
  this->m_var_types.resize(num_residuals);
  this->m_resid_names.resize(num_residuals);

  this->m_resid_names[0] = "zeta";
  this->m_var_types[0] = SYM_TENSOR;
  this->m_num_eqs[0] = get_num_eqs(SYM_TENSOR, ndims);

  this->m_resid_names[1] = "Ie";
  this->m_var_types[1] = SCALAR;
  this->m_num_eqs[1] = get_num_eqs(SCALAR, ndims);

  this->m_resid_names[2] = "alpha";
  this->m_var_types[2] = SCALAR;
  this->m_num_eqs[2] = get_num_eqs(SCALAR, ndims);

  this->m_resid_names[3] = "zeta_zz";
  this->m_var_types[3] = SCALAR;
  this->m_num_eqs[3] = get_num_eqs(SCALAR, ndims);

  m_max_iters = inputs.get<int>("nonlinear max iters");
  m_abs_tol = inputs.get<double>("nonlinear absolute tol");
  m_rel_tol = inputs.get<double>("nonlinear relative tol");

}

template <typename T>
void J2_plane_strain<T>::init_params() {

  int const num_params = 4;
  this->m_params.resize(num_params);
  this->m_param_names.resize(num_params);

  this->m_param_names.resize(num_params);
  this->m_param_names[0] = "E";
  this->m_param_names[1] = "nu";
  this->m_param_names[2] = "K";
  this->m_param_names[3] = "Y";

  int const num_elem_sets = this->m_elem_set_names.size();
  resize(this->m_param_values, num_elem_sets, num_params);

  ParameterList& all_material_params =
      this->m_params_list.sublist("materials", true);

  for (int es = 0; es < num_elem_sets; ++es) {
    std::string const& elem_set_name = this->m_elem_set_names[es];
    ParameterList& material_params =
        all_material_params.sublist(elem_set_name, true);
    material_params.validateParameters(get_valid_material_params(), 0);
    this->m_param_values[es][0] = material_params.get<double>("E");
    this->m_param_values[es][1] = material_params.get<double>("nu");
    this->m_param_values[es][2] = material_params.get<double>("K");
    this->m_param_values[es][3] = material_params.get<double>("Y");
  }

  this->m_active_indices.resize(1);
  this->m_active_indices[0].resize(1);
  this->m_active_indices[0][0] = 0;
}

template <typename T>
J2_plane_strain<T>::~J2_plane_strain() {
}

template <typename T>
void J2_plane_strain<T>::init_variables_impl() {

  int const ndims = this->m_num_dims;
  int const zeta_idx = 0;
  int const Ie_idx = 1;
  int const alpha_idx = 2;
  int const zeta_zz_idx = 3;

  T const Ie = 1.0;
  T const alpha = 0.0;
  Tensor<T> const zeta = minitensor::zero<T>(ndims);
  T const zeta_zz = 0.0;

  this->set_scalar_xi(Ie_idx, Ie);
  this->set_scalar_xi(alpha_idx, alpha);
  this->set_sym_tensor_xi(zeta_idx, zeta);
  this->set_scalar_xi(zeta_zz_idx, zeta_zz);

}

template <typename T>
void eval_be_bar_plane_strain(
    RCP<GlobalResidual<T>> global,
    Tensor<T> const& zeta,
    T const& Ie,
    T const& zeta_zz,
    Tensor<T>& be_bar_2D,
    T& be_bar_zz) {
  int const ndims = global->num_dims();
  Tensor<T> const I = minitensor::eye<T>(ndims);
  Tensor<T> const grad_u = global->grad_vector_x(0);
  Tensor<T> const grad_u_prev = global->grad_vector_x_prev(0);
  Tensor<T> const F = grad_u + I;
  Tensor<T> const F_prev = grad_u_prev + I;
  Tensor<T> const rF = F * minitensor::inverse(F_prev);
  T const det_rF = minitensor::det(rF);
  T const det_rF_13 = cbrt(det_rF);
  Tensor<T> const rF_bar = rF / det_rF_13;
  Tensor<T> const rF_barT = minitensor::transpose(rF_bar);
  be_bar_2D = rF_bar * (zeta + Ie * I) * rF_barT;
  be_bar_zz = (zeta_zz + Ie) / (det_rF_13 * det_rF_13);
}

template <typename T>
T norm_s_3D(Tensor<T> const& s_2D, T const& s_zz) {
  return sqrt(s_2D(0, 0) * s_2D(0, 0) + s_2D(1, 1) * s_2D(1, 1)
      + 2. * s_2D(0, 1) * s_2D(0, 1) + s_zz * s_zz);
}

template <typename T>
T det_be_bar_3D(Tensor<T> const& zeta_2D, T const& zeta_zz, T const& Ie) {
  return ((zeta_2D(0, 0) + Ie) * (zeta_2D(1, 1) + Ie)
      - zeta_2D(0, 1) * zeta_2D(0, 1)) * (zeta_zz + Ie);
}

template <>
int J2_plane_strain<double>::solve_nonlinear(RCP<GlobalResidual<double>>) {
  return 0;
}

template <>
int J2_plane_strain<FADT>::solve_nonlinear(RCP<GlobalResidual<FADT>> global) {

  int path;

  // pick an initial guess for the local variables
  {
    Tensor<FADT> const zeta_old = this->sym_tensor_xi_prev(0);
    FADT const Ie_old = this->scalar_xi_prev(1);
    FADT const alpha_old = this->scalar_xi_prev(2);
    FADT const zeta_zz_old = this->scalar_xi_prev(3);

    int const ndims = global->num_dims();
    Tensor<FADT> be_bar_2D_trial;
    FADT be_bar_zz_trial;
    eval_be_bar_plane_strain(global, zeta_old, Ie_old, zeta_zz_old,
        be_bar_2D_trial, be_bar_zz_trial);
    FADT const Ie_trial = (minitensor::trace(be_bar_2D_trial)
        + be_bar_zz_trial) / 3.;
    Tensor<FADT> const I = minitensor::eye<FADT>(ndims);
    Tensor<FADT> const zeta_trial = be_bar_2D_trial - Ie_trial * I;
    FADT const zeta_zz_trial = be_bar_zz_trial - Ie_trial;
    FADT const alpha_trial = alpha_old;
    this->set_sym_tensor_xi(0, zeta_trial);
    this->set_scalar_xi(1, Ie_trial);
    this->set_scalar_xi(2, alpha_trial);
    this->set_scalar_xi(3, zeta_zz_trial);
    path = ELASTIC;
  }

  // newton iteration until convergence

  int iter = 1;
  double R_norm_0 = 1.;
  bool converged = false;

  while ((iter <= m_max_iters) && (!converged)) {

    path = this->evaluate(global);

    double const R_norm = this->norm_residual();
    if (iter == 1) R_norm_0 = R_norm;
    double const R_norm_rel = R_norm / R_norm_0;
    if ((R_norm_rel < m_rel_tol) || (R_norm < m_abs_tol)) {
      converged = true;
      break;
    }

    EMatrix const J = this->eigen_jacobian();
    EVector const R = this->eigen_residual();
    EVector const dxi = J.fullPivLu().solve(-R);

    this->add_to_sym_tensor_xi(0, dxi);
    this->add_to_scalar_xi(1, dxi);
    this->add_to_scalar_xi(2, dxi);
    this->add_to_scalar_xi(3, dxi);

    iter++;

  }

  // fail if convergence was not achieved
  if ((iter > m_max_iters) && (!converged)) {
    fail("J2_plane_strain:solve_nonlinear failed in %d iterations", m_max_iters);
  }

  return path;

}

template <typename T>
int J2_plane_strain<T>::evaluate(
    RCP<GlobalResidual<T>> global,
    bool force_path,
    int path_in) {

  int path = ELASTIC;
  int const ndims = this->m_num_dims;
  double const sqrt_23 = std::sqrt(2./3.);
  double const sqrt_32 = std::sqrt(3./2.);

  T const E = this->m_params[0];
  T const nu = this->m_params[1];
  T const K = this->m_params[2];
  T const Y = this->m_params[3];
  T const mu = compute_mu(E, nu);

  Tensor<T> const zeta_old = this->sym_tensor_xi_prev(0);
  T const Ie_old = this->scalar_xi_prev(1);
  T const alpha_old = this->scalar_xi_prev(2);
  T const zeta_zz_old = this->scalar_xi_prev(3);

  Tensor<T> const zeta = this->sym_tensor_xi(0);
  T const Ie = this->scalar_xi(1);
  T const alpha = this->scalar_xi(2);
  T const zeta_zz = this->scalar_xi(3);

  Tensor<T> const I = minitensor::eye<T>(ndims);
  Tensor<T> be_bar_trial_2D;
  T be_bar_trial_zz;
  eval_be_bar_plane_strain(global, zeta_old, Ie_old, zeta_zz_old,
      be_bar_trial_2D, be_bar_trial_zz);
  T const Ie_trial = (minitensor::trace(be_bar_trial_2D)
      + be_bar_trial_zz) / 3.;
  Tensor<T> const zeta_trial = be_bar_trial_2D - Ie_trial * I;
  T const zeta_zz_trial = be_bar_trial_zz - Ie_trial;
  Tensor<T> const s_2D = mu * zeta;
  T const s_zz = mu * zeta_zz;
  T const s_mag = norm_s_3D(s_2D, s_zz);
  Tensor<T> const n_2D = s_2D / s_mag;
  T const n_zz = s_zz / s_mag;
  T const sigma_yield = Y + K * alpha;
  T const f = s_mag - sqrt_23 * sigma_yield;

  Tensor<T> R_zeta;
  T R_Ie;
  T R_alpha;
  T R_zeta_zz;

  if (!force_path) {
    // plastic step
    if (f > m_abs_tol || std::abs(f) < m_abs_tol) {
      T const dgam = sqrt_32 * (alpha - alpha_old);
      R_zeta = zeta - zeta_trial + 2. * dgam * Ie * n_2D;
      R_Ie = det_be_bar_3D(zeta, zeta_zz, Ie) - 1.;
      R_alpha = (s_mag - sqrt_23 * sigma_yield) / val(mu);
      R_zeta_zz = zeta_zz - zeta_zz_trial + 2. * dgam * Ie * n_zz;
      path = PLASTIC;
    }
    // elastic step
    else {
      R_zeta = (0. * mu + 1.) * zeta - zeta_trial;
      R_Ie = Ie - Ie_trial + 0. * mu;
      R_alpha = alpha - alpha_old + 0. * mu;
      R_zeta_zz = zeta_zz - zeta_zz_trial + 0. * mu;
      path = ELASTIC;
    }
  }

  // force the path
  else {
    path = path_in;
    // plastic step
    if (path == PLASTIC) {
      T const dgam = sqrt_32 * (alpha - alpha_old);
      R_zeta = zeta - zeta_trial + 2. * dgam * Ie * n_2D;
      R_Ie = det_be_bar_3D(zeta, zeta_zz, Ie) - 1.;
      R_alpha = (s_mag - sqrt_23 * sigma_yield) / val(mu);
      R_zeta_zz = zeta_zz - zeta_zz_trial + 2. * dgam * Ie * n_zz;
    }
    // elastic step
    else {
      R_zeta = (0. * mu + 1.) * zeta - zeta_trial;
      R_Ie = Ie - Ie_trial + 0. * mu;
      R_alpha = alpha - alpha_old + 0. * mu;
      R_zeta_zz = zeta_zz - zeta_zz_trial + 0. * mu;
    }
  }

  this->set_sym_tensor_R(0, R_zeta);
  this->set_scalar_R(1, R_Ie);
  this->set_scalar_R(2, R_alpha);
  this->set_scalar_R(3, R_zeta_zz);

  return path;

}

template <typename T>
Tensor<T> J2_plane_strain<T>::dev_cauchy(RCP<GlobalResidual<T>> global) {
  int const ndims = global->num_dims();
  T const E = this->m_params[0];
  T const nu = this->m_params[1];
  T const mu = E / (2. * (1. + nu));
  Tensor<T> const I = minitensor::eye<T>(ndims);
  Tensor<T> const grad_u = global->grad_vector_x(0);
  Tensor<T> const F = grad_u + I;
  Tensor<T> const zeta = this->sym_tensor_xi(0);
  T const J = minitensor::det(F);
  return mu * zeta / J;
}

template <typename T>
Tensor<T> J2_plane_strain<T>::cauchy(RCP<GlobalResidual<T>> global, T p) {
  int const ndims = global->num_dims();
  Tensor<T> const I = minitensor::eye<T>(ndims);
  Tensor<T> const dev_sigma = this->dev_cauchy(global);
  Tensor<T> const sigma = dev_sigma - p * I;
  return sigma;
}

template class J2_plane_strain<double>;
template class J2_plane_strain<FADT>;

}


#if !defined(BALSA_EIGEN_SHAPE_CHECKS_HPP)
#define BALSA_EIGEN_SHAPE_CHECKS_HPP
#include <Eigen/Core>
#include "balsa/eigen/concepts/shape_types.hpp"
#include <fmt/format.h>
#include <fmt/ranges.h>

namespace balsa::eigen {

// These functions return in case asserts are disabled
template<int... Cs, typename MatType>
requires(concepts::ColCompatible<Cs, MatType> || ... || false) constexpr bool col_check_oneof(const MatType &N,
                                                                                std::integer_sequence<int, Cs...>) {

    constexpr int cols = MatType::ColsAtCompileTime;
    if constexpr (cols == Eigen::Dynamic) {
        return ((N.cols() == Cs) || ... || false);
    }
    return true;
}

template<int... Rs, typename MatType>
requires(concepts::RowCompatible<Rs, MatType> || ... || false) constexpr bool row_check_oneof(const MatType &N,
                                                                                std::integer_sequence<int, Rs...>) {
    constexpr int rows = MatType::RowsAtCompileTime;
    if constexpr (rows == Eigen::Dynamic) {
        return ((N.rows() == Rs) || ... || false);
    }
    return true;
}

template<int C, typename MatType>
requires concepts::ColCompatible<C, MatType> constexpr bool col_check(const MatType &N) {
    return col_check_oneof(N, std::integer_sequence<int, C>{});
}

template<int R, typename MatType>
requires concepts::RowCompatible<R, MatType> constexpr bool row_check(const MatType &N) {
    return row_check_oneof(N, std::integer_sequence<int, R>{});
}
template<int R, int C, typename MatType>
constexpr bool shape_check(const MatType &N) {
    return row_check<R>(N) && col_check<C>(N);
}

template<int C, typename MatType>
requires concepts::ColCompatible<C, MatType> constexpr void col_check_with_throw(const MatType &N) {
    if (!col_check<C>(N)) {
        throw std::invalid_argument(fmt::format("Col check: wrong size, got {} expected {}", N.cols(), C));
    }
}

template<int R, typename MatType>
requires concepts::RowCompatible<R, MatType> constexpr void row_check_with_throw(const MatType &N) {

    if (!row_check<R>(N)) {
        throw std::invalid_argument(fmt::format("Row check: wrong size, got {} expected {}", N.rows(), R));
    }
}
template<int R, int C, typename MatType>
constexpr void shape_check_with_throw(const MatType &N) {
    row_check_with_throw<R>(N);
    col_check_with_throw<C>(N);
}

template<int... C, typename MatType>
void col_check_with_throw(const MatType &N, std::integer_sequence<int, C...> Seq) {
    if (!col_check_oneof(N, Seq)) {
        std::string msg = fmt::format("Col check: wrong size, got {} expected one of {}", N.cols(), std::make_tuple(C...));
        throw std::invalid_argument(msg);
    }
}

template<int... R, typename MatType>
void row_check_with_throw(const MatType &N, std::integer_sequence<int, R...> Seq) {

    if (!row_check_oneof(N, Seq)) {
        std::string msg = fmt::format("Row check: wrong size, got {} expected {}", N.rows(), std::make_tuple(R...));
        throw std::invalid_argument(msg);
    }
}
template<int... R, int... C, typename MatType>
void shape_check_with_throw(const MatType &N, std::integer_sequence<int, R...> RSeq, std::integer_sequence<int, C...> CSeq) {
    row_check_with_throw(N, RSeq) && col_check_with_throw(N, CSeq);
}
}// namespace balsa::eigen
#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testImuFactor.cpp
 * @brief   Unit test for ImuFactor
 * @author  Luca Carlone, Stephen Williams, Richard Roberts
 */

#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/bind.hpp>
#include <list>

using namespace std;
using namespace gtsam;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;

/* ************************************************************************* */
namespace {

Vector callEvaluateError(const ImuFactor& factor,
    const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
    const imuBias::ConstantBias& bias)
{
  return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}

Rot3 evaluateRotationError(const ImuFactor& factor,
    const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
    const imuBias::ConstantBias& bias)
{
  return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
}

ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
    )
{
  ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
      Matrix3::Identity(), Matrix3::Identity());

  list<Vector3>::const_iterator itAcc = measuredAccs.begin();
  list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
  list<double>::const_iterator itDeltaT = deltaTs.begin();
  for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
    result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
  }

  return result;
}

Vector3 evaluatePreintegratedMeasurementsPosition(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
  return evaluatePreintegratedMeasurements(bias,
      measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij;
}

Vector3 evaluatePreintegratedMeasurementsVelocity(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
  return evaluatePreintegratedMeasurements(bias,
      measuredAccs, measuredOmegas, deltaTs).deltaVij;
}

Rot3 evaluatePreintegratedMeasurementsRotation(
    const imuBias::ConstantBias& bias,
    const list<Vector3>& measuredAccs,
    const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
  return evaluatePreintegratedMeasurements(bias,
      measuredAccs, measuredOmegas, deltaTs).deltaRij;
}

Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
{
  return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}

Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
{
  return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
}

}

/* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements )
{
  cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
  // Linearization point
  imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases

  // Measurements
  Vector3 measuredAcc(0.1, 0.0, 0.0);
  Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
  double deltaT = 0.5;
  double tol = 1e-6;

  // Actual preintegrated values
  ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
		  Matrix3::Zero(), Matrix3::Zero());
  expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
		  Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
		  Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));

//           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
//           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
//           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
//           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
//           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
//           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
//           const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements

  actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  EXPECT(assert_equal(Vector(expected1.deltaPij), Vector(actual1.deltaPij), tol));
  EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
  EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
  DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
}

/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases )
{
  cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl;

  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
  imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
  LieVector v1(3, 0.5, 0.0, 0.0);
  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
  LieVector v2(3, 0.5, 0.0, 0.0);

  // Measurements
  Vector3 gravity; gravity << 0, 0, 9.81;
  Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
  Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
  double deltaT = 1.0;
  double tol = 1e-6;

  //           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
  //           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
  //           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
  //           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
  //           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
  //           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
  //           const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements

  Matrix I6x6(6,6);
  I6x6 = Matrix::Identity(6,6);

  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
		  Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());

    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

   CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
		   imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),  Vector3(0.0, 0.0, 0.0)),
  		  Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(),	I6x6 );

   Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

    // Create factor
    ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);

    noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov);
    CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel);

    Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);

    Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);

    EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));

    // Expected Jacobians
    Matrix H1e, H2e, H3e, H4e, H5e;
    (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);

    // Actual Jacobians
	Matrix H1a, H2a, H3a, H4a, H5a, H6a;
	(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);

	EXPECT(assert_equal(H1e, H1a.topRows(9)));
	EXPECT(assert_equal(H2e, H2a.topRows(9)));
	EXPECT(assert_equal(H3e, H3a.topRows(9)));
	EXPECT(assert_equal(H4e, H4a.topRows(9)));
	EXPECT(assert_equal(H5e, H5a.topRows(9)));
}

/* ************************************************************************* */
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
{
  cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
  // Linearization point
  imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases

  Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));

  // Measurements
  list<Vector3> measuredAccs, measuredOmegas;
  list<double> deltaTs;
  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
  deltaTs.push_back(0.01);
  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
  deltaTs.push_back(0.01);
  for(int i=1;i<100;i++)
  {
    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
    measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
    deltaTs.push_back(0.01);
  }

  // Actual preintegrated values
  ImuFactor::PreintegratedMeasurements preintegrated =
      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));

  // Compute numerical derivatives
  Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>(
      boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
  Matrix expectedDelPdelBiasAcc   = expectedDelPdelBias.leftCols(3);
  Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);

  Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>(
      boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
  Matrix expectedDelVdelBiasAcc   = expectedDelVdelBias.leftCols(3);
  Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);

  Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
      boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
  Matrix expectedDelRdelBiasAcc   = expectedDelRdelBias.leftCols(3);
  Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
  EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
  EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
  EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
  EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
  EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
}

#include <gtsam/linear/GaussianFactorGraph.h>

/* ************************************************************************* */
  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file testUnit3.cpp
 * @date Feb 03, 2012
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @brief Tests the Unit3 class
 */

#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/thread.hpp>
#include <boost/assign/std/vector.hpp>
#include <cmath>

using namespace boost::assign;
using namespace gtsam;
using namespace std;

GTSAM_CONCEPT_TESTABLE_INST(Unit3)
GTSAM_CONCEPT_MANIFOLD_INST(Unit3)

//*******************************************************************************
Point3 point3_(const Unit3& p) {
  return p.point3();
}
TEST(Unit3, point3) {
  vector<Point3> ps;
  ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
      / sqrt(2.0);
  Matrix actualH, expectedH;
  BOOST_FOREACH(Point3 p,ps) {
    Unit3 s(p);
    expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
    EXPECT(assert_equal(p, s.point3(actualH), 1e-8));
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
}

//*******************************************************************************
static Unit3 rotate_(const Rot3& R, const Unit3& p) {
  return R * p;
}

TEST(Unit3, rotate) {
  Rot3 R = Rot3::yaw(0.5);
  Unit3 p(1, 0, 0);
  Unit3 expected = Unit3(R.column(1));
  Unit3 actual = R * p;
  EXPECT(assert_equal(expected, actual, 1e-8));
  Matrix actualH, expectedH;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expectedH = numericalDerivative21(rotate_, R, p);
    R.rotate(p, actualH, boost::none);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
  {
    expectedH = numericalDerivative22(rotate_, R, p);
    R.rotate(p, boost::none, actualH);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
}

//*******************************************************************************
static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
  return R.unrotate(p);
}

TEST(Unit3, unrotate) {
  Rot3 R = Rot3::yaw(-M_PI / 4.0);
  Unit3 p(1, 0, 0);
  Unit3 expected = Unit3(1, 1, 0);
  Unit3 actual = R.unrotate(p);
  EXPECT(assert_equal(expected, actual, 1e-8));
  Matrix actualH, expectedH;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expectedH = numericalDerivative21(unrotate_, R, p);
    R.unrotate(p, actualH, boost::none);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
  {
    expectedH = numericalDerivative22(unrotate_, R, p);
    R.unrotate(p, boost::none, actualH);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, error) {
  Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
  r = p.retract((Vector(2) << 0.8, 0));
  EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
  EXPECT(assert_equal((Vector(2) << 0.479426, 0), p.error(q), 1e-5));
  EXPECT(assert_equal((Vector(2) << 0.717356, 0), p.error(r), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative11<Unit3>(
        boost::bind(&Unit3::error, &p, _1, boost::none), q);
    p.error(q, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
  {
    expected = numericalDerivative11<Unit3>(
        boost::bind(&Unit3::error, &p, _1, boost::none), r);
    p.error(r, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, distance) {
  Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
  r = p.retract((Vector(2) << 0.8, 0));
  EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
  EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
  EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8);

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalGradient<Unit3>(
        boost::bind(&Unit3::distance, &p, _1, boost::none), q);
    p.distance(q, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
  {
    expected = numericalGradient<Unit3>(
        boost::bind(&Unit3::distance, &p, _1, boost::none), r);
    p.distance(r, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, localCoordinates0) {
  Unit3 p;
  Vector actual = p.localCoordinates(p);
  EXPECT(assert_equal(zero(2), actual, 1e-8));
}

//*******************************************************************************
TEST(Unit3, localCoordinates1) {
  Unit3 p, q(1, 6.12385e-21, 0);
  Vector actual = p.localCoordinates(q);
  CHECK(assert_equal(zero(2), actual, 1e-8));
}

//*******************************************************************************
TEST(Unit3, localCoordinates2) {
  Unit3 p, q(-1, 0, 0);
  Vector expected = (Vector(2) << M_PI, 0);
  Vector actual = p.localCoordinates(q);
  CHECK(assert_equal(expected, actual, 1e-8));
}

//*******************************************************************************
TEST(Unit3, basis) {
  Unit3 p;
  Matrix expected(3, 2);
  expected << 0, 0, 0, -1, 1, 0;
  Matrix actual = p.basis();
  EXPECT(assert_equal(expected, actual, 1e-8));
}

//*******************************************************************************
TEST(Unit3, retract) {
  Unit3 p;
  Vector v(2);
  v << 0.5, 0;
  Unit3 expected(0.877583, 0, 0.479426);
  Unit3 actual = p.retract(v);
  EXPECT(assert_equal(expected, actual, 1e-6));
  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
}

//*******************************************************************************
TEST(Unit3, retract_expmap) {
  Unit3 p;
  Vector v(2);
  v << (M_PI / 2.0), 0;
  Unit3 expected(Point3(0, 0, 1));
  Unit3 actual = p.retract(v);
  EXPECT(assert_equal(expected, actual, 1e-8));
  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
}

//*******************************************************************************
/// Returns a random vector
inline static Vector randomVector(const Vector& minLimits,
    const Vector& maxLimits) {

  // Get the number of dimensions and create the return vector
  size_t numDims = dim(minLimits);
  Vector vector = zero(numDims);

  // Create the random vector
  for (size_t i = 0; i < numDims; i++) {
    double range = maxLimits(i) - minLimits(i);
    vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
  }
  return vector;
}

//*******************************************************************************
// Let x and y be two Unit3's.
// The equality x.localCoordinates(x.retract(v)) == v should hold.
TEST(Unit3, localCoordinates_retract) {

  size_t numIterations = 10000;
  Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
      (Vector(3) << 1.0, 1.0, 1.0);
  Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0);
  for (size_t i = 0; i < numIterations; i++) {

    // Sleep for the random number generator (TODO?: Better create all of them first).
    boost::this_thread::sleep(boost::posix_time::milliseconds(0));

    // Create the two Unit3s.
    // NOTE: You can not create two totally random Unit3's because you cannot always compute
    // between two any Unit3's. (For instance, they might be at the different sides of the circle).
    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
//      Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
    Vector v12 = randomVector(minXiLimit, maxXiLimit);
    Unit3 s2 = s1.retract(v12);

    // Check if the local coordinates and retract return the same results.
    Vector actual_v12 = s1.localCoordinates(s2);
    EXPECT(assert_equal(v12, actual_v12, 1e-3));
    Unit3 actual_s2 = s1.retract(actual_v12);
    EXPECT(assert_equal(s2, actual_s2, 1e-3));
  }
}

//*******************************************************************************
// Let x and y be two Unit3's.
// The equality x.localCoordinates(x.retract(v)) == v should hold.
TEST(Unit3, localCoordinates_retract_expmap) {

  size_t numIterations = 10000;
  Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
      (Vector(3) << 1.0, 1.0, 1.0);
  Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI);
  for (size_t i = 0; i < numIterations; i++) {

    // Sleep for the random number generator (TODO?: Better create all of them first).
    boost::this_thread::sleep(boost::posix_time::milliseconds(0));

    // Create the two Unit3s.
    // Unlike the above case, we can use any two sphers.
    Unit3 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
//      Unit3 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
    Vector v12 = randomVector(minXiLimit, maxXiLimit);

    // Magnitude of the rotation can be at most pi
    if (v12.norm() > M_PI)
      v12 = v12 / M_PI;
    Unit3 s2 = s1.retract(v12);

    // Check if the local coordinates and retract return the same results.
    Vector actual_v12 = s1.localCoordinates(s2);
    EXPECT(assert_equal(v12, actual_v12, 1e-3));
    Unit3 actual_s2 = s1.retract(actual_v12);
    EXPECT(assert_equal(s2, actual_s2, 1e-3));
  }
}

//*******************************************************************************
//TEST( Pose2, between )
//{
//  // <
//  //
//  //       ^
//  //
//  // *--0--*--*
//  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
//  Pose2 gT2(M_PI, Point2(-1,4));  // robot at (-1,4) loooking at negative x
//
//  Matrix actualH1,actualH2;
//  Pose2 expected(M_PI/2.0, Point2(2,2));
//  Pose2 actual1 = gT1.between(gT2);
//  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
//  EXPECT(assert_equal(expected,actual1));
//  EXPECT(assert_equal(expected,actual2));
//
//  Matrix expectedH1 = (Matrix(3,3) <<
//      0.0,-1.0,-2.0,
//      1.0, 0.0,-2.0,
//      0.0, 0.0,-1.0
//  );
//  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
//  EXPECT(assert_equal(expectedH1,actualH1));
//  EXPECT(assert_equal(numericalH1,actualH1));
//  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
//  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
//
//  Matrix expectedH2 = (Matrix(3,3) <<
//       1.0, 0.0, 0.0,
//       0.0, 1.0, 0.0,
//       0.0, 0.0, 1.0
//  );
//  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
//  EXPECT(assert_equal(expectedH2,actualH2));
//  EXPECT(assert_equal(numericalH2,actualH2));
//
//}

//*******************************************************************************
TEST(Unit3, Random) {
  boost::mt19937 rng(42);
  // Check that is deterministic given same random seed
  Point3 expected(-0.667578, 0.671447, 0.321713);
  Point3 actual = Unit3::Random(rng).point3();
  EXPECT(assert_equal(expected,actual,1e-5));
  // Check that means are all zero at least
  Point3 expectedMean, actualMean;
  for (size_t i = 0; i < 100; i++)
    actualMean = actualMean + Unit3::Random(rng).point3();
  actualMean = actualMean / 100;
  EXPECT(assert_equal(expectedMean,actualMean,0.1));
}

//*************************************************************************
TEST (Unit3, FromPoint3) {
  Matrix actualH;
  Point3 point(1, -2, 3); // arbitrary point
  Unit3 expected(point);
  EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8));
  Matrix expectedH = numericalDerivative11<Unit3, Point3>(
      boost::bind(Unit3::FromPoint3, _1, boost::none), point);
  EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

/* ************************************************************************* */
int main() {
  srand(time(NULL));
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check log_2hi behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2hi;
  using boost::simd::Log_2hi;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log_2hi<T>()), T);
  STF_EQUAL(Log_2hi<T>(), ref);
  STF_EQUAL(log_2hi( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log_2hi behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2hi;
  using boost::simd::Log_2hi;
  T ref = T(0.6931471803691238);
  #ifdef HAS_LONG_DOUBLE
  using boost::simd::Log_2lo;
  using boost::simd::Eps;
  long double L_2 = 0.693147180559945309417232121458;
  STF_LESS((double)((long double)(Log_2lo<double>())+(long double)(Log_2hi<double>()))- L_2, Eps<double>()/2);
  #endif

  STF_TYPE_IS(decltype(Log_2hi<T>()), T);
  STF_IEEE_EQUAL(Log_2hi<T>(), ref);
  STF_IEEE_EQUAL(log_2hi( as(T{}) ), ref);
}

STF_CASE_TPL( "Check log_2hi behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log_2hi;
  using boost::simd::Log_2hi;
  using boost::simd::Log_2lo;
  using boost::simd::Log_2;
  using boost::simd::Eps;
  STF_LESS(float(double(Log_2lo<float>())+double(Log_2hi<float>())- Log_2<double>()), Eps<float>());
  T ref = T(6.9335937500000000e-1);
  STF_TYPE_IS(decltype(Log_2hi<T>()), T);
  STF_IEEE_EQUAL(Log_2hi<T>(), ref);
  STF_IEEE_EQUAL(log_2hi( as(T{}) ), ref);
}


// BLAS level 1 (vector) -- complex numbers

#include <iostream>
#include <cmath>
#include <complex>

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "utils.h"

namespace blas = boost::numeric::bindings::blas;
namespace ublas = boost::numeric::ublas;

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;
typedef ublas::vector<cmplx_t> vct_t;

int main() {

  int n = 6;

  cout << endl;
  vct_t v (n);
  init_v (v, times_plus<cmplx_t> (cmplx_t (1, -1), cmplx_t (0, .1)));
  print_v (v, "v");

  blas::scal (2.0, v);
  print_v (v, "2.0 v");

  blas::scal (cmplx_t (-1, 0), v);
  print_v (v, "(-1, 0) v");

  blas::scal (cmplx_t (0, 1), v);
  print_v (v, "(0, 1) v");

  blas::set (cmplx_t (1, -1), v);
  print_v (v, "v");
  vct_t v1 (n);
  blas::set (cmplx_t (0, -1), v1);
  print_v (v1, "v1");

  cout << endl;
  cout << "v^T v1 = " << blas::dot (v, v1) << " == "
    << blas::dotu (v, v1) << " == "
    << inner_prod (v, v1) << endl;
  cout << "v^T v = " << blas::dot (v, v) << " == "
    << blas::dotu (v, v) << " == "
    << inner_prod (v, v) << endl;
  cout << "v1^T v1 = " << blas::dot (v1, v1) << " == "
    << blas::dotu (v1, v1) << " == "
    << inner_prod (v1, v1) << endl;

  cout << endl;
  cout << "v^H v1 = " << blas::dotc (v, v1) << " == "
    << inner_prod (conj (v), v1) << " == "
    << inner_prod (v1, conj (v)) << " != "
    << inner_prod (v, conj (v1)) << endl;
  cout << "v^H v = " << blas::dotc (v, v) << " == "
    << inner_prod (conj (v), v) << " == "
    << inner_prod (v, conj (v)) << endl;
  cout << "v1^H v1 = " << blas::dotc (v1, v1) << " == "
    << inner_prod (conj (v1), v1) << " == "
    << inner_prod (v1, conj (v1)) << endl;

  cout << endl;
  cout << "||v||_1 = " << blas::asum (v) << endl;
  cout << "||v||_2 = " << blas::nrm2 (v) << " == "
    << norm_2 (v) << endl;

  cout << endl;
}


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

TEST(MathFunctions,logical_gt) {
  using stan::math::logical_gt;
  EXPECT_TRUE(logical_gt(1,0));
  EXPECT_TRUE(logical_gt(2,1.00));
  EXPECT_TRUE(logical_gt(2.0,1));
  EXPECT_TRUE(logical_gt(0,-1));

  EXPECT_FALSE(logical_gt(1,1));
  EXPECT_FALSE(logical_gt(5.7,5.7));
  EXPECT_FALSE(logical_gt(-5.7,9.0));
  EXPECT_FALSE(logical_gt(0,0.0));
}

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

  EXPECT_FALSE(stan::math::logical_gt(1.0, nan));
  EXPECT_FALSE(stan::math::logical_gt(nan, 2.0));
  EXPECT_FALSE(stan::math::logical_gt(nan, nan));
}


//==================================================================================================
/*!
  @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_FREXP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FREXP_HPP_INCLUDED

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

  /*!
    @ingroup group-ieee
    This function object returns a mantissa and an exponent pair for the input

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

    @par Semantic:

    For every parameter of floating type

    @code
    std::tie(m, e)= frexp(x);
    @endcode

    is similar to:

    @code
    auto e = tofloat(exponent(x)+1);
    auto m = mantissa(x)/2;
    @endcode

    @par Notes:

    - Without the pedantic_ decorator,  calling @c frexp on @c Nan or @c Inf
    has undefined behavior.

    - the exponent and matissa are both returned as floating values:
      if you need integral type exponent (as in the standard library)
      use @ref ifrexp

    - This function splits a floating point value \f$x\f$ in a signed
      mantissa \f$m\f$ and an exponent \f$e\f$ so that:  \f$x = m\times 2^e\f$,
      with absolute value of \f$m \in [0.5, 1[\f$ (except for \f$x = 0\f$)

      @warningbox{Take care that these results differ from the returns
      of the functions @ref mantissa and @ref exponent}

    @par Decorators

     - pedantic_ slower, but special values as @ref Nan or @ref Inf are handled properly.

     - std_ transmits the call to @c std::frexp and converts the exponent.

    @see ifrexp, exponent, mantissa

    @par Example:

      @snippet frexp.cpp frexp

    @par Possible output:

      @snippet frexp.txt frexp

  **/
  std::pair<IEEEValue, IEEEValue> frexp(IEEEValue const& x);
} }
#endif

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

#endif


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
#include <cassert>
#include <iostream>
#include <iomanip>
#include <numeric>
#include <cmath>
#include <atomic>
#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ_pX.h>
#include <NTL/xdouble.h>
#include "NumbTh.h"
#include "PAlgebra.h"
#include "DoubleCRT.h"
#include "Context.h"
#include "sample.h"
#include "timing.h"
#include "norms.h"

NTL_CLIENT

void printHistogram(const vector<double>& data,
                    const cx_double& mean, double step)
{
  vector<long> hist(8,0); // histogram: hist[i]= # of x'es with |x|<i*stdev
  double max = 0.0;
  double sum=0.0, sumSqr=0.0;
  for (double x: data) {
    sum += x;
    sumSqr += x*x;
    long j = std::floor(x/step);
    if (j >= lsize(hist)) hist.resize(j+1, 0);
    hist[j]++;
    if (x > max) max = x;
  }
  sum /= data.size();    // E[x]
  sumSqr /= data.size(); // E[x^2]
  double stdev = sqrt(sumSqr - (sum*sum));

  for (long i=hist.size()-1; i>0; i--) // cumulative
    hist[i-1] += hist[i];

  vector<double> dhist(hist.size(), 1.0);
  vector<double> ratio(hist.size(), 0.0);
  for (long i=1; i<lsize(dhist); i++) {
    dhist[i] = hist[i]/double(hist[0]);
    if (dhist[i]>0.0) ratio[i] = dhist[i-1]/dhist[i];
  }
  cout << data.size() << " points, mean="<<mean<<endl;
  cout << "size mean="<<sum<<", stdev="<<stdev
       << ", max="<<max<<".  Step="<<step<<endl;
  cout << "  histogram ="<< hist<<endl
       << " probability="<<dhist<<endl
       << "      ratio ="<<ratio<<endl<<endl;
}

void freshCtxtNoise(zzX& f, const Context& context,
                    double sigma, bool modPhimX)
{
  zzX s, r, e1, e2, e3;
  const PAlgebra& palg = context.zMStar;
  sampleSmallBounded(s, context);
  if (modPhimX) {
    sampleSmall(r, palg.getPhiM()-1);
    sampleGaussian(e1, palg.getPhiM()-1, sigma);
    sampleGaussianBounded(e2, context, sigma);
    sampleGaussian(e3, palg.getPhiM()-1, sigma);
  } else {
    sampleSmall(r, context);
    sampleGaussian(e1, context, sigma);
    sampleGaussianBounded(e2, context, sigma);
    sampleGaussian(e3, context, sigma);
  }
  f = MulMod(s, e1, palg) + MulMod(r, e2, palg) + e3;
}

void roundingNoise(zzX& f, const Context& context,
                   long p2r, bool modPhimX)
{
  zzX s, e1, e2;
  const PAlgebra& palg = context.zMStar;
  sampleSmallBounded(s, context);
  if (modPhimX) {
    sampleUniform(e1, palg.getPhiM()-1, p2r);
    sampleUniform(e2, palg.getPhiM()-1, p2r);
  } else {
    sampleUniform(e1, context, p2r);
    sampleUniform(e2, context, p2r);
  }
  f = MulMod(s, e1, palg) + e2;
}

int main(int argc, char **argv)
{
  FHE_NTIMER_START(init);
  // get parameters from the command line
  ArgMapping amap;

  // long noPrint = 1;
  // amap.arg("noPrint", noPrint, "suppress printouts");

  long m = 15;
  amap.arg("m", m, "the cyclotomic index");
  double sigma = 0.0;
  amap.arg("sigma", sigma, "nomral standard deviation", "heristic");
  long p = 2;
  amap.arg("p", p, "plaintext base");
  long r = 1;
  amap.arg("r", r, "lifting");
  long N = 1000;
  amap.arg("N", N, "# of samples to use");
  long seed=0;
  amap.arg("seed", seed, "PRG seed");
  long nt=4;
  amap.arg("nt", nt, "number of threads");
  amap.parse(argc, argv);

  if (seed != 0) NTL::SetSeed(ZZ(seed));
  if (nt > 1)    NTL::SetNumThreads(nt);
  long p2r = power_long(p, r);
  if (sigma<=0.0) {
    if (m&1) // odd m
      sigma = 3.2*sqrt(m);
    else
      sigma=3.2;
  }

  Context context(m, p, r);
  const PAlgebra& palg = context.zMStar;
  buildModChain(context, /*L=*/5, /*c=*/3);
  long phim = palg.getPhiM();
  FHE_NTIMER_STOP(init);

  cout << "m="<<m<<", phi(m)="<<palg.getPhiM()
       << ", sigma="<<std::setprecision(3)<<sigma
       << ", p^r="<<p2r << endl;

  zzX f;
  vector<double> data;
  cx_double sum, mean;
  double step;

  step = (sigma+0.1)*0.54*(1+((m&1)? sqrt(phim*m): phim));
  // fresh ciphertext, sampling mod X^m-1
  data.resize(0);
  sum = 0;
  for (long i=0; i<N; i++) {
    std::vector<cx_double> cemb;
    freshCtxtNoise(f, context, sigma, false);
    canonicalEmbedding(cemb, f, palg); // Canonical embedding of f
    for (auto& entry : cemb) {
      double sz2 = conv<double>(std::norm<double>(entry));
      data.push_back(std::sqrt(sz2));
      sum += entry/double(lsize(cemb));
    }
  }
  mean = sum / double(N);
  cout << "fresh ctxt noise, sample mod X^m-1: ";
  printHistogram(data, mean, step);
  data.resize(0);
  sum = 0;
  // rounding error, mod Phi_m(X)

  step = (2*p2r+1)*(phim-2)/8.0;
  for (long i=0; i<N; i++) {
    std::vector<cx_double> cemb;
    roundingNoise(f, context, p2r, true);
    canonicalEmbedding(cemb, f, palg); // Canonical embedding of f
    for (auto& entry : cemb) {
      double sz2 = conv<double>(std::norm<double>(entry));
      data.push_back(std::sqrt(sz2));
      sum += entry/double(lsize(cemb));
    }
  }
  mean = sum / double(N);
  cout << "\nrounding noise, sample mod Phi_m(X): ";
  printHistogram(data, mean, step);
  data.resize(0);
  sum = 0;

  cout << endl;

  //  printAllTimers();
  return 0;
}

/********************************************************************/
#if 0 // OLD CODE
  vector<xdouble> l2(8, xdouble(0.0));
  vector<xdouble> ratio(8, xdouble(1.0));

  cout << "*** m="<<m<<", sampling "<<N<<" different f's ***\n";
  for (long i=0; i<N; i++) {
    ZZX f;
    sampleSmall(f, m);
    rem(f, f, phimX);
    if (IsZero(f)) continue;
    xdouble ll = embeddingL2NormSquared(f,m)/phim;
    l2[0] += ll;
    for (long j=1; j<lsize(l2); j++) {
      NTL::SqrMod(f, f, phimX); // fd -> fd^2
      ll *= ll;

      xdouble tt = embeddingL2NormSquared(f, m)/phim;
      l2[j] += tt;

      tt /= ll;
      if (tt>ratio[j]) ratio[j] = tt;
    }
  }
  xdouble base = l2[0] / N;
  long e = 1;
  xdouble factorial(1.0);
  for (long j=0; j<lsize(l2); j++) {
    l2[j] /= N;
    for (long i=e/2 +1; i<=e; i++) factorial *= i;
    cout << "E[|f^"<<e<<"|^2]="<<l2[j]
         << ",\t= "<<(l2[j]/base)<<"*E[|f^2|]^{"<<e<<"} (vs. "
         << e<<"! ="<<factorial<<")\n";
    cout << "\t\t\t max ratio = "<<ratio[j]<<endl;
    base *= base;
    e *= 2;
  }
#endif
#if 0
  step = (1+sqrt(phim*log(phim)))*0.85;
  for (long i=0; i<N; i++) {
    sampleSmall(f, palg);
    data.push_back(embeddingLargestCoeff(f, palg));
  }
  cout << "sampleSmall noise: ";
  printHistogram(data, mean, step);
  data.resize(0);
  step = sigma*(2+((m&1)? sqrt(m*log(phim)) : sqrt(phim*log(phim))))*1.2;
  for (long i=0; i<N; i++) {
    sampleGaussian(f, palg, sigma);
    data.push_back(embeddingLargestCoeff(f, palg));
  }
  cout << "sampleGaussian noise: ";
  printHistogram(data, mean, step);
  data.resize(0);
  exit(0);
#endif
#if 0
  // First test: fresh ciphertext, sampling mod Phi_m(X)
  for (long i=0; i<N; i++) {
    std::vector<cx_double> cemb;
    freshCtxtNoise(f, palg, sigma, true);
    canonicalEmbedding(cemb, f, palg); // Canonical embedding of f
    for (auto& entry : cemb) {
      double sz2 = conv<double>(std::norm<double>(entry));
      data.push_back(std::sqrt(sz2));
      sum += entry/double(lsize(cemb));
    }
  }
  mean = sum / double(N);
  cout << "fresh ctxt noise, sample mod Phi_m(X): ";
  printHistogram(data, mean, step);
#endif
#if 0
  for (long i=0; i<N; i++) {
    std::vector<cx_double> cemb;
    roundingNoise(f, palg, p2r, false);
    canonicalEmbedding(cemb, f, palg); // Canonical embedding of f
    for (auto& entry : cemb) {
      double sz2 = conv<double>(std::norm<double>(entry));
      data.push_back(std::sqrt(sz2));
      sum += entry/double(lsize(cemb));
    }
  }
  mean = sum / double(N);
  cout << "rounding noise, sample mod X^m-1: ";
  printHistogram(data, mean, step);
#endif


#include <rct_optimizations/validation/homography_validation.h>
#include <Eigen/Dense>
#include <rct_optimizations/types.h>

namespace rct_optimizations
{
GridCorrespondenceSampler::GridCorrespondenceSampler(const std::size_t rows_, const std::size_t cols_, const std::size_t stride_)
  : rows(rows_)
  , cols(cols_)
  , stride(stride_)
{
}

std::vector<std::size_t> GridCorrespondenceSampler::getSampleCorrespondenceIndices() const
{
  const std::size_t n_samples = 4;

  // Make sure there are at least two times as many points as the number of sample points
  if ((rows * cols / 2) < n_samples)
  {
    std::stringstream ss;
    ss << "Number of correspondences does not exceed minimum of " << n_samples * 2 << " (" << rows * cols << " provided)";
    throw std::runtime_error(ss.str());
  }

  std::vector<std::size_t> correspondence_indices;
  correspondence_indices.reserve(n_samples);

  // Sample points should be the corners of the grid, using the first element in the stride
  std::size_t upper_left_idx = 0;
  std::size_t upper_right_idx = (cols - 1) * stride;
  std::size_t lower_left_idx = (rows - 1) * (cols * stride);
  std::size_t lower_right_idx = (rows * cols * stride) - stride;

  correspondence_indices.push_back(upper_left_idx);
  correspondence_indices.push_back(upper_right_idx);
  correspondence_indices.push_back(lower_left_idx);
  correspondence_indices.push_back(lower_right_idx);

  return correspondence_indices;
}

RandomCorrespondenceSampler::RandomCorrespondenceSampler(const std::size_t n_correspondences_, const std::size_t n_samples_, const unsigned seed_)
  : n_correspondences(n_correspondences_)
  , n_samples(n_samples_)
  , seed(seed_)
{
  const unsigned min_samples = 4;
  if (n_samples < min_samples)
  {
    std::stringstream ss;
    ss << "Not enough samples specified: " << n_samples << " vs. " << min_samples << " required";
    throw std::runtime_error(ss.str());
  }
  if (n_samples > n_correspondences)
  {
    std::stringstream ss;
    ss << "Number of correspondences (" << n_correspondences << ") must exceed number of samples (" << n_samples << ")";
    throw std::runtime_error(ss.str());
  }
}

std::vector<std::size_t> RandomCorrespondenceSampler::getSampleCorrespondenceIndices() const
{
  // Create a random number generator with a uniform distribution across all indices
  std::mt19937 rand_gen(seed);
  std::uniform_int_distribution<std::size_t> dist(0, n_correspondences - 1);
  auto fn = [&rand_gen, &dist]() -> std::size_t { return dist(rand_gen); };

  // Generate a vector of 4 random correspondence indices
  std::vector<std::size_t> output(n_samples);
  std::generate(output.begin(), output.end(), fn);

  return output;
}

Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set &correspondences,
                                         const CorrespondenceSampler &correspondence_sampler)
{
  /* There is a 3x3 homography matrix, H, that can transform a point from one plane onto a different plane
   * | u | = k * | H00 H01 H02 | * | x |
   * | v |       | H10 H11 H12 |   | y |
   * | 1 |       | H20 H12  1  |   | 1 |
   *
   * In our case we have 2 sets of known corresponding planar points: points on the planar target, and points in the image plane
   * Therefore, there is some matrix, H, which can transform target points into the image plane.
   * If the target points and camera points actually match, we should be able to:
   *   1. Calculate H for a subset of corresponding points
   *   2. Transform the remaining target points by H to obtain estimates of their locations in the image plane
   *   3. Compare the calculated estimations to the actual image points to make sure they are very close. If they are not close, we know that the correspondences are not valid
   *
   * The matrix H has 8 unique values.
   * These 8 values of the homography matrix can be solved for, given a set of (at least) 8 corresponding planar vectors, by rearranging the above equations:
   *
   * A * H = b, where
   * H = inv(A) * b
   *   - A is matrix (size 2*n x 8), where n is the number of corresponding vectors
   *   - H is a vector (size 8 x 1) of the unknown elements of the homography matrix
   *   - B is a vector (size 2*n x 1) representing the elements of one set of planar vectors
   *
   *                  A               *              H    =    b
   * |-x0 -y0  -1   0    0    0   u0*x0   u0*y0 | * | H00 | = | -u0 |
   * | 0   0   0  -x0   -y0  -1   v0*x0   v0*y0 | * | H01 | = | -v0 |
   *                              ...
   * |-x7 -y7  -1   0    0    0   u7*x7   u7*y7 | * | H20 | = | -u7 |
   * | 0   0   0  -x7   -y7  -1   v7*x7   v7*y7 | * | H21 | = | -v7 |
   *
   */

  // Select the points that we want to use to create the H matrix
  std::vector<std::size_t> sample_correspondence_indices = correspondence_sampler
                                                             .getSampleCorrespondenceIndices();
  std::size_t n_samples = sample_correspondence_indices.size();

  // Ensure that there are enough points for testing outside of the sampled set
  if (correspondences.size() < 2 * n_samples)
  {
    std::stringstream ss;
    ss << "Correspondences size is not more than 2x sample size (" << correspondences.size()
       << " correspondences vs. " << n_samples << ")";
    throw std::runtime_error(ss.str());
  }

  // Create the A and b matrices
  Eigen::MatrixXd A(2 * n_samples, 8);
  Eigen::MatrixXd b(2 * n_samples, 1);

  // Fill the A and B matrices with data from the selected correspondences
  for (std::size_t i = 0; i < n_samples; ++i)
  {
    std::size_t corr_idx = sample_correspondence_indices.at(i);
    const Correspondence2D3D &corr = correspondences.at(corr_idx);

    //assign A row-th row:
    const double x = corr.in_target.x();
    const double y = corr.in_target.y();
    const double u = corr.in_image.x();
    const double v = corr.in_image.y();
    A.row(2 * i) << -x, -y, -1.0, 0.0, 0.0, 0.0, u * x, u * y;
    A.row(2 * i + 1) << 0.0, 0.0, 0.0, -x, -y, -1.0, v * x, v * y;

    b.block<2, 1>(2 * i, 0) = -1.0 * corr.in_image;
  }

  // Create the homography matrix
  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> H = Eigen::Matrix3d::Ones();

  // Map the elements of the H matrix into a column vector and solve for the first 8
  {
    Eigen::Map<Eigen::VectorXd> hv(H.data(), 9);
    hv.head<8>() = A.fullPivLu().solve(b);
  }

  // Estimate the image locations of all the target observations and compare to the actual image locations
  Eigen::VectorXd error(correspondences.size());
  for (std::size_t i = 0; i < correspondences.size(); ++i)
  {
    const Correspondence2D3D &corr = correspondences[i];

    // Calculate the scaling factor
    double ki = 1.0 / (H(2, 0) * corr.in_target.x() + H(2, 1) * corr.in_target.y() + 1.0);

    // Replace the z-element of the point with 1
    Eigen::Vector3d xy(corr.in_target);
    xy(2) = 1.0;

    // Estimate the point in the image plane
    Eigen::Vector3d in_image_estimate = ki * H * xy;

    // Calculate the error
    Eigen::Vector2d image_error = corr.in_image - in_image_estimate.head<2>();
    error(i) = image_error.norm();
  }

  return error;
}

Eigen::VectorXd calculateHomographyError(const Correspondence3D3D::Set& correspondences,
                                         const CorrespondenceSampler& correspondence_sampler)
{
  // Convert the 3D correspondence points into 2D points by scaling the x and y components by the z component
  Correspondence2D3D::Set correspondences_2d;
  correspondences_2d.reserve(correspondences.size());

  // Store the z values for scaling later
  Eigen::VectorXd z_values(correspondences.size());

  for (std::size_t i = 0; i < correspondences.size(); ++i)
  {
    const auto& corr = correspondences[i];

    // Store the z value of the correspondence
    z_values[i] = corr.in_image.z();

    // Generate a scaled 2D correspondence
    Correspondence2D3D corr_2d;
    corr_2d.in_target = corr.in_target;
    corr_2d.in_image = (corr.in_image / corr.in_image.z()).head<2>();
    correspondences_2d.push_back(corr_2d);
  }

  // Calculate the homography error
  Eigen::VectorXd error = calculateHomographyError(correspondences_2d, correspondence_sampler);

  // Scale the errors again by the z values of the original correspondences
  return error.cwiseProduct(z_values);
}

} // namespace rct_optimizations


//////////////////////////////////////////////////////////////////
//
// lazy_thunk_tests.cpp
//
// Tests for thunk functions.
//
//
/*=============================================================================
    Copyright (c) 2000-2003 Brian McNamara and Yannis Smaragdakis
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2015 John Fletcher

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

#include <iostream>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/phoenix/function/lazy_prelude.hpp>

#include <boost/detail/lightweight_test.hpp>

using namespace boost::phoenix;

using std::cout;
using std::endl;

int main() {
   using boost::phoenix::arg_names::arg1;
   using boost::phoenix::arg_names::arg2;

   BOOST_TEST( thunk1(inc,1)()()             == 2);
   BOOST_TEST( thunk1(inc,arg1)(1)()         == 2);
   BOOST_TEST( thunk2(plus,1,2)()()          == 3);
   BOOST_TEST( thunk2(plus,arg1,arg2)(1,2)() == 3);

   list<int> l  = enum_from_to(1,5);
   list<int> l4 = take(4,l)();
   BOOST_TEST( foldl(plus,0,l4)()              == 10);
   BOOST_TEST( thunk3(foldl,plus,0,l4)()()     == 10);
   BOOST_TEST( thunk3(foldl,plus,arg1,l4)(0)() == 10);

   return boost::report_errors();
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <iterator>
#include <vector>
#include <list>
// Use boost::queue instead of std::queue because std::queue doesn't
// model Buffer; it has to top() function. -Jeremy
#include <boost/pending/queue.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace std;
using namespace boost;
/*
  This example does a best-first-search (using dijkstra's) and
  simultaneously makes a copy of the graph (assuming the graph is
  connected).

  Example Graph: (p. 90 "Data Structures and Network Algorithms", Tarjan)

              g
            3+ +2
            / 1 \
           e+----f
           |+0 5++
           | \ / |
         10|  d  |12
           |8++\7|
           +/ | +|
           b 4|  c
            \ | +
            6+|/3
              a

  Sample Output:
a --> c d
b --> a d
c --> f
d --> c e f
e --> b g
f --> e g
g -->
Starting graph:
a(32767); c d
c(32767); f
d(32767); c e f
f(32767); e g
e(32767); b g
g(32767);
b(32767); a d
Result:
a(0); d c
d(4); f e c
c(3); f
f(9); g e
e(4); g b
g(7);
b(14); d a

*/

typedef property<vertex_color_t, default_color_type,
         property<vertex_distance_t,int> > VProperty;
typedef int weight_t;
typedef property<edge_weight_t,weight_t> EProperty;

typedef adjacency_list<vecS, vecS, directedS, VProperty, EProperty > Graph;

template <class Tag>
struct endl_printer
  : public boost::base_visitor< endl_printer<Tag> >
{
  typedef Tag event_filter;
  endl_printer(std::ostream& os) : m_os(os) { }
  template <class T, class Graph>
  void operator()(T, Graph&) { m_os << std::endl; }
  std::ostream& m_os;
};
template <class Tag>
endl_printer<Tag> print_endl(std::ostream& os, Tag) {
  return endl_printer<Tag>(os);
}

template <class PA, class Tag>
struct edge_printer
 : public boost::base_visitor< edge_printer<PA, Tag> >
{
  typedef Tag event_filter;

  edge_printer(PA pa, std::ostream& os) : m_pa(pa), m_os(os) { }

  template <class T, class Graph>
  void operator()(T x, Graph& g) {
    m_os << "(" << get(m_pa, source(x, g)) << ","
         << get(m_pa, target(x, g)) << ") ";
  }
  PA m_pa;
  std::ostream& m_os;
};
template <class PA, class Tag>
edge_printer<PA, Tag>
print_edge(PA pa, std::ostream& os, Tag) {
  return edge_printer<PA, Tag>(pa, os);
}

template <class NewGraph, class Tag>
struct graph_copier
  : public boost::base_visitor<graph_copier<NewGraph, Tag> >
{
  typedef Tag event_filter;

  graph_copier(NewGraph& graph) : new_g(graph) { }

  template <class Edge, class Graph>
  void operator()(Edge e, Graph& g) {
    add_edge(source(e, g), target(e, g), new_g);
  }
private:
  NewGraph& new_g;
};
template <class NewGraph, class Tag>
inline graph_copier<NewGraph, Tag>
copy_graph(NewGraph& g, Tag) {
  return graph_copier<NewGraph, Tag>(g);
}

template <class Graph, class Name>
void print(Graph& G, Name name)
{
  typename boost::graph_traits<Graph>::vertex_iterator ui, uiend;
  for (boost::tie(ui, uiend) = vertices(G); ui != uiend; ++ui) {
    cout << name[*ui] << " --> ";
    typename boost::graph_traits<Graph>::adjacency_iterator vi, viend;
    for(boost::tie(vi, viend) = adjacent_vertices(*ui, G); vi != viend; ++vi)
      cout << name[*vi] << " ";
    cout << endl;
  }

}

int
main(int , char* [])
{
  // Name and ID numbers for the vertices
  char name[] = "abcdefg";
  enum { a, b, c, d, e, f, g, N};

  Graph G(N);
  boost::property_map<Graph, vertex_index_t>::type
    vertex_id = get(vertex_index, G);

  std::vector<weight_t> distance(N, (numeric_limits<weight_t>::max)());
  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
  std::vector<Vertex> parent(N);

  typedef std::pair<int,int> E;

  E edges[] = { E(a,c), E(a,d),
                E(b,a), E(b,d),
                E(c,f),
                E(d,c), E(d,e), E(d,f),
                E(e,b), E(e,g),
                E(f,e), E(f,g) };

  int weight[] = { 3, 4,
                   6, 8,
                   12,
                   7, 0, 5,
                   10, 3,
                   1, 2 };

  for (int i = 0; i < 12; ++i)
    add_edge(edges[i].first, edges[i].second, weight[i], G);

  print(G, name);

  adjacency_list<listS, vecS, directedS,
    property<vertex_color_t, default_color_type> > G_copy(N);

  cout << "Starting graph:" << endl;

  std::ostream_iterator<int> cout_int(std::cout, " ");
  std::ostream_iterator<char> cout_char(std::cout, " ");

  boost::queue<Vertex> Q;
  boost::breadth_first_search
    (G, vertex(a, G), Q,
     make_bfs_visitor(
     boost::make_list
      (write_property(make_iterator_property_map(name, vertex_id,
                                                name[0]),
                      cout_char, on_examine_vertex()),
       write_property(make_iterator_property_map(distance.begin(),
                                                vertex_id,
                                                distance[0]),
                      cout_int, on_examine_vertex()),
       print_edge(make_iterator_property_map(name, vertex_id,
                                            name[0]),
                  std::cout, on_examine_edge()),
       print_endl(std::cout, on_finish_vertex()))),
     get(vertex_color, G));

  std::cout << "about to call dijkstra's" << std::endl;

  parent[vertex(a, G)] = vertex(a, G);
  boost::dijkstra_shortest_paths
    (G, vertex(a, G),
     distance_map(make_iterator_property_map(distance.begin(), vertex_id,
                                             distance[0])).
     predecessor_map(make_iterator_property_map(parent.begin(), vertex_id,
                                                parent[0])).
     visitor(make_dijkstra_visitor(copy_graph(G_copy, on_examine_edge()))));

  cout << endl;
  cout << "Result:" << endl;
  boost::breadth_first_search
    (G, vertex(a, G),
     visitor(make_bfs_visitor(
     boost::make_list
     (write_property(make_iterator_property_map(name, vertex_id,
                                                name[0]),
                     cout_char, on_examine_vertex()),
      write_property(make_iterator_property_map(distance.begin(),
                                                vertex_id,
                                                distance[0]),
                     cout_int, on_examine_vertex()),
      print_edge(make_iterator_property_map(name, vertex_id,
                                            name[0]),
                 std::cout, on_examine_edge()),
      print_endl(std::cout, on_finish_vertex())))));

  return 0;
}