#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;


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);
}
