#include "RoutingThread.h"
// std
#include <chrono>
// Qt
#include <QDebug>

RoutingThread::RoutingThread(QObject *parent)
    : QThread(parent), _calculating(false), _stop(false), _restart(false) {

  static std::once_flag flag;
  std::call_once(flag,
                 [] { qRegisterMetaType<PtrRoutingData>("PtrRoutingData"); });
}

RoutingThread::~RoutingThread() {
  this->_stop = true;
  Lock lk(this->_mutex);
  this->_restart = true;
  this->_cv.notify_one();
  lk.unlock();
  this->wait();
}

bool RoutingThread::calculating() { return this->_calculating; }

void RoutingThread::route(const snake::BoostPolygon &safeArea,
                          const RoutingThread::Generator &generator) {
  // Sample input.
  Lock lk(this->_mutex);
  this->_safeArea = safeArea;
  this->_generator = generator;
  lk.unlock();

  if (!this->isRunning()) {
    this->start();
  } else {
    Lock lk(this->_mutex);
    this->_restart = true;
    this->_cv.notify_one();
  }
}

void RoutingThread::run() {
  qWarning() << "RoutingWorker::run(): thread start.";
  while (!this->_stop) {
#ifdef DEBUG_CIRCULAR_SURVEY
    qWarning() << "RoutingWorker::run(): calculation "
                  "started.";
#endif
    // Copy input.
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
    this->_calculating = true;
    emit calculatingChanged();
    Lock lk(this->_mutex);
    auto safeAreaENU = this->_safeArea;
    auto generator = this->_generator;
    lk.unlock();

    PtrRoutingData pRouteData(new RoutingData());
    auto &transectsENU = pRouteData->transects;
    // Generate transects.
    if (generator(transectsENU)) {
      // Check if generation was successful.
      if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY
        qWarning() << "RoutingWorker::run(): "
                      "not able to generate transects.";
#endif
      } else {
        // Prepare data for routing.
        auto &transectsInfo = pRouteData->transectsInfo;
        auto &route = pRouteData->route;
        const auto routingStart = std::chrono::high_resolution_clock::now();
        const auto maxRoutingTime = std::chrono::minutes(10);
        const auto routingEnd = routingStart + maxRoutingTime;
        const auto &restart = this->_restart;
        auto stopLambda = [&restart, routingEnd] {
          bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
          return restart || expired;
        };
        std::string errorString;
        // Route transects;
        bool success = snake::route(safeAreaENU, transectsENU, transectsInfo,
                                    route, stopLambda, errorString);
        // Check if routing was successful.
        if ((!success || route.size() < 3) && !this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "RoutingWorker::run(): "
                        "routing failed.";
#endif
        } else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "RoutingWorker::run(): "
                        "restart requested.";
#endif
        } else {
          // Notify main thread.
          emit result(pRouteData);
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "RoutingWorker::run(): "
                        "concurrent update success.";
#endif
        }
      }
    } // end calculation
#ifdef DEBUG_CIRCULAR_SURVEY
    else {
      qWarning() << "RoutingWorker::run(): preconditions failed.";
    }
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    qWarning() << "RoutingWorker::run(): execution time: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::high_resolution_clock::now() - start)
                      .count()
               << " ms";
#endif
    this->_calculating = false;
    emit calculatingChanged();
    Lock lk2(this->_mutex);
    if (!this->_restart) {
      this->_cv.wait(lk2, [this] { return this->_restart.load(); });
    }
    this->_restart = false;
  } // main loop
  qWarning() << "RoutingWorker::run(): thread end.";
}