/**
  * @RDE_HEADER@
  * LTS  - logging facility
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Norbert Thoden, SLTE, 2023-11-08
  */

#include "trajectory_generator.h"
#include "trajectory_generator_vulcano.h"
#include <Plugins/ExerciseManagement/exm_data_plugin_interface.h>
#include <Plugins/AssetManagement/asm_data_plugin_interface.h>


#include <LtsCoordTrans/lts_coord_trans.h>
#include <LtsUtilsCore/qpoint3d.h>

#ifdef NEVER // for csv file
#  include <QFile>
#  include <QDateTime>
#  include <QTextStream>
#endif // NEVER

#include <cmath>


namespace AreaEffectSimulation
{

static TrajectoryGenerator *s_trajectoryGenerator = nullptr;


TrajectoryGenerator::TrajectoryGenerator()
  : m_logger(getLogger(QStringLiteral("TrajectoryGenerator")))
{
}

TrajectoryGenerator *TrajectoryGenerator::getInstance(int exerciseId,
                                                      const std::shared_ptr<ExerciseManagement::Interface::Dtc> &exmDtc,
                                                      const std::shared_ptr<AssetManagement::Interface::Dtc> &asmDtc)
{
  // Be aware: ONE TrajectoryGenerator per process, not per exercise
  // That feature has to be added!
  // In that case, have a look at LtsElevationManager::createInstance() and createInstanceForTrainingAreaId()

  if (!s_trajectoryGenerator)
  {
    LOGGER_T logger = getLogger(QStringLiteral("TrajectoryGenerator"));

    s_trajectoryGenerator = new TrajectoryGenerator();
    qAddPostRoutine(&TrajectoryGenerator::cleanup);

    // assign LtsElevationManager:
    LtsElevation::LTS_ELEVATION_MANAGER_TYPE ltsElevationManager = LtsElevation::LtsElevationManager::createInstance(exerciseId, exmDtc, asmDtc);
#ifndef LTS_DEBUG
    Q_ASSERT_X(ltsElevationManager, "LtsElevationManager", "createInstance failed");
#endif // LTS_DEBUG

    if (ltsElevationManager)
    {
      ltsElevationManager->waitForInitialization();
      DEBUG_LOG(logger) << LOGFUNCTION << ", ltsElevationManager->waitForInitialization() ... DONE, isValid = " << ltsElevationManager->isValid();
    }
    s_trajectoryGenerator->initElevation(ltsElevationManager);
  }
  return s_trajectoryGenerator;
}

TrajectoryGenerator *TrajectoryGenerator::getInstance()
{
  if (!s_trajectoryGenerator)
  {
    s_trajectoryGenerator = new TrajectoryGenerator();
    qAddPostRoutine(&TrajectoryGenerator::cleanup);
  }
  return s_trajectoryGenerator;
}

void TrajectoryGenerator::cleanup()
{
  delete s_trajectoryGenerator;
}

void TrajectoryGenerator::initElevation(LtsElevation::LtsElevationManager *ltsElevationManager)
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START";
  m_ltsElevationManager = ltsElevationManager;
  TRACE_LOG(m_logger) << LOGFUNCTION << " END";
}

LtsElevation::LtsElevationManager *TrajectoryGenerator::getElevationManager()
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START";
  TRACE_LOG(m_logger) << LOGFUNCTION << " END";
  return m_ltsElevationManager;
}

bool TrajectoryGenerator::hasElevationManager() const
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START";
  TRACE_LOG(m_logger) << LOGFUNCTION << " END";
  return m_ltsElevationManager;
}



// Ideal ballistic curve, but with time values:
bool TrajectoryGenerator::generateTrajectoryBiedziak(const LtsUtils::QPoint3D &originatorPosition_geo,
                                                     const LtsUtils::QPoint3D &idealHit_geo,
                                                     double elevation_angle_degree,
                                                     TrajectoryGeoPosition &trajectoryOut)
{
  return generateTrajectoryBiedziak<std::vector<Core::GeoPosition>>(originatorPosition_geo,
                                                                    idealHit_geo,
                                                                    elevation_angle_degree,
                                                                    trajectoryOut);
}

//
// Vulcano
//
bool TrajectoryGenerator::generateTrajectoryVulcano(const QPoint3D &originatorPosition_geo, const QPoint3D &idealHit_geo,
                                                    double v0, double betaRad,
                                                    double alphaRad, double tapTimeSec,
                                                    TrajectoryGeneratorVulcano::Options options,
                                                    TrajectoryGeoPositionRelTime &trajectoryTimeAwareOut,
                                                    Core::GeoPosition &idealTapAbsoluteGeoOut,
                                                    QString *reasonOut)
{
  LOGGER_T logger = getLogger(QStringLiteral("TrajectoryGenerator"));

  // use zone of idealHit:
  INFO_LOG(logger) << " orgPos geo " << originatorPosition_geo;
  INFO_LOG(logger) << " iHPos  geo " << idealHit_geo;

  const LtsCoordTrans::UtmCoordinate &idealHitUtm = LtsCoordTrans::UtmCoordinate::fromGeographic(idealHit_geo.toPointF());
  const LtsCoordTrans::UtmCoordinate &originUtm   = LtsCoordTrans::UtmCoordinate::fromGeographic(originatorPosition_geo.toPointF(), idealHitUtm.zone);
  INFO_LOG(logger) << " originUtm   " << originUtm.mgrs();
  INFO_LOG(logger) << " idealHitUtm " << idealHitUtm.mgrs();


  TRAJECTORY_METRIC trajectoryMetricOut;

  const QPoint3D origin_metric  (originUtm.x, originUtm.y, originatorPosition_geo.z());
  const QPoint3D idealHit_metric(idealHitUtm.x, idealHitUtm.y, idealHit_geo.z());

  double vGlidingOut = 1;
  double vFallingOut = 1;
  TrajectoryGeneratorVulcano::getSpeedPerPhase(v0, betaRad, vGlidingOut, vFallingOut);

  QString localReasonOut;

  QPoint3D idealTapAbsoluteOut;

  const bool success =
      TrajectoryGeneratorVulcano::trajectoryVulcano(origin_metric, idealHit_metric,
                                                    v0, betaRad,
                                                    vGlidingOut,
                                                    vFallingOut, alphaRad, tapTimeSec,
                                                    options,
                                                    trajectoryMetricOut,
                                                    idealTapAbsoluteOut,
                                                    &localReasonOut);

  if (!localReasonOut.isEmpty())
    WARNING_LOG(logger) << " trajectoryVulcano failed, reasonOut = '" << localReasonOut << "'";

  //
  // for documentation, it may be helpful to write the trajectoryMetric to a csv file
  //

#ifdef NEVER
  {
    static QString filePath(QStringLiteral("/tmp/trajectory_dump.csv"));
    QFile data(filePath);
    QTextStream output(&data);

    if (data.open(QFile::WriteOnly | QIODevice::Append |QFile::Truncate))
    {
      output.setRealNumberPrecision(3);
      output.setRealNumberNotation(QTextStream::RealNumberNotation::FixedNotation);

      output << "TrajectoryVulcano at: " << QDateTime::currentDateTime().toString(QStringLiteral("yyyy.MM.dd hh:mm::ss")) << "\n";
      output << "v0: " << v0  << "\n";
      output << "betaDeg: " << betaRad*LtsCoordTrans::rad2deg()  << "\n";
      output << "vGlidingOut: " << vGlidingOut  << "\n";
      output << "vFallingOut: " << vFallingOut  << "\n";
      output << "alphaDeg: " << alphaRad*LtsCoordTrans::rad2deg()  << "\n";
      output << "tapTimeSec: " << tapTimeSec  << "\n";

      const double distance = (origin_metric - idealHit_metric).length();
      output << "distance: " << distance  << "\n";


      output << "x_rel ; y_rel ; z ; relativeTime" << "\n";

      if (!trajectoryMetricOut.isEmpty())
      {
        QPoint4D startPoint = trajectoryMetricOut.first();

        for (const QPoint4D &p4D : qAsConst(trajectoryMetricOut))
        {
          output << p4D.x() - startPoint.x()  << ";"
                 << p4D.y() - startPoint.y() << ";"
                 << p4D.z() << ";"
                 << p4D.time() << ";"
                 << "\n";
        } // for

      }

    }
  }
#endif // NEVER
  //
  //
  //




  // convert metric to geo/wgs84
  {
    LtsCoordTrans::UtmCoordinate tapUtm(idealHitUtm.zone, idealTapAbsoluteOut.x(), idealTapAbsoluteOut.y());
    const QPointF &tapGeo = tapUtm.geographic();
    idealTapAbsoluteGeoOut = Core::GeoPosition(tapGeo.x(), tapGeo.y(), idealTapAbsoluteOut.z());
  }


  trajectoryTimeAwareOut.clear();
  trajectoryTimeAwareOut.reserve(trajectoryMetricOut.size());

  for (const QPoint4D& pnt4d : qAsConst(trajectoryMetricOut))
  {
    LtsCoordTrans::UtmCoordinate pntUtm(idealHitUtm.zone, pnt4d.x(), pnt4d.y());
    const QPointF &pntGeo = pntUtm.geographic();

    // !!! relativeTime is in msec!!!
    trajectoryTimeAwareOut.emplace_back( Core::GeoPositionRelativeTime(pntGeo.x(), pntGeo.y(), pnt4d.z(), pnt4d.time()*1000.0) );
  }


  if (reasonOut)
    *reasonOut = localReasonOut;

  return success;
}


//
// real implementation
//


inline void addItemTo(std::vector<Core::GeoPosition> &cont,
                      double x, double y, double z)
{
  cont.emplace_back(Core::GeoPosition(x, y, z));
}


template<typename CONT_T>
bool TrajectoryGenerator::generateTrajectoryBiedziak(const LtsUtils::QPoint3D &originatorPosition_geo,
                                                     const LtsUtils::QPoint3D &impactPosition_geo,
                                                     double elevation_angle_degree,
                                                     CONT_T &trajectoryOut)
{
  // use zone of impact:
  INFO_LOG(m_logger) << " oPosition geo " << originatorPosition_geo.x() << " - " << originatorPosition_geo.y();
  INFO_LOG(m_logger) << " iPosition geo " << impactPosition_geo.x() << " - "     << impactPosition_geo.y();

  const LtsCoordTrans::UtmCoordinate &iPosition = LtsCoordTrans::UtmCoordinate::fromGeographic(impactPosition_geo.toPointF());
  const LtsCoordTrans::UtmCoordinate &oPosition = LtsCoordTrans::UtmCoordinate::fromGeographic(originatorPosition_geo.toPointF(), iPosition.zone);

  INFO_LOG(m_logger) << " oPosition " << oPosition.mgrs();
  INFO_LOG(m_logger) << " iPosition " << iPosition.mgrs();

  const double distance = sqrt(pow(iPosition.x - oPosition.x, 2.) + pow(iPosition.y - oPosition.y, 2.));

  double oHeight = originatorPosition_geo.z();
  double iHeight = impactPosition_geo.z();

  if (std::isnan(oHeight) && m_ltsElevationManager)
  {
    oHeight = m_ltsElevationManager ? m_ltsElevationManager->elevation(originatorPosition_geo.x(), originatorPosition_geo.y()) : 0;
    if (oHeight < 300)
      oHeight = 100;
    INFO_LOG(m_logger) << " oPosition " << oPosition << " has no zValue provided (NaN), update via ltsElevationManager...";
  }
  if (std::isnan(iHeight) && m_ltsElevationManager)
  {
    iHeight = m_ltsElevationManager ? m_ltsElevationManager->elevation(impactPosition_geo.x(), impactPosition_geo.y()) : 0;
    if (iHeight < 300)
      iHeight = 100;
    INFO_LOG(m_logger) << " iPosition " << iPosition << " has no zValue provided (NaN), update via ltsElevationManager.. ";
  }

  INFO_LOG(m_logger) << " oPosition " << oPosition << " ; oHeight=" << oHeight;
  INFO_LOG(m_logger) << " iPosition " << iPosition << " ; iHeight=" << iHeight;

  // z-Values:
  const QVector<double> &zValues = this->trajectoryBiedziak(distance, iHeight - oHeight, elevation_angle_degree);

  if (zValues.isEmpty())
    return false;

  trajectoryOut.clear();


  const QPointF &oPositionGeo = oPosition.geographic();

  addItemTo(trajectoryOut,
            oPositionGeo.x(), oPositionGeo.y(), oHeight);

  const QPointF &step = (QPointF(iPosition.x, iPosition.y) - QPointF(oPosition.x, oPosition.y)) / (zValues.size() - 1);
  QPointF runner;
  for (int i = 1; i < zValues.size(); ++i)
  {
    runner += step;
    const QPointF &p2D = oPosition.geographic(runner);
    addItemTo(trajectoryOut,
              p2D.x(), p2D.y(), oHeight + zValues[i]); // x, y, z
  } // for

////  DEBUG_LOG(m_logger) << string4poly(geo);

  return true;
}


QVector<double> TrajectoryGenerator::trajectoryBiedziak(const double &distance,
                                                        const double &heightOffset,
                                                        double elevation_angle_degree,
                                                        int steps)
{
  const double coefLinear = tan(elevation_angle_degree * LtsCoordTrans::deg2rad());
  const double coefSquare = (heightOffset - distance * coefLinear) / pow(distance, 2.0);
  const double delta = distance / steps;

  double x = 0.;
  QVector<double> yValues; // later interpreted as z-Values
  yValues.reserve(steps);
  yValues << 0.0;
  for (int i = 0; i < steps; ++i)
  {
    x += delta;
    yValues << (coefLinear * x + coefSquare * pow(x, 2.));
  } // for
  return yValues;
}

const QString TrajectoryGenerator::string4poly(const QPair<QPolygonF, QVector<double> > &pair)
{
  if (pair.first.isEmpty())
    return (QStringLiteral("Empty Polygon!"));
  int zone = LtsCoordTrans::UtmCoordinate::fromGeographic(pair.first[0]).zone;
  QStringList pointSl;
  pointSl.reserve(pair.first.size());
  for (int i = 0; i < pair.first.size(); ++i)
    pointSl << QString(QStringLiteral("utm=%1 h=%2")).arg(LtsCoordTrans::UtmCoordinate::fromGeographic(pair.first[i], zone).mgrs(), QString::number(pair.second[i], 'f', 1));
  return QStringLiteral("%1 Values (%2)").arg(pair.first.count()).arg(pointSl.join(QStringLiteral(" # ")));
}



//const QString TrajectoryGenerator::string4poly(const TrajectoryPoint3D &trajectory)
//{
//  if (trajectory.isEmpty())
//    return (QStringLiteral("Empty Polygon!"));

//  const Point3D &p3D = trajectory.first();
//  int zone = LtsCoordTrans::UtmCoordinate::fromGeographic(p3D.toPointF()).zone;
//  QStringList pointSl;
//  pointSl.reserve(trajectory.size());
//  for (int i = 0; i < trajectory.size(); ++i)
//    pointSl << QString(QStringLiteral("utm=%1 h=%2")).arg(LtsCoordTrans::UtmCoordinate::fromGeographic(p3D.toPointF(), zone).mgrs(), QString::number(p3D.z(), 'f', 1));
//  return QStringLiteral("%1 Values (%2)").arg(trajectory.count()).arg(pointSl.join(QStringLiteral(" # ")));
//}



} // namespace AreaEffectSimulation