/**
  * @RDE_HEADER@
  * Trajectory Helper
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Norbert Thoden, SLTE, 2023-11-08
  */

#include "trajectory_helper.h"
#include "../logger.h"

#include <LtsUtilsCore/qpoint3d.h>

#include <QObject>
#include <QString>
#include <QDebug>

#include <iostream>
#include <cmath>


namespace AreaEffectSimulation
{

double TrajectoryHelper::flightDurationSec(const LtsUtils::QPoint3D &posStart, const LtsUtils::QPoint3D &posEnd, double peakHeightAbs,
                                           double *betaRadiantOut, double *vOut,
                                           QString *reasonOut)
{
  if ((posStart.z() > peakHeightAbs) || (posEnd.z() > peakHeightAbs))
  {
    if (reasonOut)
    {
      //: "Höhenfehler"
      *reasonOut = QObject::tr("HeightElevationError(%1 %2 %3)").arg(posStart.z()).arg(peakHeightAbs).arg(posEnd.z());
    }
    return -1.0;
  }


  double z = posEnd.z() - posStart.z();
  double h = peakHeightAbs - posStart.z();


  double x = posEnd.x() - posStart.x();
  double y = posEnd.y() - posStart.y();
  double r = sqrt(x * x + y * y);
  if (r < 0.01)
  {
    if (reasonOut)
    {
      //: "StartEndeAbstandsfehler"
      *reasonOut = QObject::tr("StartEndDistanceError(%1)").arg(r);
    }
    return 0.0; // No distance -> no duration -> 0.0
  }

  double beta; // Abschusswinkel / elevation_angle in radiant
  double v;

  if (fabs(z) < 0.01)
  {
    beta = atan(4.0 * h / r);
    v = sqrt(9.81 * r / sin(2.0 * beta));
  }
  else
  {
    double b;
    double p = - 4.0 * h / r;
    double q = 4.0 * h * z / (r * r);     // ehemals double q = - 4.0 * h * z / (r * r);
    if (p * p / 4.0 > q)
      b = - (p / 2.0) + sqrt(p * p / 4.0 - q);
    else
      return -1.0;
    beta = atan(b);
    double a = b * b / (4.0 * h);
    double rh = b / a;
    v = sqrt(9.81 * rh / sin(2.0 * beta));
  }

  // handle optional values:
  if (betaRadiantOut)
    *betaRadiantOut = beta;
  if (vOut)
    *vOut = v;

  const double result = r / (v * cos(beta));
  if ((result < 0) && reasonOut)
  {
    //: "FlugzeitFehlerUnbekannt"
    *reasonOut = QObject::tr("FlightDurationSecErrorUnknown(%1)").arg(result);
  }

  return result;
}

double TrajectoryHelper::peakHeightAbs(const LtsUtils::QPoint3D &posStart, const LtsUtils::QPoint3D &posEnd, double timeSec,
                                       double *betaRadiantOut, double *vOut,
                                       QString *reasonOut)
{
  double x = posEnd.x() - posStart.x();
  double y = posEnd.y() - posStart.y();
  double r = sqrt(x * x + y * y);
  if (r < 0.01)
  {
    if (reasonOut)
    {
      //: "StartEndeAbstandsfehler"
      *reasonOut = QObject::tr("StartEndDistanceError(%1)").arg(r);
    }
    return 0.0;
  }

  double v, rh;
  double t2 = timeSec * timeSec;
  double z = posEnd.z() - posStart.z();
  if (fabs(z) < 0.01)
  {
    v = sqrt((9.81 * 9.81 * t2 / 4.0) + (r * r / t2));
    rh = r;
  }
  else
  {
    double vx = r / timeSec;
    double vy = (9.81 * timeSec * timeSec / 2.0 + z) / timeSec;
    v = sqrt(vx * vx + vy * vy);
    rh = 2.0 * r * sqrt((v * v - r * r / t2)) / (9.81 * timeSec);
  }

  double arcSin = 9.81 * rh / (v * v);
  if (arcSin > 1.0)
  {
    if (reasonOut)
    {
      //: "45GradSchuss"
      *reasonOut = QObject::tr("45DegreeShot(%1)").arg(arcSin);
    }
    return -1;
  }

  // beta: Abschusswinkel / elevationsWinkel in radiant
  double beta;

  if (fabs(9.81 * rh - v * v) < 0.0001)
  {
    beta = M_PI_4;
  }
  else
  {
    if (9.81 * rh > (v * v))
    {
      if (reasonOut)
      {
        //: "GipfelhöhenFehlerUnbekannt"
        *reasonOut = QObject::tr("PeakHeightUnknownError(rh=%1,v=%2)").arg(rh).arg(v);
      }
      return -1;
    }
    beta = asin( 9.81 * rh / (v * v)) / 2.0;
  }

  // [GUEZE-2472] Inkonsitente Werte bei der Berechnung der Gipfelhöhe (peakHeight)
  // Es gibt nur eine Flugzeit zu einer maximalen Höhe h bei Vorgabe der Zielentfernung.
  //  if (beta < M_PI_2)
  //  {
  //    if (trajMode == TrajectoryMode::TrajectoryModeHigh)
  //      beta = M_PI_2 - beta;
  //  }
  //  else
  //  {
  //    if (trajMode == TrajectoryMode::TrajectoryModeLow)
  //      beta = M_PI_2 - beta;
  //  }

  double beta2;
  if (beta < M_PI_2)
    beta2 = M_PI_2 - beta;
  else
    beta2 = M_PI_2 + beta;

  if (fabs((timeSec * v) * cos(beta) - r) > fabs((timeSec * v) * cos(beta2) - r))
    beta = beta2;


  double h = v * v * sin(beta) * sin(beta) / (2.0 * 9.81);
  h += posStart.z();

  // handle optional values:
  if (betaRadiantOut)
    *betaRadiantOut = beta;
  if (vOut)
    *vOut = v;

  return h;
}

LtsUtils::QPoint3D TrajectoryHelper::realStartPoint(const LtsUtils::QPoint3D &apogaeum, const LtsUtils::QPoint3D &endPos)
{
  LtsUtils::QPoint3D realStart = apogaeum - (endPos-apogaeum);
  realStart.setZ(endPos.z());
  return realStart;
}

//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