WimaController.h

#pragma once

#include <QObject>
#include <QScopedPointer>

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

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

#include "WimaPlanData.h"

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

#include "Geometry/GeoPoint3D.h"
#include "Snake/SnakeDataManager.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"

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

#include "utilities.h"

#include <map>

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

class WimaController : public QObject {
  Q_OBJECT
  enum FileType { WimaFile, PlanFile };

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);
  QVariantList snakeTileCenterPoints(void);
  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 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 _DMFinishedHandler();
  void _switchToSnakeWaypointManager(QVariant variant);
  void _switchDataManager(SnakeDataManager &dataManager);
  void _progressChangedHandler();
  void _enableSnakeChangedHandler();
  // 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 _WMSettings; // Waypoint Manager Settings
  WaypointManager::DefaultManager _defaultWM;
  WaypointManager::DefaultManager _snakeWM;
  WaypointManager::RTLManager _rtlWM;
  WaypointManager::ManagerBase *_currentWM;
  using ManagerList = QList<WaypointManager::ManagerBase *>;
  ManagerList _WMList;

  // 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

  // Snake
  SnakeDataManager _snakeDM; // Snake Data Manager
  SnakeDataManager _emptyDM;
  SnakeDataManager *_currentDM;
  static StatusMap _nemoStatusMap;

  // Periodic tasks.
  QTimer _eventTimer;
  EventTicker _batteryLevelTicker;
};