WimaController.h

#pragma once

#include <QObject>
#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/snake.h"

#define CHECK_BATTERY_INTERVAL 1000
#define SMART_RTL_MAX_ATTEMPTS 3
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms


using namespace snake;


class WimaController : public QObject
{
    Q_OBJECT

    enum FileType {WimaFile, PlanFile};

public:
    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)



    // Property accessors
    PlanMasterController*       masterController       (void)          { return _masterController; }
    MissionController*          missionController      (void)          { return _missionController; }
    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);
    QmlObjectListModel*         missionItems           (void);
    QmlObjectListModel*         currentMissionItems    (void);
    QVariantList                waypointPath           (void);
    QVariantList                currentWaypointPath    (void);
    Fact*                       enableWimaController   (void);
    Fact*                       overlapWaypoints       (void);
    Fact*                       maxWaypointsPerPhase   (void);
    Fact*                       startWaypointIndex     (void);
    Fact*                       showAllMissionItems    (void);
    Fact*                       showCurrentMissionItems(void);
    Fact*                       flightSpeed            (void);
    Fact*                       arrivalReturnSpeed     (void);
    Fact*                       altitude               (void);    
    Fact*                       reverse                (void);
    bool                        uploadOverrideRequired (void) const;
    double                      phaseDistance          (void) const;
    double                      phaseDuration          (void) const;
    bool                        vehicleHasLowBattery   (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;

    // Member Methodes
    QJsonDocument saveToJson(FileType fileType);

    bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path);
    /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList
    bool extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList);
    /// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList.
    bool extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList, int startIndex, int endIndex);
    /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList
    bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList);
    /// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList.
    bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex);

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);
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 smartRTLCleanUp                    (bool flying); // cleans up after successfull smart RTL
    void enableDisableLowBatteryHandling    (QVariant enable);
    void reverseChangedHandler              ();
    void _initSmartRTL                      ();
    void _executeSmartRTL                   ();


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();

private:
    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
    QVariantList            _waypointPath; // 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.

    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                       _checkBatteryTimer;
    QTimer                       _smartRTLAttemptTimer;
    SRTL_Reason                  _srtlReason;

    bool                         _vehicleHasLowBattery;
    bool                         _lowBatteryHandlingTriggered;
    bool                         _executingSmartRTL;

    Scenario                     _snakeScenario;
    FlightPlan                   _snakeFlightPlan;
};

/*
 * The following explains the structure of
 * _missionController.visualItems(). The indices
 * are not that important and only specified for
 * reasons of completeness.
 *
 * 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
 */