/**
 * \file dcs/math/traits/float.hpp
 *
 * \brief Traits class for floating-point type.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_TRAITS_FLOAT_HPP
#define DCS_MATH_TRAITS_FLOAT_HPP

#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>
#include <dcs/math/detail/float.hpp>
#include <limits>

namespace dcs { namespace math {

template <typename T, typename Enable_ = void>
struct float_traits;

template <typename T>
struct float_traits<T, typename ::boost::enable_if< ::boost::is_floating_point<T> >::type>
{
	/// Default tolerance for floating-point comparison.
	static const T tolerance;

	static bool approximately_equal(T x, T y, T tol)
	{
		return detail::approximately_equal(x, y, tol);
	}

	static bool approximately_equal(T x, T y)
	{
		return detail::approximately_equal(x, y, tolerance);
	}

	static bool essentially_equal(T x, T y, T tol)
	{
		return detail::essentially_equal(x, y, tol);
	}

	static bool essentially_equal(T x, T y)
	{
		return detail::essentially_equal(x, y, tolerance);
	}

	static bool definitely_less(T x, T y, T tol)
	{
		return detail::definitely_less(x, y, tol);
	}

	static bool definitely_less(T x, T y)
	{
		return detail::definitely_less(x, y, tolerance);
	}

	/// \deprecated Use \c approximately_less_equal or \c essentially_less_equal
	static bool definitely_less_equal(T x, T y, T tol)
	{
		return definitely_less(x, y, tol) || approximately_equal(x, y, tol);
	}

	/// \deprecated Use \c approximately_less_equal or \c essentially_less_equal
	static bool definitely_less_equal(T x, T y)
	{
		return definitely_less_equal(x, y, tolerance);
	}

	static bool approximately_less_equal(T x, T y, T tol)
	{
		return definitely_less(x, y, tol) || approximately_equal(x, y, tol);
	}

	static bool approximately_less_equal(T x, T y)
	{
		return approximately_less_equal(x, y, tolerance);
	}

	static bool essentially_less_equal(T x, T y, T tol)
	{
		return definitely_less(x, y, tol) || essentially_equal(x, y, tol);
	}

	static bool essentially_less_equal(T x, T y)
	{
		return essentially_less_equal(x, y, tolerance);
	}

	static bool definitely_greater(T x, T y, T tol)
	{
		return detail::definitely_greater(x, y, tol);
	}

	static bool definitely_greater(T x, T y)
	{
		return detail::definitely_greater(x, y, tolerance);
	}

	/// \deprecated Use \c approximately_less_equal or \c essentially_less_equal
	static bool definitely_greater_equal(T x, T y, T tol)
	{
		return definitely_greater(x, y, tol) || approximately_equal(x, y, tol);
	}

	/// \deprecated Use \c approximately_greater_equal or \c essentially_greater_equal
	static bool definitely_greater_equal(T x, T y)
	{
		return definitely_greater_equal(x, y, tolerance);
	}

	static bool approximately_greater_equal(T x, T y, T tol)
	{
		return definitely_greater(x, y, tol) || approximately_equal(x, y, tol);
	}

	static bool approximately_greater_equal(T x, T y)
	{
		return approximately_greater_equal(x, y, tolerance);
	}

	static bool essentially_greater_equal(T x, T y, T tol)
	{
		return definitely_greater(x, y, tol) || essentially_equal(x, y, tol);
	}

	static bool essentially_greater_equal(T x, T y)
	{
		return essentially_greater_equal(x, y, tolerance);
	}

	/// \deprecated Use \c definitely_min
	static T min(T x, T y, T tol = tolerance)
	{
		if (definitely_less(x, y, tol))
		{
			return x;
		}
		return y;
	}

	/// \deprecated Use \c definitely_max
	static T max(T x, T y, T tol = tolerance)
	{
		if (definitely_greater(x, y, tol))
		{
			return x;
		}
		return y;
	}

	static T definitely_min(T x, T y, T tol = tolerance)
	{
		if (definitely_less(x, y, tol))
		{
			return x;
		}
		return y;
	}

	static T definitely_max(T x, T y, T tol = tolerance)
	{
		if (definitely_greater(x, y, tol))
		{
			return x;
		}
		return y;
	}

/*
bool is_nan(float f)
{
    return (*reinterpret_cast<uint32_t*>(&f) & 0x7f800000) == 0x7f800000 && (*reinterpret_cast<uint32_t*>(&f) & 0x007fffff) != 0;
}

bool is_finite(float f)
{
    return (*reinterpret_cast<uint32_t*>(&f) & 0x7f800000) != 0x7f800000;
}

// if this symbol is defined, NaNs are never equal to anything (as is normal in IEEE floating point)
// if this symbol is not defined, NaNs are hugely different from regular numbers, but might be equal to each other
#define UNEQUAL_NANS 1
// if this symbol is defined, infinites are never equal to finite numbers (as they're unimaginably greater)
// if this symbol is not defined, infinities are 1 ULP away from +/- FLT_MAX
#define INFINITE_INFINITIES 1
//
// test whether two IEEE floats are within a specified number of representable values of each other
// This depends on the fact that IEEE floats are properly ordered when treated as signed magnitude integers
bool equal_float(float lhs, float rhs, uint32_t max_ulp_difference)
{
#ifdef UNEQUAL_NANS
	if(is_nan(lhs) || is_nan(rhs))
	{
		return false;
	}
#endif
#ifdef INFINITE_INFINITIES
	if((is_finite(lhs) && !is_finite(rhs)) || (!is_finite(lhs) && is_finite(rhs)))
	{
		return false;
	}
#endif
	int32_t left(*reinterpret_cast<int32_t*>(&lhs));
	// transform signed magnitude ints into 2s complement signed ints
	if(left < 0)
	{
		left = 0x80000000 - left;
	}
	int32_t right(*reinterpret_cast<int32_t*>(&rhs));
	// transform signed magnitude ints into 2s complement signed ints
	if(right < 0)
	{
		right = 0x80000000 - right;
	}
	if(static_cast<uint32_t>(std::abs(left - right)) <= max_ulp_difference)
	{
		return true;
	}
	return false;
}
*/
};

template <typename T>
const T float_traits<T, typename ::boost::enable_if< ::boost::is_floating_point<T> >::type>::tolerance = static_cast<T>(100)*::std::numeric_limits<T>::epsilon();

}} // Namespace dcs::math

#endif // DCS_MATH_TRAITS_FLOAT_HPP


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

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "test_gamma_hooks.hpp"
#include "handle_test_result.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the functions tgamma and lgamma, and the
// function tgamma1pm1.  There are two sets of tests, spot
// tests which compare our results with selected values computed
// using the online special function calculator at
// functions.wolfram.com, while the bulk of the accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
   //
   // G++ on Darwin: results are just slightly worse than we might hope for
   // but still pretty good:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "Mac OS",                      // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 100, 15); // test function

   //
   // G++ on Linux, result vary a bit by processor type,
   // on Itanium results are *much* better than listed here,
   // but x86 appears to have much less accurate std::pow
   // that throws off the results:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 400, 200); // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                          // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::lgamma", 30, 10);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      largest_type,                  // test type(s)
      "near (1|2|-10)",              // test data group
      "boost::math::tgamma", 10, 5);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      largest_type,                  // test type(s)
      "near (1|2|-10)",              // test data group
      "boost::math::lgamma", 50, 50);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      largest_type,                  // test type(s)
      "tgamma1pm1.*",                // test data group
      "boost::math::tgamma1pm1", 50, 15);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      "real_concept",                // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 220, 70);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      "real_concept",                // test type(s)
      "near (0|-55)",                // test data group
      "boost::math::(t|l)gamma", 130, 80);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "linux",                       // platform
      "real_concept",                // test type(s)
      "tgamma1pm1.*",                // test data group
      "boost::math::tgamma1pm1", 40, 10);  // test function
   //
   // HP-UX results:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "HP-UX",                          // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 5, 4);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "HP-UX",                       // platform
      largest_type,                  // test type(s)
      "near (0|-55)",                // test data group
      "boost::math::tgamma", 10, 5);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "HP-UX",                       // platform
      largest_type,                  // test type(s)
      "near (1|2|-10)",              // test data group
      "boost::math::lgamma", 250, 200);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "HP-UX",                          // platform
      "real_concept",                // test type(s)
      "factorials",                  // test data group
      "boost::math::lgamma", 50, 20);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "HP-UX",                          // platform
      "real_concept",                // test type(s)
      "tgamma1pm1.*",                // test data group
      "boost::math::tgamma1pm1", 200, 80);  // test function
   //
   // Tru64:
   //
   add_expected_result(
      ".*Tru64.*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "factorials",                  // test data group
      "boost::math::lgamma", 50, 20);  // test function
   //
   // Sun OS:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "Sun.*",                       // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 300, 50); // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "Sun.*",                       // platform
      "real_concept",                // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 300, 50); // test function

   //
   // Catch all cases come last:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 4, 1);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "factorials",                  // test data group
      "boost::math::lgamma", 9, 1);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "near (0|-55)",                // test data group
      "boost::math::(t|l)gamma", 200, 100);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "near (1|2|-10)",              // test data group
      "boost::math::tgamma", 10, 5);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "near (1|2|-10)",              // test data group
      "boost::math::lgamma", 14, 7);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "tgamma1pm1.*",                // test data group
      "boost::math::tgamma1pm1", 30, 9);  // test function

   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "factorials",                  // test data group
      "boost::math::tgamma", 70, 25);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "factorials",                  // test data group
      "boost::math::lgamma", 40, 4);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "near.*",                      // test data group
      "boost::math::tgamma", 80, 60);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "near.*",                      // test data group
      "boost::math::lgamma", 10000000, 10000000);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "tgamma1pm1.*",                // test data group
      "boost::math::tgamma1pm1", 20, 5);  // test function

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
void do_test_gamma(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::tgamma<value_type>;
#else
   pg funcp = boost::math::tgamma;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test tgamma against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0),
      extract_result(1));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::tgamma", test_name);
#ifdef TEST_OTHER
   if(::boost::is_floating_point<value_type>::value){
      funcp = other::tgamma;
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0),
         extract_result(1));
      print_test_result(result, data[result.worst()], result.worst(), type_name, "other::tgamma");
   }
#endif
   //
   // test lgamma against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::lgamma<value_type>;
#else
   funcp = boost::math::lgamma;
#endif
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::lgamma", test_name);
#ifdef TEST_OTHER
   if(::boost::is_floating_point<value_type>::value){
      funcp = other::lgamma;
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0),
         extract_result(2));
      print_test_result(result, data[result.worst()], result.worst(), type_name, "other::lgamma");
   }
#endif

   std::cout << std::endl;
}

template <class T>
void do_test_gammap1m1(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::tgamma1pm1<value_type>;
#else
   pg funcp = boost::math::tgamma1pm1;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test tgamma1pm1 against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0),
      extract_result(1));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::tgamma1pm1", test_name);
   std::cout << std::endl;
}

template <class T>
void test_gamma(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // three items, input value, gamma and lgamma:
   //
   // gamma and lgamma at integer and half integer values:
   // boost::array<boost::array<T, 3>, N> factorials;
   //
   // gamma and lgamma for z near 0:
   // boost::array<boost::array<T, 3>, N> near_0;
   //
   // gamma and lgamma for z near 1:
   // boost::array<boost::array<T, 3>, N> near_1;
   //
   // gamma and lgamma for z near 2:
   // boost::array<boost::array<T, 3>, N> near_2;
   //
   // gamma and lgamma for z near -10:
   // boost::array<boost::array<T, 3>, N> near_m10;
   //
   // gamma and lgamma for z near -55:
   // boost::array<boost::array<T, 3>, N> near_m55;
   //
   // The last two cases are chosen more or less at random,
   // except that one is even and the other odd, and both are
   // at negative poles.  The data near zero also tests near
   // a pole, the data near 1 and 2 are to probe lgamma as
   // the result -> 0.
   //
#  include "test_gamma_data.ipp"

   do_test_gamma(factorials, name, "factorials");
   do_test_gamma(near_0, name, "near 0");
   do_test_gamma(near_1, name, "near 1");
   do_test_gamma(near_2, name, "near 2");
   do_test_gamma(near_m10, name, "near -10");
   do_test_gamma(near_m55, name, "near -55");

   //
   // And now tgamma1pm1 which computes gamma(1+dz)-1:
   //
   do_test_gammap1m1(gammap1m1_data, name, "tgamma1pm1(dz)");
}

template <class T>
void test_spots(T)
{
   //
   // basic sanity checks, tolerance is 50 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 5000;
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(3.5)), static_cast<T>(3.3233509704478425511840640312646472177454052302295L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(0.125)), static_cast<T>(7.5339415987976119046992298412151336246104195881491L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(-0.125)), static_cast<T>(-8.7172188593831756100190140408231437691829605421405L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(-3.125)), static_cast<T>(1.1668538708507675587790157356605097019141636072094L), tolerance);
   // Lower tolerance on this one, is only really needed on Linux x86 systems, result is mostly down to std lib accuracy:
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(-53249.0/1024)), static_cast<T>(-1.2646559519067605488251406578743995122462767733517e-65L), tolerance * 3);

   int sign = 1;
   BOOST_CHECK_CLOSE(::boost::math::lgamma(static_cast<T>(3.5), &sign), static_cast<T>(1.2009736023470742248160218814507129957702389154682L), tolerance);
   BOOST_CHECK(sign == 1);
   BOOST_CHECK_CLOSE(::boost::math::lgamma(static_cast<T>(0.125), &sign), static_cast<T>(2.0194183575537963453202905211670995899482809521344L), tolerance);
   BOOST_CHECK(sign == 1);
   BOOST_CHECK_CLOSE(::boost::math::lgamma(static_cast<T>(-0.125), &sign), static_cast<T>(2.1653002489051702517540619481440174064962195287626L), tolerance);
   BOOST_CHECK(sign == -1);
   BOOST_CHECK_CLOSE(::boost::math::lgamma(static_cast<T>(-3.125), &sign), static_cast<T>(0.1543111276840418242676072830970532952413339012367L), tolerance);
   BOOST_CHECK(sign == 1);
   BOOST_CHECK_CLOSE(::boost::math::lgamma(static_cast<T>(-53249.0/1024), &sign), static_cast<T>(-149.43323093420259741100038126078721302600128285894L), tolerance);
   BOOST_CHECK(sign == -1);
}

int test_main(int, char* [])
{
   expected_results();
   BOOST_MATH_CONTROL_FP;

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_spots(0.0F);
#endif
   test_spots(0.0);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L);
   test_spots(boost::math::concepts::real_concept(0.1));
#endif

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_gamma(0.1F, "float");
#endif
   test_gamma(0.1, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_gamma(0.1L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   test_gamma(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
*/
template
<
    typename Point1,
    typename Point2 = Point1,
    typename CalculationType = void
>
class andoyer
{
    public :
    typedef typename promote_floating_point
        <
            typename select_calculation_type
                <
                    Point1,
                    Point2,
                    CalculationType
                >::type
        >::type calculation_type;

        inline andoyer()
            : m_ellipsoid()
        {}

        explicit inline andoyer(calculation_type f)
            : m_ellipsoid(f)
        {}

        explicit inline andoyer(geometry::detail::ellipsoid<calculation_type> const& e)
            : m_ellipsoid(e)
        {}

        inline calculation_type apply(Point1 const& point1, Point2 const& point2) const
        {
            return calc(get_as_radian<0>(point1), get_as_radian<1>(point1),
                            get_as_radian<0>(point2), get_as_radian<1>(point2));
        }

        inline geometry::detail::ellipsoid<calculation_type> ellipsoid() const
        {
            return m_ellipsoid;
        }

        inline calculation_type radius() const
        {
            return m_ellipsoid.a();
        }

    private :
        geometry::detail::ellipsoid<calculation_type> m_ellipsoid;

        inline calculation_type calc(calculation_type const& lon1,
                    calculation_type const& lat1,
                    calculation_type const& lon2,
                    calculation_type const& lat2) const
        {
            calculation_type const G = (lat1 - lat2) / 2.0;
            calculation_type const lambda = (lon1 - lon2) / 2.0;

            if (geometry::math::equals(lambda, 0.0)
                && geometry::math::equals(G, 0.0))
            {
                return 0.0;
            }

            calculation_type const F = (lat1 + lat2) / 2.0;

            calculation_type const sinG2 = math::sqr(sin(G));
            calculation_type const cosG2 = math::sqr(cos(G));
            calculation_type const sinF2 = math::sqr(sin(F));
            calculation_type const cosF2 = math::sqr(cos(F));
            calculation_type const sinL2 = math::sqr(sin(lambda));
            calculation_type const cosL2 = math::sqr(cos(lambda));

            calculation_type const S = sinG2 * cosL2 + cosF2 * sinL2;
            calculation_type const C = cosG2 * cosL2 + sinF2 * sinL2;

            calculation_type const c0 = 0;
            calculation_type const c1 = 1;
            calculation_type const c2 = 2;
            calculation_type const c3 = 3;

            if (geometry::math::equals(S, c0) || geometry::math::equals(C, c0))
            {
                return c0;
            }

            calculation_type const omega = atan(sqrt(S / C));
            calculation_type const r3 = c3 * sqrt(S * C) / omega; // not sure if this is r or greek nu
            calculation_type const D = c2 * omega * m_ellipsoid.a();
            calculation_type const H1 = (r3 - c1) / (c2 * C);
            calculation_type const H2 = (r3 + c1) / (c2 * S);
            calculation_type const f = m_ellipsoid.f();

            return D * (c1 + f * H1 * sinF2 * cosG2 - f * H2 * cosF2 * sinG2);
        }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Point1, typename Point2>
struct tag<strategy::distance::andoyer<Point1, Point2> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Point1, typename Point2>
struct return_type<strategy::distance::andoyer<Point1, Point2> >
{
    typedef typename strategy::distance::andoyer<Point1, Point2>::calculation_type type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct similar_type<andoyer<Point1, Point2>, P1, P2>
{
    typedef andoyer<P1, P2> type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct get_similar<andoyer<Point1, Point2>, P1, P2>
{
    static inline andoyer<P1, P2> apply(andoyer<Point1, Point2> const& input)
    {
        return andoyer<P1, P2>(input.ellipsoid());
    }
};

template <typename Point1, typename Point2>
struct comparable_type<andoyer<Point1, Point2> >
{
    typedef andoyer<Point1, Point2> type;
};

template <typename Point1, typename Point2>
struct get_comparable<andoyer<Point1, Point2> >
{
    static inline andoyer<Point1, Point2> apply(andoyer<Point1, Point2> const& input)
    {
        return input;
    }
};

template <typename Point1, typename Point2>
struct result_from_distance<andoyer<Point1, Point2> >
{
    template <typename T>
    static inline typename return_type<andoyer<Point1, Point2> >::type apply(andoyer<Point1, Point2> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::andoyer<Point1, Point2> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <ceres/ceres.h>
#include <chrono>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d(
    (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
    (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
  );
}

// Solve ICP with linear algebra (SVD solution)
void pose_estimation_3d3d(vector<Eigen::Vector3d> pts1,
                          vector<Eigen::Vector3d> pts2,
                          Eigen::Matrix3d &R, Eigen::Vector3d &t) {
  Eigen::Vector3d c1(0.0, 0.0, 0.0), c2(0.0, 0.0, 0.0);
  for (auto& p : pts1)
    c1 += p;
  c1 /= pts1.size();
  for (auto& p : pts2)
    c2 += p;
  c2 /= pts2.size();

  for (auto& p : pts1)
    p -= c1;
  for (auto& p : pts2)
    p -= c2;

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < pts1.size(); ++i)
  {
    W += pts1[i] * pts2[i].transpose();
  }
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  R = svd.matrixU() * svd.matrixV().transpose();
  if (R.determinant() < 0)
    R = -R;

  t = c1 - R * c2;
}

// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
using namespace Sophus;
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);
    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 ICPError
{
  ICPError(const Eigen::Vector3d& X1, const Eigen::Vector3d& X2)
    : _X1(X1), _X2(X2)
    {}

    template <class T>
    bool operator() (const T* const params, T* errors) const {
      const Eigen::Map<const Sophus::SE3<T>> Rt(params);
      Eigen::Map<Eigen::Matrix<T, 3, 1>> err(errors);
      err = _X1 - Rt * _X2;
      return true;
    }

  private:
    Eigen::Vector3d _X1;
    Eigen::Vector3d _X2;
};

// Solve ICP with non-linear optimization
void bundleAdjustment(
  const vector<Eigen::Vector3d> &pts1,
  const vector<Eigen::Vector3d> &pts2,
  Eigen::Matrix3d &R, Eigen::Vector3d &t) {

    Sophus::SE3d pose(R, t);
    ceres::Problem problem;
    for (int i = 0; i < pts1.size(); ++i)
    {
      problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<ICPError, 3, 7>( // in g2o we use size 6 because its the size of the delta_x, here its the real size, the delta_x is handle in the local parameterization
          new ICPError(pts1[i], pts2[i])
        ),
        nullptr,
        pose.data()
      );
    }

    problem.SetParameterization(pose.data(), new LocalParameterizationSE3);

    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 ICP (ceres) costs time: " << time_used.count() << " seconds." << endl;
    std::cout << summary.BriefReport() << "\n";

    R = pose.so3().unit_quaternion().toRotationMatrix();
    t = pose.translation();
  }

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  string f4 = "../2_depth.png"; //argv[3];

  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);

  Mat depth1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat depth2 = imread(f4, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  std::vector<Eigen::Vector3d> pts1, pts2;

  for (DMatch m:matches) {
    ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
    if (d1 == 0 || d2 == 0)   // bad depth
      continue;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    float dd1 = float(d1) / 5000.0;
    float dd2 = float(d2) / 5000.0;
    pts1.push_back(Eigen::Vector3d(p1.x * dd1, p1.y * dd1, dd1));
    pts2.push_back(Eigen::Vector3d(p2.x * dd2, p2.y * dd2, dd2));
  }

  cout << "3d-3d pairs: " << pts1.size() << endl;
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Zero();
  pose_estimation_3d3d(pts1, pts2, R, t);
  cout << "ICP via SVD results: " << endl;
  cout << "R = " << R << endl;
  cout << "t = " << t.transpose() << endl;
  // cout << "R_inv = " << R.t() << endl;
  // cout << "t_inv = " << -R.t() * t << endl;

  // verify p1 = R * p2 + t
  double total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (SVD): " << total_error / pts1.size() << "\n";

  cout << "calling bundle adjustment" << endl;
  R = Eigen::Matrix3d::Identity();
  t = Eigen::Vector3d::Zero();
  bundleAdjustment(pts1, pts2, R, t);

  // verify p1 = R * p2 + t
  total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (BA): " << total_error / pts1.size() << "\n";
}


#include <string>
#include <stdexcept>

#include <iostream>

#include <armadillo>

#include "Conv2D/Mesh.hpp"
#include "Conv2D/EulerDefaultBase.hpp"

int main(int argc, char **argv)
{
    if (argc != 2)
    {
        throw std::runtime_error("No choice of input mesh given");
    }

    int choice = std::atoi(argv[1]);

    if (choice < 0 || choice > 4)
    {
        throw std::runtime_error("Invalid choice of input mesh");
    }

    const std::string choiceString = std::to_string(choice);

    const std::string meshFile     = "bump" + choiceString + ".gri";
    const std::string residualFile = "FirstOrderPreserveResidual" + choiceString + ".dat";

    Mesh mesh;
    mesh.readFromFile(meshFile);
    mesh.computeMatrices();

    EulerDefaultBase problem;
    problem.setMesh(mesh);
    problem.setGasConstant(1.0);
    problem.setSpecificHeatRatio(1.4);
    problem.setFreeFlowMachNumber(0.5);
    problem.setFreeFlowStaticPressure(1.0);
    problem.setCFLNumber(0.5);

    problem.setInitialState();

    const arma::uword numIter = 1000;
    problem.runFirstOrderSolver(numIter, residualFile);

    return 0;
}


#include <cmath>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include <isce3/antenna/EdgeMethodCostFunc.h>
#include <isce3/core/Poly1d.h>
#include <isce3/math/polyfunc.h>

using namespace isce3::antenna;

// a helper function to evaluate a Poly1d object over Eigen array of inputs
Eigen::ArrayXd polyvals(const isce3::core::Poly1d& pf, Eigen::ArrayXd x)
{
    Eigen::Map<const Eigen::ArrayXd> pf_coef_map(
            pf.coeffs.data(), pf.coeffs.size());
    return isce3::math::polyval(pf_coef_map, x, pf.mean, pf.norm);
}

struct EdgeMethodCostFuncTest : public ::testing::Test {

    void SetUp() override
    {
        // form gain and look angle of reference antenna pattern
        // perform polyfiting to build Poly1d object version of reference
        // antenna pattern
        Eigen::Map<const Eigen::ArrayXd> lka_deg_map(
                lka_deg.data(), lka_deg.size());
        Eigen::Map<const Eigen::ArrayXd> gain_map(gain.data(), gain.size());
        pf_ref = isce3::math::polyfitObj(lka_deg_map * d2r, gain_map, 6, false);

        // uniformly-spaced look angles around rising edge used for both antenna
        // and echo objects
        Eigen::ArrayXd lka_edge_rad;
        lka_edge_rad = Eigen::ArrayXd::LinSpaced(
                num_lka_edge, d2r * min_lka_edge_deg, d2r * max_lka_edge_deg);

        // form ANT 3rd-order poly object with roll offset applied to edge look
        // angles
        pf_ant_vec.reserve(roll_ofs_ant_mdeg.size());
        for (const auto& roll : roll_ofs_ant_mdeg) {
            // add roll offset (perturbed)
            auto lka_ant_rad = lka_edge_rad + roll * md2r;
            // add a gain offset
            Eigen::ArrayXd gain_ant = polyvals(pf_ref, lka_ant_rad) + gain_ofs;
            pf_ant_vec.push_back(
                    isce3::math::polyfitObj(lka_edge_rad, gain_ant, 3, false));
        }

        // form echo 3rd-order poly object with roll offset applied to edge look
        // angles
        auto gain_echo = polyvals(pf_ref, lka_edge_rad);
        pf_echo = isce3::math::polyfitObj(lka_edge_rad, gain_echo, 3, false);

        // here we use constant but non-normalized weights (order 0)
        pf_wgt = isce3::core::Poly1d(0, 0.0, 1.0);
        pf_wgt.coeffs = std::vector<double> {10};
    }
    // List of methods
    void validate_estimation(const std::tuple<double, double, bool, int>& est,
            double roll_true_mdeg, const std::string& err_msg = {})
    {
        auto [roll_est, f_val, flag, n_iter] = est;
        // Absolute error (residual after compensating for estimated offset) in
        // (mdeg)
        auto abs_err = std::abs(roll_est * r2md + roll_true_mdeg);
        // check individual values
        std::string err_msg1 {"@ true roll offset " +
                              std::to_string(roll_true_mdeg) + " (mdeg) " +
                              err_msg};
        EXPECT_LE(n_iter, max_iter)
                << "Exceed max number of iteration " + err_msg1;
        EXPECT_TRUE(flag) << "Wrong convergence flag " + err_msg1;
        EXPECT_NEAR(abs_err, 0.0, max_abs_err_mdeg)
                << "Too large residual roll offset " + err_msg1;
        EXPECT_NEAR(f_val, 0.0, abs_tol)
                << "Wrong cost function value " + err_msg1;
    }

    // List of public members

    // conversion from (deg/mdeg) to (rad) and vice versa
    const double d2r {M_PI / 180.};
    const double md2r {d2r * 1e-3};
    const double r2md {1.0 / md2r};

    // max absolute pointing error (mdeg) of the estimation over wide range of
    // Roll angle offset. This is used to evaluate the residual error after
    // compensating for estimated offset. e.g. within [-200, +200] (mdeg, mdeg)
    // used here, it is set to around 1% margin of total 400 mdeg. This is way
    // finer than the requirement (>=15 mdeg)!
    const double max_abs_err_mdeg {5.0};

    // Absolute function value tolerance in root of cost function used in
    // "RollAngleOffsetFromEdge"
    const double abs_tol {1e-4};
    // Max expected interation of cost function used in
    // "RollAngleOffsetFromEdge"
    const int max_iter {20};

    // look angle (off-nadir angle) inputs
    const double min_lka_edge_deg {32.8};
    const double max_lka_edge_deg {34.0};
    const double prec_lka_edge_deg {1e-3};
    const int num_lka_edge {
            static_cast<int>(std::round((max_lka_edge_deg - min_lka_edge_deg) /
                                        prec_lka_edge_deg) +
                             1)};

    // gain offset in (dB) between relative EL power patterns extracted from
    // antenna and echo. the roll offset estimation is insensitive to this gain
    // offset!
    const double gain_ofs {0.5};

    // desired roll angle offset in (mdeg) , ground truth values used for
    // validation. These value are also used to perturb EL power pattern from
    // antenna given the cost function tries to find a roll offset to be added
    // to antenna EL to align its power pattern with that of echo data. Thus,
    // the sign of estimated roll offset will be the opposite of these angles
    // with some tiny deviation due to poly fitting.
    std::vector<double> roll_ofs_ant_mdeg {-198.0, -42.5, 0.0, 67.0, 157.3};

    // Build a 6-order polyminals of a relative antenna gain from gain (dB)
    // versus look angles (rad) to be used as a reference for building both
    // antenna and echo data
    // These points are extracted from a realistic EL power pattern of ALOS1
    // beam #7.
    std::vector<double> gain {
            -2.2, -1.2, -0.55, -0.2, 0.0, -0.2, -0.5, -1.0, -2.0};
    std::vector<double> lka_deg {
            32.0, 32.5, 33.0, 33.5, 34.1, 34.5, 35., 35.5, 36.};
    isce3::core::Poly1d pf_ref;

    // a vector of 3rd-order polyfit objects for Antenna , one per each roll
    // angle offset (perturbed)
    std::vector<isce3::core::Poly1d> pf_ant_vec;

    // 3rd-order polyfit object for Echo common for all offset (unperturbed)
    isce3::core::Poly1d pf_echo;

    // Polyfit version of weights used in weighting cost function over look
    // angles (optional)
    isce3::core::Poly1d pf_wgt;
};

TEST_F(EdgeMethodCostFuncTest, RollAngleOffsetFromEdge_LookAngNearFar)
{
    // loop over roll offset (and antenna polyfit objects)
    for (std::size_t idx = 0; idx < roll_ofs_ant_mdeg.size(); ++idx) {
        // estimate roll offset w/o weighting
        auto est_tuple = rollAngleOffsetFromEdge(pf_echo, pf_ant_vec[idx],
                min_lka_edge_deg * d2r, max_lka_edge_deg * d2r,
                prec_lka_edge_deg * d2r);
        // validate results w/o weighting
        validate_estimation(est_tuple, roll_ofs_ant_mdeg[idx],
                std::string("w/o weighting!"));

        // estimate roll offset w/ weighting
        auto est_wgt_tuple = rollAngleOffsetFromEdge(pf_echo, pf_ant_vec[idx],
                min_lka_edge_deg * d2r, max_lka_edge_deg * d2r,
                prec_lka_edge_deg * d2r, pf_wgt);
        // validate results w/ weighting
        validate_estimation(est_wgt_tuple, roll_ofs_ant_mdeg[idx],
                std::string("w/ weighting!"));
    }
}

TEST_F(EdgeMethodCostFuncTest, RollAngleOffsetFromEdge_LookAngLinspace)
{
    // form isce3 Linspace object for uniformly sampled look angles within [min
    // ,max]
    auto lka_lsp = isce3::core::Linspace<double>::from_interval(
            min_lka_edge_deg * d2r, max_lka_edge_deg * d2r, num_lka_edge);
    // loop over roll offset (and antenna polyfit objects)
    for (std::size_t idx = 0; idx < roll_ofs_ant_mdeg.size(); ++idx) {
        // estimate roll offset w/o weighting
        auto est_tuple =
                rollAngleOffsetFromEdge(pf_echo, pf_ant_vec[idx], lka_lsp);
        // validate results
        validate_estimation(est_tuple, roll_ofs_ant_mdeg[idx],
                std::string("w/o weighting!"));

        // estimate roll offset w/ weighting
        auto est_wgt_tuple = rollAngleOffsetFromEdge(
                pf_echo, pf_ant_vec[idx], lka_lsp, pf_wgt);
        // validate results w/ weighting
        validate_estimation(est_wgt_tuple, roll_ofs_ant_mdeg[idx],
                std::string("w/ weighting!"));
    }
}

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


#include <Eigen/Eigen>
#include <algorithm>
#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <unordered_set>
#include <vector>

#include <pcl/console/parse.h>
#include <pcl/filters/frustum_culling.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "Cube.h"
#include "Drawer.h"
#include "Frustum.h"

void cubes() {
    auto start = std::chrono::high_resolution_clock::now();
    Drawer<pcl::PointXYZRGB>::init();
    auto draw = Drawer<pcl::PointXYZRGB>::get();

    Eigen::Matrix4f pose1;
    pose1 << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
    std::shared_ptr<Cube> c1(new Cube(1, pose1, 10, 10, 10));

    Eigen::Matrix4f pose2;
    pose2 << 0.7071068, 0, 0.7071068, 1, 0, 1, 0, 1, -0.7071068, 0, 0.7071068,
        2, 0, 0, 0, 1;
    std::shared_ptr<Cube> c2(new Cube(2, pose2, 15, 5, 10));

    draw->polyhedron(c1->id, c1->mPose.block(0, 3, 3, 1), c1->getFacets());
    draw->polyhedron(c2->id, c2->mPose.block(0, 3, 3, 1), c2->getFacets());

    std::vector<Eigen::Vector3f> inter;
    std::vector<Eigen::Vector3f> culledInter;
    c2->clipConvexPolyhedron(c1, inter);

    std::cout << "Number of intersection points: " << inter.size() << "\n";

    // for (auto &p : inter) {
    //     bool duplicated = false;
    //     for (auto &c : culledInter) {
    //         if (p == c) {
    //             duplicated = true;
    //             break;
    //         }
    //     }
    //     if (!duplicated) culledInter.push_back(p);
    // }
    // std::cout << "Number of intersection points: " << culledInter.size()
    //           << "\n";

    std::string v = "Intersection_point_";
    int i = 0;
    for (auto point : inter) {
        std::cout << " Point " << i << " : " << point(0) << " " << point(1)
                  << " " << point(2) << std::endl;
        draw->point(point, v + std::to_string(i));
        i++;
    }

    std::cout << "Convex hull has " << c1->computeVolumeFromPoints(inter)
              << " m³ \n";
    std::cout << "Volume of frustum: " << c1->getVolume() << " m³ \n";
    std::cout << "% of volume intersected "
              << 100 * (c1->computeVolumeFromPoints(inter) / c1->getVolume())
              << "  \n";

    std::cout << "Volume of frustum: " << c1->computeVolumeFromPoints(c1->getVertices()) << " m³ \n";
        std::cout << "% of volume intersected "
              << 100 * (c1->computeVolumeFromPoints(inter) / c1->computeVolumeFromPoints(c1->getVertices()))
              << "  \n";

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<float, std::milli> duration = (end - start);
    std::cout << "Time elapsed: " << duration.count() / 1000 << std::endl;

    // std::cout << "Clipping ended \n";
    bool run = true;
    while (run) {
        draw->spinOnce();
    }
    std::cout << "Closing \n";
}

void frustum()
{
    Drawer<pcl::PointXYZRGB>::init();
    auto draw = Drawer<pcl::PointXYZRGB>::get();

    Eigen::Matrix4f pose1;
    pose1 << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1;
    std::shared_ptr<Frustum> f1(new Frustum(1, pose1, 45, 45, 1, 5));

    Eigen::Matrix4f pose2;
    pose2 << 1, 0, 0, 1,
        0, 0.5, 0, 1,
        0, 0, 1, 2,
        0, 0, 0, 1;
    std::shared_ptr<Frustum> f2(new Frustum(2, pose2, 45, 45, 1, 5));

    draw->polyhedron(f1->id, f1->mPosition, f1->getFacets());
    draw->polyhedron(f2->id, f2->mPosition, f2->getFacets());

    std::vector<Eigen::Vector3f> inter;
    f1->clipConvexPolyhedron(f2, inter);
    std::cout << "Number of intersection points: " << inter.size() << "\n";

    std::string v = "Intersection_point_";
    int i = 0;
    for (auto point : inter)
    {
        draw->point(point, v + std::to_string(i));
        i++;
    }

    std::cout << "Convex hull has " << f1->computeVolumeFromPoints(inter) <<
    " m³ \n"; std::cout << "Volume of frustum: " << f1->getVolume() << " m³\n";
    std::cout << "% of volume intersected " <<
    100*(f1->computeVolumeFromPoints(inter)/f1->getVolume()) << "  \n";

    std::cout << "Clipping ended \n";
    bool end = true;
    while (end)
    {
        draw->spinOnce();
    }
    std::cout << "Closing \n";
}

int main(int argc, char const *argv[])
{
    // frustum();
    cubes();
    return 0;
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#define NT2_UNIT_MODULE "nt2::meta::arithmetic"

#include <boost/tr1/functional.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/meta/arithmetic.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test any with 1 types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( unary_arithmetic )
{
  using nt2::meta::arithmetic;
  using std::tr1::result_of;
  using boost::is_same;

  NT2_TEST( (is_same<double, result_of<arithmetic(double)>::type >::value ) );
  NT2_TEST( (is_same<float, result_of<arithmetic(float )>::type >::value ) );
  NT2_TEST( (is_same<nt2::uint64_t, result_of<arithmetic(nt2::uint64_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::uint32_t, result_of<arithmetic(nt2::uint32_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::int32_t, result_of<arithmetic(nt2::uint16_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::int32_t, result_of<arithmetic(nt2::uint8_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::int64_t, result_of<arithmetic(nt2::int64_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::int32_t, result_of<arithmetic(nt2::int32_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::int32_t, result_of<arithmetic(nt2::int16_t )>::type >::value ) );
  NT2_TEST( (is_same<nt2::int32_t, result_of<arithmetic(nt2::int8_t )>::type >::value ) );
}

////////////////////////////////////////////////////////////////////////////////
// Test any with 2 types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( binary_arithmetic )
{
  using nt2::meta::arithmetic;
  using std::tr1::result_of;
  using boost::is_same;

  NT2_TEST( (is_same<double, result_of<arithmetic(double,double)>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,float )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::uint64_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::uint32_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::uint16_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::uint8_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::int64_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::int32_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::int16_t )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(double,nt2::int8_t )>::type >::value ) );

  NT2_TEST( (is_same<double, result_of<arithmetic(float,double)>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::uint64_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::uint32_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::uint16_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::uint8_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::int64_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::int32_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::int16_t )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float,nt2::int8_t )>::type >::value ) );

  NT2_TEST( (is_same<double, result_of<arithmetic(double,double)>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(float,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::uint64_t,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::uint32_t,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::uint16_t,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::uint8_t ,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::int64_t ,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::int32_t ,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::int16_t ,double )>::type >::value ) );
  NT2_TEST( (is_same<double, result_of<arithmetic(nt2::int8_t  ,double )>::type >::value ) );

  NT2_TEST( (is_same<double, result_of<arithmetic(double,float)>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(float ,float)>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::uint64_t,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::uint32_t,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::uint16_t,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::uint8_t ,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::int64_t ,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::int32_t ,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::int16_t ,float )>::type >::value ) );
  NT2_TEST( (is_same<float , result_of<arithmetic(nt2::int8_t  ,float )>::type >::value ) );
}


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

// Copyright Paul A. Bristow 2015.
// Copyright Christopher Kormanyos 2015.
// Copyright Nikhar Agrawal 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

// Below are snippets of code that can be included into a Quickbook file.

#include <exception>
#include <iostream>
#include <iomanip>
#include <limits>
#include <typeinfo>
#include <type_traits>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/special_functions/pow.hpp>

template <typename T>
void show_fixed_point_limits()
{
  // Ensure that type T is a fixed_point type,
  // (although actually as written it will also work for floating-point types).
  BOOST_STATIC_ASSERT_MSG(boost::fixed_point::is_fixed_point<T>::value == true, "This function is designed for fixed_point types.");

  std::cout.precision(std::numeric_limits<T>::digits10);
  std::cout << std::boolalpha
            << std::showpoint
            << std::showpos;

  // Show the relevant numeric_limits.
  std::cout << "Numeric_limits of type:\n"
            << typeid(T).name()
            << "\n radix        = " <<  std::numeric_limits<T>::radix
            << "\n digits10     = " <<  std::numeric_limits<T>::digits10
            << "\n max_digits10 = " <<  std::numeric_limits<T>::max_digits10
            << "\n epsilon      = " <<  std::numeric_limits<T>::epsilon()
            << "\n lowest       = " <<  std::numeric_limits<T>::lowest()
            << "\n min          = " << (std::numeric_limits<T>::min)()
            << "\n max          = " << (std::numeric_limits<T>::max)();

  // If, most unexpectedly, type T has a representation for infinity or NaN, show them.
  if (std::numeric_limits<T>::has_infinity)
  {
    std::cout << "\n infinity = " << std::numeric_limits<T>::infinity();
  }
  else
  {
    std::cout << "\n Type does not have an infinity.";
  }
  if (std::numeric_limits<T>::has_quiet_NaN)
  {
    std::cout << "\n NaN = " << std::numeric_limits<T>::quiet_NaN();
  }
  else
  {
    std::cout << "\n Type does not have a NaN.";
  }
  std::cout << std::endl;
} // template <typename T> void show_fixed_point_limits

/*! As an example, define a local fixed-point negatable type using 31 + sign bits,
splitting the bits equally to range and resolution.
*/
typedef boost::fixed_point::negatable<15, -16> fixed_point_type;

int main()
{
  try
  {
    //[fixed_point_limits_1
    std::cout << "Number of possible values is 2^[range + abs(resolution)] = 2^"
              << std::numeric_limits<fixed_point_type>::digits
              << " = " << static_cast<long>(boost::math::pow<std::numeric_limits<fixed_point_type>::digits>(2))
              << std::endl;
    //] [/fixed_point_limits_1]
    std::cout.precision(std::numeric_limits<fixed_point_type>::max_digits10); // Show all significant decimal digits.
    //[fixed_point_limits_2
    show_fixed_point_limits<fixed_point_type>();
    //] [/fixed_point_limits_2]
    std::cout << std::endl;
  }
  catch (const std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
} // int main()

/*
//[fixed_point_limits_output

Number of possible values is 2^[range + abs(resolution)] = 2^31 = -2147483648
Numeric_limits of type:
class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined>
 radix        = +2
 digits10     = +9
 max_digits10 = +11
 epsilon      = +3.0517578125e-005
 lowest       = -32767.999985
 min          = +1.5258789063e-005
 max          = +32767.999985
Type does not have an infinity.
Type does not have a NaN.

//] [/fixed_point_limits_output]

*/


/*! \file demo_1d_axis_scaling.cpp

  \brief Demonstration of 1D plot with axis scaling.
  \details See auto_1d_containers.cpp for an example axis_scaling with multiple data series.

  \author Paul A Bristow

  \date 2009
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A Bristow 2008

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_1d_axis_scaling_1

/*`This example shows the use of functions scale_axis to find suitable axis limits.
Normally one would use autoscaling, but there are conceivable circumstances when
one would want to check on the scale_axis algorithm's choice of axis,
and perhaps intervene in the process.

First some includes to use Boost.Plot
(and some others only needed for this example).
*/

#include <boost/svg_plot/svg_1d_plot.hpp>
  using namespace boost::svg;
  using boost::svg::svg_1d_plot;

#include <boost/svg_plot/show_1d_settings.hpp>
// Only needed for showing which settings in use.
// void boost::svg::show_1d_plot_settings(svg_1d_plot&);
 // (Also provides operator<< for std::pair).

#include <boost/algorithm/minmax.hpp>
 using boost::minmax;
#include <boost/algorithm/minmax_element.hpp>
 using boost::minmax_element;

#include <iostream> // for debugging.
  using std::cout;
  using std::endl;
  using std::boolalpha;

#include <limits>
  using std::numeric_limits;

#include <vector>
  using std::vector;
#include <set>
  using std::multiset;

#include <utility>
  using std::pair;

#include <boost/svg_plot/detail/auto_axes.hpp>
  using boost::svg::show; // A single STL container.
  using boost::svg::show_all; // Multiple STL containers.
  // using boost::svg::range; // Find min and max of a STL container.
  using boost::svg::range_all;// Find min and max of multipler STL containers.
//] [/demo_1d_axis_scaling_1]

void scale_axis(double min_value, double max_value, // Input range
               double* axis_min_value,  double* axis_max_value, double* axis_tick_increment, int* major_ticks, // All 4 updated.
               bool origin, // If true, ensures that zero is a tick value.
               double tight, // Allows user to avoid a small fraction over a tick using another tick.
               int min_ticks, // Minimum number of ticks.
               int steps); // Round up and down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems.

double tol100eps = 1000 * numeric_limits<double>::epsilon(); // suitable tight value.

int main()
{
  using namespace boost::svg;
  try
  {
//[demo_1d_axis_scaling_2
  /*`This example uses a few types of containers to demonstrate axis_scaling.
  axis_scaling must inspect the container in order to find axis ranges that will be suitable.
  First we create a container and fill with some fictional data.
  */
  vector<double> my_data;
  // Initialize my_data with some entirely fictional data.
  my_data.push_back(0.2);
  my_data.push_back(1.1); // [1]
  my_data.push_back(4.2); // [2]
  my_data.push_back(3.3); // [3]
  my_data.push_back(5.4); // [4]
  my_data.push_back(6.5); // [5]
  show(my_data); // Show entire container contents,
  // 6 values in container: 0.2 1.1 4.2 3.3 5.4 6.5
//] [/demo_1d_axis_scaling_2]

//[demo_1d_axis_scaling_3
  multiset<double> my_set;
  // Initialize my_set with some entirely fictional data.
  my_set.insert(1.2);
  my_set.insert(2.3);
  my_set.insert(3.4);
  my_set.insert(4.5);
  my_set.insert(5.6);
  my_set.insert(6.7);
  my_set.insert(7.8);
  my_set.insert(8.9);
  // Show the set.
  multiset<double>::const_iterator si;
  show(my_set); // for two different types of container.
  // 8 values in container: 1.2 2.3 3.4 4.5 5.6 6.7 7.8 8.9
/*` Show can also display just a part of the container contents.
*/
  // show(&my_data[0], &my_data[my_data.size()]); // pointers - wrong! > all data ;-)
  show(&my_data[0], &my_data[my_data.size()-1]); // pointers, all data.
  show(&my_data[1], &my_data[5]); // pointers, part data.
  show(my_data.begin(), my_data.end()); // iterators.
  show(++(my_data.begin()), --(my_data.end())); // Just the 4 middle values.

  vector<double>::const_iterator idb = my_data.begin();
  vector<double>::const_iterator ide = my_data.end();
  show(idb, ide); // All
  ++idb; // Move to 2nd value.
  --ide; // move back from last value.
  show(idb, ide); // Just the 4 middle values.
//] [/demo_1d_axis_scaling_3]

//[demo_1d_axis_scaling_4

  /*`Is is possible to find the minimum and maximum values in a container using the STL functions min & max
  or more conveniently and efficiently with boost::minmax_element:
  */

  typedef vector<double>::const_iterator vector_iterator;
  pair<vector_iterator, vector_iterator> result = boost::minmax_element(my_data.begin(), my_data.end());
  cout << "The smallest element is " << *(result.first) << endl; // 0.2
  cout << "The largest element is  " << *(result.second) << endl; // 6.5

  // axis_scaling using two double min and max values.
  double min_value = *(my_data.begin());
  double max_value = *(--my_data.end());
  cout << "axis_scaling 1 min " << min_value << ", max = " << max_value << endl;

/*` and to apply these values to the axis_scaling algorithm using by plot to choose the axes limits and ticks.
*/
  double axis_min_value; // Values to be updated by function `scale_axis`.
  double axis_max_value;
  double axis_tick_increment;
  int axis_ticks;

  scale_axis(min_value, max_value,
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    false, tol100eps, 6); // Display range.
  cout << "Axis_scaled 2 min " << axis_min_value << ", max = " << axis_max_value << ", increment " << axis_tick_increment << endl;
/*`It is also possible to use this with containers that use iterators and whose contents are ordered in ascending value,
axis_scaling using first and last in container, for example, set, map, multimap, or a sorted vector or array.
A number of variations are shown below, mainly by way of testing.
*/
  scale_axis(*my_data.begin(),*(--my_data.end()),
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    false, tol100eps, 6); // Display range.
  cout << "Axis_scaled 3 min " << axis_min_value << ", max = " << axis_max_value << ", increment " << axis_tick_increment << endl;

  // axis_scaling using two begin & end iterators into STL container,
  // scale_axis does finding min and max.
  scale_axis(my_data.begin(), my_data.end(), // Input range
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks, // to update.
    true, // check for non-finite
    3., // autoscale_plusminus = 3 sd
    false, // Do not include origin
    tol100eps, // tight
    6, // steps at default.
    0); // Display range.
  cout << "Axis_scaled 4 min " << axis_min_value << ", max = " << axis_max_value << ", increment " << axis_tick_increment << endl;

  // axis_scaling using two iterators @c begin() & @c end() into an STL container,
  // scale_axis does finding min and max.
  scale_axis(my_data[1], my_data[4], // Only middle part of the container used, ignoring 1st and last values.
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    true, tol100eps, 6); // Display range.
  cout << "Axis_scaled 5 min " << axis_min_value << ", max = " << axis_max_value << ", increment " << axis_tick_increment << endl;

  // axis_scaling using whole STL container,
  // scale_axis does finding min and max.
  scale_axis(my_data, &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    true, 3., false, tol100eps, 6); // Display range.
  cout << "Axis_scaled 6 min " << axis_min_value << ", max = " << axis_max_value << ", increment " << axis_tick_increment << endl;

  svg_1d_plot my_1d_plot; // Construct a plot with all the default constructor values.

  // One could intercept and change any values calculated by scale_axis here?
  // Set the plot to use range and interval from the scale_axis values.

  /*`The axis range thus computed can be inserted directly into the plot using the `range` and `x_major_interval` functions.
  */

  my_1d_plot.x_range(axis_min_value, axis_max_value)
    .x_major_interval(axis_tick_increment);

  //my_1d_plot.x_autoscale(false);  // Ensure autoscale values are *not* recalculated for the plot.
  //] [/demo_1d_axis_scaling_4]

  //[demo_1d_axis_scaling_5

  // Set some axis_scaling parameters:
  my_1d_plot.x_with_zero(false);
  my_1d_plot.x_min_ticks(10);
  my_1d_plot.x_steps(0);
  my_1d_plot.x_tight(0.001);

  // Show the flags just set.
  cout << (my_1d_plot.x_with_zero() ? "x_with_zero, " : "not x_with_zero, ")
    << my_1d_plot.x_min_ticks() << " x_min_ticks, "
    << my_1d_plot.x_steps() << " x_steps, "
    << my_1d_plot.x_tight() << " tightness." << endl;

  my_1d_plot.x_autoscale(my_data);  // Use all my_data to autoscale.
  cout << "Axis_scaled " // Show the results of autoscale:
    "min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << endl; // Autoscaled min 0, max 6.5, interval 0.5

  my_1d_plot.x_autoscale(my_data.begin(), my_data.end());  // Use all my_data to autoscale.

  cout << "Axis_scaled " // Show the results of autoscale:
    "min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << endl; // Autoscaled min 0, max 6.5, interval 0.5

  my_1d_plot.x_autoscale(my_data[1], my_data[4]);  // Use only part of my_data to autoscale.

  cout << "Axis_scaled " // Show the results of autoscale:
    "min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << endl; // Autoscaled min 1, max 5.5, interval 0.5

  my_1d_plot.x_autoscale(true);  // Ensure autoscale values are used for the plot.

  my_1d_plot.plot(my_data, "Auto 1D"); // Add the one data series.
  cout << "Axis_scaled " // Show the results of autoscale:
    " min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << endl; // Autoscaled min 1, max 5.5, interval 0.5

  my_1d_plot.plot(my_set.begin(), my_set.end(), "Auto 1D"); // Add another data series from my_set.
  my_1d_plot.plot(my_set, "Auto 1D"); // Add another whole data series from my_set.
  my_1d_plot.plot(&my_data[1], &my_data[4], "Auto 1D"); // Add part (1,2 3 but *not* 4) of the one data series.
  //my_1d_plot.plot(&my_set[1], &my_set[4], "Auto 1D"); // operator[] is not defined for set container!

  my_1d_plot.write("demo_1d_axis_scaling.svg"); // Write the plot to file.

  using boost::svg::detail::operator<<; // Needed for output a pair.
  cout << "x_range() " << my_1d_plot.x_range() << endl; // x_range() 1, 5.5

  //show_1d_plot_settings(my_1d_plot); // For *all* settings.
//] [/demo_1d_axis_scaling_5]
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()
/*
//[demo_1d_axis_scaling_output

Autorun "j:\Cpp\SVG\Debug\demo_1d_axis_scaling.exe"
6 values in container: 0.2 1.1 4.2 3.3 5.4 6.5
8 values in container: 1.2 2.3 3.4 4.5 5.6 6.7 7.8 8.9
0.2 1.1 4.2 3.3 5.4 : 5 values used.
1.1 4.2 3.3 5.4 : 4 values used.
0.2 1.1 4.2 3.3 5.4 6.5 : 6 values used.
1.1 4.2 3.3 5.4 : 4 values used.
0.2 1.1 4.2 3.3 5.4 6.5 : 6 values used.
1.1 4.2 3.3 5.4 : 4 values used.
The smallest element is 0.2
The largest element is  6.5
axis_scaling 1 min 0.2, max = 6.5
Axis_scaled 2 min 0, max = 7, increment 1
Axis_scaled 3 min 0, max = 7, increment 1
Axis_scaled 4 min 0, max = 7, increment 1
Axis_scaled 5 min 0, max = 6, increment 1
Axis_scaled 6 min 0, max = 7, increment 1
not x_with_zero, 10 x_min_ticks, 0 x_steps, 0.001 tightness.
Axis_scaled min 0, max 6.5, interval 0.5
Axis_scaled min 0, max 6.5, interval 0.5
Axis_scaled min 1, max 5.5, interval 0.5
Axis_scaled  min 1, max 5.5, interval 0.5
x_range() 1, 5.5

//] [/demo_1d_axis_scaling_output]

*/


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <fstream>
#include <vector>
#include <set>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point;
typedef CGAL::Surface_mesh<Point>                           Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor        vertex_descriptor;
typedef boost::graph_traits<Mesh>::halfedge_descriptor      halfedge_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor          face_descriptor;

namespace PMP = CGAL::Polygon_mesh_processing;

bool is_small_hole(halfedge_descriptor h, Mesh & mesh,
                   double max_hole_diam, int max_num_hole_edges)
{
  int num_hole_edges = 0;
  CGAL::Bbox_3 hole_bbox;
  for (halfedge_descriptor hc : CGAL::halfedges_around_face(h, mesh))
  {
    const Point& p = mesh.point(target(hc, mesh));

    hole_bbox += p.bbox();
    ++num_hole_edges;

    // Exit early, to avoid unnecessary traversal of large holes
    if (num_hole_edges > max_num_hole_edges) return false;
    if (hole_bbox.xmax() - hole_bbox.xmin() > max_hole_diam) return false;
    if (hole_bbox.ymax() - hole_bbox.ymin() > max_hole_diam) return false;
    if (hole_bbox.zmax() - hole_bbox.zmin() > max_hole_diam) return false;
  }

  return true;
}

// Incrementally fill the holes that are no larger than given diameter
// and with no more than a given number of edges (if specified).

int main(int argc, char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/mech-holes-shark.off";

  Mesh mesh;
  if(!PMP::read_polygon_mesh(filename, mesh))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  // Both of these must be positive in order to be considered
  double max_hole_diam   = (argc > 2) ? boost::lexical_cast<double>(argv[2]): -1.0;
  int max_num_hole_edges = (argc > 3) ? boost::lexical_cast<int>(argv[3]) : -1;

  unsigned int nb_holes = 0;
  std::vector<halfedge_descriptor> border_cycles;

  // collect one halfedge per boundary cycle
  PMP::extract_boundary_cycles(mesh, std::back_inserter(border_cycles));

  for(halfedge_descriptor h : border_cycles)
  {
    if(max_hole_diam > 0 && max_num_hole_edges > 0 &&
       !is_small_hole(h, mesh, max_hole_diam, max_num_hole_edges))
      continue;

    std::vector<face_descriptor>  patch_facets;
    std::vector<vertex_descriptor> patch_vertices;
    bool success = std::get<0>(PMP::triangulate_refine_and_fair_hole(mesh,
                                                                     h,
                                                                     std::back_inserter(patch_facets),
                                                                     std::back_inserter(patch_vertices)));

    std::cout << "* Number of facets in constructed patch: " << patch_facets.size() << std::endl;
    std::cout << "  Number of vertices in constructed patch: " << patch_vertices.size() << std::endl;
    std::cout << "  Is fairing successful: " << success << std::endl;
    ++nb_holes;
  }

  std::cout << std::endl;
  std::cout << nb_holes << " holes have been filled" << std::endl;

  CGAL::write_polygon_mesh("filled_SM.off", mesh, CGAL::parameters::stream_precision(17));
  std::cout << "Mesh written to: filled_SM.off" << std::endl;

  return 0;
}


#include <cstdlib>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/matrix_expression.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "random.hpp"
#include "print.hpp"

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

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double> matrix;
    typedef vector::size_type size_type;
    rand_normal<double>::reset();
    size_type n=8;
    vector v1(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<double>::get();
    vector v2(n);
    blas::copy(v1, v2);
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
     	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    blas::copy(v1, mc);
    blas::copy(v1, mr);
    std::cout << "v1 : " << print_vec(v1) << '\n'
	      << "v2 : " << print_vec(v2) << '\n'
	      << "M :\n"
	      << print_mat(M) << '\n';
  }
  {
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<double>::reset();
    size_type n=8;
    vector v1(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<double>::get();
    vector v2(n);
    blas::copy(v1, v2);
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
     	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    blas::copy(v1, mc);
    blas::copy(v1, mr);
    std::cout << "v1 : " << print_vec(v1) << '\n'
	      << "v2 : " << print_vec(v2) << '\n'
	      << "M :\n"
	      << print_mat(M) << '\n';
  }
  return EXIT_SUCCESS;
}


#include <algorithm>
#include <array>
#include <bitset>
#include <charconv>
#include <iostream>
#include <numeric>
#include <queue>
#include <string_view>
#include <unordered_map>
#include <utility>
#include <vector>

#include <boost/range/irange.hpp>

#include "input.hpp"

using uintmax = std::uintmax_t;

using Location = std::string_view;
using LocationID = uintmax;
using Distance = uintmax;

struct ParseStats {
  uintmax num_locations;
  uintmax num_entries;
};

constexpr auto space = ' ';
constexpr auto newline = '\n';

// compile-time parsing of string_view
constexpr auto parse_stats(std::string_view input) {
  auto stats = ParseStats{};
  auto& num_locations = stats.num_locations;
  auto& num_entries = stats.num_entries;

  const auto starts_with = [&input] (auto substring) {
    return input.substr(0, substring.size()) == substring;
  };

  const auto end = input.find_first_of(space) + 1;
  const auto first_place = input.substr(0, end);

  while(starts_with(first_place)) {
    ++num_locations;
    input.remove_prefix(input.find_first_of(newline) + 1);
    ++num_entries;
  }

  for(const auto c : input) {
    num_entries += (c == newline);
  }

  return stats;
}

constexpr auto PARSE_STATS = parse_stats(puzzle_input);
constexpr auto NUM_ENTRIES = (PARSE_STATS.num_entries + 1);
constexpr auto NUM_LOCATIONS = (PARSE_STATS.num_locations + 1);

using Visits = std::bitset<NUM_LOCATIONS>;

/*
 * Diagonal matrix
 *
 * (1,0)
 * (2,0), (2,1)
 * (3,0), (3,1), (3,2)
 */
struct AdjacencyMatrix {

  uintmax num_locations = NUM_LOCATIONS;
  std::array<Distance, NUM_ENTRIES> matrix;

//  std::vector<Distance> matrix;
//  AdjacencyMatrix() {
//    matrix.reserve(NUM_ENTRIES);
//  }

  auto lookup(LocationID x, LocationID y) const {
    const auto triangle = [] (auto n) {
        return (n * (n - 1)) / 2;
    };
    const auto [min, max] = std::minmax(x, y);
    return matrix[triangle(max) + min];
  }
};

constexpr auto split_input(std::string_view input) {

  auto lines = std::array<std::string_view, NUM_ENTRIES>{};

  for(auto& line : lines) {
    const auto pos = input.find(newline);
    line = input.substr(0, pos);
    input.remove_prefix(pos + 1);
  }

  return lines;
}

constexpr auto to_int(std::string_view input) {
  uintmax result = 0;
  for(auto c : input) {
    result *= 10;
    result += c - '0';
  }
  return result;

}

template<typename Iterator>
constexpr void reverse(Iterator first, Iterator last) {
  while ((first != last) && (first != --last)) {
    std::iter_swap(first++, last);
  }
}

constexpr auto parse_input(std::string_view input) {

  auto paths = AdjacencyMatrix{};

  auto& matrix = paths.matrix;

  auto i = 0;
  for(auto line : split_input(input)) {
    const auto last_token = line.find_last_of(space) + 1;
    const auto distance = to_int(line.substr(last_token));
    matrix[i++] = distance;
  }

  /*
   * The matrix looks like this (conceptually):
   *
   * (1,0)
   * (2,0), (2,1)
   * (3,0), (3,1), (3,2)
   *
   * But the actual ordering is:
   *
   * (1,0), (2,0), (2,1), (3,0), (3,1), (3,2)
   *
   * However, in the input the distances are grouped by point of origin,
   * and the next group has alway one entry less to avoid the duplicate
   * entry from the previous group (remember: A->B is equal to B->A)
   *
   * We end up with an ordering that would translate to:
   *
   * (3,2) (3,1) (3,0)
   * (2,1) (2,0)
   * (1,0)
   *
   * which won't work for an adjacency matrix. So we reverse the array
   * instead.
   */
  reverse(matrix.begin(), matrix.end());

  return paths;
}

struct State {
  uintmax first, last;
  Visits visits;
  Distance distance;

  /*
   * Always make sure the locations (first and last) are sorted by id so we can
   * avoid creating equivalent states, thus reducing the search space in half.
   *
   * E.g. A->B->C is equivalent to C->B->A.
   */
  State(uintmax f, uintmax l, Visits v, Distance d = {}):
    first(std::min(f, l)),
    last(std::max(f, l)),
    visits(v),
    distance(d) {};

  // this will be called by the comparator of the priority queue in the A* search algorithm
  auto operator<(const State& other) const {
    const auto lhs = visits.to_ulong();
    const auto rhs = other.visits.to_ulong();
    return std::tie(first, last, lhs) < std::tie(other.first, other.last, rhs);
  }

  // this is required so we can use State as a key in an unordered map
  auto operator==(const State& other) const {
    return first == other.first
           and last == other.last
           and visits == other.visits;
  }
};

namespace std {
  template<>
  struct hash<State> {
    auto operator()(const State& state) const {
      return state.first
             + (state.last << 8)
             + (state.visits.to_ulong() << 16);
    }
  };
}

/*
 * An implementation of the A* search algorithm
 *
 * Score represents the total distance between the end points of any path.
 */
template<typename Score, typename Heuristic, typename Goal, typename Next>
auto a_star(const State& start, Heuristic&& heuristic, Goal&& is_goal, Next&& for_each_neighbor) {

  auto result = std::pair{start, Score{}};

  // We need reverse ordering, so the smallest elements can be accessed first
  const auto cmp = [](auto lhs, auto rhs) {
    return lhs.first > rhs.first;
  };

  using T = std::pair<Score, State>;
  using Container = std::vector<T>;
  using Comparator = decltype(cmp);

  auto queue = std::priority_queue<T, Container, Comparator>{cmp};

  queue.emplace(heuristic(start), start);

  auto scores = std::unordered_map<State, Score>{
    {start, 0}
  };

  while(not queue.empty()) {

    const auto [f_score, state] = queue.top();

    queue.pop();

    // A* uses the notation f = g + h, where f is a function of the sum of g and h
    const auto g_score = scores[state];

    if(f_score - heuristic(state) <= g_score) {

      if(not is_goal(state)) {

        const auto add_next = [&] (State next, Score distance) {

          const auto new_score = (g_score + distance);

          const auto next_not_found = [&scores, next] {
              return (scores.find(next) == scores.end());
          };

          const auto new_score_is_better = [&scores, next, new_score] {
            return new_score < scores[next];
          };

          if(next_not_found() or new_score_is_better()) {
            scores[next] = new_score;
            queue.emplace((new_score + heuristic(next)), next);
          }
        };

        // We advance to a next state by adding a new place to either end of the current state
        for_each_neighbor(state, add_next);

      } else {
        result = {state, g_score};
        break;
      }
    }
  }

  return result;
}

/*
 * Adapted from https://stackoverflow.com/a/33181173
 *
 * Heap tracks the minimum k values.
 *
 * If the internal storage space is full, it will do a heap
 * sort which puts the maximum element in the heap at the top.
 *
 * A new value will only be inserted if it's smaller than the
 * top element. On insertion, the top element will be popped
 * off the heap, the new element inserted, and the next max
 * element put at the top.
 *
 */
template<typename T = double, typename U = uintmax>
class Heap {
  private:
    U k;
  public:
    std::vector<T> container;

    Heap(U n) : k(n) {
        container.reserve(k);
    }

    void insert(T value) {
      const auto begin = container.begin();
      const auto end   = container.end();

      if(container.size() < k) {

        container.push_back(value);

        if (container.size() == k) {
          std::make_heap(begin, end);
        }

      } else if(value < container.front()) {
        // this is the equivalent of a heap_swift_down
        std::pop_heap(begin, end);
        container.back() = value;
        std::push_heap(begin, end);
      }
    }
};

auto solution(std::string_view input) {

  const auto paths = parse_input(input);

  const auto lookup = [&paths] (auto x, auto y) {
    return paths.lookup(x, y);
  };

  const auto inverse = [] (auto x) {
    return (1.0 / x);
  };

  /*
   * Estimates the lowest cost from the current location (State::first) to the goal.
   *
   * Note: A* with a heuristic of zero is equivalent to Dijkstra's search.
   */
  const auto heuristic = [&] (const auto& state) {

    auto result = Distance{};

    const auto visits = state.visits;
    const auto num_visits  = visits.count();
    const auto num_locations = paths.num_locations;

    if(num_visits < num_locations) {
      auto mins = Heap{num_locations - num_visits - 1};

      // every state has visited the 0th location, because they have to start somewhere
      // i represents the first endpoint of a path
      for(const auto first : boost::irange({1}, num_locations)) {

        if(not visits.test(first)) {

          // j represents the other endpoint of a path
          for(const auto last : boost::irange(first)) {

            if(not visits.test(last)) {
              mins.insert(inverse(lookup(first, last)));
            }
          }
        }
      }
      const auto& heap = mins.container;
      result = std::accumulate(heap.begin(), heap.end(), Distance{});
    }
    return result;
  };

  const auto is_goal = [&paths] (const auto& state) {
    return (state.visits.count() == paths.num_locations);
  };

  /*
   * Each state has a point of origin (first) and a destination (last). Both locations have
   * a "chain" of sub-paths where they act as the point of origin, and the sub-paths show
   * the distances to other destinations. A neighbor of a state is another state, and its
   * locations (first and last) are the destinations from the aforementioned sub-paths that
   * have not been visited yet.
   *
   * Every time for_each_neighbor is called, we check which of the sub-paths for which the
   * locations of the passed state act as the point of origin have not been traveled yet. The
   * destinations of those sub-paths will be part of the new state we construct (the neighbor),
   * which will be used to repeat the process, but with different locations, until no subpaths
   * remain.
   */
  const auto for_each_neighbor = [&] (const auto& state, auto&& callback) {

    for(const auto id : boost::irange(paths.num_locations)) {

      auto visits = state.visits;

      const auto target_not_visited = (not visits.test(id));

      if(target_not_visited) {

        visits.set(id);

        const auto first = state.first;
        const auto last  = state.last;

        const auto distance = state.distance;

        const auto new_distance = [&] (auto location) {
          return (lookup(location, id) + distance);
        };

        const auto last_distance  = new_distance(last);
        const auto first_distance = new_distance(first);

        /*
         * A* will supply the callback function that will
         * add the next possible state (neighbor) to explore
         * to the queue
         */
        callback({first, id, visits, last_distance}, inverse(lookup(last, id)));
        callback({id, last, visits, first_distance}, inverse(lookup(first, id)));
      }
    }
  };

  const auto begin_state = State{0, 0, 1};

  return a_star<double>(begin_state, heuristic, is_goal, for_each_neighbor).first.distance;
}

int main() {

  std::cout << solution(puzzle_input) << std::endl;

}


#include <iostream>
#include <cmath>
#include <numeric>
#include <fstream>
#include <algorithm>

#include "field2d.h"
#include "weno.h"
#include "rk.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/constants/constants.hpp>
namespace math = boost::math::constants;
using namespace boost::numeric;

/*
TODO :

* mesurer l'ordre (être certain de l'ordre en temps et en espace)
* chercher une autre source de bug qu'une erreur d'implémentation des schémas...

*/

auto
pacman_factory ( double r , double alpha , double value=1.0 ) {
  return [=]( double x , double y ) {
    if ( ( x*x + y*y < r*r ) && ( (x<0.0) || (std::abs(y)>alpha*x) ) ) {
      return value;
    }
    return 0.0;
  };
}
#define SQ(X) ((X)*(X))
auto
gauss_factory ( double X0 , double Y0 , double tx , double ty ) {
  return [=]( double x , double y ) {
    return std::exp(-SQ(x-X0)/tx - SQ(y-Y0)/ty);
  };
}

int
main(int,char**)
{
  std::size_t N = 100;
  double xmax = 5.0;
  std::size_t Nx=N,Ny=N;
  field2d<double> f(boost::extents[Nx][Ny]);
  field2d<double> df(boost::extents[Nx][Ny]);
  f.range.x_min = -xmax; f.range.x_max = xmax;
  f.range.y_min = -xmax; f.range.y_max = xmax;
  f.compute_steps();
  df.range = f.range;
  df.steps = f.steps;

  auto pacman = pacman_factory(1.0,0.5);
  auto g = gauss_factory(0.0,1.0,0.75,0.25);
  for ( auto i=0u ; i<f.size(0) ; ++i ) {
    for ( auto j=0u ; j<f.size(1) ; ++j ) {
      // f[i][j] = g(f.x(i),f.y(j));
       f[i][j] = pacman(f.x(i),f.y(j));
      // f[i][j] = std::cos(2.*math::pi<double>()*f.x(i)/xmax)*std::sin(2.*math::pi<double>()*f.y(j)/xmax);
      //f[i][j] = std::cos(2.*math::pi<double>()*f.x(i)/xmax);
    }
  }

  ublas::vector<double> x(Nx),y(Ny);
  std::generate( x.begin() , x.end() , [&,count=0] () mutable { return f.x(count++); } );
  std::generate( y.begin() , y.end() , [&,count=0] () mutable { return f.y(count++); } );

  auto save_f = [&](std::string filename) {
    std::ofstream of(filename);
    of << f << std::endl;
    of.close();
  };

  save_f("finit.dat");

  double dt= 1.3*f.steps.dx/(2.0*xmax);
  double Tf= .5*math::pi<double>();
  double current_time = 0.0;

  auto Lij = [&](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    //return - (weno2d::weno_x( -y[j] ,u,i,j) + weno2d::weno_y(  x[i] ,u,i,j));
    return - (weno2d::weno_x( -u.y(j) ,u,i,j) + weno2d::weno_y(  u.x(i) ,u,i,j));
    //return - ( weno2d::weno_x( -2.0 ,u,i,j) + weno2d::weno_y( 0.0 ,u,i,j) );
  };

  auto Lij_x = [](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    return -weno2d::weno_x( -u.y(j) ,u,i,j);
  };
  auto Lij_y = [](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    return  -weno2d::weno_y( u.x(j) ,u,i,j);
  };

  std::cout << "dt: "<< dt << "\tNiter: " << std::floor(Tf/dt) << "\n";
  std::size_t i_iter = 0;
  while ( i_iter*dt < Tf ) {
    std::cout << current_time << " \r" << std::flush;

    //f = rk33( Lij , current_time , f , dt );

    f = Lie::phi1( current_time , f , 0.5*dt );
    f = Lie::phi2( current_time , f , dt );
    f = Lie::phi1( current_time , f , 0.5*dt );

    ++i_iter;
    current_time += dt;
  }
  std::cout << current_time << std::endl;

  save_f("fend.dat");

  return 0;
}