#pragma once

#include <vector>
#include <string>
#include <array>

#include "snake_geometry.h"

#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"

using namespace std;
using namespace snake_geometry;

using namespace operations_research;

namespace snake {

    enum AreaType {MeasurementArea, ServiceArea, Corridor};

    struct Area {
        GeoPoint2DList      geoPolygon;
        double              altitude;
        size_t              layers;
        AreaType            type;
    };

    //========================================================================================
    // Scenario
    //========================================================================================
    class Scenario{
    public:
        Scenario();

        bool addArea(Area &area);

        const Area &getMeasurementArea() const {return _measurementArea;}
        const Area &getServiceArea() const {return _serviceArea;}
        const Area &getCorridor() const {return _corridor;}

        const BoostPolygon &getMeasurementAreaENU() {return _measurementAreaENU;}
        const BoostPolygon &getServiceAreaENU()     {return _serviceAreaENU;}
        const BoostPolygon &getCorridorENU()        {return _corridorENU;}
        const BoostPolygon &getJoineAreaENU()       {return _joinedAreaENU;}
        const GeoPoint3D   &getOrigin()             {return _geoOrigin;}

        const vector<BoostPolygon> &getTilesENU()               {return _tilesENU;}
        const BoostLineString      &getTileCenterPointsENU()    {return _tileCenterPointsENU;}
        const min_bbox_rt          &getMeasurementAreaBBoxENU() {return _mAreaBoundingBox;}
        const BoostPoint           &getHomePositonENU()         {return _homePositionENU;}

        bool defined(double tileWidth, double tileHeight, double minTileArea);

        string errorString;

    private:
        bool _areas2enu();
        bool _setMeasurementArea(Area &area);
        bool _setServiceArea(Area &area);
        bool _setCorridor(Area &area);
        bool _calculateBoundingBox();
        bool _calculateTiles(double tileWidth, double tileHeight, double minTileArea);
        bool _calculateJoinedArea();

        Area _measurementArea;
        Area _serviceArea;
        Area _corridor;

        BoostPolygon _measurementAreaENU;
        BoostPolygon _serviceAreaENU;
        BoostPolygon _corridorENU;
        BoostPolygon _joinedAreaENU;

        min_bbox_rt _mAreaBoundingBox;

        vector<BoostPolygon>    _tilesENU;
        BoostLineString         _tileCenterPointsENU;

        GeoPoint3D _geoOrigin;
        BoostPoint _homePositionENU;
    };

    //========================================================================================
    // FlightPlan
    //========================================================================================

    class FlightPlan{
    public:
        FlightPlan();

        void setScenario(const Scenario &scenario)          {_scenario = scenario;}
        void setProgress(const vector<int8_t> &progress)    {_progress = progress;}

        const Scenario          &getScenario(void)          const {return _scenario;}
        const BoostLineString   &getWaypointsENU(void)      const {return _waypointsENU;}
        const GeoPoint2DList    &getWaypoints(void)         const {return _waypoints;}
        const vector<uint64_t>  &getArrivalPathIdx(void)    const {return _arrivalPathIdx;}
        const vector<uint64_t>  &getReturnPathIdx(void)     const {return _returnPathIdx;}
#ifndef NDEBUG
        const BoostLineString   &getPathVertices(void)      const {return _PathVertices;}
#endif

        bool                            generate(double lineDistance, double minTransectLength);
        const vector<BoostLineString>  &getTransects() const {return _transects;}

        string errorString;

    private:
        // Search Filter to speed up routing.SolveWithParameters(...);
        // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
        class SearchFilter : public LocalSearchFilter{

        };

        typedef struct{
            Matrix<int64_t>                 distanceMatrix;
            long                            numVehicles;
            RoutingIndexManager::NodeIndex  depot;
        }RoutingDataModel_t;

        bool _generateTransects(double lineDistance, double minTransectLength);
        void _generateRoutingModel(const BoostLineString &vertices,
                                   const BoostPolygon &polygonOffset,
                                   size_t n0,
                                   RoutingDataModel_t &dataModel,
                                   Matrix<double> &graph);

        Scenario                                _scenario;
        BoostLineString                         _waypointsENU;
        GeoPoint2DList                          _waypoints;
        vector<BoostLineString>                 _transects;
        vector<int8_t>                          _progress;
        vector<uint64_t>                        _arrivalPathIdx;
        vector<uint64_t>                        _returnPathIdx;

#ifndef NDEBUG
        BoostLineString                         _PathVertices;
#endif
    };

    namespace detail {
        const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
    }
}