#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include "snake_geometry.h"

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

using namespace snake_geometry;
using namespace std;

typedef boost::geometry::model::d2::point_xy<double> point_type;
typedef boost::geometry::model::polygon<point_type> polygon_type;


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);
    REQUIRE( zero[0] == Approx(0));
    REQUIRE( zero[1] == Approx(0));
    REQUIRE( zero[2] == Approx(0));

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

    Point3D diff = toENU(poi_same, poi);

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

TEST_CASE( "Test polygonCenter(), check out polygonCenter0.svg!" ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{100, 0},
                        Point2D{100, 100},
                        Point2D{0, 100}};
    Point2D center = polygonCenter(polygon);


    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }
    point_type boost_center(center[0], center[1]);

    // Write results to svg file.
    std::ofstream svg("polygonCenter0.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_center);
    mapper.add(boost_polygon);
    mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(boost_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>(boost_polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(boost_center) << endl;

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


TEST_CASE( "Test polygonCenter(), check out polygonCenter1.svg!" ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{50, 0},
                        Point2D{50, 50},
                        Point2D{100, 50},
                        Point2D{100, 100},
                        Point2D{0, 100}};
    Point2D center = polygonCenter(polygon);


    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }
    point_type boost_center(center[0], center[1]);

    // Write results to svg file.
    std::ofstream svg("polygonCenter1.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_center);
    mapper.add(boost_polygon);
    mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(boost_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>(boost_polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(boost_center) << endl;

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


TEST_CASE( "Test polygonCenter(), check out polygonCenter2.svg!" ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{20, 0},
                        Point2D{20, 50},
                        Point2D{100, 50},
                        Point2D{100, 100},
                        Point2D{0, 100}};
    Point2D center = polygonCenter(polygon);


    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }
    point_type boost_center(center[0], center[1]);

    // Write results to svg file.
    std::ofstream svg("polygonCenter2.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_center);
    mapper.add(boost_polygon);
    mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(boost_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>(boost_polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(boost_center) << endl;

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

TEST_CASE( "Test polygonCenter(), check out polygonCenter3.svg!" ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{70, 0},
                        Point2D{100, 100},
                        Point2D{0, 100}};
    Point2D center = polygonCenter(polygon);


    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }
    point_type boost_center(center[0], center[1]);

    // Write results to svg file.
    std::ofstream svg("polygonCenter3.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_center);
    mapper.add(boost_polygon);
    mapper.map(boost_polygon, "fill-opacity:0.1;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    mapper.map(boost_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>(boost_polygon) << endl;
    //cout << boost::geometry::wkt<point_type>(boost_center) << endl;

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

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox0.svg!" ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{70, 0},
                        Point2D{100, 100},
                        Point2D{0, 100}};

    min_bbox_rt bbox = minimalBoundingBox(polygon);


    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }

    polygon_type boost_bbox;
    for (auto vertex : bbox.corners) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_bbox.outer().push_back(boost_vertex);
    }

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox0.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_bbox);
    mapper.add(boost_polygon);
    mapper.map(boost_bbox,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(boost_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>(boost_polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(boost_bbox) << endl;

    //REQUIRE(boost::geometry::within(boost_polygon, boost_bbox) == true);
}

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox1.svg! Rotated polygon." ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{70, 0},
                        Point2D{100, 100},
                        Point2D{0, 100}};

    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }

    // Rotate polygon
    boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(45);
    polygon_type rotated_boost_polygon;
    boost::geometry::transform(boost_polygon, rotated_boost_polygon, rotate);
    Point2DList rotated_polygon;
    auto outer = rotated_boost_polygon.outer();
    for (auto vertex : outer){
        rotated_polygon.push_back(Point2D{vertex.get<0>(), vertex.get<1>()});
    }


    min_bbox_rt bbox = minimalBoundingBox(rotated_polygon);

    polygon_type boost_bbox;
    for (auto vertex : bbox.corners) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_bbox.outer().push_back(boost_vertex);
    }

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox1.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_bbox);
    mapper.add(rotated_boost_polygon);
    mapper.map(boost_bbox,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(rotated_boost_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_boost_polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(boost_bbox) << endl;

    //REQUIRE(boost::geometry::within(rotated_boost_polygon, boost_bbox) == true);
}

TEST_CASE( "Test minimalBoundingBox(), check out minimalBoundingBox2.svg! Non convex." ) {
    Point2DList polygon{Point2D{0, 0},
                        Point2D{70, 0},
                        Point2D{30, 50},
                        Point2D{100, 100},
                        Point2D{0, 100}};

    // Convert to boost.
    polygon_type boost_polygon;
    for (auto vertex : polygon) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_polygon.outer().push_back(boost_vertex);
    }

    // Rotate polygon
    boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(45);
    polygon_type rotated_boost_polygon;
    boost::geometry::transform(boost_polygon, rotated_boost_polygon, rotate);
    Point2DList rotated_polygon;
    auto outer = rotated_boost_polygon.outer();
    for (auto vertex : outer){
        rotated_polygon.push_back(Point2D{vertex.get<0>(), vertex.get<1>()});
    }


    min_bbox_rt bbox = minimalBoundingBox(rotated_polygon);

    polygon_type boost_bbox;
    for (auto vertex : bbox.corners) {
        point_type boost_vertex = point_type{vertex[0], vertex[1]};
        boost_bbox.outer().push_back(boost_vertex);
    }

    // Write results to svg file.
    std::ofstream svg("minimalBoundingBox2.svg");
    boost::geometry::svg_mapper<point_type> mapper(svg, 200, 200);
    mapper.add(boost_bbox);
    mapper.add(rotated_boost_polygon);
    mapper.map(boost_bbox,    "fill-opacity:0.1;fill:rgb(255,0,0);stroke:rgb(255,0,0);stroke-width:2");
    mapper.map(rotated_boost_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_boost_polygon) << endl;
    //cout << boost::geometry::wkt<polygon_type>(boost_bbox) << endl;

    //REQUIRE(boost::geometry::within(rotated_boost_polygon, boost_bbox) == true);
}