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

/**
  * Handle a nine-liner from a nasat aircraft and publish them
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2019-12-01
  */


#include "handler_nasat_nine_liner.h"
#include "handler_nasat_aircraft.h"
#include "handler_nasat_aircraft_participant.h"
#include "handler_participant.h"
#include "initialization.h"
#include "dds_handler.h"

#include <QDateTime>
#include <LtsCoordTrans/lts_coord_trans.h>

namespace GuezNasat
{
HandlerNasatNineLiner::HandlerNasatNineLiner(Initialization *mainHandler)
  : m_mainHandler(mainHandler)
  , m_logger(getLogger("HandlerNasatNineLiner"))
{}

bool HandlerNasatNineLiner::init() const
{
  // if an association between nasatAircraftId and participant) is removed
  // Planning | Assign NASAT Aircrafts) the associated NineLiner should also be removed:
  connect(m_mainHandler->handlerParticipant().get(), &HandlerParticipant::nasatAircraftParticipantRemoved,
          this, &HandlerNasatNineLiner::onNasatAircraftParticipantRemoved, Qt::DirectConnection);

  DEBUG_LOG(m_logger) << "HandlerNasatNineLiner init done";
  // currently we don't read old 9-liner data from the database to publish
  // them again on restart - too much work without any real gain.
  return true;
}

static Nasat9Liner createDds9Liner(const NasatNineLiner &nasatNineLiner)
{
  Nasat9Liner nl;
  const qint64 tsMSec = nasatNineLiner.timestamp <= 0 ? QDateTime::currentMSecsSinceEpoch() : qRound64(nasatNineLiner.timestamp * 1000);
  nl.sampleTime(tsMSec);

  if (nasatNineLiner.initialPointMode != 0)
    nl.initialPoint(Core::GeoPosition2D(LtsCoordTrans::deg2rad() * nasatNineLiner.initialPoint.lon,
                                        LtsCoordTrans::deg2rad() * nasatNineLiner.initialPoint.lat));

  if (nasatNineLiner.heading2InitialPointMode != Heading2InitialPointMode::Undefined)
  {
    nl.hdg2InitialPointMode(static_cast<Hdg2InitialPointMode::inner_enum>(nasatNineLiner.heading2InitialPointMode));
    nl.hdgInitialPoint2Target(nasatNineLiner.headingInitialPoint2Target);
  }
  else
  {
    nl.hdg2InitialPointMode(Hdg2InitialPointMode_def::Undefined);
    nl.hdgInitialPoint2Target(-1);
  }

  if (nasatNineLiner.distance2InitialPointMode != 0)
    nl.distanceInitialPoint2Target(nasatNineLiner.distanceInitialPoint2Target);
  else
    nl.distanceInitialPoint2Target(-1);

  if (nasatNineLiner.targetPositionMode != 0)
  {
    const auto z = nasatNineLiner.altitudeTargetMode != 0 ? nasatNineLiner.altitudeTarget : -1;
    nl.targetPosition(Core::GeoPosition(LtsCoordTrans::deg2rad() * nasatNineLiner.targetPosition.lon,
                                        LtsCoordTrans::deg2rad() * nasatNineLiner.targetPosition.lat,
                                        z));
  }
  else
  {
    nl.targetPosition(Core::GeoPosition(std::nan(""), std::nan(""), std::nan("")));
  }

  if (nasatNineLiner.modeFriendlies != 0)
  {
    nl.friendliesBearing(nasatNineLiner.friendlies.bearing);
    nl.friendliesDistance(nasatNineLiner.friendlies.distance);
  }
  else
  {
    nl.friendliesBearing(-1);
    nl.friendliesDistance(-1);
  }

  if (nasatNineLiner.finalAttackHeadingMode != FinalAttackHeadingMode::None)
  {
    nl.finalAttackHeading(nasatNineLiner.finalAttackHeading.heading);
    if (nasatNineLiner.finalAttackHeadingMode == FinalAttackHeadingMode::HeadingWithSector)
    {
      nl.finalAttackHeadingLow(nasatNineLiner.finalAttackHeading.headingLow);
      nl.finalAttackHeadingHigh(nasatNineLiner.finalAttackHeading.headingHigh);
    }
    else
    {
      nl.finalAttackHeadingLow(-1);
      nl.finalAttackHeadingHigh(-1);
    }
  }
  else
    nl.finalAttackHeading(-1);

  if (nasatNineLiner.timeForTargetMode != TimeForTargetMode::None)
    nl.timeForTarget(nasatNineLiner.timeForTargetMode == TimeForTargetMode::TimeToTarget ? -nl.timeForTarget() : nl.timeForTarget());
  else
    nl.timeForTarget(0);

  return nl;
}

void HandlerNasatNineLiner::nineLinerReceived(const NasatNineLiner &nasatNineLiner)
{
  // QHash<ExerciseParticipantKey, NasatAircraftParticipantEx>>
  DEBUG_LOG(m_logger) << "nineLinerReceived, aircraftId=" << nasatNineLiner.aircraftId
                      << "  -  nasatNineLiner.aircraftId is aka nasatId";

  const auto participants = m_mainHandler->handlerParticipant()->participantsForNasatId(nasatNineLiner.aircraftId);
  if (participants.isEmpty())
  {
    INFO_LOG(m_logger) << "No exercise participants found to publish 9-liner for nasat aircraft with id "
                       << nasatNineLiner.aircraftId;
    return;
  }

  DEBUG_LOG(m_logger) << "nineLinerReceived, publishing to exercises";

  static int s_id = 1;
  Nasat9Liner nl = createDds9Liner(nasatNineLiner);
  for (auto it = participants.begin(); it != participants.end(); ++it)
  {
    nl.id(s_id++);
    const auto &exerciseParticipantId = it.key();
    auto nape = it.value();

    DEBUG_LOG(m_logger) << "nineLinerReceived, publishing to exercises, s_id=" << s_id
                        << ", exerciseParticipantId.exerciseId=" << exerciseParticipantId.exerciseId
                        << ", exerciseParticipantId.participantId=" << exerciseParticipantId.participantId;

    auto &nineLinerParticipant = m_nineLiner[exerciseParticipantId];
    nl.isCurrent(true);
    nl.participantId(exerciseParticipantId.participantId);

    nineLinerParticipant.push_back(nl);

    // GUEZE-1676: Nineliner sollen zeitlich wie Trefferlinien angezeigt werden
    // isCurrent is outdated
//    if (nineLinerParticipant.size() > 1)
//    {
//      auto &oldLast = nineLinerParticipant[nineLinerParticipant.size() - 2];
//      if (oldLast.sampleTime() > nl.sampleTime())
//      {
//        nineLinerParticipant.last().isCurrent(false);  // in vector
//        nl.isCurrent(false); // for publishing

//        std::sort(nineLinerParticipant.begin(), nineLinerParticipant.end(),
//        [](const Nasat9Liner & a, const Nasat9Liner & b) { return a.sampleTime() < b.sampleTime();});
//      }
//      else
//      {
//        // outdated
//        oldLast.isCurrent(false);  // in vector + for publishing
//        oldLast.sampleTime(QDateTime::currentMSecsSinceEpoch()); // update sample_time for proper replay
//        nape->publisher()->publish(oldLast);
//      }
//    }

    nape->publisher()->publish(nl);
  }
}

void HandlerNasatNineLiner::onNasatAircraftParticipantRemoved(const NasatAircraftParticipant &nap)
{
  // don´t know, how to remove the samples from dds.
  // -> set to isCurrent(false)...

  const int nasatId = m_mainHandler->nasatAircraft()->nasatIdFromNasatAircraftId(nap.nasatAircraftId());
  INFO_LOG(m_logger) << " onNasatAircraftParticipantRemoved,  nap = " << nap << ", nasatId = " << nasatId;
  if (nasatId < 0)
  {
    WARNING_LOG(m_logger) << " nasatIdFromNasatAircraftId failed, nap=" << nap;
    return;
  }

  const auto participants = m_mainHandler->handlerParticipant()->participantsForNasatId(nasatId);
  if (participants.isEmpty())
  {
    INFO_LOG(m_logger) << "No exercise participants found to publish 9-liner for nasat aircraft with id "
                       << nap.nasatAircraftId();
    return;
  }

  for (auto it = participants.begin(); it != participants.end(); ++it)
  {
    const auto &exerciseParticipantId = it.key();
    auto nape = it.value();
    auto &nineLinerParticipant = m_nineLiner[exerciseParticipantId];

    DEBUG_LOG(m_logger) << LOGFUNCTION << " updating for removing "
                        << ", exerciseParticipantId.exerciseId=" << exerciseParticipantId.exerciseId
                        << ", exerciseParticipantId.participantId=" << exerciseParticipantId.participantId
                        << ", nineLinerParticipant.isEmpty()=" << nineLinerParticipant.isEmpty();

    if (!nineLinerParticipant.isEmpty())
    {
      auto &oldLast = nineLinerParticipant.last();
      if (oldLast.isCurrent())
      {
        DEBUG_LOG(m_logger) << LOGFUNCTION << " updating for removing "
                            << ", exerciseParticipantId.exerciseId=" << exerciseParticipantId.exerciseId
                            << ", exerciseParticipantId.participantId=" << exerciseParticipantId.participantId;
        oldLast.sampleTime(QDateTime::currentMSecsSinceEpoch()); // update sample_time for proper replay
        oldLast.isCurrent(false);  // in vector - will suppress the according situation map object
        nape->publisher()->publish(oldLast);
      }
    }

  } // for
}


} // namespace GuezNasat