#include "CircularSurveyComplexItem.h"


const char* CircularSurveyComplexItem::settingsGroup =              "Survey";
const char* CircularSurveyComplexItem::deltaRName =                 "DeltaR";
const char* CircularSurveyComplexItem::deltaAlphaName =             "DeltaAlpha";

CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent)
    :   TransectStyleComplexItem    (vehicle, flyView, settingsGroup, parent)
    ,   _referencePoint             (QGeoCoordinate(47.770859, 16.531076,0))
    ,   _metaDataMap                (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this))
    ,   _deltaR                     (settingsGroup, _metaDataMap[deltaRName])
    ,   _deltaAlpha                 (settingsGroup, _metaDataMap[deltaAlphaName])
{
    _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";

    //connect(&_deltaR,       &Fact::valueChanged, this, &CircularSurveyComplexItem::_setDirty);
    //connect(&_deltaAlpha,   &Fact::valueChanged, this, &CircularSurveyComplexItem::_setDirty);


    connect(&_updateTimer, &QTimer::timeout, this, &CircularSurveyComplexItem::_updateItem);
    _updateTimer.start(100);
}

void CircularSurveyComplexItem::setRefPoint(const QGeoCoordinate &refPt)
{
    if (refPt != _referencePoint){
        _referencePoint = refPt;

        emit refPointChanged();
    }
}

QGeoCoordinate CircularSurveyComplexItem::refPoint() const
{
    return _referencePoint;
}

Fact *CircularSurveyComplexItem::deltaR()
{
    return &_deltaR;
}

Fact *CircularSurveyComplexItem::deltaAlpha()
{
    return &_deltaAlpha;
}

bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString)
{
    return false;
}

void CircularSurveyComplexItem::save(QJsonArray &planItems)
{

}

void CircularSurveyComplexItem::appendMissionItems(QList<MissionItem *> &items, QObject *missionItemParent)
{

}

void CircularSurveyComplexItem::applyNewAltitude(double newAltitude)
{

}

double CircularSurveyComplexItem::timeBetweenShots()
{
    return 1;
}

bool CircularSurveyComplexItem::readyForSave() const
{
    return false;
}

double CircularSurveyComplexItem::additionalTimeDelay() const
{
    return 0;
}

void CircularSurveyComplexItem::_rebuildTransectsPhase1()
{
    using namespace GeoUtilities;
    using namespace PolygonCalculus;
    using namespace PlanimetryCalculus;

    if ( _surveyAreaPolygon.count() < 3)
        return;

    _transects.clear();
    QPolygonF surveyPolygon = toQPolygonF(toCartesian2D(_surveyAreaPolygon.coordinateList(), _referencePoint));

    QVector<double> distances;
    for (const QPointF &p : surveyPolygon) distances.append(norm(p));

    // check if input is valid
    if (   _deltaAlpha.rawValue() > _deltaAlpha.rawMax()
           && _deltaAlpha.rawValue() < _deltaAlpha.rawMin())
        return;
    if (   _deltaR.rawValue() > _deltaR.rawMax()
           && _deltaR.rawValue() < _deltaR.rawMin())
        return;


    double dalpha = _deltaAlpha.rawValue().toDouble()/180*M_PI; // radiants
    double dr = _deltaR.rawValue().toDouble(); // meter
    double r_min = dr; // meter
    double r_max = (*std::max_element(distances.begin(), distances.end())); // meter

    QPointF origin(0, 0);
    IntersectType type;
    bool originInside = true;
    if (!contains(surveyPolygon, origin, type)) {
        QVector<double> angles;
        for (const QPointF &p : surveyPolygon) angles.append(truncateAngle(angle(p)));

        // determine r_min by successive approximation
        double r = r_min;
        while ( r < r_max) {
            Circle circle(r, origin);

            if (intersects(circle, surveyPolygon)) {
                r_min = r;
                break;
            }

            r += dr;
        }
        originInside = false;
    }

//    qWarning("r_min, r_max:");
//    qWarning() << r_min;
//    qWarning() << r_max;

    QList<QPolygonF> convexPolygons;
    decomposeToConvex(surveyPolygon, convexPolygons);

    QList<QList<QPointF>> fullPath;
    for (int i = 0; i < convexPolygons.size(); i++) {
        const QPolygonF &polygon = convexPolygons[i];
        double r = r_min;

        QList<QList<QPointF>> currPolyPath;
        while (r < r_max) {
            Circle circle(r, origin);
            QList<QPointFList> intersectPoints;
            QList<IntersectType> typeList;
            QList<QPair<int, int>> neighbourList;
            if (intersects(circle, polygon, intersectPoints, neighbourList, typeList)) {

                // intersection Points between circle and polygon, entering polygon
                // when walking in counterclockwise direction along circle
                QPointFList entryPoints;
                // intersection Points between circle and polygon, leaving polygon
                // when walking in counterclockwise direction along circle
                QPointFList exitPoints;
                // determine entryPoints and exit Points
                for (int j = 0; j < intersectPoints.size(); j++) {
                    QList<QPointF> intersects = intersectPoints[j];

                    QPointF p1 = polygon[neighbourList[j].first];
                    QPointF p2 = polygon[neighbourList[j].second];
                    QLineF intersetLine(p1, p2);
                    double lineAngle = truncateAngle(angle(intersetLine));

                    for (QPointF ipt : intersects) {
                        double circleTangentAngle = truncateAngle(angle(ipt)+M_PI_2);
                        // compare line angle and circle tangent at intersection point
                        // to determine between exit and entry point
                        if (   !qFuzzyCompare(lineAngle, circleTangentAngle)
                            && !qFuzzyCompare(lineAngle, truncateAngle(circleTangentAngle + M_PI))) {
                            if (truncateAngle(lineAngle - circleTangentAngle)  < M_PI) {
                                entryPoints.append(ipt);
                            } else {
                                exitPoints.append(ipt);
                            }
                        }
                    }
                }

                // sort
                std::sort(entryPoints.begin(), entryPoints.end(), [](QPointF p1, QPointF p2) {
                   return angle(p1) < angle(p2);
                });
                std::sort(exitPoints.begin(), exitPoints.end(), [](QPointF p1, QPointF p2) {
                   return angle(p1) < angle(p2);
                });

                // match entry and exit points
                int offset = 0;
                double minAngle = std::numeric_limits<double>::infinity();
                for (int k = 0; k < exitPoints.size(); k++) {
                    QPointF pt = exitPoints[k];
                    double alpha = truncateAngle(angle(pt) - angle(entryPoints[0]));
                    if (minAngle > alpha) {
                        minAngle = alpha;
                        offset = k;
                    }
                }

                for (int k = 0; k < entryPoints.size(); k++) {
                    double alpha1 = angle(entryPoints[k]);
                    double alpha2 = angle(exitPoints[(k+offset) % entryPoints.size()]);

                    QList<QPointF> sectorPath = circle.approximateSektor(double(dalpha), alpha1, alpha2);
                    // use shortestPath() here if necessary, could be a problem if dr >>
                    if (sectorPath.size() > 0)
                        currPolyPath.append(sectorPath);
                }
            } else if (originInside) {
                // circle fully inside polygon
                QList<QPointF> sectorPath = circle.approximateSektor(double(dalpha), 0, 2*M_PI);
                // use shortestPath() here if necessary, could be a problem if dr >>
                currPolyPath.append(sectorPath);
            }
            r += dr;
         }
        if (currPolyPath.size() > 0) {
            fullPath.append(currPolyPath);
        }
    }

    // optimize path to lawn pattern
    if (fullPath.size() == 0)
        return;
    QList<QPointF> currentSection = fullPath.takeFirst();
    QList<QList<QPointF>> optiPath; // optimized path
    while( !fullPath.empty() ) {
        optiPath.append(currentSection);
        QPointF endVertex = currentSection.last();
        double minDist = std::numeric_limits<double>::infinity();
        int index = 0;
        bool reversePath = false;

        // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection
        for (int i = 0; i < fullPath.size(); i++) {
            auto iteratorPath = fullPath[i];
            double dist = PlanimetryCalculus::distance(endVertex, iteratorPath.first());
            if ( dist < minDist ) {
                minDist = dist;
                index = i;
            }
            dist = PlanimetryCalculus::distance(endVertex, iteratorPath.last());
            if (dist < minDist) {
                minDist = dist;
                index = i;
                reversePath = true;
            }
        }
        currentSection = fullPath.takeAt(index);
        if (reversePath) {
            PolygonCalculus::reversePath(currentSection);
        }
    }


    // convert to CoordInfo_t
    for ( const QList<QPointF> &transect : optiPath) {
        QList<QGeoCoordinate> geoPath = toGeo(transect, _referencePoint);
        QList<CoordInfo_t> transectList;
        for ( const QGeoCoordinate &coordinate : geoPath) {
            CoordInfo_t coordinfo = {coordinate, CoordTypeInterior};
            transectList.append(coordinfo);
        }
        _transects.append(transectList);
    }

}

void CircularSurveyComplexItem::_recalcComplexDistance()
{

}

void CircularSurveyComplexItem::_recalcCameraShots()
{

}

void CircularSurveyComplexItem::_updateItem()
{
    if (_dirty) {
        _rebuildTransects();
        setDirty(false);
    }

}

/*!
    \class CircularSurveyComplexItem
    \inmodule Wima

    \brief The \c CircularSurveyComplexItem class provides a survey mission item with circular transects around a point of interest.

    CircularSurveyComplexItem class provides a survey mission item with circular transects around a point of interest. Within the
    \c Wima module it's used to scan a defined area with constant angle (circular transects) to the base station (point of interest).

    \sa WimaArea
*/