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

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

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

void WaypointManager::RTLManager::setWaypoints(
    const QVector<QGeoCoordinate> &waypoints) {
  (void)waypoints;
  return;
}

void WaypointManager::RTLManager::push_back(const QGeoCoordinate &wp) {
  (void)wp;
  return;
}

void WaypointManager::RTLManager::push_front(const QGeoCoordinate &wp) {
  (void)wp;
  return;
}

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

void WaypointManager::RTLManager::insert(std::size_t i,
                                         const QGeoCoordinate &wp) {
  (void)i;
  (void)wp;
  return;
}

std::size_t WaypointManager::RTLManager::size() const { return 0; }

void WaypointManager::RTLManager::remove(std::size_t i) { (void)i; }

bool WaypointManager::RTLManager::update() {
  this->clear();
  return _worker();
}

bool WaypointManager::RTLManager::next() { return true; }

bool WaypointManager::RTLManager::previous() { return true; }

bool WaypointManager::RTLManager::reset() { return true; }

bool WaypointManager::RTLManager::checkPrecondition(QString &errorString) {
  // Be aware of the error message order!
  Vehicle *managerVehicle = _settings->masterController()->managerVehicle();

  if (managerVehicle->isOfflineEditingVehicle()) {
    errorString.append("No vehicle connected. Smart RTL not available.");
    return false;
  }

  if (!managerVehicle->flying()) {
    errorString.append("Vehicle is not flying. Smart RTL not available.");
    return false;
  }

  if (!_areaInterface->joinedArea()->containsCoordinate(
          managerVehicle->coordinate())) {
    errorString.append(
        "Vehicle not inside save area. Smart RTL not available.");
    return false;
  }

  return true;
}

bool WaypointManager::RTLManager::_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::RTLManager::next(): insertMissionItem failed.");
    Q_ASSERT(false);
    return false;
  }
  return true;
}

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

bool WaypointManager::RTLManager::_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::RTLManager::_worker() {
  // Precondition: settings must be valid.

  using namespace WaypointManager::Utils;

  if (!_settings->valid()) {
    return false;
  }

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

  // Calculate path from vehicle to home position.
  QVector<QGeoCoordinate> returnPath;
  auto vehicleCoordinate =
      _settings->masterController()->managerVehicle()->coordinate();
  if (!_calcShortestPath(vehicleCoordinate, _settings->homePosition(),
                         returnPath)) {
    qWarning("WaypointManager::RTLManager::next(): Not able to calc path from "
             "vehicle to home position.");
    return false;
  }

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

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

  // Insert return path coordinates.
  for (auto coordinate : returnPath) {
    index = _currentMissionItems.count();
    _insertMissionItem(coordinate, index /*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::RTLManager::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);

  // Append return path to _currentWaypoints.
  for (auto c : returnPath)
    _currentWaypoints.push_back(c);

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

  return true;
}