#include <chrono>
#include <iostream>
#include <fstream>
#include <time.h>

#include "snake.h"


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


#include "catch.hpp"

using namespace snake;
using namespace std;

//===========================================================================================
// Scenario
//===========================================================================================
TEST_CASE( "Test Scenario Class, check out Scenario0.svg" ) {
    // Data
    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
                                     GeoPoint2D{47.768205, 16.530558},
                                     GeoPoint2D{47.768147, 16.530566},
                                     GeoPoint2D{47.768326, 16.530926},
                                     GeoPoint2D{47.768135, 16.531025},
                                     GeoPoint2D{47.768144, 16.530845},
                                     GeoPoint2D{47.768103, 16.530978},
                                     GeoPoint2D{47.767940, 16.530393}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::MeasurementArea};

    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
                                     GeoPoint2D{47.767968, 16.530677},
                                     GeoPoint2D{47.767860, 16.530723},
                                     GeoPoint2D{47.767777, 16.530447}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::ServiceArea};

    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
                                        GeoPoint2D{47.768004, 16.530382},
                                        GeoPoint2D{47.768035, 16.530549},
                                        GeoPoint2D{47.767880, 16.530629}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::Corridor};

    Scenario scenario;
    scenario.addArea(measurementArea);
    scenario.addArea(serviceArea);
    scenario.addArea(corridor);

    // Scenario defined?
    double tileWidth = 3; // m
    double tileHeight = 3; // m
    double minTileArea = 2; // m^2
    auto start = std::chrono::high_resolution_clock::now();
    scenario.defined(tileWidth, tileHeight, minTileArea);
    cout << "Execution time scenario.defined(): ";
    cout << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start).count();
    cout << " ms"  << endl;

    // Store to svg.
    std::ofstream svg("Scenario0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 400, 400);

    const BoostPolygon &boost_measurement_area  = scenario.getMeasurementAreaENU();
    const BoostPolygon &boost_service_area      = scenario.getServiceAreaENU();
    const BoostPolygon &boost_corridor_area     = scenario.getCorridorENU();
    const BoostPolygon &boos_joined_area        = scenario.getJoineAreaENU();
    const BoostPolygon &boost_bbox              = scenario.getMeasurementAreaBBoxENU().corners;

    mapper.add(boost_measurement_area);
    mapper.add(boost_service_area);
    mapper.add(boost_corridor_area);
    mapper.add(boos_joined_area);
    mapper.add(boost_bbox);
    mapper.map(boost_measurement_area, "fill-opacity:0.1;fill:rgb(0,255,0);stroke:rgb(0,255,0);stroke-width:2");
    mapper.map(boost_service_area,     "fill-opacity:0.1;fill:rgb(235, 232, 52);stroke:rgb(235, 232, 52);stroke-width:2");
    mapper.map(boost_corridor_area,    "fill-opacity:0.1;fill:rgb(52, 171, 235);stroke:rgb(52, 171, 235);stroke-width:2");
    mapper.map(boos_joined_area,       "fill-opacity:0.0;fill:rgb(245, 66, 218);stroke:rgb(245, 66, 218);stroke-width:3");
    mapper.map(boost_bbox,             "fill-opacity:0.0;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:2");

    auto tiles = scenario.getTilesENU();
    //cout << "Tile count: " << tiles.size() << endl;
    for (auto tile : tiles) {
        mapper.add(tile);
        mapper.map(tile, "fill-opacity:0.1;fill:rgb(0,0,0);stroke:rgb(0,0,0);stroke-width:2");
    }

    auto center_points = scenario.getTileCenterPointsENU();
    for (auto point : center_points) {
        mapper.add(point);
        mapper.map(point, "fill-opacity:0.5;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:2", 2);
    }

    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea));
    //cout << scenario.errorString;

}

TEST_CASE( "Test Scenario Class. Negative Parameter." ) {
    // Data
    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
                                     GeoPoint2D{47.768205, 16.530558},
                                     GeoPoint2D{47.768147, 16.530566},
                                     GeoPoint2D{47.768326, 16.530926},
                                     GeoPoint2D{47.768135, 16.531025},
                                     GeoPoint2D{47.768144, 16.530845},
                                     GeoPoint2D{47.768103, 16.530978},
                                     GeoPoint2D{47.767940, 16.530393}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::MeasurementArea};

    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
                                     GeoPoint2D{47.767968, 16.530677},
                                     GeoPoint2D{47.767860, 16.530723},
                                     GeoPoint2D{47.767777, 16.530447}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::ServiceArea};

    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
                                        GeoPoint2D{47.768004, 16.530382},
                                        GeoPoint2D{47.768035, 16.530549},
                                        GeoPoint2D{47.767880, 16.530629}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::Corridor};

    Scenario scenario;
    scenario.addArea(measurementArea);
    scenario.addArea(serviceArea);
    scenario.addArea(corridor);

    // Scenario defined?
    double tileWidth = 5; // m
    double tileHeight = 5; // m
    double minTileArea = -2; // m^2

    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea) == false);

}

TEST_CASE( "Test Scenario Class. Missing Corridor." ) {
    // Data
    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
                                     GeoPoint2D{47.768205, 16.530558},
                                     GeoPoint2D{47.768147, 16.530566},
                                     GeoPoint2D{47.768326, 16.530926},
                                     GeoPoint2D{47.768135, 16.531025},
                                     GeoPoint2D{47.768144, 16.530845},
                                     GeoPoint2D{47.768103, 16.530978},
                                     GeoPoint2D{47.767940, 16.530393}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::MeasurementArea};

    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
                                     GeoPoint2D{47.767968, 16.530677},
                                     GeoPoint2D{47.767860, 16.530723},
                                     GeoPoint2D{47.767777, 16.530447}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::ServiceArea};

    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
                                        GeoPoint2D{47.768004, 16.530382},
                                        GeoPoint2D{47.768035, 16.530549},
                                        GeoPoint2D{47.767880, 16.530629}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::Corridor};

    Scenario scenario;
    scenario.addArea(measurementArea);
    scenario.addArea(serviceArea);
    //scenario.addArea(corridor);

    // Scenario defined?
    double tileWidth = 5; // m
    double tileHeight = 5; // m
    double minTileArea = 2; // m^2

    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea) == false);

}


//===========================================================================================
// FlightPlan
//===========================================================================================
TEST_CASE( "Test FlightPlan Class. Visual." ) {
    // Define Scenario.
    Area measurementArea{GeoPoint2DList{GeoPoint2D{47.768120, 16.530380},
                                     GeoPoint2D{47.768205, 16.530558},
                                     GeoPoint2D{47.768147, 16.530566},
                                     GeoPoint2D{47.768326, 16.530926},
                                     GeoPoint2D{47.768135, 16.531025},
                                     GeoPoint2D{47.768144, 16.530845},
                                     GeoPoint2D{47.768103, 16.530978},
                                     GeoPoint2D{47.767940, 16.530393}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::MeasurementArea};

    Area     serviceArea{GeoPoint2DList{GeoPoint2D{47.767874, 16.530409},
                                     GeoPoint2D{47.767968, 16.530677},
                                     GeoPoint2D{47.767860, 16.530723},
                                     GeoPoint2D{47.767777, 16.530447}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::ServiceArea};

    Area        corridor{GeoPoint2DList{GeoPoint2D{47.767853, 16.530482},
                                        GeoPoint2D{47.768004, 16.530382},
                                        GeoPoint2D{47.768035, 16.530549},
                                        GeoPoint2D{47.767880, 16.530629}},
                        10, /*altitude*/
                        1, /*layers*/
                        AreaType::Corridor};

    Scenario scenario;
    scenario.addArea(measurementArea);
    scenario.addArea(serviceArea);
    scenario.addArea(corridor);

    // Parameters
    double tileWidth            = 5; // m
    double tileHeight           = 5; // m
    double minTileArea          = 5; // m^2
    double lineDistance         = 1; // m
    double minTransectLength    = 2; // m


    REQUIRE(scenario.defined(tileWidth, tileHeight, minTileArea) == true);


    // Generate pseudo progress.
    size_t n = scenario.getTilesENU().size();
    vector<int8_t> progress;
    progress.reserve(n);
    // Random Seed =======================================================================
    //srand(time(NULL)); // init. random seed
    for (size_t i=0; i < n; ++i){
        int p = int(rand()/double(RAND_MAX)*200);
        progress.push_back(p >= 100 ? 100 : p);
    }

    // Generate flightplan.
    FlightPlan flightPlan;
    flightPlan.setProgress(progress);
    flightPlan.setScenario(scenario);

    auto start = std::chrono::high_resolution_clock::now();
    REQUIRE(flightPlan.generate(lineDistance, minTransectLength) == true);
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time flightPlan.generate(): ";
    cout << delta.count();
    cout << " ms" << endl;


    // Store to svg.
    std::ofstream svg("FlightPlan0.svg");
    boost::geometry::svg_mapper<BoostPoint> mapper(svg, 400, 400);

    const BoostPolygon &boost_measurement_area  = scenario.getMeasurementAreaENU();
    const BoostPolygon &boost_service_area      = scenario.getServiceAreaENU();
    const BoostPolygon &boost_corridor_area     = scenario.getCorridorENU();
    const BoostPolygon &boos_joined_area        = scenario.getJoineAreaENU();
    const BoostPolygon &boost_bbox              = scenario.getMeasurementAreaBBoxENU().corners;

    mapper.add(boost_measurement_area);
    mapper.add(boost_service_area);
    mapper.add(boost_corridor_area);
    mapper.add(boos_joined_area);
    mapper.add(boost_bbox);
    mapper.map(boost_measurement_area, "fill-opacity:0.1;fill:rgb(0,255,0);stroke:rgb(0,255,0);stroke-width:2");
    mapper.map(boost_service_area,     "fill-opacity:0.1;fill:rgb(235, 232, 52);stroke:rgb(235, 232, 52);stroke-width:2");
    mapper.map(boost_corridor_area,    "fill-opacity:0.1;fill:rgb(52, 171, 235);stroke:rgb(52, 171, 235);stroke-width:2");
    mapper.map(boos_joined_area,       "fill-opacity:0.0;fill:rgb(245, 66, 218);stroke:rgb(245, 66, 218);stroke-width:3");
    mapper.map(boost_bbox,             "fill-opacity:0.0;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:2");

    auto tiles = scenario.getTilesENU();
    //cout << "Tile count: " << tiles.size() << endl;
    for (auto tile : tiles) {
        mapper.add(tile);
        mapper.map(tile, "fill-opacity:0.1;fill:rgb(0,0,0);stroke:rgb(0,0,0);stroke-width:2");
    }

    auto center_points = scenario.getTileCenterPointsENU();
    for (size_t i=0; i < n; ++i) {
        BoostPoint point{scenario.getTileCenterPointsENU()[i]};
        mapper.add(point);
        if (progress[i] < 50)
            mapper.map(point, "fill-opacity:0.5;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:2", 1);
        else if (progress[i] < 100)
            mapper.map(point, "fill-opacity:0.5;fill:rgb(245, 155, 66);stroke:rgb(245, 155, 66);stroke-width:2", 1);
        else
            mapper.map(point, "fill-opacity:0.5;fill:rgb(17, 255, 0);stroke:rgb(17, 255, 0);stroke-width:2", 1);
    }

    const vector<BoostLineString> &transects = flightPlan.getTransects();

    for (auto t : transects){
        mapper.add(t);
        mapper.map(t, "fill-opacity:0.1;fill:rgb(255, 55, 0);stroke:rgb(255, 55, 0);stroke-width:6");
    }

    const BoostLineString &waypoints = flightPlan.getWaypointsENU();
    cout << "Number of waypoints: " << waypoints.size() << endl;
    mapper.add(waypoints);
    mapper.map(waypoints, "fill-opacity:0.1;fill:rgb(170, 255, 0);stroke:rgb(170, 255, 0);stroke-width:2");


#ifndef NDEBUG
    const BoostLineString &pathVertices = flightPlan.getPathVertices();
    for (size_t i=0; i<pathVertices.size(); ++i){
        mapper.text(pathVertices[i], to_string(i), ".small { font: italic 12px sans-serif; }");
    }
#endif


    // Waypoints inside joined Area?
    BoostPolygon jAreaOffset;
    offsetPolygon(scenario.getJoineAreaENU(), jAreaOffset, 0.1);
    REQUIRE(bg::within(flightPlan.getWaypointsENU(), jAreaOffset));

}