#include "PolygonCalculus.h"
#include "PlanimetryCalculus.h"
#include "OptimisationTools.h"

#include <QVector3D>

#include <functional>

namespace PolygonCalculus {
    namespace  {
        bool isReflexVertex(const QPolygonF& polygon, const QPointF  *vertex) {
            // Original Code from SurveyComplexItem::_VertexIsReflex()
            auto vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
            auto vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
            auto area = ( ((vertex->x() - vertexBefore->x())*(vertexAfter->y() - vertexBefore->y()))
                         -((vertexAfter->x() - vertexBefore->x())*(vertex->y() - vertexBefore->y())));
            return area > 0;
        }

    } // end anonymous namespace

    /*!
     * \fn bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
     * Returns true if the shortest path between the two coordinates is not fully inside the \a area.
     *
     * \sa QPointF, QPolygonF
     */
    bool containsPath(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
    {

        if ( !polygon.isEmpty()) {
            if (   !polygon.containsPoint(c1, Qt::FillRule::OddEvenFill)
                || !polygon.containsPoint(c2, Qt::FillRule::OddEvenFill))
                return false;

            QLineF line(c1, c2);
            if (PlanimetryCalculus::intersectsFast(polygon, line))
                return false;
            return true;
        } else {
            return false;
        }
    }

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

    /*!auto distance
     * \fn  QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate);
     *  Returns the vertex of \a polygon with the least distance to \a coordinate.
     *
     * \sa QPointF, QPolygonF
     */
    QPointF closestVertex(const QPolygonF &polygon, const QPointF &coordinate)
    {
        int index = closestVertexIndex(polygon, coordinate);
        if (index >=0 ) {
            return polygon[index];
        } else {
            return QPointF();
        }
    }

    /*!
     * \fn int nextPolygonIndex(int pathsize, int index)
     * Returns the index of the next vertex (of a polygon), which is \a index + 1 if \a index is smaller than \c {\a pathsize - 1},
     * or 0 if \a index equals \c {\a pathsize - 1}, or -1 if the \a index is out of bounds.
     * \note \a pathsize is usually obtained by invoking polygon.size()
     */
    int nextVertexIndex(int pathsize, int index)
    {
        if (index >= 0 && index < pathsize-1) {
            return index + 1;
        } else if (index == pathsize-1) {
            return 0;
        } else {
            qWarning("nextPolygonIndex(): Index out of bounds! index:count = %i:%i", index, pathsize);
            return -1;
        }
    }

    /*!
     * \fn int previousPolygonIndex(int pathsize, int index)
     * Returns the index of the previous vertex (of a polygon), which is \a index - 1 if \a index is larger 0,
     * or \c {\a pathsize - 1} if \a index equals 0, or -1 if the \a index is out of bounds.
     * \note pathsize is usually obtained by invoking polygon.size()
     */
    int previousVertexIndex(int pathsize, int index)
    {
        if (index > 0 && index <pathsize) {
            return index - 1;
        } else if (index == 0) {
            return pathsize-1;
        } else {
            qWarning("previousVertexIndex(): Index out of bounds! index:count = %i:%i", index, pathsize);
            return -1;
        }
    }

    /*!
     * \fn JoinPolygonError joinPolygon(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon);
     * Joins \a polygon1 and \a polygon2 such that a \l {Simple Polygon} is created.
     * Stores the result inside \a joinedArea.
     * Returns \c NotSimplePolygon1 if \a polygon1 isn't a Simple Polygon, \c NotSimplePolygon2 if \a polygon2 isn't a Simple Polygon, \c Disjoind if the polygons are disjoint,
     * \c PathSizeLow if at least one polygon has a size samler than 3, or \c PolygonJoined on success.
     * The algorithm will be able to join the areas, if either their edges intersect with each other,
     * or one area is inside the other.
     * The algorithm assumes that \a joinedPolygon is empty.
     */
    JoinPolygonError join(QPolygonF polygon1, QPolygonF polygon2, QPolygonF &joinedPolygon)
    {
        using namespace PlanimetryCalculus;
        if (polygon1.size() >= 3 && polygon2.size() >= 3) {

            if ( !isSimplePolygon(polygon1) || !isSimplePolygon(polygon2)) {
                return JoinPolygonError::NotSimplePolygon;
            }

            if (polygon1 == polygon2) {
                joinedPolygon = polygon1;
                return JoinPolygonError::PolygonJoined;
            }
            if ( !hasClockwiseWinding(polygon1)) {
                reversePath(polygon1);
            }
            if ( !hasClockwiseWinding(polygon2)) {
                reversePath(polygon2);
            }

            const QPolygonF *walkerPoly    = &polygon1; // "walk" on this polygon towards higher indices
            const QPolygonF *crossPoly     = &polygon2; // 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->size(); i++) {
                if ( !contains(*crossPoly, walkerPoly->at(i)) ) {
                    crossContainsWalker = false;
                    startIndex = i;
                    break;
                }
            }

            if ( crossContainsWalker == true) {
                joinedPolygon.append(*crossPoly);
                return JoinPolygonError::PolygonJoined;
            }
            QPointF currentVertex    = walkerPoly->at(startIndex);
            QPointF startVertex      = currentVertex;
            // possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly)
            int nextVertexNumber     = nextVertexIndex(walkerPoly->size(), startIndex);
            QPointF protoNextVertex  = walkerPoly->value(nextVertexNumber);
            long counter = 0;
            while (1) {
                //qDebug("nextVertexNumber: %i", nextVertexNumber);
                joinedPolygon.append(currentVertex);

                QLineF walkerPolySegment;
                walkerPolySegment.setP1(currentVertex);
                walkerPolySegment.setP2(protoNextVertex);

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

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

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

                    double minDist = std::numeric_limits<double>::infinity();
                    for (int i = 0; i < intersectionList.size(); i++) {
                        double currentDist = PlanimetryCalculus::distance(currentVertex, intersectionList[i]);

                        if ( minDist > currentDist && !qFuzzyIsNull(distance(currentVertex, intersectionList[i])) ) {
                            minDist         = currentDist;
                            minDistIndex    = i;
                        }
                    }

                    if (minDistIndex != -1){
                        currentVertex                   = intersectionList.value(minDistIndex);
                        QPair<int, int> neighbours      = neighbourList.value(minDistIndex);
                        protoNextVertex                 = crossPoly->value(neighbours.second);
                        nextVertexNumber                 = neighbours.second;

                        // switch walker and cross poly
                        const QPolygonF *temp   = walkerPoly;
                        walkerPoly              = crossPoly;
                        crossPoly               = temp;
                    } else {
                        currentVertex    = walkerPoly->value(nextVertexNumber);
                        nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
                        protoNextVertex  = walkerPoly->value(nextVertexNumber);
                    }

                } else {
                    currentVertex    = walkerPoly->value(nextVertexNumber);
                    nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber);
                    protoNextVertex  = walkerPoly->value(nextVertexNumber);
                }

                if (currentVertex == startVertex) {
                    if (polygon1.size() == joinedPolygon.size()) {
                        for (int i = 0; i < polygon1.size(); i++) {
                            if (polygon1[i] != joinedPolygon[i])
                                return PolygonJoined;
                        }
                        return Disjoint;
                    } else {
                        return PolygonJoined;
                    }
                }

                counter++;
                if (counter > 1e5) {
                    qWarning("PolygonCalculus::join(): no successfull termination!");
                    return Error;
                }
            }

        } else {
            return PathSizeLow;
        }
    }

    /*!
     * \fn bool isSimplePolygon(const QPolygonF &polygon);
     * Returns \c true if \a polygon is a \l {Simple Polygon}, \c false else.
     * \note A polygon is a Simple Polygon iff it is not self intersecting.
     */
    bool isSimplePolygon(const QPolygonF &polygon)
    {
        int i = 0;
        if (polygon.size() > 3) {
            // check if any edge of the area (formed by two adjacent vertices) intersects with any other edge of the area
            while(i < polygon.size()-1) {
                double cCIntersectCounter = 0; // corner corner intersection counter
                QPointF refBeginCoordinate = polygon[i];
                QPointF refEndCoordinate = polygon[nextVertexIndex(polygon.size(), i)];
                QLineF refLine;
                refLine.setP1(refBeginCoordinate);
                refLine.setP2(refEndCoordinate);
                int j = nextVertexIndex(polygon.size(), i);
                while(j < polygon.size()) {

                    QPointF intersectionPt;
                    QLineF iteratorLine;
                    iteratorLine.setP1(polygon[j]);
                    iteratorLine.setP2(polygon[nextVertexIndex(polygon.size(), j)]);
                    PlanimetryCalculus::IntersectType intersectType;
                    PlanimetryCalculus::intersects(refLine, iteratorLine, intersectionPt, intersectType);
                    if ( intersectType == PlanimetryCalculus::CornerCornerIntersection) {
                        cCIntersectCounter++;
                        // max two corner corner intersections allowed, a specific coordinate can appear only once in a simple polygon
                    }
                    if ( cCIntersectCounter > 2
                         || intersectType == PlanimetryCalculus::EdgeEdgeIntersection
                         || intersectType == PlanimetryCalculus::EdgeCornerIntersection
                         || intersectType == PlanimetryCalculus::LinesEqual
                         || intersectType == PlanimetryCalculus::Error){
                        return false;
                    }

                    j++;
                }
                i++;
            }
        }
        return true;
    }

    /*!
     * \fn bool hasClockwiseWinding(const QPolygonF &polygon)
     * Returns \c true if \a path has clockwiauto distancese winding, \c false else.
     */
    bool hasClockwiseWinding(const QPolygonF &polygon)
    {
        if (polygon.size() <= 2) {
            return false;
        }

        double sum = 0;
        for (int i=0; i <polygon.size(); i++) {
            QPointF coord1 = polygon[i];
            QPointF coord2 = polygon[nextVertexIndex(polygon.size(), i)];

            sum += (coord2.x() - coord1.x()) * (coord2.y() + coord1.y());
        }

        if (sum < 0.0)
            return false;

        return true;
    }

    /*!
     * \fn void reversePath(QPolygonF &polygon)
     * Reverses the path, i.e. changes the first Vertex with the last, the second with the befor last and so forth.
     */
    void reversePath(QPolygonF &polygon)
    {
        QPolygonF pathReversed;
        for (int i = 0; i < polygon.size(); i++) {
            pathReversed.prepend(polygon[i]);
        }
        polygon.clear();
        polygon.append(pathReversed);
    }

    /*!
     * \fn void offsetPolygon(QPolygonF &polygon, double offset)
     * Offsets a \a polygon by the given \a offset. The algorithm assumes that polygon is a \l {SimplePolygon}.
     */
    void offsetPolygon(QPolygonF &polygon, double offset)
    {
        // Original code from QGCMapPolygon::offset()
        QPolygonF newPolygon;
        if (polygon.size() > 2) {

            // Walk the edges, offsetting by theauto distance specified distance
            QList<QLineF> rgOffsetEdges;
            for (int i = 0; i < polygon.size(); i++) {
                int     nextIndex = nextVertexIndex(polygon.size(), i);
                QLineF  offsetEdge;
                QLineF  originalEdge(polygon[i], polygon[nextIndex]);

                QLineF workerLine = originalEdge;
                workerLine.setLength(offset);
                workerLine.setAngle(workerLine.angle() - 90.0);
                offsetEdge.setP1(workerLine.p2());

                workerLine.setPoints(originalEdge.p2(), originalEdge.p1()); bool             containsPath        (const QPointF &c1, const QPointF &c2, QPolygonF polygon);
                workerLine.setLength(offset);
                workerLine.setAngle(workerLine.angle() + 90.0);
                offsetEdge.setP2(workerLine.p2());

                rgOffsetEdges << offsetEdge;
            }

            // Intersect the offset edges to generate new vertices
            polygon.clear();
            QPointF         newVertex;
            for (int i=0; i<rgOffsetEdges.count(); i++) {
                int prevIndex = previousVertexIndex(rgOffsetEdges.size(), i);
                if (rgOffsetEdges[prevIndex].intersect(rgOffsetEdges[i], &newVertex) == QLineF::NoIntersection) {
                    // FIXME: Better error handling?
                    qWarning("Intersection failed");
                    return;
                }
                polygon.append(newVertex);
            }
        }
    }

    void decomposeToConvex(const QPolygonF &polygon, QList<QPolygonF> &convexPolygons)
    {
        // Original Code SurveyComplexItem::_PolygonDecomposeConvex()
        // this follows "Mark Keil's Algorithm" https://mpen.ca/406/keil
        int decompSize = std::numeric_limits<int>::max();
        if (polygon.size() < 3) return;
        if (polygon.size() == 3) {
            convexPolygons << polygon;
            return;
        }

        QList<QPolygonF> decomposedPolygonsMin{};

        for (const QPointF *vertex = polygon.begin(); vertex != polygon.end(); ++vertex)
        {
            // is vertex reflex?
            bool vertexIsReflex = isReflexVertex(polygon, vertex);

            if (!vertexIsReflex) continue;

            for (const QPointF *vertexOther = polygon.begin(); vertexOther != polygon.end(); ++vertexOther)
            {
                const QPointF *vertexBefore = vertex == polygon.begin() ? polygon.end() - 1 : vertex - 1;
                const QPointF *vertexAfter = vertex == polygon.end() - 1 ? polygon.begin() : vertex + 1;
                if (vertexOther == vertex) continue;
                if (vertexAfter == vertexOther) continue;
                if (vertexBefore == vertexOther) continue;
                bool canSee = containsPath(polygon, *vertex, *vertexOther);
                if (!canSee) continue;

                QPolygonF polyLeft;
                const QPointF *v = vertex;
                bool polyLeftContainsReflex = false;
                while ( v != vertexOther) {
                    if (v != vertex && isReflexVertex(polygon, v)) {
                        polyLeftContainsReflex = true;
                    }
                    polyLeft << *v;
                    ++v;
                    if (v == polygon.end()) v = polygon.begin();
                }
                polyLeft << *vertexOther;
                bool polyLeftValid = !(polyLeftContainsReflex && polyLeft.size() == 3);

                QPolygonF polyRight;
                v = vertexOther;
                bool polyRightContainsReflex = false;
                while ( v != vertex) {
                    if (v != vertex && isReflexVertex(polygon, v)) {
                        polyRightContainsReflex = true;
                    }
                    polyRight << *v;
                    ++v;
                    if (v == polygon.end()) v = polygon.begin();
                }
                polyRight << *vertex;
                auto polyRightValid = !(polyRightContainsReflex && polyRight.size() == 3);

                if (!polyLeftValid || ! polyRightValid) {
    //                decompSize = std::numeric_limits<int>::max();
                    continue;
                }

                // recursion
                QList<QPolygonF> polyLeftDecomposed{};
                decomposeToConvex(polyLeft, polyLeftDecomposed);

                QList<QPolygonF> polyRightDecomposed{};
                decomposeToConvex(polyRight, polyRightDecomposed);

                // compositon
                int subSize = polyLeftDecomposed.size() + polyRightDecomposed.size();
                if (   (polyLeftContainsReflex && polyLeftDecomposed.size() == 1)
                    || (polyRightContainsReflex && polyRightDecomposed.size() == 1))
                {
                    // don't accept polygons that contian reflex vertices and were not split
                    subSize = std::numeric_limits<int>::max();
                }
                if (subSize < decompSize) {
                    decompSize = subSize;
                    decomposedPolygonsMin = polyLeftDecomposed + polyRightDecomposed;
                }
            }
        }

        // assemble output
        if (decomposedPolygonsMin.size() > 0) {
            convexPolygons << decomposedPolygonsMin;
        } else {
            convexPolygons << polygon;
        }

        return;
    }

    bool shortestPath(const QPolygonF &polygon,const QPointF &startVertex, const QPointF &endVertex, QVector<QPointF> &shortestPath)
    {
        using namespace PlanimetryCalculus;
        QPolygonF bigPolygon(polygon);
        offsetPolygon(bigPolygon, 0.1); // solves numerical errors
        if (    bigPolygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill)
             && bigPolygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) {

            QVector<QPointF> elementList;
            elementList.append(startVertex);
            elementList.append(endVertex);
            for (auto vertex : polygon)  elementList.append(vertex);
            const int listSize = elementList.size();


            std::function<double(const int, const int)> distanceDij = [bigPolygon, elementList](const int ind1,  const int ind2) -> double {
                QPointF p1 = elementList[ind1];
                QPointF p2 = elementList[ind2];
                double dist = std::numeric_limits<double>::infinity();
                if (containsPathFast(bigPolygon, p1, p2)){ // containsPathFast can be used since all points are inside bigPolygon
                    double dx = p1.x()-p2.x();
                    double dy = p1.y()-p2.y();
                    dist =  sqrt((dx*dx)+(dy*dy));
                }

                return dist;
            };

            QVector<int> shortestPathIndex;
            bool retVal = OptimisationTools::dijkstraAlgorithm(listSize, 0, 1, shortestPathIndex, distanceDij);
            for (auto index : shortestPathIndex) shortestPath.append(elementList[index]);
            return retVal;
        } else {
            return false;
        }
    }

    QVector3DList toQVector3DList(const QPolygonF &polygon)
    {
        QVector3DList list;
        for ( auto vertex : polygon )
            list.append(QVector3D(vertex));

        return list;
    }

    QPolygonF toQPolygonF(const QPointFList &listF)
    {
        QPolygonF polygon;
        for ( auto vertex : listF )
            polygon.append(vertex);

        return polygon;
    }

    QPointFList toQPointFList(const QPolygonF &polygon)
    {
        QPointFList listF;
        for ( auto vertex : polygon )
            listF.append(vertex);

        return listF;
    }

    void reversePath(QPointFList &path)
    {
        QPointFList pathReversed;
        for ( auto element : path) {
            pathReversed.prepend(element);
        }
        path = pathReversed;
    }

    void reversePath(QPointFVector &path)
    {
        QPointFVector pathReversed;
        for ( auto element : path) {
            pathReversed.prepend(element);
        }
        path = pathReversed;
    }



    bool containsPathFast(QPolygonF polygon, const QPointF &c1, const QPointF &c2)
    {

        if ( !polygon.isEmpty()) {
            QLineF line(c1, c2);
            if (PlanimetryCalculus::intersectsFast(polygon, line))
                return false;
            return true;
        } else {
            return false;
        }
    }



} // end PolygonCalculus namespace