qt5-legatus-legacy/Lts/Proxies/GuezNasat/handler_participant.cpp

/**
  * Handle the mapping real aircraft <-> nasat configuration
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, NLSE, 2018-08-16
  */

#include "handler_participant.h"
#include "handler_nasat_aircraft.h"
#include "handler_nasat_aircraft_participant.h"
#include "initialization.h"
#include "dds_handler.h"
#include <LtsCoordTrans/lts_coord_trans.h>

#include <QTimer>

namespace GuezNasat
{

// ----------------------------------------------------------------
// NasatAircraftParticipantEx
// ----------------------------------------------------------------
NasatAircraftParticipantEx::NasatAircraftParticipantEx()
  : m_logger(getLogger("NasatAircraftParticipantEx"))
{
  DEBUG_LOG(m_logger) << LOGFUNCTION << " START";
  DEBUG_LOG(m_logger) << LOGFUNCTION << " END";
}

NasatAircraftParticipantEx::NasatAircraftParticipantEx(const NasatAircraftParticipant &nap, const std::shared_ptr<Participant2Dds> &publisher)
  : m_nap(nap)
  , m_publisher(publisher)
  , m_logger(getLogger("NasatAircraftParticipantEx"))
{
  DEBUG_LOG(m_logger) << LOGFUNCTION << " START";
  m_publisher->addParticipantToWatch(m_nap.participantId());
  DEBUG_LOG(m_logger) << LOGFUNCTION << " END";
}

NasatAircraftParticipantEx::~NasatAircraftParticipantEx()
{
  DEBUG_LOG(m_logger) << LOGFUNCTION << " START";
  m_publisher->removeParticipantToWatch(m_nap.participantId());
  DEBUG_LOG(m_logger) << LOGFUNCTION << " END";
}

int NasatAircraftParticipantEx::partyId() const
{
  return m_publisher ? m_publisher->partyId(participantId()) : -1;
}


// ----------------------------------------------------------------
// HandlerParticipant
// ----------------------------------------------------------------
HandlerParticipant::HandlerParticipant(Initialization *mainHandler)
  : m_mainHandler(mainHandler)
  , m_logger(getLogger("ParticipantPositionPublisher"))
{}

HandlerParticipant::~HandlerParticipant()
{
  for (const auto &ex : qAsConst(m_activePublisher))
    qDeleteAll(ex);
}

QHash<ExerciseParticipantKey, NasatAircraftParticipantEx *> HandlerParticipant::participantsForNasatId(int nasatId) const
{
  return m_activePublisher.value(nasatId);
}

void HandlerParticipant::onNasatAircraftParticipantCreated(const NasatAircraftParticipant &nap)
{
  const int nasatId = m_mainHandler->nasatAircraft()->nasatIdFromNasatAircraftId(nap.nasatAircraftId());
  auto it = m_activePublisher.find(nasatId);
  if (it == m_activePublisher.end())
    return createNasatAircraftParticipant(nap, nasatId);

  auto itParticipants = it.value().find(ExerciseParticipantKey(nap.exerciseId(), nap.participantId()));
  if (itParticipants == it.value().end())
    return createNasatAircraftParticipant(nap, nasatId);
}

void HandlerParticipant::onNasatAircraftParticipantRemoved(const NasatAircraftParticipant &nap)
{
  const int nasatId = m_mainHandler->nasatAircraft()->nasatIdFromNasatAircraftId(nap.nasatAircraftId());
  auto it = m_activePublisher.find(nasatId);
  if (it == m_activePublisher.end())
  {
    WARNING_LOG(m_logger) << "No publisher for NasatAircraftParticipant " << nasatId << "(" << nap.nasatAircraftId() << ") found !?";
    return;
  }
  auto itParticipants = it.value().find(ExerciseParticipantKey(nap.exerciseId(), nap.participantId()));
  if (itParticipants == it.value().end())
  {
    WARNING_LOG(m_logger) << "No participant entry for NasatAircraftParticipant " << nasatId << "("
                          << nap.exerciseId() << "," << nap.participantId() << ") found !?";
    return;
  }

  // to enable removing of according Nasat9Liner:
  DEBUG_LOG(m_logger) << " emit nasatAircraftParticipantRemoved, nap = " << nap;
  emit nasatAircraftParticipantRemoved(nap);


  DEBUG_LOG(m_logger) << "Remove position publisher for " << nap;

  // spent time for samples to publish (UI: change link between aircraft and participant)
  NasatAircraftParticipantEx* napeCopy= itParticipants.value();
  if (napeCopy && napeCopy->publisher())
  {
    // do cleanup as like ~NasatAircraftParticipantEx
    napeCopy->publisher()->removeParticipantToWatch(napeCopy->participantId());
  }

  QTimer::singleShot(2000, this, [napeCopy]() {
    delete napeCopy;
  });

  it.value().erase(itParticipants);
  if (it.value().isEmpty())
    m_activePublisher.erase(it);
}

Core::GeoPosition HandlerParticipant::nasatPosition(int nasatId) const
{
  DEBUG_LOG(m_logger) << LOGFUNCTION << " START, nasatId=" << nasatId;
  auto it = m_lastNasatPositionsAsCoreGeoposition.find(nasatId);
  if (it == m_lastNasatPositionsAsCoreGeoposition.end())
  {
    INFO_LOG(m_logger) << "No NasatPosition for nasat id " << nasatId << " found";
    return {};
  }
  return *it;
}

void HandlerParticipant::onNasatPosition(const NasatPosition &position)
{
  DEBUG_LOG(m_logger) << LOGFUNCTION << " START, position.aircraftId=" << position.aircraftId;
  const Core::GeoPosition geoPos(position.position.lon * LtsCoordTrans::deg2rad(),
                                 position.position.lat * LtsCoordTrans::deg2rad(),
                                 position.position.height);  // heigth == aboveGround!?! (normally, 'height' is relative to ground)

  const qint64 tsMSec = position.timestamp <= 0 ? QDateTime::currentMSecsSinceEpoch() : qRound64(position.timestamp * 1000);
  onNasatAircraftPosition(position.aircraftId, geoPos, tsMSec);

  m_lastNasatPositionsAsCoreGeoposition[position.aircraftId] = geoPos;
}

void HandlerParticipant::onNasatAircraftPosition(int nasatId, const Core::GeoPosition &pos, qint64 ts)
{
  DEBUG_LOG(m_logger) << LOGFUNCTION << " START, nasatId=" << nasatId;
  auto it = m_activePublisher.find(nasatId);
  if (it == m_activePublisher.end())
  {
    INFO_LOG(m_logger) << "No position publisher for nasat id " << nasatId << " found - ignoring " << m_activePublisher.size();
    return;
  }

  const auto &h = it.value();
  TrainingAreaBase::EquipmentPosition tabPos;
  tabPos.position().position(pos);
  tabPos.sampleTime(ts);
  for (const NasatAircraftParticipantEx *napEx : h)
  {
    tabPos.equipmentNumber(-napEx->participantId());
    int success = napEx->publisher()->publish(tabPos);
    if (success)
      DEBUG_LOG(m_logger) << "Publishing positon: " << tabPos << " to exercise " << napEx->exerciseId()
                          << ", napEx: " << napEx->participant()
                          << " -> success = " << success;
    else
      ERROR_LOG(m_logger) << "Error on Publishing positon: " << tabPos << " to exercise " << napEx->exerciseId()
                          << ", napEx: " << napEx->participant()
                          << " -> success = " << success;

  }
}

void HandlerParticipant::onNasatIdChanged(int oldNasatId, int newNasatId)
{
  m_activePublisher[newNasatId] = m_activePublisher[oldNasatId];
  m_activePublisher.remove(oldNasatId);
}

void HandlerParticipant::createNasatAircraftParticipant(const NasatAircraftParticipant &nap, int nasatId)
{
  DEBUG_LOG(m_logger) << "Creating position publisher for " << nap
                      << ", nasatId=" << nasatId;
  auto *napEx = new NasatAircraftParticipantEx(nap,
                                               m_mainHandler->dds()->ddsParticipant(nap.exerciseId()));
  m_activePublisher[nasatId][ExerciseParticipantKey(nap.exerciseId(), nap.participantId())] = napEx;
  connect(napEx->publisher().get(), &Participant2Dds::participantRemoved,
          m_mainHandler->nasatAircraftParticipant().get(), &HandlerNasatAircraftParticipant::participantRemoved,
          Qt::QueuedConnection);
}

} // namespace GuezNasat