#include "WimaArea.h"

const double WimaArea::numericalAccuracy = 1e-3; // meters

WimaArea::WimaArea(QObject *parent) :
    WimaArea           (nullptr, parent)
{
    //qWarning() << "WimaPolygon:: polygon count" << _polygon->count();
}

WimaArea::WimaArea(QGCMapPolygon *other, QObject *parent)
    : QGCMapPolygon (other, parent)
{
    WimaArea* wimaPoly = qobject_cast<WimaArea*>(other);
    if (wimaPoly != nullptr) {
        _maxAltitude = wimaPoly->maxAltitude();
        _wimaVehicle = wimaPoly->vehicle();
    } else {
        _maxAltitude = 30;
        _wimaVehicle = new WimaVehicle(this);
    }
}


WimaArea::~WimaArea()
{

}


void WimaArea::setMaxAltitude(double alt)
{
    if(alt > 0 && alt != _maxAltitude){
        _maxAltitude = alt;
        emit maxAltitudeChanged();
    }
}


void WimaArea::setVehicle(WimaVehicle *vehicle)
{
    if(_wimaVehicle != vehicle){
        _wimaVehicle->deleteLater();
        _wimaVehicle =  vehicle;
        emit vehicleChanged();
    }
}

/*QList<WimaArea *>* WimaArea::splitArea<T>(WimaArea *polygonToSplitt, int numberOfFractions)
{
    if(numberOfFractions > 0 && polygonToSplitt != nullptr){
        WimaArea* poly;
        if(p)
        = new WimaArea(polygonToSplitt, this);
        QList<WimaArea*>* list = new QList<WimaArea*>();
        list->append(poly);
        return list;
    }
    return nullptr;
}


QList<QGCMapPolygon*>* WimaArea::splitArea(int numberOfFractions)
{
    return splitPolygonArea(this, numberOfFractions);
}*/

int WimaArea::getClosestVertexIndex(QGeoCoordinate coordinate)
{
    if (this->count() == 0) {
        qWarning("Polygon count == 0!");
        return -1;
    }else if (this->count() == 1) {
        return 0;
    }else {
        int index = 0;
        double min_dist = coordinate.distanceTo(this->vertexCoordinate(index));
        for(int i = 1; i < this->count(); i++){
            double dist = coordinate.distanceTo(this->vertexCoordinate(i));
            if (dist < min_dist){
                min_dist = dist;
                index = i;
            }
        }

        return index;
    }
}

QGeoCoordinate WimaArea::getClosestVertex(QGeoCoordinate coordinate)
{
    return this->vertexCoordinate(getClosestVertexIndex(coordinate));
}

QGCMapPolygon* WimaArea::toQGCPolygon(WimaArea *poly)
{
    if (poly != nullptr) {
        QGCMapPolygon* qgcPoly = new QGCMapPolygon(this);
        qgcPoly->setPath(poly->path());
        qgcPoly->setCenter(poly->center());
        qgcPoly->setCenterDrag(poly->centerDrag());
        qgcPoly->setInteractive(poly->interactive());

        return qgcPoly;
    } else {
        qWarning("WimaArea::toQGCPolygon(): poly == nullptr");
        return nullptr;
    }
}

WimaArea *WimaArea::joinPolygons(QList<WimaArea *>* polyList)
{
    return nullptr;
}


WimaArea *WimaArea::joinPolygons(WimaArea *poly1, WimaArea *poly2)
{
    if (poly1 != nullptr && poly2 != nullptr) {
        if (poly1->count() >= 3 && poly2->count() >= 3) {
            WimaArea* joinedPolygon = new WimaArea(this);

            poly1->verifyClockwiseWinding();
            poly2->verifyClockwiseWinding();

            WimaArea* walkerPoly    = poly1; // "walk" on this polygon towards higher indices
            WimaArea* crossPoly     = poly2; // check for crossings with this polygon while "walking"
                                             // and swicht to this polygon on a intersection,
                                             // continue to walk towards higher indices
            QGCMapPolyline lineSegment; // segment of walkerPoly, containing only two coordinates
            int currentPolyIndex = 0;
            do {
                QGeoCoordinate currentVertex    = walkerPoly->vertexCoordinate(currentPolyIndex);// change req.
                QGeoCoordinate nextVertex       = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(currentPolyIndex));
                joinedPolygon->appendVertex(currentVertex);

                QGCMapPolyline walkerPolySegment;
                walkerPolySegment.appendVertex(currentVertex);
                walkerPolySegment.appendVertex(nextVertex);

                QList<QGeoCoordinate*>* intersectionList = intersects(&walkerPolySegment, crossPoly);

                if (intersectionList != nullptr) {
                    // todo
                } else {
                    return nullptr;
                }

            } while (currentPolyIndex != 0 || walkerPoly != poly1);

            return joinedPolygon;
        } else {
            qWarning("WimaArea::joinPolygons(poly1, poly2): poly->count() < 3");
            return nullptr;
        }

    } else {
        qWarning("WimaArea::joinPolygons(poly1, poly2): nullptr");
        return nullptr;
    }
}

bool WimaArea::isDisjunct(QList<WimaArea *>* polyList)
{
    // needs improvement
    if (polyList != nullptr){
        for (int i = 0;i < polyList->size()-1; i++) {
            WimaArea* currPoly = polyList->takeAt(i);
            for (int j = i+1; i < polyList->size(); j++) {
                if (isDisjunct(currPoly, polyList->takeAt(j))) {
                    return false;
                }
            }
        }
        return true;
    } else {
        qWarning("WimaArea::isDisjunct(polyList): polyList == nullptr!");
        return false;
    }
}

bool WimaArea::isDisjunct(WimaArea *poly1, WimaArea *poly2)
{
    if (poly1 != nullptr && poly2 != nullptr) {
        QGCMapPolygon* poly1Copy = new QGCMapPolygon(this);
        poly1Copy->setPath(poly1->path());
        poly1Copy->offset(numericalAccuracy);// take numerical errors in account
        for(int i = 0; i < poly2->count(); i++){
            if (poly1Copy->containsCoordinate(poly2->vertexCoordinate(i))){
                return false;
            }
        }
        return true;
    } else {
        qWarning("WimaArea::isDisjunct(poly1, poly2):  poly1 == nullptr || poly2 == nullptr!");
        return false;
    }
}

int WimaArea::nextVertexIndex(int index)
{
    if (index >= 0 && index < count()-1) {
        return index + 1;
    } else if (index == count()-1) {
        return 0;
    } else {
        qWarning("WimaArea::nextVertexIndex(): Index out of bounds! index:count = %i:%i", index, count());
        return -1;
    }
}

int WimaArea::previousVertexIndex(int index)
{
    if (index > 0 && index < count()) {
        return index - 1;
    } else if (index == 0) {
        return count()-1;
    } else {
        qWarning("WimaArea::previousVertexIndex(): Index out of bounds! index:count = %i:%i", index, count());
        return -1;
    }
}

QGeoCoordinate* WimaArea::intersects(QGCMapPolyline *geoLine1, QGCMapPolyline *geoLine2)
{
    if (geoLine1 != nullptr && geoLine2 != nullptr) {
        if (geoLine1->count() == 2 && geoLine2->count() == 2 ) {
            QPointF pt11(0, 0);

            double x, y, z;
            QGeoCoordinate origin = geoLine1->vertexCoordinate(0);
            convertGeoToNed(geoLine1->vertexCoordinate(1), origin, &x, &y, &z);
            QPointF pt12(x, y);

            QLineF kartLine1(pt11, pt12);


            convertGeoToNed(geoLine2->vertexCoordinate(0), origin, &x, &y, &z);
            QPointF pt21(x, y);

            convertGeoToNed(geoLine2->vertexCoordinate(1), origin, &x, &y, &z);
            QPointF pt22(x, y);

            QLineF kartLine2(pt21, pt22);

            QPointF intersectionPoint;
            if (kartLine1.intersect(kartLine2, &intersectionPoint) != QLineF::NoIntersection) {
                QGeoCoordinate* retValue = new QGeoCoordinate();
                convertNedToGeo(intersectionPoint.x(), intersectionPoint.y(), origin.altitude(), origin, retValue);

                return retValue;
            } else {
                return nullptr;
            }


        } else {
            qWarning("WimaArea::intersect(geoLine1, geoLine2):  geoLine1->count() != 2 || geoLine2->count() != 2!");
            return nullptr;
        }
    } else {
        qWarning("WimaArea::intersect(geoLine1, geoLine2):  geoLine1 == nullptr || geoLine2 == nullptr!");
        return nullptr;
    }
}

QList<QGeoCoordinate *>* WimaArea::intersects(QGCMapPolyline *line, WimaArea *poly)
{
    QList<QGeoCoordinate*>* intersectionList = new QList<QGeoCoordinate*>();
    if (line != nullptr && poly != nullptr) {
        if (line->count() == 2 && poly->count() >= 3) {
            QVariantList polyPath = poly->path();
            QGeoCoordinate pt1;
            QGeoCoordinate pt2;
            for (int i = 0; i < poly->count(); i++) {
                QGCMapPolyline polySegment;
                polySegment.appendVertex(polyPath.takeAt(i).value<QGeoCoordinate>());
                polySegment.appendVertex(polyPath.takeAt(poly->nextVertexIndex(i)).value<QGeoCoordinate>());

                QGeoCoordinate* intersectionPoint = intersects(line, &polySegment);

                if (intersectionPoint != nullptr){
                    intersectionList->append(intersectionPoint);
                }
            }
            return intersectionList;
        } else {
            qWarning("WimaArea::intersects(line, poly): line->count() != 2 || poly->count() < 3");
            return nullptr;
        }

    } else {
        qWarning("WimaArea::intersects(line, poly):  line == nullptr || poly == nullptr!");
        return nullptr;
    }
}