#include <iostream>

#include "snake.h"

#include "clipper.hpp"

using namespace snake_geometry;
using namespace std;

namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;

namespace snake {

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

}

bool Scenario::addArea(Area &area)
    {
        if (area.geoPolygon.size() < 3){
            errorString = "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::defined(double tileWidth, double tileHeight, double minTileArea)
{
    if (!_areas2enu())
        return false;
    if (!_calculateBoundingBox())
        return false;
    if (!_calculateTiles(tileWidth, tileHeight, minTileArea))
        return false;
    if (!_calculateJoinedArea())
        return false;

    return true;
}

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}, ENUVertex);
                _measurementAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
        }
        bg::correct(_measurementAreaENU);

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

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

        return true;
    }

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

bool Scenario::_setMeasurementArea(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    GeoPoint2D origin2D = area.geoPolygon[0];
    _geoOrigin = GeoPoint3D{origin2D[0], origin2D[1], 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()
{
    minimalBoundingBox(_measurementAreaENU, _mAreaBoundingBox);
    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 (>0) of a tile.
 * @param tileHeight The heigth (>0) of a tile.
 * @param minTileArea The minimal area (>0) of a tile.
 *
 * @return Returns true if successful.
 */
bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea)
{
    _tilesENU.clear();
    _tileCenterPointsENU.clear();

    if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) {
        errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
        return false;
    }

    double bbox_width = _mAreaBoundingBox.width;
    double bbox_height = _mAreaBoundingBox.height;

    BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];

    //cout << "Origin: " << origin[0] << " " << origin[1] << endl;
    // Transform _measurementAreaENU polygon to bounding box coordinate system.
    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(), -origin.get<1>());
    BoostPolygon translated_polygon;
    BoostPolygon rotated_polygon;
    boost::geometry::transform(_measurementAreaENU, translated_polygon, translate);
    boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
    bg::correct(rotated_polygon);

    //cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;

    size_t i_max = ceil(bbox_width/tileWidth);
    size_t j_max = ceil(bbox_height/tileHeight);

    if (i_max < 1 || j_max < 1) {
        errorString = "tileWidth or tileHeight to small.";
        return false;
    }


    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(), origin.get<1>());
    for (size_t i = 0; i < i_max; ++i){
        double x_min = tileWidth*i;
        double x_max = x_min + tileWidth;
        for (size_t j = 0; j < j_max; ++j){
            double y_min = tileHeight*j;
            double y_max = y_min + tileHeight;

            BoostPolygon tile_unclipped;
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_max});
            tile_unclipped.outer().push_back(BoostPoint{x_max, y_max});
            tile_unclipped.outer().push_back(BoostPoint{x_max, y_min});
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});


            std::deque<BoostPolygon> boost_tiles;
            if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles))
                continue;

           for (BoostPolygon t : boost_tiles)
           {
                if (bg::area(t) > minTileArea){
                    // Transform boost_tile to world coordinate system.
                    BoostPolygon rotated_tile;
                    BoostPolygon translated_tile;
                    boost::geometry::transform(t, rotated_tile, rotate_back);
                    boost::geometry::transform(rotated_tile, translated_tile, translate_back);

                    // Store tile and calculate center point.
                    _tilesENU.push_back(translated_tile);
                    BoostPoint tile_center;
                    polygonCenter(translated_tile, tile_center);
                    _tileCenterPointsENU.push_back(tile_center);
                }
           }
        }
    }

    if (_tilesENU.size() < 1){
        errorString = "No tiles calculated. Is the minTileArea parameter large enough?";
        return false;
    }

    return true;
}

bool Scenario::_calculateJoinedArea()
{
    _joinedAreaENU.clear();
    // Measurement area and service area overlapping?
    bool overlapingSerMeas = bg::intersects(_measurementAreaENU, _serviceAreaENU) ? true : false;
    bool corridorValid = _corridorENU.outer().size() > 0 ? true : false;


    // Check if corridor is connecting measurement area and service area.
    bool corridor_is_connection = false;
    if (corridorValid) {
        // Corridor overlaping with measurement area?
        if ( bg::intersects(_corridorENU, _measurementAreaENU) ) {
            // Corridor overlaping with service area?
            if ( bg::intersects(_corridorENU, _serviceAreaENU) )
                corridor_is_connection = true;
        }
    }

    // Are areas joinable?
    std::deque<BoostPolygon> sol;
    BoostPolygon partialArea = _measurementAreaENU;
    if (overlapingSerMeas){
        if(corridor_is_connection){
            bg::union_(partialArea, _corridorENU, sol);
        }
    } else if (corridor_is_connection){
        bg::union_(partialArea, _corridorENU, sol);
    } else {
        errorString = "Areas are not overlapping";
        return false;
    }

    if (sol.size() > 0) {
        partialArea = sol[0];
        sol.clear();
    }

    // Join areas.
    bg::union_(partialArea, _serviceAreaENU, sol);

    if (sol.size() > 0) {
        _joinedAreaENU = sol[0];
    } else {
        return false;
    }

    return true;
}

bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
{
    if (_scenario.getTilesENU().size() != _progress.size()){
        errorString = "Number of tiles is not equal progress array length";
        return false;
    }

    size_t num_tiles = _progress.size();
    vector<BoostPolygon> processedTiles;
    const auto &tiles = _scenario.getTilesENU();
    for (size_t i=0; i<num_tiles; ++i) {
        if (_progress[i] == 100)
            processedTiles.push_back(tiles[i]);
    }


    _transects.clear();
    const min_bbox_rt &bbox     = _scenario.getMeasurementAreaBBoxENU();
    double alpha                = bbox.angle;
    double x0                   = bbox.corners.outer()[0].get<0>();
    double y0                   = bbox.corners.outer()[0].get<1>();
    double bboxWidth            = bbox.width;
    double bboxHeight           = bbox.height;
    double delta                = detail::polygonOffset;

    size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
    vector<double> yCoords;
    yCoords.reserve(num_t);
    double y = -delta;
    for (size_t i=0; i < num_t; ++i) {
        yCoords.push_back(y);
        y += lineDistance;
    }

    for (size_t i=0; i < num_t; ++i) {
        BoostPoint v1{-delta, yCoords[i]};
        BoostPoint v2{bboxWidth+delta, yCoords[i]};
        _transects.push_back(tuple<BoostPoint, BoostPoint>{v1, v2});
    }


}

}