/**
  * @RDE_HEADER@
  * Helper functions related to CoordinateTranslation
  * $Id$
  * $HeadURL$
  * $Author$
  */

// Own
#include "simple_helper.h"
#include "math_const.h"
#include "clipper.hpp"
#include <LtsCoordTrans/lts_coord_trans.h>
// Qt
#include <QList>
#include <QTransform>

namespace SituationMap
{
namespace MathUtils
{


QPointF rotate(const QPointF &p, const QPointF &center, qreal sinA, qreal cosA)
{
  qreal x = p.x() - center.x();
  qreal y = p.y() - center.y();
  return QPointF(x * cosA - y * sinA + center.x(), y * cosA + x * sinA + center.y());
}


QPointF rotate(const QPointF &p, const QPointF &center, qreal phi) { return rotate(p, center, sin(phi), cos(phi)); }


qreal getMathAngle(qreal sinPhi, qreal cosPhi)
{
  qreal phi = acos(cosPhi);
  if (sinPhi < 0) phi = MathConst::pi_two - phi;
  return phi;
}


qreal getMathAngle(const QPointF &oVec) { return getMathAngle(oVec.y(), oVec.x()); }


double getAngleDegreeNorth(const QPointF &p1, const QPointF &p2)
{
  double x = p2.x() - p1.x();
  double y = p2.y() - p1.y();

  // Winkel der Geraden bestimmen
  double alphaRad = atan2(y, x);

//  // Fuer Winkel > PI liefert atan2  math. rechts drehende (also negative) Werte.
//  // Um Werte von 0-2PI zu bekommen muss deshalb hier von 2Pi abgezogen werden.

  alphaRad -= M_PI / 2.0;
  alphaRad *= -1.0;

  if (alphaRad < 0.0)
    alphaRad = 2 * M_PI + alphaRad;

  return (alphaRad * 180.0 / M_PI);
}

QPointF getPointOnLine(const QPointF &p1, const QPointF &p2, double position_0_to_1)
{
  QPointF pRet;

  pRet.rx() = p1.x() + (p2.x() - p1.x()) * position_0_to_1;
  pRet.ry() = p1.y() + (p2.y() - p1.y()) * position_0_to_1;

  return pRet;
}

Point3D getPointOnLine(const Point3D &p1, const Point3D &p2, double position_0_to_1)
{
  Point3D pRet;

  pRet.rx() = p1.x() + (p2.x() - p1.x()) * position_0_to_1;
  pRet.ry() = p1.y() + (p2.y() - p1.y()) * position_0_to_1;
  pRet.rz() = p1.z() + (p2.z() - p1.z()) * position_0_to_1;

  return pRet;
}

void rotatePoint(QPointF &pToBeRotated, const QPointF &center, double angleDegree)
{
  // angle to rad
  double radAngle = ((M_PI) / -180.0) * angleDegree;

  // Translate
  QPointF t = pToBeRotated - center;

  // Rotate
  QPointF tmp;
  tmp.rx() = t.x() * cos(radAngle) - t.y() * sin(radAngle);
  tmp.ry() = t.x() * sin(radAngle) + t.y() * cos(radAngle);

  // Translate back and write result
  pToBeRotated = tmp + center;
}



QList<QPointF> mineFieldCenterLineToOutline(const QList<QPointF> &centerLine,
                                            double width)
{
  QList<QPointF> outline;

  // Clipper is using ints (typedef signed long long cInt;)
  // The coordinate of centerline will have values much lower the the maximum.
  // Therefore the factor can be 1000. So the resolution is bit enough (->Millimeter):
  const int    factorInt    = 10;
  const double factorDouble = 10.0;

  ClipperLib::ClipperOffset                        pathOffset;
  ClipperLib::Path                                 borderPath;      // vector< int >
  ClipperLib::Paths                                offsetPathVec;   // vector< vector< int > >

  for (auto point : centerLine)
  {
    borderPath << ClipperLib::IntPoint(static_cast<int>(point.x()*factorInt),
                                       static_cast<int>(point.y()*factorInt));
  }

  pathOffset.AddPath(borderPath, ClipperLib::JoinType::jtSquare, ClipperLib::EndType::etOpenButt);
  pathOffset.Execute(offsetPathVec, static_cast<int>(fabs(width)*factorInt));

  for (auto path : offsetPathVec)
  {
    ClipperLib::Path::const_iterator it = path.begin();

    for (; it != path.end(); ++it)
    {
      outline.append(QPointF(static_cast<double>(it->X) / factorDouble,
                             static_cast<double>(it->Y) / factorDouble)
                    );

    }
  }

  return outline;
}

qreal getAngleDegree2North(qreal angleDeg)
{
  double rotationAngle = angleDeg < 360.  ? angleDeg : std::fmod(angleDeg, 360.);
  // set rotation to 0° North(12h) counter clock wise
  rotationAngle = rotationAngle < 90. ? std::fabs(90 - rotationAngle) : std::fabs(360 - rotationAngle + 90) ;
  return rotationAngle;
}

qreal getAngleDegree2Math(qreal angleDeg)
{
  // set rotation to 0° math(3h) clockwise
  const double rotationMath = angleDeg < 90. ? std::fabs(angleDeg - 90) : std::fabs(360 - angleDeg + 90) ;
  return rotationMath;
}

QPolygonF getCircleSectorPolygon(const QPointF &centerPoint, qreal radius, double rotationAngleDeg, double centralAngleDeg)
{
  //       | <-rotationAngle
  //   | <StartPoint
  //
  //   \   |   /
  //    \     /
  //     \ | /
  //      \ /

  rotationAngleDeg = 90 - rotationAngleDeg;

  // rotate point with ~5 degree steps:
  int steps = centralAngleDeg/5.0;
  if (steps<3)  // at least 3 steps
    steps=3;
  const double singleIncrementDegree = centralAngleDeg/steps;

  QPolygonF poly;
  poly.reserve(steps+1);

  // CenterPunkt erstellen und einfügen;
  const LtsCoordTrans::UtmCoordinate centerUtm = LtsCoordTrans::UtmCoordinate::fromGeographic(centerPoint);
  poly << centerUtm.geographic(QPointF(0,0));

  // Startwinkel:
  const double angle = rotationAngleDeg - (centralAngleDeg/2.0); // start angle ->.\ | /
  const double angleRad = angle * LtsCoordTrans::deg2rad();

  // relPosition des StartPoint (point on circle)
  const QPointF position(cos(angleRad) * radius, sin(angleRad) * radius);

  // StartPoint einfügen
  poly << centerUtm.geographic(position);

  // calculate all steps, inclusive EndPos
  for (int step = 1; step<=steps; ++step)
  {
    double curAngle = angle + (step*singleIncrementDegree);
    double curAngleRad = curAngle * LtsCoordTrans::deg2rad();
    poly << centerUtm.geographic( QPointF(cos(curAngleRad) * radius, sin(curAngleRad) * radius) );
  } // for
  return poly;
}

} // namespace SituationMap::MathUtils
} // namespace SituationMap