DefaultManager.cpp

#include "DefaultManager.h"
#include "Wima/Geometry/GeoUtilities.h"
#include "Wima/Geometry/PolygonCalculus.h"

#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"

WaypointManager::DefaultManager::DefaultManager(Settings &settings,
                                                AreaInterface &interface)
    : ManagerBase(settings), _areaInterface(&interface) {}

void WaypointManager::DefaultManager::clear() {
  _dirty = true;
  _waypoints.clear();
  _currentWaypoints.clear();
  _missionItems.clearAndDeleteContents();
  _currentMissionItems.clearAndDeleteContents();
  _waypointsVariant.clear();
  _currentWaypointsVariant.clear();
}

bool WaypointManager::DefaultManager::update() {
  // extract waypoints
  _currentWaypoints.clear();
  Slicer::update(_waypoints, _currentWaypoints);
  return _worker();
}

bool WaypointManager::DefaultManager::next() {
  // extract waypoints
  _currentWaypoints.clear();
  Slicer::next(_waypoints, _currentWaypoints);
  return _worker();
}

bool WaypointManager::DefaultManager::previous() {
  // extract waypoints
  _currentWaypoints.clear();
  Slicer::previous(_waypoints, _currentWaypoints);
  return _worker();
}

bool WaypointManager::DefaultManager::reset() {
  // extract waypoints
  _currentWaypoints.clear();
  Slicer::reset(_waypoints, _currentWaypoints);
  return _worker();
}

bool WaypointManager::DefaultManager::_insertMissionItem(
    const QGeoCoordinate &c, size_t index, QmlObjectListModel &list,
    bool doUpdate) {
  using namespace WaypointManager::Utils;

  if (!insertMissionItem(c, index /*insertion index*/, list,
                         _settings->vehicle(), _settings->isFlyView(),
                         &list /*parent*/, doUpdate /*do update*/)) {
    qWarning(
        "WaypointManager::DefaultManager::next(): insertMissionItem failed.");
    Q_ASSERT(false);
    return false;
  }
  return true;
}

bool WaypointManager::DefaultManager::_insertMissionItem(
    const QGeoCoordinate &c, size_t index, bool doUpdate) {
  return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}

bool WaypointManager::DefaultManager::_calcShortestPath(
    const QGeoCoordinate &start, const QGeoCoordinate &destination,
    QVector<QGeoCoordinate> &path) {
  using namespace GeoUtilities;
  using namespace PolygonCalculus;
  QPolygonF joinedArea2D;
  toCartesianList(_areaInterface->joinedArea()->coordinateList(),
                  /*origin*/ start, joinedArea2D);
  QPointF start2D(0, 0);
  QPointF end2D;
  toCartesian(destination, start, end2D);
  QVector<QPointF> path2DOut;

  bool retVal =
      PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
  toGeoList(path2DOut, /*origin*/ start, path);

  return retVal;
}

bool WaypointManager::DefaultManager::_worker() {
  // Precondition:
  // _waypoints must contain valid coordinates.
  // Slicer must be called befor invoking this function.
  // E.g. Slicer::reset(_waypoints, _currentWaypoints);

  using namespace WaypointManager::Utils;

  if (_waypoints.count() < 1 || !_settings->valid()) {
    return false;
  }

  if (_dirty) {
    _missionItems.clearAndDeleteContents();
    _waypointsVariant.clear();
    // No initialization of _missionItems, don't need MissionSettingsItem here.
    for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
      auto &c = _waypoints.at(i);
      _insertMissionItem(c, _missionItems.count(), _missionItems,
                         false /*update list*/);
      _waypointsVariant.push_back(QVariant::fromValue(c));
    }
    updateHirarchy(_missionItems);
    updateSequenceNumbers(
        _missionItems, 1); // Start with 1, since MissionSettingsItem missing.
    _dirty = false;
  }

  _currentMissionItems.clearAndDeleteContents();
  initialize(_currentMissionItems, _settings->vehicle(),
             _settings->isFlyView());

  // Calculate path from home to first waypoint.
  QVector<QGeoCoordinate> arrivalPath;
  if (!_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(),
                         arrivalPath)) {
    qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
             "from home position to first waypoint.");
    qWarning() << "from: " << _settings->homePosition();
    qWarning() << "to: " << _currentWaypoints.first();
    return false;
  }
  if (!arrivalPath.empty())
    arrivalPath.removeFirst();
  if (!arrivalPath.empty())
    arrivalPath.removeLast();

  // calculate path from last waypoint to home
  QVector<QGeoCoordinate> returnPath;
  if (!_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(),
                         returnPath)) {
    qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
             "from home position to last waypoint.");
    qWarning() << "from: " << _currentWaypoints.last();
    qWarning() << "to: " << _settings->homePosition();
    return false;
  }
  if (!returnPath.empty())
    returnPath.removeFirst();
  if (!returnPath.empty())
    returnPath.removeLast();

  // Create mission items.
  // Set home position of MissionSettingsItem.
  MissionSettingsItem *settingsItem =
      _currentMissionItems.value<MissionSettingsItem *>(0);
  if (settingsItem == nullptr) {
    Q_ASSERT(false);
    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
    return false;
  }
  settingsItem->setCoordinate(_settings->homePosition());

  // First mission item is takeoff command.
  _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/,
                     false /*do update*/);
  SimpleMissionItem *takeoffItem =
      _currentMissionItems.value<SimpleMissionItem *>(1);
  if (takeoffItem == nullptr) {
    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
    Q_ASSERT(takeoffItem != nullptr);
    return false;
  }
  makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle());

  // Create change speed item.
  _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
                     false /*do update*/);
  SimpleMissionItem *speedItem =
      _currentMissionItems.value<SimpleMissionItem *>(2);
  if (speedItem == nullptr) {
    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
    Q_ASSERT(speedItem != nullptr);
    return false;
  }
  makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());

  // insert arrival path
  for (auto coordinate : arrivalPath) {
    _insertMissionItem(coordinate,
                       _currentMissionItems.count() /*insertion index*/,
                       false /*do update*/);
  }

  // Create change speed item (after arrival path).
  int index = _currentMissionItems.count();
  _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/,
                     false /*do update*/);
  speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
  if (speedItem == nullptr) {
    Q_ASSERT(false);
    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
    return false;
  }
  makeSpeedCmd(speedItem, _settings->flightSpeed());

  // Insert slice.
  for (auto coordinate : _currentWaypoints) {
    _insertMissionItem(coordinate,
                       _currentMissionItems.count() /*insertion index*/,
                       false /*do update*/);
  }

  // Create change speed item, after slice.
  index = _currentMissionItems.count();
  _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/,
                     false /*do update*/);
  speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
  if (speedItem == nullptr) {
    Q_ASSERT(false);
    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
    return false;
  }
  makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());

  // Insert return path coordinates.
  for (auto coordinate : returnPath) {
    _insertMissionItem(coordinate,
                       _currentMissionItems.count() /*insertion index*/,
                       false /*do update*/);
  }

  // Set land command for last mission item.
  index = _currentMissionItems.count();
  _insertMissionItem(_settings->homePosition(), index /*insertion index*/,
                     false /*do update*/);
  SimpleMissionItem *landItem =
      _currentMissionItems.value<SimpleMissionItem *>(index);
  if (landItem == nullptr) {
    Q_ASSERT(false);
    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
    return false;
  }
  makeLandCmd(landItem, _settings->masterController()->managerVehicle());

  // Set altitude.
  for (int i = 1; i < _currentMissionItems.count(); ++i) {
    SimpleMissionItem *item =
        _currentMissionItems.value<SimpleMissionItem *>(i);
    if (item == nullptr) {
      Q_ASSERT(false);
      qWarning("WimaController::updateAltitude(): nullptr");
      return false;
    }
    item->altitude()->setRawValue(_settings->altitude());
  }

  // Update list _currentMissionItems.
  updateHirarchy(_currentMissionItems);
  updateSequenceNumbers(_currentMissionItems);

  // Prepend arrival path to slice.
  for (long i = arrivalPath.size() - 1; i >= 0; --i)
    _currentWaypoints.push_front(arrivalPath[i]);
  _currentWaypoints.push_front(_settings->homePosition());
  // Append return path to slice.
  for (auto c : returnPath)
    _currentWaypoints.push_back(c);
  _currentWaypoints.push_back(_settings->homePosition());

  // Create variant list.
  _currentWaypointsVariant.clear();
  for (auto c : _currentWaypoints)
    _currentWaypointsVariant.push_back(QVariant::fromValue(c));

  return true;
}