/***************************************************************************
 *   Copyright (C) 2012 by Nicolas PASCAL                                  *
 *   nicolas.pascal@icare.univ-lille1.fr                                   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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 General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

/***************************************************************************
 *  Descrition :                                                           *
 *  implements geometry objects, coordinates conversions and common        *
 *  operations                                                             *
 *                                                                         *
 *  History :                                                              *
 *  2012/02/01 : start                                                     *
 ***************************************************************************/

#include "geometry.h"

Carthesian::Point3D::Point3D (double x, double y, double z) {
    set (x, y, z);
}

Carthesian::Point3D::Point3D (const Carthesian::Point3D &p) {
    set (p.x, p.y, p.z);
}

Carthesian::Point3D& Carthesian::Point3D::operator = (const Carthesian::Point3D &p) {
    if (this != &p)
        set (p.x, p.y, p.z);
    return *this;
}

const Carthesian::Vector3D Carthesian::Vector3D::operator * (const double & val) {
    Carthesian::Vector3D result = *this;
    result.set (val * x, val * y, val * z);
    return result;
}

const Carthesian::Vector3D Carthesian::Vector3D::operator / (const double & val) {
    Carthesian::Vector3D result = *this;
    result.set (x / val, y / val, z / val);
    return result;
}

void Carthesian::Point3D::set(double x, double y, double z) {
    this->x = x;
    this->y = y;
    this->z = z;
}

Carthesian::Segment3D::Segment3D(const Point3D & p1, const Point3D & p2) {
    set (p1, p2);
}

Carthesian::Segment3D::Segment3D(const Carthesian::Segment3D & s) {
    set (s.p1, s.p2);
}

// Carthesian::Segment3D::Segment3D(const Geocentric::Point3D & p1, const Geocentric::Point3D & p2) {
//     set (p1.to_carthesian(), p2.to_carthesian());
// }

Carthesian::Segment3D::Segment3D(const Geodetic::Point3D & p1, const Geodetic::Point3D & p2) {
    set (p1.to_carthesian(), p2.to_carthesian());
}

Carthesian::Segment3D & Carthesian::Segment3D::operator =(const Carthesian::Segment3D & s) {
    if (this != &s)
        set (s.p1, s.p2);
    return *this;
}
void Carthesian::Segment3D::set (const Carthesian::Point3D & p1, const Carthesian::Point3D & p2) {
    this->p1 = p1;
    this->p2 = p2;
}

Spherical::Point3D Carthesian::Point3D::to_spherical (void) const {
    double r     = sqrt(this->x * this->x + this->y * this->y + this->z * this->z);
    double theta   = acos(this->z / r);
    double phi = atan2(this->y, this->x);
    return Spherical::Point3D(r, theta, phi);
};

Spherical::Point3D::Point3D(double r, double theta, double phi) {
    set (r, theta, phi);
}

Spherical::Point3D::Point3D(const Spherical::Point3D & p) {
    set (p.r, p.theta, p.phi);
}

Spherical::Point3D & Spherical::Point3D::operator =(const Spherical::Point3D & p) {
    if (this != &p)
        set (p.r, p.theta, p.phi);
    return *this;
}

void Spherical::Point3D::set(double r, double theta, double phi) {
    this->r     = r;
    this->theta = theta;
    this->phi   = phi;
}

Carthesian::Point3D Spherical::Point3D::to_carthesian(void ) const {
    double v = r * sin (theta); // for speed
    double x = v * cos(phi);
    double y = v * sin(phi);
    double z = r * cos(theta);
    return Carthesian::Point3D(x, y, z);
}

Geocentric::Point3D::Point3D(double lat, double lon, double alt) {
    set (lat, lon, alt);
}

Geocentric::Point3D::Point3D(const Geocentric::Point3D & p) {
    set (p.lat, p.lon, p.alt);
}

Geocentric::Point3D & Geocentric::Point3D::operator =(const Geocentric::Point3D & p) {
    if (this != &p)
        set (p.lat, p.lon, p.alt);
    return *this;
}

void Geocentric::Point3D::set(double lat, double lon, double alt) {
    this->lat = lat;
    this->lon = lon;
    this->alt = alt;
}

Spherical::Point3D Geocentric::Point3D::to_spherical(void ) const {
    double r     = Earth::get_radius() + alt;
    double theta = radians (90. - lat);
    double phi   = radians (lon);
    return Spherical::Point3D(r, theta, phi);
}

Carthesian::Point3D Geocentric::Point3D::to_carthesian() const {
    Spherical::Point3D p = to_spherical();
    return p.to_carthesian ();
}

Geocentric::Point3D Spherical::Point3D::to_geocentric(void ) const {
    double lat = 90. - degrees (theta);
    lat = format_lat (lat);
    double lon = degrees (phi);
    lon = format_lon (lon);
    double alt = r - Earth::get_radius();
    return Geocentric::Point3D(lat, lon, alt);
}

Geocentric::Point3D Carthesian::Point3D::to_geocentric(void ) const {
    Spherical::Point3D p = to_spherical();
    return p.to_geocentric();
}

bool Carthesian::intersection(const Carthesian::Point3D & p1, const Carthesian::Point3D & p2,
                              const Carthesian::Point3D & p3, const Carthesian::Point3D & p4,
                                    Carthesian::Point3D & pa, Carthesian::Point3D & pb,
                                    double & mua, double & mub) {
   Carthesian::Point3D p13, p43, p21;
   double d1343, d4321, d1321, d4343, d2121;
   double numer, denom;

   p13.x = p1.x - p3.x;
   p13.y = p1.y - p3.y;
   p13.z = p1.z - p3.z;
   p43.x = p4.x - p3.x;
   p43.y = p4.y - p3.y;
   p43.z = p4.z - p3.z;
   if (fabs(p43.x) < DBL_EPSILON && fabs(p43.y) < DBL_EPSILON && fabs(p43.z) < DBL_EPSILON)
      return(false);

   p21.x = p2.x - p1.x;
   p21.y = p2.y - p1.y;
   p21.z = p2.z - p1.z;
   if (fabs(p21.x) < DBL_EPSILON && fabs(p21.y) < DBL_EPSILON && fabs(p21.z) < DBL_EPSILON)
      return(false);

   d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
   d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
   d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
   d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
   d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;

   denom = d2121 * d4343 - d4321 * d4321;
   if (fabs(denom) < DBL_EPSILON)
      return(false);
   numer = d1343 * d4321 - d1321 * d4343;

   mua = numer / denom;
   mub = (d1343 + d4321 * (mua)) / d4343;

   pa.x = p1.x + mua * p21.x;
   pa.y = p1.y + mua * p21.y;
   pa.z = p1.z + mua * p21.z;
   pb.x = p3.x + mub * p43.x;
   pb.y = p3.y + mub * p43.y;
   pb.z = p3.z + mub * p43.z;

   return(true);
}

Carthesian::Vector3D Carthesian::Segment3D::to_vector() const {
    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;
    double dz = p2.z - p1.z;
    return Carthesian::Vector3D (dx, dy, dz);
}

Carthesian::Vector3D Carthesian::Segment3D::to_unit_vector() const {
    Carthesian::Vector3D v = to_vector();
    return v / v.norm();
}

Carthesian::Vector3D::Vector3D(double x, double y, double z) {
    set (x, y, z);
}

Carthesian::Vector3D::Vector3D(const Carthesian::Vector3D & v) {
    set (v.x, v.y, v.z);
}

Carthesian::Vector3D & Carthesian::Vector3D::operator =(const Carthesian::Vector3D & v) {
    if (this != &v)
        set (v.x, v.y, v.z);
    return *this;
}
void Carthesian::Vector3D::set (double x, double y, double z) {
    this->x = x;
    this->y = y;
    this->z = z;
}

double Carthesian::Vector3D::norm() {
    return sqrt ((x * x) + (y * y) + (z * z));
}

bool Carthesian::intersection(const Carthesian::Segment3D & p1p2, const Carthesian::Segment3D & p3p4, Carthesian::Segment3D & papb, double & mua, double & mub) {
    return intersection (p1p2.p1, p1p2.p2, p3p4.p1, p3p4.p2, papb.p1, papb.p2, mua, mub);
}

double Carthesian::angle(const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2) {
    double dotprod = dotproduct(v1, v2);
    Carthesian::Vector3D cross (crossproduct(v1, v2));
    double angle = atan2 (cross.norm(), dotprod);
    return angle;
}

double Carthesian::dotproduct(const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2) {
    return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
}

void Carthesian::crossproduct(const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2, Carthesian::Vector3D & vout) {
    double x = v1.y * v2.z - v1.z * v2.y;
    double y = v1.z * v2.x - v1.x * v2.z;
    double z = v1.x * v2.y - v1.y * v2.x;
    vout.set (x, y, z);
}

Carthesian::Vector3D Carthesian::crossproduct(const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2) {
    Carthesian::Vector3D vout;
    crossproduct (v1, v2, vout);
    return vout;
}

Carthesian::Vector3D::Vector3D(const Carthesian::Point3D & p1, const Carthesian::Point3D & p2) {
    set (p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
}

const double Geocentric::get_orthodormic_angle(double lat1, double lon1, double lat2, double lon2) {
        // degrees to radians conversion
        lat1 = DEG2RAD * lat1;
        lon1 = DEG2RAD * lon1;
        lat2 = DEG2RAD * lat2;
        lon2 = DEG2RAD * lon2;

        double dlon = lon2 - lon1;
        double dlat = lat2 - lat1;
        double a = pow ( sin ( dlat *.5 ), 2. )+
                cos ( lat1 ) * cos( lat2 ) *    pow ( sin ( dlon * .5 ), 2. );
        // take care of asin validity domain
        if ( a > 1. ) a = 1.;
    //     printf ( "a coeff between (%f,%f) and (%f,%f) is %f rad\n", lat1, lon1, lat2, lon2, a );
        return 2. * asin ( sqrt ( a ) ) ;
}

const double Geocentric::get_square_plane_dist(const double dlat, const double dlon) {
    double dlon_mod = fmod (dlon, 360.);
    return sqrt ((dlat * dlat) + (dlon_mod * dlon_mod));
}

const double Geocentric::get_square_plane_dist(const double lat1, const double lon1, const double lat2, const double lon2) {
    double dlat = lat1 - lat2;
    double dlon = lon1 - lon2;
    return Geocentric::get_square_plane_dist (dlat, dlon);
}

const double Geocentric::get_haversine_dist(double lat1, double lon1, double lat2, double lon2) {
    // angle in radians between the 2 points
    double a = Geocentric::get_orthodormic_angle (lat1, lon1, lat2, lon2);
    return Earth::get_radius() * a;
}

Carthesian::Matrix3D::Matrix3D(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz) {
    set (xx, xy, xz, yx, yy, yz, zx, zy, zz);
}

Carthesian::Matrix3D::Matrix3D(const Matrix3D & m) {
    set (m.xx, m.xy, m.xz, m.yx, m.yy, m.yz, m.zx, m.zy, m.zz);
}

Carthesian::Matrix3D & Carthesian::Matrix3D::operator =(const Carthesian::Matrix3D & m) {
    if (this != &m)
        set (m.xx, m.xy, m.xz, m.yx, m.yy, m.yz, m.zx, m.zy, m.zz);
    return *this;
}

void Carthesian::dotproduct(const Carthesian::Matrix3D & m, const Carthesian::Vector3D & v, Carthesian::Vector3D & vout) {
    double vx = m.xx * v.x + m.xy * v.y + m.xz * v.z;
    double vy = m.yx * v.x + m.yy * v.y + m.yz * v.z;
    double vz = m.zx * v.x + m.zy * v.y + m.zz * v.z;
    vout.set(vx, vy, vz);
}

Carthesian::Vector3D Carthesian::dotproduct(const Carthesian::Matrix3D & m, const Carthesian::Vector3D & v) {
    Carthesian::Vector3D vout;
    Carthesian::dotproduct (m, v, vout);
    return vout;
}

void Carthesian::Matrix3D::set(double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz){
    this->xx = xx;
    this->xy = xy;
    this->xz = xz;
    this->yx = yx;
    this->yy = yy;
    this->yz = yz;
    this->zx = zx;
    this->zy = zy;
    this->zz = zz;
}

void Carthesian::rotation_matrix(const Carthesian::Vector3D & axis, const double theta, Carthesian::Matrix3D &matrix) {
    // precomputations for speed
    double ctheta = cos (theta);
    double stheta = sin (theta);
    double one_min_ctheta = 1.0 - ctheta;
    double x = axis.x;
    double y = axis.y;
    double z = axis.z;
    double xx = x * x;
    double xy = x * y;
    double xz = x * z;
    double yy = y * y;
    double yz = y * z;
    double zz = z * z;
    matrix.xx =       ctheta + xx * one_min_ctheta;
    matrix.xy = - z * stheta + xy * one_min_ctheta ;
    matrix.xz =   y * stheta + xz * one_min_ctheta;
    matrix.yx =   z * stheta + xy * one_min_ctheta;
    matrix.yy =       ctheta + yy * one_min_ctheta;
    matrix.yz = - x * stheta + yz * one_min_ctheta;
    matrix.zx = - y * stheta + xz * one_min_ctheta;
    matrix.zy =   x * stheta + yz * one_min_ctheta;
    matrix.zz =       ctheta + zz * one_min_ctheta;
}

Carthesian::Vector3D Carthesian::rotate(const Carthesian::Vector3D & v, const Carthesian::Vector3D & axis, const double theta) {
    Carthesian::Vector3D vout;
    rotate(v, axis, theta, vout);
    return vout;
}

void Carthesian::rotate(const Carthesian::Vector3D & v, const Carthesian::Vector3D & axis, const double theta, Carthesian::Vector3D & vout) {
    // build rotation matrix
    Carthesian::Matrix3D m_rot;
    rotation_matrix (axis, theta, m_rot);
    dotproduct(m_rot, v, vout);
}

void Carthesian::Point3D::translate(const Carthesian::Vector3D & v) {
    this->x += v.x;
    this->y += v.y;
    this->z += v.z;
}

const Carthesian::Point3D Carthesian::Point3D::operator +(const Carthesian::Vector3D & v) const {
    Carthesian::Point3D p (*this);
    p.translate (v);
    return p;
}

Geodetic::Point3D::Point3D(double lat, double lon, double alt) {
    set (lat, lon, alt);
}

Geodetic::Point3D::Point3D(const Geodetic::Point3D & p) {
    set (p.lat, p.lon, p.alt);
}

Geodetic::Point3D & Geodetic::Point3D::operator =(const Geodetic::Point3D & p) {
    if (this != &p)
        set (p.lat, p.lon, p.alt);
    return *this;
}

void Geodetic::Point3D::set(double lat, double lon, double alt) {
    this->lat = lat;
    this->lon = lon;
    this->alt = alt;
}

Spherical::Point3D Geodetic::Point3D::to_spherical(void ) const {
    double r     = Earth::WGS84::get_N(radians (lat)) + alt;
    double theta = radians (90. - lat);
    double phi   = radians (lon);
    return Spherical::Point3D(r, theta, phi);
}

Carthesian::Point3D Geodetic::Point3D::to_carthesian() const {
    // source : http://en.wikipedia.org/wiki/Geodetic_system#From_geodetic_to_ECEF
    // precomputation for speed
    double lat_rad = radians (lat);
    double slat = sin (lat_rad);
    double clat = cos (lat_rad);
    double lon_rad = radians (lon);
    double slon = sin (lon_rad);
    double clon = cos (lon_rad);

    double n = Earth::WGS84::get_N(lat_rad);
    double x = (n + alt) * clat * clon;
    double y = (n + alt) * clat * slon;
    double z = ((n * (1.0 - Earth::WGS84::e2)) + alt) * slat;
    Carthesian::Point3D  pout (x, y, z);
    return pout;
}

Geodetic::Point3D Carthesian::Point3D::to_geodetic(void ) const {
    // source : http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
    // GNU gama library : http://www.gnu.org/software/gama

    /* ****************************************************************
     * B. R. Bowring: Transformation from spatial to geographical     *
     * coordinates, Survey Review XXIII, 181, July 1976, pp. 323--327 *
     * ****************************************************************/

    Geodetic::Point3D pout;
    double t, tan_u, cos2_u, cos_u, sin2_u, sin_u, lat, lon, alt;
    lon = atan2(y, x);

    double _x = fabs(x);
    double _y = fabs(y);
    if (_x > _y) {
        t = _y/_x;
        _x = _x * sqrt(1 + t*t);
    } else if (_y) {
        t = _x/_y;
        _x = _y * sqrt(1 + t*t);
    } else {
        lon = 0;
        if (z > 0) {
            lat = M_PI/2;
            alt = z - Earth::WGS84::Ime2*Earth::WGS84::get_N(lat);
            }
        else
            {
            lat = -M_PI/2;
            alt = -z - Earth::WGS84::Ime2*Earth::WGS84::get_N(lat);
            }
        goto exit;
        }
    tan_u  = Earth::WGS84::ab * z / _x;
    cos2_u = 1/(1 + tan_u*tan_u);
    cos_u  = sqrt(cos2_u);
    sin2_u = 1 - cos2_u;
    sin_u  = sqrt(sin2_u);
    if (z < 0) sin_u = -sin_u;

    lat = atan2(z + Earth::WGS84::e22*Earth::WGS84::b*sin2_u*sin_u, _x - Earth::WGS84::e2*Earth::WGS84::a*cos2_u*cos_u);
    /* next iteration is never needed in earth bound region; max error
    * is 0.0018" for H=2a
    */
    if (_x > fabs(z))
        alt = _x/cos(lat) - Earth::WGS84::get_N(lat);
    else
        alt = z/sin(lat) - Earth::WGS84::Ime2*Earth::WGS84::get_N(lat);
exit :
    // to degrees
    lat = degrees (lat);
    lon = degrees (lon);
    pout.set (lat, lon, alt);
    return pout;
}