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

// test_rayleigh.cpp

#ifdef _MSC_VER
#  pragma warning(disable: 4127) // conditional expression is constant.
#  pragma warning(disable: 4100) // unreferenced formal parameter.
#endif

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/rayleigh.hpp>
    using boost::math::rayleigh_distribution;
#include <boost/math/tools/test.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/tools/floating_point_comparison.hpp>
#include "test_out_of_range.hpp"

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;

template <class RealType>
void test_spot(RealType s, RealType x, RealType p, RealType q, RealType tolerance)
{
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         rayleigh_distribution<RealType>(s),
         x),
         p,
         tolerance); // %
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         complement(rayleigh_distribution<RealType>(s),
         x)),
         q,
         tolerance); // %
   // Special extra tests for p and q near to unity.
   if(p < 0.999)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(
            rayleigh_distribution<RealType>(s),
            p),
            x,
            tolerance); // %
   }
   if(q < 0.999)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(
            complement(rayleigh_distribution<RealType>(s),
            q)),
            x,
            tolerance); // %
   }
   if(std::numeric_limits<RealType>::has_infinity)
   {
      RealType inf = std::numeric_limits<RealType>::infinity();
      BOOST_CHECK_EQUAL(pdf(rayleigh_distribution<RealType>(s), inf), 0);
      BOOST_CHECK_EQUAL(cdf(rayleigh_distribution<RealType>(s), inf), 1);
      BOOST_CHECK_EQUAL(cdf(complement(rayleigh_distribution<RealType>(s), inf)), 0);
   }
} // void test_spot

template <class RealType>
void test_spots(RealType T)
{
   using namespace std; // ADL of std names.
   // Basic sanity checks.
   // 50 eps as a percentage, up to a maximum of double precision
   // (that's the limit of our test data: obtained by punching
   // numbers into a calculator).
   RealType tolerance = (std::max)(
      static_cast<RealType>(boost::math::tools::epsilon<double>()),
      boost::math::tools::epsilon<RealType>());
   tolerance *= 10 * 100; // 10 eps as a percent
   cout << "Tolerance for type " << typeid(T).name()  << " is " << tolerance << " %" << endl;

  using namespace boost::math::constants;

   // Things that are errors:
   rayleigh_distribution<RealType> dist(0.5);

   check_out_of_range<rayleigh_distribution<RealType> >(1);
   BOOST_MATH_CHECK_THROW(
       quantile(dist,
       RealType(1.)), // quantile unity should overflow.
       std::overflow_error);
   BOOST_MATH_CHECK_THROW(
       quantile(complement(dist,
       RealType(0.))), // quantile complement zero should overflow.
       std::overflow_error);
   BOOST_MATH_CHECK_THROW(
       pdf(dist, RealType(-1)), // Bad negative x.
       std::domain_error);
   BOOST_MATH_CHECK_THROW(
       cdf(dist, RealType(-1)), // Bad negative x.
       std::domain_error);
   BOOST_MATH_CHECK_THROW(
       cdf(rayleigh_distribution<RealType>(-1), // bad sigma < 0
       RealType(1)),
       std::domain_error);
   BOOST_MATH_CHECK_THROW(
       cdf(rayleigh_distribution<RealType>(0), // bad sigma == 0
       RealType(1)),
       std::domain_error);
   BOOST_MATH_CHECK_THROW(
       quantile(dist, RealType(-1)), // negative quantile probability.
       std::domain_error);
   BOOST_MATH_CHECK_THROW(
       quantile(dist, RealType(2)), // > unity  quantile probability.
       std::domain_error);

   test_spot(
      static_cast<RealType>(1.L), // sigma
      static_cast<RealType>(1.L), // x
      static_cast<RealType>(1 - exp_minus_half<RealType>()), // p
      static_cast<RealType>(exp_minus_half<RealType>()), // q
      tolerance);

   test_spot(
      static_cast<RealType>(0.5L), // sigma
      static_cast<RealType>(0.5L), // x
      static_cast<RealType>(1 - exp_minus_half<RealType>()), // p
      static_cast<RealType>(exp_minus_half<RealType>()), //q
      tolerance);

   test_spot(
      static_cast<RealType>(3.L), // sigma
      static_cast<RealType>(3.L), // x
      static_cast<RealType>(1 - exp_minus_half<RealType>()), // p
      static_cast<RealType>(exp_minus_half<RealType>()), //q
      tolerance);

   BOOST_CHECK_CLOSE(
      ::boost::math::pdf(
         rayleigh_distribution<RealType>(1.L),
         static_cast<RealType>(1.L)),              // x
         static_cast<RealType>(exp_minus_half<RealType>()), // probability.
         tolerance); // %
   BOOST_CHECK_CLOSE(
      ::boost::math::pdf(
         rayleigh_distribution<RealType>(0.5L),
         static_cast<RealType>(0.5L)),              // x
         static_cast<RealType>(2 * exp_minus_half<RealType>()), // probability.
         tolerance); // %
   BOOST_CHECK_CLOSE(
      ::boost::math::pdf(
         rayleigh_distribution<RealType>(2.L),
         static_cast<RealType>(2.L)),              // x
         static_cast<RealType>(exp_minus_half<RealType>() /2),  // probability.
         tolerance); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::mean(
         rayleigh_distribution<RealType>(1.L)),
         static_cast<RealType>(root_half_pi<RealType>()),
         tolerance); // %
   BOOST_CHECK_CLOSE(
      ::boost::math::variance(
         rayleigh_distribution<RealType>(root_two<RealType>())),
         static_cast<RealType>(four_minus_pi<RealType>()),
         tolerance * 100); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::mode(
         rayleigh_distribution<RealType>(1.L)),
         static_cast<RealType>(1.L),
         tolerance); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::median(
         rayleigh_distribution<RealType>(1.L)),
         static_cast<RealType>(sqrt(log(4.L))),  // sigma * sqrt(log_four)
         tolerance); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::skewness(
         rayleigh_distribution<RealType>(1.L)),
         static_cast<RealType>(2.L * root_pi<RealType>()) * (pi<RealType>() - 3) / (pow((4 - pi<RealType>()), static_cast<RealType>(1.5L))),
         tolerance * 100); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::skewness(
         rayleigh_distribution<RealType>(1.L)),
         static_cast<RealType>(0.63111065781893713819189935154422777984404221106391L),
         tolerance * 100); // %

   BOOST_CHECK_CLOSE(
     ::boost::math::kurtosis_excess(
     rayleigh_distribution<RealType>(1.L)),
     -static_cast<RealType>(6 * pi<RealType>() * pi<RealType>() - 24 * pi<RealType>() + 16) /
        ((4 - pi<RealType>()) * (4 - pi<RealType>())),
        // static_cast<RealType>(0.2450893006876380628486604106197544154170667057995L),
         tolerance * 1000); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::kurtosis(
         rayleigh_distribution<RealType>(1.L)),
         static_cast<RealType>(3.2450893006876380628486604106197544154170667057995L),
         tolerance * 100); // %

   BOOST_CHECK_CLOSE(
      ::boost::math::kurtosis_excess(rayleigh_distribution<RealType>(2)),
      ::boost::math::kurtosis(rayleigh_distribution<RealType>(2)) -3,
         tolerance* 100); // %
   return;

} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{
  // Check that can generate rayleigh distribution using the two convenience methods:
   boost::math::rayleigh ray1(1.); // Using typedef
   rayleigh_distribution<> ray2(1.); // Using default RealType double.

  using namespace boost::math::constants;
   // Basic sanity-check spot values.

  // Double only tests.
   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::pdf(
      rayleigh_distribution<double>(1.),
      static_cast<double>(1)), // x
      static_cast<double>(exp_minus_half<double>()), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::pdf(
      rayleigh_distribution<double>(0.5),
      static_cast<double>(0.5)), // x
      static_cast<double>(2 * exp_minus_half<double>()), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::pdf(
      rayleigh_distribution<double>(2.),
      static_cast<double>(2)), // x
      static_cast<double>(exp_minus_half<double>() /2 ), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(
      rayleigh_distribution<double>(1.),
      static_cast<double>(1)), // x
      static_cast<double>(1- exp_minus_half<double>()), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(
      rayleigh_distribution<double>(2.),
      static_cast<double>(2)), // x
      static_cast<double>(1- exp_minus_half<double>()), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(
      rayleigh_distribution<double>(3.),
      static_cast<double>(3)), // x
      static_cast<double>(1- exp_minus_half<double>()), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(
      rayleigh_distribution<double>(4.),
      static_cast<double>(4)), // x
      static_cast<double>(1- exp_minus_half<double>()), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(complement(
      rayleigh_distribution<double>(4.),
      static_cast<double>(4))), // x
      static_cast<double>(exp_minus_half<double>()), // q = 1 - p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::quantile(
      rayleigh_distribution<double>(4.),
      static_cast<double>(1- exp_minus_half<double>())), // x
      static_cast<double>(4), // p
         1e-15); // %

   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::quantile(complement(
      rayleigh_distribution<double>(4.),
      static_cast<double>(exp_minus_half<double>()))), // x
      static_cast<double>(4), // p
         1e-15); // %

   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#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::endl;
#endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output is:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_rayleigh.exe"
Running 1 test case...
Tolerance for type float is 0.000119209 %
Tolerance for type double is 2.22045e-013 %
Tolerance for type long double is 2.22045e-013 %
Tolerance for type class boost::math::concepts::real_concept is 2.22045e-013 %
*** No errors detected

*/


#include <stdlib.h>
#include <boost/test/unit_test.hpp>
#include "sphere_shape.h"
#include "octree.h"

BOOST_AUTO_TEST_SUITE (test_octree)

BOOST_AUTO_TEST_CASE (test_subcluster_bounds)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(0);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(-1, -1, -1)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(0, 0, 0)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(1);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(0, -1, -1)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(1, 0, 0)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(2);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(-1, 0, -1)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(0, 1, 0)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(3);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(0, 0, -1)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(1, 1, 0)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(4);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(-1, -1, 0)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(0, 0, 1)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(5);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(0, -1, 0)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(1, 0, 1)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(6);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(-1, 0, 0)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(0, 1, 1)).length_sq() < math::EPSILON);
	}

	{
		math::aabb<3> bounds = octree.get_subcluster_bounds(7);
		BOOST_REQUIRE ((bounds.lo - math::vec<3>(0, 0, 0)).length_sq() < math::EPSILON);
		BOOST_REQUIRE ((bounds.hi - math::vec<3>(1, 1, 1)).length_sq() < math::EPSILON);
	}
}

BOOST_AUTO_TEST_CASE (test_subcluster_by_point)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-10, -100, -1000), math::vec<3>(10, 100, 1000)));

	for (size_t i = 0; i < 1000; i++)
	{
		math::vec<3> v(math::scalar((rand() % 20) - 10), math::scalar((rand() % 200) - 100),
			math::scalar((rand() % 2000) - 1000));

		unsigned index = octree.get_subcluster_by_point(v);
		math::aabb<3> bounds = octree.get_subcluster_bounds(index);

		BOOST_REQUIRE (bounds.contains(v));
	}
}

BOOST_AUTO_TEST_CASE (test_subclusters_mask_by_aabb)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-5, -5, -5), math::vec<3>(5, 5, 5)));

	// -2,-2 -1,-2 | +1,-2 +2,-2
    // -2,-1 -1,-1 | +1,-1 +2,-1
	// ------------+------------
    // -2,+1 -1,+1 | +1,+1 +2,+1
    // -2,+1 -1,+2 | +1,+1 +2,+2

	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, -2), math::vec<3>(-1, -1, -1))) == 1);
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(1, -2, -2), math::vec<3>(2, -1, -1))) == 2);
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, 1, -2), math::vec<3>(-1, 2, -1))) == 4);
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(1, 1, -2), math::vec<3>(2, 2, -1))) == 8);

	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, 1), math::vec<3>(-1, -1, 2))) == 16);
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(1, -2, 1), math::vec<3>(2, -1, 2))) == 32);
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, 1, 1), math::vec<3>(-1, 2, 2))) == 64);
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(1, 1, 1), math::vec<3>(2, 2, 2))) == 128);

	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, -2), math::vec<3>(2, -1, -1))) == (1 | 2));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, -2), math::vec<3>(-1, 2, -1))) == (1 | 4));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(1, -2, -2), math::vec<3>(2, 2, -1))) == (2 | 8));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, 1, -2), math::vec<3>(2, 2, -1))) == (4 | 8));

	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, 1), math::vec<3>(2, -1, 2))) == (16 | 32));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, 1), math::vec<3>(-1, 2, 2))) == (16 | 64));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(1, -2, 1), math::vec<3>(2, 2, 2))) == (32 | 128));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, 1, 1), math::vec<3>(2, 2, 2))) == (64 | 128));

	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, -2), math::vec<3>(2, 2, -1))) == (1 | 2 | 4 | 8));
	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, 1), math::vec<3>(2, 2, 2))) == (16 | 32 | 64 | 128));

	BOOST_REQUIRE (octree.get_subclusters_mask_by_aabb(math::aabb<3>(math::vec<3>(-2, -2, -2), math::vec<3>(2, 2, 2))) == (1 | 2 | 4 | 8 | 16 | 32 | 64 | 128));
}

BOOST_AUTO_TEST_CASE (test_intersect_correctness_1)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	std::vector<spatial::ShapePtr> shapes;
	shapes.reserve(8);

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>(-0.5f, -0.5f, -0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>( 0.5f, -0.5f, -0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>(-0.5f,  0.5f, -0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>( 0.5f,  0.5f, -0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>(-0.5f, -0.5f,  0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>( 0.5f, -0.5f,  0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>(-0.5f,  0.5f,  0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(math::vec<3>(0.5f,  0.5f,  0.5f), 0.1f)));
	octree.add_shape(shapes.back());

	octree.build_subclusters();

	for (size_t i = 0; i < 100; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		boost::shared_ptr<spatial::SphereShape> shape(new spatial::SphereShape(centre, radius));

		spatial::ResultSetPtr result(new spatial::ResultSet());
		octree.query_intersection(shape, result);

		for (size_t j = 0; j < shapes.size(); j++)
		{
			bool p1 = shapes[j]->test_intersection(shape.get());
			bool p2 = result->have(shapes[j]);

			BOOST_REQUIRE (p1 == p2);
		}
	}
}

BOOST_AUTO_TEST_CASE (test_intersect_correctness_2)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	static size_t const N = 500;
	static size_t const M = 100;

	std::vector<spatial::ShapePtr> shapes;
	shapes.reserve(N);

	for (size_t i = 0; i < N; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(centre, radius)));
		octree.add_shape(shapes.back());
	}

	for (size_t i = 0; i < M; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		spatial::ShapePtr shape(new spatial::SphereShape(centre, radius));

		spatial::ResultSetPtr result(new spatial::ResultSet());
		octree.query_intersection(shape, result);

		for (size_t j = 0; j < N; j++)
		{
			bool p1 = shapes[j]->test_intersection(shape.get());
			bool p2 = result->have(shapes[j]);

			BOOST_REQUIRE (p1 == p2);
		}
	}
}

BOOST_AUTO_TEST_CASE (test_remove_correctness)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	static size_t const N = 500;
	static size_t const M = 100;

	std::vector<spatial::ShapePtr> shapes;
	shapes.reserve(N);

	for (size_t i = 0; i < N; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(centre, radius)));
		octree.add_shape(shapes.back());
	}

	{
		std::vector<spatial::ShapePtr> shapes_clone;
		shapes_clone.swap(shapes);

		shapes.reserve(N);

		for (size_t i = 0; i < N; i++)
		{
			if (rand() & 1)
			{
				shapes.push_back(shapes_clone[i]);
			}
			else
			{
				octree.remove_shape(shapes_clone[i]);
			}
		}
	}

	for (size_t i = 0; i < M; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		spatial::ShapePtr shape(new spatial::SphereShape(centre, radius));

		spatial::ResultSetPtr result(new spatial::ResultSet());		octree.query_intersection(shape, result);

		for (size_t j = 0; j < shapes.size(); j++)
		{
			bool p1 = shapes[j]->test_intersection(shape.get());
			bool p2 = result->have(shapes[j]);

			BOOST_REQUIRE (p1 == p2);
		}
	}
}

BOOST_AUTO_TEST_CASE (test_move_correctness)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	static size_t const N = 500;
	static size_t const M = 100;

	std::vector<spatial::ShapePtr> shapes;
	shapes.reserve(N);

	for (size_t i = 0; i < N; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(centre, radius)));
		octree.add_shape(shapes.back());
	}

	for (size_t i = 0; i < N; i++)
	{
		if (rand() & 1)
		{
			spatial::SphereShape *shape = static_cast<spatial::SphereShape *>(shapes[i].get());

			math::aabb<3> old_bounds = shape->get_bounds();

			shape->centre += math::vec<3>(math::random(-1, 1),
				math::random(-1, 1),
				math::random(-1, 1));

			shape->radius *= math::random(0, 2);

			octree.post_move_shape(shapes[i], old_bounds);
		}
	}

	for (size_t i = 0; i < M; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		spatial::ShapePtr shape(new spatial::SphereShape(centre, radius));

		spatial::ResultSetPtr result(new spatial::ResultSet());
		octree.query_intersection(shape, result);

		for (size_t j = 0; j < shapes.size(); j++)
		{
			bool p1 = shapes[j]->test_intersection(shape.get());
			bool p2 = result->have(shapes[j]);

			BOOST_REQUIRE (p1 == p2);
		}
	}
}

BOOST_AUTO_TEST_CASE (test_add_shape_result)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	static size_t const N = 500;
	static size_t const M = 100;

	std::vector<spatial::ShapePtr> shapes;
	shapes.reserve(N);

	for (size_t i = 0; i < N; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		spatial::ShapePtr shape(new spatial::SphereShape(centre, radius));
		shapes.push_back(shape);

		 // result may not contain original shape
		spatial::ResultSetPtr result(new spatial::ResultSet());
		octree.add_shape(shape, result);

		for (size_t j = 0; j < i; j++)
		{
			bool p1 = shapes[j]->test_intersection(shape.get());
			bool p2 = result->have(shapes[j]);

			BOOST_REQUIRE (p1 == p2);
		}
	}
}

BOOST_AUTO_TEST_CASE (test_move_result)
{
	spatial::Octree octree(math::aabb<3>(math::vec<3>(-1, -1, -1), math::vec<3>(1, 1, 1)));

	static size_t const N = 500;
	static size_t const M = 100;

	std::vector<spatial::ShapePtr> shapes;
	shapes.reserve(N);

	for (size_t i = 0; i < N; i++)
	{
		math::vec<3> centre(math::random(-1, 1), math::random(-1, 1),
			math::random(-1, 1));
		math::scalar radius = math::scalar(rand()) / RAND_MAX;

		shapes.push_back(spatial::ShapePtr(new spatial::SphereShape(centre, radius)));
		octree.add_shape(shapes.back());
	}

	for (size_t i = 0; i < N; i++)
	{
		spatial::SphereShape *shape = static_cast<spatial::SphereShape *>(shapes[i].get());

		math::aabb<3> old_bounds = shape->get_bounds();

		shape->centre += math::vec<3>(math::random(-1, 1), math::random(-1, 1), math::random(-1, 1));
		shape->radius *= math::random(0, 2);

		spatial::ResultSetPtr result(new spatial::ResultSet());
		octree.post_move_shape(shapes[i], old_bounds, result);

		for (size_t j = 0; j < i; j++)
		{
			bool p1 = shapes[j]->test_intersection(shape);
			bool p2 = result->have(shapes[j]);

			BOOST_REQUIRE (p1 == p2);
		}
	}
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>
#include <limits>

#include <solvers/qp.hpp>

namespace sqp {

template <typename T>
class SQP;

template <typename Scalar>
struct sqp_settings_t {
    Scalar tau = 0.5;       /**< line search iteration decrease, 0 < tau < 1 */
    Scalar eta = 0.25;      /**< line search parameter, 0 < eta < 1 */
    Scalar rho = 0.5;       /**< line search parameter, 0 < rho < 1 */
    Scalar eps_prim = 1e-4; /**< primal step termination threshold, eps_prim > 0 */
    Scalar eps_dual = 1e-4; /**< dual step termination threshold, eps_dual > 0 */
    int max_iter = 100;
    int line_search_max_iter = 20;
    bool second_order_correction = false;
    std::function<void(SQP<Scalar>&)> iteration_callback;

    bool validate() {
        bool valid;
        valid = 0.0 < tau && tau < 1.0 && 0.0 < eta && eta < 1.0 && 0.0 < rho && rho < 1.0 &&
                eps_prim < 0.0 && eps_dual < 0.0 && max_iter > 0 && line_search_max_iter > 0;
        return valid;
    }
};

typedef enum { SOLVED, MAX_ITER_EXCEEDED, INVALID_SETTINGS } Status;

struct Info {
    int iter;
    int qp_solver_iter;
    Status status;

    void print() {
        printf("SQP info:\n");
        printf("  iter: %d\n", iter);
        printf("  qp_solver_iter: %d\n", qp_solver_iter);
        printf("  status: ");
        switch (status) {
            case SOLVED:
                printf("SOLVED\n");
                break;
            case MAX_ITER_EXCEEDED:
                printf("MAX_ITER_EXCEEDED\n");
                break;
            case INVALID_SETTINGS:
                printf("INVALID_SETTINGS\n");
                break;
            default:
                printf("UNKNOWN\n");
                break;
        }
    }
};

template <typename Scalar_ = double>
struct NonLinearProblem {
    using Scalar = Scalar_;
    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

    int num_var;
    int num_constr;

    virtual void objective(const Vector& x, Scalar& obj) = 0;
    virtual void objective_linearized(const Vector& x, Vector& grad, Scalar& obj) = 0;
    virtual void constraint(const Vector& x, Vector& c, Vector& l, Vector& u) = 0;
    virtual void constraint_linearized(const Vector& x, Matrix& Jc, Vector& c, Vector& l,
                                       Vector& u) = 0;
};

/*
 * minimize     f(x)
 * subject to   l <= c(x) <= u
 */
template <typename Scalar_>
class SQP {
   public:
    using Scalar = Scalar_;
    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using Vector = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using Problem = NonLinearProblem<Scalar>;
    using Settings = sqp_settings_t<Scalar>;

    // Constants
    static constexpr Scalar DIV_BY_ZERO_REGUL = std::numeric_limits<Scalar>::epsilon();

    // enforce 16 byte alignment
    // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SQP();
    ~SQP() = default;

    void solve(Problem& prob, const Vector& x0, const Vector& lambda0);
    void solve(Problem& prob);

    inline const Vector& primal_solution() const { return x_; }
    inline Vector& primal_solution() { return x_; }

    inline const Vector& dual_solution() const { return lambda_; }
    inline Vector& dual_solution() { return lambda_; }

    inline const Settings& settings() const { return settings_; }
    inline Settings& settings() { return settings_; }

    inline const Info& info() const { return info_; }
    inline Info& info() { return info_; }

    // private:
    void run_solve(Problem& prob);

    bool termination_criteria(const Vector& x, Problem& prob);
    void solve_qp(Problem& prob, Vector& p, Vector& lambda);
    bool run_solve_qp(const Matrix& P, const Vector& q, const Matrix& A, const Vector& l,
                      const Vector& u, Vector& prim, Vector& dual);

    /** Second order correction by solving the same QP with corrected constraints. */
    void second_order_correction(Problem& prob, Vector& p, Vector& lambda);

    /** Line search in direction p using l1 merit function. */
    Scalar line_search(Problem& prob, const Vector& p);

    /** L1 norm of constraint violation */
    Scalar constraint_norm(const Vector& x, Problem& prob);

    /** L1 norm of constraint violation, for given constraint evaluation */
    Scalar constraint_norm(const Vector &constr, const Vector &l, const Vector &u) const;

    /** L_inf norm of constraint violation */
    Scalar max_constraint_violation(const Vector& x, Problem& prob);

    // Solver state variables
    Vector x_;
    Vector lambda_;
    Vector step_prev_;
    Vector grad_L_;
    Vector delta_grad_L_;

    Matrix Hess_;
    Vector grad_obj_;
    Scalar obj_;
    Matrix Jac_constr_;
    Vector constr_;
    Vector l_, u_;

    // info
    Scalar dual_step_norm_;
    Scalar primal_step_norm_;

    Settings settings_;
    Info info_;

    qp_solver::QPSolver<Scalar> qp_solver_;
};

extern template class SQP<double>;

}  // namespace sqp


/**
 * @file scaling_test.cpp
 *
 * Tests for Scaling of dataset.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/data/scaler_methods/pca_whitening.hpp>
#include <mlpack/core/data/scaler_methods/zca_whitening.hpp>
#include <mlpack/core/data/scaler_methods/min_max_scaler.hpp>
#include <mlpack/core/data/scaler_methods/max_abs_scaler.hpp>
#include <mlpack/core/data/scaler_methods/standard_scaler.hpp>
#include <mlpack/core/data/scaler_methods/mean_normalization.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::data;
using namespace std;

BOOST_AUTO_TEST_SUITE(ScalingTest);

arma::mat dataset = "-1 -0.5 0 1;"
                    "2 6 10 18;";
arma::mat scaleddataset;
arma::mat temp;

/**
 * Test For MinMax Scaler Class.
 */
BOOST_AUTO_TEST_CASE(MinMaxScalerTest)
{
  arma::mat scaled = "0 0.2500 0.5000 1.000;"
                     "0 0.2500 0.5000 1.000;";
  data::MinMaxScaler scale;
  scale.Fit(dataset);
  scale.Transform(dataset, scaleddataset);
  scale.InverseTransform(scaleddataset, temp);
  CheckMatrices(scaleddataset, scaled);
  CheckMatrices(dataset, temp);
}

/**
 * Test For MaxAbs Scaler Class.
 */
BOOST_AUTO_TEST_CASE(MaxAbsScalerTest)
{
  arma::mat scaled = "-1 -0.5 0 1;"
                     "0.1111111111 0.3333333333 0.55555556 1.0000;";
  data::MaxAbsScaler scale;
  scale.Fit(dataset);
  scale.Transform(dataset, scaleddataset);
  scale.InverseTransform(scaleddataset, temp);
  CheckMatrices(scaleddataset, scaled);
  CheckMatrices(dataset, temp);
}

/**
 * Test For Standard Scaler Class.
 */
BOOST_AUTO_TEST_CASE(StandardScalerTest)
{
  arma::mat scaled = "-1.18321596 -0.50709255  0.16903085 1.52127766;"
                     "-1.18321596 -0.50709255  0.16903085 1.52127766;";
  data::StandardScaler scale;
  scale.Fit(dataset);
  scale.Transform(dataset, scaleddataset);
  scale.InverseTransform(scaleddataset, temp);
  CheckMatrices(scaleddataset, scaled);
  CheckMatrices(dataset, temp);
}

/**
 * Test For MeanNormalization Scaler Class.
 */
BOOST_AUTO_TEST_CASE(MeanNormalizationTest)
{
  arma::mat scaled = "-0.43750000000 -0.187500000 0.062500000 0.562500000;"
                     "-0.43750000000 -0.187500000 0.062500000 0.562500000;";
  data::MeanNormalization scale;
  scale.Fit(dataset);
  scale.Transform(dataset, scaleddataset);
  scale.InverseTransform(scaleddataset, temp);
  CheckMatrices(scaleddataset, scaled);
  CheckMatrices(dataset, temp);
}

/**
 * Test to pass same matrix as input and output
 */
BOOST_AUTO_TEST_CASE(SameInputOutputTest)
{
  temp = dataset;
  arma::mat scaled = "-0.43750000000 -0.187500000 0.062500000 0.562500000;"
                     "-0.43750000000 -0.187500000 0.062500000 0.562500000;";
  data::MeanNormalization scale;
  scale.Fit(temp);
  scale.Transform(temp, temp);
  CheckMatrices(scaled, temp);
  scale.InverseTransform(temp, temp);
  CheckMatrices(dataset, temp);
}

/**
 * Test for Zero Matrix.
 */
BOOST_AUTO_TEST_CASE(ZeroMatrixTest)
{
  arma::mat input(2, 4, arma::fill::zeros);
  data::MeanNormalization scale;
  scale.Fit(input);
  scale.Transform(input, temp);
  CheckMatrices(input, temp);
  scale.InverseTransform(input, temp);
  CheckMatrices(input, temp);
}

/**
 * Test for Zero Scale.
 */
BOOST_AUTO_TEST_CASE(ZeroScaleTest)
{
  dataset = "1 1 1 1;"
            "2 6 10 18;";
  arma::mat scaled = "0 0 0 0;"
                     "0 0.2500 0.5000 1.000;";
  data::MinMaxScaler scale;
  scale.Fit(dataset);
  scale.Transform(dataset, scaleddataset);
  scale.InverseTransform(scaleddataset, temp);
  CheckMatrices(scaleddataset, scaled);
  CheckMatrices(dataset, temp);
}

/**
 * Test for PCA whitening Scale.
 */
BOOST_AUTO_TEST_CASE(PCAWhiteningTest)
{
  data::PCAWhitening scale;
  arma::mat output;
  scale.Fit(dataset);
  scale.Transform(dataset, output);
  arma::vec diagonals = (mlpack::math::ColumnCovariance(output)).diag();
  // Checking covarience is close to 1.0
  double ccovsum = 0.0;
  for (size_t i = 0; i < diagonals.n_elem; i++)
    ccovsum += diagonals(i);
  BOOST_REQUIRE_CLOSE(ccovsum, 1.0, 1e-3);
  scale.InverseTransform(output, temp);
  CheckMatrices(dataset, temp);
}

/**
 * Test for ZCA whitening Scale.
 */
BOOST_AUTO_TEST_CASE(ZCAWhiteningTest)
{
  data::ZCAWhitening scale;
  arma::mat output;
  scale.Fit(dataset);
  scale.Transform(dataset, output);
  arma::vec diagonals = (mlpack::math::ColumnCovariance(output)).diag();
  // Check that the covariance is close to 1.0.
  double ccovsum = 0.0;
  for (size_t i = 0; i < diagonals.n_elem; i++)
    ccovsum += diagonals(i);
  BOOST_REQUIRE_CLOSE(ccovsum, 1.0, 1e-3);
  scale.InverseTransform(output, temp);
  CheckMatrices(dataset, temp);
}

BOOST_AUTO_TEST_SUITE_END();


/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_GEOMETRY_HPP
#define UTILITIES_GEOMETRY_GEOMETRY_HPP

#include "../UtilitiesAPI.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  class Point3d;
  class PointLatLon;
  class Vector3d;

  /// convert degrees to radians
  UTILITIES_API double degToRad(double degrees);

  /// convert radians to degrees
  UTILITIES_API double radToDeg(double radians);

  /// compute area from surface as Point3dVector
  UTILITIES_API boost::optional<double> getArea(const std::vector<Point3d>& points);

  /// compute Newall vector from surface as Point3dVector, direction is same as outward normal
  /// magnitude is twice the area
  UTILITIES_API boost::optional<Vector3d> getNewallVector(const std::vector<Point3d>& points);

  /// compute outward normal from surface as Point3dVector
  UTILITIES_API boost::optional<Vector3d> getOutwardNormal(const std::vector<Point3d>& points);

  /// compute centroid from surface as Point3dVector
  UTILITIES_API boost::optional<Point3d> getCentroid(const std::vector<Point3d>& points);

  /// reorder points to upper-left-corner convention
  UTILITIES_API std::vector<Point3d> reorderULC(const std::vector<Point3d>& points);

  /// removes collinear points, tolerance is for length of cross product after normalizing each line segment
  UTILITIES_API std::vector<Point3d> removeCollinear(const std::vector<Point3d>& points, double tol = 0.001);

  /// return distance between two points
  UTILITIES_API double getDistance(const Point3d& point1, const Point3d& point2);

  /// return distance between a point and a line segment
  /// returns 0 if lineSegment does not have length 2
  UTILITIES_API double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment);

  /// return distance between a point and a triangle
  /// returns 0 if triangle does not have length 3
  UTILITIES_API double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle);

  /// return angle (in radians) between two vectors
  UTILITIES_API double getAngle(const Vector3d& vector1, const Vector3d& vector2);

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  UTILITIES_API double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2);

  /// check if two vectors of points are equal (within tolerance) irregardless of initial ordering.
  UTILITIES_API bool circularEqual(const std::vector<Point3d>& points1, const std::vector<Point3d>& points2, double tol = 0.001);

  /// if point3d is within tol of any existing points then returns existing point
  /// otherwise adds point3d to allPoints and returns point3d
  UTILITIES_API Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol = 0.001);

  /// compute triangulation of vertices, holes are removed in the triangulation
  /// requires that vertices and holes are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > computeTriangulation(const std::vector<Point3d>& vertices, const std::vector<std::vector<Point3d> >& holes, double tol = 0.001);

  /// move all vertices towards point by distance, pass negative distance to move away from point
  /// no guarantee that resulting polygon will be valid
  UTILITIES_API std::vector<Point3d> moveVerticesTowardsPoint(const std::vector<Point3d>& vertices, const Point3d& point, double distance);

  /// reverse order of vertices
  UTILITIES_API std::vector<Point3d> reverse(const std::vector<Point3d>& vertices);

} // openstudio

#endif //UTILITIES_GEOMETRY_GEOMETRY_HPP


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#include <stapl/containers/sequential/graph/bgl_undirected_graph_adapter.hpp>
#include <stapl/containers/sequential/graph/algorithms/graph_algo.h>
#include <stapl/containers/sequential/graph/algorithms/find_cycle.h>
#include <stapl/containers/sequential/graph/algorithms/connected_components.h>
#include <stapl/containers/sequential/graph/algorithms/dijkstra.h>
#include "test_util.h"
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/unordered_map.hpp>
#include <papi.h>

using namespace boost;
using namespace stapl;
using namespace std;
using namespace __gnu_cxx;

namespace dijkstra_test {

template <class G, class ColorMap>
void dijkstra_test_core_graph(G& g, ColorMap& cmap){
  typedef typename G::vertex_descriptor VD;
  stapl::graph<stapl::DIRECTED,stapl::NONMULTIEDGES,typename G::vertex_property, double> sssptree;

  bool err;
  err=false;

  double tw1, tw2, tw3, tw4, tw5;

  // Initialize the library
  PAPI_library_init(PAPI_VER_CURRENT);

		/////////////////STAPL_ALGO ON STAPL_GRAPH///////////////////////////////////
  long_long t = PAPI_get_real_usec();

		vector<VD> parentmap(g.get_num_vertices());
		vector<double> weightmap(g.get_num_vertices());

  dijkstra_sssp(g, parentmap, weightmap, 0);
  t = PAPI_get_real_usec() - t;
  std::cout <<"\t"<< t << ", ";

		stapl::convert_to_graph(parentmap, weightmap, sssptree);

  cmap.reset();
  if((sssptree.get_num_vertices() == g.get_num_vertices())
     && (sssptree.get_num_vertices() == sssptree.get_num_edges()+1)
     && (is_cycle(sssptree, cmap) == false)) {
    cmap.reset();
    if (get_cc_count(sssptree, cmap) == 1) {
      tw1 = total_weight(sssptree);
      cout<<"<PASSED>\t"; // << endl;
    }
  } else {
    cout<<"{FAILED}\t";
    cmap.reset();
  }
  if(N < 15) display1(sssptree);

		/////////////////BGL_ALGO ON STAPL_GRAPH///////////////////////////////////
  std::vector < typename G::vertex_descriptor >
    p(g.get_num_vertices());

  typedef boost::unordered_map<size_t, int> map_type;
  //typedef std::map<size_t, int> map_type;
  map_type distances(g.get_num_vertices());  // better performance.
  boost::associative_property_map<map_type> dist_map(distances);

  {
    using namespace boost;

				// boost::property_map<G, edge_weight_t>::type weightmap = boost::get(edge_weight, g);
				// boost::property_map<G, vertex_distance_t>::type dist_map = get(vertex_distance, g);

				std::vector<typename G::edge_property> weights_map(g.get_num_edges());
				for (typename G::edge_iterator ei = g.edges_begin(); ei != g.edges_end(); ++ei)
						weights_map[ei.descriptor().id()] = ei.property();

				vector<typename G::edge_property>* w_vec_ptr = const_cast<vector<typename G::edge_property>* >(&weights_map);

				PAPI_library_init(PAPI_VER_CURRENT);
				long_long t = PAPI_get_real_usec();

				std::less<double> lsop;
				std::plus<double> combine;

				dijkstra_shortest_paths
						(g, *vertices(g).first, &p[0], dist_map,
							stapl_graph_edge_wt_id_map<typename G::vertex_property,
							typename G::edge_property>(const_cast<G*>(&g),
																																		w_vec_ptr),
						stapl_graph_id_map<typename G::vertex_property, typename G::edge_property>(),
							lsop, combine, std::numeric_limits<double>::max(), 0,
							default_dijkstra_visitor());

    t = PAPI_get_real_usec() - t;
    std::cout <<"\t"<< t << ", ";

    stapl::graph<stapl::DIRECTED,stapl::MULTIEDGES,typename G::vertex_property, int> boost_out_graph;
    convert_to_graph(p, distances, boost_out_graph);

    cmap.reset();
    if((boost_out_graph.get_num_vertices() == g.get_num_vertices())
       && (boost_out_graph.get_num_vertices() == boost_out_graph.get_num_edges()+1)
       && (is_cycle(boost_out_graph, cmap) == false)) {
      cmap.reset();
      if (get_cc_count(boost_out_graph, cmap) == 1) {
        tw3 = total_weight(boost_out_graph);
        cout<<"<PASSED>\t"; // << endl;
      }
    } else {
      cout<<"{FAILED}\t";
      cmap.reset();
    }
    if(N < 15) display2(boost_out_graph);
  }

  /////////////////BGL_ALGO ON BGL_GRAPH///////////////////////////////////
  {
    using namespace boost;
    typedef adjacency_list <vecS,
      vecS,
      undirectedS,
      property<vertex_distance_t, int>,
      property <edge_weight_t, double> > BOOST_GRAPH;

    BOOST_GRAPH bg;

    property_map<BOOST_GRAPH, edge_weight_t>::type weightmap;
    generate_boost_graph_from_stapl(g, bg, weightmap);

    PAPI_library_init(PAPI_VER_CURRENT);
    long_long t = PAPI_get_real_usec();
    std::vector < graph_traits < BOOST_GRAPH >::vertex_descriptor >
      parentmap(num_vertices(bg));
    property_map<BOOST_GRAPH, vertex_distance_t>::type distmap = get(vertex_distance, bg);
    property_map<BOOST_GRAPH, vertex_index_t>::type indexmap = get(vertex_index, bg);

    std::less<double> lsop;
    // detail::_project2nd<double,double> combine;  // for Prim's MST.
    std::plus<double> combine;

    dijkstra_shortest_paths
      (bg, *vertices(bg).first, &parentmap[0], distmap, weightmap, indexmap,
       lsop, combine, std::numeric_limits<double>::max(), 0,
       default_dijkstra_visitor());

    t = PAPI_get_real_usec() - t;
    std::cout << "\t"<< t << ", ";

    stapl::graph<stapl::DIRECTED,stapl::MULTIEDGES,typename G::vertex_property, int> boost_out_graph1;
    convert_to_graph(parentmap, distmap, boost_out_graph1);

    cmap.reset();
    if((boost_out_graph1.get_num_vertices() == g.get_num_vertices())
       && (boost_out_graph1.get_num_vertices() == boost_out_graph1.get_num_edges()+1)
       && (is_cycle(boost_out_graph1, cmap) == false)) {
      cmap.reset();
      if (get_cc_count(boost_out_graph1, cmap) == 1) {
        tw5 = total_weight(boost_out_graph1);
        cout<<"<PASSED>\t"; // << endl;
      }
    } else {
      cout<<"{FAILED}\t";
      cmap.reset();
    }
    if(N < 15) display1(boost_out_graph1);
  }

  cout << endl;
  cout << tw1 << ", "  << tw2 << ", "  << tw3 /*<< ", " << tw4 */ << ", "  << tw5 << "\n";
}

}  // end namespace dijkstra_test