/**
  * @RDE_HEADER@
  * Handler for incoming data from minelayer trialer/thrower and thier lane data
  * $Id$
  * $HeadURL$
  * $Author$
  * created by Philipp Dassow, SLEA, 2017-04-06
  */

#include "handler_minelayer_data.h"

#if BUILD_RTI
#include <AreaEffectSimulation/aes_ftc.h>
#endif
#include <AssetManagement/asm_dtc.h>
#include <ExerciseManagement/exm_dtc.h>

#include "dds_handler.h"
#include "zssi_wi_Minenfeld_Interpreter.h"

#include <QtCore/QLineF>

#if BUILD_RTI

namespace GuezMinelayerProcessing
{

HandlerMinelayerData::HandlerMinelayerData(const std::shared_ptr<DdsHandler> &ddsHandler,
                                           QObject *parent)
  : QObject(parent)
  , m_logger(getLogger("GuezMinelayerProcessing.HandlerMinelayerData"))
  , m_ddsHandler(ddsHandler)
#if BUILD_RTI
  , m_tabDtc(nullptr)
  , m_ppmDtc(nullptr)
  , m_aesFtc(nullptr)
#endif
  , m_minefieldInterpreter(new ZSSI_WI_Minenfeld_Interpreter(this))
  , m_minefieldInterpreterTimer(new QTimer(this))
{}

HandlerMinelayerData::~HandlerMinelayerData()
{}

bool HandlerMinelayerData::init()
{
#if BUILD_RTI
  // AES-Ftc
  m_aesFtc = AreaEffectSimulation::Interface::Ftc::create(m_ddsHandler->ddsConnector());
  bool ret = m_aesFtc->enableMineAreaModifyFromCtaData();
  if (!ret)
  {
    ERROR_LOG(m_logger) << "AesFtc Can not enable MineAreaModifyFromCtaData()";
    return false;
  }
#endif


  // Data from training area base
  m_tabDtc = TrainingAreaBase::Interface::Dtc::create(m_ddsHandler->ddsConnector());
  bool ret = true;
#if BUILD_RTI
  ret &= m_tabDtc->enableSpecialSensorData();
#endif
  ret &= m_tabDtc->enableMinelayerTrailerData();
  ret &= m_tabDtc->enableMinelayerThrowerData();
  if (!ret)
  {
    ERROR_LOG(m_logger) << "TabDtc Can not ebale Data";
    return false;
  }

#if BUILD_RTI
  connect(m_tabDtc.get(), &TrainingAreaBase::Interface::Dtc::specialSensorDataChanged, this, &HandlerMinelayerData::onSpecialSensorDataChanged);
#endif
  connect(m_tabDtc.get(), &TrainingAreaBase::Interface::Dtc::minelayerTrailerDataChanged, this, &HandlerMinelayerData::onMinelayerTrailerData);
  connect(m_tabDtc.get(), &TrainingAreaBase::Interface::Dtc::minelayerThrowerDataChanged, this, &HandlerMinelayerData::onMinelayerThrowerData);

  // PPM for partyId Mapping
  m_ppmDtc = ParticipantManagement::Interface::Dtc::create(m_ddsHandler->ddsConnector());
#if BUILD_RTI
  ret &= m_ppmDtc->enableParticipant();
#endif
  if (!ret)
  {
    ERROR_LOG(m_logger) << "PpmDtc Can not enable Participant Data";
    return false;
  }

  // connect to minefield generator
  connect(m_minefieldInterpreter, &ZSSI_WI_Minenfeld_Interpreter::sendeWirkfeld, this, &HandlerMinelayerData::onSendMinefield);


  // start Interpreter
  connect(m_minefieldInterpreterTimer, &QTimer::timeout, this, &HandlerMinelayerData::onStartMinefieldInterpreter);
  m_minefieldInterpreterTimer->start(m_minefieldInterpreter->triggerIntervall * 1000);

  return ret;
}

void HandlerMinelayerData::onSpecialSensorDataChanged(const TrainingAreaBase::SpecialSensorData &data, ParticipantManagement::DataModification modType)
{
  if (modType != DataModification::eAdded &&
      modType != DataModification::eModified)
    return;
  if (data.operationalState() != MV_VERLEGEN &&
      data.operationalState() != WMV_VERLEGEN)
    return;
  if (!data.position().is_set())
    return;
  {
    // convert TrainingAreaBase::LaneData back to sondersensorik_fahrspur_rt
    FAHRSPUR_INTERN daten_intern;
    //Interpretationsstatus auf 0 setzen
    daten_intern.intstat = 0;

    daten_intern.daten.header.identitaet = data.equipmentNumber();
    daten_intern.daten.gps_zeit.gps_zeit = data.sampleTime() / 1000;
    daten_intern.daten.gps_position = relativeFromGeographic(data.position().get().lon(), data.position().get().lat());
    daten_intern.daten.ss_betriebszustand = data.operationalState();

    // old code copied from zssi_wi_Empfaenger.cpp
    /*
      if ( checkeTelegrammZeit(telegramm_rt.gps_zeit.gps_zeit) ) {
        ERROR_LOG(m_logger) << QStringLiteral("Keine plausible gps_zeit !!  gps_zeit=%1").arg(telegramm_rt.gps_zeit.gps_zeit);
        return;
      }

      if ( checkeKoordinaten(telegramm_rt.gps_pos)!=0 ) {
        ERROR_LOG(m_logger) << QStringLiteral("Keine plausible gps_pos !!  pos_x=%1 Keine plausible gps_pos !!  pos_y=%2").arg(telegramm_rt.gps_pos.utm_ostwert).arg(telegramm_rt.gps_pos.utm_nordwert);
        return;
      }
    */

    VERLEGEDATEN_KEY key_intern;
    key_intern.fz_id = daten_intern.daten.header.identitaet;
    key_intern.zeitpunkt.gps_zeit = daten_intern.daten.gps_zeit.gps_zeit;

    interne_datenbasis_Fahrspur.eintrag_container(key_intern, daten_intern);

    INFO_LOG(m_logger) << "Inserted new lane data into old interne_datenbasis_Fahrspur succesfully! Lane data:" << data;

    // end old code
  }
}

template<typename T>
void HandlerMinelayerData::convertMinelayerDataToXdrData(const TrainingAreaBase::MinelayerTrailerData &inData, T &outData)
{
  outData.header.identitaet = inData.equipmentNumber();
  outData.gps_zeit.gps_zeit = inData.sampleTime() / 1000; // derived from Tetra telegram

  outData.gps_position = relativeFromGeographic(inData.position().lon(), inData.position().lat());
  outData.verlegedaten.sperren_id = inData.barrierId();
  outData.verlegedaten.verlegedichte = inData.layingDensity();
  outData.verlegedaten.wirkbeginn.gps_zeit = inData.effectStart() / 1000; // derived from Tetra telegram
  outData.verlegedaten.wirkzeit = inData.effectDuration();
  outData.verlegedaten.minentyp = inData.chargeTypeId();
  outData.verlegedaten.anzahl_verlegter_minen = inData.mineCount();
}

void HandlerMinelayerData::onMinelayerTrailerData(const TrainingAreaBase::MinelayerTrailerData &data, ParticipantManagement::DataModification modType)
{
  if ((modType == DataModification::eAdded) ||
      (modType == DataModification::eModified))
  {
    VERLEGEDATEN_INTERN daten_intern;
    convertMinelayerDataToXdrData(data, daten_intern.daten);

    // old code copied from zssi_wi_Empfaenger.cpp
    /*
        INFO_LOG(m_logger) << log_tr("Telegramm 'SONDERSENSORIK_MIVANH_VERLEGEDATEN' empfangen");

        if ( checkeTelegrammZeit(telegramm_rt.gps_zeit.gps_zeit) ) {
          ERROR_LOG(m_logger) << log_tr("Keine plausible gps_zeit !!  gps_zeit=%1").arg(telegramm_rt.gps_zeit.gps_zeit);
          return;
        }

        if ( checkeKoordinaten(telegramm_rt.gps_pos)!=0 ) {
          ERROR_LOG(m_logger) << log_tr("Keine plausible gps_pos !!  pos_x=%1 Keine plausible gps_pos !!  pos_y=%2").arg(telegramm_rt.gps_pos.utm_ostwert).arg(telegramm_rt.gps_pos.utm_nordwert);
          return;
        }
    */

    daten_intern.daten.verlegedaten.ausstossart = ausstossart_et(0);
    //Diese Null gilt als Unterscheidung zwischen Scorpion und
    //Minenverlegeangaenger, da der Scorpion nur ausstossart 1..3 hat

    if (daten_intern.daten.verlegedaten.anzahl_verlegter_minen > 1200)
    {
      WARNING_LOG(m_logger) << QStringLiteral("Anzahl_verlegter_minen = %1, wird auf 1200 zurueckgesetzt").arg(daten_intern.daten.verlegedaten.anzahl_verlegter_minen);
      daten_intern.daten.verlegedaten.anzahl_verlegter_minen = 1200;
    }

    VERLEGEDATEN_KEY key_intern;
    key_intern.fz_id = daten_intern.daten.header.identitaet;
    key_intern.zeitpunkt.gps_zeit = daten_intern.daten.gps_zeit.gps_zeit;
    interne_datenbasis_Verlegedaten.eintrag_container(
      key_intern, daten_intern);

    INFO_LOG(m_logger) << "Inserted new minelayertrailer data into old interne_datenbasis_Verlegedaten succesfully! Minelayerthrower data:" << data;
  }
}

void HandlerMinelayerData::onMinelayerThrowerData(const TrainingAreaBase::MinelayerThrowerData &data, ParticipantManagement::DataModification modType)
{
  if ((modType == DataModification::eAdded) ||
      (modType == DataModification::eModified))
  {
    VERLEGEDATEN_INTERN daten_intern;
    convertMinelayerDataToXdrData(data, daten_intern.daten);
    daten_intern.daten.verlegedaten.ausstossart = static_cast<ausstossart_et>(data.throwingType().underlying());

    /*
        if ( checkeTelegrammZeit(telegramm_rt.gps_zeit.gps_zeit) ) {
          ERROR_LOG(m_logger) << log_tr("Keine plausible gps_zeit !!  gps_zeit=%1").arg(telegramm_rt.gps_zeit.gps_zeit);
          return;
        }

        if ( checkeKoordinaten(telegramm_rt.gps_pos) !=0 ) {
          ERROR_LOG(m_logger) << log_tr("Keine plausible gps_pos !!  pos_x=%1 Keine plausible gps_pos !!  pos_y=%2").arg(telegramm_rt.gps_pos.utm_ostwert).arg(telegramm_rt.gps_pos.utm_nordwert);
          return;
        }
    */

    if (daten_intern.daten.verlegedaten.anzahl_verlegter_minen > 1200)
    {
      WARNING_LOG(m_logger) << QStringLiteral("Anzahl_verlegter_minen = %1, wird auf 1200 zurueckgesetzt").arg(daten_intern.daten.verlegedaten.anzahl_verlegter_minen);
      daten_intern.daten.verlegedaten.anzahl_verlegter_minen = 1200;
    }

    VERLEGEDATEN_KEY key_intern;
    key_intern.fz_id = daten_intern.daten.header.identitaet;
    key_intern.zeitpunkt.gps_zeit = daten_intern.daten.gps_zeit.gps_zeit;

    if (daten_intern.daten.verlegedaten.ausstossart >= 5 && daten_intern.daten.verlegedaten.ausstossart <= 7)
    {
      daten_intern.daten.verlegedaten.anzahl_verlegter_minen = 5;
      m_minefieldInterpreter->ZSSI_WI_modifiziere_Verlegedaten(key_intern, daten_intern);
    }
    else
    {
      interne_datenbasis_Verlegedaten.eintrag_container(
        key_intern, daten_intern);
    }

    INFO_LOG(m_logger) << "Inserted new minelayerthrower data into old interne_datenbasis_Verlegedaten succesfully! Minelayerthrower data:" << data;
  }
}

Core::GeoPosition2D HandlerMinelayerData::geographicFromRelative(const gps_position_rt &position) const
{
  // this function is called after m_baseCoord is set within relativeFromGeographic()
  // since we do a mgrs->geographic conversion only when a mine area is created
  const QPointF point(m_baseCoord.geographic(position.utm_ostwert - m_baseCoord.x, position.utm_nordwert - m_baseCoord.y));

  Core::GeoPosition2D ret;
  ret.lon(point.x());
  ret.lat(point.y());
  return ret;
}

gps_position_rt HandlerMinelayerData::relativeFromGeographic(double lon, double lat)
{
  const LtsCoordTrans::UtmCoordinate coord(LtsCoordTrans::UtmCoordinate::fromGeographic(lon, lat, m_baseCoord.zone));
  if (m_baseCoord.zone == -1)
  {
    const QString mgrs = coord.mgrs(0) + QStringLiteral("00000000000000");
    // our mgrs base point (taken from the first coordinate we get from Tetra)

    m_baseCoord = LtsCoordTrans::UtmCoordinate::fromMgrs(mgrs);
  }
  gps_position_rt ret;
  ret.utm_ostwert = qRound(coord.x);
  ret.utm_nordwert = qRound(coord.y);
  return ret;
}

int HandlerMinelayerData::partyIdFromEquipmentNumber(int eqNumber) const
{
  int partyId = 0;
  std::function<bool(const ParticipantManagement::Participant &data)> cb =
    [eqNumber, &partyId](const ParticipantManagement::Participant & participant) -> bool
  {
    if (participant.equipmentNumber() == eqNumber)
    {
      partyId = participant.partyId();
      return true;
    }
    return false;
  };
  m_ppmDtc->participants(cb);
  return partyId;
}

// mathmatisch positiv nordbezogen <- mathematisch negativ, ost = 0°, 0,01degree in 1/100° -> Faktor 100
static inline double compassFromMathAngleForSfm(int mathAngle)
{
  double ret = (9000.0 - mathAngle) / 100;
  while (ret < 0.0)
    ret += 360.0;
  while (ret > 360.0)
    ret -= 360.0;
  return ret;
}

void HandlerMinelayerData::onSendMinefield(const wirkfeld_minensperre_rt &minefield, int fzArt)
{
#if BUILD_RTI
  AreaEffectSimulation::MineArea outData;
  outData.participantId(minefield.header.identitaet);
  outData.partyId(partyIdFromEquipmentNumber(minefield.header.identitaet)); // Get from PPM;
  outData.name(QString::fromUtf8(tr_lts("dds", "_%1")).arg(minefield.beschreibung.kennung).toStdString());
  outData.effectStart(minefield.beschreibung.wirkbeginn.gps_zeit * 1000LL); // derived from Tetra telegram
  outData.effectDuration(minefield.beschreibung.wirkdauer * (1000 * 60 * 60)); // orig resolution is 1h based
  outData.density(minefield.beschreibung.verlegedichte / 100.0); // derived from Tetra telegram
  outData.chargeStruct().chargeTypeId(minefield.beschreibung.minentyp); // is already in TAB converted to chargeTypeId
  outData.layingProcedureType(static_cast<AreaEffectSimulation::AesMaLayingProcedureType::inner_enum>(fzArt));
  outData.width(minefield.beschreibung.verlegetiefe);
  outData.isLaidHidden(false);
  outData.layingProcedureType(AreaEffectSimulation::AesMaLayingProcedureType::LptPioneer);
  outData.isCenterLine(true);

  // segments
  std::vector<AreaEffectSimulation::MineSegment> segVec;
  segVec.reserve(minefield.beschreibung.segmente.size());
  for (const auto &seg : minefield.beschreibung.segmente)
  {
    AreaEffectSimulation::MineSegment segment;
    segment.positionStart(geographicFromRelative(seg.startpunkt));
    const LtsCoordTrans::UtmCoordinate startPositionUtm = LtsCoordTrans::UtmCoordinate::fromGeographic({segment.positionStart().lon(), segment.positionStart().lat()});

    QLineF line;
    line.setP1({startPositionUtm.x, startPositionUtm.y});
    line.setAngle(seg.drehwinkel);
    line.setLength(double(seg.anzahl / outData.density()));

    LtsCoordTrans::UtmCoordinate endPositionUtm = startPositionUtm;
    endPositionUtm.x = line.p2().x();
    endPositionUtm.y = line.p2().y();

    QPointF endPos = endPositionUtm.geographic();
    segment.positionEnd({endPos.x(), endPos.y()});
    segVec.emplace_back(segment);
  }
  outData.segments(segVec);

  INFO_LOG(m_logger) << "Sending new minefield from generator to AES: " << outData;

  // request create mine area from aes
  m_aesFtc->mineAreaModifyFromCtaDataRequest(outData, this);
#endif
}

void HandlerMinelayerData::onStartMinefieldInterpreter()
{
  INFO_LOG(m_logger) << "Starting minefield interpreter ...";
  m_minefieldInterpreter->processData();
}

void HandlerMinelayerData::onMineAreaModifyFromCtaDataReply(const Core::FunctionReplyObject &replyObject)
{
  if (replyObject.error())
    ERROR_LOG(m_logger) << "Error inserting minefield via AES->FTC";
  else
    INFO_LOG(m_logger) << "Got onMineAreaCreateFromCtaDataReply reply with id " << replyObject.id();
}

} // namespace GuezMinelayerProcessing

#else // BUILD_RTI

namespace GuezMinelayerProcessing
{

HandlerMinelayerData::HandlerMinelayerData(const std::shared_ptr<DdsHandler> &ddsHandler,
                                           QObject *parent)
  : QObject(parent)
  , m_logger(getLogger("GuezMinelayerProcessing.HandlerMinelayerData"))
  , m_ddsHandler(ddsHandler)
{}

HandlerMinelayerData::~HandlerMinelayerData()
{}

} // namespace GuezMinelayerProcessing

#endif // BUILD_RTI