#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)
{}

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::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)
{
  
  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);
      }
    }

  } 
}


}