#include "SnakeWorker.h"
#include <QGeoCoordinate>


#include "snake_geometry.h"
#include "snake.h"



using namespace snake;
using namespace snake_geometry;

SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
    , _lineDistance(3)
    , _minTransectLength(2)
    , _tileWidth(5)
    , _tileHeight(5)
    , _minTileArea(3)
{
}

SnakeWorker::~SnakeWorker()
{

}

WorkerResult_t &SnakeWorker::result()
{
    return _result;
}

bool SnakeWorker::precondition() const
{

    return _mArea.size() > 0
            && _sArea.size() > 0
            && _corridor.size() > 0
            && _progress.size() > 0;
}

double SnakeWorker::lineDistance() const
{
    return _lineDistance;
}

void SnakeWorker::run()
{
    // Reset _result struct.
    _result.clear();

    if ( !precondition())
        return;

    Scenario scenario;// Initialize scenario.

    Area mArea;
    for (auto vertex : _mArea){
        mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
    }
    mArea.type = AreaType::MeasurementArea;

    Area sArea;
    for (auto vertex : _sArea){
        sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
    }
    sArea.type = AreaType::ServiceArea;

    Area corridor;
    for (auto vertex : _corridor){
        corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
    }
    corridor.type = AreaType::Corridor;

    scenario.addArea(mArea);
    scenario.addArea(sArea);
    scenario.addArea(corridor);

    if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) )
        return;

    FlightPlan flightplan;
    flightplan.setScenario(scenario);
    flightplan.setProgress(_progress);

    // Trying to generate flight plan.
    if ( !flightplan.generate(_lineDistance, _minTransectLength) ){
        // error
        for (auto c : flightplan.errorString){
            _result.errorMessage.push_back(QChar(c));
        }
    } else {
        //success!!!
        _result.success = true;

        // Fill result struct.
        auto& waypoints = flightplan.getWaypoints();
        for (auto vertex : waypoints){
            _result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0));
        }

        auto& arrivalPathIdx = flightplan.getArrivalPathIdx();
        for (auto idx : arrivalPathIdx){
            _result.arrivalPathIdx.append(idx);
        }

        auto& returnPathIdx = flightplan.getReturnPathIdx();
        for (auto idx : returnPathIdx){
            _result.returnPathIdx.append(idx);
        }


        // Get tiles and origin.
        auto origin = scenario.getOrigin();
        _result.origin.setLatitude(origin[0]);
        _result.origin.setLongitude(origin[1]);
        _result.origin.setAltitude(origin[2]);
        const auto &tiles = scenario.getTiles();
        const auto &cps = scenario.getTileCenterPoints();
        for ( unsigned int i=0; i < tiles.size(); ++i ) {
            const auto &tile = tiles[i];
            GeoTile geoTile;
            for ( const auto &vertex : tile) {
                QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
                geoTile.append(QVertex);
            }
            const auto &centerPoint = cps[i];
            QGeoCoordinate c(centerPoint[0], centerPoint[1], centerPoint[2]);
            geoTile.setCenter(c);
            _result.tiles.polygons().append(geoTile);
            _result.tileCenterPoints.append(QVariant::fromValue(c));
         }

        // Get local tiles.
        const auto &tilesENU = scenario.getTilesENU();
        for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
            const auto &tile = tilesENU[i];
            Polygon2D localTile;
            for ( const auto &vertex : tile.outer()) {
                QPointF QVertex(vertex.get<0>(), vertex.get<1>());
                localTile.path().append(QVertex);
            }
            _result.tilesLocal.polygons().append(localTile);
        }

    }
}

double SnakeWorker::minTransectLength() const
{
    return _minTransectLength;
}

void SnakeWorker::setMinTransectLength(double minTransectLength)
{
    if (minTransectLength > 0)
        _minTransectLength = minTransectLength;
}

double SnakeWorker::minTileArea() const
{
    return _minTileArea;
}

void SnakeWorker::setLineDistance(double lineDistance)
{
    if (lineDistance > 0)
        _lineDistance = lineDistance;
}

double SnakeWorker::tileWidth() const
{
    return _tileWidth;
}

void SnakeWorker::setTileWidth(double tileWidth)
{
    if (tileWidth > 0)
        _tileWidth = tileWidth;
}

double SnakeWorker::tileHeight() const
{
    return _tileHeight;
}

void SnakeWorker::setTileHeight(double tileHeight)
{
    if (tileHeight > 0)
        _tileHeight = tileHeight;
}

void SnakeWorker::setMinTileArea(double minTileArea)
{
    if (minTileArea > 0)
        _minTileArea = minTileArea;
}

void Result::clear()
{
    this->success = false;
    this->errorMessage = "";
    this->waypoints.clear();
    this->returnPathIdx.clear();
    this->arrivalPathIdx.clear();
    this->tiles.polygons().clear();
    this->tilesLocal.polygons().clear();
    this->tileCenterPoints.clear();
}