#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 <QtCore/QDateTime>
#include <LtsCoordTrans/lts_coord_trans.h>

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

bool HandlerNasatNineLiner::init() const
{
  
  
  connect(m_mainHandler->handlerParticipant().get(), &HandlerParticipant::nasatAircraftParticipantRemoved,
          this, &HandlerNasatNineLiner::onNasatAircraftParticipantRemoved, Qt::DirectConnection);

  DEBUG_LOG(m_logger) << "HandlerNasatNineLiner init done";
  
  
  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>(nasatNineLiner.heading2InitialPointMode);
    nl.hdgInitialPoint2Target = nasatNineLiner.headingInitialPoint2Target;
  }
  else
  {
    nl.hdg2InitialPointMode = Hdg2InitialPointMode::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)
{
  
  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);

    
    




















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

void HandlerNasatNineLiner::onNasatAircraftParticipantRemoved(const NasatAircraftParticipant &nap)
{
  
  

  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) << Q_FUNC_INFO << " 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) << Q_FUNC_INFO << " updating for removing "
                            << ", exerciseParticipantId.exerciseId=" << exerciseParticipantId.exerciseId
                            << ", exerciseParticipantId.participantId=" << exerciseParticipantId.participantId;
        oldLast.sampleTime = QDateTime::currentMSecsSinceEpoch(); 
        oldLast.isCurrent = false;  
        nape->publisher()->publish(oldLast);
      }
    }

  } 
}


}