/***************************************************************************
**
**  This file is part of QGpCoreMath.
**
**  This library is free software; you can redistribute it and/or
**  modify it under the terms of the GNU Lesser General Public
**  License as published by the Free Software Foundation; either
**  version 2.1 of the License, or (at your option) any later version.
**
**  This file is distributed in the hope that it will be useful, but WITHOUT
**  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
**  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
**  License for more details.
**
**  You should have received a copy of the GNU Lesser General Public
**  License along with this library; if not, write to the Free Software
**  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2002-04-15
**  Copyright: 2002-2019
**    Marc Wathelet
**    Marc Wathelet (ULg, Liège, Belgium)
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <math.h>

#include "Point2D.h"
#include "Angle.h"
#include "UtmZone.h"
#include "DoubleMatrix.h"

namespace QGpCoreMath {

  /*!
    \class Point2D Point2d.h

    Basic properties of a 2D point (coordinates in double precision)
  */

  /*!
    \fn Point2D::Point2D()
    Default constructor
  */

  /*!
    \fn Point2D::Point2D(double x, double y)
    Constructor setting x and y
  */

  /*!
    \fn Point2D::Point2D(const Point& p)
    Copy constructor from Point
  */

  /*!
    Copy constructor from a vector
  */
  Point2D::Point2D(const QVector<double>& p)
  {
    ASSERT(p.count()>1);
    _x=p.at(0);
    _y=p.at(1);
  }

  /*!
    \fn void Point2D::interpole(double valX, const Point2D& p1, const Point2D& p2)
    Interpole between two points
  */

  /*!
    \fn void Point2D::setInvalid()
    Does nothing,. Used by Curve.
  */

  /*!
    \fn bool Point2D::isValid() const
    Always return true. Used by Curve.
  */

  /*!
    Returns the point as a string with space separation between coordinates.
    \a precision is the number of significant digits. \a format is 'g' or 'f'.
  */
  QString Point2D::toString(int precision, char format) const
  {
    TRACE;
    QString tmp;
    tmp+=QString::number(_x, format, precision);
    tmp+=" ";
    tmp+=QString::number(_y, format, precision);
    return tmp;
  }

  /*!
    Extracts coordinates from string \a str.
  */
  bool Point2D::fromString(const StringSection& str)
  {
    TRACE;
    const QChar * ptr=nullptr;
    StringSection f;
    f=str.nextField(ptr);
    if(f.isValid()) _x=f.toDouble(); else return false;
    f=str.nextField(ptr);
    if(f.isValid()) _y=f.toDouble(); else return false;
    return true;
  }

  /*!
    Checks whether \a p is in the neighbourhood of this point (+/- \a tolX,+/- \a tolY).
  */
  bool Point2D::isHit(const Point2D &p, double tolX, double tolY) const
  {
    if((_x-tolX<=p.x() && p.x()<_x+tolX)
        && (_y-tolY<=p.y() && p.y()<_y+tolY))
      return true;
    else
      return false;
  }

  /*!
    \fn double length()
    Calculates length.
  */

  /*!
    Calculates distance to \a p.
  */
  double Point2D::distanceTo(const Point2D &p) const
  {
    double tmp, dist;

    dist=0.;
    tmp=_x-p.x();
    dist+=tmp*tmp;
    tmp=_y-p.y();
    dist+=tmp*tmp;
    return ::sqrt(dist);
  }

  /*!
    Calculates distance to \a p.
  */
  double Point2D::squareDistanceTo(const Point2D &p) const
  {
    double tmp, dist;

    dist=0.;
    tmp=_x-p.x();
    dist+=tmp*tmp;
    tmp=_y-p.y();
    dist+=tmp*tmp;
    return dist;
  }

  /*!
    Returns the azimuth from the origin to this point in radians in the mathematical sense (in [0, 2*PI[, 0 is East).
  */
  double Point2D::azimuth() const
  {
    double a=atan2(_y, _x);
    if(a<0.0) a+=2.0*M_PI;
    return a;
  }

  /*!
    Returns the azimuth from this point to \a p in radians in the mathematical sense (from 0 to 2 PI, 0 is East).
  */
  double Point2D::azimuthTo(const Point2D &p) const
  {
    double a=atan2(p.y()-_y, p.x()-_x);
    if(a<0.0) a+=2*M_PI;
    return a;
  }

  /*!
    Returns the azimuth from this point to \a p in radians from north, assuming that points have geographical coordinates.
    Returned values are between 0 and 2 PI.

    The azimuth is defined by the great circle according to Snyder, J. P. (1987). Map Projections - A Working Manual,
    USGS Professional Paper 1395.
  */
  double Point2D::geographicalAzimuthTo(const Point2D &p) const
  {
    double phi1=Angle::degreesToRadians(_y);
    double phi2=Angle::degreesToRadians(p._y);
    double cphi1=::cos(phi1);
    double cphi2=::cos(phi2);
    double sphi1=::sin(phi1);
    double sphi2=::sin(phi2);
    double deltaL=Angle::degreesToRadians(p._x-_x);
    double a=::atan2(cphi2*::sin(deltaL), cphi1*sphi2-sphi1*cphi2*::cos(deltaL));
    if(a<0.0) a+=2*M_PI;
    return a;
  }

  /*!
    Returns the distance from this point to \a p in meters, assuming that points have geographical coordinates.

    The distance is defined by the great circle according to Snyder, J. P. (1987). Map Projections - A Working Manual,
    USGS Professional Paper 1395. Same formula can be found at http://en.wikipedia.org/wiki/Haversine_formula.

    Radius of earth taken at equator from WSG84 (http://en.wikipedia.org/wiki/World_Geodetic_System).
  */
  double Point2D::geographicalDistanceTo(const Point2D &p) const
  {
    double phi1=Angle::degreesToRadians(_y);
    double phi2=Angle::degreesToRadians(p._y);
    double deltaL=Angle::degreesToRadians(p._x-_x);
    double sdeltaphi=::sin(0.5*(phi2-phi1));
    sdeltaphi*=sdeltaphi;
    double sdeltalamda=::sin(0.5*deltaL);
    sdeltalamda*=sdeltalamda;
    double sc=sdeltaphi+::cos(phi1)*::cos(phi2)*sdeltalamda;
    if(sc<0.0) sc=0.0;
    sc=::sqrt(sc);
    if(sc>1.0) sc=1.0;
    return 6378137.0*2.0*::asin(sc);
  }

  /*!
    Calculate the distance to a segment defined by \a p1 and \a p2.
  */
  double Point2D::distanceToSegment(const Point2D &p1, const Point2D &p2) const
  {
    // Find the intersection between segment and its perpendicular passing at this
    double dys=p2.y()-p1.y();
    if(dys==0.0) {
      if(p1.x()<p2.x()) {
        if(_x>p2.x()) return distanceTo(p2);
        if(_x<p1.x()) return distanceTo(p1);
      } else {
        if(_x>p1.x()) return distanceTo(p1);
        if(_x<p2.x()) return distanceTo(p2);
      }
      return fabs(_y-p1.y());
    }
    double dxs=p2.x()-p1.x();
    if(dxs==0.0) {
      if(p1.y()<p2.y()) {
        if(_y>p2.y()) return distanceTo(p2);
        if(_y<p1.y()) return distanceTo(p1);
      } else {
        if(_y>p1.y()) return distanceTo(p1);
        if(_y<p2.y()) return distanceTo(p2);
      }
      return fabs(_x-p1.x());
    }
    double coeff=dys/dxs;
    double coeffp=dxs/dys;
    double interx=(coeff*p1.x()-coeffp*_x+_y-p1.y())/(coeff-coeffp);
    if(p1.x()<p2.x()) {
      if(interx>p2.x()) return distanceTo(p2);
      if(interx<p1.x()) return distanceTo(p1);
    } else {
      if(interx>p1.x()) return distanceTo(p1);
      if(interx<p2.x()) return distanceTo(p2);
    }
    double intery=coeffp*(interx-_x)+_y;
    if(p1.y()<p2.y()) {
      if(intery>p2.y()) return distanceTo(p2);
      if(intery<p1.y()) return distanceTo(p1);
    } else {
      if(intery>p1.y()) return distanceTo(p1);
      if(intery<p2.y()) return distanceTo(p2);
    }
    return distanceTo(Point2D(interx,intery));
  }

  /*!
    Moves this point by \a distance in direction \a azimuth (radians, mathematical sense, 0 is east).
  */
  void Point2D::move(double distance, const Angle& azimuth)
  {
    _x+=distance*azimuth.cos();
    _y+=distance*azimuth.sin();
  }

  /*!
    Rotates point by \a angle degrees clockwize around Z axis.
  */
  void Point2D::rotate(const Angle& a)
  {
    double y=a.cos()*_y-a.sin()*_x;
    _x=a.sin()*_y+a.cos()*_x;
    _y=y;
  }

  /*!
    Round so that the difference between two distinct values is less
    than \a maximum .
  */
  void Point2D::round(double maximum)
  {
    _x=Number::round(_x, maximum);
    _y=Number::round(_y, maximum);
  }

  double Point2D::utmM(double phi)
  {
    double e=0.0818192;   // Eccentricity
    double a=6378137;     // Equatorial radius in m for reference ellipsoide
    double e2=e*e;
    double e4=e2*e2;
    double e6=e4*e2;
    return a*((1.0-e2*0.25-3.0/64.0*e4-5.0/256.0*e6)*phi
              -(3.0/8.0*e2+3.0/32.0*e4+45.0/1024.0*e6)*::sin(2.0*phi)
              +(15.0/256.0*e4+45.0/1024.0*e6)*::sin(4.0*phi)
              -35.0/3072*e6*::sin(6.0*phi));
  }

  /*!
    Converts from longitude, latitude to Universal Transverse Mercator (UTM). The implemented method
    delivers a cm precision. Results are in agreement with Google Earth coordinates with a 1-cm error.

    Equations from USGS Bulletin 1532 (p 63 to 69), by Snyder (1982).

    If \a zone is valid, the zone is forced to \a zone, else \zone is set to the standard UTM zone.
  */
  void Point2D::geographicalToUtm(UtmZone& zone)
  {
    TRACE;
    double phi=Angle::degreesToRadians(y());                          // Latitude (radians)
    double lamda=Angle::degreesToRadians(x());                        // Longitude (radians)
    // Find reference longitude according to UTM band
    if(!zone.isValid()) {
      zone=UtmZone(*this);
    }
    Point2D reference=zone.reference();
    double lamda0=Angle::degreesToRadians(reference.x());   // Reference longitude (radians)
    //double phi0=Angle::degreesToRadians(reference.y());

    double e=0.0818192;   // Eccentricity
    double a=6378137;     // Equatorial radius in m for reference ellipsoide

    double sphi=::sin(phi);
    double cphi=::cos(phi);
    double e2=e*e;
    double ep2=e2/(1.0-e2);
    double N=a/::sqrt(1.0-e2*sphi*sphi);
    double T=sphi/cphi;
    double T2=T*T;
    double T4=T2*T2;
    double C=ep2*cphi*cphi;
    double A=(lamda-lamda0)*cphi;
    double M=utmM(phi);
    //double M0=utmM(phi0);

    double N0;
    if(phi>0.0) {
      N0=0.0;         // Northern hemisphere in m
    } else {
      N0=10000000.0;  // Southern hemisphere in m
    }

    double k0=0.9996;        // Scale on central meridian, standard value for UTM
    double E0=500000.0;      // in m

    double A2=A*A;
    double A3=A2*A;
    double A4=A3*A;
    double A5=A4*A;
    double A6=A5*A;

    setX(E0+k0*N*(A+(1.0-T2+C)*A3/6.0+(5.0-18.0*T2+T4+72.0*C-58.0*ep2)*A5/120.0));
    // The point of origin of each UTM zone is the intersection of the equator and the zone's central meridian.
    // M-M0 is replaced by M in Snyder formula
    setY(N0+k0*(M+N*T*(A2*0.5+(5.0-T2+9.0*C+4.0*C*C)*A4/24.0+(61.0-58.0*T2+T4+600.0*C-330*ep2)*A6/720.0)));
  }

  /*!
    Convert UTM coordinates into geographical coordinates (WSG84)

    Equations from USGS Bulletin 1532 (p 63 to 69), by Snyder (1982).
  */
  void Point2D::utmToGeographical(const UtmZone& zone)
  {
    TRACE;
    Point2D reference=zone.reference();
    double lamda0=Angle::degreesToRadians(reference.x());   // Reference longitude (radians)
    double phi0=Angle::degreesToRadians(reference.y());     // Reference latitude (radians)
    //double M0=utmM(phi0);
    double N0;
    if(phi0>0.0) {
      N0=0.0;         // Northern hemisphere in m
    } else {
      N0=10000000.0;  // Southern hemisphere in m
    }
    double E0=500000.0;   // in m

    double e=0.0818192;   // Eccentricity
    double a=6378137;     // Equatorial radius in m for reference ellipsoide
    double k0=0.9996;     // Scale on central meridian, standard value for UTM

    double e2=e*e;
    double e4=e2*e2;
    double e6=e4*e2;
    double ep2=e2/(1.0-e2);

    // The point of origin of each UTM zone is the intersection of the equator and the zone's central meridian.
    // M-M0 is replaced by M in Snyder formula. Original formula: M=M0+(y()-N0)/k0
    double M=(y()-N0)/k0;
    double mu=M/(a*(1.0-e2*0.25-3.0/64.0*e4-5.0/256.0*e6));

    double e2m1=1.0-e2;
    double sqrte2=::sqrt(e2m1);
    double e1=(1.0-sqrte2)/(1.0+sqrte2);
    double e12=e1*e1;
    double e13=e12*e1;
    double e14=e13*e1;

    double phi1=mu+(3.0/2.0*e1-27.0/32.0*e13)*::sin(2.0*mu)+(21.0/16.0*e12-55.0/32.0*e14)*::sin(4.0*mu)+(151.0/96.0*e13)*::sin(6.0*mu)+
        (1097.0/512.0*e14)*::sin(8.0*mu);

    double sphi1=::sin(phi1);
    double cphi1=::cos(phi1);
    double C1=ep2*cphi1*cphi1;
    double C12=C1*C1;
    double T1=sphi1/cphi1;
    double T12=T1*T1;
    double T14=T12*T12;
    double esphim1=1.0-e2*sphi1*sphi1;
    double N1=a/::sqrt(esphim1);
    double R1=a*e2m1/::pow(esphim1, 1.5);
    double D=(x()-E0)/(N1*k0);
    double D2=D*D;
    double D3=D2*D;
    double D4=D2*D2;
    double D5=D4*D;
    double D6=D3*D3;

    setY(Angle::radiansToDegrees(phi1-(N1*T1/R1)*(D2*0.5-(5.0+3*T12+10*C1-4.0*C12-9.0*ep2)*D4/24.0+
                                 (61.0+90.0*T12+298.0*C1+45.0*T14+252.0*ep2-3.0*C12)*D6/720.0)));
    setX(Angle::radiansToDegrees(lamda0+(D-(1.0+2.0*T12+C1)*D3/6.0+(5.0-2.0*C1+28.0*T12-3.0*C12+8.0*ep2+24.0*T14)*D5/120.0)/cphi1));

  }

  #define E_RAD 6378.163
  #define E_FLAT  298.26
  #define DRAD  1.7453292e-2
  #define DRLT  9.9330647e-1

  /*!
    Converts point from geographical coordinates (long and lat, WGS84) to rectangular coordinates in meters,
    using reference point \a ref (in geographical coordinates). Longitude is along X axis, latitude is along Y axis.

    \a rotation is an angle to rotate north reference (clockwize, in degrees).

    Kindly provided by Matthias Ohrnberger.
  */
  void Point2D::geographicalToRectangular(const Point2D& ref, double rotation)
  {
    double olon;
    double olat;
    double lat_fac;  // conversion factor for latitude in km
    double lon_fac;  // conversion factor for longitude in km
    double snr;      // sin of rotation angle
    double csr;      // cos of rotation angle
    double dlt1;
    double dlt2;
    double del;
    double radius;
    double tmp;
    double tmp_x, tmp_y;

    /* convert everything to minutes */
    olon=60.0*ref.x();
    olat=60.0*ref.y();

    /* latitude */
    dlt1=::atan(DRLT * ::tan((double)olat * DRAD/60.0));
    dlt2=::atan(DRLT * ::tan(((double)olat +1.0) * DRAD/60.0));
    del =dlt2 - dlt1;
    radius=E_RAD * (1.0 - (::sin(dlt1)*::sin(dlt1)/E_FLAT));
    lat_fac=radius * del;

    /* longitude */
    del=acos(1.0 - (1.0 - ::cos(DRAD/60.0)) * ::cos(dlt1) * ::cos(dlt1));
    dlt2=radius * del;
    lon_fac=dlt2/::cos(dlt1);

    /* rotation */
    snr=::sin((double)rotation * DRAD);
    csr=::cos((double)rotation * DRAD);

    _y *= 60.0;
    _x *= 60.0;

    tmp_x=_x - olon;
    tmp_y=_y - olat;
    tmp  =::atan(DRLT * ::tan(DRAD * (_y+olat)/120.0));
    tmp_x=(double)tmp_x * lon_fac * ::cos(tmp);
    tmp_y=(double)tmp_y * lat_fac;

    _x=(csr*tmp_x - snr*tmp_y)*1000.0;
    _y=(csr*tmp_y + snr*tmp_x)*1000.0;
  }

  /*!
    Converts point from rectangular coordinates in meters to geographical coordinates (lat and long, WGS84),
    using reference point \a ref (in geographical coordinates). Longitude is along X axis, latitude is along Y axis.

    \a rotation is an angle to rotate north reference (clockwize in degrees).

    Kindly provided by Matthias Ohrnberger.
  */
  void Point2D::rectangularToGeographical(const Point2D& ref, double rotation)
  {
    double olon;
    double olat;
    double lat_fac;  // conversion factor for latitude in km
    double lon_fac;  // conversion factor for longitude in km
    double snr;      // sin of rotation angle
    double csr;      // cos of rotation angle
    double dlt1;
    double dlt2;
    double del;
    double radius;
    double tmp;
    double tmp_x, tmp_y;

    // Convert to km
    _x*=0.001;
    _y*=0.001;

    /* convert everything to minutes */
    olon=60.0 * ref.x();
    olat=60.0 * ref.y();

    /* latitude */
    dlt1=atan(DRLT * ::tan((double)olat * DRAD/60.0));
    dlt2=atan(DRLT * ::tan(((double)olat +1.0) * DRAD/60.0));
    del =dlt2 - dlt1;
    radius=E_RAD * (1.0 - (::sin(dlt1)*::sin(dlt1)/E_FLAT));
    lat_fac=radius * del;

    /* longitude */
    del=acos(1.0 - (1.0 - ::cos(DRAD/60.0)) * ::cos(dlt1) * ::cos(dlt1));
    dlt2=radius * del;
    lon_fac=dlt2/::cos(dlt1);

    /* rotation */
    snr=::sin((double)rotation * DRAD);
    csr=::cos((double)rotation * DRAD);
    tmp_x=snr* _y + csr* _x;
    tmp_y=csr* _y - snr* _x;

    tmp_y=tmp_y/lat_fac;
    tmp_y += olat;

    tmp=::atan(DRLT * ::tan(DRAD * (tmp_y+olat)/120.0));
    tmp_x=tmp_x/(lon_fac * ::cos(tmp));
    tmp_x += olon;

    _x=tmp_x/60.0;
    _y=tmp_y/60.0;
  }

  /*!
    Converts point from geographical coordinates in degrees to D.MMSSsss... (degrees, minutes, seconds).

    \sa DMSToDegrees(), Angle::degreeToDMS()
  */
  void Point2D::degreesToDMS()
  {
    _x=Angle::degreesToDMS(_x);
    _y=Angle::degreesToDMS(_y);
  }

  /*!
    Converts point from geographical coordinates in D.MMSSsss... (degrees, minutes, seconds) to degrees.

    \sa degreesToDMS(), Angle::DMSToDegrees()
  */
  void Point2D::DMSToDegrees()
  {
    _x=Angle::DMSToDegrees(_x);
    _y=Angle::DMSToDegrees(_y);
  }

  /*!
    Converts point from geographical coordinates in degrees to D.MMmm... (degrees, minutes).

    \sa DMToDegrees(), Angle::degreeToDM()
  */
  void Point2D::degreesToDM()
  {
    _x=Angle::degreesToDM(_x);
    _y=Angle::degreesToDM(_y);
  }

  /*!
    Converts point from geographical coordinates in D.MMmm... (degrees, minutes) to degrees.

    \sa degreesToDM(), Angle::DMToDegrees()
  */
  void Point2D::DMToDegrees()
  {
    _x=Angle::DMToDegrees(_x);
    _y=Angle::DMToDegrees(_y);
  }


  QTextStream& operator<<(QTextStream& s,const Point2D& p)
  {
    TRACE;
    s.setRealNumberNotation(QTextStream::SmartNotation);
    s.setRealNumberPrecision(20);
    s << p.x() << " " << p.y();
    return s;
  }

  QTextStream& operator>>(QTextStream& s,Point2D& p)
  {
    TRACE;
    double x, y;
    s >> x >> y;
    p.setX(x);
    p.setY(y);
    return s;
  }

  QDataStream& operator<<(QDataStream& s,const Point2D& p)
  {
    TRACE;
    s << p.x() << p.y();
    return s;
  }

  QDataStream& operator>>(QDataStream& s,Point2D& p)
  {
    TRACE;
    double x, y;
    s >> x >> y;
    p.setX(x);
    p.setY(y);
    return s;
  }

  QTextStream& operator<<(QTextStream& s, const QList<Point2D>& plist)
  {
    TRACE;
    s << "%x      y"<< Qt::endl;
    QList<Point2D>::const_iterator it;
    for(it=plist.begin();it!=plist.end();++it) s << *it << Qt::endl;
    return s;
  }

  QTextStream& operator>> (QTextStream& s, QList<Point2D>& plist)
  {
    TRACE;
    QString str;
    plist.clear();
    Point2D p;
    while(!s.atEnd()) {
      str=s.readLine();
      while(str[0]!='%') str=s.readLine();
      QTextStream currentLine(&str, QIODevice::ReadOnly);
      currentLine >> p;
      plist.append(p);
    }
    return s;
  }

  /*!
    Global Hash function to store Point2D into QHash

    Copied from uint qHash(const QPair<T1, T2> &key)
  */
  uint qHash(Point2D key)
  {
    uint h1=::qHash(quint64(key.x()));
    uint h2=::qHash(quint64(key.y()));
    return ((h1 << 16) | (h1 >> 16)) ^ h2;
  }

  void Point2D::operator*=(const Matrix2x2& transformation)
  {
    *this=transformation*(*this);
  }

  double Point2D::at(int index) const
  {
    switch(index) {
    case 1:
      return _y;
    default:
      return _x;
    }
  }

} // namespace QGpCoreMath
