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

#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(RoutingWorkerLog, "RoutingWorkerLog")

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() const { return this->_calculating; }

void RoutingThread::route(const RoutingParameter &par,
                          const Generator &generator) {
  // Sample input.
  Lock lk(this->_mutex);
  this->_par = par;
  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() {
  qCWarning(RoutingWorkerLog) << "run(): thread start.";
  while (!this->_stop) {
    qCWarning(RoutingWorkerLog) << "run(): calculation "
                                   "started.";
    // Copy input.
    auto start = std::chrono::high_resolution_clock::now();

    this->_calculating = true;
    emit calculatingChanged();
    Lock lk(this->_mutex);
    auto par = this->_par;
    auto generator = this->_generator;
    lk.unlock();
    auto safeAreaENU = par.safeArea;
    auto numRuns = par.numRuns;
    auto numSolutionsPerRun = par.numSolutions;

    PtrRoutingData pRouteData(new RoutingData());
    auto &transectsENU = pRouteData->transects;
    // Generate transects.
    if (generator(transectsENU)) {
      // Check if generation was successful.
      if (transectsENU.size() == 0) {
        qCWarning(RoutingWorkerLog) << "run(): "
                                       "not able to generate transects.";
      } else {
        // Prepare data for routing.
        auto &solutionVector = pRouteData->solutionVector;

        snake::RouteParameter snakePar;
        snakePar.numSolutionsPerRun = numSolutionsPerRun;
        snakePar.numRuns = numRuns;

        // Set time limit to 10 min.
        const auto maxRoutingTime = std::chrono::minutes(10);
        const auto routingEnd =
            std::chrono::high_resolution_clock::now() + maxRoutingTime;
        const auto &restart = this->_restart;
        snakePar.stop = [&restart, routingEnd] {
          bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
          return restart || expired;
        };

        // Route transects.
        bool success =
            snake::route(safeAreaENU, transectsENU, solutionVector, snakePar);

        // Check if routing was successful.
        if ((!success || solutionVector.size() < 1) && !this->_restart) {
          qCWarning(RoutingWorkerLog) << "run(): "
                                         "routing failed. "
                                      << snakePar.errorString.c_str();
        } else if (this->_restart) {
          qCWarning(RoutingWorkerLog) << "run(): "
                                         "restart requested.";
        } else {
          // Notify main thread.
          emit result(pRouteData);
          qCWarning(RoutingWorkerLog) << "run(): "
                                         "concurrent update success.";
        }
      }
    } // end calculation
    else {
      qCWarning(RoutingWorkerLog) << "run(): generator() failed.";
    }
    qCWarning(RoutingWorkerLog)
        << "run(): execution time: "
        << std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::high_resolution_clock::now() - start)
               .count()
        << " ms";
    // Signal calulation end and set thread to sleep.
    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
  qCWarning(RoutingWorkerLog) << "run(): thread end.";
}