#define CATCH_CONFIG_MAIN

#include "snake_geometry.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include "catch.hpp"

using namespace snake_geometry;
using namespace std;


TEST_CASE( "Test toENU() and fromENU()", "[WGS84]" ) {
    GeoPoint3D ref{48.230612, 16.297824, 0};
    GeoPoint3D poi{48.231159, 16.298406, 2};

    Point3D zero;
    toENU(ref, ref, zero);
    REQUIRE( zero[0] == Approx(0));
    REQUIRE( zero[1] == Approx(0));
    REQUIRE( zero[2] == Approx(0));

    Point3D poiENU;
    toENU(ref, poi, poiENU);
    GeoPoint3D poi_same;
    fromENU(ref, poiENU, poi_same);

    Point3D diff;
    toENU(poi_same, poi, diff);

    REQUIRE( abs(diff[0]) <= 1e6);
    REQUIRE( abs(diff[1]) <= 1e6);
    REQUIRE( abs(diff[2]) <= 1e6);
}

TEST_CASE( "Test polygonCenter(), check out polygonCenter0.svg!" ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{100, 0});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPoint center;
    polygonCenter(polygon, center);


    // Write results to svg file.
    std::ofstream svg("polygonCenter0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(center);
    mapper.add(polygon);
    mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5);

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(center) << endl;

    // Center inside polygon?
    REQUIRE(boost::geometry::within(center, polygon) == true);
}


TEST_CASE( "Test polygonCenter(), check out polygonCenter1.svg!" ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{50, 0});
    polygon.outer().push_back(BoostPoint{50, 50});
    polygon.outer().push_back(BoostPoint{100, 50});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPoint center;
    polygonCenter(polygon, center);

    // Write results to svg file.
    std::ofstream svg("polygonCenter1.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(center);
    mapper.add(polygon);
    mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5);

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(center) << endl;

    // Center inside polygon?
    REQUIRE(boost::geometry::within(center, polygon) == true);
}


TEST_CASE( "Test polygonCenter(), check out polygonCenter2.svg!" ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{20, 0});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{100, 50});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPoint center;
    polygonCenter(polygon, center);


    // Convert to boost.

    // Write results to svg file.
    std::ofstream svg("polygonCenter2.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(center);
    mapper.add(polygon);
    mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5);

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(center) << endl;

    // Center inside polygon?
    REQUIRE(boost::geometry::within(center, polygon) == true);
}

TEST_CASE( "Test polygonCenter(), check out polygonCenter3.svg!" ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPoint center;
    polygonCenter(polygon, center);

    // Write results to svg file.
    std::ofstream svg("polygonCenter3.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(center);
    mapper.add(polygon);
    mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(center, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5);

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(center) << endl;

    // Center inside polygon?
    REQUIRE(boost::geometry::within(center, polygon) == true);
}

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox0.svg!" ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    min_bbox_rt bbox;
    minimalBoundingBox(polygon, bbox);

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(bbox.corners);
    mapper.add(polygon);
    mapper.map(bbox.corners,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(bbox.corners) << endl;
}

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox1.svg! Rotated polygon." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    // Rotate polygon
    boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(45);
    BoostPolygon rotated_polygon;
    boost::geometry::transform(polygon, rotated_polygon, rotate);

    min_bbox_rt bbox;
    minimalBoundingBox(rotated_polygon, bbox);

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox1.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(bbox.corners);
    mapper.add(rotated_polygon);
    mapper.map(bbox.corners,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(rotated_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(rotated_polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(bbox.corners) << endl;
}

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox2.svg! Non convex." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{30, 50});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    // Rotate polygon
    boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(45);
    BoostPolygon rotated_polygon;
    boost::geometry::transform(polygon, rotated_polygon, rotate);

    min_bbox_rt bbox;
    minimalBoundingBox(rotated_polygon, bbox);

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox2.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(bbox.corners);
    mapper.add(rotated_polygon);
    mapper.map(bbox.corners,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(rotated_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(rotated_polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(bbox.corners) << endl;
}

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox3.svg! Rotated polygon." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{10, 70});
    polygon.outer().push_back(BoostPoint{30, 50});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    // Rotate polygon
    boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(45);
    BoostPolygon rotated_polygon;
    boost::geometry::transform(polygon, rotated_polygon, rotate);

    min_bbox_rt bbox;
    minimalBoundingBox(rotated_polygon, bbox);

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox3.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(bbox.corners);
    mapper.add(rotated_polygon);
    mapper.map(bbox.corners,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(rotated_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    // Print to standard output.
    //cout << boost::geometry::wkt<polygon_type>(rotated_polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(bbox.corners) << endl;
}

TEST_CASE( "Various tests with empty polygons" ) {
    BoostPolygon empty_polygon;
    min_bbox_rt bbox;
    minimalBoundingBox(empty_polygon, bbox);
    BoostPoint center;
    polygonCenter(empty_polygon, center);
}

TEST_CASE( "Test isClockwise()" ) {
    Point2DList list1{Point2D{0,0},
                      Point2D{0,1},
                      Point2D{1,1}};
    REQUIRE(isClockwise(list1) == true);
    Point2DList list2{Point2D{0,0},
                      Point2D{1,1},
                      Point2D{0,1}};
    REQUIRE(isClockwise(list2) == false);
}

TEST_CASE( "Test offsetPolygon(), positive." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{30, 50});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, 10);


    // Write results to svg file.
    std::ofstream svg("offsetPolygon0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygon);
    mapper.add(polygonOffset);
    mapper.map(polygon,       "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    REQUIRE(bg::within(polygon, polygonOffset) == true);

}

TEST_CASE( "Test offsetPolygon(), negative." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{30, 50});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, -10);


    // Write results to svg file.
    std::ofstream svg("offsetPolygon1.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygon);
    mapper.add(polygonOffset);
    mapper.map(polygon,       "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    REQUIRE(bg::within(polygonOffset, polygon) == true);

}

TEST_CASE( "Test offsetPolygon(), positive, huge (miter test)." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{70, 0});
    polygon.outer().push_back(BoostPoint{30, 50});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{0, 40});
    polygon.outer().push_back(BoostPoint{0, 0});
    bg::correct(polygon);

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, 100);


    // Write results to svg file.
    std::ofstream svg("offsetPolygon2.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygon);
    mapper.add(polygonOffset);
    mapper.map(polygon,       "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");

    REQUIRE(bg::within(polygon, polygonOffset) == true);
}

TEST_CASE( "Test dijkstraAlgorithm() 1." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{100, 0});
    polygon.outer().push_back(BoostPoint{80, 0});
    polygon.outer().push_back(BoostPoint{50, 30});
    polygon.outer().push_back(BoostPoint{20, 0});
    polygon.outer().push_back(BoostPoint{0, 0});

    BoostPoint startVertex{10, 10};
    BoostPoint endVertex{90, 10};

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, 0.1);

    BoostLineString vertices;
    vertices.push_back(startVertex);
    vertices.push_back(endVertex);
    for (size_t i=0; i < polygon.outer().size()-1; ++i)
        vertices.push_back(polygon.outer()[i]);
    size_t n = vertices.size();

    Matrix<double> graph(n, n);
    graphFromPolygon(polygonOffset, vertices, graph);
//    cout << "graph:" << endl;
//    for (size_t i=0; i<n; ++i){
//        for (size_t j=0; j<n; ++j){
//            cout << graph[i][j] << " ";
//        }
//        cout << endl;
//    }


    std::vector<size_t> pathIndices;
    auto distance = [graph](size_t i, size_t j){ return graph.get(i, j); };
//    cout << "distance():" << endl;
//    for (size_t i=0; i<n; ++i){
//        for (size_t j=0; j<n; ++j){
//            cout << distance(i, j) << " ";
//        }
//        cout << endl;
//    }

    REQUIRE(dijkstraAlgorithm(n, 0, 1, pathIndices, distance));

    BoostLineString path;
    for (auto idx : pathIndices)
        path.push_back(vertices[idx]);


    REQUIRE(bg::within(path, polygonOffset));



    // Write results to svg file.
    std::ofstream svg("dijkstraAlgorithm0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygonOffset);
    mapper.add(path);
    mapper.add(startVertex);
    mapper.add(endVertex);
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(path,          "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(startVertex,   "fill-opacity:0.5;fill:rgb(250, 231, 25);stroke:rgb(250, 231, 25);stroke-width:2", 2);
    mapper.map(endVertex,     "fill-opacity:0.5;fill:rgb(25, 250, 243);stroke:rgb(25, 250, 243);stroke-width:2", 2);
}

TEST_CASE( "Test dijkstraAlgorithm() 2, more complex Polygon." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{100, 0});
    polygon.outer().push_back(BoostPoint{80, 0});
    polygon.outer().push_back(BoostPoint{80, 30});
    polygon.outer().push_back(BoostPoint{95, 30});
    polygon.outer().push_back(BoostPoint{95, 80});
    polygon.outer().push_back(BoostPoint{20, 80});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{40, 50});
    polygon.outer().push_back(BoostPoint{40, 30});
    polygon.outer().push_back(BoostPoint{40, 0});
    polygon.outer().push_back(BoostPoint{0, 0});

    BoostPoint startVertex{10, 10};
    BoostPoint endVertex{90, 10};

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, 0.1);

    BoostLineString vertices;
    vertices.push_back(startVertex);
    vertices.push_back(endVertex);
    for (size_t i=0; i < polygon.outer().size()-1; ++i)
        vertices.push_back(polygon.outer()[i]);
    size_t n = vertices.size();
    Matrix<double> graph(n, n);
    graphFromPolygon(polygonOffset, vertices, graph);
//    cout << "graph:" << endl;
//    for (size_t i=0; i<n; ++i){
//        for (size_t j=0; j<n; ++j){
//            cout << graph[i][j] << " ";
//        }
//        cout << endl;
//    }


    std::vector<size_t> pathIndices;
    auto distance = [graph](size_t i, size_t j){ return graph.get(i, j); };
//    cout << "distance():" << endl;
//    for (size_t i=0; i<n; ++i){
//        for (size_t j=0; j<n; ++j){
//            cout << distance(i, j) << " ";
//        }
//        cout << endl;
//    }

    REQUIRE(dijkstraAlgorithm(n, 0, 1, pathIndices, distance));

    BoostLineString path;
    for (auto idx : pathIndices)
        path.push_back(vertices[idx]);


    REQUIRE(bg::within(path, polygonOffset));



    // Write results to svg file.
    std::ofstream svg("dijkstraAlgorithm1.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygonOffset);
    mapper.add(path);
    mapper.add(startVertex);
    mapper.add(endVertex);
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(path,          "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(startVertex,   "fill-opacity:0.5;fill:rgb(250, 231, 25);stroke:rgb(250, 231, 25);stroke-width:2", 2);
    mapper.map(endVertex,     "fill-opacity:0.5;fill:rgb(25, 250, 243);stroke:rgb(25, 250, 243);stroke-width:2", 2);
}

TEST_CASE( "Test dijkstraAlgorithm() 3, performance Test." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{100, 0});
    polygon.outer().push_back(BoostPoint{80, 0});
    polygon.outer().push_back(BoostPoint{80, 30});
    polygon.outer().push_back(BoostPoint{95, 30});
    polygon.outer().push_back(BoostPoint{95, 80});
    polygon.outer().push_back(BoostPoint{20, 80});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{40, 50});
    polygon.outer().push_back(BoostPoint{40, 30});
    polygon.outer().push_back(BoostPoint{40, 0});
    polygon.outer().push_back(BoostPoint{0, 0});

    BoostPoint startVertex{10, 10};
    BoostPoint endVertex{90, 10};

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, 0.1);

    BoostLineString vertices;
    vertices.push_back(startVertex);
    vertices.push_back(endVertex);
    for (size_t i=0; i < polygon.outer().size()-1; ++i)
        vertices.push_back(polygon.outer()[i]);
    //  add extra load...
    size_t n = 100;
    size_t lim = n-vertices.size();
    BoostPoint v{10,90};
    for (size_t i=0; i < lim; ++i)
        vertices.push_back(v);
    Matrix<double> graph(n, n);

    auto start = std::chrono::high_resolution_clock::now();
    graphFromPolygon(polygonOffset, vertices, graph);
    cout << "Execution time graphFromPolygon() n = " << n << ": ";
    cout << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start).count();
    cout << " ms"  << endl;
//    cout << "graph:" << endl;
//    for (size_t i=0; i<n; ++i){
//        for (size_t j=0; j<n; ++j){
//            cout << graph[i][j] << " ";
//        }
//        cout << endl;
//    }


    std::vector<size_t> pathIndices;
    auto distance = [graph](size_t i, size_t j){ return graph.get(i, j); };
//    cout << "distance():" << endl;
//    for (size_t i=0; i<n; ++i){
//        for (size_t j=0; j<n; ++j){
//            cout << distance(i, j) << " ";
//        }
//        cout << endl;
//    }


    start = std::chrono::high_resolution_clock::now();
    REQUIRE(dijkstraAlgorithm(n, 0, 1, pathIndices, distance));
    cout << "Execution time dijkstraAlgorithm() n = " << n << ": ";
    cout << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start).count();
    cout << " ms"  << endl;

    BoostLineString path;
    for (auto idx : pathIndices)
        path.push_back(vertices[idx]);


    REQUIRE(bg::within(path, polygonOffset));



    // Write results to svg file.
    std::ofstream svg("dijkstraAlgorithm2.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygonOffset);
    mapper.add(path);
    mapper.add(startVertex);
    mapper.add(endVertex);
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(path,          "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(startVertex,   "fill-opacity:0.5;fill:rgb(250, 231, 25);stroke:rgb(250, 231, 25);stroke-width:2", 2);
    mapper.map(endVertex,     "fill-opacity:0.5;fill:rgb(25, 250, 243);stroke:rgb(25, 250, 243);stroke-width:2", 2);
}

TEST_CASE( "Test shortestPathFromGraph() 1." ) {
    BoostPolygon polygon;
    polygon.outer().push_back(BoostPoint{0, 0});
    polygon.outer().push_back(BoostPoint{0, 100});
    polygon.outer().push_back(BoostPoint{100, 100});
    polygon.outer().push_back(BoostPoint{100, 0});
    polygon.outer().push_back(BoostPoint{80, 0});
    polygon.outer().push_back(BoostPoint{80, 30});
    polygon.outer().push_back(BoostPoint{95, 30});
    polygon.outer().push_back(BoostPoint{95, 80});
    polygon.outer().push_back(BoostPoint{20, 80});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{20, 50});
    polygon.outer().push_back(BoostPoint{40, 50});
    polygon.outer().push_back(BoostPoint{40, 30});
    polygon.outer().push_back(BoostPoint{40, 0});
    polygon.outer().push_back(BoostPoint{0, 0});

    BoostPoint startVertex{10, 10};
    BoostPoint endVertex{90, 10};

    BoostPolygon polygonOffset;
    offsetPolygon(polygon, polygonOffset, 0.2);

    BoostLineString vertices;
    vertices.push_back(startVertex);
    vertices.push_back(endVertex);
    for (size_t i=0; i < polygon.outer().size()-1; ++i)
        vertices.push_back(polygon.outer()[i]);
    size_t n = vertices.size();

    Matrix<double> graph(n, n);
    graphFromPolygon(polygonOffset, vertices, graph);

    std::vector<size_t> pathIndices;
    shortestPathFromGraph(graph, 0, 1, pathIndices);

    BoostLineString path;
    for (auto idx : pathIndices)
        path.push_back(vertices[idx]);



    // Write results to svg file.
    std::ofstream svg("shortestPathFromGraph0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 200, 200);
    mapper.add(polygonOffset);
    mapper.add(path);
    mapper.add(startVertex);
    mapper.add(endVertex);
    mapper.map(polygonOffset, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(path,          "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(startVertex,   "fill-opacity:0.5;fill:rgb(250, 231, 25);stroke:rgb(250, 231, 25);stroke-width:2", 2);
    mapper.map(endVertex,     "fill-opacity:0.5;fill:rgb(25, 250, 243);stroke:rgb(25, 250, 243);stroke-width:2", 2);

    REQUIRE(bg::within(path, polygonOffset));
}