CircularSurveyComplexItem.cc

#include "CircularSurveyComplexItem.h"
#include "JsonHelper.h"
#include "QGCApplication.h"

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

const char* CircularSurveyComplexItem::jsonComplexItemTypeValue     =   "circularSurvey";
const char* CircularSurveyComplexItem::jsonDeltaRKey                =   "deltaR";
const char* CircularSurveyComplexItem::jsonDeltaAlphaKey            =   "deltaAlpha";
const char* CircularSurveyComplexItem::jsonReferencePointLatKey     =   "referencePointLat";
const char* CircularSurveyComplexItem::jsonReferencePointLongKey    =   "referencePointLong";
const char* CircularSurveyComplexItem::jsonReferencePointAltKey     =   "referencePointAlt";

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

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

    _deltaR.setRawValue(_deltaR.rawDefaultValue());
    _deltaAlpha.setRawValue(_deltaAlpha.rawDefaultValue());
    qDebug() << _deltaAlpha.rawDefaultValue().toDouble();
    qDebug() << _deltaAlpha.rawValue().toDouble();

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

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

        emit refPointChanged();
        //qDebug() << _referencePoint.toString();
    }
}

void CircularSurveyComplexItem::setAutoGenerated(bool autoGen)
{
    if (autoGen != _autoGenerated) {
        _autoGenerated = autoGen;

        emit autoGeneratedChanged();
    }
}

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

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

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

bool CircularSurveyComplexItem::autoGenerated()
{
    return _autoGenerated;
}

bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString)
{
    // We need to pull version first to determine what validation/conversion needs to be performed
    QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
        { JsonHelper::jsonVersionKey, QJsonValue::Double, true },
    };
    if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, errorString)) {
        return false;
    }

    int version = complexObject[JsonHelper::jsonVersionKey].toInt();
    if (version != 1) {
        errorString = tr("Survey items do not support version %1").arg(version);
        return false;
    }

    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
        { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
        { jsonDeltaRKey,                                QJsonValue::Double, true },
        { jsonDeltaAlphaKey,                            QJsonValue::Double, true },
        { jsonReferencePointLatKey,                     QJsonValue::Double, true },
        { jsonReferencePointLongKey,                     QJsonValue::Double, true },
        { jsonReferencePointAltKey,                     QJsonValue::Double, true },
    };


    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
        return false;
    }

    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
        errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
        return false;
    }

    _ignoreRecalc = true;

    setSequenceNumber(sequenceNumber);

    if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) {
        _surveyAreaPolygon.clear();
        return false;
    }

    if (!_load(complexObject, errorString)) {
        _ignoreRecalc = false;
        return false;
    }

    _deltaR.setRawValue             (complexObject[jsonDeltaRKey].toDouble());
    _deltaAlpha.setRawValue         (complexObject[jsonDeltaAlphaKey].toDouble());
    _referencePoint.setLongitude    (complexObject[jsonReferencePointLongKey].toDouble());
    _referencePoint.setLatitude     (complexObject[jsonReferencePointLatKey].toDouble());
    _referencePoint.setAltitude     (complexObject[jsonReferencePointAltKey].toDouble());
    _autoGenerated = true;

    _ignoreRecalc = false;

    _recalcComplexDistance();
    if (_cameraShots == 0) {
        // Shot count was possibly not available from plan file
        _recalcCameraShots();
    }

    return true;
}

void CircularSurveyComplexItem::save(QJsonArray &planItems)
{
    QJsonObject saveObject;

    _save(saveObject);

    saveObject[JsonHelper::jsonVersionKey] =                    1;
    saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;

    saveObject[jsonDeltaRKey]               = _deltaR.rawValue().toDouble();
    saveObject[jsonDeltaAlphaKey]           = _deltaAlpha.rawValue().toDouble();
    saveObject[jsonReferencePointLongKey]   = _referencePoint.longitude();
    saveObject[jsonReferencePointLatKey]    = _referencePoint.latitude();
    saveObject[jsonReferencePointAltKey]    = _referencePoint.altitude();

    // Polygon shape
    _surveyAreaPolygon.saveToJson(saveObject);

    planItems.append(saveObject);
}

void CircularSurveyComplexItem::appendMissionItems(QList<MissionItem *> &items, QObject *missionItemParent)
{
    if (_loadedMissionItems.count()) {
        // We have mission items from the loaded plan, use those
        _appendLoadedMissionItems(items, missionItemParent);
    } else {
        // Build the mission items on the fly
        _buildAndAppendMissionItems(items, missionItemParent);
    }
}

void CircularSurveyComplexItem::_appendLoadedMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    //qCDebug(SurveyComplexItemLog) << "_appendLoadedMissionItems";

    int seqNum = _sequenceNumber;

    for (const MissionItem* loadedMissionItem: _loadedMissionItems) {
        MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent);
        item->setSequenceNumber(seqNum++);
        items.append(item);
    }
}

void CircularSurveyComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
    // original code: SurveyComplexItem::_buildAndAppendMissionItems()
    //qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems";

    // Now build the mission items from the transect points

    MissionItem* item;
    int seqNum =                    _sequenceNumber;
    // bool imagesEverywhere =         _cameraTriggerInTurnAroundFact.rawValue().toBool();
    // bool addTriggerAtBeginning =    !hoverAndCaptureEnabled() && imagesEverywhere;
    bool firstOverallPoint =        true;

    MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;

    for (const QList<TransectStyleComplexItem::CoordInfo_t>& transect: _transects) {
        //bool transectEntry = true;

        for (const CoordInfo_t& transectCoordInfo: transect) {
            item = new MissionItem(seqNum++,
                                   MAV_CMD_NAV_WAYPOINT,
                                   mavFrame,
                                   0,                                           // Hold time (delay for hover and capture to settle vehicle before image is taken)
                                   0.0,                                         // No acceptance radius specified
                                   0.0,                                         // Pass through waypoint
                                   std::numeric_limits<double>::quiet_NaN(),    // Yaw unchanged
                                   transectCoordInfo.coord.latitude(),
                                   transectCoordInfo.coord.longitude(),
                                   transectCoordInfo.coord.altitude(),
                                   true,                                        // autoContinue
                                   false,                                       // isCurrentItem
                                   missionItemParent);
            items.append(item);
            // implement capture if desired
//            if (hoverAndCaptureEnabled()) {
//                item = new MissionItem(seqNum++,
//                                       MAV_CMD_IMAGE_START_CAPTURE,
//                                       MAV_FRAME_MISSION,
//                                       0,                                   // Reserved (Set to 0)
//                                       0,                                   // Interval (none)
//                                       1,                                   // Take 1 photo
//                                       qQNaN(), qQNaN(), qQNaN(), qQNaN(),  // param 4-7 reserved
//                                       true,                                // autoContinue
//                                       false,                               // isCurrentItem
//                                       missionItemParent);
//                items.append(item);
//            }

//            if (firstOverallPoint && addTriggerAtBeginning) {
//                // Start triggering
//                addTriggerAtBeginning = false;
//                item = new MissionItem(seqNum++,
//                                       MAV_CMD_DO_SET_CAM_TRIGG_DIST,
//                                       MAV_FRAME_MISSION,
//                                       triggerDistance(),   // trigger distance
//                                       0,                   // shutter integration (ignore)
//                                       1,                   // trigger immediately when starting
//                                       0, 0, 0, 0,          // param 4-7 unused
//                                       true,                // autoContinue
//                                       false,               // isCurrentItem
//                                       missionItemParent);
//                items.append(item);
//            }
            firstOverallPoint = false;

//            // Possibly add trigger start/stop to survey area entrance/exit
//            if (triggerCamera() && !hoverAndCaptureEnabled() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) {
//                if (transectEntry) {
//                    // Start of transect, always start triggering. We do this even if we are taking images everywhere.
//                    // This allows a restart of the mission in mid-air without losing images from the entire mission.
//                    // At most you may lose part of a transect.
//                    item = new MissionItem(seqNum++,
//                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
//                                           MAV_FRAME_MISSION,
//                                           triggerDistance(),   // trigger distance
//                                           0,                   // shutter integration (ignore)
//                                           1,                   // trigger immediately when starting
//                                           0, 0, 0, 0,          // param 4-7 unused
//                                           true,                // autoContinue
//                                           false,               // isCurrentItem
//                                           missionItemParent);
//                    items.append(item);
//                    transectEntry = false;
//                } else if (!imagesEverywhere && !transectEntry){
//                    // End of transect, stop triggering
//                    item = new MissionItem(seqNum++,
//                                           MAV_CMD_DO_SET_CAM_TRIGG_DIST,
//                                           MAV_FRAME_MISSION,
//                                           0,           // stop triggering
//                                           0,           // shutter integration (ignore)
//                                           0,           // trigger immediately when starting
//                                           0, 0, 0, 0,  // param 4-7 unused
//                                           true,        // autoContinue
//                                           false,       // isCurrentItem
//                                           missionItemParent);
//                    items.append(item);
//                }
//            }
        }
    }

    // implemetn photo capture if desired
//    if (triggerCamera() && !hoverAndCaptureEnabled() && imagesEverywhere) {
//        // Stop triggering
//        MissionItem* item = new MissionItem(seqNum++,
//                                            MAV_CMD_DO_SET_CAM_TRIGG_DIST,
//                                            MAV_FRAME_MISSION,
//                                            0,           // stop triggering
//                                            0,           // shutter integration (ignore)
//                                            0,           // trigger immediately when starting
//                                            0, 0, 0, 0,  // param 4-7 unused
//                                            true,        // autoContinue
//                                            false,       // isCurrentItem
//                                            missionItemParent);
//        items.append(item);
//    }
}

void CircularSurveyComplexItem::applyNewAltitude(double newAltitude)
{
    _cameraCalc.valueSetIsDistance()->setRawValue(true);
    _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
    _cameraCalc.setDistanceToSurfaceRelative(true);
}

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

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

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.0*M_PI; // radiants
    double dr = _deltaR.rawValue().toDouble(); // meter
//    double dalpha = 1.0/180.0*M_PI; // radiants
//    double dr = 10.0; // 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();
    if ( currentSection.isEmpty() )
        return;
    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);
        }
    }

    optiPath.append(currentSection); // append last section


    // convert to CoordInfo_t
    for ( const QList<QPointF> &transect : optiPath) {
//    for ( const QList<QPointF> &transect : fullPath) {
        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()
{
    _complexDistance = 0;
    for (int i=0; i<_visualTransectPoints.count() - 1; i++) {
        _complexDistance += _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(_visualTransectPoints[i+1].value<QGeoCoordinate>());
    }
    emit complexDistanceChanged();
}

// no cameraShots in Circular Survey, add if desired
void CircularSurveyComplexItem::_recalcCameraShots()
{
    _cameraShots = 0;
}

void CircularSurveyComplexItem::_updateItem()
{
    if (_dirty) {
        _rebuildTransects();
        qDebug() << "CircularSurveyComplexItem::_updateItem()";
        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
*/