#include <boost/assign/std/vector.hpp>
#include <cradle/anonymous.hpp>
#include <cradle/geometry/polygonal.hpp>

#include <cradle/test.hpp>

using namespace cradle;
using namespace boost::assign;

// polygon2

TEST_CASE("simple_ccw_poly_test")
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(0, 0), make_vector<double>(5, 0),
            make_vector<double>(2, 2), make_vector<double>(0, 2);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 7.);
    REQUIRE(!is_inside(poly, make_vector<double>(-1, 1)));
    REQUIRE(is_inside(poly, make_vector<double>(1, 1)));
}

TEST_CASE("simple_cw_poly_test")
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(0, 0), make_vector<double>(0, 2),
            make_vector<double>(2, 2), make_vector<double>(5, 0);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 7.);
    REQUIRE(!is_inside(poly, make_vector<double>(-1, 1)));
    REQUIRE(is_inside(poly, make_vector<double>(1, 1)));
}

TEST_CASE("triangle_test")
{
    triangle<3, double> tri(
        make_vector<double>(0, 0, 0),
        make_vector<double>(0, 0, 2),
        make_vector<double>(0, 1, 0));

    CRADLE_CHECK_ALMOST_EQUAL(get_normal(tri), make_vector<double>(-1, 0, 0));
}

TEST_CASE("square_test")
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-2, -2), make_vector<double>(-2, 2),
            make_vector<double>(2, 2), make_vector<double>(2, -2);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 16.);
    REQUIRE(!is_inside(poly, make_vector<double>(-3, 3)));
    REQUIRE(is_inside(poly, make_vector<double>(-1, 1)));
}

TEST_CASE("concave_poly_test")
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-2, -2), make_vector<double>(-2, 2),
            make_vector<double>(2, 2), make_vector<double>(0, 0),
            make_vector<double>(2, -2);
        initialize(&poly.vertices, vertices);
    }
    CRADLE_CHECK_ALMOST_EQUAL(get_area(poly), 12.);
    REQUIRE(!is_inside(poly, make_vector<double>(-3, 3)));
    REQUIRE(!is_inside(poly, make_vector<double>(1, 0)));
    REQUIRE(is_inside(poly, make_vector<double>(-1, 1)));
}

TEST_CASE("edge_view_test")
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(0, 0), make_vector<double>(0, 1),
            make_vector<double>(1, 1);
        initialize(&poly.vertices, vertices);
    }

    vertex2_array::const_iterator vertex_i = poly.vertices.begin(),
                                  vertex_j = poly.vertices.end() - 1;
    for (polygon2_edge_view ev(poly); !ev.done();
         ev.advance(), vertex_j = vertex_i++)
    {
        REQUIRE(ev.p0() == *vertex_j);
        REQUIRE(ev.p1() == *vertex_i);
    }
}

TEST_CASE("circle_test")
{
    circle<double> circle(make_vector<double>(0, 0), 2);
    polygon2 poly = as_polygon(circle, 8);

    std::vector<vector<2, double>> correct_vertices;
    double srt = std::sqrt(2.);
    correct_vertices += make_vector<double>(2, 0),
        make_vector<double>(srt, srt), make_vector<double>(0, 2),
        make_vector<double>(-srt, srt), make_vector<double>(-2, 0),
        make_vector<double>(-srt, -srt), make_vector<double>(0, -2),
        make_vector<double>(srt, -srt);

    CRADLE_CHECK_RANGES_ALMOST_EQUAL(poly.vertices, correct_vertices);
}

TEST_CASE("box_test")
{
    box2d box(make_vector<double>(0, 0), make_vector<double>(2, 2));
    polygon2 poly = as_polygon(box);

    std::vector<vector<2, double>> correct_vertices;
    correct_vertices += make_vector<double>(0, 0), make_vector<double>(2, 0),
        make_vector<double>(2, 2), make_vector<double>(0, 2);

    CRADLE_CHECK_RANGES_ALMOST_EQUAL(poly.vertices, correct_vertices);
}

// polyset

static double const tolerance = 0.00001;

TEST_CASE("two_polygons_test")
{
    polygon2 poly0, poly1;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-2, -2), make_vector<double>(-2, 2),
            make_vector<double>(2, 2), make_vector<double>(2, -2);
        initialize(&poly0.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(4, 4), make_vector<double>(4, 5),
            make_vector<double>(5, 5), make_vector<double>(5, 4);
        initialize(&poly1.vertices, vertices);
    }

    polyset region;
    add_polygon(region, poly0);
    add_polygon(region, poly1);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(region), 17., tolerance);

    REQUIRE(!is_inside(region, make_vector<double>(-3, 3)));
    REQUIRE(!is_inside(region, make_vector<double>(3, 3)));
    REQUIRE(!is_inside(region, make_vector<double>(6, 6)));
    REQUIRE(is_inside(region, make_vector<double>(-1, 1)));
    REQUIRE(is_inside(region, make_vector<double>(4.5, 4.5)));
    REQUIRE(is_inside(region, make_vector<double>(0, 0)));
}

TEST_CASE("polygon_with_hole_test")
{
    polygon2 poly, hole;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-2, -2), make_vector<double>(-2, 2),
            make_vector<double>(2, 2), make_vector<double>(2, -2);
        initialize(&poly.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-1, -1), make_vector<double>(-1, 1),
            make_vector<double>(1, 1), make_vector<double>(1, -1);
        initialize(&hole.vertices, vertices);
    }
    polyset frame;
    add_polygon(frame, poly);
    add_hole(frame, hole);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(frame), 12., tolerance);

    REQUIRE(!is_inside(frame, make_vector<double>(-3, 3)));
    REQUIRE(!is_inside(frame, make_vector<double>(0, 0)));
    REQUIRE(is_inside(frame, make_vector<double>(-1.5, 1.5)));
}

TEST_CASE("polyset_as_polygon_list_test")
{
    polygon2 outside, hole, inside;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-3, -3), make_vector<double>(-3, 3),
            make_vector<double>(3, 3), make_vector<double>(3, -3);
        initialize(&outside.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-2, -2), make_vector<double>(-2, 2),
            make_vector<double>(2, 2), make_vector<double>(2, -2);
        initialize(&hole.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-1, -1), make_vector<double>(-1, 1),
            make_vector<double>(1, 1), make_vector<double>(1, -1);
        initialize(&inside.vertices, vertices);
    }

    polyset frame;
    add_polygon(frame, outside);
    add_hole(frame, hole);
    add_polygon(frame, inside);

    std::vector<polygon2> polys = as_polygon_list(frame);
    REQUIRE(polys.size() == 2);
    CRADLE_CHECK_WITHIN_TOLERANCE(
        get_area(polys[0]) + get_area(polys[1]), get_area(frame), tolerance);

    polyset reconstructed;
    add_polygon(reconstructed, polys[0]);
    add_polygon(reconstructed, polys[1]);
    CRADLE_CHECK_WITHIN_TOLERANCE(
        get_area(reconstructed), get_area(frame), tolerance);
}

TEST_CASE("polyset_set_operations_test")
{
    polygon2 p;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-6, -3), make_vector<double>(-6, 3),
            make_vector<double>(6, 3), make_vector<double>(6, -3);
        initialize(&p.vertices, vertices);
    }
    polyset wide_rect;
    create_polyset(&wide_rect, p);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(wide_rect), 72., tolerance);

    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-3, -6), make_vector<double>(-3, 6),
            make_vector<double>(3, 6), make_vector<double>(3, -6);
        initialize(&p.vertices, vertices);
    }
    polyset tall_rect;
    create_polyset(&tall_rect, p);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(tall_rect), 72., tolerance);

    polyset cross, square;
    do_set_operation(&cross, set_operation::UNION, wide_rect, tall_rect);
    do_set_operation(
        &square, set_operation::INTERSECTION, wide_rect, tall_rect);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(cross), 108., tolerance);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(square), 36., tolerance);

    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-1, -1), make_vector<double>(-1, 1),
            make_vector<double>(1, 1), make_vector<double>(1, -1);
        initialize(&p.vertices, vertices);
    }
    polyset small_square;
    create_polyset(&small_square, p);
    do_set_operation(&cross, set_operation::DIFFERENCE, cross, small_square);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_area(cross), 104., tolerance);
    REQUIRE(!is_inside(cross, make_vector<double>(0, 0)));
    REQUIRE(is_inside(cross, make_vector<double>(2, 0)));
    REQUIRE(!is_inside(cross, make_vector<double>(8, 0)));
    REQUIRE(is_inside(cross, make_vector<double>(0, -5)));
}

TEST_CASE("polyset_comparisons_test")
{
    polygon2 poly1, poly2;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-2, -2), make_vector<double>(-2, 2),
            make_vector<double>(2, 2), make_vector<double>(2, -2);
        initialize(&poly1.vertices, vertices);
    }
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-1, -1), make_vector<double>(-1, 1),
            make_vector<double>(1, 1), make_vector<double>(1, -1);
        initialize(&poly2.vertices, vertices);
    }

    polyset region1;
    add_polygon(region1, poly1);
    add_hole(region1, poly2);

    polyset region2;
    add_polygon(region2, poly1);
    add_hole(region2, poly2);

    polyset region3;
    add_polygon(region3, poly1);
    add_polygon(region3, poly2);

    REQUIRE(almost_equal(region1, region2, 0.001));
    REQUIRE(!almost_equal(region1, region3, 0.001));
}

// structure_geometry

TEST_CASE("structure_geometry_test0")
{
    polyset area0, area1, area2;

    add_polygon(
        area0,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 6),
            make_vector<double>(3, 3))));
    REQUIRE(area0.polygons.size() == 1);

    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(1, 0),
            make_vector<double>(0, 0),
            make_vector<double>(0, 1))));
    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(1, 3),
            make_vector<double>(0, 3),
            make_vector<double>(0, 4))));
    REQUIRE(area1.polygons.size() == 2);

    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 1),
            make_vector<double>(1, 0))));
    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(2, 0),
            make_vector<double>(2, 1),
            make_vector<double>(3, 0))));
    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(4, 0),
            make_vector<double>(4, 1),
            make_vector<double>(5, 0))));
    REQUIRE(area2.polygons.size() == 3);

    structure_geometry volume;
    volume.slices += structure_geometry_slice(0.0, 1.5, area0),
        structure_geometry_slice(1.5, 1.5, area1),
        structure_geometry_slice(3.0, 1.5, area2);

    REQUIRE(get_slice(volume, -0.8) == 0);
    REQUIRE(get_slice(volume, -0.7) != 0);
    REQUIRE(get_slice(volume, 3.7) != 0);
    REQUIRE(get_slice(volume, 3.8) == 0);

    REQUIRE(get_slice(volume, 1)->position == 1.5);
    REQUIRE(get_slice(volume, 1)->region.polygons.size() == 2);
    REQUIRE(get_slice(volume, 2.5)->position == 3);
    REQUIRE(get_slice(volume, 2.5)->region.polygons.size() == 3);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume), 17.25, tolerance);
}

TEST_CASE("structure_geometry_test1")
{
    polyset area0, area1, area2, area3;

    add_polygon(
        area0,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 6),
            make_vector<double>(3, 3))));

    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(1, 0),
            make_vector<double>(0, 0),
            make_vector<double>(0, 1))));
    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(1, 3),
            make_vector<double>(0, 3),
            make_vector<double>(0, 4))));

    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 1),
            make_vector<double>(1, 0))));
    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(2, 0),
            make_vector<double>(2, 1),
            make_vector<double>(3, 0))));

    add_polygon(
        area3,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(4, 0),
            make_vector<double>(4, 1),
            make_vector<double>(5, 0))));

    structure_geometry volume1;
    volume1.slices += structure_geometry_slice(1.0, 1, area1),
        structure_geometry_slice(2.0, 1, area2),
        structure_geometry_slice(2.5, 1, area3);

    structure_geometry volume2;
    volume2.slices += structure_geometry_slice(0.0, 1, area0),
        structure_geometry_slice(1.0, 1, area1),
        structure_geometry_slice(2.0, 1, area2),
        structure_geometry_slice(2.5, 1, area3);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 11.125, tolerance);

    REQUIRE(almost_equal(volume2, volume2, tolerance));
    REQUIRE(!almost_equal(volume1, volume2, tolerance));
}

TEST_CASE("set_operation_test")
{
    polyset area0, area1, area2, area3, area4, area5;

    add_polygon(
        area0,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 5),
            make_vector<double>(4, 5),
            make_vector<double>(4, 0))));
    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 5),
            make_vector<double>(4, 5),
            make_vector<double>(4, 0))));
    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 5),
            make_vector<double>(4, 5),
            make_vector<double>(4, 0))));

    add_polygon(
        area3,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(6, 0),
            make_vector<double>(6, 5),
            make_vector<double>(10, 5),
            make_vector<double>(10, 0))));
    add_polygon(
        area4,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(2, 0),
            make_vector<double>(2, 5),
            make_vector<double>(6, 5),
            make_vector<double>(6, 0))));
    add_polygon(
        area5,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(0, 5),
            make_vector<double>(4, 5),
            make_vector<double>(4, 0))));

    {
        structure_geometry volume1;
        volume1.slices += structure_geometry_slice(1.0, 2, area0),
            structure_geometry_slice(3.0, 2, area1),
            structure_geometry_slice(4.5, 1, area2),
            structure_geometry_slice(6.0, 2, polyset());
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

        structure_geometry volume2;
        volume2.slices += structure_geometry_slice(1.0, 2, polyset()),
            structure_geometry_slice(3.0, 2, area3),
            structure_geometry_slice(4.5, 1, area4),
            structure_geometry_slice(6.0, 2, area5);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

        structure_geometry union_;
        do_set_operation(&union_, set_operation::UNION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 190., tolerance);

        structure_geometry intersection;
        do_set_operation(
            &intersection, set_operation::INTERSECTION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(
            get_volume(intersection), 10., tolerance);

        structure_geometry xor_;
        do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 180., tolerance);

        structure_geometry difference;
        do_set_operation(
            &difference, set_operation::DIFFERENCE, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 90., tolerance);
    }

    {
        structure_geometry volume;
        volume.slices += structure_geometry_slice(1.0, 2, area0),
            structure_geometry_slice(3.0, 2, area1),
            structure_geometry_slice(4.5, 1, area2),
            structure_geometry_slice(6.0, 2, polyset());

        structure_geometry result;

        structure_geometry mismatched_volume1;
        mismatched_volume1.slices
            += structure_geometry_slice(1.0, 2, polyset()),
            structure_geometry_slice(3.0, 2, area3),
            structure_geometry_slice(4.5, 1, area4);
        REQUIRE_THROWS_AS(
            do_set_operation(
                &result, set_operation::UNION, volume, mismatched_volume1),
            std::exception);

        structure_geometry mismatched_volume2;
        mismatched_volume2.slices
            += structure_geometry_slice(1.0, 2, polyset()),
            structure_geometry_slice(3.0, 2, area1),
            structure_geometry_slice(4.5, 1, area2),
            structure_geometry_slice(6.0, 1, area3);
        REQUIRE_THROWS_AS(
            do_set_operation(
                &result, set_operation::UNION, volume, mismatched_volume2),
            std::exception);

        structure_geometry mismatched_volume3;
        mismatched_volume3.slices
            += structure_geometry_slice(1.1, 2, polyset()),
            structure_geometry_slice(3.0, 2, area1),
            structure_geometry_slice(4.5, 1, area2),
            structure_geometry_slice(6.0, 2, area3);
        REQUIRE_THROWS_AS(
            do_set_operation(
                &result, set_operation::UNION, volume, mismatched_volume3),
            std::exception);
    }

    {
        structure_geometry volume1;
        volume1.slices += structure_geometry_slice(1.0, 2, polyset()),
            structure_geometry_slice(3.0, 2, area0),
            structure_geometry_slice(4.5, 1, area1),
            structure_geometry_slice(6.0, 2, area2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

        structure_geometry volume2;
        volume2.slices += structure_geometry_slice(1.0, 2, area3),
            structure_geometry_slice(3.0, 2, area4),
            structure_geometry_slice(4.5, 1, area5),
            structure_geometry_slice(6.0, 2, polyset());
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

        structure_geometry union_;
        do_set_operation(&union_, set_operation::UNION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 160., tolerance);

        structure_geometry intersection;
        do_set_operation(
            &intersection, set_operation::INTERSECTION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(
            get_volume(intersection), 40., tolerance);

        structure_geometry xor_;
        do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 120., tolerance);

        structure_geometry difference;
        do_set_operation(
            &difference, set_operation::DIFFERENCE, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 60., tolerance);
    }

    {
        structure_geometry volume1;
        volume1.slices += structure_geometry_slice(1.0, 2, polyset()),
            structure_geometry_slice(3.0, 2, area0),
            structure_geometry_slice(4.5, 1, area1),
            structure_geometry_slice(6.0, 2, area2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

        structure_geometry volume2;
        volume2.slices += structure_geometry_slice(1.0, 2, area3),
            structure_geometry_slice(3.0, 2, area4),
            structure_geometry_slice(4.5, 1, area5),
            structure_geometry_slice(6.0, 2, polyset());
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

        structure_geometry union_;
        do_set_operation(&union_, set_operation::UNION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 160., tolerance);

        structure_geometry intersection;
        do_set_operation(
            &intersection, set_operation::INTERSECTION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(
            get_volume(intersection), 40., tolerance);

        structure_geometry xor_;
        do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 120., tolerance);

        structure_geometry difference;
        do_set_operation(
            &difference, set_operation::DIFFERENCE, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 60., tolerance);
    }

    {
        structure_geometry volume1;
        volume1.slices += structure_geometry_slice(1.0, 2, area0),
            structure_geometry_slice(3.0, 2, polyset()),
            structure_geometry_slice(4.5, 1, area1),
            structure_geometry_slice(6.0, 2, area2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

        structure_geometry volume2;
        volume2.slices += structure_geometry_slice(1.0, 2, area3),
            structure_geometry_slice(3.0, 2, area4),
            structure_geometry_slice(4.5, 1, area5),
            structure_geometry_slice(6.0, 2, polyset());
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

        structure_geometry union_;
        do_set_operation(&union_, set_operation::UNION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 180., tolerance);

        structure_geometry intersection;
        do_set_operation(
            &intersection, set_operation::INTERSECTION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(
            get_volume(intersection), 20., tolerance);

        structure_geometry xor_;
        do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 160., tolerance);

        structure_geometry difference;
        do_set_operation(
            &difference, set_operation::DIFFERENCE, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 80., tolerance);
    }

    {
        structure_geometry volume1;
        volume1.slices += structure_geometry_slice(1.0, 2, area0),
            structure_geometry_slice(3.0, 2, area1),
            structure_geometry_slice(4.5, 1, area2),
            structure_geometry_slice(6.0, 2, polyset());
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume1), 100., tolerance);

        structure_geometry volume2;
        volume2.slices += structure_geometry_slice(1.0, 2, area3),
            structure_geometry_slice(3.0, 2, polyset()),
            structure_geometry_slice(4.5, 1, area4),
            structure_geometry_slice(6.0, 2, area5);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(volume2), 100., tolerance);

        structure_geometry union_;
        do_set_operation(&union_, set_operation::UNION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(union_), 190., tolerance);

        structure_geometry intersection;
        do_set_operation(
            &intersection, set_operation::INTERSECTION, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(
            get_volume(intersection), 10., tolerance);

        structure_geometry xor_;
        do_set_operation(&xor_, set_operation::XOR, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(xor_), 180., tolerance);

        structure_geometry difference;
        do_set_operation(
            &difference, set_operation::DIFFERENCE, volume1, volume2);
        CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(difference), 90., tolerance);
    }
}

TEST_CASE("expansion_2d")
{
    polyset area0, area1, area2, area3;

    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(6, 0),
            make_vector<double>(6, 6),
            make_vector<double>(0, 6))));

    add_polygon(
        area2,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(1, 0),
            make_vector<double>(1, 1),
            make_vector<double>(0, 1))));

    structure_geometry original;
    original.slices += structure_geometry_slice(1, 1, area1),
        structure_geometry_slice(2, 1, area2);

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(original), 37., tolerance);

    structure_geometry expanded;
    expand_in_2d(&expanded, original, 1.);

    REQUIRE(get_volume(expanded) > 65);
    REQUIRE(get_volume(expanded) < 83);
    REQUIRE(is_inside(expanded, make_vector<double>(6.9, 1, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.9, 1, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(1, 6.9, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(1, -0.9, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.7, -0.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(6.7, -0.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(6.7, 6.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.7, 6.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(1.9, 1, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.9, 1, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(1, 1.9, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(1, -0.9, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.7, -0.7, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(1.7, -0.7, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(1.7, 1.7, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.7, 1.7, 2)));
}

TEST_CASE("expansion_3d")
{
    polyset area0, area1, area2, area3;

    add_polygon(
        area1,
        make_polygon2(anonymous<std::vector<vector2d>>(
            make_vector<double>(0, 0),
            make_vector<double>(6, 0),
            make_vector<double>(6, 6),
            make_vector<double>(0, 6))));

    structure_geometry original;
    original.slices += structure_geometry_slice(-1, 1, polyset()),
        structure_geometry_slice(0, 1, polyset()),
        structure_geometry_slice(1, 1, area1),
        structure_geometry_slice(2, 1, polyset()),
        structure_geometry_slice(3, 1, polyset());

    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(original), 36., tolerance);

    structure_geometry expanded;
    expand_in_3d(&expanded, original, 1.);

    REQUIRE(get_volume(expanded) > 136);
    REQUIRE(get_volume(expanded) < 197);
    REQUIRE(is_inside(expanded, make_vector<double>(6.9, 1, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.9, 1, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(1, 6.9, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(1, -0.9, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.7, -0.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(6.7, -0.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(6.7, 6.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(-0.7, 6.7, 1)));
    REQUIRE(is_inside(expanded, make_vector<double>(0, 0, 0)));
    REQUIRE(is_inside(expanded, make_vector<double>(6, 0, 0)));
    REQUIRE(is_inside(expanded, make_vector<double>(6, 6, 0)));
    REQUIRE(is_inside(expanded, make_vector<double>(0, 6, 0)));
    REQUIRE(is_inside(expanded, make_vector<double>(0, 0, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(6, 0, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(6, 6, 2)));
    REQUIRE(is_inside(expanded, make_vector<double>(0, 6, 2)));

    structure_geometry contracted;
    expand_in_3d(&contracted, original, -1.);
    CRADLE_CHECK_WITHIN_TOLERANCE(get_volume(contracted), 0., tolerance);
}

TEST_CASE("polygon_bounding_box_test")
{
    polygon2 poly;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(0, 0), make_vector<double>(-1, -1),
            make_vector<double>(-3, 0), make_vector<double>(0, 7),
            make_vector<double>(3, 3), make_vector<double>(3, 2);
        initialize(&poly.vertices, vertices);
    }
    REQUIRE(
        bounding_box(poly)
        == box2d(make_vector<double>(-3, -1), make_vector<double>(6, 8)));
}

TEST_CASE("polyset_bounding_box_test")
{
    polygon2 p;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-6, -3), make_vector<double>(-6, 3),
            make_vector<double>(-4, 3), make_vector<double>(-4, -3);
        initialize(&p.vertices, vertices);
    }
    polyset area;
    create_polyset(&area, p);
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(2, -6), make_vector<double>(2, -2),
            make_vector<double>(4, -2), make_vector<double>(4, -6);
        initialize(&p.vertices, vertices);
    }
    add_polygon(area, p);

    box<2, double> bb = bounding_box(area);
    REQUIRE(almost_equal(bb.corner, make_vector<double>(-6, -6), 0.00001));
    REQUIRE(almost_equal(bb.size, make_vector<double>(10, 9), 0.00001));
}


/*
 * Copyright (c) 2011-2014 Burkhard Ritter
 * This code is distributed under the two-clause BSD License.
 */
#ifndef __SYSTEM_HPP__
#define __SYSTEM_HPP__

#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include "basis.hpp"
using namespace Eigen;

typedef SparseMatrix<double> SMatrix;
typedef MatrixXd DMatrix;
typedef VectorXd DVector;

enum Spin {UP=0, DOWN=1};

/**
 * Caching creator.
 *
 * Constructs one creator martrix for each orbital of the system and caches
 * them.
 *
 * @tparam System
 */
template<class System>
class Creator
{
public:
    Creator (const System& s_)
    : s(s_)
    {}

    void construct ()
    {
        const size_t N_orbital = s.basis.numberOfOrbitals();
        cs = std::vector<SMatrix>(N_orbital);
        for (size_t i=0; i<N_orbital; i++)
            constructMatrix(i);
    }

    /**
     * Creator matrix for the given orbital.
     *
     * \f$c^{\dag}_i\f$
     *
     * @param i orbital of the system
     *
     * @return
     */
    const SMatrix& operator() (int i) const
    {
        return cs[i];
    }

private:
    // TODO: update to use Eigen's new SparseMatrix interface
    void constructMatrix (int i)
    {
        cs[i] = SMatrix(s.basis.size(), s.basis.size());
        // we expect one entry per column
        cs[i].reserve(VectorXi::Constant(s.basis.size(),1));
        for (size_t col=0; col<s.basis.size(); col++)
        {
            if (s.basis(col)[i] == 1)
                continue;
            State state(s.basis(col));
            state[i] = 1;
            const size_t row = s.basis(state);
            const size_t sum = state.count(i);
            const double sign = (sum%2==0)?1:-1; // probably faster than using (-1)^sum
            cs[i].insert(row, col) = sign;
        }
        cs[i].makeCompressed();
    }

    const System& s;
    std::vector<SMatrix> cs;
};

/**
 * Caching annihilator.
 *
 * Constructs one annihilator matrix for each orbital of the system and caches
 * them.
 *
 * @tparam System
 */
template<class System>
class Annihilator
{
public:
    Annihilator (const System& s_)
    : s(s_)
    {}

    void construct ()
    {
        const size_t N_orbital = s.basis.numberOfOrbitals();
        as = std::vector<SMatrix>(N_orbital);
        for (size_t i=0; i<N_orbital; i++)
            as[i] = SMatrix(s.creator(i).transpose());
    }

    /**
     * Annihilator matrix for the given orbital.
     *
     * \f$c_i\f$
     *
     * @param i orbital of the system
     *
     * @return
     */
    const SMatrix& operator() (int i) const
    {
        return as[i];
    }

private:
    const System& s;
    std::vector<SMatrix> as;
};

/**
 * Hamiltonian of the system.
 *
 * The heart of the fermionic quantum system. Although it is an
 * operator it behaves a little bit different than the other operators, which
 * maybe doesn't entirely come as a surprise. It is
 * not a function object. It has to be subclassed and
 * overwritten for each different physical system. Typical usage:
 * @code
 * Hamiltonian H(mySystem);
 * H.construct();
 * H.diagonalize(); //here the real work happens
 * H.eigenvalues(); //use eigenenergies
 * @endcode
 *
 * @tparam System
 */
template<class System>
class Hamiltonian
{
protected:
    class SortIndicesAccordingToSizeOfRanges
    {
    public:
        SortIndicesAccordingToSizeOfRanges (const std::vector<Range>& rs_)
            : rs(rs_)
        {}

        bool operator() (size_t i, size_t j) const
        {
            return (rs[i].b-rs[i].a)>(rs[j].b-rs[j].a);
        }

    private:
        const std::vector<Range>& rs;
    };

    System& s;
    SMatrix H;
    DVector es; // eigenvalues
    // SMatrix vs; // eigenvalues, unused
    std::vector<DVector> es_s; // eigenvalues, sector-wise
    std::vector<DMatrix> vs_s; // eigenvectors, sector-wise
    double E_min;

public:
    Hamiltonian (System& s_)
    : s(s_), H(), E_min(0)
    {}

    /**
     * Construct the Hamiltonian matrix.
     *
     * Has to be overwritten by deriving classes.
     */
    void construct() {}

    /**
     * Diagonalizes the Hamiltonian matrix using symmetries.
     *
     * The Hamiltonian is diagonalized block-wise. Consequently, eigenvalues and
     * eigenvectors should be accessed via the eigenvaluesBySector and
     * eigenvectorsBySector methods.
     */
    void diagonalize ()
    {
        es.resize(0);
        const std::vector<Range>& rs = s.basis.getRanges();
        es_s.resize(rs.size());
        vs_s.resize(rs.size());
        std::vector<size_t> is(rs.size());
        for (size_t i=0; i<is.size(); i++)
            is[i] = i;
        std::sort(is.begin(), is.end(), SortIndicesAccordingToSizeOfRanges(rs));

        const size_t n_largest = rs[is[0]].b-rs[is[0]].a;
        SelfAdjointEigenSolver<DMatrix> s(n_largest);

        for (size_t i=0; i<is.size(); i++)
        {
            const int a = rs[is[i]].a;
            const int b = rs[is[i]].b;
            // useful debug output when diagonalizing very large Hamiltonians
            // std::cerr << "-> " << "Diagonalizing range " << is[i] << " out of "
            //           << rs.size() << " ranges." << std::endl
            //           << "-> Size of range " << is[i] << ": " << b-a << std::endl;
            assert(4E9 > (b-a)*(b-a)*sizeof(double));
            s.compute(H.block(a,a,b-a,b-a));
            es_s[is[i]] = s.eigenvalues();
            vs_s[is[i]] = s.eigenvectors();
        }
        E_min = es_s[0].minCoeff();
        for (size_t i=1; i<es_s.size(); i++)
            if (es_s[i].minCoeff() < E_min)
                E_min = es_s[i].minCoeff();
    }

    const std::vector<DVector>& eigenvaluesBySector ()
    {
        return es_s;
    }

    const std::vector<DMatrix>& eigenvectorsBySector ()
    {
        return vs_s;
    }

    const DVector& eigenvalues ()
    {
        if (es.size() == 0 && es_s.size() > 0)
        {
            es.resize(H.rows());
            int k=0;
            for (size_t i=0; i<es_s.size(); i++)
                for (int j=0; j<es_s[i].size(); j++)
                {
                    assert(k<es.size());
                    es(k) = es_s[i](j);
                    k++;
                }
        }
        return es;
    }

    double Emin () const
    {
        return E_min;
    }
};

/**
 * Calculate the ensemble average "by sectors".
 *
 * "By sectors" essentially means that the eigenvalues are accessed as an
 * std::vector of dense vectors and the eigenvectors are accessed as an
 * std::vector of dense matrices. Hence this class should be used whenever
 * Hamiltonian::diagonlize is used, for best performance. Example usage:
 * @code
 * EnsembleAverage ensembleAverage(mySystem);
 * MyFunkyOperator O(mySystem);
 * ensembleAverage(beta, O); //will expect and use mySystem.H
 * @endcode
 *
 * Dependencies: System.basis and System.H (Hamiltonian)
 *
 * @tparam System
 */
template<class System>
class EnsembleAverage
{
public:
    EnsembleAverage (System& s_)
    : s(s_)
    {}

    /**
     * Calculate the ensemble average for the given operator.
     *
     * @param beta = 1/T (temperature)
     * @param O operator matrix
     *
     * @return
     */
    double operator() (double beta, const SMatrix& O) const
    {
        double sum = 0;
        size_t index = 0;
        const std::vector<DVector>& eigenvalues = s.H.eigenvaluesBySector();
        const std::vector<DMatrix>& eigenvectors = s.H.eigenvectorsBySector();
        for (size_t i=0; i<eigenvalues.size(); i++)
        {
            // note: usually O is very sparse, so it is essential to use a
            // sparse matrix for the block
            const int size = eigenvalues[i].size();
            const SMatrix& O_block = O.block(index,index,size,size);
            for (int j=0; j<size; j++)
                sum +=
                    std::exp(-beta * (eigenvalues[i](j) - s.H.Emin())) *
                    eigenvectors[i].col(j).adjoint() * O_block * eigenvectors[i].col(j);
            index += size;
        }
        return sum / partitionFunction(beta);
    }

    /**
     * Calculate the partition function at the given temperature.
     *
     * @param beta = 1/T (temperature)
     *
     * @return
     */
    double partitionFunction (double beta) const
    {
        double Z = 0;
        const std::vector<DVector>& eigenvalues = s.H.eigenvaluesBySector();
        for (size_t i=0; i<eigenvalues.size(); i++)
            for (int j=0; j<eigenvalues[i].size(); j++)
                Z += std::exp(-beta * (eigenvalues[i](j) - s.H.Emin()));
        return Z;
    }

private:
    System& s;
};

#endif // __SYSTEM_HPP__


/// NTL Wrappers
///
/// The functions in this module wrap a select few functions for lattice
/// reduction in NTL. They take matrices with 'int64_t' elements, but use
/// unlimited precision internally.

#include <climits>
#include <string>
#include <fstream>
#include <stdexcept>

#include <boost/numeric/ublas/matrix.hpp>
#include "NTL/matrix.h"
#include "NTL/ZZ.h"
#include "NTL/LLL.h"

#include "debug.h"
#include "types.h"

#include "ntl_wrapper.h"

using std::string;
using std::ifstream;
using std::runtime_error;
using boost::numeric::ublas::matrix;
using NTL::Mat;
using NTL::Vec;
using NTL::ZZ;

/////////////////////////////////////////////////////////////////////
/// LLL reduction

/// LLL reduction using exact arithmetic
///
/// This is the default used in BLT.
int64 LLL(matrix<int64>& B, matrix<int64>& U) {
    Mat<ZZ> ntlB;
    Mat<ZZ> ntlU;
    ZZ det2;

    matrix_to_ntl(B, ntlB);
    TRACE("lattice reduction method: LLL");
    int64 rank = NTL::LLL(det2, ntlB, ntlU);

    ZZ det = SqrRoot(det2);
    TRACE("reduced lattice det: " << det);

    ntl_to_matrix(ntlB, B);
    ntl_to_matrix(ntlU, U);
    return rank;
}

/// LLL_FP wrapper
///
/// This method is generally faster than 'LLL', but uses floating point
/// arithmetic.
int64 LLL_FP(matrix<int64>& B, matrix<int64>& U) {
    Mat<ZZ> ntlB;
    Mat<ZZ> ntlU;

    matrix_to_ntl(B, ntlB);
    TRACE("lattice reduction method: LLL_FP");
    int64 rank = NTL::LLL_FP(ntlB, ntlU);

    ZZ det;
    determinant(det, ntlB);
    TRACE("reduced lattice det: " << det);

    ntl_to_matrix(ntlB, B);
    ntl_to_matrix(ntlU, U);
    return rank;
}

/// G_LLL_FP wrapper
///
/// This method is more stable than 'LLL_FP', but may take longer.
int64 G_LLL_FP(matrix<int64>& B, matrix<int64>& U) {
    Mat<ZZ> ntlB;
    Mat<ZZ> ntlU;

    matrix_to_ntl(B, ntlB);
    TRACE("lattice reduction method: G_LLL_FP");
    int64 rank = NTL::G_LLL_FP(ntlB, ntlU);

    ZZ det;
    determinant(det, ntlB);
    TRACE("reduced lattice det: " << det);

    ntl_to_matrix(ntlB, B);
    ntl_to_matrix(ntlU, U);
    return rank;
}

/////////////////////////////////////////////////////////////////////
/// Utilities

/// Estimate the number of solutions of a given lattice constraint pair.
///
/// If 'inv == true' then return value is ~1/expected_solns.
long expected_solns(const matrix<int64>& L, const matrix<int64>& C, bool& inv) {
    Mat<ZZ> ntlL;
    ZZ det;
    matrix_to_ntl(L, ntlL);

    // det computation must be done using unlimited precision
    determinant(det, ntlL);
    det = abs(det);

    // volume computation must be done using unlimited precision
    int64 r, maxr = 0;
    ZZ vol(1L);
    for (size_t i = 0; i < C.size1(); ++i) {
        r = C(i,1) - C(i,0);
        if (r <= 0)
            TRACE("i=" << i << " C(i,0)=" << C(i,0) << " C(i,1)=" << C(i,1));
        assert(r > 0 && "a constraint is inconsistent");
        vol *= r;
        if (r > maxr)
            maxr = r;
    }
    TRACE("orthotope radius: " << (double)maxr/2.0);

    ZZ q;
    if (det <= vol) {
        inv = false;
        q = vol / det;
    } else {
        inv = true;
        q = det / vol;
    }
    TRACE("NTL expected solutions = " << (inv ? "1 / " : "") << q);
    if (q >= LONG_MAX)
        return LONG_MAX;
    return to_long(q);
}

/////////////////////////////////////////////////////////////////////
// Conversion

// copy a matrix m into a mat_ZZ `n`; `n` is resized.
void matrix_to_ntl(const matrix<int64>& m, Mat<ZZ>& n) {
    size_t r = m.size1();  // num rows
    size_t c = m.size2();  // num columns
    if (r == 0 || c == 0) {
        n.SetDims(0,0);
        return;
    }
    n.SetDims(r, c);
    for (size_t i=0; (int)i != n.NumRows(); i++) {
        for (size_t j=0; (int)j != n.NumCols(); j++) {
            n[i][j] = m(i, j);  // int64 -> ZZ coercion
        }
    }
}

// vice-versa: copy a Mat<ZZ> to a matrix<int64> with possible
// loss of precision
void ntl_to_matrix(const Mat<ZZ>& N, matrix<int64>& M) {
    size_t r = N.NumRows();
    size_t c = N.NumCols();
    M.resize(r, c);
    for (size_t i=0; i != M.size1(); i++) {
        for (size_t j=0; j != M.size2(); j++) {
            M(i, j) = to_long(N[i][j]);  // ZZ -> int64 coercion
        }
    }
}

/////////////////////////////////////////////////////////////////////
// Misc

// Return a view of n consisting of the first c columns
Mat<ZZ> take_cols(const Mat<ZZ>& n, size_t c) {
    if ((int) c > n.NumCols())
        return n;
    size_t r = n.NumRows();
    Mat<ZZ> ret;
    ret.SetDims(r, c);
    for (size_t i=0; i != r; i++) {
        Vec<ZZ> t;
        t.SetLength(c);
        for (size_t j=0; j != c; j++) {
            t[j] = n[i][j];
        }
        ret[i] = t;
    }
    return ret;
}

// Read a lattice from file into a Mat<ZZ> using NTLs >> operators
Mat<ZZ> read_lattice_ntl(string infile) {
    ifstream in(infile.c_str());
    if (!in)
        throw runtime_error("could not open input file: " + infile);
    Mat<ZZ> N;
    in >> N;
    if (!in)
        throw runtime_error("could not read matrix from input file: " + infile);
    return N;
}


#include <iostream>
#include <Eigen/Dense>
#include <vector>
using Eigen::MatrixXd;
using Eigen::Vector3d;
int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << m << std::endl;

 Vector3d v(0,0,0);

 std::vector<Vector3d> vv;

 for(int i=0;i<10;i++){
    v(0)=i;
    vv.push_back(v);
 }

    for(int i=0;i<10;i++){
     std::cout << i << ":" << vv[i] << std::endl;
    }
}


#ifndef BOOST_METAPARSE_GETTING_STARTED_5_2_4_HPP
#define BOOST_METAPARSE_GETTING_STARTED_5_2_4_HPP

// Automatically generated header file

// Definitions before section 5.2.3
#include "5_2_3.hpp"

// Definitions of section 5.2.3
#include <boost/metaparse/foldl.hpp>

using exp_parser10 =
 build_parser<
   transform<
     sequence<
       int_token,
       foldl<
         sequence<plus_token, int_token>,
         boost::mpl::int_<0>,
         boost::mpl::quote2<sum_items>
       >
     >,
     boost::mpl::quote1<sum_vector>>
 >;

#endif


#pragma once

#include "loader.hpp"
#include <Eigen/Dense>
#include <any>
#include <chrono>
#include <map>
#include <random>
#include <unordered_set>
#include <vector>

class Config;
class SpTensor_Hash;
class SpTensor_dX;
class DataStream;

class TensorStream;
TensorStream* generateTensorStream(DataStream& paperX, const Config& config); // Generate the tensor stream

class TensorStream {
public:
    TensorStream(DataStream& paperX, const Config& config);
    virtual ~TensorStream(void);

    void updateTensor(const DataStream::Event& e);
    void updateFactor(void);

    double elapsedTime(void) const; // sec

    double find_reconst(const std::vector<int>& coord) const;
    double density(void) const;

    /* Load matrix */
    void saveFactor(std::string fileName) const;

    /* Get errors */
    double rmse(void) const;
    double fitness(void) const;
    double fitness_latest(void) const;
    double error(const std::vector<int>& coord) const; // Error of the given entry

    void updateAtA(void); // Update the AtA when _use_AtA is false

protected:
    const Config* _config;

    std::vector<int> _compute_order;

    virtual void _updateAlgorithm(void) {} // It will change the current updateAlgorithm later

    double _norm_frobenius_reconst(void) const;
    double _innerprod_X_X_reconst(void) const;

    SpTensor_Hash* _X = nullptr;
    SpTensor_dX* _dX = nullptr;
    DataStream* _paperX;

    Eigen::ArrayXd _lambda;
    std::vector<Eigen::MatrixXd> _A;
    std::vector<Eigen::ArrayXXd> _AtA;

    bool _use_AtA = true;

    long long _nextTime;

    std::chrono::nanoseconds _elapsed_time; // Elapsed time

    void _rand_init_A(void); // Randomly initialize factor matrices

    void _als_base(void); // Base code for ALS
    void _unnormalize_A(void); // Unnormalize the factor matrices

    /* Basic factor update algorithms */
    void _als(void);
    void _recurrent_als(void);
};

/* General ALS until the convergence */
class TensorStream_ALS : public TensorStream {
public:
    TensorStream_ALS(DataStream& paperX, const Config& config)
        : TensorStream(paperX, config)
    {
    }
    virtual ~TensorStream_ALS(void) {}

protected:
    virtual void _updateAlgorithm(void) override
    {
        _als();
    }
};

class TensorStream_GD : public TensorStream {
public:
    TensorStream_GD(DataStream& paperX, const Config& config)
        : TensorStream(paperX, config)
        , _lr(_config->findAlgoSettings<double>("learningRate"))
    {
    }
    virtual ~TensorStream_GD(void) {}

protected:
    virtual void _updateAlgorithm(void) override;

    const double _lr; // learning rate
};

class TensorStream_SGD : public TensorStream_GD {
public:
    TensorStream_SGD(DataStream& paperX, const Config& config);
    virtual ~TensorStream_SGD(void) {}

protected:
    virtual void _updateAlgorithm(void) override;

    // Sample entries with replacement including the currently updated entries.
    // It returns the number of sampled entries.
    int _sampleEntry(std::unordered_set<std::vector<int>>& sampledIdx) const;
    void _compute_gradA(std::vector<std::unordered_map<int, Eigen::MatrixXd>>& gradA) const;

    const int _numSample; // The number of samples
};

class TensorStream_Momentum : public TensorStream_SGD {
public:
    TensorStream_Momentum(DataStream& paperX, const Config& config);
    virtual ~TensorStream_Momentum(void) {}

protected:
    virtual void _updateAlgorithm(void) override;

    const double _momentum;
    const double _momentumNew;
    std::vector<Eigen::MatrixXd> _V;
};

class TensorStream_RMSProp : public TensorStream_SGD {
public:
    TensorStream_RMSProp(DataStream& paperX, const Config& config);
    virtual ~TensorStream_RMSProp(void) {}

protected:
    virtual void _updateAlgorithm(void) override;

    const double _decay;
    std::vector<Eigen::VectorXd> _G;
};

class TensorStream_Adam : public TensorStream_SGD {
public:
    TensorStream_Adam(DataStream& paperX, const Config& config);
    virtual ~TensorStream_Adam(void) {}

protected:
    virtual void _updateAlgorithm(void) override;

    const double _beta1;
    const double _beta1New;
    const double _beta2;

    std::vector<Eigen::MatrixXd> _M;
    std::vector<Eigen::VectorXd> _V;

    std::vector<Eigen::VectorXd> _t; // The number of update
};


//==============================================================================
//          Copyright 2015 - J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SIGNIFICANTS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SIGNIFICANTS_HPP_INCLUDED

#include <nt2/exponential/functions/significants.hpp>
#include <nt2/include/functions/scalar/round.hpp>
#include <nt2/include/functions/scalar/tenpower.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/if_zero_else.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_gtz.hpp>
#include <nt2/include/functions/scalar/log10.hpp>
#include <nt2/include/functions/scalar/minus.hpp>
#include <nt2/include/functions/scalar/iceil.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/assert.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/scalar/if_else.hpp>
#include <nt2/include/functions/scalar/is_invalid.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT( significants_, tag::cpu_
                          , (A0)(A1)
                          , ((scalar_< floating_<A0>>))
                            ((scalar_< integer_<A1>>))
                          )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      BOOST_ASSERT_MSG( assert_all(is_gtz(a1))
                      , "Number of significant digits must be positive"
                      );
      typedef typename boost::dispatch::meta::as_integer<A0>::type iA0;
      if (is_eqz(a0)) return a0;
      iA0 exp = a1 - iceil(log10(abs(a0)));
      A0 fac = tenpower(exp);
      A0 scaled = round(a0*fac);
#ifndef BOOST_SIMD_NO_INVALIDS
      A0 r = if_else(is_invalid(a0), a0, scaled/fac);
#else
      A0 r =  scaled/fac;
#endif
      return r;
    }
  };

} }

#endif


/*
 *
 * Copyright (c) Andreas Kloeckner 2004
 *               Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GEEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GEEV_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
// #include <boost/numeric/bindings/traits/std_vector.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a general matrix A * V = V * D
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * geev() computes the eigendecomposition of a N x N matrix,
     * where V is a N x N matrix and D is a diagonal matrix. The
     * diagonal element D(i,i) is an eigenvalue of A and Q(:,i) is
     * a corresponding eigenvector.
     *
     *
     * int geev (char jobz, char uplo, A& a, W& w, V* vl, V* vr, optimal_workspace);
     *
     * a is the matrix whose eigendecomposition you're interested in. (input)
     *
     * w contains the diagonal of D, above. w must always be complex. (output)
     *
     * vl is an N x N matrix containing the left eigenvectors of a in its
     * columns. See remark on complex vs. real below. May be left NULL to indicate
     * that you do not want left eigenvectors.
     *
     * vr is an N x N matrix containing the right ("usual") eigenvectors of a in its
     * columns. See remark on complex vs. real below. As a matrix, vr fulfills
     * A * VR = VR * D. (except if real, see below). May be left NULL to indicate
     * that you do not want right eigenvectors.
     *
     *
     * For real A, vr and vl may be either complex or real, at your option.
     * If you choose to leave them real, you have to pick apart the complex-conjugate
     * eigenpairs as per the LAPACK documentation. If you choose them complex,
     * the code will do the picking-apart on your behalf, at the expense of 4*N
     * extra storage. Only if vr is complex, it will really fulfill its invariant
     * on exit to the code in all cases, since complex pairs spoil that relation.
     */

    namespace detail {

      inline
      int geev_backend(const char* jobvl, const char* jobvr, const int* n, float* a,
	       const int* lda, float* wr, float* wi, float* vl, const int* ldvl,
	       float* vr, const int* ldvr, float* work, const int* lwork)
      {
	int info;
	LAPACK_SGEEV(jobvl, jobvr, n, a, lda, wr, wi, vl, ldvl, vr, ldvr, work, lwork, &info);
	return info;
      }

      inline
      int geev_backend(const char* jobvl, const char* jobvr, const int* n, double* a,
	       const int* lda, double* wr, double* wi, double* vl, const int* ldvl,
	       double* vr, const int* ldvr, double* work, const int* lwork)
      {
	int info;
	LAPACK_DGEEV(jobvl, jobvr, n, a, lda, wr, wi, vl, ldvl, vr, ldvr, work, lwork, &info);
	return info;
      }

      inline
      int geev_backend(const char* jobvl, const char* jobvr, const int* n, traits::complex_f* a,
	       const int* lda, traits::complex_f* w, traits::complex_f* vl, const int* ldvl,
	       traits::complex_f* vr, const int* ldvr, traits::complex_f* work, const int* lwork,
	       float* rwork)
      {
	int info;
	LAPACK_CGEEV(jobvl, jobvr, n,
		     traits::complex_ptr(a), lda,
		     traits::complex_ptr(w),
		     traits::complex_ptr(vl), ldvl,
		     traits::complex_ptr(vr), ldvr,
		     traits::complex_ptr(work), lwork,
		     rwork, &info);
	return info;
      }

      inline
      int geev_backend(const char* jobvl, const char* jobvr, const int* n, traits::complex_d* a,
	       const int* lda, traits::complex_d* w, traits::complex_d* vl, const int* ldvl,
	       traits::complex_d* vr, const int* ldvr, traits::complex_d* work, const int* lwork,
	       double* rwork)
      {
	int info;
	LAPACK_ZGEEV(jobvl, jobvr, n,
		     traits::complex_ptr(a), lda,
		     traits::complex_ptr(w),
		     traits::complex_ptr(vl), ldvl,
		     traits::complex_ptr(vr), ldvr,
		     traits::complex_ptr(work), lwork,
		     rwork, &info);
	return info;
      }

      struct real_case {};
      struct mixed_case {};
      struct complex_case {};

      // real case
      template <typename A, typename W, typename V>
      int geev(real_case, const char jobvl, const char jobvr, A& a, W& w,
	       V* vl, V *vr)
      {
	int const n = traits::matrix_size1(a);
	typedef typename A::value_type value_type;
	traits::detail::array<value_type> wr(n);
	traits::detail::array<value_type> wi(n);

	traits::detail::array<value_type> vl2(vl ? 0 : n);
	traits::detail::array<value_type> vr2(vr ? 0 : n);
	value_type* vl_real = vl ? traits::matrix_storage(*vl) : vl2.storage();
	const int ldvl = vl ? traits::matrix_size2(*vl) : 1;
	value_type* vr_real = vr ? traits::matrix_storage(*vr) : vr2.storage();
	const int ldvr = vr ? traits::matrix_size2(*vr) : 1;

	// workspace query
	int lwork = -1;
	value_type work_temp;
	int result = geev_backend(&jobvl, &jobvr, &n,
				  traits::matrix_storage(a), &n,
				  wr.storage(), wi.storage(),
				  vl_real, &ldvl, vr_real, &ldvr,
				  &work_temp, &lwork);
	if (result != 0)
	  return result;

	lwork = (int) work_temp;
	traits::detail::array<value_type> work(lwork);
	result = geev_backend(&jobvl, &jobvr, &n,
			      traits::matrix_storage(a), &n,
			      wr.storage(), wi.storage(),
			      vl_real, &ldvl, vr_real, &ldvr,
			      work.storage(), &lwork);

	for (int i = 0; i < n; i++)
	  traits::vector_storage(w)[i] = std::complex<value_type>(wr[i], wi[i]);
	return result;
      }

      // mixed (i.e. real with complex vectors) case
      template <typename A, typename W, typename V>
      int geev(mixed_case, const char jobvl, const char jobvr, A& a, W& w,
	       V* vl, V *vr)
      {
	int const n = traits::matrix_size1(a);
	typedef typename A::value_type value_type;
	traits::detail::array<value_type> wr(n);
	traits::detail::array<value_type> wi(n);

	traits::detail::array<value_type> vl2(vl ? n*n : n);
	traits::detail::array<value_type> vr2(vr ? n*n : n);
	const int ldvl2 = vl ? n : 1;
	const int ldvr2 = vr ? n : 1;

	// workspace query
	int lwork = -1;
	value_type work_temp;
	int result = geev_backend(&jobvl, &jobvr, &n,
				  traits::matrix_storage(a), &n,
				  wr.storage(), wi.storage(),
				  vl2.storage(), &ldvl2, vr2.storage(), &ldvr2,
				  &work_temp, &lwork);
	if (result != 0)
	  return result;

	lwork = (int) work_temp;
	traits::detail::array<value_type> work(lwork);
	result = geev_backend(&jobvl, &jobvr, &n,
			      traits::matrix_storage(a), &n,
			      wr.storage(), wi.storage(),
			      vl2.storage(), &ldvl2, vr2.storage(), &ldvr2,
			      work.storage(), &lwork);

	typedef typename V::value_type vec_value_type;
	vec_value_type* vl_stor = NULL;
	vec_value_type* vr_stor = NULL;
	int ldvl = 0, ldvr = 0;
	if (vl)
	{
	  vl_stor = traits::matrix_storage(*vl);
	  ldvl = traits::matrix_size2(*vl);
	}
	if (vr)
	{
	  vr_stor = traits::matrix_storage(*vr);
	  ldvr = traits::matrix_size2(*vr);
	}

	for (int i = 0; i < n; i++)
        {
          traits::vector_storage(w)[i] = std::complex<value_type>(wr[i], wi[i]);
	  if (wi[i] != 0)
	  {
	    assert(i+1 < n);
	    assert(wr[i+1] == wr[i]);
	    assert(wi[i+1] == -wi[i]);

            traits::vector_storage(w)[i+1] = std::complex<value_type>(wr[i+1], wi[i+1]);
	    for (int j = 0; j < n; j++)
	    {
	      if (vl)
	      {
		vl_stor[i*ldvl+j] = std::complex<value_type>(vl2[i*n+j], vl2[(i+1)*n+j]);
		vl_stor[(i+1)*ldvl+j] = std::complex<value_type>(vl2[i*n+j], -vl2[(i+1)*n+j]);
	      }
	      if (vr)
	      {
		vr_stor[i*ldvr+j] = std::complex<value_type>(vr2[i*n+j], vr2[(i+1)*n+j]);
		vr_stor[(i+1)*ldvr+j] = std::complex<value_type>(vr2[i*n+j], -vr2[(i+1)*n+j]);
	      }
	    }

	    i++;
	  }
	  else
	  {
	    for (int j = 0; j < n; j++)
	    {
	      if (vl)
		vl_stor[i*ldvl+j] = vl2[i*n+j];
	      if (vr)
		vr_stor[i*ldvr+j] = vr2[i*n+j];
	    }
	  }
	}
	return result;
      }

      // complex case
      template <typename A, typename W, typename V>
      int geev(complex_case, const char jobvl, const char jobvr, A& a, W& w,
	       V* vl, V *vr)
      {
	typedef typename A::value_type value_type;
	typedef typename traits::type_traits<value_type>::real_type real_type;

	int const n = traits::matrix_size1(a);
	traits::detail::array<real_type> rwork(2*n);

	traits::detail::array<value_type> vl2(vl ? 0 : n);
	traits::detail::array<value_type> vr2(vr ? 0 : n);
	value_type* vl_real = vl ? traits::matrix_storage(*vl) : vl2.storage();
	const int ldvl = vl ? traits::matrix_size2(*vl) : 1;
	value_type* vr_real = vr ? traits::matrix_storage(*vr) : vr2.storage();
	const int ldvr = vr ? traits::matrix_size2(*vr) : 1;

	// workspace query
	int lwork = -1;
	value_type work_temp;
	int result = geev_backend(&jobvl, &jobvr, &n,
				  traits::matrix_storage(a), &n,
				  traits::vector_storage(w),
				  vl_real, &ldvl, vr_real, &ldvr,
				  &work_temp, &lwork, rwork.storage());
	if (result != 0)
	  return result;

	lwork = (int) std::real(work_temp);
	traits::detail::array<value_type> work(lwork);
	result = geev_backend(&jobvl, &jobvr, &n,
			      traits::matrix_storage(a), &n,
			      traits::vector_storage(w),
			      vl_real, &ldvl, vr_real, &ldvr,
			      work.storage(), &lwork,
			      rwork.storage());

	return result;
      }

    } // namespace detail

    // gateway / dispatch routine
    template <typename A, typename W, typename V>
    int geev(A& a, W& w,  V* vl, V* vr, optimal_workspace)
    {
      // input checking
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
			   typename traits::matrix_traits<A>::matrix_structure,
			   traits::general_t
			   >::value));
#endif

#ifndef NDEBUG
      int const n = traits::matrix_size1(a);
#endif

      assert(traits::matrix_size2(a)==n);
      assert(traits::vector_size(w)==n);
      assert(traits::vector_size(w)==n);
      assert(!vr || traits::matrix_size1(*vr)==n);
      assert(!vl || traits::matrix_size1(*vl)==n);

      // preparation
      typedef typename A::value_type value_type;
      typedef typename V::value_type vec_value_type;
      typedef typename traits::type_traits<value_type>::real_type real_type;

      // dispatch
      return detail::geev(typename boost::mpl::if_<
			  boost::is_same<value_type, real_type>,
			  typename boost::mpl::if_<
			  boost::is_same<vec_value_type, real_type>,
			  detail::real_case,
			  detail::mixed_case>::type,
			  detail::complex_case>::type(),
			  vl != 0 ? 'V' : 'N',
			  vr != 0 ? 'V' : 'N',
			  a, w, vl, vr);
    }

  }

}}}

#endif


#include <tiny_math_types.h>
#include <tiny_coordsys_functions.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(tiny_coordsys_prod);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef tiny::MathTypes<double> math_types;

  typedef math_types::value_traits     value_traits;
  typedef math_types::vector3_type     V;
  typedef math_types::quaternion_type  Q;
  typedef math_types::coordsys_type    X;

  {
    X I;
    I.identity();

    X L;
    L.T() = V::make(1.0,2.0,3.0);
    L.Q() = Q::Ru( value_traits::pi(), V::make(1.0, 2.0, 3.0) );

    X R = tiny::prod( L, I );

    BOOST_CHECK( fabs( R.T()(0) - L.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - L.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - L.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().real() - L.Q().real() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(0) - L.Q().imag()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(1) - L.Q().imag()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(2) - L.Q().imag()(2) ) < 10e-10 );
  }
  {
    X I;
    I.identity();

    X L;
    L.T() = V::make(1.0,2.0,3.0);
    L.Q() = Q::Ru( value_traits::pi(), V::make(1.0, 2.0, 3.0) );

    X R = tiny::prod( I, L );

    BOOST_CHECK( fabs( R.T()(0) - L.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - L.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - L.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().real() - L.Q().real() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(0) - L.Q().imag()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(1) - L.Q().imag()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(2) - L.Q().imag()(2) ) < 10e-10 );
  }
  {
    X I;
    I.identity();

    X R = tiny::prod( I, I );

    BOOST_CHECK( fabs( R.T()(0) - I.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - I.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - I.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().real() - I.Q().real() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(0) - I.Q().imag()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(1) - I.Q().imag()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(2) - I.Q().imag()(2) ) < 10e-10 );
  }
  {
    X I;
    I.identity();

    X L;
    L.T() = V::make(1.0,2.0,3.0);
    L.Q() = Q::Ru( value_traits::pi(), V::make(1.0, 2.0, 3.0) );

    X invL = tiny::inverse( L );
    X R = tiny::prod( invL, L );

    BOOST_CHECK( fabs( R.T()(0) - I.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - I.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - I.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().real() - I.Q().real() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(0) - I.Q().imag()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(1) - I.Q().imag()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().imag()(2) - I.Q().imag()(2) ) < 10e-10 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


// File: multi_vector.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    typedef dense_vector<double>   Vector;

    Vector                      v(2, 3.4), w(3, 2.5);
    mtl::multi_vector<Vector> 	A(2, 3);
    dense2D<double>		B(2,2), C(3,2), D(3,3);

    // Initialize matrices
    A= 3.0; B= 4.0; C= 5.0; D= 6.0;

    // vector= multi_vector * vector
    v= A * w;

    // vector= transposed multi_vector * vector
    w= trans(A) * v;

    // vector= matrix * vector
    v= B * A.vector(1);

    // vector= matrix * vector
    A.vector(0)= B * A.vector(1);

    // Orthogonalize multi_vector
    orth(A);

    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_PUBKEY_SHAMIR_SSS_HPP
#define CRYPTO3_PUBKEY_SHAMIR_SSS_HPP

#include <vector>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <iterator>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

#include <nil/crypto3/random/algebraic_random_device.hpp>

#include <nil/crypto3/pubkey/operations/deal_shares_op.hpp>
#include <nil/crypto3/pubkey/operations/reconstruct_secret_op.hpp>
#include <nil/crypto3/pubkey/operations/reconstruct_public_secret_op.hpp>

#include <nil/crypto3/pubkey/keys/share_sss.hpp>
#include <nil/crypto3/pubkey/keys/secret_sss.hpp>
#include <nil/crypto3/pubkey/keys/public_share_sss.hpp>
#include <nil/crypto3/pubkey/keys/public_secret_sss.hpp>

#include <nil/crypto3/pubkey/secret_sharing/weighted_basic_policy.hpp>

namespace nil {
    namespace crypto3 {
        namespace pubkey {
            template<typename Group>
            struct shamir_sss : public sss_weighted_basic_policy<Group> {
                typedef Group group_type;
                typedef sss_basic_policy<group_type> basic_policy;

                //===========================================================================
                // secret sharing scheme output types

                typedef std::vector<typename basic_policy::coeff_type> coeffs_type;
                typedef std::vector<typename basic_policy::public_coeff_type> public_coeffs_type;

                static inline typename basic_policy::private_element_type
                    eval_basis_poly(const typename basic_policy::indexes_type &indexes, std::size_t i) {
                    assert(basic_policy::check_participant_index(i));

                    typename basic_policy::private_element_type e_i(i);
                    typename basic_policy::private_element_type result = basic_policy::private_element_type::one();

                    for (auto j : indexes) {
                        if (j != i) {
                            result = result * (typename basic_policy::private_element_type(j) /
                                               (typename basic_policy::private_element_type(j) - e_i));
                        }
                    }
                    return result;
                }

                //===========================================================================
                // TODO: refactor
                // polynomial generation functions

                static inline coeffs_type get_poly(std::size_t t, std::size_t n) {
                    assert(basic_policy::check_threshold_value(t, n));

                    return get_poly(t);
                }

                template<
                    typename Generator = random::algebraic_random_device<typename basic_policy::coeff_type::field_type>,
                    typename Distribution = void>
                static inline coeffs_type get_poly(std::size_t t) {
                    assert(basic_policy::check_minimal_size(t));

                    coeffs_type coeffs;
                    Generator gen;
                    for (std::size_t i = 0; i < t; i++) {
                        coeffs.emplace_back(gen());
                    }
                    return coeffs;
                }

                //===========================================================================
                // TODO: refactor
                // general purposes functions

                template<typename Coeffs>
                static inline public_coeffs_type get_public_coeffs(const Coeffs &coeffs) {
                    BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const Coeffs>));

                    return get_public_coeffs(std::cbegin(coeffs), std::cend(coeffs));
                }

                template<typename CoeffsIt>
                static inline public_coeffs_type get_public_coeffs(CoeffsIt first, CoeffsIt last) {
                    BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIt>));
                    assert(basic_policy::check_minimal_size(std::distance(first, last)));

                    public_coeffs_type public_coeffs;
                    for (auto it = first; it != last; it++) {
                        public_coeffs.emplace_back(basic_policy::get_public_element(*it));
                    }
                    return public_coeffs;
                }
            };

            template<typename Group>
            struct public_share_sss<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef typename scheme_type::indexed_public_element_type public_share_type;
                typedef public_share_type data_type;
                typedef typename public_share_type::first_type index_type;
                typedef typename public_share_type::second_type value_type;

                public_share_sss() = default;

                public_share_sss(std::size_t i) : public_share(i, public_share_type::second_type::zero()) {
                    assert(scheme_type::check_participant_index(get_index()));
                }

                public_share_sss(const public_share_type &in_public_share) : public_share(in_public_share) {
                    assert(scheme_type::check_participant_index(get_index()));
                }

                public_share_sss(std::size_t i, const typename public_share_type::second_type &ps) :
                    public_share(i, ps) {
                    assert(scheme_type::check_participant_index(get_index()));
                }

                inline index_type get_index() const {
                    return public_share.first;
                }

                inline const value_type &get_value() const {
                    return public_share.second;
                }

                inline const data_type &get_data() const {
                    return public_share;
                }

                bool operator==(const public_share_sss &other) const {
                    return this->public_share == other.public_share;
                }

                bool operator<(const public_share_sss &other) const {
                    return this->get_index() < other.get_index();
                }

            protected:
                public_share_type public_share;
            };

            template<typename Group>
            struct share_sss<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef typename scheme_type::indexed_private_element_type share_type;
                typedef share_type data_type;
                typedef typename share_type::first_type index_type;
                typedef typename share_type::second_type value_type;

                share_sss() = default;

                share_sss(std::size_t i) : share(i, share_type::second_type::zero()) {
                    assert(scheme_type::check_participant_index(get_index()));
                }

                share_sss(const share_type &in_share) : share(in_share) {
                    assert(scheme_type::check_participant_index(get_index()));
                }

                share_sss(std::size_t i, const typename share_type::second_type &s) : share(i, s) {
                    assert(scheme_type::check_participant_index(get_index()));
                }

                inline index_type get_index() const {
                    return share.first;
                }

                inline const value_type &get_value() const {
                    return share.second;
                }

                inline const data_type &get_data() const {
                    return share;
                }

                template<
                    typename Scheme,
                    typename std::enable_if<
                        std::is_convertible<typename std::remove_cv<typename std::remove_reference<Scheme>::type>::type,
                                            scheme_type>::value,
                        bool>::type = true>
                operator public_share_sss<Scheme>() const {
                    using To = public_share_sss<Scheme>;

                    return To(share.first, share.second * To::public_share_type::second_type::one());
                }

                bool operator==(const share_sss &other) const {
                    return this->share == other.share;
                }

                bool operator<(const share_sss &other) const {
                    return this->get_index() < other.get_index();
                }

                //
                //  0 <= k < t
                //
                inline void update(const typename scheme_type::coeff_type &coeff, std::size_t exp) {
                    assert(scheme_type::check_exp(exp));

                    share.second =
                        share.second + coeff * typename scheme_type::private_element_type(share.first).pow(exp);
                }

            protected:
                share_type share;
            };

            template<typename Group>
            struct public_secret_sss<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef typename scheme_type::public_element_type public_secret_type;
                typedef typename scheme_type::indexes_type indexes_type;
                typedef public_secret_type value_type;

                template<typename PublicShares>
                public_secret_sss(const PublicShares &public_shares) :
                    public_secret_sss(std::cbegin(public_shares), std::cend(public_shares)) {
                }

                template<typename PublicShareIt>
                public_secret_sss(PublicShareIt first, PublicShareIt last) :
                    public_secret(reconstruct_public_secret(first, last)) {
                }

                template<typename PublicShares>
                public_secret_sss(const PublicShares &public_shares, const indexes_type &indexes) :
                    public_secret_sss(std::cbegin(public_shares), std::cend(public_shares), indexes) {
                }

                template<typename PublicShareIt>
                public_secret_sss(PublicShareIt first, PublicShareIt last, const indexes_type &indexes) :
                    public_secret(reconstruct_public_secret(first, last, indexes)) {
                }

                inline const value_type &get_value() const {
                    return public_secret;
                }

                bool operator==(const public_secret_sss &other) const {
                    return this->public_secret == other.public_secret;
                }

                bool operator<(const public_secret_sss &other) const {
                    return this->public_secret < other.public_secret;
                }

            private:
                template<
                    typename PublicShareIt,
                    typename std::enable_if<
                        std::is_convertible<typename std::remove_cv<typename std::remove_reference<
                                                typename std::iterator_traits<PublicShareIt>::value_type>::type>::type,
                                            public_share_sss<scheme_type>>::value,
                        bool>::type = true>
                static inline public_secret_type reconstruct_public_secret(PublicShareIt first, PublicShareIt last) {
                    BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<PublicShareIt>));

                    return reconstruct_public_secret(first, last, scheme_type::get_indexes(first, last));
                }

                template<
                    typename PublicShareIt,
                    typename std::enable_if<
                        std::is_convertible<typename std::remove_cv<typename std::remove_reference<
                                                typename std::iterator_traits<PublicShareIt>::value_type>::type>::type,
                                            public_share_sss<scheme_type>>::value,
                        bool>::type = true>
                static inline public_secret_type reconstruct_public_secret(PublicShareIt first, PublicShareIt last,
                                                                           const indexes_type &indexes) {
                    BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<PublicShareIt>));

                    public_secret_type public_secret = public_secret_type::zero();
                    for (auto it = first; it != last; it++) {
                        public_secret =
                            public_secret + it->get_value() * scheme_type::eval_basis_poly(indexes, it->get_index());
                    }

                    return public_secret;
                }

                public_secret_type public_secret;
            };

            template<typename Group>
            struct secret_sss<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef typename scheme_type::private_element_type secret_type;
                typedef typename scheme_type::indexes_type indexes_type;
                typedef secret_type value_type;

                template<typename Shares>
                secret_sss(const Shares &shares) : secret_sss(std::cbegin(shares), std::cend(shares)) {
                }

                template<typename ShareIt>
                secret_sss(ShareIt first, ShareIt last) : secret(reconstruct_secret(first, last)) {
                }

                template<typename Shares>
                secret_sss(const Shares &shares, const indexes_type &indexes) :
                    secret_sss(std::cbegin(shares), std::cend(shares), indexes) {
                }

                template<typename ShareIt>
                secret_sss(ShareIt first, ShareIt last, const indexes_type &indexes) :
                    secret(reconstruct_secret(first, last, indexes)) {
                }

                inline const value_type &get_value() const {
                    return secret;
                }

                bool operator==(const secret_sss &other) const {
                    return this->secret == other.secret;
                }

                bool operator<(const secret_sss &other) const {
                    return this->secret < other.secret;
                }

                template<
                    typename Scheme,
                    typename std::enable_if<
                        std::is_convertible<typename std::remove_cv<typename std::remove_reference<Scheme>::type>::type,
                                            scheme_type>::value,
                        bool>::type = true>
                operator public_secret_sss<Scheme>() const {
                    using To = public_secret_sss<Scheme>;

                    return To(secret * To::public_secret_type::one());
                }

            protected:
                template<typename ShareIt,
                         typename std::enable_if<
                             std::is_convertible<typename std::remove_cv<typename std::remove_reference<
                                                     typename std::iterator_traits<ShareIt>::value_type>::type>::type,
                                                 share_sss<scheme_type>>::value,
                             bool>::type = true>
                static inline secret_type reconstruct_secret(ShareIt first, ShareIt last) {
                    BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<ShareIt>));

                    return reconstruct_secret(first, last, scheme_type::get_indexes(first, last));
                }

                template<typename ShareIt,
                         typename std::enable_if<
                             std::is_convertible<typename std::remove_cv<typename std::remove_reference<
                                                     typename std::iterator_traits<ShareIt>::value_type>::type>::type,
                                                 share_sss<scheme_type>>::value,
                             bool>::type = true>
                static inline secret_type reconstruct_secret(ShareIt first, ShareIt last, const indexes_type &indexes) {
                    BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<ShareIt>));

                    secret_type secret = secret_type::zero();
                    for (auto it = first; it != last; it++) {
                        secret = secret + it->get_value() * scheme_type::eval_basis_poly(indexes, it->get_index());
                    }

                    return secret;
                }

                secret_type secret;
            };

            template<typename Group>
            struct deal_shares_op<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef share_sss<scheme_type> share_type;
                typedef std::vector<share_type> shares_type;
                typedef shares_type internal_accumulator_type;
                typedef shares_type result_type;

            protected:
                template<typename Share, typename InternalAccumulator>
                static inline void _init_accumulator(InternalAccumulator &acc, std::size_t n, std::size_t t) {
                    assert(scheme_type::check_threshold_value(t, n));

                    std::size_t i = 1;
                    std::generate_n(std::inserter(acc, std::end(acc)), n, [&i]() { return Share(i++); });
                }

                template<typename Scheme, typename InternalAccumulator>
                static inline void _update(InternalAccumulator &acc, std::size_t exp,
                                           const typename Scheme::coeff_type &coeff) {
                    for (auto shares_iter = std::begin(acc); shares_iter != std::end(acc); ++shares_iter) {
                        shares_iter->update(coeff, exp);
                    }
                }

                template<typename ResultType, typename InternalAccumulator>
                static inline ResultType _process(InternalAccumulator &acc) {
                    return acc;
                }

            public:
                static inline void init_accumulator(internal_accumulator_type &acc, std::size_t n, std::size_t t) {
                    _init_accumulator<share_type>(acc, n, t);
                }

                static inline void update(internal_accumulator_type &acc, std::size_t exp,
                                          const typename scheme_type::coeff_type &coeff) {
                    _update<scheme_type>(acc, exp, coeff);
                }

                static inline result_type process(internal_accumulator_type &acc) {
                    return _process<result_type>(acc);
                }
            };

            template<typename Group>
            struct reconstruct_public_secret_op<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef public_share_sss<scheme_type> public_share_type;
                typedef public_secret_sss<scheme_type> public_secret_type;
                typedef std::pair<typename scheme_type::indexes_type, std::set<public_share_type>>
                    internal_accumulator_type;
                typedef public_secret_type result_type;

            protected:
                template<typename InternalAccumulator, typename PublicShare>
                static inline void _update(InternalAccumulator &acc, const PublicShare &public_share) {
                    bool emplace_status = acc.first.emplace(public_share.get_index()).second;
                    assert(emplace_status);
                    // acc.second.push_back(public_share);
                    emplace_status = acc.second.emplace(public_share).second;
                    assert(emplace_status);
                }

                template<typename ResultType, typename InternalAccumulator>
                static inline ResultType _process(InternalAccumulator &acc) {
                    return ResultType(acc.second /*, acc.first*/);
                }

            public:
                static inline void init_accumulator() {
                }

                static inline void update(internal_accumulator_type &acc, const public_share_type &public_share) {
                    _update(acc, public_share);
                }

                static inline result_type process(internal_accumulator_type &acc) {
                    return _process<result_type>(acc);
                }
            };

            template<typename Group>
            struct reconstruct_secret_op<shamir_sss<Group>> {
                typedef shamir_sss<Group> scheme_type;
                typedef share_sss<scheme_type> share_type;
                typedef secret_sss<scheme_type> secret_type;
                typedef std::pair<typename scheme_type::indexes_type, std::set<share_type>> internal_accumulator_type;
                typedef secret_type result_type;

            protected:
                template<typename InternalAccumulator, typename Share>
                static inline void _update(InternalAccumulator &acc, const Share &share) {
                    bool emplace_status = acc.first.emplace(share.get_index()).second;
                    assert(emplace_status);
                    // acc.second.push_back(public_share);
                    emplace_status = acc.second.emplace(share).second;
                    assert(emplace_status);
                }

                template<typename ResultType, typename InternalAccumulator>
                static inline ResultType _process(InternalAccumulator &acc) {
                    return ResultType(acc.second /*, acc.first*/);
                }

            public:
                static inline void init_accumulator() {
                }

                static inline void update(internal_accumulator_type &acc, const share_type &share) {
                    _update(acc, share);
                }

                static inline result_type process(internal_accumulator_type &acc) {
                    return _process<result_type>(acc);
                }
            };
        }    // namespace pubkey
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_PUBKEY_SHAMIR_SSS_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_EXPX2_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_EXPX2_HPP_INCLUDED
#include <nt2/exponential/functions/expx2.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/constants/expx2c1.hpp>
#include <nt2/include/constants/expx2c2.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/maxlog.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/floor.hpp>
#include <nt2/include/functions/scalar/fma.hpp>
#include <nt2/include/functions/scalar/signnz.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/scalar/is_inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( expx2_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (nt2::is_inf(a0)) return nt2::Inf<A0>();
      #endif
      A0 x =  nt2::abs(a0);
      /* Represent x as an exact multiple of 1/32 plus a residual.  */
      A0 m = nt2::Expx2c1<A0>() * nt2::floor(nt2::Expx2c2<A0>() * x + Half<A0>());
      x -= m;
      /* x**2 = m**2 + 2mf + f**2 */
      A0 u = sqr(m);
      A0 u1 = nt2::Two<A0>() * m * x  +  nt2::sqr(x);
      if ((u+u1) > nt2::Maxlog<A0>()) return nt2::Inf<A0>();
      /* u is exact, u1 is small.  */
      return nt2::exp(u) * nt2::exp(u1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( expx2_, tag::cpu_
                            , (A0)
                            , ((scalar_<floating_<A0> >))
                              ((scalar_<floating_<A0> >))
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0& a0,  const A0 & s) const
    {
      A0 sgn =  signnz(s);
      A0 x =  a0*sgn;
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (nt2::is_inf(a0)) return nt2::Inf<A0>();
      #endif
      // Represent x as an exact multiple of 1/32 plus a residual.
      A0 m = Expx2c1<A0>()*nt2::floor(fma(Expx2c2<A0>(), x, nt2::Half<A0>()));
      A0 f =  x-m;
      // x**2 = m**2 + 2mf + f**2
      A0 u = sgn*nt2::sqr(m);
      A0 u1 = sgn*fma(m+m,f,sqr(f));
      // u is exact, u1 is small.
      if (u+u1 >  nt2::Maxlog<A0>()) return nt2::Inf<A0>();
      return nt2::exp(u)*nt2::exp(u1);
    }
  };
} }

#endif


#include <Eigen/Dense>
#include <fmt/core.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <ostream>
#include <string>
#include <vector>

struct Line {
  std::array<int, 2> x = {};
  std::array<int, 2> y = {};
};

std::ostream &operator<<(std::ostream &os, Line const &l) {
  os << "[(" << l.y[0] << "," << l.x[0] << "),(" << l.y[1] << "," << l.x[1]
     << ")]";
  return os;
}

Line parseLine(std::string_view v) {
  auto first_comma = v.find_first_of(',');
  std::string first_int(v.data(), v.data() + first_comma);
  Line l;
  l.x[0] = std::stoi(first_int);

  auto first_space = v.find_first_of(' ');
  std::string second_int(v.data() + first_comma + 1, first_space);
  l.y[0] = std::stoi(second_int);

  auto last_space = v.find_last_of(' ');
  auto last_comma = v.find_last_of(',');
  std::string third_int(v.data() + last_space + 1, v.data() + last_comma);
  l.x[1] = std::stoi(third_int);

  std::string fourth_int(v.data() + last_comma + 1, v.data() + v.size());
  l.y[1] = std::stoi(fourth_int);

  return l;
}

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<Line> out;
  while (std::getline(file, line)) {
    out.push_back(parseLine(line));
  }

  return out;
}

auto maxExtent(std::vector<Line> const &ls) {
  auto const &l =
      *std::max_element(ls.begin(), ls.end(), [](Line const &a, Line const &b) {
        return std::max({a.x[0], a.x[1], a.y[0], a.y[1]}) <
               std::max({b.x[0], b.x[1], b.y[0], b.y[1]});
      });
  return std::max({l.x[0], l.x[1], l.y[0], l.y[1]});
}

int main(int _, char **argv) {
  auto input = parseFile(argv[1]);
  auto max_extent = maxExtent(input);
  fmt::print("Max extent {}\n", max_extent);
  Eigen::MatrixXi M(max_extent + 1, max_extent + 1);
  M.setZero();

  for (auto const &l : input) {
    if (l.x[0] == l.x[1]) {
      auto col = M.col(l.x[0]);
      auto y = l.y;
      std::sort(y.begin(), y.end());
      for (auto i = y[0]; i <= y[1]; ++i) {
        ++col(i);
      }
    } else if (l.y[0] == l.y[1]) {
      auto row = M.row(l.y[0]);
      auto x = l.x;
      std::sort(x.begin(), x.end());
      for (auto i = x[0]; i <= x[1]; ++i) {
        ++row(i);
      }
    } else {
      auto len = std::max(l.x[0], l.x[1]) - std::min(l.x[0], l.x[1]) + 1;
      auto ymove = (l.y[0] < l.y[1]) ? 1 : -1;
      auto xmove = (l.x[0] < l.x[1]) ? 1 : -1;
      auto x = l.x[0];
      auto y = l.y[0];
      for (auto i = 0; i < len; ++i) {
        ++M(y, x);
        x += xmove;
        y += ymove;
      }
    }
  }

  if (max_extent < 20) {
    for(auto r = 0; r < max_extent + 1; ++r){
      for(auto c = 0; c < max_extent + 1; ++c){
        if(M(r,c) == 0){
          std::cout << ". ";
        } else {
          std::cout << M(r,c) << " ";
        }
      }
      std::cout << "\n";
    }
  }

  auto count = 0;
  for (auto i = 0; i < M.size(); ++i) {
    if (M.data()[i] > 1) {
      ++count;
    }
  }

  fmt::print("Num overlap {}\n", count);
}


#include <boost/test/unit_test.hpp>

#include "math/algo/dp.h" // header to test

using namespace biosim;

size_t fibonacci_function(math::tensor<size_t> const &__input, std::vector<size_t> const &__pos) {
  if(__pos.size() != 1) {
    throw std::invalid_argument("fibonacci numbers cannot be calculated for multidimensional positions");
  }
  return __pos[0] < 2 ? 1 : __input({(__pos[0] - 1)}) + __input({(__pos[0] - 2)});
}

BOOST_AUTO_TEST_SUITE(suite_dp)

BOOST_AUTO_TEST_CASE(dp_fib) {
  math::algo::dp<size_t> fib;
  math::tensor<size_t> fibonacci_numbers(fib.calculate(math::tensor<size_t>({10}), &fibonacci_function));
  BOOST_CHECK(fibonacci_numbers({9}) == 55);
  BOOST_REQUIRE_THROW(fib.calculate(math::tensor<size_t>({10, 3}), &fibonacci_function), std::invalid_argument);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef PARTICLE_HPP
#define PARTICLE_HPP

#define _USE_MATH_DEFINES
#include <math.h>

#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include "constants.hpp"
#include "functions.hpp"

using namespace Eigen;

class Particle {
	private:

	public:

		Particle(const int &N_LM);

		void AddNewLM(const MatrixXd &z, const MatrixXd &Q);
		double ComputeWeight(const MatrixXd &z, const MatrixXd &Q);
		void UpdateLM(const MatrixXd &z, const MatrixXd &Q);

		double w_ = 0.0;
		double x_ = 0.0;
		double y_ = 0.0;
		double yaw_ = 0.0;

		//landmark x-y postion
		MatrixXd lm_;

		//landmarks position covariance
		MatrixXd lmp_;
};

std::array<MatrixXd,4> ComputeJacobian(const Particle* p, const MatrixXd &xf, const MatrixXd &pf, const MatrixXd &Q);
void UpdateKF(MatrixXd &xf, MatrixXd &pf, const MatrixXd &v, const MatrixXd &Q, const MatrixXd &Hf);

#endif // PARTICLE_HPP