#include "saa_interface.h"
#include "qpoint.h"
#include <LtsCoordTrans/lts_coord_trans.h>
#include <TrainingAreaBase/Plugins/AdlerJson/telegrams.h>
#include <TrainingAreaBase/training_area_telegrams.h>

#include <QTransform>

namespace AdlerJson
{
QString statusToStr(ScenarioStatus status)
{
  switch (status)
  {
    case ScenarioStatus::INACTIVE:
      return QStringLiteral("Inaktiv");
    case ScenarioStatus::PLANNED:
      return QStringLiteral("Geplant");
    case ScenarioStatus::ACTIVE:
      return QStringLiteral("Aktiv");
    case ScenarioStatus::STOPPED:
      return QStringLiteral("Gestoppt");
    default:
      return QStringLiteral("");
  }
}
ScenarioStatus strToStatus(QString status)
{
  if (status == QStringLiteral("Inaktiv"))
  {
    return ScenarioStatus::INACTIVE;
  }
  else if (status == QStringLiteral("Geplant"))
  {
    return ScenarioStatus::PLANNED;
  }
  else if (status == QStringLiteral("Aktiv"))
  {
    return ScenarioStatus::ACTIVE;
  }
  else if (status == QStringLiteral("Gestoppt"))
  {
    return ScenarioStatus::STOPPED;
  }
  else
  {
    return ScenarioStatus::NONE;
  }
}

SAAInterface::SAAInterface(int wsId, int partyId, const QString &saaName, const QString &szenarName, bool isVerbund, QObject *parent)
    : QObject(parent)
    , m_wsId(wsId)
    , m_partyId(partyId)
    , m_saaName(saaName)
    , m_szenarName(szenarName)
    , m_status(Szenarstatus::Inaktiv)
    , m_isVerbund(isVerbund)
    , m_logger(getLogger(QStringLiteral("SAAInterface.") + QString::number(partyId)))
{
}

bool SAAInterface::init(std::shared_ptr<PpmHandler> ppmHandler,
                        std::shared_ptr<PdmHandler> pdmHandler,
                        std::shared_ptr<AesHandler> aesHandler,
                        std::shared_ptr<TrainingAreaBase::Interface::Dts> dtsTab)
{
  m_ppmHandler = ppmHandler;
  m_pdmHandler = pdmHandler;
  m_aesHandler = aesHandler;
  m_dtsTab = dtsTab;

  m_ftcAes = m_aesHandler->getFtc();

  DEBUG_LOG(m_logger) << "INIT SAA Interface";

  return true;
}

void SAAInterface::setStatus(Szenarstatus status)
{
  m_status = status;
  if (m_status == Szenarstatus::Gestoppt)
  {
    // Remove all Particpants
    m_ppmHandler->removeAllSAAParticipants(m_partyId);
    // Remove all pending effect cluster
    m_aesHandler->removeAllPendingEffectCluster(m_partyId);
  }
}


static Core::RelativePositionRelativeTime convertFlugbahnpunkt(const Flugbahnpunkt &fbPos,
                                                               const QTransform &transformMatrix,
                                                               const LtsCoordTrans::UtmCoordinate &utmLaunch,
                                                               LOGGER_T logger)
{
  // vergl. GUEZSZ50_ICD-0036_0.1.0_Zentrale-SAA.pdf
  // fbPos.x -> y ??
  // fbPos.z -> x ??
  // fbPos.y -> z ??

  const QPointF fbPosOrdered = QPointF(0, fbPos.xRelativ);

  const auto rotatePoint = transformMatrix.map(fbPosOrdered);

  if (logger.isDebugEnabled())
  {
    // see also: coordtrans_test::testRelativeMetricalOffet()
    const LtsCoordTrans::UtmCoordinate utmCoord(utmLaunch.zone,
                                                utmLaunch.x + rotatePoint.x(),
                                                utmLaunch.y + rotatePoint.y());

    DEBUG_LOG(logger) << "-----Flugbahnpunkt fbPosOrdered:" << fbPosOrdered << " -> rotatePoint=" << rotatePoint;
    DEBUG_LOG(logger) << " -> utmCoord:" << utmCoord << "; mgrs: " << utmCoord.mgrs();
  }

  return Core::RelativePositionRelativeTime(rotatePoint.x(), rotatePoint.y(), fbPos.yRelativ,
                                            std::round(fbPos.zeit * 1000.0));
}

// Kann nur mit aktiver SAA-Verbindung angefragt werden, deshalb verbleiben die Flugbahndaten hier
void SAAInterface::parseFlugbahndaten(const Flugbahndaten &flugbahndaten, int aesItemId, int exposionId, const Core::GeoPosition &positionLaunch)
{
  DEBUG_LOG(m_logger) << "SAAInterface::parseFlugbahndaten";

  std::vector<Core::RelativePositionRelativeTime> geometryRef;
  geometryRef.clear();
  geometryRef.reserve(flugbahndaten.flugbahnpunkt.size());

  AreaEffectSimulation::TrajectoryAdlerModifyRequest *newRequest = new AreaEffectSimulation::TrajectoryAdlerModifyRequest();
  newRequest->aesItemId(aesItemId);
  newRequest->explosionIndex(exposionId);

  int zone = -1;

  const auto &utmLaunch = LtsCoordTrans::UtmCoordinate::fromGeographic(positionLaunch.lon(), positionLaunch.lat(), zone);

  // Rotiert im Uhrzeigersinn um den Nullpunkt (0,0)
  DEBUG_LOG(m_logger) << " mmilirad: " << flugbahndaten.schussrichtung;
  double grad = (flugbahndaten.schussrichtung / 6400.) * 360.0;
  DEBUG_LOG(m_logger) << " milli rad to degree" << (flugbahndaten.schussrichtung / 6400.) * 360;
  DEBUG_LOG(m_logger) << "Degree to target" << grad;

  DEBUG_LOG(m_logger) << "Start point: " << positionLaunch;

  const auto &coordEnd = LtsCoordTrans::UtmCoordinate::fromGeographic(flugbahndaten.zielkoordinate.koordinate.longitude * (M_PI / 180.0),
                                                                      flugbahndaten.zielkoordinate.koordinate.latitude * (M_PI / 180.0));
  DEBUG_LOG(m_logger) << "End point: " << coordEnd;
  LtsCoordTrans::UtmCoordinate coordDiff = coordEnd;

  coordDiff.x = coordDiff.x - utmLaunch.x;
  coordDiff.y = coordDiff.y - utmLaunch.y;
  const double useNewGrad = -90 + std::atan2(coordDiff.y, coordDiff.x) * (180.0 / M_PI);
  DEBUG_LOG(m_logger) << "use new Degree " << useNewGrad;

  QTransform matrix;
  matrix.rotate(useNewGrad);


  for (Flugbahnpunkt pos : qAsConst(flugbahndaten.flugbahnpunkt))
  {
    //
    //
    // an easier solution should be this:
    //
    //
    if (m_logger.isDebugEnabled())
    {
      const auto &fbPosRelative = convertFlugbahnpunkt(pos, matrix, utmLaunch, m_logger);
      DEBUG_LOG(m_logger) << " fbPosRelative " << fbPosRelative;
    }


    TRACE_LOG(m_logger) << "Flugbahnpunkt:"
                         << ", xRelativ :" << pos.xRelativ
                         << ", yRelativ :" << pos.yRelativ
                         << ", zRelativ :" << pos.zRelativ
                         << ", zeit :" << pos.zeit
                         << ", positionLaunch :" << positionLaunch.lon()
                         << " / " << positionLaunch.lat()
                         << " / " << positionLaunch.z()
                         << "<< utmLaunch :" << utmLaunch.x
                         << " / " << utmLaunch.y
                         << " / " << utmLaunch.zone;

    QPointF utm = QPointF(pos.xRelativ, 0);//pos.zRelativ);
    QPointF utmLaunchPt(utmLaunch.x, utmLaunch.y);

    utm += utmLaunchPt;
    double z = positionLaunch.z() + pos.yRelativ;

    // see also: coordtrans_test::testRelativeMetricalOffet()
    LtsCoordTrans::UtmCoordinate utmCoord;
    utmCoord.x = utm.x();
    utmCoord.y = utm.y();
    utmCoord.zone = utmLaunch.zone;
    QPointF pntGeo = utmCoord.geographic();

    TRACE_LOG(m_logger) << "utmCoord :" << utmCoord.x << " / " << utmCoord.y << ", utmCoord.zone :" << utmCoord.zone << ", utmCoord.isValid() :" << utmCoord.isValid()
                        << ", pntGeo :" << pntGeo.x() << " / " << pntGeo.y() << " / " << z;
    QTransform matrix;
    matrix.rotate(useNewGrad);

    Core::RelativePositionRelativeTime flightPoint;
    flightPoint.offsetX() = 0;// pos.zRelativ;
    flightPoint.offsetY() = pos.xRelativ;
    flightPoint.offsetZ() = pos.yRelativ;
    flightPoint.relativeTime() = pos.zeit;
    QPointF point(flightPoint.offsetX(), flightPoint.offsetY());

    TRACE_LOG(m_logger) << " POS bevore rot: " << point;

    auto rotatePoint = matrix.map(point);
    TRACE_LOG(m_logger) << " POS after rot: " << rotatePoint;

    pntGeo.setX(rotatePoint.x());
    pntGeo.setY(rotatePoint.y());

    TRACE_LOG(m_logger) << "utmCoord :" << utmCoord.x << " / " << utmCoord.y << ", utmCoord.zone :" << utmCoord.zone << ", utmCoord.isValid() :" << utmCoord.isValid()
                        << ", pntGeo :" << pntGeo.x() << " / " << pntGeo.y() << " / " << z;
    // geometryRef.emplace_back(Core::GeoPositionRelativeTime(pntGeo.x(), pntGeo.y(), z, pos.zeit * 1000));
    // We Send the Trajectory as IS as the Effector will use them now.
    // Then we would apply the
    // Core::GeoPositionRelativeTime flightPoint;
    flightPoint.offsetX() = rotatePoint.x();
    flightPoint.offsetY() = rotatePoint.y();
    flightPoint.offsetZ() = pos.yRelativ;
    flightPoint.relativeTime() = std::round(pos.zeit * 1000.0);

    geometryRef.emplace_back(flightPoint);
  }

  Core::GeoPosition impactPosition;
  impactPosition.lat(flugbahndaten.zielkoordinate.koordinate.latitude * M_PI / 180.0);
  impactPosition.lon(flugbahndaten.zielkoordinate.koordinate.longitude * M_PI / 180.0);
  impactPosition.z(flugbahndaten.zielkoordinate.hoehe);
  newRequest->impactPosition(impactPosition);
  newRequest->geometry(geometryRef);
  newRequest->peakAltitude(flugbahndaten.gipfelhoehe);
  newRequest->mhpScatterLenght(flugbahndaten.streuung.laengsstreuung);
  newRequest->mhpScatterLengthDetonationHeight(flugbahndaten.streuung.laengsstreuungH);
  newRequest->mhpScatterWidth(flugbahndaten.streuung.seitenstreuung);
  newRequest->mhpScatterWidthDetonationHeight(flugbahndaten.streuung.seitenstreuungH);
  newRequest->shootDirection((flugbahndaten.schussrichtung / 6400.) * 360);
  newRequest->errorcode(static_cast<int>(flugbahndaten.fehlercode));
  DEBUG_LOG(m_logger) << "!!!!!!!!!!!geometryRef.size=" << geometryRef.size() << ", newRequest->geometry().size() :" << newRequest->geometry().size()
                      << ", newRequest->errorcode :" << newRequest->errorcode() << ", newRequest->Peak:" << newRequest->peakAltitude()
                      << ", newRequest->direction:" << newRequest->shootDirection() << ", flugbahndaten.fehlercode :" << static_cast<int>(flugbahndaten.fehlercode);

  m_ftcAes->trajectoryAdlerModifyRequest(*newRequest, this);
}

void SAAInterface::onTrajectoryAdlerModifyReply(const Core::FunctionReplyObject &reply)
{
  DEBUG_LOG(m_logger) << "onTrajectoryAdlerModifyReply";
  if (reply.error())
  {
    ERROR_LOG(m_logger) << reply.translated();
  }
}

} // namespace AdlerJson