WimaController.h

#pragma once

#include <QObject>
#include <QScopedPointer>

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

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

#include "WimaPlanData.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 "Snake/SnakeWorker.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"
#include "Geometry/GeoPoint3D.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"

#include "ros_bridge/include/ROSBridge.h"

#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"

#include <map>

#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 1000 // 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:


    WimaController(QObject *parent = nullptr);

    // Controllers.
    Q_PROPERTY(PlanMasterController*    masterController
               READ                     masterController
               WRITE                    setMasterController
               NOTIFY                   masterControllerChanged
               )
    Q_PROPERTY(MissionController*       missionController
               READ                     missionController
               WRITE                    setMissionController
               NOTIFY                   missionControllerChanged
               )
    // Wima Data.
    Q_PROPERTY(QmlObjectListModel*      visualItems
               READ                     visualItems
               NOTIFY                   visualItemsChanged
               )
    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)
    // Waypoint navigaton.
    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
               )
    // Waypoint settings.
    Q_PROPERTY(Fact*    flightSpeed
               READ     flightSpeed
               CONSTANT
               )
    Q_PROPERTY(Fact*    altitude
               READ     altitude
               CONSTANT
               )
    Q_PROPERTY(Fact*    arrivalReturnSpeed
               READ     arrivalReturnSpeed
               CONSTANT
               )
    // Waypoint statistics.
    Q_PROPERTY(double   phaseDistance
               READ     phaseDistance
               NOTIFY   phaseDistanceChanged
               )
    Q_PROPERTY(double   phaseDuration
               READ     phaseDuration
               NOTIFY   phaseDurationChanged
               )

    // Snake
    Q_PROPERTY(Fact*    enableSnake
               READ     enableSnake
               CONSTANT
               )
    Q_PROPERTY(int      nemoStatus
               READ     nemoStatus
               NOTIFY   nemoStatusChanged
               )
    Q_PROPERTY(QString  nemoStatusString
               READ     nemoStatusString
               NOTIFY   nemoStatusStringChanged
               )
    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
    // Controllers.
    PlanMasterController*       masterController       (void);
    MissionController*          missionController      (void);
    // Wima Data
    QmlObjectListModel*         visualItems            (void);
    QGCMapPolygon               joinedArea             (void) const;
    // Waypoints.
    QmlObjectListModel*         missionItems           (void);
    QmlObjectListModel*         currentMissionItems    (void);
    QVariantList                waypointPath           (void);
    QVariantList                currentWaypointPath    (void);
    // Settings facts.
    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);
    // Snake settings facts.
    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;}
    // Snake data.
    QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
    QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
    QVector<int>                nemoProgress           (void);
    int                         nemoStatus             (void) const;
    QString                     nemoStatusString       (void) const;
    bool                        snakeCalcInProgress    (void) const;

    // Smart RTL.
    bool                        uploadOverrideRequired (void) const;
    bool                        vehicleHasLowBattery   (void) const;
    // Waypoint statistics.
    double                      phaseDistance          (void) const;
    double                      phaseDuration          (void) const;


    // Property setters
    void setMasterController        (PlanMasterController* masterController);
    void setMissionController       (MissionController* missionController);
    bool setWimaPlanData            (const WimaPlanData &planData);

    // Member Methodes
    Q_INVOKABLE WimaController *thisPointer();
    // Waypoint navigation.
    Q_INVOKABLE void nextPhase();
    Q_INVOKABLE void previousPhase();
    Q_INVOKABLE void resetPhase();
    // Smart RTL.
    Q_INVOKABLE void requestSmartRTL();
    Q_INVOKABLE void initSmartRTL();
    Q_INVOKABLE void executeSmartRTL();
    // Other.
    Q_INVOKABLE void removeVehicleTrajectoryHistory();
    Q_INVOKABLE bool upload();
    Q_INVOKABLE bool forceUpload();
    Q_INVOKABLE void removeFromVehicle();


    // static Members
    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* snakeTileWidthName;
    static const char* snakeTileHeightName;
    static const char* snakeMinTileAreaName;
    static const char* snakeLineDistanceName;
    static const char* snakeMinTransectLengthName;

signals:
    // Controllers.
    void masterControllerChanged            (void);
    void missionControllerChanged           (void);
    // Wima data.
    void visualItemsChanged                 (void);
    // Waypoints.
    void missionItemsChanged                (void);
    void currentMissionItemsChanged         (void);
    void waypointPathChanged                (void);
    void currentWaypointPathChanged         (void);
    // Smart RTL.
    void smartRTLRequestConfirm             (void);
    void smartRTLPathConfirm                (void);
    // Upload.
    void forceUploadConfirm                 (void);
    // Waypoint statistics.
    void phaseDistanceChanged               (void);
    void phaseDurationChanged               (void);
    // Snake.
    void snakeConnectionStatusChanged       (void);
    void snakeCalcInProgressChanged         (void);
    void snakeTilesChanged                  (void);
    void snakeTileCenterPointsChanged       (void);
    void nemoProgressChanged                (void);
    void nemoStatusChanged                  (void);
    void nemoStatusStringChanged            (void);

private slots:

    // Waypoint navigation / helper.
    bool _calcNextPhase         (void);
    void _recalcCurrentPhase    (void);
    bool _calcShortestPath      (const QGeoCoordinate &start,
                                 const QGeoCoordinate &destination,
                                 QVector<QGeoCoordinate> &path);
    // Slicing parameters
    bool _setStartIndex             (void);
    void _updateOverlap             (void);
    void _updateMaxWaypoints        (void);
    // Waypoint settings.
    void _updateflightSpeed         (void);
    void _updateArrivalReturnSpeed  (void);
    void _updateAltitude            (void);
    // Waypoint Statistics.
    void _setPhaseDistance  (double distance);
    void _setPhaseDuration  (double duration);
    // SMART RTL
    void    _checkBatteryLevel                  (void);
    bool    _checkSmartRTLPreCondition          (QString &errorString);
    void    _initSmartRTL                       ();
    void    _smartRTLCleanUp                    (bool flying);
    // Snake.
    void _setSnakeCalcInProgress             (bool inProgress);
    void _snakeStoreWorkerResults            ();
    void _startStopRosBridge                 ();
    void _initStartSnakeWorker               ();
    void _switchSnakeManager                 (QVariant variant);
    // Periodic tasks.
    void _eventTimerHandler                  (void);
    // Waypoint manager handling.
    void _switchWaypointManager(WaypointManager::ManagerBase &manager);

private:
    using StatusMap = std::map<int, QString>;

    // Controllers.
    PlanMasterController   *_masterController;
    MissionController      *_missionController;

    // Wima Data.
    QmlObjectListModel      _areas; // 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;

    // Waypoint Managers.
    WaypointManager::AreaInterface      _areaInterface;
    WaypointManager::Settings           _managerSettings;
    WaypointManager::DefaultManager     _defaultManager;
    WaypointManager::DefaultManager     _snakeManager;
    WaypointManager::RTLManager         _rtlManager;
    WaypointManager::ManagerBase       *_currentManager;
    using ManagerList = QList<WaypointManager::ManagerBase*>;
    ManagerList                         _managerList;

    // Settings Facts.
    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                 _enableSnake; // Enable Snake (see snake.h)
    SettingsFact                 _snakeTileWidth;
    SettingsFact                 _snakeTileHeight;
    SettingsFact                 _snakeMinTileArea;
    SettingsFact                 _snakeLineDistance;
    SettingsFact                 _snakeMinTransectLength;

    // Smart RTL.
    QTimer          _smartRTLTimer;
    bool            _lowBatteryHandlingTriggered;

    // Waypoint statistics.
    double                       _measurementPathLength; // the lenght of the phase in meters
//    double                       _phaseDistance; // the lenth in meters of the current phase
//    double                       _phaseDuration; // the phase duration in seconds

    // Snake
    bool                    _snakeCalcInProgress;
    bool                    _snakeRecalcNecessary;
    bool                    _scenarioDefinedBool;
    SnakeWorker             _snakeWorker;
    Scenario                _scenario;
    ::GeoPoint3D            _snakeOrigin;
    SnakeTiles              _snakeTiles; // tiles
    SnakeTilesLocal         _snakeTilesLocal; // tiles local coordinate system
    QVariantList            _snakeTileCenterPoints;
    QNemoProgress           _nemoProgress; // measurement progress
    QNemoHeartbeat          _nemoHeartbeat; // measurement progress
    int                     _fallbackStatus;
    ROSBridgePtr            _pRosBridge;
    bool                    _bridgeSetupDone;
    static StatusMap        _nemoStatusMap;

    // Periodic tasks.
    QTimer                  _eventTimer;

};