/**
  * Handler for all oc situation data at the server per client
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Philipp Dassow, NLST, 27-02-2018
  */

#include "handler_oc_situation_data.h"

#include <LtsCoordTrans/lts_coord_trans.h>
#include <LtsUtilsCore/qbytearray_byteaccess.h>
#include <LtsUtilsCore/dds_vector.h>
#include <LtsUtilsCore/outdated_position.h>
#include <ParticipantManagement/ppm_common.h>
#include "settings.h"
#include <algorithm>
namespace OcSituationServer
{

//
// ThreadOcSituationData
//
ThreadOcSituationData::ThreadOcSituationData(MainHandler *mainHandler,
                                             int equipmentId,
                                             int centerEquipmentId,
                                             int radius,
                                             bool filterActive,
                                             QSet<int> partys,
                                             QSet<ParticipantManagement::TypeClass::type> typeClasses)
  : m_baseCoordinate(mainHandler->positionConverter()->baseCoordinate())
  , m_equipmentId(equipmentId)
  , m_centerEquipmentId(centerEquipmentId)
  , m_radius(radius)
  , m_partyList(partys)
  , m_filterActive(filterActive)
  , m_typeClassList(typeClasses)
  , m_mainHandler(mainHandler)
  , m_logger(getLogger(QStringLiteral("ThreadOcSituationData.") + QString::number(equipmentId)))
{}

void ThreadOcSituationData::requestOcSituationData(OcSituation::SituationCenterType centerType,
                                                   int centerEquipmentId,
                                                   const Core::GeoPosition2D &position,
                                                   int radius,
                                                   bool filterActive,
                                                   QSet<int> partyList,
                                                   QSet<ParticipantManagement::TypeClass::type> typeClassList,
                                                   bool initial)
{
  INFO_LOG(m_logger) << "OC-Situation Request for centerEquipmentId: " << centerEquipmentId
                     << " from O/C-laptop-Id: " << m_equipmentId
                     << " with radius of " << radius
                     << "m, initial: " << initial;

  QMutexLocker l(&m_accessMutex);

  m_centerType = centerType;
  m_centerEquipmentId = centerEquipmentId;
  m_radius = radius;
  m_position = position;
  m_filterActive = filterActive;
  m_partyList = partyList;
  m_typeClassList = typeClassList;

  if (m_handler)
    m_handler->setRequestOcSituationData(centerType, centerEquipmentId, position, radius, filterActive, partyList, typeClassList, initial);
  else
    DEBUG_LOG(m_logger) << "Thread doesn't exist up to now! Do nothing.";
}

void ThreadOcSituationData::run()
{
  static QMutex s_ddsAccesMutex;
  {
    QMutexLocker l(&m_accessMutex);
    QMutexLocker m(&s_ddsAccesMutex);
    DEBUG_LOG(m_logger) << "Instance new OC-Thread for O/C-Laptop-Id: " << m_equipmentId;

    m_handler = new HandlerOcSituationData(m_mainHandler->dds(),
                                           m_baseCoordinate,
                                           m_equipmentId);
    connect(m_handler, &HandlerOcSituationData::sendOcData, this, &ThreadOcSituationData::sendOcData, Qt::QueuedConnection);

    m_handler->setRequestOcSituationData(m_centerType, m_centerEquipmentId, m_position, m_radius, m_filterActive, m_partyList, m_typeClassList, true);
  }

  INFO_LOG(m_logger) << "OC-Thread started for O/C-Laptop-Id: " << m_equipmentId;
  exec();

  {
    QMutexLocker l(&m_accessMutex);
    QMutexLocker m(&s_ddsAccesMutex);
    delete m_handler;
    m_handler = nullptr;
  }
}

//
// HandlerOcSituationData
//
HandlerOcSituationData::HandlerOcSituationData(const std::shared_ptr<DdsHandler> &dds,
                                               const LtsCoordTrans::UtmCoordinate &baseCoordinate,
                                               int equipmentId)
  : m_baseCoordinate(baseCoordinate)
  , m_ppmDtc(ParticipantManagement::Interface::Dtc::create(dds->dds()))
  , m_aesDtc(AreaEffectSimulation::Interface::Dtc::create(dds->dds()))
  , m_gimDtc(GuezIedManagement::Interface::Dtc::create(dds->dds()))
  , m_equipmentId(equipmentId)
  , m_partitionTime(QDateTime::currentMSecsSinceEpoch())
  , m_aliveTimer(new QTimer(this))
  , m_txTimer(new QTimer(this))
  , m_logger(getLogger(QStringLiteral("HandlerOcSituationData.") + QString::number(equipmentId)))
{
  INFO_LOG(m_logger) << "Create O/C-Handler for O/C-Laptop-Id: " << equipmentId
                     << " aliveTimeout: " << Settings::aliveTimeout() << " sec"
                     << " txCyleTime: " << Settings::txCycleTime() << " sec"
                     << " maxTransferBytes: " << Settings::maxNumOfBytePerTxCycle();

  connect(dds.get(), &DdsHandler::partitionTimeChanged, this, &HandlerOcSituationData::onPartitionTimeChanged);

  connect(m_aliveTimer, &QTimer::timeout, this, &HandlerOcSituationData::aliveTimeout);
  m_aliveTimer->setSingleShot(true);
  m_aliveTimer->start(Settings::aliveTimeout());

  connect(m_txTimer, &QTimer::timeout, this, &HandlerOcSituationData::handleSituation);
  m_txTimer->start(Settings::txCycleTime());

  connect(this, &HandlerOcSituationData::resetInternalData,
          this, &HandlerOcSituationData::onResetInternalData, Qt::QueuedConnection);

  connect(m_ppmDtc.get(), &ParticipantManagement::Interface::Dtc::participantChanged,
          this, &HandlerOcSituationData::onParticipantChanged);
  connect(m_ppmDtc.get(), &ParticipantManagement::Interface::Dtc::dynamicParticipantDataChanged,
          this, &HandlerOcSituationData::onDynamicParticipantDataChanged);
  connect(m_ppmDtc.get(), &ParticipantManagement::Interface::Dtc::participantPositionChanged,
          this, &HandlerOcSituationData::onParticipantPositionChanged);
  connect(m_aesDtc.get(), &AreaEffectSimulation::Interface::Dtc::aesItemViewChanged,
          this, &HandlerOcSituationData::onAesItemViewChanged);

  // REGG-5592 - Von DUS-Minen ausgefallene Teilnehmer werden in der Lagedarstellung nicht mit einer
  // Trefferanzeige in der Farbe der Sperre sondern in der Farbe des Ltg/Ausb-Dienstes (Ocker) dargestellt.
  // to handle race condition between DynamicParticipantData and DusMineState:
  connect(m_aesDtc.get(), &AreaEffectSimulation::Interface::Dtc::dusMineStateChanged,
          this, &HandlerOcSituationData::onDusMineStateChanged);

  m_ppmDtc->enableParticipant();
  m_ppmDtc->enableDynamicParticipantData();
  m_ppmDtc->enableParticipantPosition();
  m_ppmDtc->enableType();
  m_aesDtc->enableAesItemView();
}

void HandlerOcSituationData::setRequestOcSituationData(OcSituation::SituationCenterType centerType,
                                                       int centerEquipmentId,
                                                       const Core::GeoPosition2D &centerPosition,
                                                       int radius,
                                                       bool filterActive,
                                                       QSet<int> partys,
                                                       QSet<ParticipantManagement::TypeClass::type> typeClasses,
                                                       bool initial)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  QMutexLocker l(&m_accessMutex);
  DEBUG_LOG(m_logger) << "Requested CenterEquipmentId: " << centerEquipmentId << " radius: " << radius;

  bool endThread = false;

  m_centerType = centerType;
  m_centerEquipmentId = centerEquipmentId;
  m_centerPosition = centerPosition;
  m_centerParticipantId = 0;
  m_radius = radius;
  m_partyList.clear();
  m_typeClassList.clear();
  m_partyList  = partys;
  m_typeClassList = typeClasses;
  m_filterActive = filterActive;

  if (centerType == OcSituation::SituationCenterType::Equipment)
  {
    if (m_centerEquipmentId > 0)
    {
      ParticipantManagement::Participant centerParticipant;
      for (const auto &participant : qAsConst(m_allParticipants))
      {
        if (participant.equipmentNumber().is_set() && participant.equipmentNumber().get() == m_centerEquipmentId)
        {
          centerParticipant = participant;
          break;
        }
      };

      if (centerParticipant.id() <= 0)
      {
        ERROR_LOG(m_logger) << "Participant with EqId " << m_centerEquipmentId
                            << " as center of the oc situation request does not exist!";
        endThread = true;
      }
      else
      {
        const ParticipantManagement::ParticipantPosition &partPosition = m_allParticipantPositions.value(centerParticipant.id());
        if (partPosition.participantId() == 0)
        {
          ERROR_LOG(m_logger) << "Participant with EqId " << m_centerEquipmentId
                              << " as center of the oc situation request has no position!";
          endThread = true;
        }
        else
        {
          m_centerPosition = partPosition.pos();
          m_centerParticipantId = centerParticipant.id();
        }
      }
      if (endThread && ++m_retryCount < 5)
      {
        INFO_LOG(m_logger) << "Retry setting data - maybe participant is not yet available... " << m_retryCount;
        QTimer::singleShot(1000 * m_retryCount, this, [this]()
        {
          // must be initial here
          setRequestOcSituationData(m_centerType, m_centerEquipmentId, m_centerPosition, m_radius, m_filterActive, m_partyList, m_typeClassList, true);
        });
        return;
      }
    }
    else
    {
      INFO_LOG(m_logger) << "Situation is not requested anymore -> end thread";
      endThread = true;
    }
  }

  if (!endThread)
  {
    if (centerType == OcSituation::SituationCenterType::Equipment)
    {
      if (m_centerParticipantId == 0)
        endThread = true;
    }
    else if ((centerPosition.lon() == std::nan("")) || (centerPosition.lat() == std::nan("")))
      endThread = true;
  }

  m_retryCount = 0;

  if (endThread == true)
    m_radius = 0; // ==> no data transfer on handleSituation() and quit

  QTimer::singleShot(0, this, &HandlerOcSituationData::resetAliveTimer);
  if (initial || endThread)
  {
    // thread change ...
    QTimer::singleShot(0, this, &HandlerOcSituationData::handleSituation);
    emit resetInternalData();
  }
}

void HandlerOcSituationData::onParticipantChanged(const ParticipantManagement::Participant &data, DataModification modType)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  if (modType == DataModification::eRemoved)
    return;

  const int participantId = data.id();
  m_allParticipants[participantId] = data;

  if (!m_participants.contains(participantId))
  {
    DEBUG_LOG(m_logger) << "m_participants does not contain participant id: " << participantId;
    return; // participant is not of interest (now)
  }

  // old data to check against
  const ParticipantManagement::Participant &oldParticipantData = m_participants[participantId];

  // check structure
  if (oldParticipantData.equipmentNumber() != data.equipmentNumber() ||
      oldParticipantData.superiorId()      != data.superiorId() ||
      oldParticipantData.symbol()          != data.symbol() ||
      oldParticipantData.typeId()          != data.typeId() ||
      oldParticipantData.partyId()         != data.partyId())
    setModified(participantId, OcSituation::ModificationFlag::eStructure);

  // check designator
  if (oldParticipantData.designator()         != data.designator() ||
      oldParticipantData.tacticalIdentifier() != data.tacticalIdentifier())
    setModified(participantId, OcSituation::ModificationFlag::eDesignator);

  m_participants[participantId] = data;
  DEBUG_LOG(m_logger) << "updated data in m_participants" << data;

  if (m_modifiedFlags.contains(participantId))
  {
    INFO_LOG(m_logger) << "setModified for: " << m_participantId2EqId[participantId]
                       << " / " << participantId
                       << " bits: 0x" << QString::number(m_modifiedFlags[participantId], 16);
  }
}

static inline int getEquipmentStateBits(const ParticipantManagement::DynamicParticipantData &data)
{
  int ret = 0;

  ret |= int(data.hasNbcProtectionConfigured()) << 1;
  ret |= int(data.hasNbcProtectionActive()) << 2;
  ret |= int(data.manipulation().size() > 0) << 7;

  return ret;
}

void HandlerOcSituationData::onDynamicParticipantDataChanged(const ParticipantManagement::DynamicParticipantData &data, DataModification modType)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;
  if (modType == DataModification::eRemoved)
    return;

  const int participantId = data.participantId();
  m_allDynamicParticpantData[participantId] = data;

  if (!m_participants.contains(participantId))
  {
    DEBUG_LOG(m_logger) << "m_participants does not contain participant id: " << participantId;
    return; // participant does not interest us yet;
  }

  // old data to check against
  const ParticipantManagement::DynamicParticipantData &oldDynamicData = m_dynamicParticpantData[participantId];

  // check injury state
  ParticipantManagement::StateInformation newYoungestInformation = ParticipantManagement::youngestStateInformation(data.stateHistory());
  ParticipantManagement::StateInformation oldYoungestInformation = ParticipantManagement::youngestStateInformation(oldDynamicData.stateHistory());
  if (oldYoungestInformation.stateTime() != newYoungestInformation.stateTime() ||
      oldYoungestInformation.vulnerabilityState() != newYoungestInformation.vulnerabilityState() ||
      oldYoungestInformation.vulnerabilityBits() != newYoungestInformation.vulnerabilityBits() || // TODO: do we really want to send those bit?
      m_allParticipants.value(oldYoungestInformation.originatorId()).partyId() != m_allParticipants.value(newYoungestInformation.originatorId()).partyId())
    setModified(participantId, OcSituation::ModificationFlag::eVBG);

  // check special bits
  if (getEquipmentStateBits(oldDynamicData)      != getEquipmentStateBits(data))
    setModified(participantId, OcSituation::ModificationFlag::eState);

  m_dynamicParticpantData[participantId] = data;
  DEBUG_LOG(m_logger) << "updated data in m_dynamicParticpantData" << data;

  if (m_modifiedFlags.contains(participantId))
  {
    INFO_LOG(m_logger) << "setModified for: " << m_participantId2EqId[participantId]
                       << " / " << participantId
                       << " bits: 0x" << QString::number(m_modifiedFlags[participantId], 16);
  }
}

void HandlerOcSituationData::onParticipantPositionChanged(const ParticipantManagement::ParticipantPosition &data, DataModification modType)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;
  if (modType == DataModification::eRemoved)
    return;
  const int participantId = data.participantId();
  m_allParticipantPositions[participantId] = data;

  // update current center position when we're following a participant
  if (m_centerType == OcSituation::SituationCenterType::Equipment &&
      participantId == m_centerParticipantId)
    m_centerPosition = data.pos();

  if (!m_participants.contains(participantId))
  {
    DEBUG_LOG(m_logger) << "m_participants does not contain participant id: " << participantId;
    return; // participant does not interest us yet;
  }

  // old data to check against
  const ParticipantManagement::ParticipantPosition &oldPosition = m_participantPositions[participantId];

  // check for ePosition / ePosition3D
  if (oldPosition.sampleTime() <= data.sampleTime())
  {
    //bool isNullOld = qFuzzyIsNull(oldPosition.pos().z());
    bool isNullNew = qFuzzyIsNull(data.pos().z());

//  if (isNullOld && isNullNew)
    if (isNullNew)
      setModified(participantId, OcSituation::ModificationFlag::ePosition2D);
    else
      setModified(participantId, OcSituation::ModificationFlag::ePosition3D);



    /*
        if (isNullOld && isNullNew)
          setModified(participantId, OcSituation::ModificationFlag::ePosition2D);
        else if ((isNullOld  && !isNullNew) ||
                 (!isNullOld && isNullNew))
          setModified(participantId, OcSituation::ModificationFlag::ePosition3D);
        else if (!qFuzzyCompare(oldPosition.pos().z(),
                                data.pos().z()))
          setModified(participantId, OcSituation::ModificationFlag::ePosition3D);
        else
          setModified(participantId, OcSituation::ModificationFlag::ePosition2D);
    */
  }

  m_participantPositions[participantId] = data;
  DEBUG_LOG(m_logger) << "updated data in m_participantPositions" << data;

  if (m_modifiedFlags.contains(participantId))
  {
    INFO_LOG(m_logger) << "setModified for: " << m_participantId2EqId[participantId]
                       << " / " << participantId
                       << " bits: 0x" << QString::number(m_modifiedFlags[participantId], 16);
  }
}

void HandlerOcSituationData::onAesItemViewChanged(const AreaEffectSimulation::AesItemView &data, DataModification modType)
{
  switch (modType)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
    if (data.partyId().is_set())
      m_aesId2PartyId[data.id()] = data.partyId().get();
    else
      m_aesId2PartyId.remove(data.id());
    break;
  case DataModification::eRemoved:
    m_aesId2PartyId.remove(data.id());
    break;
  }
}

void HandlerOcSituationData::onDusMineStateChanged(const AreaEffectSimulation::DusMineState &/*data*/, DataModification /*modType*/)
{
  auto copy = m_dynamicParticpantData;
  for (const ParticipantManagement::DynamicParticipantData &dynPartData : qAsConst(copy))
  {
    if (!m_participantWithMaybeMissingPartyId.contains(dynPartData.participantId()))
      continue;
    onDynamicParticipantDataChanged(dynPartData, DataModification::eModified);
  }
}

void HandlerOcSituationData::handleSituation()
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  QMutexLocker l(&m_accessMutex);

  if (m_radius == 0)
  {
    INFO_LOG(m_logger) << "Do not send Situation as radius is 0, centerEquipmentId " << m_centerEquipmentId
                       << ", m_position " << m_centerPosition;
    QTimer::singleShot(0, this, &HandlerOcSituationData::aliveTimeout); // ending thread
    return;
  }

  fetchParticipantsInRange();

  auto prioQueue = createPrioQueue();
  const QByteArray data = createDataStream(Settings::maxNumOfBytePerTxCycle(), prioQueue);
  if (data.isEmpty())
  {
    DEBUG_LOG(m_logger) << "Don't send empty data to TAB.";
    return;
  }

  QByteArray payload;
  LtsUtils::set1Byte(m_bInitialDataTransfer ? OcSituation::TelegramId::SituationDataInitial : OcSituation::TelegramId::SituationData, payload);
  LtsUtils::set1Byte(m_participantOverloaded, payload);
  m_bInitialDataTransfer = false;

  payload += data;
  updatePrioOfNotSendParticipants();

  TrainingAreaBase::SendOcDataRequest req;
  req.equipmentNumber(m_equipmentId);
  req.payload(LtsUtils::convert<std::vector<uint8_t>>(payload));
  emit sendOcData(req);
}

void HandlerOcSituationData::resetAliveTimer()
{
  m_aliveTimer->start();
}

void HandlerOcSituationData::aliveTimeout()
{
  INFO_LOG(m_logger) << "O/C-Thread keep alive timeout for O/C-Laptop-Id: " << m_equipmentId
                     << " - Thread will be finished id: " << QThread::currentThreadId();

  QThread::currentThread()->quit();
}

void HandlerOcSituationData::onResetInternalData()
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  INFO_LOG(m_logger) << "Resetting internal data!";
  m_participants.clear();
  m_dynamicParticpantData.clear();
  m_participantPositions.clear();
  m_modifiedFlags.clear();
  m_participantsToSend.clear();
  m_outdatedParticipants.clear();
  m_newlyOutdatedParticipants.clear();
  m_participantsNotSend.clear();
  m_participantId2EqId.clear();
  m_participantWithMaybeMissingPartyId.clear();

  m_bInitialDataTransfer = true;
}

void HandlerOcSituationData::fetchParticipantsInRange()
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;
  const QPointF relCenterPos = PositionConverter::relativeFromGeographic(m_centerPosition, m_baseCoordinate);

  if (m_centerType == OcSituation::SituationCenterType::Equipment)
    INFO_LOG(m_logger) << "Fetching participants in radius around equipment " << m_centerEquipmentId
                       << " with position " << relCenterPos.x() << "/" << relCenterPos.y() << " and radius: " << m_radius << ", " << m_baseCoordinate.mgrs()
                       << ", " << m_centerPosition.lon() << ", " << m_centerPosition.lat();
  else
    INFO_LOG(m_logger) << "Fetching participants in radius of position " << relCenterPos.x() << "/" << relCenterPos.y() << " radius: " << m_radius;

  const double rangeSqr = m_radius * m_radius;
  QMultiMap<double, int> curParticipantsInRange;
  QSet<int> curParticipants;
  const QHash<int, ParticipantManagement::Participant> oldParticipants = m_participants;

  for (const auto &position : qAsConst(m_allParticipantPositions))
  {
    if (!LtsUtils::isPositionTimeOutdated(m_partitionTime, position.sampleTime())) // only if position is not outdated
    {
      ParticipantManagement::Participant participant = m_allParticipants.value(position.participantId());
      if (!participant.equipmentNumber().is_set() || participant.partyId() == 0)
        continue; // no units and participants without equipmentnumber or party: pool
      if (m_filterActive)
      {
        ParticipantManagement::TypeClass typeClass = m_ppmDtc->type(participant.typeId()).typeClass();
        if ((m_partyList.count() > 0 && m_partyList.contains(participant.partyId()) == false) ||
            (m_typeClassList.count() > 0 && m_typeClassList.contains(typeClass.underlying()) == false))
        {
          DEBUG_LOG(m_logger) << participant.id() << " filtered out";
          continue;
        }
      }
      const QPointF relPartPos(PositionConverter::relativeFromGeographic(position.pos(), m_baseCoordinate));

      double x = relCenterPos.x() - relPartPos.x();
      double y = relCenterPos.y() - relPartPos.y();
      int distanceSqr = qRound(x * x + y * y);

      if (distanceSqr > rangeSqr)
      {
        DEBUG_LOG(m_logger) << "Participant NOT in range equipment number: " << participant.equipmentNumber().get()
                            << " pos: " << relPartPos.x() << "/" << relPartPos.y()
                            << " distance: " << std::sqrt(distanceSqr) << " distanceSqr: " << distanceSqr;
        continue;
      }

      DEBUG_LOG(m_logger) << "Participant in range equipment number: " << participant.equipmentNumber().get()
                          << " pos: " << relPartPos.x() << "/" << relPartPos.y()
                          << " distance: " << std::sqrt(distanceSqr) << " distanceSqr: " << distanceSqr;
      curParticipantsInRange.insertMulti(distanceSqr, participant.id());
    }
  }
  m_participantOverloaded = quint32(curParticipantsInRange.count()) > Settings::maxParticipantsInSituation();

  // only the hundred nearest will be transmitted
  unsigned int cnt = 0;
  for (const int id : curParticipantsInRange)
  {
    handleParticipant(m_allParticipants.value(id));
    curParticipants.insert(id);
    DEBUG_LOG(m_logger) << "Added participant to hundred nearest: " << m_participantId2EqId[id]
                        << " / " << id;
    ++cnt;
    if (cnt >= Settings::maxParticipantsInSituation())
      break;
  }

  // insert participants, that are now out of range/outdated
  for (const auto &oldPart : oldParticipants)
  {
    const int id = oldPart.id();
    if (!curParticipants.contains(id))
    {
      if (!m_outdatedParticipants.contains(id))
      {
        m_newlyOutdatedParticipants.insert(id);
        m_outdatedParticipants.insert(id);
        DEBUG_LOG(m_logger) << "Participant outdated: " << m_participantId2EqId[id]
                            << " / " << id;
      }
    }
  }
}

void HandlerOcSituationData::handleParticipant(const ParticipantManagement::Participant &participant)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  int participantId = participant.id();
  if (participant.equipmentNumber().is_set())
    m_participantId2EqId[participantId] = participant.equipmentNumber().get();
  else
    m_participantId2EqId.remove(participantId);

  // now check if is a new participant
  if (!m_participants.contains(participantId))
  {
    DEBUG_LOG(m_logger) << "Adding previously unknown participant to m_participants: " << m_participantId2EqId.value(participantId)
                        << " / " << participantId;
    // new participant -> all modified
    setModified(participantId, OcSituation::ModificationFlag::eAll);
    m_participants.insert(participantId, participant);
    m_dynamicParticpantData.insert(participantId, m_allDynamicParticpantData.value(participantId));
    m_participantPositions.insert(participantId, m_allParticipantPositions.value(participantId));
    return;
  }
  else
  {
    // old participant, check if was outdated
    if (m_outdatedParticipants.remove(participantId))
    {
      DEBUG_LOG(m_logger) << "Adding old player that is inside the radius again: " << m_participantId2EqId.value(participantId)
                          << " / " << participantId;
      if (m_modifiedFlags[participantId] == OcSituation::ModificationFlag::eNone)
        setModified(participantId, OcSituation::ModificationFlag::ePosition2D);
    }
  }
}

static inline bool sortParticipantsForRange(const QPair<int, float> &p1,
                                            const QPair<int, float> &p2)
{
  return p1.second < p2.second;
}

QVector<QPair<int, float>> HandlerOcSituationData::createPrioQueue() const
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  QVector<QPair<int, float>> participantsPrioQueue;
  participantsPrioQueue.reserve(m_participantsToSend.size());

  for (auto participantId : qAsConst(m_participantsToSend))
  {
    // ignore (currently) outdated players
    if (m_outdatedParticipants.contains(participantId))
      continue;
    if (!m_participants.contains(participantId))
      continue;
    const ParticipantManagement::ParticipantPosition &position = m_participantPositions[participantId];
    float x = m_centerPosition.lon() - position.pos().lon();
    float y = m_centerPosition.lat() - position.pos().lon();
    float r2 = x * x + y * y;
    float prio = r2;
    int cntNotSendDbg = 0;
    QMap<int, int>::const_iterator participantIt = m_participantsNotSend.constFind(participantId);
    if (participantIt != m_participantsNotSend.constEnd())
    {
      cntNotSendDbg = participantIt.value();
      // Je nicht gesendeten -> 5 % abziehen, aber nie alles
      const int cntNotSend = participantIt.value() > 20 ? 19 : participantIt.value();
      DEBUG_LOG(m_logger) << "prio: " << prio << ", prio / 20 * cntNotSend: " << prio / 20 * cntNotSend;
      prio -= (prio / 20 * cntNotSend);
    }
    participantsPrioQueue.push_back(qMakePair(participantId, prio));

    DEBUG_LOG(m_logger) << "Radius from center for: " << m_participantId2EqId[participantId]
                        << " / " << participantId << " = " << sqrt(r2) << "m, prio: " << prio
                        << ", not sent: " << cntNotSendDbg << " time(s)";
  }
  std::sort(participantsPrioQueue.begin(), participantsPrioQueue.end(), sortParticipantsForRange);
  return participantsPrioQueue;
}

QByteArray HandlerOcSituationData::createDataStream(int maxSize,
                                                    const QVector<QPair<int, float>> &participantsPrioQueue)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;

  QByteArray ret;

  // do not send to much in one swoop
  if (maxSize > 100)
    maxSize -= 5;

  // first: participants that are not in range any more
  DEBUG_LOG(m_logger) << "m_newlyOutdatedParticipants: " << m_newlyOutdatedParticipants.size();
  QSet<int> copyOfOutdated = m_newlyOutdatedParticipants;
  for (const auto participantId : qAsConst(m_newlyOutdatedParticipants))
  {
    addParticipantId(ret, participantId, OcSituation::ModificationFlag::eNone);
    copyOfOutdated.remove(participantId);

    if (ret.size() < maxSize)
      continue;

    QByteArray ba = m_bWithCompression ? qCompress(ret) : ret;
    DEBUG_LOG(m_logger) << "Added info about oudated player " << m_participantId2EqId[participantId]
                        << " / " << participantId
                        << " data size: " << ret.size()
                        << " compressed size: " << ba.size();
    if (ba.size() > maxSize)
    {
      m_newlyOutdatedParticipants = copyOfOutdated;
      TRACE_LOG(m_logger) << "Uncompressed Data: " << ret.toHex(' ');
      return ba;
    }
  }
  m_newlyOutdatedParticipants = copyOfOutdated;

  // Data updates
  DEBUG_LOG(m_logger) << "participantsPrioQueue: " << participantsPrioQueue.size();
  for (auto prioPair : participantsPrioQueue)
  {
    int participantId = prioPair.first;
    if (!m_modifiedFlags.contains(participantId))
      continue;
    OcSituation::ModificationBits modifiedFlags = m_modifiedFlags[participantId];

    // should never happen
    if (modifiedFlags == OcSituation::ModificationFlag::eNone)
      continue;

    const ParticipantManagement::Participant &data = m_participants[participantId];
    const ParticipantManagement::DynamicParticipantData &dynamicParticipantData = m_dynamicParticpantData[participantId];
    const ParticipantManagement::ParticipantPosition &participantPosition = m_participantPositions[participantId];

    // default DynamicParticipantData crashes the tablet software. we skip it ... for now
    if (dynamicParticipantData == ParticipantManagement::DynamicParticipantData())
    {
      INFO_LOG(m_logger) << "There is only default DynamicParticipantData for participant with id " << participantId << " --> skipping";
      continue;
    }

    addParticipantId(ret, participantId, modifiedFlags);
    addPosition2D(ret, participantPosition, (modifiedFlags & OcSituation::ModificationFlag::ePosition2D) == OcSituation::ModificationFlag::ePosition2D);
    addPosition3D(ret, participantPosition, (modifiedFlags & OcSituation::ModificationFlag::ePosition3D) == OcSituation::ModificationFlag::ePosition3D);
    addVBG(ret, dynamicParticipantData, (modifiedFlags & OcSituation::ModificationFlag::eVBG) == OcSituation::ModificationFlag::eVBG);
    addState(ret, dynamicParticipantData, (modifiedFlags & OcSituation::ModificationFlag::eState) == OcSituation::ModificationFlag:: eState);
    addStructure(ret, data, (modifiedFlags & OcSituation::ModificationFlag::eStructure) == OcSituation::ModificationFlag::eStructure);
    addDesignator(ret, data, (modifiedFlags & OcSituation::ModificationFlag::eDesignator) == OcSituation::ModificationFlag::eDesignator);
    addSymbolSidc(ret, data, (modifiedFlags & OcSituation::ModificationFlag::eSymbol) == OcSituation::ModificationFlag::eSymbol);

    clearModified(participantId);

    if (ret.size() < maxSize)
      continue;

    QByteArray ba = m_bWithCompression ? qCompress(ret) : ret;
    DEBUG_LOG(m_logger) << "Added info about changed player " << m_participantId2EqId[participantId]
                        << " / " << participantId
                        << " bits: 0x" << QString::number(modifiedFlags, 16)
                        << " data size: " << ret.size()
                        << " compressed: " << ba.size();
    if (ba.size() > maxSize)
    {
      DEBUG_LOG(m_logger) << "Uncompressed Data: " << ret.toHex(' ');
      return ba;
    }
  }

  if (!ret.isEmpty())
  {
    DEBUG_LOG(m_logger) << "Uncompressed Data: " << ret.toHex(' ');
    return m_bWithCompression ? qCompress(ret) : ret;
  }
  else
    return ret;
}

void HandlerOcSituationData::updatePrioOfNotSendParticipants()
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;
  // add a prio bonus to all participants not send this cycle
  QMap<int, OcSituation::ModificationBits>::const_iterator it = m_modifiedFlags.constBegin();
  for (; it != m_modifiedFlags.constEnd(); ++it)
  {
    const int participantId = it.key();
    QMap<int, int>::iterator participantIt = m_participantsNotSend.find(participantId);
    int notSentSince = 0;
    if (participantIt == m_participantsNotSend.end())
    {
      m_participantsNotSend.insert(participantId, 1);
      notSentSince = 1;
    }
    else
    {
      participantIt.value()++;
      notSentSince = participantIt.value();
    }
    DEBUG_LOG(m_logger) << "Player " << m_participantId2EqId[participantId]
                        << " / " << participantId << " not sent for " << notSentSince << " time(s)";
  }
}

void HandlerOcSituationData::setModified(int participantId, OcSituation::ModificationBits bits)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;
  DEBUG_LOG(m_logger) << "setModified " << bits << " for: " << m_participantId2EqId[participantId]
                      << " / " << participantId;
  m_participantsToSend.insert(participantId);

  if (!m_modifiedFlags.contains(participantId))
  {
    // new entry
    m_modifiedFlags.insert(participantId, bits);
    return;
  }
  m_modifiedFlags[participantId] |= bits;
}

void HandlerOcSituationData::clearModified(int participantId)
{
  TRACE_LOG(m_logger) << "Thread " << QThread::currentThreadId() << " function: " << LOGFUNCTION;
  DEBUG_LOG(m_logger) << "clearModified for: " << m_participantId2EqId[participantId]
                      << " / " << participantId;
  m_modifiedFlags.remove(participantId);
  m_participantsNotSend.remove(participantId);
  m_participantsToSend.remove(participantId);
}

void HandlerOcSituationData::addParticipantId(QByteArray &buf, const int participantId, OcSituation::ModificationBits modifiedFlags) const
{
  LtsUtils::set2Byte(participantId, buf);
  LtsUtils::set1Byte(modifiedFlags, buf);
}

void HandlerOcSituationData::addPosition2D(QByteArray &buf, const ParticipantManagement::ParticipantPosition &data, bool bAdd) const
{
  if (!bAdd)
    return;
  LtsUtils::set8Byte(data.pos().lon(), buf);
  LtsUtils::set8Byte(data.pos().lat(), buf);
  // no timestamp, as it is set in the client anyway

}

void HandlerOcSituationData::addPosition3D(QByteArray &buf, const ParticipantManagement::ParticipantPosition &data, bool bAdd) const
{
  if (!bAdd)
    return;
  addPosition2D(buf, data, bAdd);
  LtsUtils::set8Byte(data.pos().z(), buf);
  LtsUtils::set2Byte(data.buildingId(), buf);
  LtsUtils::set4Byte(data.roomId(), buf);
}

void HandlerOcSituationData::addVBG(QByteArray &buf, const ParticipantManagement::DynamicParticipantData &data, bool bAdd) const
{
  if (!bAdd)
    return;

  const ParticipantManagement::StateInformation &youngestStateInformation = ParticipantManagement::youngestStateInformation(data.stateHistory(), true, false);

  LtsUtils::set1Byte(youngestStateInformation.vulnerabilityState().underlying(), buf);
  LtsUtils::set4Byte(youngestStateInformation.vulnerabilityKindId(), buf);
  LtsUtils::set2Byte(youngestStateInformation.vulnerabilityBits(), buf);

  int injuryPartyId = 1;
  switch (youngestStateInformation.originatorType().underlying())
  {
  case ProjectDataManagement::StateChangeOriginator_def::Unknown:
  case ProjectDataManagement::StateChangeOriginator_def::ExerciseControl:
  case ProjectDataManagement::StateChangeOriginator_def::ObserverController:
  case ProjectDataManagement::StateChangeOriginator_def::Initialization:
    injuryPartyId = 1;  // O/C
    break;
  case ProjectDataManagement::StateChangeOriginator_def::Participant:
    injuryPartyId = m_allParticipants.value(youngestStateInformation.originatorId()).partyId();
    if (injuryPartyId == 0)
      injuryPartyId = 1;
    break;
  case ProjectDataManagement::StateChangeOriginator_def::AreaEffectSimulation:
  case ProjectDataManagement::StateChangeOriginator_def::FunctionalArea:
  {
    auto it = m_aesId2PartyId.constFind(youngestStateInformation.originatorId());
    if (it != m_aesId2PartyId.constEnd())
      injuryPartyId = it.value();
    break;
  }
  case ProjectDataManagement::StateChangeOriginator_def::DusMine:
  {
    int partyId = m_aesDtc->partyForDusMine(youngestStateInformation.originatorId());
    if (partyId >= 0)
      injuryPartyId = partyId;

    if (partyId == -1)
      m_participantWithMaybeMissingPartyId.insert(data.participantId());
    else
      m_participantWithMaybeMissingPartyId.remove(data.participantId());

    break;
  }
  case ProjectDataManagement::StateChangeOriginator_def::ImprovisedExplosiveDevice:
  {
    const auto ied = m_gimDtc->iedForEquipmentNumber(youngestStateInformation.originatorId());
    if (ied.equipmentNumber() != 0)
      injuryPartyId = ied.partyId();
    break;
  }
  case ProjectDataManagement::StateChangeOriginator_def::WoundTreatmentDevice:
  case ProjectDataManagement::StateChangeOriginator_def::MaintainanceDevice:
  case ProjectDataManagement::StateChangeOriginator_def::Sanitation:
  case ProjectDataManagement::StateChangeOriginator_def::MedicalTimeout:
    // not yet used
    break;
  }
  LtsUtils::set1Byte(injuryPartyId, buf);

  if (m_bWithVBGTime)
    LtsUtils::set4Byte(youngestStateInformation.stateTime() / 1000, buf);

  //add vitality and sanitation result
  // Quick fix: If vitality is
  uint8_t vitRes = (youngestStateInformation.vitality() / 10);
  if (vitRes > 10)
  {
    WARNING_LOG(m_logger) << "Vitality of Participant " << data.participantId() << " is not between 0 and 100%. Putting to 100%";
    vitRes = 10;
  }
  vitRes += ((int)youngestStateInformation.sanitationResult().underlying() << 4);
  LtsUtils::set1Byte(vitRes, buf);
  //finall 1 Byte for  originator and maybe more
  LtsUtils::set1Byte(youngestStateInformation.originatorType().underlying(), buf);


}

void HandlerOcSituationData::addState(QByteArray &buf, const ParticipantManagement::DynamicParticipantData &data, bool bAdd) const
{
  if (!bAdd)
    return;
  LtsUtils::set1Byte(getEquipmentStateBits(data), buf);
}

void HandlerOcSituationData::addStructure(QByteArray &buf, const ParticipantManagement::Participant &data, bool bAdd) const
{
  if (!bAdd)
    return;
  LtsUtils::set2Byte(data.equipmentNumber().is_set() ? data.equipmentNumber().get() : 0, buf);
  LtsUtils::set2Byte(data.typeId(), buf);
  LtsUtils::set2Byte(data.partyId(), buf);
#ifndef GUEZ_MODE
  LtsUtils::set2Byte(data.superiorId(), buf);
#endif
}

void HandlerOcSituationData::addDesignator(QByteArray &buf, const ParticipantManagement::Participant &data, bool bAdd) const
{
  if (!bAdd)
    return;
  const size_t len1 = std::min(data.designator().size(), size_t(15));
  const size_t len2 = std::min(data.tacticalIdentifier().size(), size_t(15));
  unsigned char len = ((len1 & 0x0F) | ((len2 & 0x0F) << 4));
  LtsUtils::set1Byte(len, buf);
  buf.append(data.designator().c_str(), len1);
  buf.append(data.tacticalIdentifier().c_str(), len2);
}

void HandlerOcSituationData::addSymbolSidc(QByteArray &buf, const ParticipantManagement::Participant &data, bool bAdd) const
{
  if (!bAdd)
    return;
  const auto type = m_ppmDtc->type(data.typeId());
  if (type.symbol() == data.symbol())
  {
    LtsUtils::set1Byte(0, buf); // 0 bytes -> marker for not sending the symbol sidc since it can be taken from the type
    return;
  }
  if (data.symbol().size() > 255)
    return;
  const size_t len1 = std::min(data.symbol().size(), size_t(255));
  LtsUtils::set1Byte(len1, buf);
  buf.append(data.symbol().c_str(), len1);
}

} // namespace OcSituationServer