/**
  * Calculate the focal point for units based on it's superiors
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2017-09-05
  */


#include "unit_position_calculator.h"
#include "participant_handler.h"
#include <QTimer>
#include <LtsCoordTrans/lts_coord_trans.h>
#include <LtsUtilsCore/qpoint3d.h>
#include <LtsUtilsCore/outdated_position.h>
#include <QPointF>
#include <QDateTime>
#include <QtConcurrent>

namespace ParticipantManagement
{
namespace ParticipantHandling
{
UnitPositionCalculator::UnitPositionCalculator(ParticipantHandler *participantHandler, QObject *parent)
  : QObject(parent)
  , m_participantHandler(participantHandler)
  , m_bSomethingChangedSinceLastCalculation(false)
  , m_timer(new QTimer(this))
  , m_bRunning(true)
  , m_logger(getLogger("UnitPositionCalculator"))
{
  connect(m_timer, &QTimer::timeout, this, &UnitPositionCalculator::calculate);
  m_timer->setSingleShot(true);

  connect(&m_calculationWatcher, &QFutureWatcher<Result>::finished,
          this, &UnitPositionCalculator::onCalculationDone);
}

UnitPositionCalculator::~UnitPositionCalculator()
{
  stopCalculation();
  m_calculationWatcher.disconnect(this);
  m_calculationWatcher.cancel();
  m_calculationWatcher.waitForFinished();
}

void UnitPositionCalculator::startCalculation()
{
  DEBUG_LOG(m_logger) << "Starting calculation timer";
  m_timer->start();
  m_bRunning = true;
}

void UnitPositionCalculator::stopCalculation()
{
  m_timer->stop();
  m_participantData.clear();
  m_publishedPositionData.clear();
  m_bRunning = false;
}

void UnitPositionCalculator::waitForCalculationFinished()
{
  if (m_calculationWatcher.isRunning())
    m_calculationWatcher.waitForFinished();
}


void UnitPositionCalculator::onParticipantDeleted(int participantId)
{
  DEBUG_LOG(m_logger) << "Participant " << participantId << " deleted";
  auto it = m_participantData.find(participantId);
  if (it == m_participantData.end())
    return;
  m_participantData.erase(it);
}

void UnitPositionCalculator::onPositionChanged(int participantId, int64_t positionTime, const Core::GeoPosition &pos)
{
  DEBUG_LOG(m_logger) << "Position for participant " << participantId << " changed to "
                      << pos << " (" << positionTime / 1000 << "s)";
  CalculatedUnitPosition &data = getParticipant(participantId);
  if (data.sampleTimeLeader() >= positionTime)
    return;

  data.sampleTimeCenter(positionTime);
  data.sampleTimeLeader(positionTime);
  data.positionCenter(pos);
  data.positionLeader(pos);
  m_bSomethingChangedSinceLastCalculation = true;
}

void UnitPositionCalculator::onSuperiorChanged(int participantId, int superiorId)
{
  DEBUG_LOG(m_logger) << "Superior for participant " << participantId << " changed to " << superiorId;
  CalculatedUnitPosition &data = getParticipant(participantId);
  data.superiorId = superiorId;
  m_bSomethingChangedSinceLastCalculation = true;
}

void UnitPositionCalculator::onInferiorChanged(int participantId, const std::vector<int> &inferiorIds)
{
  if (m_logger.isDebugEnabled())
  {
    QStringList slIds;
    slIds.reserve(inferiorIds.size());
    for (int id : inferiorIds)
      slIds += QString::number(id);
    DEBUG_LOG(m_logger) << "Inferior for participant " << participantId << " changed, size: " << slIds.join(QLatin1Char(' '));
  }

  CalculatedUnitPosition &data = getParticipant(participantId);
  data.inferiorIds = inferiorIds;
  m_bSomethingChangedSinceLastCalculation = true;
}

UnitPositionCalculator::CalculatedUnitPosition &UnitPositionCalculator::getParticipant(int participantId)
{
  auto it = m_participantData.find(participantId);
  if (it == m_participantData.end())
  {
    DEBUG_LOG(m_logger) << "No information for participant " << participantId << " - creating dummy data";

    CalculatedUnitPosition data;
    data.participantId(participantId);
    static const Core::GeoPosition s_invalidPos(std::nan(""), std::nan(""), std::nan(""));
    data.positionCenter(s_invalidPos);
    data.positionLeader(s_invalidPos);
    it = m_participantData.insert({participantId, std::move(data)}).first;
  }
  return it->second;
}

void UnitPositionCalculator::calculate()
{
  static bool s_bDebugOnce = false;
  // theoretisches Problem: wenn sich keiner bewegt werden die Unit-Symbole nicht aktualisiert wenn
  // dann Teilnehmer out-of-date gehen
  if (!m_bSomethingChangedSinceLastCalculation)
  {
    if (!s_bDebugOnce)
      DEBUG_LOG(m_logger) << "Nothing changed - no calculation needed -> waiting for new data";
    s_bDebugOnce = true;
    m_timer->start(1000);
    return;
  }

  s_bDebugOnce = false;
  m_bSomethingChangedSinceLastCalculation = false;
  // copy data (before thread starts), can be done better with c++14 (lambda captures)
  std::unordered_map<int, CalculatedUnitPosition> *participantData(new std::unordered_map<int, CalculatedUnitPosition>(m_participantData));
  const qint64 currentTime = m_participantHandler->currentTimeMSec();
  auto cb = [this, participantData, currentTime]() -> Result
  {
    const auto ret = doCalculation(*participantData, currentTime, m_logger);
    delete participantData;
    return ret;
  };
  m_calculationWatcher.setFuture(QtConcurrent::run(cb));
}

UnitPositionCalculator::Result UnitPositionCalculator::doCalculation(std::unordered_map<int, CalculatedUnitPosition> &participantData,
                                                                     qint64 currentTime,
                                                                     NGL::INGLLogger &logger)
{
  DEBUG_LOG(logger) << "Starting calculation loop";

  Result result;
  result.reserve(participantData.size());

  // search all top-level units
  std::vector<int> topLevelIds;
  topLevelIds.reserve(10);
  for (const auto &data : participantData)
  {
    const int superiorId = data.second.superiorId;
    const int participantId = data.second.participantId();
    DEBUG_LOG(logger) << "Participant " << participantId << " has superior " << superiorId;
    if (superiorId == 0 || participantData.find(superiorId) == participantData.end())
    {
      DEBUG_LOG(logger) << "Participant " << participantId << " is a toplevel participant";
      topLevelIds.push_back(data.second.participantId());
    }
  }
  // TODO: check for correct (non-cyclic) hierarchy?

  for (const int topLevelId : topLevelIds)
  {
    DEBUG_LOG(logger) << "Calculating positions for toplevel participant " << topLevelId;
    calculatePosition(topLevelId, participantData, currentTime, 1, logger, result);
  }

  return result;
}

void UnitPositionCalculator::calculatePosition(int participantId,
                                               std::unordered_map<int, CalculatedUnitPosition> &participantData,
                                               qint64 currentTime,
                                               int level,
                                               NGL::INGLLogger &logger,
                                               Result &result)
{
  static const Core::GeoPosition s_invalidPos(std::nan(""), std::nan(""), std::nan(""));

  const QByteArray dbgLvlSpaces(level, ' ');
  auto it = participantData.find(participantId);
  if (it == participantData.end())
  {
    WARNING_LOG(logger) << "Participant " << participantId << " not found for unit position calculation!";
    return;
  }
  CalculatedUnitPosition &data = it->second;
  // found leaf -> return
  if (data.inferiorIds.size() == 0)
  {
    DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId
                      << " is a leaf: " << data.positionCenter()
                      << " (" << data.sampleTimeCenter() / 1000 << "s)";
    return;
  }

  data.positionCenter(s_invalidPos);
  data.positionLeader(s_invalidPos);

  // calculate position for inferiors
  DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId << ": Start calculating child positions";
  for (const int inferiorId : data.inferiorIds)
  {
    DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId << ": calculating child position for " << inferiorId;
    calculatePosition(inferiorId, participantData, currentTime, level + 1, logger, result);
  }
  DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId << ": Finished calculating child positions";

  // now calc own position
  int utmZone = -1;
  int childCount = 0;
  LtsUtils::QPoint3D pos;
  qint64 childPositionTime = 0;
  bool bFirst = true;
  for (const int inferiorId : data.inferiorIds)
  {
    auto itChild = participantData.find(inferiorId);
    // Warning was already given in loop before
    if (itChild == participantData.end())
    {
      WARNING_LOG(logger) << dbgLvlSpaces << "Child " << inferiorId << ": could not find data";
      continue;
    }

    const CalculatedUnitPosition &child = itChild->second;
    // older than 10 minutes -> ignore
    if (LtsUtils::isPositionTimeOutdated(currentTime, child.sampleTimeCenter()))
    {
      DEBUG_LOG(logger) << dbgLvlSpaces << "Child " << inferiorId << ": position out-of-date - ignoring";
      continue;
    }

    // invalid position -> ignore
    if (std::isnan(child.positionCenter().lon()) ||
        std::isnan(child.positionCenter().lat()))
    {
      DEBUG_LOG(logger) << dbgLvlSpaces << "Child " << inferiorId << ": has invalid position - ignoring";
      continue;
    }

    if (bFirst)
    {
      bFirst = false;
      data.positionLeader(child.positionLeader());
      data.sampleTimeLeader(child.sampleTimeLeader());
    }

    const auto utm = LtsCoordTrans::UtmCoordinate::fromGeographic(child.positionCenter().lon(),
                                                                  child.positionCenter().lat(),
                                                                  utmZone);
    if (utmZone == -1)
      utmZone = utm.zone;

    pos += LtsUtils::QPoint3D(utm.x, utm.y, child.positionCenter().z());
    if (child.sampleTimeCenter() > childPositionTime)
      childPositionTime = child.sampleTimeCenter();

    ++childCount;
  }

  data.sampleTimeCenter(childPositionTime);
  if (childCount > 0)
  {
    pos /= childCount;
    const LtsCoordTrans::UtmCoordinate utm(utmZone, pos.x(), pos.y(),true);
    const QPointF lonLat = utm.geographic();
    data.positionCenter(Core::GeoPosition(lonLat.x(), lonLat.y(), pos.z()));
  }
  DEBUG_LOG(logger) << dbgLvlSpaces
                    << "Participant " << participantId
                    << ": Mean position: " << data.positionCenter()
                    << " #relavant children: " << childCount
                    << " (" << data.sampleTimeCenter() / 1000 << "s)";

  result.push_back(std::move(data));
}

void UnitPositionCalculator::onCalculationDone()
{
  const Result &calculatedPositions = m_calculationWatcher.future().result();

  auto dts = m_participantHandler->dts();
  for (const auto &pos : calculatedPositions)
  {
    auto it = m_publishedPositionData.find(pos.participantId());
    if (it != m_publishedPositionData.end())
    {
      ParticipantManagement::UnitPosition &up = it->second;
      DEBUG_LOG(m_logger) << "old: " << up << ", new: " << pos << ", equal: " << (up == pos);
      if (up == pos)
        continue;
      // nan == nan is always false, have to check it on our own
      if (std::isnan(up.positionCenter().lat()) && std::isnan(pos.positionCenter().lat()))
        continue;
      up = pos;
    }
    else
    {
      m_publishedPositionData.insert(std::make_pair(pos.participantId(), pos));
    }
    dts->publish(pos);
  }

  m_timer->start(1000);
}


} // namespace ParticipantHandling
} // namespace ParticipantManagement