#pragma once

#include <sstream>
#include <cmath>

#include <LtsUtilsCore/ltsutilscore_export.h>

#include <QtCore/qglobal.h>
#include <QtCore/qpoint.h>  


namespace LtsUtils
{

class QPoint3D
{
public:
  enum class Axis { X, Y, Z };

  Q_DECL_CONSTEXPR QPoint3D() = default;
  Q_DECL_CONSTEXPR QPoint3D(double xpos, double ypos, double zpos);
  template<typename T>
  explicit QPoint3D(const T &data)
    : xp(data.offsetX())
    , yp(data.offsetY())
    , zp(data.offsetZ())
  {}

  inline bool isNull() const;

  Q_DECL_CONSTEXPR inline double x() const;
  Q_DECL_CONSTEXPR inline double y() const;
  Q_DECL_CONSTEXPR inline double z() const;
  Q_DECL_RELAXED_CONSTEXPR inline void setX(double xpos);
  Q_DECL_RELAXED_CONSTEXPR inline void setY(double ypos);
  Q_DECL_RELAXED_CONSTEXPR inline void setZ(double zpos);

  Q_DECL_RELAXED_CONSTEXPR inline double &rx();
  Q_DECL_RELAXED_CONSTEXPR inline double &ry();
  Q_DECL_RELAXED_CONSTEXPR inline double &rz();

  Q_DECL_CONSTEXPR inline QPointF toPointF() const;           
  static QPoint3D fromQPointF(const QPointF &p)               
  {
    return QPoint3D(p.x(), p.y(), .0);
  }

  inline double lengthSquared() const { return (xp * xp) + (yp * yp) + (zp * zp); }
  inline double length() const { return std::sqrt(lengthSquared()); }
  inline void normalize()
  {
    double len = xp * xp + yp * yp + zp * zp;
    if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
      return;

    len = std::sqrt(len);

    xp /= len;
    yp /= len;
    zp /= len;
  }
  inline QPoint3D normalized() const
  {
    double len = xp * xp + yp * yp + zp * zp;
    if (qFuzzyIsNull(len - 1.0f))
      return *this;
    else if (!qFuzzyIsNull(len))
      return *this / std::sqrt(len);
    else
      return QPoint3D(*this);
  }

  inline void rotate(const double &phi, const Axis &axis)
  {
    const double sin = std::sin(phi);
    const double cos = std::cos(phi);
    switch (axis)
    {
    case Axis::X:
    {
      const double _yp = (cos * yp) - (sin * zp);
      const double _zp = (sin * yp) + (cos * zp);
      yp = _yp;
      zp = _zp;
      break;
    }
    case Axis::Y:
    {
      const double _xp = (cos * xp) + (sin * zp);
      const double _zp = (-sin * xp) + (cos * zp);
      xp = _xp;
      zp = _zp;
      break;
    }
    case Axis::Z:
    {
      const double _xp = (cos * xp) - (sin * yp);
      const double _yp = (sin * xp) + (cos * yp);
      xp = _xp;
      yp = _yp;
      break;
    }
    }
  }
  inline QPoint3D rotated(const double &phi, const Axis &axis) const
  {
    QPoint3D p(*this);
    p.rotate(phi, axis);
    return p;
  }

  inline double distanceToLine(const QPoint3D &point, const QPoint3D &direction) const;
  inline double distanceToPlane(const QPoint3D &plane, const QPoint3D &normal) const;
  inline double distanceToPlane(const QPoint3D &plane1, const QPoint3D &plane2, const QPoint3D &plane3) const;
  inline double distanceToPoint(const QPoint3D &point) const;

  friend Q_DECL_CONSTEXPR inline bool operator==(const QPoint3D &p1, const QPoint3D &p2);
  friend Q_DECL_CONSTEXPR inline bool operator!=(const QPoint3D &p1, const QPoint3D &p2);
  friend Q_DECL_CONSTEXPR inline  QPoint3D operator +(const QPoint3D &p1, const QPoint3D &p2)
  {
    return QPoint3D(p1.xp + p2.xp, p1.yp + p2.yp, p1.zp + p2.zp);
  }

  friend Q_DECL_CONSTEXPR inline  QPoint3D operator -(const QPoint3D &p1, const QPoint3D &p2)
  {
    return QPoint3D(p1.xp - p2.xp, p1.yp - p2.yp, p1.zp - p2.zp);
  }

  friend Q_DECL_CONSTEXPR inline  QPoint3D operator *(const QPoint3D &p1, double factor)
  {
    return QPoint3D(p1.xp * factor, p1.yp * factor, p1.zp * factor);
  }
  friend Q_DECL_CONSTEXPR inline  QPoint3D operator *(double factor, const QPoint3D &p1)
  {
    return QPoint3D(p1.xp * factor, p1.yp * factor, p1.zp * factor);
  }

  friend Q_DECL_CONSTEXPR inline  QPoint3D operator /(const QPoint3D &p1, double factor)
  {
    return QPoint3D(p1.xp / factor, p1.yp / factor, p1.zp / factor);
  }

  Q_DECL_RELAXED_CONSTEXPR inline QPoint3D &operator +=(const QPoint3D &o)
  {
    xp += o.xp;
    yp += o.yp;
    zp += o.zp;
    return *this;
  }
  Q_DECL_RELAXED_CONSTEXPR inline QPoint3D &operator -=(const QPoint3D &o)
  {
    xp -= o.xp;
    yp -= o.yp;
    zp -= o.zp;
    return *this;
  }
  Q_DECL_RELAXED_CONSTEXPR inline QPoint3D &operator *=(double factor)
  {
    xp *= factor;
    yp *= factor;
    zp *= factor;
    return *this;
  }
  Q_DECL_RELAXED_CONSTEXPR inline QPoint3D &operator /=(double divisor)
  {
    xp /= divisor;
    yp /= divisor;
    zp /= divisor;
    return *this;
  }

  Q_DECL_CONSTEXPR static inline double dotProduct(const QPoint3D& v1, const QPoint3D& v2)
  {
    return v1.xp * v2.xp + v1.yp * v2.yp + v1.zp * v2.zp;
  }

  Q_DECL_CONSTEXPR static inline QPoint3D crossProduct(const QPoint3D& v1, const QPoint3D& v2)
  {
    return QPoint3D(v1.yp * v2.zp - v1.zp * v2.yp,
                    v1.zp * v2.xp - v1.xp * v2.zp,
                    v1.xp * v2.yp - v1.yp * v2.xp);
  }

  static inline QPoint3D normal(const QPoint3D &v1, const QPoint3D &v2)
  {
    return crossProduct(v1, v2).normalized();
  }
  static inline QPoint3D normal(const QPoint3D &v1, const QPoint3D &v2, const QPoint3D &v3)
  {
    return crossProduct((v2 - v1), (v3 - v1)).normalized();
  }

  Q_DECL_CONSTEXPR friend inline bool qFuzzyCompare(const QPoint3D& v1, const QPoint3D& v2);

private:
  double xp = 0;
  double yp = 0;
  double zp = 0;
};



LTSUTILSCORE_EXPORT QDataStream &operator<<(QDataStream &s, const QPoint3D &p);
LTSUTILSCORE_EXPORT QDataStream &operator>>(QDataStream &s, QPoint3D &p);




Q_DECL_CONSTEXPR inline QPoint3D::QPoint3D(double xpos, double ypos, double zpos) : xp(xpos), yp(ypos), zp(zpos) { }

inline bool QPoint3D::isNull() const
{
  return qIsNull(xp) && qIsNull(yp) && qIsNull(zp);
}

Q_DECL_CONSTEXPR inline double QPoint3D::x() const
{
  return xp;
}

Q_DECL_CONSTEXPR inline double QPoint3D::y() const
{
  return yp;
}

Q_DECL_CONSTEXPR inline double QPoint3D::z() const
{
  return zp;
}

Q_DECL_RELAXED_CONSTEXPR inline void QPoint3D::setX(double xpos)
{
  xp = xpos;
}

Q_DECL_RELAXED_CONSTEXPR inline void QPoint3D::setY(double ypos)
{
  yp = ypos;
}

Q_DECL_RELAXED_CONSTEXPR inline void QPoint3D::setZ(double zpos)
{
  zp = zpos;
}

Q_DECL_RELAXED_CONSTEXPR inline double &QPoint3D::rx()
{
  return xp;
}

Q_DECL_RELAXED_CONSTEXPR inline double &QPoint3D::ry()
{
  return yp;
}

Q_DECL_RELAXED_CONSTEXPR inline double &QPoint3D::rz()
{
  return zp;
}


Q_DECL_CONSTEXPR inline QPointF QPoint3D::toPointF() const
{
  return QPointF(xp, yp);
}


Q_DECL_CONSTEXPR inline bool operator==(const QPoint3D &p1, const QPoint3D &p2)
{
  return p1.xp == p2.xp && p1.yp == p2.yp && p1.zp == p2.zp;
}

Q_DECL_CONSTEXPR inline bool operator!=(const QPoint3D &p1, const QPoint3D &p2)
{
  return p1.xp != p2.xp || p1.yp != p2.yp || p1.zp != p2.zp;
}

Q_DECL_CONSTEXPR inline bool qFuzzyCompare(const QPoint3D& v1, const QPoint3D& v2)
{
    return ::qFuzzyCompare(v1.xp, v2.xp) &&
           ::qFuzzyCompare(v1.yp, v2.yp) &&
           ::qFuzzyCompare(v1.zp, v2.zp);
}

inline std::ostream&  operator<< (std::ostream& stream, const QPoint3D& pos)
{
  stream << "(" << pos.x() << "," << pos.y() << "," << pos.z() << ")";
  return stream;
}

} 

Q_DECLARE_TYPEINFO(LtsUtils::QPoint3D, Q_MOVABLE_TYPE);