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


#include "snake.h"


SnakeDataManager::SnakeDataManager(QObject *parent)
    : QThread(parent)
  , _pScenario(std::make_shared<snake::Scenario>())
  , _pProgress(std::make_shared<std::vector<int>>())
  , _pFlightplan(std::make_shared<snake::Flightplan>(_pScenario, _pProgress))
  , _pData(std::make_shared<SnakeData>())

{
}

SnakeDataManager::~SnakeDataManager()
{

}

SnakeDataManager::SnakeDataPtr SnakeDataManager::snakeData()
{
    return _pData;
}

bool SnakeDataManager::precondition() const
{

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

double SnakeDataManager::lineDistance() const
{
    _pFlightplan->lineDistance();
}

void SnakeDataManager::run()
{
    {
        QMutexLocker lk(&_pData->m);
        _pData->clear();
    }

    if ( !precondition() )
        return;

    if ( _mArea.size() <= 0
         || _sArea.size() <= 0
         || _corridor.size() <= 0 ){
        QMutexLocker lk(&_pData->m);
        _pData->errorMessage = "At least one area invald. Size < 1.";
    }

    auto origin = _mArea.front();
    // Measurement area update necessary.
    if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){
        {
            QMutexLocker lk(&_pData->m);
            _pData->ENUorigin = origin;
        }
        for (auto geoVertex : _mArea){
            snake::BoostPoint p;
            snake::toENU(origin, geoVertex, p);
            _pScenario->measurementArea().outer().push_back(p);
        }
    }

    // Service area update necessary.
    if ( _sArea.size() != _pScenario->serviceArea().outer().size() ){
        for (auto geoVertex : _sArea){
            snake::BoostPoint p;
            snake::toENU(origin, geoVertex, p);
            _pScenario->serviceArea().outer().push_back(p);
        }
    }

    // Service area update necessary.
    if ( _corridor.size() != _pScenario->corridor().outer().size() ){
        for (auto geoVertex : _corridor){
            snake::BoostPoint p;
            snake::toENU(origin, geoVertex, p);
            _pScenario->corridor().outer().push_back(p);
        }
    }

    if ( !_pScenario->update() ){
        {
            QMutexLocker lk(&_pData->m);
            for (auto c : _pScenario->errorString){
                _pData->errorMessage.push_back(QChar(c));
            }
        }
        return;
    }

    // Asynchronously update flightplan.
    auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan));

    // Continue with storing scenario data in the mean time.
    {
        QMutexLocker lk(&_pData->m);
        // Get tiles.
        const auto &tiles = _pScenario->tiles();
        const auto &centerPoints = _pScenario->tileCenterPoints();
        for ( unsigned int i=0; i < tiles.size(); ++i ) {
            const auto &tile = tiles[i];
            GeoTile     geoTile;
            Polygon2D   enuTile;
            for ( size_t i = tile.outer().size(); i < tile.outer().size()-1; ++i) {
                auto &p = tile.outer()[i];
                QPointF enuVertex(p.get<0>(), p.get<1>(), 0.0);
                QGeoCoordinate geoVertex;
                snake::fromENU(origin, *it, geoVertex);
                enuTile.polygon().push_back(enuVertex);
                geoTile.push_back(QVertex);
            }
            const auto &boostPoint = centerPoints[i];
            QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>(), 0.0);
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, enuVertex, geoVertex);
            geoTile.setCenter(geoVertex);
            _pData->tiles.polygons().push_back(geoTile);
            _pData->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
            _pData->tilesENU.polygons().push_back(enuTile);
            _pData->tileCenterPointsENU.push_back(enuVertex);
         }
    }

    future.wait();
    // Trying to generate flight plan.
    if ( !future.get() ){
        // error
        QMutexLocker lk(&_pData->m);
        for (auto c : _pFlightplan->errorString){
            _pData->errorMessage.push_back(QChar(c));
        }
    } else {
        //success!!!
        QMutexLocker lk(&_pData->m);
        // Store waypoints.
        for (auto boostVertex : _pFlightplan->waypoints()){
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, enuVertex, geoVertex);
            _pData->waypointsENU.push_back(enuVertex);
            _pData->waypoints.push_back(geoVertex);
        }
        // Store arrival path.
        for (auto boostVertex : _pFlightplan->arrivalPath()){
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, enuVertex, geoVertex);
            _pData->arrivalPathENU.push_back(enuVertex);
            _pData->arrivalPath.push_back(geoVertex);
        }
        // Store return path.
        for (auto boostVertex : _pFlightplan->returnPath()){
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, enuVertex, geoVertex);
            _pData->returnPathENU.push_back(enuVertex);
            _pData->returnPath.push_back(geoVertex);
        }
    }
}

double SnakeDataManager::minTransectLength() const
{
    return _pFlightplan->minTransectLength();
}

void SnakeDataManager::setMinTransectLength(double minTransectLength)
{
    _pFlightplan->setMinTransectLength(minTransectLength);
}

double SnakeDataManager::minTileArea() const
{
    return _pScenario->minTileArea();
}

void SnakeDataManager::setLineDistance(double lineDistance)
{
    _pFlightplan->setLineDistance(lineDistance);
}

double SnakeDataManager::tileWidth() const
{
    return _pScenario->tileWidth();
}

void SnakeDataManager::setTileWidth(double tileWidth)
{
    _pScenario->setTileWidth(tileWidth);
}

double SnakeDataManager::tileHeight() const
{
    return _pScenario->_tileHeight();
}

void SnakeDataManager::setTileHeight(double tileHeight)
{
    _pScenario->setTileHeight(tileHeight);
}

void SnakeDataManager::setMinTileArea(double minTileArea)
{
    _pScenario->setMinTileArea(minTileArea);
}

SnakeData::SnakeData()
{

}

void SnakeData::clear()
{
    this->waypoints.clear();
    this->arrivalPath.clear();
    this->returnPath.clear();
    this->tiles.polygons().clear();
    this->tilesLocal.polygons().clear();
    this->tileCenterPoints.clear();
    this->waypointsENU.clear();
    this->arrivalPathENU.clear();
    this->returnPathENU.clear();
    this->tilesENU.polygons().clear();
    this->tileCenterPoints.clear();
    this->errorMessage = "";
}

void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
{
    _corridor.clear();
    _pScenario->corridor().clear();
    for (auto vertex : corridor) {
        _corridor.push_back(vertex);
    }
}

void SnakeDataManager::setProgress(const QList<IntType> &progress)
{
    _pProgress->clear();
    for (auto p : progress) {
        assert(p >= -1 && p <= 100);
        _pProgress->push_back(p);
    }
}

void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
{
    _sArea.clear();
    _pScenario->serviceArea().clear();
    for (auto vertex : serviceArea) {
        _sArea.push_back(vertex);
    }
}

void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
{
    _mArea.clear();
    _pScenario->measurementArea().clear();
    for (auto vertex : measurementArea) {
        _mArea.push_back(vertex);
    }
}