#include "snake.h"

#include "clipper.hpp"
#include "assert.h"


namespace snake {

Scenario::Scenario() :
    _mAreaBoundingBox(min_bbox_rt{0, 0, 0, Point2D{0,0}})
{

}

bool Scenario::setArea(Area &area)
    {
        if (area.geoPolygon.size() < 3){
            error_str = "Area has less than three vertices.";
            return false;
        }
        if (area.type == MeasurementArea)
            return Scenario::_setMeasurementArea(area);
        else if (area.type == ServiceArea)
            return Scenario::_setServiceArea(area);
        else if (area.type == Corridor)
            return Scenario::_setCorridor(area);
        return false;
    }

    bool Scenario::_areas2enu()
    {
        if (_measurementArea.geoPolygon.size() > 0){
            _measurementAreaENU.clear();
            for(auto vertex : _measurementArea.geoPolygon) {
                    Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude});
                    _measurementAreaENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]});
            }
            _homePositionENU = polygonCenter(_measurementAreaENU);

            if (_serviceArea.geoPolygon.size() > 0){
                _serviceAreaENU.clear();
                for(auto vertex : _serviceArea.geoPolygon) {
                        Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude});
                        _serviceAreaENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]});
                }
            } else{
                error_str = "Service area has no vertices.";
                return false;
            }

            if (_corridor.geoPolygon.size() > 0){
                _corridorENU.clear();
                for(auto vertex : _corridor.geoPolygon) {
                        Point3D ENUVertex = toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude});
                        _corridorENU.push_back(Point2D{ENUVertex[0], ENUVertex[1]});
                }
            }

            return true;
        }

        error_str = "Measurement area has no vertices.";
        return false;
    }

    bool Scenario::_setMeasurementArea(Area &area)
    {
        if (area.geoPolygon.size() <= 0)
            return false;
        _geoOrigin = area.geoPolygon[0];
        _measurementArea = area;
        _measurementAreaENU.clear();
        _serviceAreaENU.clear();
        _corridorENU.clear();
        return true;

    }

    bool Scenario::_setServiceArea(Area &area)
    {
        if (area.geoPolygon.size() <= 0)
            return false;
        _serviceArea = area;
        _serviceAreaENU.clear();
        return true;
    }

    bool Scenario::_setCorridor(Area &area)
    {
        if (area.geoPolygon.size() <= 0)
            return false;
        _corridor = area;
        _corridorENU.clear();
        return true;
    }

    bool Scenario::_calculateBoundingBox()
    {
        _mAreaBoundingBox = minimalBoundingBox(_measurementAreaENU);
        return true;
    }
    /**
     * Devides the (measurement area) bounding  box into tiles and clips it to the measurement area.
     *
     * Devides the (measurement area) bounding  box into tiles of width \p tileWidth and height \p tileHeight.
     * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
     * The function assumes that \a _measurementAreaENU and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
     * Scenario::_calculateBoundingBox().
     *
     * @param tileWidth The width of a tile.
     * @param tileHeight The heigth of a tile.
     * @param minTileArea The minimal area of a tile.
     *
     * @return Returns true if successful.
     */
    bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea)
    {
        std::vector<Point2DList> tiles_unclipped;

        return true;
    }

    bool Scenario::_calculateJoinedArea()
    {
        ClipperLib::Path measurementArea;
        ClipperLib::Path serviceArea;
        ClipperLib::Path corridor;

        // Apply scaling and convert ENU polygons (double) to ClipperLib::cInt polygons.
        for (auto vertex : _measurementAreaENU){
            ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale),
                                           ClipperLib::cInt(vertex[1]*clipper_scale)};
            measurementArea.push_back(intVertex);
        }

        for (auto vertex : _serviceAreaENU){
            ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale),
                                           ClipperLib::cInt(vertex[1]*clipper_scale)};
            serviceArea.push_back(intVertex);
        }

        bool corridorValid = false;
        if (_corridorENU.size() > 0) {
            for (auto vertex : _corridorENU){
                ClipperLib::IntPoint intVertex{ClipperLib::cInt(vertex[0]*clipper_scale),
                                               ClipperLib::cInt(vertex[1]*clipper_scale)};
                corridor.push_back(intVertex);
            }
            corridorValid = true;
        }

        // Check if measurement area and service area are overlapping.
        ClipperLib::Clipper cp1;
        cp1.AddPath(measurementArea, ClipperLib::ptClip, true);
        cp1.AddPath(serviceArea, ClipperLib::ptSubject, true);

        // Execute clipper
        ClipperLib::Paths solution;
        cp1.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);

        // Measurement area and service area overlapping?
        bool overlaping_s_m = false;
        if (solution.size() > 0){
            overlaping_s_m = true;
        }


        // Check if corridor is connecting measurement area and service area.
        bool corridor_is_connection = false;
        if (corridorValid) {
            ClipperLib::Clipper cp2;
            solution.clear();
            cp2.AddPath(measurementArea, ClipperLib::ptClip, true);
            cp2.AddPath(corridor, ClipperLib::ptSubject, true);
            cp2.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);

            // Corridor overlaping with measurement area?
            if (solution.size() > 0) {
                // Check if corridor overlaps with service area.
                cp2.Clear();
                solution.clear();
                cp2.AddPath(serviceArea, ClipperLib::ptClip, true);
                cp2.AddPath(corridor, ClipperLib::ptSubject, true);
                cp2.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);

                // Corridor overlaping with service area?
                if (solution.size() > 0) {
                    corridor_is_connection = true;
                }
            }
        }

        // Are areas joinable?
        if (overlaping_s_m){
            if(corridor_is_connection){
                cp1.AddPath(corridor, ClipperLib::ptSubject, true);
            }
        } else if (corridor_is_connection){
            cp1.AddPath(corridor, ClipperLib::ptSubject, true);
        } else {
            error_str = "Areas are not overlapping";
            return false;
        }

        // Join areas.
        solution.clear();
        cp1.Execute(ClipperLib::ctUnion, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);

        if (solution.size() != 1)
            assert(0);

        _joinedAreaENU.clear();
        for (auto intVertex : solution[0]){
            Point2D vertex{double(intVertex.X)/clipper_scale, double(intVertex.Y)/clipper_scale};
            _joinedAreaENU.push_back(vertex);
        }

        return true;
    }

}