#include "SphereCalculus.h"




// Constructors
WimaArea::WimaArea(QObject *parent)
    :  QGCMapPolygon (parent)
{
    init();
    _maxAltitude = 30;
}

WimaArea::WimaArea(const WimaArea &other, QObject *parent)
    : QGCMapPolygon (parent)
{
    init();
    *this = other;
}

/*!
 *\fn WimaArea &WimaArea::operator=(const WimaArea &other)
 *
 * Assigns \a other to this \c WimaArea and returns a reference to this \c WimaArea.
 *
 * Copies only path and maximum altitude.
 */
WimaArea &WimaArea::operator=(const WimaArea &other)
{
    QGCMapPolygon::operator=(other);
    this->_maxAltitude = other.maxAltitude();
    this->setPath(other.path());

    return *this;
}

/*!
  \fn void WimaArea::setMaxAltitude(double altitude)

  Sets the \c _maxAltitude member to \a altitude and emits the signal \c maxAltitudeChanged()
  if \c _maxAltitude is not equal to altitude.
 */
void WimaArea::setMaxAltitude(double altitude)
{
    if ( altitude > 0 && qFuzzyCompare(altitude, _maxAltitude) ) {
        _maxAltitude = altitude;
        emit maxAltitudeChanged();
    }
}



/*!
 * \fn  QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const
 *  Returns the vertex of the polygon path with the least distance to \a coordinate.
 *
 * \sa QGeoCoordinate
 */
QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const
{
    return this->vertexCoordinate(getClosestVertexIndex(coordinate));
}

/*!
 * \fn QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &area)
 * Converts the \c WimaArea \a area to \c QGCMapPolygon by copying the path only.
 */
QGCMapPolygon WimaArea::toQGCPolygon(const WimaArea &area)
{
    QGCMapPolygon qgcPoly;
    qgcPoly.setPath(area.path());

    return QGCMapPolygon(qgcPoly);
}

/*!
 * \fn QGCMapPolygon WimaArea::toQGCPolygon() const
 * Converts the calling \c WimaArea to \c QGCMapPolygon by copying the path only.
 */
QGCMapPolygon WimaArea::toQGCPolygon() const
{
    return toQGCPolygon(*this);
}




/*!
 * \fn bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea &joinedArea)
 * Joins the areas \a area1 and \a area2 such that a \l {Simple Polygon} is created.
 * Stores the result inside \a joinedArea.
 * Returns \c true if the algorithm was able to join the areas; false else.
 * The algorithm will be able to join the areas, if either their edges intersect with each other,
 * or one area contains the other.
 */
bool WimaArea::join(WimaArea &area1, WimaArea &area2, WimaArea &joinedArea)
{
    QString dummy;
    return join(area1, area2, joinedArea, dummy);
}

/*!
 * \fn bool WimaArea::join(WimaArea &area)
 * Joins the calling \c WimaArea and the \a area such that a \l {Simple Polygon} is created.
 * Overwrites the calling \c WimaArea with the result, if the algorithm was successful.
 * Returns \c true if the algorithm was able to join the areas; false else.
 * The algorithm will be able to join the areas, if either their edges intersect with each other,
 * or one area contains the other.
 */
bool WimaArea::join(WimaArea &area)
{
    WimaArea joinedArea;
    if ( join(*this, area, joinedArea) ) {
        this->setPath(joinedArea.path());
        return true;
    } else {
        return false;
    }
}


/*!
 * \fn bool WimaArea::join(WimaArea &area, QString &errorString)
 * Joins the calling \c WimaArea and the \a area such that a \l {Simple Polygon} is created.
 * Overwrites the calling \c WimaArea with the result, if the algorithm was successful.
 *
 * Returns \c true if the algorithm was able to join the areas; false else.
 * Stores error messages in \a errorString.
 *
 * The algorithm will be able to join the areas, if either their edges intersect with each other,
 * or one area contains the other.
 */
bool WimaArea::join(WimaArea &area, QString &errorString)
{
    WimaArea joinedArea;
    if ( join(*this, area, joinedArea, errorString) ) {
        this->setPath(joinedArea.path());
        return true;
    } else {
        return false;
    }
}

/*!
 * \fn int WimaArea::nextVertexIndex(int index) const
 * Returns the index of the next vertex (of the areas path), which is \a index + 1 if \a index is smaller than \c {area.count() - 1},
 * or 0 if \a index equals \c {area.count() - 1}, or -1 if the \a index is out of bounds.
 * \note The function \c {area.count()} (derived from \c QGCMapPolygon) returns the number of vertices defining the area.
 */
int WimaArea::nextVertexIndex(int index) const
{
    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;
    }
}

/*!
 * \fn int WimaArea::previousVertexIndex(int index) const
 * Returns the index of the previous vertex (of the areas path), which is \a index - 1 if \a index is larger 0,
 * or \c {area.count() - 1} if \a index equals 0, or -1 if the \a index is out of bounds.
 * \note The function \c {area.count()} (derived from \c QGCMapPolygon) returns the number of vertices defining the area.
 */
int WimaArea::previousVertexIndex(int index) const
{
    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;
    }
}

/*!
 * \fn bool WimaArea::intersects(const QGCMapPolyline &line1, const QGCMapPolyline &line2, QGeoCoordinate &intersectionPt)
 * Returns \c true if \a line1 and \a line2 intersect with each other.
 * Stores the intersection point in \a intersectionPt
 *
 * \sa QGeoCoordinate
 */
bool WimaArea::intersects(const QGCMapPolyline &line1, const QGCMapPolyline &line2, QGeoCoordinate &intersectionPt)
{

        if (line1.count() == 2 && line2.count() == 2 ) {
            QPointF pt11(0, 0);

            double x, y, z;
            QGeoCoordinate origin = line1.vertexCoordinate(0);
            convertGeoToNed(line1.vertexCoordinate(1), origin, &x, &y, &z);
            QPointF pt12(x, y);

            QLineF kartLine1(pt11, pt12);


            convertGeoToNed(line2.vertexCoordinate(0), origin, &x, &y, &z);
            QPointF pt21(x, y);

            convertGeoToNed(line2.vertexCoordinate(1), origin, &x, &y, &z);
            QPointF pt22(x, y);;

            QLineF kartLine2(pt21, pt22);

            QPointF intersectionPoint;
            if (kartLine1.intersect(kartLine2, &intersectionPoint) == QLineF::BoundedIntersection) {
                convertNedToGeo(intersectionPoint.x(), intersectionPoint.y(), origin.altitude(), origin, &intersectionPt);
                return true;
            }
            else {
                return false;
            }


        } else {
            qWarning("WimaArea::intersect(line1, line2):  line1->count() != 2 || line2->count() != 2!");
            return false;
        }
}

/*!
 * \fn bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &neighbourList)
 * Returns \c true if \a line and \a area intersect with each other at least once.bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &neighbourList)
 * Stores the intersection points in \a intersectionList.
 * Stores the indices of the closest two \a area vetices for each of coorespoinding intersection points in \a neighbourList.
 *
 * For example if an intersection point is found between the first and the second vertex of the \a area the intersection point will
 * be stored in \a intersectionList and the indices 1 and 2 will be stored in \a neighbourList.
 * \a neighbourList has entries of type \c {QPair<int, int>}, where \c{pair.first} would contain 1 and \c{pair.second} would contain 2, when
 * relating to the above example.
 *
 * \sa QPair, QList
 */
bool WimaArea::intersects(const QGCMapPolyline &line, const WimaArea &area, QList<QGeoCoordinate> &intersectionList, QList<QPair<int, int>> &neighbourList)// don't seperate parameters with new lines or documentation will break
{
        intersectionList.clear();
        neighbourList.clear();


        if (line.count() == 2 && area.count() >= 3) { // are line a proper line and poly a proper poly?other,

            // Asseble a line form each tow consecutive polygon vertices and check whether it intersects with line
            for (int i = 0; i < area.count(); i++) {

                QGCMapPolyline interatorLine;
                QGeoCoordinate currentVertex    = area.vertexCoordinate(i);
                QGeoCoordinate nextVertex       = area.vertexCoordinate(area.nextVertexIndex(i));
                interatorLine.appendVertex(currentVertex);
                interatorLine.appendVertex(nextVertex);

                QGeoCoordinate intersectionPoint;
                if ( intersects(line, interatorLine, intersectionPoint) ){
                    intersectionList.append(intersectionPoint);

                    QPair<int, int>     neighbours;
                    neighbours.first    = i;
                    neighbours.second   = area.nextVertexIndex(i);
                    neighbourList.append(neighbours);
                }
            }

            if (intersectionList.count() > 0) {
                return true;
            } else {
                return false;
            }
        } else {
            qWarning("WimaArea::intersects(line, poly): line->count() != 2 || poly->count() < 3");
            return false;
        }
}

/*!other,
 * \fn double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea area)
 * Returns the distance between the coordinate \a c1 and coordinate \a c2, or infinity if the shortest path between
 * the two coordinates is not fully inside the \a area.
 * \note Both coordinates must lie inside the \a area.
 *
 * \sa QGeoCoordinate
 */
double WimaArea::distInsidePoly(const QGeoCoordinate &c1, const QGeoCoordinate &c2, WimaArea area)
{
        area.offset(0.1); // hack to compensate for numerical issues, migh be replaced in the future...
        if ( area.containsCoordinate(c1) && area.containsCoordinate(c2)) {
            QList<QGeoCoordinate>   intersectionList;
            QList<QPair<int, int>>  neighbourlist;
            QGCMapPolyline line;

            line.appendVertex(c1);
            line.appendVertex(c2);
            intersects(line, area, intersectionList, neighbourlist);

            if ( intersectionList.size() == 0 ){ // if an intersection was found the path between c1 and c2 is not fully inside area.
                return c1.distanceTo(c2);
            } else {
                return std::numeric_limits<qreal>::infinity();
            }

        } else {
            return std::numeric_limits<qreal>::infinity();
        }
}

/*!
 * \fn bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList<QGeoCoordinate> &dijkstraPath, QString &errorstring)
 * Calculates the shortest path (inside \a area) between \a start and \a end.
 * The \l {Dijkstra Algorithm} is used to find the shorest path.
 * Stores the result inside \a dijkstraPath when sucessfull.
 * Stores error messages in \a errorString.
 * Returns \c true if successful, \c false else.
 *
 * \sa QList
 */
bool WimaArea::dijkstraPath(const QGeoCoordinate &start, const QGeoCoordinate &end, const WimaArea &area, QList<QGeoCoordinate> &dijkstraPath, QString &errorString) // don't seperate parameters with new lines or documentation will break
{
    if ( isSelfIntersecting(area) ) {
        errorString.append("Area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n");
        return false;
    }

    // Each QGeoCoordinate gets stuff into a Node
    /// @param distance is the distance between the Node and it's predecessor
    struct Node{
        QGeoCoordinate coordinate;
        double distance = std::numeric_limits<qreal>::infinity();
        Node* predecessorNode = nullptr;
    };

    // The list with all Nodes (start, end + poly.path())
    QList<Node> nodeList;
    // This list will be initalized with (pointer to) all elements of nodeList.
    // Elements will be successively remove during the execution of the Dijkstra Algorithm.
    QList<Node*> workingSet;

    // initialize nodeList_maxAltitude
    // start cooridnate
    Node startNode;
    startNode.coordinate    = start;
    startNode.distance      = 0;
    nodeList.append(startNode);

    //poly cooridnates
    for (int i = 0; i < area.count(); i++) {
        Node node;
        node.coordinate = area.vertexCoordinate(i);
        nodeList.append(node);
    }

    //end coordinate
    Node endNode;
    endNode.coordinate  = end;
    nodeList.append(endNode);

    // initialize working set
    for (int i = 0; i < nodeList.size(); i++) {
        Node* nodePtr = &nodeList[i];
        workingSet.append(nodePtr);
    }


    // Dijkstra Algorithm
    // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
    while (workingSet.size() > 0) {
        // serach Node with minimal distance
        double minDist = std::numeric_limits<qreal>::infinity();
        int minDistIndex = 0;
        for (int i = 0; i < workingSet.size(); i++) {
            Node* node = workingSet.value(i);
            double dist = node->distance;
            if (dist < minDist) {
                minDist = dist;
                minDistIndex = i;
            }
        }
        Node* u = workingSet.takeAt(minDistIndex);


        //update distance
        for (int i = 0; i < workingSet.size(); i++) {
            Node* v = workingSet[i];

            // is neighbour? dist == infinity if no neihbour
            double dist = distInsidePoly(u->coordinate, v->coordinate, area);
            // is ther a alternative path which is shorter?
            double alternative = u->distance + dist;
            if (alternative < v->distance)  {
                v->distance         = alternative;
                v->predecessorNode  = u;
            }
        }

    }
    // end Djikstra Algorithm


    // check it the Algorithm was sucessfulepsilonMeter
    Node* Node = &nodeList.last();
    if (Node->predecessorNode == nullptr) {

    }

    // reverse assemble path
    while (1) {
        dijkstraPath.prepend(Node->coordinate);

        //Update Node
        Node = Node->predecessorNode;
        if (Node == nullptr) {
            if (dijkstraPath[0].distanceTo(start) < epsilonMeter)// check if starting point was reached
                break;
            qWarning("WimaArea::dijkstraPath(): Error, no path found!\n");
            return false;
        }
    }

    return true;
}

/*!
 * \fn bool WimaArea::isSelfIntersecting(const WimaArea &area)
 * Returns \c true if the \a area is self intersecting, \c false else.
 * \note If the \a area is self intersecting, it's not a \l {Simple Polygon}.
 */
bool WimaArea::isSelfIntersecting(const WimaArea &area)
{
    int i = 0;
    if (area.count() > 3) {
        // check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area
        while(i < area.count()-1) {
            QGeoCoordinate refBeginCoordinate = area.vertexCoordinate(i);
            QGeoCoordinate refEndCoordinate = area.vertexCoordinate(area.nextVertexIndex(i));
            QGCMapPolyline refLine;
            refLine.appendVertex(refBeginCoordinate);
            refLine.appendVertex(refEndCoordinate);
            int j = area.nextVertexIndex(i);
            while(j < area.count()) {
                QGeoCoordinate intersectionPt;
                QGCMapPolyline iteratorLine;
                iteratorLine.appendVertex(area.vertexCoordinate(j));
                iteratorLine.appendVertex(area.vertexCoordinate(area.nextVertexIndex(j)));

                if ( intersects(refLine, iteratorLine, intersectionPt) ){
                    if (    !(intersectionPt.distanceTo(refBeginCoordinate) < epsilonMeter)
                         && !(intersectionPt.distanceTo(refEndCoordinate)   < epsilonMeter) ) {
                        return true;
                    }
                }



                j++;
            }
            i++;
        }
    }

    return false;
}

/*!
 * \fn bool WimaArea::isSelfIntersecting()
 * Returns \c true if the calling area is self intersecting, \c false else.
 * \note If the calling area is self intersecting, it's not a \l {Simple Polygon}.
 */
bool WimaArea::isSelfIntersecting()
{
    return isSelfIntersecting(*this);
}

/*!
 * \fn void WimaArea::saveToJson(QJsonObject &json)
 * Saves the calling area to \c QJsonObject object and stores it inside \a json.
 */*!
* \fn  QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const
*  Returns the vertex of the polygon path with the least distance to \a coordinate.
*
* \sa QGeoCoordinate
*/
 * \sa QJsonObject
 */
void WimaArea::saveToJson(QJsonObject &json)
{
    this->QGCMapPolygon::saveToJson(json);
    json[maxAltitudeName]   = _maxAltitude;
    json[areaTypeName]      = wimaAreaName;
    // add WimaVehicle if necessary..
}

/*!
 * \fn bool WimaArea::loadFromJson(const QJsonObject &json, QString& errorString)
 * Loads data from \a json and stores it inside the calling area.
 * Returns \c true if loading was successful, \c false else.
 * Stores error messages inside \a errorString.
 *
 * \sa QJsonObject
 */
bool WimaArea::loadFromJson(const QJsonObject &json, QString& errorString)
{
    if ( this->QGCMapPolygon::loadFromJson(json, false /*no poly required*/, errorString) ) {
        if ( json.contains(maxAltitudeName) && json[maxAltitudeName].isDouble()) {
            _maxAltitude = json[maxAltitudeName].toDouble();
            return true;
        } else {
            errorString.append(tr("Could not load Maximum Altitude value!\n"));
            return false;
        }
    } else {
        qWarning() << errorString;
        return false;
    }
}

/*!
 * \fn void WimaArea::init()
 * Funtion to be called during construction.
 */
void WimaArea::init()
{
    this->setObjectName(wimaAreaName);
}

/*!
 * \fn void print(const WimaArea &area)
 * Prints the data contained in \a area to the console.
 */
void print(const WimaArea &area)
{
    QString message;
    print(area, message);
    qWarning() << messa/*!
               * \fn int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate) const
               * Returns the index of the vertex (element of the polygon path)
               * which has the least distance to \a coordinate.
               *
               * \sa QGeoCoordinate
               */
              int WimaArea::getClosestVertexIndex(const QGeoCoordinate &coordinate) const
              {
                  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;
                  }
              }ge;
}

/*!
 * \fn void print(const WimaArea &area)
 * Prints the data contained in \a area to the \a outputString.
 */
void print(const WimaArea &area, QString &outputString)
{
    outputString.append(QString("Type: %1\n").arg(area.objectName()));
    print(static_cast<const QGCMapPolygon&>(area), outputString);
    outputString.append(QString("Maximum Altitude: %1\n").arg(area._maxAltitude));
}


// QDoc Documentation

/*!
    \group WimaAreaGroup
    \title Group of WimaAreas

    Every \c WimaArea of the equally named group uses a \l {Simple Polygon} derived from \c {QGCMapPolygon}
    to define areas inside which certain taskts are performed.
*/

/*!
    \class WimaArea
    \inmodule Wima
    \ingroup WimaArea

    \brief The \c WimaArea class provides the a base class for
    all areas used within the Wima extension.

    \c WimaArea uses a \l {Simple Polygon} derived from \c {QGCMapPolygon}
    to define areas inside which certain taskts are performed. The polygon (often refered to as the path) can
    be displayed visually on a map.
*/

/*!
  \variable WimaArea::_maxAltitude
  \brief The maximum altitude vehicles are allowed to fly inside this area.
*/

/*!
  \property WimaArea::maxAltitude
  \brief The maximum altitude at which vehicles are allowed to fly.
*/

/*!
  \property WimaArea::mapVisualQML
  \brief A string containing the name of the QML file used to displays this area on a map.
*/

/*!
  \property WimaArea::editorQML
  \brief A string containing the name of the QML file allowing to edit the area's properties.
*/

/*!
    \externalpage https://en.wikipedia.org/wiki/Simple_polygon
    \title Simple Polygon
*/

/*!
    \externalpage https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
    \title Dijkstra Algorithm
*/



/*!
 * \fn  QGeoCoordinate WimaArea::getClosestVertex(const QGeoCoordinate& coordinate) const
 *  Returns the vertex of the polygon path with the least distance to \a coordinate.
 *
 * \sa QGeoCoordinate
 */

SphereCalculus::SphereCalculus(const SphereCalculus &other, QObject *parent)
    :   QObject (parent)
{
    *this = other;
}

SphereCalculus &SphereCalculus::operator=(const SphereCalculus &other)
{
    setEpsilonMeter(other.epsilonMeter());
}

void SphereCalculus::setEpsilonMeter(double epsilon)
{
    if (epsilon > 0) {
        _epsilonMeter = epsilon;
    }
}

double SphereCalculus::epsilonMeter() const
{
    return _epsilonMeter;
}

/*!
 * \fn int SphereCalculus::closestVertexIndex(const QList<QGeoCoordinate> &path, const QGeoCoordinate &coordinate) const
 * which has the least distance to \a coordinate.
 *
 * \sa QGeoCoordinate
 */
int SphereCalculus::closestVertexIndex(const QList<QGeoCoordinate> &path, const QGeoCoordinate &coordinate) const
{
    if (path.size() == 0) {
        qWarning("Path is empty!");
        return -1;
    }else if (path.size() == 1) {
        return 0;
    }else {
        int index = 0; // the index of the closest vertex
        double min_dist = coordinate.distanceTo(path[index]);
        for(int i = 1; i < path.size(); i++){
            double dist = coordinate.distanceTo(path[i]);
            if (dist < min_dist){
                min_dist = dist;
                index = i;
            }
        }
        return index;
    }
}

/*!
 * \fn  QGeoCoordinate SphereCalculus::closestVertex(const QList<QGeoCoordinate> &path, const QGeoCoordinate &coordinate) const
 *  Returns the vertex of the \a path with the least distance to \a coordinate.
 *
 * \sa QGeoCoordinate
 */
QGeoCoordinate SphereCalculus::closestVertex(const QList<QGeoCoordinate> &path, const QGeoCoordinate &coordinate) const
{
    int index = closestVertexIndex(path, coordinate);
    if (index >=0 ) {
        return path[index];
    } else {
        return QGeoCoordinate();
    }
}


/*!
 * \fn SphereCalculus::JoinPolygonError SphereCalculus::joinPolygon(const SphereCalculus::QList<QGeoCoordinate> &polygon1, const SphereCalculus::QList<QGeoCoordinate> &polygon2, SphereCalculus::QList<QGeoCoordinate> &joinedPolygon) const
 * Joins \a polygon1 and \a polygon such that a \l {Simple Polygon} is created.
 * Stores the result inside \a joinedArea.
 * Stores error messages in \a errorString.
 * Returns \c true if the algorithm was able to join the areas; false else.
 * The algorithm will be able to join the areas, if either their edges intersect with each other,
 * or one area contains the other.
 */
SphereCalculus::JoinPolygonError SphereCalculus::joinPolygon(const QList<QGeoCoordinate> &polygon1, const QList<QGeoCoordinate> &polygon2, QList<QGeoCoordinate> &joinedPolygon) const
{
    if (polygon1.size() >= 3 && polygon2.size() >= 3) {

        if ( isSimplePolygon(area1) ) {
            errorString.append("Area 1 is self intersecting.\n");
            return false;
        }

        if ( isSimplePolygon(area2) ) {
            errorString.append("Area 2 is self intersecting.\n");
            return false;
        }

        joinedArea.clear();

        area1.verifyClockwiseWinding();
        area2.verifyClockwiseWinding();

        WimaArea* walkerPoly    = &area1; // "walk" on this polygon towards higher indices
        WimaArea* crossPoly     = &area2; // check for crossings with this polygon while "walking"
                                         // and swicht to this polygon on a intersection,
                                         // continue to walk towards higher indices

        // begin with the first index which is not inside crosspoly, if all Vertices are inside crosspoly return crosspoly
        int startIndex = 0;
        bool crossContainsWalker = true;
        for (int i = 0; i < walkerPoly->count(); i++) {
            if ( !crossPoly->containsCoordinate(walkerPoly->vertexCoordinate(i)) ) {
                crossContainsWalker = false;
                startIndex = i;
                break;
            }
        }


        if ( crossContainsWalker == true) {
            joinedArea.appendVertices(crossPoly->QList<QGeoCoordinate>());
            return true;
        }


        QGeoCoordinate currentVertex    = walkerPoly->vertexCoordinate(startIndex);
        QGeoCoordinate startVertex      = currentVertex;
        // possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly)
        QGeoCoordinate protoNextVertex  = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(startIndex));

        int nextVertexIndex = walkerPoly->nextVertexIndex(startIndex);
        while (1) {
            //qDebug("nextVertexIndex: %i", nextVertexIndex);
            joinedArea.appendVertex(currentVertex);

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

            QList<QPair<int, int>> neighbourList;
            QList<QGeoCoordinate> intersectionList;
            //qDebug("IntersectionList.size() on init: %i", intersectionList.size());
            intersects(walkerPolySegment, *crossPoly, intersectionList, neighbourList);

            //qDebug("IntersectionList.size(): %i", intersectionList.size());

            if (intersectionList.size() >= 1) {
                int minDistIndex = 0;

                if (intersectionList.size() > 1) {
                    double minDist = currentVertex.distanceTo(intersectionList.value(minDistIndex));
                    for (int i = 1; i < intersectionList.size(); i++) {
                        double currentDist = currentVertex.distanceTo(intersectionList.value(i));

                        if ( minDist > currentDist ) {
                            minDist         = currentDist;
                            minDistIndex    = i;
                        }
                    }
                }

                //qDebug("MinDistIndex: %i", minDistIndex);
                QGeoCoordinate protoCurrentVertex = intersectionList.value(minDistIndex);
                // take numerical erros into account
                if (protoCurrentVertex.distanceTo(currentVertex) > epsilonMeter) {
                    currentVertex                   = protoCurrentVertex;
                    QPair<int, int> neighbours      = neighbourList.value(minDistIndex);
                    protoNextVertex                 = crossPoly->vertexCoordinate(neighbours.second);
                    nextVertexIndex                 = neighbours.second;

                    // swap walker and cross poly
                    WimaArea* temp  = walkerPoly;
                    walkerPoly      = crossPoly;
                    crossPoly       = temp;
                } else {
                    currentVertex   = walkerPoly->vertexCoordinate(nextVertexIndex);
                    protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(nextVertexIndex));
                    nextVertexIndex = walkerPoly->nextVertexIndex(nextVertexIndex);
                }

            } else {
                currentVertex   = walkerPoly->vertexCoordinate(nextVertexIndex);
                protoNextVertex = walkerPoly->vertexCoordinate(walkerPoly->nextVertexIndex(nextVertexIndex));
                nextVertexIndex = walkerPoly->nextVertexIndex(nextVertexIndex);
            }

            if (currentVertex == startVertex) {
                if (area1.count() == joinedArea.count()) { // is the case if poly1 and poly2 don't intersect
                    return false;
                } else {
                    return true;
                }
            }
        }

    } else {
        return SphereCalculus::JoinPolygonError::PathSizeLow;
    }
}



