WimaController.h

#pragma once

#include <QObject>
#include <QScopedPointer>

#include "QGCMapPolygon.h"
#include "QmlObjectListModel.h"

#include "WimaArea.h"
#include "WimaMeasurementArea.h"
#include "WimaServiceArea.h"
#include "WimaCorridor.h"
#include "WimaDataContainer.h"
#include "WimaMeasurementAreaData.h"
#include "WimaCorridorData.h"
#include "WimaServiceAreaData.h"

#include "PlanMasterController.h"
#include "MissionController.h"
#include "SurveyComplexItem.h"
#include "SimpleMissionItem.h"
#include "MissionSettingsItem.h"
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "SettingsFact.h"
#include "WimaSettings.h"
#include "SettingsManager.h"

#include "snake.h"
#include "SnakeWorker.h"
#include "SnakeTiles.h"
#include "SnakeTilesLocal.h"
#include "GeoPoint3D.h"
#include "QNemoProgress.h"

#include "ros_bridge/include/ROSBridge.h"

#define CHECK_BATTERY_INTERVAL 1000 // ms
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 5000 // ms


using namespace snake;

typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;

class WimaController : public QObject
{
    Q_OBJECT

    enum FileType {WimaFile, PlanFile};

    typedef QScopedPointer<ROSBridge::ROSBridge> ROSBridgePtr;

public:

    enum SnakeConnectionStatus {Connected = 1, NotConnected = 0};


    WimaController(QObject *parent = nullptr);


    Q_PROPERTY(PlanMasterController*    masterController
               READ                     masterController
               WRITE                    setMasterController
               NOTIFY                   masterControllerChanged
               )
    Q_PROPERTY(MissionController*       missionController
               READ                     missionController
               WRITE                    setMissionController
               NOTIFY                   missionControllerChanged
               )
    Q_PROPERTY(QmlObjectListModel*      visualItems
               READ                     visualItems
               NOTIFY                   visualItemsChanged
               )
//    Q_PROPERTY(QString                  currentFile             READ currentFile                                            NOTIFY currentFileChanged)
//    Q_PROPERTY(QStringList              loadNameFilters         READ loadNameFilters                                        CONSTANT)
//    Q_PROPERTY(QStringList              saveNameFilters         READ saveNameFilters                                        CONSTANT)
//    Q_PROPERTY(QString                  fileExtension           READ fileExtension                                          CONSTANT)
    Q_PROPERTY(WimaDataContainer*   dataContainer
               READ                 dataContainer
               WRITE                setDataContainer
               NOTIFY               dataContainerChanged
               )
    Q_PROPERTY(QmlObjectListModel*  missionItems
               READ                 missionItems
               NOTIFY               missionItemsChanged
               )
    Q_PROPERTY(QmlObjectListModel*  currentMissionItems
               READ                 currentMissionItems
               NOTIFY               currentMissionItemsChanged
               )
    Q_PROPERTY(QVariantList waypointPath
               READ         waypointPath
               NOTIFY       waypointPathChanged
               )
    Q_PROPERTY(QVariantList currentWaypointPath
               READ         currentWaypointPath
               NOTIFY       currentWaypointPathChanged
               )
    Q_PROPERTY(Fact*                    enableWimaController
               READ enableWimaController
               CONSTANT)
    Q_PROPERTY(Fact*    overlapWaypoints
               READ     overlapWaypoints
               CONSTANT
               )
    Q_PROPERTY(Fact* maxWaypointsPerPhase
               READ  maxWaypointsPerPhase
               CONSTANT
               )
    Q_PROPERTY(Fact*    startWaypointIndex
               READ     startWaypointIndex
               CONSTANT
               )
    Q_PROPERTY(Fact*    showAllMissionItems
               READ     showAllMissionItems
               CONSTANT
               )
    Q_PROPERTY(Fact*    showCurrentMissionItems
               READ     showCurrentMissionItems
               CONSTANT
               )
    Q_PROPERTY(Fact*    flightSpeed
               READ     flightSpeed
               CONSTANT
               )
    Q_PROPERTY(Fact*    altitude
               READ     altitude
               CONSTANT
               )
    Q_PROPERTY(Fact*    arrivalReturnSpeed
               READ     arrivalReturnSpeed
               CONSTANT
               )
    Q_PROPERTY(Fact*    reverse
               READ     reverse
               CONSTANT
               )
    Q_PROPERTY(bool     uploadOverrideRequired
               READ     uploadOverrideRequired
               WRITE    setUploadOverrideRequired
               NOTIFY   uploadOverrideRequiredChanged
               )
    Q_PROPERTY(double   phaseDistance
               READ     phaseDistance
               NOTIFY   phaseDistanceChanged
               )
    Q_PROPERTY(double   phaseDuration
               READ     phaseDuration
               NOTIFY   phaseDurationChanged
               )
    Q_PROPERTY(bool     vehicleHasLowBattery
               READ     vehicleHasLowBattery
               NOTIFY   vehicleHasLowBatteryChanged
               )

    // Snake
    Q_PROPERTY(Fact*    enableSnake
               READ     enableSnake
               CONSTANT
               )
    Q_PROPERTY(long     snakeConnectionStatus
               READ     snakeConnectionStatus
               NOTIFY   snakeConnectionStatusChanged
               )
    Q_PROPERTY(bool     snakeCalcInProgress
               READ     snakeCalcInProgress
               NOTIFY   snakeCalcInProgressChanged
               )
    Q_PROPERTY(Fact*    snakeTileWidth
               READ     snakeTileWidth
               CONSTANT
               )
    Q_PROPERTY(Fact*    snakeTileHeight
               READ     snakeTileHeight
               CONSTANT
               )
    Q_PROPERTY(Fact*    snakeMinTileArea
               READ     snakeMinTileArea
               CONSTANT
               )
    Q_PROPERTY(Fact*    snakeLineDistance
               READ     snakeLineDistance
               CONSTANT
               )
    Q_PROPERTY(Fact*    snakeMinTransectLength
               READ     snakeMinTransectLength
               CONSTANT
               )
    Q_PROPERTY(QmlObjectListModel*  snakeTiles
               READ                 snakeTiles
               NOTIFY               snakeTilesChanged
               )
    Q_PROPERTY(QVariantList snakeTileCenterPoints
               READ         snakeTileCenterPoints
               NOTIFY       snakeTileCenterPointsChanged
               )
    Q_PROPERTY(QVector<int>     nemoProgress
               READ             nemoProgress
               NOTIFY           nemoProgressChanged
               )



    // Property accessors
    PlanMasterController*       masterController       (void);
    MissionController*          missionController      (void);
    QmlObjectListModel*         visualItems            (void);
//    QString                     currentFile            (void) const     { return _currentFile; }
//    QStringList                 loadNameFilters        (void) const;
//    QStringList                 saveNameFilters        (void) const;
//    QString                     fileExtension          (void) const     { return wimaFileExtension; }
    QGCMapPolygon               joinedArea             (void) const;
    WimaDataContainer*          dataContainer          (void)           { return _container; }
    QmlObjectListModel*         missionItems           (void)           { return &_missionItems; }
    QmlObjectListModel*         currentMissionItems    (void)           { return &_currentMissionItems; }
    QVariantList                waypointPath           (void) const;
    QVariantList                currentWaypointPath    (void)           { return _currentWaypointPath; }
    Fact*                       enableWimaController   (void)           { return &_enableWimaController; }
    Fact*                       overlapWaypoints       (void)           { return &_overlapWaypoints; }
    Fact*                       maxWaypointsPerPhase   (void)           { return &_maxWaypointsPerPhase; }
    Fact*                       startWaypointIndex     (void)           { return &_nextPhaseStartWaypointIndex; }
    Fact*                       showAllMissionItems    (void)           { return &_showAllMissionItems; }
    Fact*                       showCurrentMissionItems(void)           { return &_showCurrentMissionItems; }
    Fact*                       flightSpeed            (void)           { return &_flightSpeed; }
    Fact*                       arrivalReturnSpeed     (void)           { return &_arrivalReturnSpeed; }
    Fact*                       altitude               (void)           { return &_altitude; }
    Fact*                       reverse                (void)           { return &_reverse; }

    Fact*                       enableSnake            (void)           { return &_enableSnake; }
    Fact*                       snakeTileWidth         (void)           { return &_snakeTileWidth;}
    Fact*                       snakeTileHeight        (void)           { return &_snakeTileHeight;}
    Fact*                       snakeMinTileArea       (void)           { return &_snakeMinTileArea;}
    Fact*                       snakeLineDistance      (void)           { return &_snakeLineDistance;}
    Fact*                       snakeMinTransectLength (void)           { return &_snakeMinTransectLength;}
    QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
    QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
    QVector<int>                nemoProgress           (void)           { return _nemoProgress.progress();}

    bool                        uploadOverrideRequired (void) const;
    double                      phaseDistance          (void) const;
    double                      phaseDuration          (void) const;
    bool                        vehicleHasLowBattery   (void) const;

    long                        snakeConnectionStatus (void) const;
    bool                        snakeCalcInProgress   (void) const;


    // Property setters
    void setMasterController        (PlanMasterController* masterController);
    void setMissionController       (MissionController* missionController);
    void setDataContainer           (WimaDataContainer* container);
    void setUploadOverrideRequired  (bool overrideRequired);

    // Member Methodes
    Q_INVOKABLE void nextPhase();
    Q_INVOKABLE void previousPhase();
    Q_INVOKABLE void resetPhase();
    Q_INVOKABLE bool uploadToVehicle();
    Q_INVOKABLE bool forceUploadToVehicle();
    Q_INVOKABLE void removeFromVehicle();    
    Q_INVOKABLE bool checkSmartRTLPreCondition(); // wrapper for _checkSmartRTLPreCondition(QString &errorString)
    Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)#
    Q_INVOKABLE void executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring)
    Q_INVOKABLE void initSmartRTL();
    Q_INVOKABLE void removeVehicleTrajectoryHistory();


//    Q_INVOKABLE void saveToCurrent  ();
//    Q_INVOKABLE void saveToFile     (const QString& filename);
//    Q_INVOKABLE bool loadFromCurrent();
//    Q_INVOKABLE bool loadFromFile   (const QString& filename);


    // static Members
//    static const char* wimaFileExtension;
    static const char* areaItemsName;
    static const char* missionItemsName;    
    static const char* settingsGroup;
    static const char* endWaypointIndexName;
    static const char* enableWimaControllerName;
    static const char* overlapWaypointsName;
    static const char* maxWaypointsPerPhaseName;
    static const char* startWaypointIndexName;
    static const char* showAllMissionItemsName;
    static const char* showCurrentMissionItemsName;
    static const char* flightSpeedName;
    static const char* arrivalReturnSpeedName;
    static const char* altitudeName;
    static const char* reverseName;
    static const char* snakeTileWidthName;
    static const char* snakeTileHeightName;
    static const char* snakeMinTileAreaName;
    static const char* snakeLineDistanceName;
    static const char* snakeMinTransectLengthName;

    // Member Methodes
    QJsonDocument saveToJson(FileType fileType);

    bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path);

signals:
    void masterControllerChanged            (void);
    void missionControllerChanged           (void);
    void visualItemsChanged                 (void);
//    void currentFileChanged                 ();
    void dataContainerChanged               ();
    void readyForSaveSendChanged            (bool ready);
    void missionItemsChanged                (void);
    void currentMissionItemsChanged         (void);
    void waypointPathChanged                (void);
    void currentWaypointPathChanged         (void);
    void uploadOverrideRequiredChanged      (void);
    void phaseDistanceChanged               (void);
    void phaseDurationChanged               (void);
    void vehicleHasLowBatteryChanged        (void);
    void returnBatteryLowConfirmRequired    (void);
    void returnUserRequestConfirmRequired   (void);
    void snakeConnectionStatusChanged       (void);
    void snakeCalcInProgressChanged         (void);
    void snakeTilesChanged                  (void);
    void snakeTileCenterPointsChanged       (void);
    void nemoProgressChanged                (void);
private:
    enum SRTL_Reason {BatteryLow, UserRequest};
private slots:
    bool _fetchContainerData();
    bool _calcNextPhase(void);
    void _updateWaypointPath                 (void);
    void _updateCurrentPath                  (void);
    void _updateNextWaypoint                 (void);
    void _recalcCurrentPhase                 (void);
    bool _setTakeoffLandPosition             (void);
    void _updateflightSpeed                  (void);
    void _updateArrivalReturnSpeed           (void);
    void _updateAltitude                     (void);
    void _checkBatteryLevel                  (void);
    void _eventTimerHandler                  (void);
    void _smartRTLCleanUp                    (bool flying); // cleans up after successfull smart RTL
    void _enableDisableLowBatteryHandling    (QVariant enable);
    void _reverseChangedHandler              ();
    void _initSmartRTL                       ();
    void _executeSmartRTL                    ();
    void _setSnakeConnectionStatus           (SnakeConnectionStatus status);
    void _setSnakeCalcInProgress             (bool inProgress);
    bool _verifyScenarioDefined                    (void);
    bool _verifyScenarioDefinedWithErrorMessage    (void);
    void _snakeStoreWorkerResults            ();
    void _startStopRosBridge                 ();
    void _initStartSnakeWorker               ();

private:
    void _setPhaseDistance(double distance);
    void _setPhaseDuration(double duration);
    bool _checkSmartRTLPreCondition(QString &errorString); // should be called from gui, befor calcReturnPath()
    bool _calcReturnPath(QString &errorSring); // Calculates return path (destination: service area center) for a flying vehicle
    void _setVehicleHasLowBattery(bool batteryLow);
    void _loadCurrentMissionItemsFromBuffer();
    void _saveCurrentMissionItemsToBuffer();

    void _progressFromJson(JsonDocUPtr pDoc,
                           QNemoProgress &progress);

    PlanMasterController   *_masterController;
    MissionController      *_missionController;
//    QString                 _currentFile; // file for saveing
    WimaDataContainer      *_container; // container for data exchange with WimaController
    QmlObjectListModel      _visualItems; // contains all visible areas
    WimaJoinedAreaData      _joinedArea; // joined area fromed by opArea, serArea, _corridor
    WimaMeasurementAreaData _measurementArea; // measurement area
    WimaServiceAreaData     _serviceArea; // area for supplying
    WimaCorridorData        _corridor; // corridor connecting opArea and serArea
    bool                    _localPlanDataValid;
    QmlObjectListModel      _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView
    QmlObjectListModel      _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems,
                                                  // _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage
    QmlObjectListModel      _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked
    QVector<QGeoCoordinate> _waypoints; // path connecting the items in _missionItems
    QVariantList            _currentWaypointPath; // path connecting the items in _currentMissionItems
    QGeoCoordinate          _takeoffLandPostion;


    QMap<QString, FactMetaData*> _metaDataMap;
    SettingsFact                 _enableWimaController; // enables or disables the wimaControler
    SettingsFact                 _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases
    SettingsFact                 _maxWaypointsPerPhase; // determines the maximum number waypoints per phase
    SettingsFact                 _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
                                                      // defining the first element of the next phase
    SettingsFact                 _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not.
    SettingsFact                 _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not.
    SettingsFact                 _flightSpeed; // mission flight speed
    SettingsFact                 _arrivalReturnSpeed; // arrival and return path speed
    SettingsFact                 _altitude; // mission altitude
    SettingsFact                 _reverse; // Reverses the phase direction. Phases go from high to low waypoint numbers, if true.
    SettingsFact                 _enableSnake; // Enable Snake (see snake.h)

    int                          _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element
                                                    // (which is not part of the return path) of _currentMissionItem
    int                          _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element
                                                      // (which is not part of the arrival path) of _currentMissionItem
    bool                         _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area.
                                                          // The user can override the upload lock with a slider, this will reset this variable to false.
    double                       _measurementPathLength; // the lenght of the phase in meters
    double                       _arrivalPathLength; // the length of the arrival and return path in meters
    double                       _returnPathLength; // the length of the arrival and return path in meters
    double                       _phaseDistance; // the lenth in meters of the current phase
    double                       _phaseDuration; // the phase duration in seconds
    double                       _phaseDistanceBuffer; // buffer for storing _phaseDistance when doing smart RTL
    double                       _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL

    QTimer                       _eventTimer;
    QTimer                       _smartRTLAttemptTimer;
    SRTL_Reason                  _srtlReason;

    bool                         _vehicleHasLowBattery;
    bool                         _lowBatteryHandlingTriggered;
    bool                         _executingSmartRTL;

    // Snake
    SnakeConnectionStatus        _snakeConnectionStatus;
    bool                         _snakeCalcInProgress;
    bool                         _snakeRecalcNecessary;
    bool                         _scenarioDefinedBool;
    SnakeWorker                  _snakeWorker;
    Scenario                     _scenario;
    SettingsFact                 _snakeTileWidth;
    SettingsFact                 _snakeTileHeight;
    SettingsFact                 _snakeMinTileArea;
    SettingsFact                 _snakeLineDistance;
    SettingsFact                 _snakeMinTransectLength;
    ::GeoPoint3D                 _snakeOrigin;
    SnakeTiles                   _snakeTiles; // tiles
    SnakeTilesLocal              _snakeTilesLocal; // tiles local coordinate system
    QVariantList                 _snakeTileCenterPoints;
    QNemoProgress                _nemoProgress; // measurement progress

    ROSBridgePtr                 _pRosBridge;

};

/*
 * The following explains the structure of
 * _missionController.visualItems().
 *
 * Index        Description
 * --------------------------------------------
 * 0            MissionSettingsItem
 * 1            Takeoff Command
 * 2            Speed Command: arrivalReturnSpeed
 * 3            Arrival Path Waypoint 0
 * ...
 * 3+n-1        Arrival Path Waypoint n-1
 * 3+n          Speed Command: flightSpeed
 * 3+n+1        Circular Survey Waypoint 0
 * ...
 * 3+n+m        Circular Survey Waypoint m-1
 * 3+n+m+1      Speed Command: arrivalReturnSpeed
 * 3+n+m+2      Return Path Waypoint 0
 * ...
 * 3+n+m+2+l    Return Path Waypoint l-1
 * 3+n+m+2+l+1  Land command
 *
 * _currentMissionItems is equal to
 * _missionController.visualItems() except that it
 * is missing the MissionSettingsItem
 */



template<class CoordinateType>
CoordinateType getCoordinate(const SimpleMissionItem* item){
    return item->coordinate();
}

template<>
QVariant getCoordinate<QVariant>(const SimpleMissionItem* item);


/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool getCoordinates(QmlObjectListModel &missionItems,
                    ContainerType<CoordinateType> &coordinateList,
                    std::size_t startIndex,
                    std::size_t  endIndex){

    if (   startIndex < std::size_t(missionItems.count())
        && endIndex < std::size_t(missionItems.count()) ) {
        if (startIndex > endIndex) {
            if (!getCoordinates(missionItems, coordinateList, startIndex, missionItems.count()-1))
                return false;
            if (!getCoordinates(missionItems, coordinateList, 0, endIndex))
                return false;
        } else {
            for (std::size_t i = startIndex; i <= endIndex; i++) {
                SimpleMissionItem *mItem = missionItems.value<SimpleMissionItem *>(int(i));

                if (mItem == nullptr) {
                    coordinateList.clear();
                    return false;
                }
                coordinateList.append(getCoordinate<CoordinateType>(mItem));
            }
        }
    } else
        return false;

    return true;
}

/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool getCoordinates(QmlObjectListModel &missionItems,
                    ContainerType<CoordinateType> &coordinateList){

    return getCoordinates(missionItems, coordinateList, 0, missionItems.count());
}