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

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

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <cstddef>
#include <string>

#include <boost/geometry/algorithms/is_convex.hpp>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/geometries/adapted/boost_variant.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include <boost/geometry/algorithms/is_valid.hpp>

template <typename Geometry>
void test_one(std::string const& case_id, std::string const& wkt, bool expected)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    bg::correct(geometry);

    bool detected = bg::is_convex(geometry);
    BOOST_CHECK_MESSAGE(detected == expected,
        "Not as expected, case: " << case_id
            << " / expected: " << expected
            << " / detected: " << detected);
}

template <typename P>
void test_all()
{
    // rectangular, with concavity
    std::string const concave1 = "polygon((1 1, 1 4, 3 4, 3 3, 4 3, 4 4, 5 4, 5 1, 1 1))";
    std::string const triangle = "polygon((1 1, 1 4, 5 1, 1 1))";
    std::string const rectangle_without_holes = "polygon((1 1, 1 4, 4 4, 4 1, 1 1))";
    std::string const rectangle_with_holes = "polygon((1 1, 1 4, 4 4, 4 1, 1 1),(2 2, 3 2, 3 3, 2 3, 2 2))";

    using box_t = bg::model::box<P>;
    using ring_t = bg::model::ring<P>;
    using polygon_t = bg::model::polygon<P>;
    using mpolygon_t = bg::model::multi_polygon<polygon_t>;
    using variant_t = boost::variant<polygon_t, mpolygon_t>;
    using collection_t = bg::model::geometry_collection<variant_t>;

    test_one<ring_t>("triangle", triangle, true);
    test_one<ring_t>("concave1", concave1, false);
    test_one<bg::model::ring<P, false, false> >("triangle", triangle, true);
    test_one<bg::model::ring<P, false, false> >("concave1", concave1, false);

    test_one<polygon_t>("triangle", triangle, true);
    test_one<polygon_t>("concave1", concave1, false);
    test_one<polygon_t>("rectangle_without_holes", rectangle_without_holes, true);
    test_one<polygon_t>("rectangle_with_holes", rectangle_with_holes, false);

    test_one<box_t>("box", "box(0 0,2 2)", true);

    test_one<mpolygon_t>("mpoly1", "multipolygon(((1 1, 1 4, 5 1, 1 1)))", true);
    test_one<mpolygon_t>("mpoly1", "multipolygon(((1 1, 1 4, 3 4, 3 3, 4 3, 4 4, 5 4, 5 1, 1 1)))", false);
    test_one<mpolygon_t>("mpoly2", "multipolygon(((1 1, 1 4, 5 1, 1 1)),((3 0, 3 1, 4 0, 3 0)))", false);

    test_one<variant_t>("variant1", triangle, true);
    test_one<variant_t>("variant2", concave1, false);

    std::string const pref = "geometrycollection(";
    std::string const post = ")";
    test_one<collection_t>("collection1", pref + triangle + post, true);
    test_one<collection_t>("collection2", pref + concave1 + post, false);
    test_one<collection_t>("collection3", pref + triangle + ", " + concave1 + post, false);
    test_one<collection_t>("collection4", pref + triangle + ", polygon((3 0, 3 1, 4 0, 3 0))" + post, false);
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/cast/size.hpp>
#include <fcppt/container/grid/apply.hpp>
#include <fcppt/container/grid/object.hpp>
#include <fcppt/math/at_c.hpp>
#include <fcppt/math/dim/comparison.hpp>
#include <fcppt/math/dim/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <string>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_apply
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::container::grid::object<
		std::string,
		2
	>
	string_grid;

	typedef
	fcppt::container::grid::object<
		unsigned,
		2
	>
	uint_grid;

	uint_grid const grid1(
		uint_grid::dim{
			2u,
			3u
		},
		[](
			uint_grid::pos const _pos
		)
		{
			return
				fcppt::cast::size<
					unsigned
				>(
					fcppt::math::at_c<
						0
					>(
						_pos
					)
					+
					fcppt::math::at_c<
						1
					>(
						_pos
					)
				);
		}
	);

	string_grid const grid2(
		string_grid::dim{
			2u,
			3u
		},
		[](
			string_grid::pos const _pos
		)
		{
			return
				std::to_string(
					fcppt::math::at_c<
						0
					>(
						_pos
					)
					+
					fcppt::math::at_c<
						1
					>(
						_pos
					)
				);
		}
	);

	auto const function(
		[](
			unsigned const _value1,
			std::string const &_value2
		)
		{
			return
				std::to_string(
					_value1
				)
				+
				_value2
				;
		}
	);

	string_grid const result(
		fcppt::container::grid::apply(
			function,
			grid1,
			grid2
		)
	);

	BOOST_REQUIRE_EQUAL(
		grid1.size(),
		result.size()
	);

	BOOST_CHECK_EQUAL(
		result[
			string_grid::pos(
				0u,
				0u
			)
		],
		std::string(
			"00"
		)
	);

	BOOST_CHECK_EQUAL(
		result[
			string_grid::pos(
				1u,
				2u
			)
		],
		std::string(
			"33"
		)
	);

	BOOST_REQUIRE_EQUAL(
		fcppt::container::grid::apply(
			function,
			grid1,
			string_grid()
		).size(),
		string_grid::dim(
			0u,
			0u
		)
	);
}


#include "Object.h"
#include "Ray.h"
#include "Camera.h"
#include "Sphere.h"
#include "Plane.h"
#include "read_json.h"
#include "write_ppm.h"
#include "viewing_ray.h"
#include "first_hit.h"
#include <Eigen/Core>
#include <vector>
#include <iostream>
#include <memory>
#include <limits>
#include <functional>

int main(int argc, char * argv[])
{
  // list of colors used for per-object id coloring
  const std::vector<unsigned char> color_map = {
    228,26,28,
    55,126,184,
    77,175,74,
    152,78,163,
    255,127,0,
    255,255,51,
    166,86,40,
    247,129,191,
    153,153,153
  };

  Camera camera;
  std::vector< std::shared_ptr<Object> > objects;
  // Read a camera and scene description from given .json file
  read_json(argc<=1?"../data/sphere-and-plane.json":argv[1],camera,objects);

  int width = 640;
  int height = 360;
  std::vector<unsigned char> id_image(3*width*height);
  std::vector<unsigned char> normal_image(3*width*height);
  std::vector<unsigned char> depth_image(1*width*height);
  // For each pixel (i,j)
  for(unsigned i=0; i<height; ++i)
  {
    for(unsigned j=0; j<width; ++j)
    {
      // Set background color
      normal_image[0+3*(j+width*i)] = 0;
      normal_image[1+3*(j+width*i)] = 0;
      normal_image[2+3*(j+width*i)] = 0;
      depth_image[j+width*i] = 0;
      id_image[0+3*(j+width*i)] = 0;
      id_image[1+3*(j+width*i)] = 0;
      id_image[2+3*(j+width*i)] = 0;

      // Compute viewing ray
      Ray ray;
      viewing_ray(camera,i,j,width,height,ray);

      // Find first visible object hit by ray and its surface normal n
      double t;
      Eigen::Vector3d n;
      int hit_id;
      if(first_hit(ray,1.0,objects,hit_id,t,n))
      {
        // object-id image
        const int color_id = hit_id%(color_map.size()/3);
        id_image[0+3*(j+width*i)] = color_map[0+3*color_id];
        id_image[1+3*(j+width*i)] = color_map[1+3*color_id];
        id_image[2+3*(j+width*i)] = color_map[2+3*color_id];

        // depth image
        const double zNear = camera.d;
        double linearized_depth = zNear/(t*ray.direction.norm());
        linearized_depth = linearized_depth<1?linearized_depth:1;
        depth_image[j+width*i] = 255.0*(linearized_depth);

        // set pixel color to value computed from hit point, light, and n
        // normal image
        auto normal_to_rgb = [](const Eigen::Vector3d & n, unsigned char & r, unsigned char & g, unsigned char & b)
        {
          r = 255.0*(n(0)*0.5+0.5);
          g = 255.0*(n(1)*0.5+0.5);
          b = 255.0*(n(2)*0.5+0.5);
        };
        normal_to_rgb(
          n,
          normal_image[0+3*(j+width*i)],
          normal_image[1+3*(j+width*i)],
          normal_image[2+3*(j+width*i)]);
      }
    }
  }

  write_ppm("normal.ppm",normal_image,width,height,3);
  write_ppm("depth.ppm",depth_image,width,height,1);
  write_ppm("id.ppm",id_image,width,height,3);
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef LIBS_ICL_TEST_TEST_ICL_interval_shared_hpp_JOFA_100306__
#define LIBS_ICL_TEST_TEST_ICL_interval_shared_hpp_JOFA_100306__

#include <boost/icl/interval_set.hpp>

template <class DomainT, ICL_COMPARE Compare,
          ICL_INTERVAL(ICL_COMPARE)  Interval>
void test_inner_complement(const ICL_INTERVAL_TYPE(Interval,DomainT,Compare)& itv1,
                           const ICL_INTERVAL_TYPE(Interval,DomainT,Compare)& itv2)
{
    typedef interval_set<DomainT,Compare,Interval> ItvSetT;
    typedef ICL_INTERVAL_TYPE(Interval,DomainT,Compare) IntervalT;

    BOOST_CHECK_EQUAL(icl::length(inner_complement(itv1,itv2)), icl::distance(itv1,itv2));
    BOOST_CHECK_EQUAL(icl::length(inner_complement(itv1,itv2)), icl::distance(itv2,itv1));
    BOOST_CHECK_EQUAL(icl::length(inner_complement(itv2,itv1)), icl::distance(itv1,itv2));
    BOOST_CHECK_EQUAL(icl::length(inner_complement(itv2,itv1)), icl::distance(itv2,itv1));

    IntervalT in_comp = inner_complement(itv1,itv2);
    ItvSetT itvset, inner_comp;
    itvset.add(itv1).add(itv2);
    ItvSetT hullset = ItvSetT(hull(itvset));
    inner_comp = hullset - itvset;
    IntervalT inner_comp_itv;
    if(inner_comp.begin() != inner_comp.end())
        inner_comp_itv = *inner_comp.begin();

    BOOST_CHECK_EQUAL(inner_complement(itv1,itv2), inner_comp_itv);
    BOOST_CHECK_EQUAL(inner_complement(itv2,itv1), inner_comp_itv);
    BOOST_CHECK_EQUAL(icl::length(inner_comp), icl::distance(itv1,itv2));
    BOOST_CHECK_EQUAL(icl::length(inner_comp), icl::distance(itv2,itv1));

    BOOST_CHECK(icl::disjoint(itv1, in_comp));
    BOOST_CHECK(icl::disjoint(itv2, in_comp));

    IntervalT itv1_comp = hull(itv1, in_comp);
    IntervalT itv2_comp = hull(itv2, in_comp);

    if(!icl::is_empty(in_comp))
    {
        BOOST_CHECK(icl::intersects(itv1_comp, in_comp));
        BOOST_CHECK(icl::intersects(itv2_comp, in_comp));

        BOOST_CHECK_EQUAL(itv1_comp & itv2_comp, in_comp);
        BOOST_CHECK_EQUAL( icl::is_empty(itv1_comp & itv2_comp), icl::disjoint(itv1_comp, itv2_comp));
        BOOST_CHECK_EQUAL(!icl::is_empty(itv1_comp & itv2_comp), icl::intersects(itv1_comp, itv2_comp));
    }
}

template <class IntervalT>
void test_inner_complement_(const IntervalT& itv1, const IntervalT& itv2)
{
    typedef typename interval_traits<IntervalT>::domain_type DomainT;
    // For the test of plain interval types we assume that std::less is
    // the compare functor
    test_inner_complement<DomainT, std::less, IntervalT>(itv1, itv2);
}

#ifndef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS

void interval_ctor_specific()
{
    BOOST_CHECK_EQUAL(icl::length(icl::interval<double>::type()), 0.0);
    BOOST_CHECK_EQUAL(icl::cardinality(icl::interval<double>::closed(5.0, 5.0)), 1);
    BOOST_CHECK_EQUAL(icl::cardinality(icl::interval<std::string>::closed("test", "test")), 1);
    BOOST_CHECK_EQUAL(icl::cardinality(icl::interval<std::string>::closed("best","test")),
                      icl::cardinality(icl::interval<double>::closed(0.0,0.1)));
    BOOST_CHECK_EQUAL(icl::cardinality(icl::interval<std::string>::right_open("best","test")),
                      icl::infinity<size_type_of<icl::interval<std::string>::type>::type >::value() );
    BOOST_CHECK_EQUAL(icl::cardinality(icl::interval<double>::right_open(0.0, 1.0)),
                      icl::infinity<size_type_of<icl::interval<double>::type>::type >::value() );
}

#endif // ndef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS

template <class T>
void interval_equal_4_integral_types()
{
    typedef typename icl::interval<T>::type IntervalT;
    T v2 = make<T>(2);
    T v3 = make<T>(3);
    T v7 = make<T>(7);
    T v8 = make<T>(8);
    BOOST_CHECK_EQUAL(IntervalT(), IntervalT(v7,v3));

    //I: (I)nside  = closed bound
    //C: left open bound
    //D: right open bound
    IntervalT  I3_7I  = icl::interval<T>::closed(v3,v7);
    IntervalT  I3__8D = icl::interval<T>::right_open(v3,v8);
    IntervalT C2__7I  = icl::interval<T>::left_open(v2,v7);
    IntervalT C2___8D = icl::interval<T>::open(v2,v8);

    BOOST_CHECK_EQUAL(  I3_7I ,  I3_7I  );
    BOOST_CHECK_EQUAL(  I3_7I ,  I3__8D );
    BOOST_CHECK_EQUAL(  I3_7I , C2__7I  );
    BOOST_CHECK_EQUAL(  I3_7I , C2___8D );

    BOOST_CHECK_EQUAL(  I3__8D,  I3__8D );
    BOOST_CHECK_EQUAL(  I3__8D, C2__7I  );
    BOOST_CHECK_EQUAL(  I3__8D, C2___8D );

    BOOST_CHECK_EQUAL( C2__7I , C2__7I  );
    BOOST_CHECK_EQUAL( C2__7I , C2___8D );

    BOOST_CHECK_EQUAL( C2___8D, C2___8D );
}

template <class T>
void interval_less_4_integral_types()
{
    typedef typename icl::interval<T>::type IntervalT;
    T v2 = make<T>(2);
    T v3 = make<T>(3);
    T v4 = make<T>(4);
    T v7 = make<T>(7);
    T v8 = make<T>(8);
    BOOST_CHECK_EQUAL(IntervalT() < IntervalT(v7,v3), false);
    BOOST_CHECK_EQUAL(icl::interval<T>::open(v2,v3) < icl::interval<T>::right_open(v7,v7), false);
    BOOST_CHECK_EQUAL(icl::interval<T>::left_open(v3,v3) < icl::interval<T>::closed(v7,v3), false);

    BOOST_CHECK_EQUAL(IntervalT() < IntervalT(v3,v4), true);
    BOOST_CHECK_EQUAL(icl::interval<T>::open(v2,v3) < icl::interval<T>::right_open(v7,v8), true);

    //I: (I)nside  = closed bound
    //C: left open bound
    //D: right open bound
    IntervalT  I3_7I  = icl::interval<T>::closed(v3,v7);
    IntervalT  I4_7I  = icl::interval<T>::closed(v4,v7);

    IntervalT  I3__8D = icl::interval<T>::right_open(v3,v8);
    IntervalT C2__7I  = icl::interval<T>::left_open(v2,v7);
    IntervalT C2___8D = icl::interval<T>::open(v2,v8);

    BOOST_CHECK_EQUAL(  I3_7I <  I3_7I  , false);
    BOOST_CHECK_EQUAL(  I3_7I <  I3__8D , false);
    BOOST_CHECK_EQUAL(  I3_7I < C2__7I  , false);
    BOOST_CHECK_EQUAL(  I3_7I < C2___8D , false);

    BOOST_CHECK_EQUAL(  I3_7I <  I4_7I  , true);

    BOOST_CHECK_EQUAL(  I3__8D<  I3__8D , false);
    BOOST_CHECK_EQUAL(  I3__8D< C2__7I  , false);
    BOOST_CHECK_EQUAL(  I3__8D< C2___8D , false);

    BOOST_CHECK_EQUAL( C2__7I < C2__7I  , false);
    BOOST_CHECK_EQUAL( C2__7I < C2___8D , false);

    BOOST_CHECK_EQUAL( C2___8D< C2___8D , false);
}

template <class T>
void interval_equal_4_bicremental_continuous_types()
{
    typedef typename icl::interval<T>::type IntervalT;
    T v3 = make<T>(3);
    T v7 = make<T>(7);
    BOOST_CHECK_EQUAL(IntervalT(), IntervalT(v7,v3));

    //I: (I)nside  = closed bound
    //O: (O)utside = open bound
    IntervalT I3_7I = icl::interval<T>::closed(v3,v7);
    IntervalT I3_7D = icl::interval<T>::right_open(v3,v7);
    IntervalT C3_7I = icl::interval<T>::left_open(v3,v7);
    IntervalT C3_7D = icl::interval<T>::open(v3,v7);

    BOOST_CHECK_EQUAL( I3_7I ,  I3_7I  );
    BOOST_CHECK_EQUAL( I3_7I == I3_7D, false  );
    BOOST_CHECK_EQUAL( I3_7I == C3_7D, false  );
    BOOST_CHECK_EQUAL( I3_7I == C3_7D, false );
    BOOST_CHECK_EQUAL( I3_7I != I3_7D, true  );
    BOOST_CHECK_EQUAL( I3_7I != C3_7D, true  );
    BOOST_CHECK_EQUAL( I3_7I != C3_7D, true );

    BOOST_CHECK_EQUAL( I3_7D ,  I3_7D  );
    BOOST_CHECK_EQUAL( I3_7D == C3_7I, false  );
    BOOST_CHECK_EQUAL( I3_7D == C3_7D, false );
    BOOST_CHECK_EQUAL( I3_7D != C3_7I, true  );
    BOOST_CHECK_EQUAL( I3_7D != C3_7D, true );

    BOOST_CHECK_EQUAL( C3_7I ,  C3_7I  );
    BOOST_CHECK_EQUAL( C3_7I == C3_7D, false );
    BOOST_CHECK_EQUAL( C3_7I != C3_7D, true );

    BOOST_CHECK_EQUAL( C3_7D,   C3_7D  );
}

template <class T>
void interval_touches_4_bicremental_types()
{
    typedef typename icl::interval<T>::type IntervalT;
    T v3 = make<T>(3);
    T v7 = make<T>(7);
    T v9 = make<T>(9);

    IntervalT I3_7D = icl::interval<T>::right_open(v3,v7);
    IntervalT I7_9I = icl::interval<T>::closed(v7,v9);
    BOOST_CHECK_EQUAL( icl::touches(I3_7D, I7_9I), true );

    IntervalT I3_7I = icl::interval<T>::closed(v3,v7);
    IntervalT C7_9I = icl::interval<T>::left_open(v7,v9);
    BOOST_CHECK_EQUAL( icl::touches(I3_7I, C7_9I), true );

    BOOST_CHECK_EQUAL( icl::touches(I3_7D, C7_9I), false );
    BOOST_CHECK_EQUAL( icl::touches(I3_7I, I7_9I), false );
}

template <class T>
void interval_touches_4_integral_types()
{
    typedef typename icl::interval<T>::type IntervalT;
    T v3 = make<T>(3);
    T v6 = make<T>(6);
    T v7 = make<T>(7);
    T v9 = make<T>(9);

    IntervalT I3_6I = icl::interval<T>::closed(v3,v6);
    IntervalT I7_9I = icl::interval<T>::closed(v7,v9);
    BOOST_CHECK_EQUAL( icl::touches(I3_6I, I7_9I), true );

    IntervalT I3_7D = icl::interval<T>::right_open(v3,v7);
    IntervalT C6_9I = icl::interval<T>::left_open(v6,v9);
    BOOST_CHECK_EQUAL( icl::touches(I3_7D, C6_9I), true );
}

template <class T>
void interval_infix_intersect_4_bicremental_types()
{
    typedef typename icl::interval<T>::type IntervalT;

    IntervalT section;
    IntervalT I3_7D = I_D(3,7);

    IntervalT I0_3D = I_D(0,3);
    section = I3_7D & I0_3D;
    BOOST_CHECK_EQUAL( icl::disjoint(I0_3D, I3_7D), true );
    BOOST_CHECK_EQUAL( icl::is_empty(section), true );
    BOOST_CHECK_EQUAL( section, IntervalT() );

    IntervalT I0_5D = I_D(0,5);
    section = I3_7D & I0_5D;
    BOOST_CHECK_EQUAL( section, I_D(3,5) );

    IntervalT I0_9D = I_D(0,9);
    section = I3_7D & I0_9D;
    BOOST_CHECK_EQUAL( section, I3_7D );

    IntervalT I4_5I = I_I(4,5);
    section = I3_7D & I4_5I;
    BOOST_CHECK_EQUAL( section, I4_5I );

    IntervalT C4_6D = C_D(4,6);
    section = I3_7D & C4_6D;
    BOOST_CHECK_EQUAL( section, C4_6D );

    IntervalT C4_9I = C_I(4,9);
    section = I3_7D & C4_9I;
    BOOST_CHECK_EQUAL( section, C_D(4,7) );

    IntervalT I7_9I = I_I(7,9);
    section = I3_7D & I7_9I;
    BOOST_CHECK_EQUAL( icl::exclusive_less(I3_7D, I7_9I), true );
    BOOST_CHECK_EQUAL( icl::disjoint(I3_7D, I7_9I), true );
    BOOST_CHECK_EQUAL( icl::is_empty(section), true );
}

template <class T>
void interval_subtract_4_bicremental_types()
{
    typedef typename icl::interval<T>::type IntervalT;

    IntervalT diff_1, diff_2;
    IntervalT I0_3D = I_D(0,3);
    IntervalT I2_6D = I_D(2,6);
    IntervalT I4_7D = I_D(4,7);
    IntervalT I6_7D = I_D(6,7);
    IntervalT I2_4D = I_D(2,4);

    diff_1 = right_subtract(I2_6D, I4_7D);
    BOOST_CHECK_EQUAL( diff_1, I2_4D );

    diff_1 = right_subtract(I0_3D, I4_7D);
    BOOST_CHECK_EQUAL( diff_1, I0_3D );

    // ---------------------------------
    diff_1 = left_subtract(I4_7D, I2_6D);
    BOOST_CHECK_EQUAL( diff_1, I6_7D );

    diff_1 = left_subtract(I4_7D, I0_3D);
    BOOST_CHECK_EQUAL( diff_1, I4_7D );
}

#endif // LIBS_ICL_TEST_TEST_ICL_interval_shared_hpp_JOFA_100306__


//
//  cnn.cpp
//  Jacobian
//
//  Created by David Freifeld
//

#include "bpnn.hpp"
#include "utils.hpp"

//#include <Eigen/unsupported/CXX11/Tensor>
#include "cnn.hpp"

#define LARGE_NUM 1000000 // Remove me.

#if (!RECKLESS)
#define checknan(x, loc) if(x==INFINITY || x==NAN || x == -INFINITY) throw ValueError("Detected NaN in operation", loc)
#else
#define checknan(x, loc)
#endif

// NOTE: Below three functions not mine, from https://compvisionlab.wordpress.com/2014/01/01/c-code-for-reading-mnist-data-set/
int ReverseInt (int i)
{
    unsigned char ch1, ch2, ch3, ch4;
    ch1=i&255;
    ch2=(i>>8)&255;
    ch3=(i>>16)&255;
    ch4=(i>>24)&255;
    return((int)ch1<<24)+((int)ch2<<16)+((int)ch3<<8)+ch4;
}

void ReadMNIST(int NumberOfImages, int DataOfAnImage,std::vector<std::vector<double>> &arr)
{
    arr.resize(NumberOfImages,std::vector<double>(DataOfAnImage));
    std::ifstream file ("./t10k-images-idx3-ubyte",std::ios::binary);
    if (file.is_open())
    {
        int magic_number=0;
        int number_of_images=0;
        int n_rows=0;
        int n_cols=0;
        file.read((char*)&magic_number,sizeof(magic_number));
        magic_number= ReverseInt(magic_number);
        file.read((char*)&number_of_images,sizeof(number_of_images));
        number_of_images= ReverseInt(number_of_images);
        file.read((char*)&n_rows,sizeof(n_rows));
        n_rows= ReverseInt(n_rows);
        file.read((char*)&n_cols,sizeof(n_cols));
        n_cols= ReverseInt(n_cols);
        for(int i=0;i<number_of_images;++i)
        {
            for(int r=0;r<n_rows;++r)
            {
                for(int c=0;c<n_cols;++c)
                {
                    unsigned char temp=0;
                    file.read((char*)&temp,sizeof(temp));
                    arr[i][(n_rows*r)+c]= (double)temp;
                }
            }
        }
    }
}

unsigned char* read_mnist_labels(std::string full_path, int number_of_labels) {
    auto reverseInt = [](int i) {
        unsigned char c1, c2, c3, c4;
        c1 = i & 255, c2 = (i >> 8) & 255, c3 = (i >> 16) & 255, c4 = (i >> 24) & 255;
        return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
    };

    typedef unsigned char uchar;

    std::ifstream file(full_path, std::ios::binary);

    if(file.is_open()) {
        int magic_number = 0;
        file.read((char *)&magic_number, sizeof(magic_number));
        magic_number = reverseInt(magic_number);

        if(magic_number != 2049) throw std::runtime_error("Invalid MNIST label file!");

        file.read((char *)&number_of_labels, sizeof(number_of_labels)), number_of_labels = reverseInt(number_of_labels);

        uchar* _dataset = new uchar[number_of_labels];
        for(int i = 0; i < number_of_labels; i++) {
            file.read((char*)&_dataset[i], 1);
        }
        return _dataset;
    } else {
        throw std::runtime_error("Unable to open file `" + full_path + "`!");
    }
}

ConvLayer::ConvLayer(int x, int y, int stride, int kern_x, int kern_y, int pad, std::function<float(float)> activ, std::function<float(float)> activ_deriv)
    :stride_len(stride), padding(pad), activation(activ), activation_deriv(activ_deriv)
{
    pad*=2;
    input = new Eigen::MatrixXf (x+pad,y+pad);
    dZ = new Eigen::MatrixXf (x+pad,y+pad);
    for (int i = 0; i < (x+pad)*(y+pad); i++) {
        (*input)((int)i / (y+pad),i%(y+pad)) = 0;
        (*dZ)((int)i / (y+pad),i%(y+pad)) = 0;
    }
    kernel = new Eigen::MatrixXf (kern_x, kern_y);
    for (int i = 0; i < kern_x*kern_y; i++) {
        (*kernel)((int)i / kern_y,i%kern_y) = (float) rand() / RAND_MAX;
    }
    output = new Eigen::MatrixXf ((x-kern_x+1+pad/stride_len), (y-kern_y+1+pad/stride_len));
    for (int i = 0; i < (x-kern_y+1+pad/stride_len)*(y-kern_x+1+pad/stride_len); i++) {
        (*output)((int)i / (y-kern_y+1+pad/stride_len),i%(y-kern_y+1+pad/stride_len)) = 0;
    }
    bias = 0;
};

void ConvLayer::convolute()
{
    for (int i = 0; i < (*input).rows(); i++) {
        for (int j = 0; j < (*input).cols(); j++) {
            (*dZ)(i, j) = activation_deriv((*input)(i, j));
            (*input)(i, j) = activation((*input)(i, j));
        }
    }
    for (int i = 0; i < output->rows(); i+=stride_len) {
        for (int j = 0; j < output->cols(); j+=stride_len) {
            (*output)(i, j) = (*kernel * (input->block(i, j, kernel->rows(), kernel->cols()))).sum();
        }
    }
    *output = (output->array() + bias).matrix();
}

void ConvLayer::set_input(Eigen::MatrixXf* matrix)
{
    input->block(padding, padding, matrix->rows(), matrix->cols()) = *matrix;
}

// Will eventually be different from ConvLayer
PoolingLayer::PoolingLayer(int x, int y, int stride, int kern_x, int kern_y, int pad)
    :stride_len(stride), padding(pad)
{
    input = new Eigen::MatrixXf (x+pad,y+pad);
    for (int i = 0; i < (x+pad)*(y+pad); i++) {
        (*input)((int)i / (y+pad),i%(y+pad)) = 0;
    }
    kernel = new Eigen::MatrixXf (kern_x, kern_y);
    for (int i = 0; i < kern_x*kern_y; i++) {
        (*kernel)((int)i / kern_y,i%kern_y) = (float) rand()/RAND_MAX;
    }
    output = new Eigen::MatrixXf (x-kern_x+1, y-kern_y+1);
    for (int i = 0; i < (x-kern_x+1)*(y-kern_y+1); i++) {
        (*output)((int)i / (y-kern_y+1),i%(y-kern_y+1)) = (float) rand()/RAND_MAX;
    }
};

void PoolingLayer::pool()
{
  // It doesn't look like anything better than O(n^4) is doable for this as kernel needs to go through matrix and you need to index kernel. LOOK INTO ME!!
    float maxnum = -LARGE_NUM;
    for (int i = 0; i < input->cols() - kernel->cols(); i+=stride_len) {
        for (int j = 0; j < input->rows() - kernel->rows(); j+=stride_len) {
            for (int k = 0; k < kernel->cols(); k++) {
                for (int l = 0; l < kernel->rows(); l++) {
                    if ((input->block(j, i, kernel->rows(), kernel->cols()))(l, k) > maxnum) {
                        maxnum = (input->block(j, i, kernel->rows(), kernel->cols()))(l, k);
                    }
                }
            }
        }
    }
}

ConvNet::ConvNet(const char* path, float learn_rate, float bias_rate, Regularization reg, float l, float ratio)
    :Network(path, 1, learn_rate, bias_rate, reg, l, ratio), preprocess_length{0}
{
    ReadMNIST(10000,784,data);
    data_labels = read_mnist_labels("./t10k-labels-idx1-ubyte",10000);
    labels = new Eigen::MatrixXf (1, 1);
}

void ConvNet::add_conv_layer(int x, int y, int stride, int kern_x, int kern_y, int pad, std::function<float(float)> activ, std::function<float(float)> activ_deriv)
{
    preprocess_length+=1;
    conv_layers.emplace_back(x,y,stride,kern_x, kern_y,pad,activ,activ_deriv);
}

// May make this inaccessible to user code and just have it called from add_conv_layer as pooling is basically always paired with conv.
void ConvNet::add_pool_layer(int x, int y, int stride, int kern_x, int kern_y, int pad)
{
    pool_layers.emplace_back(x,y,stride,kern_x,kern_y,pad);
}

void ConvNet::initialize()
{
    for (int i = 0; i < length-1; i++) {
        layers[i].init_weights(layers[i+1]);
    }
}

void ConvNet::next_batch()
{
    for (int i = 0; i < 784; i++) {
        (*conv_layers[0].input)(i/28, i%28) = data[batches][i];
    }
    (*labels)(0,0) = (float)(int)data_labels[batches];
}

void ConvNet::process()
{
    // Assumes pooling is immediately after any conv layer.
    for (int i = 0; i < preprocess_length-1; i++) {
        conv_layers[i].convolute();
        //   pool_layers[i].input = conv_layers[i].output;
        //   pool_layers[i].pool();
        conv_layers[i+1].input = conv_layers[i].output;
    }
    conv_layers[preprocess_length-1].convolute();
    //pool_layers[preprocess_length-1].input = conv_layers[preprocess_length-1].output;
    //pool_layers[preprocess_length-1].pool();
    //  std::cout << "Output:\n" << *pool_layers[preprocess_length-1].output << "\n\n";
    Eigen::Map<Eigen::RowVectorXf> flattened (conv_layers[preprocess_length-1].output->data(), conv_layers[preprocess_length-1].output->size());
    // std::cout << "Flattened:\n" << flattened << "\n\n";
    for (int i = 0; i < flattened.cols(); i++) {
        (*layers[0].contents)(0, i) = flattened[i];
    }
}

void ConvNet::set_label(Eigen::MatrixXf newlabels)
{
    *labels = newlabels;
}

void ConvNet::list_net()
{
    for (int i = 0; i < preprocess_length; i++) {
        std::cout << "-----------------------\nCONVOLUTIONAL LAYER " << i << "\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nStride: " << conv_layers[i].stride_len << "\nPadding: " << conv_layers[i].padding <<  "\n\n\u001b[31mINPUT:\x1B[0;37m\n" << *conv_layers[i].input << "\n\n\u001b[31mKERNEL:\x1B[0;37m\n" << *conv_layers[i].kernel << "\n\n\u001b[31mOUTPUT:\x1B[0;37m\n" << *conv_layers[i].output << "\n\n\u001b[31mBIAS:\x1B[0;37m\n" << conv_layers[i].bias << "\n\n\n";
        //std::cout << "-----------------------\nPOOLING LAYER " << i << "\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nStride: " << pool_layers[i].stride_len << "\nPadding: " << conv_layers[i].padding << "\n\n\u001b[31mINPUT:\x1B[0;37m\n" << *pool_layers[i].input << "\n\n\u001b[31mKERNEL:\x1B[0;37m\n-" << *pool_layers[i].kernel << "\n\n\u001b[31mOUTPUT:\x1B[0;37m\n" << *pool_layers[i].output << "\n\n\n";
    }
    std::cout << "-----------------------\nINPUT LAYER (LAYER 0)\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nActivation Function: " << layers[0].activation_str << "\n\n\u001b[31mACTIVATIONS:\x1B[0;37m\n" << *layers[0].contents << "\n\n\u001b[31mWEIGHTS:\x1B[0;37m\n" << *layers[0].weights << "\n\n\u001b[31mBIASES:\x1B[0;37m\n" << *layers[0].bias << "\n\n\n";
    for (int i = 1; i < length-1; i++) {
        std::cout << "-----------------------\nLAYER " << i << "\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nActivation Function: " << layers[i].activation_str << "\n\n\u001b[31mACTIVATIONS:\x1B[0;37m\n" << *layers[i].contents << "\n\n\u001b[31mBIASES:\x1B[0;37m\n" << *layers[i].bias << "\n\n\u001b[31mWEIGHTS:\x1B[0;37m\n" << *layers[i].weights << "\n\n\n";
    }
    std::cout << "-----------------------\nOUTPUT LAYER (LAYER " << length-1 << ")\n-----------------------\n\n\u001b[31mGENERAL INFO:\x1B[0;37m\nActivation Function: " << layers[length-1].activation_str <<"\n\n\u001b[31mACTIVATIONS:\x1B[0;37m\n" << *layers[length-1].contents << "\n\n\u001b[31mBIASES:\x1B[0;37m\n" << *layers[length-1].bias <<  "\n\n\n";
}

void ConvNet::backpropagate()
{
    list_net();
    char a;
    std::cin >> a;
    std::vector<Eigen::MatrixXf> gradients;
    gradients.push_back(Network::backpropagate());
    Eigen::Map<Eigen::MatrixXf> reshaped(gradients[gradients.size()-1].data(),
                           conv_layers.back().output->rows(),
                           conv_layers.back().output->cols());
    gradients[gradients.size()-1] = reshaped;
    std::vector<Eigen::MatrixXf> conv_deltas;
    for (int layer = conv_layers.size()-1; layer >= 0; layer--) {
        conv_deltas.emplace_back(conv_layers[layer].kernel->rows(),
                                 conv_layers[layer].kernel->cols());
        std::cout << conv_layers[layer].input->cols() << " " << gradients.back().cols() << "\n";
        for (int i = 0; i < conv_layers[layer].input->rows() - gradients.back().rows() + 1; i++) {
            for (int j = 0; j < conv_layers[layer].input->cols() - gradients.back().cols() + 1; j++) {
                conv_deltas[conv_deltas.size()-1](i, j) = (gradients.back() * conv_layers[layer].input->block(i, j, gradients.back().rows(), gradients.back().cols())).sum();
            }
        }
        *conv_layers[layer].kernel -= conv_deltas.back();

        Eigen::MatrixXf flipped_kernel =
            Eigen::MatrixXf::Zero(conv_layers[layer].kernel->rows(), conv_layers[layer].kernel->cols());
        flipped_kernel = conv_layers[layer].kernel->transpose().colwise().reverse().transpose().colwise().reverse();
        Eigen::MatrixXf padded_grad = Eigen::MatrixXf::Zero(gradients.back().rows() + ((flipped_kernel.rows() - 1)*2), gradients.back().cols() + ((flipped_kernel.cols() - 1)*2));
        padded_grad.block(flipped_kernel.rows() - 1, flipped_kernel.cols() - 1, gradients.back().rows(), gradients.back().cols()) = gradients.back();
        Eigen::MatrixXf final_grad (padded_grad.rows() - flipped_kernel.rows() + 1, padded_grad.cols() - flipped_kernel.cols() + 1);
        for (int i = 0; i < padded_grad.rows() - flipped_kernel.rows() + 1; i++) {
            for (int j = 0; j < padded_grad.cols() - gradients.back().cols() + 1; j++) {
                final_grad(i, j) = (flipped_kernel * padded_grad.block(i, j, flipped_kernel.rows(), flipped_kernel.cols())).sum();
            }
        }
        gradients.push_back(final_grad.cwiseProduct(*conv_layers[layer].dZ));
    }
    list_net();
    assert(2<1);
}

void ConvNet::train()
{
    float cost_sum = 0;
    float acc_sum = 0;
    for (int i = 0; i <= 100; i++) {
        if (i != instances-batch_size) { // Don't try to advance batch on final batch.
            next_batch();
        }
        process();
        feedforward();
        backpropagate();
        cost_sum += cost();
        acc_sum += accuracy();
        batches++;
    }
    list_net();
    epoch_acc = 1.0/(100) * acc_sum;
    epoch_cost = 1.0/(100) * cost_sum;
    printf("Epoch %i complete - cost %f - acc %f\n", epochs, epoch_cost, epoch_acc);
    batches=0;
    decay();
    epochs++;
}

int main()
{
    ConvNet net ("../data_banknote_authentication.txt", 0.05, 0.01, L2, 0, 0.9);
    Eigen::MatrixXf labels (1,1);
    net.add_conv_layer(28, 28, 1, 9, 9, 0, lecun_tanh, lecun_tanh_deriv);
    //  net.add_pool_layer(20,20,1,6,6,0);
    net.add_conv_layer(20, 20, 1, 6, 6, 0, lecun_tanh, lecun_tanh_deriv);
    std::cout << net.conv_layers[net.conv_layers.size()-1].output->rows() << "\n";
    //net.add_pool_layer(10,10,1,2,2,0);
    net.add_layer(400, "sigmoid", sigmoid, sigmoid_deriv);
    net.add_layer(5, "lecun_tanh", lecun_tanh, lecun_tanh_deriv);
    net.add_layer(10, "resig", rectifier(sigmoid), rectifier(sigmoid_deriv));
    //  net.list_net();
    //  net.init_decay("step", 1, 2);
    net.initialize();
    //net.list_net();

    for (int i = 0; i < 1; i++) {
        net.train();
    }
    net.list_net();
}


// Copyright (c) 2019 The Bitcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <test/test_bitcoin.h>
#include <test/test_random.h>
#include <bitmanip.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(bitmanip_tests, BasicTestingSetup)

static void CheckBitCount(uint32_t value, uint32_t expected_count) {
    // Count are rotation invariant.
    for (int i = 0; i < 32; i++) {
        BOOST_CHECK_EQUAL(countBits(value), expected_count);
        value = (value << 1) | (value >> 31);
    }
}

static uint32_t countBitsNaive(uint32_t value) {
    uint32_t ret = 0;
    while (value != 0) {
        ret += (value & 0x01);
        value >>= 1;
    }

    return ret;
}

const size_t COUNT=4096;

BOOST_AUTO_TEST_CASE(bit_count) {
    // Check various known values.
    CheckBitCount(0, 0);
    CheckBitCount(1, 1);
    CheckBitCount(0xffffffff, 32);
    CheckBitCount(0x01234567, 12);
    CheckBitCount(0x12345678, 13);
    CheckBitCount(0xfedcba98, 20);
    CheckBitCount(0x5a55aaa5, 16);
    CheckBitCount(0xdeadbeef, 24);

    for (uint32_t i = 2; i != 0; i <<= 1) {
        // Check two bit set for all combinations.
        CheckBitCount(i | 0x01, 2);
    }

    // Check many small values against a naive implementation.
    for (uint32_t v = 0; v <= 0xfff; v++) {
        CheckBitCount(v, countBitsNaive(v));
    }

    // Check random values against a naive implementation.
    for (size_t i = 0; i < COUNT; i++) {
        uint32_t v = insecure_rand();
        CheckBitCount(v, countBitsNaive(v));
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*=============================================================================
    Copyright (c) 2002-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A primitive calculator that knows how to add and subtract.
//  [ demonstrating phoenix ]
//
//  [ JDG 6/28/2002 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/core.hpp>
#include <boost/spirit/phoenix/primitives.hpp>
#include <boost/spirit/phoenix/operators.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;
using namespace phoenix;

///////////////////////////////////////////////////////////////////////////////
//
//  Our primitive calculator
//
///////////////////////////////////////////////////////////////////////////////
template <typename IteratorT>
bool primitive_calc(IteratorT first, IteratorT last, double& n)
{
    return parse(first, last,

        //  Begin grammar
        (
            real_p[var(n) = arg1]
            >> *(   ('+' >> real_p[var(n) += arg1])
                |   ('-' >> real_p[var(n) -= arg1])
                )
        )
        ,
        //  End grammar

        space_p).full;
}

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA primitive calculator...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";

    cout << "Give me a list of numbers to be added or subtracted.\n";
    cout << "Example: 1 + 10 + 3 - 4 + 9\n";
    cout << "The result is computed using Phoenix.\n";
    cout << "Type [q or Q] to quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        double n;
        if (primitive_calc(str.begin(), str.end(), n))
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << str << " Parses OK: " << endl;

            cout << "result = " << n;
            cout << "\n-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


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

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

#include <algorithms/test_length.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/std_pair_as_segment.hpp>

#include <test_geometries/all_custom_linestring.hpp>
#include <test_geometries/wrapped_boost_array.hpp>

template <typename P>
void test_all()
{
    // 3-4-5 triangle
    test_geometry<std::pair<P, P> >("LINESTRING(0 0,3 4)", 5);

    // 3-4-5 plus 1-1
    test_geometry<bg::model::linestring<P> >("LINESTRING(0 0,3 4,4 3)", 5 + sqrt(2.0));
    test_geometry<all_custom_linestring<P> >("LINESTRING(0 0,3 4,4 3)", 5 + sqrt(2.0));
    test_geometry<test::wrapped_boost_array<P, 3> >("LINESTRING(0 0,3 4,4 3)", 5 + sqrt(2.0));

    // Geometries with length zero
    test_geometry<P>("POINT(0 0)", 0);
    test_geometry<bg::model::polygon<P> >("POLYGON((0 0,0 1,1 1,1 0,0 0))", 0);
}

template <typename P>
void test_empty_input()
{
    test_empty_input(bg::model::linestring<P>());
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    // test_empty_input<bg::model::d2::point_xy<int> >();

    return 0;
}


#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <chrono>
#include "oful.h"
#include "contlinrep.h"
#include "utils.h"
#include "gzip.h"
#include "bandit.h"
#include "leader.h"
#include "adversarial_master.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

int PREC = 4;
int EVERY = 1;

int main()
{
    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    srand (time(NULL));
    // rng.seed(10000); // warm it up
    int n_runs = 20, T = 1000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 0.2;
    double bonus_scale = 1;
    bool adaptive_ci = true;

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    vec2double regrets(n_runs), pseudo_regrets(n_runs);

    ContToy1_phi1 lrep1 = ContToy1_phi1(noise_std, seeds[0]);
    ContToy1_phi2 lrep2 = ContToy1_phi2(noise_std, seeds[0]);

    std::cerr << "OFUL" << std::endl;
    for (int i = 0; i < n_runs; ++i)
    {
        OFUL<std::vector<double>> localg(lrep1, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(lrep1, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "$\\phi#1$_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "$\\phi#1$_pseudoregrets.csv.gz", EVERY, PREC);

    for (int i = 0; i < n_runs; ++i)
    {
        OFUL<std::vector<double>> localg(lrep2, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(lrep2, localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "$\\phi#2$_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "$\\phi#2$_pseudoregrets.csv.gz", EVERY, PREC);

    std::vector<std::shared_ptr<ContRepresentation<std::vector<double>>>> lreps;

    auto tmp = std::make_shared<ContToy1_phi1>(lrep1);
    lreps.push_back(tmp);
    tmp = std::make_shared<ContToy1_phi2>(lrep2);
    lreps.push_back(tmp);

    //LEADER
    std::cerr << "LEADER" << std::endl;
    for (int i = 0; i < n_runs; ++i)
    {
        LEADER<std::vector<double>> localg(lreps, reg_val, noise_std, bonus_scale, delta/lreps.size(), adaptive_ci);
        ContBanditProblem<std::vector<double>> prb(*lreps[0], localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
        // delete localg;
    }
    save_vector_csv_gzip(regrets, "\\algo_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "\\algo_pseudoregrets.csv.gz", EVERY, PREC);

    //EXP4.IX
    std::cerr << "EXP4" << std::endl;
    for (int i = 0; i < n_runs; ++i)
    {
        std::vector<std::shared_ptr<Algo<std::vector<double>>>> base_algs;
        for(auto& ll : lreps)
        {
            base_algs.push_back(
                std::make_shared<OFUL<std::vector<double>>>(
                    OFUL<std::vector<double>>(*tmp, reg_val,noise_std,bonus_scale,delta,adaptive_ci)
                )
            );
        }
        double exp4_gamma = sqrt(2*log(base_algs.size())/(4*T));
        double exp4_lr = 2*exp4_gamma;
        EXP4dotIX<std::vector<double>> localg(base_algs, exp4_lr, exp4_gamma, seeds[i]);
        ContBanditProblem<std::vector<double>> prb(*lreps[0], localg);
        prb.reset();
        prb.run(T);
        regrets[i] = prb.instant_regret;
        pseudo_regrets[i] = prb.exp_instant_regret;
    }
    save_vector_csv_gzip(regrets, "\\expfour_regrets.csv.gz", EVERY, PREC);
    save_vector_csv_gzip(pseudo_regrets, "\\expfour_pseudoregrets.csv.gz", EVERY, PREC);

    return 0;
}


#pragma once

#include <polyfem/Quadrature.hpp>
#include <polyfem/ElementAssemblyValues.hpp>

#include <Eigen/Dense>

namespace polyfem
{
	class RBFWithQuadratic
	{
	public:
		inline static int index_mapping(const int alpha, const int beta, const int d, const int ass_dim)
		{
			return ass_dim*ass_dim*d + ass_dim * beta + alpha;
		}

		static void setup_monomials_vals_2d(const int star_index, const Eigen::MatrixXd &pts, ElementAssemblyValues &vals);
		static void setup_monomials_strong_2d(const int dim, const std::string &assembler_name, const Eigen::MatrixXd &pts, const QuadratureVector &da, std::array<Eigen::MatrixXd, 5> &strong);

		///
		/// @brief      { Initialize RBF functions over a polytope element. }
		///
		/// @param[in]  centers               { #C x dim positions of the kernels used to define
		///                                   functions over the polytope. The centers are placed at
		///                                   a small offset distance from the boundary of the
		///                                   element, due to the singularity at the centers }
		/// @param[in]  collocation_points    { #S x dim positions of the collocation points, used
		///                                   to approximate the RBF functions over the boundary of
		///                                   the element }
		/// @param[in]  local_basis_integral  { #B x dim+dim*(dim+1)/2 of the constant right-hand
		///                                   side for the integral constraint for each basis over
		///                                   the polytope }
		/// @param[in]  quadr                 { Quadrature points and weights inside the polytope }
		/// @param[in]  rhs                   { #S x #B of boundary conditions. Each column defines
		///                                   how the i-th basis of the mesh should evaluate on the
		///                                   collocation points sampled on the boundary of the
		///                                   polytope }
		/// @param[in]  with_constraints      { Impose integral constraints to guarantee linear
		///                                   reproduction for the Poisson equation }
		///
		RBFWithQuadratic(const std::string &assembler_name, const Eigen::MatrixXd &centers, const Eigen::MatrixXd &collocation_points,
			const Eigen::MatrixXd &local_basis_integral, const Quadrature &quadr,
			Eigen::MatrixXd &rhs, bool with_constraints = true);

		///
		/// @brief      { Evaluates one RBF function over a list of coordinates }
		///
		/// @param[in]  local_index  { i-th RBF function to evaluate }
		/// @param[in]  uv           { #uv x dim matrix of coordinates to evaluate (in object
		///                          domain) }
		/// @param[out] val          { #uv x 1 matrix of computed values }
		///
		void basis(const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) const;

		///
		/// @brief      { Evaluates the gradient of one RBF function over a list of coordinates }
		///
		/// @param[in]  local_index  { i-th RBF function to evaluate }
		/// @param[in]  uv           { #uv x dim matrix of coordinates to evaluate (in object
		///                          domain) }
		/// @param[out] val          { #uv x dim matrix of computed gradients }
		///
		void grad(const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) const;

		///
		/// @brief      { Batch evaluates the RBF + polynomials on a set of sample points }
		///
		/// @param[in]  uv    { #uv x dim matrix of points to evaluate }
		/// @param[out] val   { #uv x n_loc_bases of bases values over the sample points }
		///
		void bases_values(const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const;

		///
		/// @brief      { Batch evaluates the gradient of the RBF + polynomials on a set of sample
		///             points }
		///
		/// @param[in]  axis  { The axis (0, 1, 2) with respect to which to compute the gradient }
		/// @param[in]  uv    { #uv x dim matrix of points to evaluate }
		/// @param[out] val   { #uv x n_loc_bases of bases gradient wrt axis over the sample points }
		///
		void bases_grads(const int axis, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const;

	private:
		bool is_volume() const { return centers_.cols() == 3; }

		// Computes the matrix that evaluates the kernels + polynomial terms on the given sample points
		void compute_kernels_matrix(const Eigen::MatrixXd &samples, Eigen::MatrixXd &A) const;

		// Computes the relationship w = L v + t between the unknowns (v) and the weights w
		void compute_constraints_matrix_2d_old(const int num_bases, const Quadrature &quadr,
			const Eigen::MatrixXd &local_basis_integral, Eigen::MatrixXd &L, Eigen::MatrixXd &t) const;

		// Computes the relationship w = L v + t between the unknowns (v) and the weights w
		void compute_constraints_matrix_2d(const std::string &assembler_name, const int num_bases, const Quadrature &quadr,
			const Eigen::MatrixXd &local_basis_integral, Eigen::MatrixXd &L, Eigen::MatrixXd &t) const;

		// Computes the relationship w = L v + t between the unknowns (v) and the weights w
		void compute_constraints_matrix_3d(const std::string &assembler_name, const int num_bases, const Quadrature &quadr,
			const Eigen::MatrixXd &local_basis_integral, Eigen::MatrixXd &L, Eigen::MatrixXd &t) const;

		// Computes the weights by solving a (possibly constrained) linear least square
		void compute_weights(const std::string &assembler_name, const Eigen::MatrixXd &collocation_points,
			const Eigen::MatrixXd &local_basis_integral, const Quadrature &quadr,
			Eigen::MatrixXd &rhs, bool with_constraints);

	private:
		// #C x dim matrix of kernel center positions
		Eigen::MatrixXd centers_;

		// (#C + dim + 1) x #B matrix of weights extending the #B bases that are non-vanishing on the polytope
		Eigen::MatrixXd weights_;
	};
}


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#include <scitbx/math/zernike.h>
#include <scitbx/math/zernike_moments.h>

namespace scitbx { namespace math {
namespace {

//
//
  struct grid_wrapper
  {
    typedef grid  < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("sphere_grid", no_init)
        .def( init<
                   int const&,
                   int const&
                  >
             ((
                arg("np"),
                arg("n_max")
             ))
            )
        .def("get_ss", &w_t::get_ss)
        .def("get_ss", &w_t::get_all_ss)
        .def("clean_space", &w_t::clean_space)
        .def("construct_space_sum",&w_t::construct_space_sum)
        .def("construct_space_sum_via_list",&w_t::construct_space_sum_via_list)
        .def("construct_space_sum_via_list",&w_t::construct_space_sum_via_list_only)
        .def("unit_sphere_index", &w_t::unit_sphere_index)
        .def("occupied_sites", &w_t::occupied_sites)
      ;
    }
  };

//
//

  struct moments_wrapper
  {
    typedef moments  < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("zernike_moments", no_init)
        .def( init<
                   grid<double>,
                   int const&
                  >
             ((
                arg("grid"),
                arg("nmax")
             ))
            )
        .def("moments", &w_t::all_moments)
        .def("get_moment",&w_t::get_moment)
        .def("calc_moments",&w_t::calc_moments)
        .def("update_ss",&w_t::update_ss)
        .def("fnn",&w_t::fnn)
        .def("fnl",&w_t::fnl)
        .def("fnnl",&w_t::fnnl)
      ;
    }
  };

//
//
  struct voxel_wrapper
  {
    typedef voxel < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("sphere_voxel", no_init)
        .def( init<
                   int const&,
                   int const&,
                   bool const&,
                   bool const&,
                   double const&,
                   double const&,
                   double const&,
                   scitbx::af::const_ref< scitbx::vec3<double> >,
                   scitbx::af::const_ref< double >
                  >
             ((
                arg("np"),
                arg("splat_range"),
                arg("uniform"),
                arg("fixed_dx"),
                arg("external_rmax"),
                arg("fraction"),
                arg("dx"),
                arg("xyz"),
                arg("density")
             ))
            )
        .def("value", &w_t::get_value)
        .def("rmax", &w_t::rmax)
        .def("rg", &w_t::rg)
        .def("map", &w_t::map)
        .def("xyz", &w_t::xyz)
        .def("rotate", &w_t::rotate)
        .def("np",&w_t::np)
        .def("weight_sum",&w_t::weight_sum)
        .def("occupied_sites", &w_t::occupied_sites)
        .def("status", &w_t::print_status)
        .def("border", &w_t::border)
      ;
    }
  };

} //namespace <anonymous>

namespace boost_python {

  void wrap_zernike_mom()
  {
    voxel_wrapper::wrap();
    moments_wrapper::wrap();
    grid_wrapper::wrap();
  }

}}}


#pragma once
#include <Eigen/Core>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>

namespace gtsam
{
  class LidarPlaneFactor2 : public NoiseModelFactor2<Pose3, Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor2<Pose3, Pose3>;
    using This = LidarPlaneFactor2;

  public:
    LidarPlaneFactor2(Key key1, Key key2, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key1, key2), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarPlaneFactor2() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1).dot(u)
              = (T2*p2 - p1).dot(u)
    dplane_err/dT2 = u'*d(T2*p2)/dT2 = u'*R2*[[-p2]x I3x3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1w).dot(uw)
              = (T2*p2 - T1*p1).dot(R1*u)
    dplane_err/dT1 = (T2*p2 - T1*p1)'* d(R1*u)/dT1 - (R1*u)'*d(T1*p1)/dT1
    dplane_err/dT2 = (R1*u)'*d(T2*p2)/dT2
    */

    Vector evaluateError(const X &pose1, const X &pose2,
                         boost::optional<Matrix&> H1 = boost::none,
                         boost::optional<Matrix&> H2 = boost::none) const
    {
      const auto &rotation1 = pose1.rotation().matrix();
      const auto &rotation2 = pose2.rotation().matrix();
      const auto p12w = pose2.transformFrom(p2_) - pose1.transformFrom(p1_);
      const auto uw = rotation1 * u_;
      if (H1)
        *H1 = p12w.transpose() * rotation1 * (Matrix36() << skewSymmetric(-u_[0], -u_[1], -u_[2]), Z_3x3).finished()
            - uw.transpose() * rotation1 * (Matrix36() << skewSymmetric(-p1_[0], -p1_[1], -p1_[2]), I_3x3).finished();
      if (H2)
        *H2 = uw.transpose() * rotation2 * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return Vector1(p12w.dot(uw));
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarPlaneFactor2 on (" << keyFormatter(key1())
           << ", " << keyFormatter(key2()) << ")\n"
           << "  Plane Point: " << p1_.transpose() << "\n"
           << "  Plane norm Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;
  }; // class LidarPlaneFactor2

  class LidarPlaneFactor1 : public NoiseModelFactor1<Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor1<Pose3>;
    using This = LidarPlaneFactor1;

  public:
    LidarPlaneFactor1(Key key, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarPlaneFactor1() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1).dot(u)
              = (T2*p2 - p1).dot(u)
    dplane_err/dT2 = u'*d(T2*p2)/dT2 = u'*R2*[[-p2]x I3x3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1w).dot(uw)
              = (T2*p2 - T1*p1).dot(R1*u)
    dplane_err/dT1 = (T2*p2 - T1*p1)'* d(R1*u)/dT1 - (R1*u)'*d(T1*p1)/dT1
    dplane_err/dT2 = (R1*u)'*d(T2*p2)/dT2
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    plane_err = (p2w-p1).dot(u)
              = (T2*p2 - p1).dot(u)
    dplane_err/dT2 = u'*d(T2*p2)/dT2 = u'*R2*[[-p2]x I3x3]
    */

    Vector evaluateError(const X &pose,
                         boost::optional<Matrix&> H = boost::none) const
    {
      const auto &rotation = pose.rotation();
      if (H)
        *H = u_.transpose() * rotation.matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return Vector1((pose.transformFrom(p2_) - p1_).dot(u_));
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarPlaneFactor1 on (" << keyFormatter(key()) <<  ")\n"
           << "  Plane Point: " << p1_.transpose() << "\n"
           << "  Plane norm Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;
  }; // class LidarPlaneFactor1

} // namespace gtsam


#include "SignedDistance.h"
#include "PointTriangleDistance.h"
#include <boost/assert.hpp>

using namespace MeshSdf;

SignedDistance::SignedDistance(Mesh mesh)
	: mesh(std::forward<Mesh>(mesh))
	, meshadj(this->mesh.tris, static_cast<int>(this->mesh.verts.size()))
	, usdist(this->mesh.verts, this->mesh.tris)
{
}

double SignedDistance::operator()(double x, double y, double z) const
{
	auto udinfo = usdist(x, y, z, 0);

	auto pnormal = PseudoNormal(udinfo.triEntityNearest, udinfo.triNearest);
	auto qc = Vec3{ x,y,z } - udinfo.triPtNearest;
	auto sign = boost::math::sign(qc.Dot(pnormal));

	return sign * udinfo.udist;
}

Vec3 SignedDistance::PseudoNormal_Vertex(int vnearest) const
{
	Vec3 pnormal{ 0.0, 0.0, 0.0 };

	auto trisadj = meshadj.Vert2Tris(vnearest);
	BOOST_ASSERT_MSG(!trisadj.empty(), "no incident triangles");

	for (auto itriadj : trisadj)
	{
		auto const& triadj = mesh.tris.at(itriadj);

		// find other 2 vertices of tri
		auto edgevert0 = -1;
		auto edgevert1 = -1;

		if (vnearest == triadj[0])
		{
			edgevert0 = triadj[1];
			edgevert1 = triadj[2];
		}
		else if (vnearest == triadj[1])
		{
			edgevert0 = triadj[0];
			edgevert1 = triadj[2];
		}
		else if (vnearest == triadj[2])
		{
			edgevert0 = triadj[0];
			edgevert1 = triadj[1];
		}
		else
		{
			BOOST_ASSERT_MSG(false, "no nearest vertex");
		}

		// weigh triangle normal by incident angle at nearest vertex
		auto edge1 = mesh.verts.at(edgevert0) - mesh.verts.at(vnearest);
		auto edge2 = mesh.verts.at(edgevert1) - mesh.verts.at(vnearest);

		auto incidentAngle = acos(edge1.Dot(edge2) / (edge1.Norm()*edge2.Norm()));
		pnormal += incidentAngle * mesh.triNormals.at(itriadj);
	}

	return pnormal.Normalized();
}

Vec3 SignedDistance::PseudoNormal_Edge(NearestTriEntity ent, int tri) const
{
	auto const& triadj = mesh.tris.at(tri);

	// find edge vertices
	auto nearestvert0 = -1;
	auto nearestvert1 = -1;

	if (ent == NearestTriEntity::Edge0)
	{
		nearestvert0 = triadj[0];
		nearestvert1 = triadj[1];
	}
	else if (ent == NearestTriEntity::Edge1)
	{
		nearestvert0 = triadj[1];
		nearestvert1 = triadj[2];
	}
	else if (ent == NearestTriEntity::Edge2)
	{
		nearestvert0 = triadj[2];
		nearestvert1 = triadj[0];
	}

	// equal weight for triangle normal of both adjacent triangles
	auto adjtris = meshadj.Edge2Tris(nearestvert0, nearestvert1);

	BOOST_ASSERT_MSG(adjtris.size() == 1 || adjtris.size() == 2,
		"invalid number of incident triangles");

	Vec3 pnormal{ 0.0, 0.0 };

	for (auto adjtri : adjtris)
	{
		pnormal += mesh.triNormals.at(adjtri);
	}

	return pnormal.Normalized();
}

Vec3 SignedDistance::PseudoNormal(NearestTriEntity ent, int tri) const
{
	switch (ent)
	{
		case NearestTriEntity::Vert0: return PseudoNormal_Vertex(mesh.tris.at(tri)[0]);
		case NearestTriEntity::Vert1: return PseudoNormal_Vertex(mesh.tris.at(tri)[1]);
		case NearestTriEntity::Vert2: return PseudoNormal_Vertex(mesh.tris.at(tri)[2]);
		case NearestTriEntity::Edge0:
		case NearestTriEntity::Edge1:
		case NearestTriEntity::Edge2: return PseudoNormal_Edge(ent, tri);
		case NearestTriEntity::Face: return mesh.triNormals.at(tri);
		default: throw std::runtime_error{"unknown entity"};
	}

	return {};
}


#include <iostream>
#include "turn_cost_grid_dijkstra.h"
#include "DFieldId.h"
#include <fstream>
#include <boost/algorithm/string.hpp>

int
main()
{
  using namespace turncostgrid;
  std::vector<GridCoordinate> coords;
  std::string filename{
          "/home/doms/Repositories/IBR-SVN/thesis-alg-2015-krupke-ma-robots/MasterThesis/ALENEX/Code/gridgraph/apx/p_1400_dense_2.gg"};
  std::ifstream file(filename);
  if (file.is_open()) {
    std::string line;
    while (std::getline(file, line)) {
      if (line.empty()) { continue; }
      if (line.at(0) == '#') { continue; }
      std::vector<std::string> tokens;
      boost::split(tokens, line, boost::is_any_of(" "));
      if (tokens.size() >= 2) {
        coords.push_back({std::stoi(tokens[0]), std::stoi(tokens[1])});
      }
    }
    file.close();
  } else {
    std::cerr << "Could not load grid graph from file \"" << filename << "\"" << std::endl;
  }
  std::sort(coords.begin(), coords.end());

  //std::vector<GridCoordinate> g{{0,0},{1,0}, {1,1}, {0,1}, {0,2}};
  // std::sort(g.begin(), g.end());
  GridGraph gd{coords, true};
  gd.print_graph(coords);

  for (unsigned int i = 0; i < coords.size(); ++i) {
    TurnCostGridDijkstra lgd{gd, {3, 1}, i, NORTH};
    lgd.lazy_dijkstra([&](int id, Direction d) {
        //std::cout << id << " " << to_string(d) << ": " << lgd.distance(state, id, d) << std::endl;
        return true;
    });
  }

  return 0;
}


#define BOOST_TEST_MODULE blas

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/vector/all.h++>

#include <mla/operations/level1/scale.h++>

typedef boost::mpl::list<
	mla::vector::Dense<float>,
	mla::vector::Dense<double>,
	mla::vector::SparseCS<float>,
	mla::vector::SparseCS<double>
> vector_type_list;

BOOST_AUTO_TEST_SUITE(test_boost_level1)

BOOST_AUTO_TEST_CASE_TEMPLATE( blas_level1_test_scale_one, VectorType, vector_type_list )
{
	using namespace mla;

	VectorType x(3);
	x.setValue(0, 1.0f);
	x.setValue(1, 1.0f);
	x.setValue(2, 1.0f);

	typename VectorType::scalar_type value = 1.0f;
	scale( value, x);

	for( unsigned int i = 0; i < x.size(); i++)
	{
		BOOST_CHECK_CLOSE(x.getValue(i), 1.0f, 1e-6f);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <stack>
#include <iostream>
#include <vector>

//==============================================================================
struct node_properties
{
    int component;
};
struct edge_properties
{
};
struct graph_properties
{
    int number_of_components;
};

//--------------------------------------
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                              node_properties, edge_properties, graph_properties>
    graph_t;
typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename boost::graph_traits<graph_t>::edge_descriptor edge_t;

//==============================================================================
void print_graph(const graph_t &graph)
{
    std::cout << "Graph:" << std::endl;
    auto edges = boost::edges(graph);
    for (auto it = edges.first; it != edges.second; ++it)
    {
        std::cout << boost::source(*it, graph) << " -> "
                  << boost::target(*it, graph) << std::endl;
    }
}

//==============================================================================
void print_components(const graph_t &graph)
{
    std::cout << "\nComponents: "
              << graph[boost::graph_bundle].number_of_components << std::endl;
    auto nodes = boost::vertices(graph);
    for (auto it = nodes.first; it != nodes.second; ++it)
    {
        std::cout << "Node: " << *it << " component: " << graph[*it].component
                  << std::endl;
    }
}

//==============================================================================
void strong_components(graph_t &graph)
{
    std::vector<edge_t> temp_edges;

    auto edges = boost::edges(graph);

    for (auto it = edges.first; it != edges.second; ++it)
    {
        auto pair = boost::add_edge(boost::target(*it, graph),
                                    boost::source(*it, graph),
                                    graph);
        temp_edges.push_back(pair.first);
    }

    graph[boost::graph_bundle].number_of_components =
        boost::strong_components(graph,
                                 boost::get(&node_properties::component, graph));

    for (const auto &e : temp_edges)
    {
        boost::remove_edge(e, graph);
    }
}

//==============================================================================

graph_t graph;
std::vector<vertex_t> vertices;

int nn = 10;
std::vector<int> r_index(nn, 0);
std::stack<int> S;
int vindex = 1;
int c = nn - 1;

void visit(int v)
{
    bool root = true;
    r_index[v] = vindex;
    vindex = vindex + 1;
    auto edges = boost::edges(graph);
    for (auto it = edges.first; it != edges.second; ++it)
    {
        int w = boost::target(*it, graph);
        int s = boost::source(*it, graph);
        if (v == s)
        {
            // std::cout << "\n Visit for node: " << v << ", edge: " << s << "->" << w << "\n";
            if (r_index[w] == 0)
            {
                visit(w);
            }
            if (r_index[w] < r_index[v])

            {
                r_index[v] = r_index[w];
                root = false;
            }
        }
    }
    if (root)
    {
        vindex = vindex - 1;
        std::cout << "scc: ";
        while (!S.empty() && (r_index[v] <= r_index[S.top()]))
        {
            int w = S.top();
            S.pop();
            std::cout << "" << w;
            r_index[w] = c;
            vindex = vindex - 1;
        }
        std::cout << v << "\n";
        r_index[v] = c;
        c = c - 1;
    }
    else
    {
        S.push(v);
    }
}

//==============================================================================
int main()
{

    ///graph 1

    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));

    // boost::add_edge(vertices.at(0), vertices.at(1), graph);
    // boost::add_edge(vertices.at(1), vertices.at(2), graph);
    // boost::add_edge(vertices.at(2), vertices.at(0), graph);
    // boost::add_edge(vertices.at(2), vertices.at(3), graph);
    // boost::add_edge(vertices.at(3), vertices.at(4), graph);
    // boost::add_edge(vertices.at(4), vertices.at(5), graph);
    // boost::add_edge(vertices.at(5), vertices.at(3), graph);

    //graph2
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    boost::add_edge(vertices.at(0), vertices.at(4), graph);
    boost::add_edge(vertices.at(0), vertices.at(1), graph);
    boost::add_edge(vertices.at(4), vertices.at(0), graph);
    boost::add_edge(vertices.at(4), vertices.at(1), graph);
    boost::add_edge(vertices.at(4), vertices.at(5), graph);
    boost::add_edge(vertices.at(5), vertices.at(6), graph);
    boost::add_edge(vertices.at(6), vertices.at(4), graph);
    boost::add_edge(vertices.at(1), vertices.at(2), graph);
    boost::add_edge(vertices.at(2), vertices.at(7), graph);
    boost::add_edge(vertices.at(2), vertices.at(3), graph);
    boost::add_edge(vertices.at(3), vertices.at(1), graph);
    boost::add_edge(vertices.at(8), vertices.at(9), graph);
    boost::add_edge(vertices.at(9), vertices.at(8), graph);
    /*
  boost::add_edge(vertices.at(0), vertices.at(1), graph);
  boost::add_edge(vertices.at(2), vertices.at(3), graph);
  boost::add_edge(vertices.at(3), vertices.at(4), graph);
  boost::add_edge(vertices.at(4), vertices.at(5), graph);
  boost::add_edge(vertices.at(7), vertices.at(8), graph);
  boost::add_edge(vertices.at(8), vertices.at(9), graph);
  boost::add_edge(vertices.at(9), vertices.at(7), graph);*/
    boost::mt19937 rng;
    //boost::generate_random_graph(graph, nn, 8, rng);
    size_t e = boost::num_edges(graph);
    size_t n = boost::num_vertices(graph);
    std::cout << "generated " << e << " edges, " << n << " vertices\n";

    graph[boost::graph_bundle].number_of_components =
        boost::connected_components(graph,
                                    boost::get(&node_properties::component, graph));

    for (int i = 0; i < nn; i++)
    {
        if (r_index[i] == 0)
        {
            visit(i);
        }
    }

    std::cout
        << "\n Components number SCC1: ----------" << c - 1 << "\n"
        << std::endl;
    for (auto i = r_index.begin(); i != r_index.end(); ++i)
        std::cout << *i << ' ';
    //std::cout << "\n ***************************************" << std::endl;
    //std::cout << "\nTrue boost graph implementation : \n----------------------" << std::endl;
    //strong_components(graph);
    //print_graph(graph);
    //print_components(graph);
}


/*

  Código con grafo ya establecido (se encuentra en el main)
  Se realizan y se calcula el tiempo de insertar un vertice y una arista, eliminar un vertice y una arista, DFS, BFS, Prim, Kruskal, Dijkstra y Floyd Warshall
  Equipo: Sebastián Medina  y Carlos de la Garza

*/

#include <iostream>
#include <deque>
#include <iterator>
#include <vector>
#include <time.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/exterior_property.hpp>

using namespace std;
using namespace boost;

struct node{
    float id;
};

typedef property<edge_weight_t, float> EdgeWeightProperty;
typedef adjacency_list<listS,vecS,directedS,node,EdgeWeightProperty> DirectedGraph;
typedef property_map<DirectedGraph, edge_weight_t>::type EdgeWeightMap;
typedef graph_traits<DirectedGraph>::vertex_descriptor vertex_descriptor;
typedef graph_traits < DirectedGraph >::edge_descriptor edge_descriptor;
typedef exterior_vertex_property<DirectedGraph, float> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;
typedef pair<int, int> Edge;

vertex_descriptor addVertex(DirectedGraph & g, int n)
{
    vertex_descriptor v0 = add_vertex(g);
    g[v0].id = n;
    return v0;
}

void addEdge(int vertex1, int vertex2, float weight, DirectedGraph & g)
{
    add_edge(vertex1-1, vertex2-1, weight, g);
}

bool vertexExists(DirectedGraph & g, int n, int & t)
{
   int pos = 0;
   DirectedGraph::vertex_iterator start,end;
   for(tie(start,end) = vertices(g); start!=end; start++)
   {
       if(g[*start].id == n)
       {
           t = pos;
           return true;
       }
       pos++;
   }
   return false;
}

bool edgeExists(DirectedGraph & g, int v1, int v2, float w)
{
   DirectedGraph::edge_iterator edgeIt, edgeEnd;
   for (tie(edgeIt, edgeEnd) = edges(g); edgeIt != edgeEnd; ++edgeIt)
   {
       if(source(*edgeIt, g) == v1 && target(*edgeIt, g) == v2 && get(edge_weight, g, *edgeIt) == w)
           return true;
   }
   return false;
}

void insertarVertice(DirectedGraph & g){
  clock_t t;
  int vertice;
  cout << "Ingrese el nodo que quiera agregar: ";
  while(!(cin >> vertice)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Intente otra vez: ";
    }
  int posicion;
  if(vertexExists(g, vertice, posicion)) {
     cout << "El vértice: " << vertice << " ya existe" << endl;
  } else {
    t = clock();
    vertex_descriptor v0 = addVertex(g,vertice);
    t = clock() - t;
    double time_taken = ((double)t)/CLOCKS_PER_SEC;
    printf("Tiempo en insertar vértice: %f ms \n", time_taken*1000);
  }
  cout << "" << endl;
}

void insertarArista(DirectedGraph & g){
  clock_t t;
  bool negativeEdge = false;
  bool unDirected = false;
  float v1, v2;
  int pos1, pos2;
  cout << "Vértice 1: ";
  while(!(cin >> v1)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Entrada inválida, intenta otra vez: ";
  }
  if(!vertexExists(g, v1, pos1)) {
    cout << "El vértice con id " << v1 << " no existe, debe crearse antes" << endl;
  } else {
      cin.ignore();
      cout << "Vértice 2: ";
        while(!(cin >> v2)){
          cin.clear();
          cin.ignore(numeric_limits<streamsize>::max(), '\n');
          cout << "Entrada inválida, intenta otra vez: ";
        }
        if(!vertexExists(g, v2, pos2)) {
          cout << "El vértice con id " << v2 << " no existe, debe crearse antes " << endl;
        } else {
            float weight;
            cin.ignore();
            cout << "Peso de la arista a insertar: ";
            while(!(cin >> weight)){
              cin.clear();
              cin.ignore(numeric_limits<streamsize>::max(), '\n');
              cout << "Entrada inválida, intenta otra vez: ";
            }
      EdgeWeightProperty w = weight;
      if(edgeExists(g,pos1,pos2,weight)) {
        cout << "La arista de " << pos1 << " a " << pos2 << " con peso " << weight << " ya existe" << endl;
      } else {
        if(weight < 0) {
          negativeEdge = true;
          cout << "No se pueden agregar aristas negativas" << endl;
        }
        t = clock();
        add_edge(pos1,pos2,w,g);
        if(unDirected){
          add_edge(pos2,pos1,w,g);
        }
        t = clock() - t;
        double time_taken = ((double)t)/CLOCKS_PER_SEC;
        printf("Tiempo en insertar arista: %f ms \n", time_taken*1000);
      }
    }
  }
  cout << "" << endl;
}

void eliminarVertice(DirectedGraph & g){
  clock_t t;
  float vertice;
  int pos;
  cout << "Ingrese el nodo que quiera borrar: ";
  while(!(cin >> vertice)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Entrada inválida, intenta otra vez: ";
  }
  if(vertexExists(g, vertice, pos)){
    graph_traits<DirectedGraph>::vertex_iterator start,end,next;
    tie(start, end) = vertices(g);
    for (next = start; start != end; start = next) {
      ++next;
      if(g[*start].id == vertice){
        t = clock();
        clear_vertex(*start, g);
        remove_vertex(*start,g);
        t = clock() - t;
        double time_taken = ((double)t)/CLOCKS_PER_SEC;
        printf("Tiempo en borrar vértice: %f ms \n", time_taken*1000);
      }
    }
  } else {
      cout << "El vértice con id " << vertice << " no existe " << endl;
  }
  cout << "" << endl;
}

void eliminarArista(DirectedGraph & g){
  clock_t t;
  float v1,v2;
  int pos1,pos2;
  cout << "Id del vértice 1 conectado por la arista: ";
  while(!(cin >> v1)){
    cin.clear();
    cin.ignore(numeric_limits<streamsize>::max(), '\n');
    cout << "Entrada inválida, intenta otra vez: ";
  }
  if(vertexExists(g, v1,pos1)) {
    cin.ignore();
    cout << "Id del vértice 2 conectado por la arista: ";
    while(!(cin >> v2)){
      cin.clear();
      cin.ignore(numeric_limits<streamsize>::max(), '\n');
      cout << "Entrada inválida, intenta otra vez: ";
    }
    if(vertexExists(g, v2,pos2)){
      if(edge(pos1,pos2,g).second){
        pair<DirectedGraph::edge_descriptor, bool> retrievedEdge = edge(pos1, pos2, g);
        t = clock();
        remove_edge(retrievedEdge.first,g);
        t = clock() - t;
        double time_taken = ((double)t)/CLOCKS_PER_SEC;
        printf("Tiempo en borrar arista: %f ms \n", time_taken*1000);
      } else {
          cout << "La arista de " << v1 << " a " << v2 << " no existe" << endl;
      }
    } else {
        cout << "El vértice con id " << v2 << " no existe " << endl;
      }
  } else {
      cout << "El vértice con id " << v1 << " no existe " << endl;
  }
  cout << "" << endl;
}

void DFS(DirectedGraph & g){

  clock_t t;
  default_dfs_visitor vis;
  t = clock();
  depth_first_search(g, visitor(vis));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de DFS: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void BFS(DirectedGraph & g){

  clock_t t;
  default_bfs_visitor vis;
  t = clock();
  breadth_first_search(g, vertex(0, g), visitor(vis));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de BFS: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void Prim(DirectedGraph & g){

  clock_t t;
  vector <vertex_descriptor> p(num_vertices(g));
  t = clock();
  prim_minimum_spanning_tree(g, &p[0]);
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Prim: %f ms \n", time_taken*1000);

  cout << "" << endl;

  for (size_t i = 0; i != p.size(); ++i)
  {
      if (p[i] != i)
          cout << g[p[i]].id << " --- " << g[i].id << endl;
      else
          cout << "start -> " << g[i].id << endl;
  }

  cout << "" << endl;

}

void Kruskal(DirectedGraph & g){

  clock_t t;
  property_map<DirectedGraph, edge_weight_t>::type weight = get(edge_weight, g);
  vector <edge_descriptor> spanning_tree;
  t = clock();
  kruskal_minimum_spanning_tree(g, back_inserter(spanning_tree));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Kruskal: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void Dijkstra(DirectedGraph & g){

  clock_t t;
  vector<vertex_descriptor> p(num_vertices(g));
  vector<int> d(num_vertices(g));
  vertex_descriptor s = vertex(0, g);
  t = clock();
  dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Dijkstra: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

void FloydWarshall(DirectedGraph & g){

  clock_t t;
  map<vertex_descriptor, map<vertex_descriptor, float> >matrix;
  EdgeWeightMap weight_pmap = get(edge_weight, g);
  DistanceMatrix dist(num_vertices(g));
  DistanceMatrixMap dm(dist, g);
  t = clock();
  bool noNegCycles = floyd_warshall_all_pairs_shortest_paths (g, dm, weight_map(weight_pmap));
  t = clock() - t;
  double time_taken = ((double)t)/CLOCKS_PER_SEC;
  printf("Tiempo de Floyd Warshall: %f ms \n", time_taken*1000);
  cout << "" << endl;

}

int main(int argc, const char * argv[]){

  DirectedGraph g;
  vector<vertex_descriptor> verts;

  for(int i=1; i<15; i++)
  {
      verts.push_back(addVertex(g, i));
  }

  addEdge(1,4,8,g);
  addEdge(1,3,8,g);
  addEdge(2,5,7,g);
  addEdge(3,10,4,g);
  addEdge(3,2,7,g);
  addEdge(3,5,8,g);
  addEdge(4,7,3,g);
  addEdge(4,5,1,g);
  addEdge(4,8,2,g);
  addEdge(5,6,9,g);
  addEdge(6,13,4,g);
  addEdge(7,4,6,g);
  addEdge(8,9,3,g);
  addEdge(8,7,3,g);
  addEdge(9,10,2,g);
  addEdge(9,12,4,g);
  addEdge(10,3,10,g);
  addEdge(10,6,6,g);
  addEdge(11,12,6,g);
  addEdge(12,11,8,g);
  addEdge(12,9,2,g);
  addEdge(12,14,9,g);
  addEdge(13,14,6,g);
  addEdge(14,13,2,g);

  cout << "" << endl;

  insertarVertice(g);
  insertarArista(g);
  eliminarVertice(g);
  eliminarArista(g);
  DFS(g);
  BFS(g);
  Prim(g);
  Kruskal(g);
  Dijkstra(g);
  FloydWarshall(g);

}