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

#ifndef GEOMETRY_H
#define GEOMETRY_H

#include <cmath>
#include <cfloat>
#include <iostream>
#include <string>

#include "earth_geometry.h"

// forward declarations
namespace Spherical {
class Point3D;
}
namespace Geocentric {
class Point3D;
}
namespace Geodetic {
class Point3D;
}
namespace Carthesian {
class Point3D;
class Segment3D;
class Vector3D;
class Matrix3D;
}

/**
 * @namespace Carthesian Carthesian geometry kernel
 */
namespace Carthesian {
    /**
     * @brief  Calculate the line segment PaPb that is the shortest route between the segments P1P2 and P3P4.
     * Calculate the line segment PaPb that is the shortest route between two lines P1P2 and P3P4.
     * Calculate also the values of mua and mub where
     *     Pa = P1 + mua (P2 - P1)
     *     Pb = P3 + mub (P4 - P3)
     * source : http://paulbourke.net/geometry/lineline3d
     * @param p1 [IN] first segment start point
     * @param p2 [IN] first segment end point
     * @param p3 [IN] 2nd segment start point
     * @param p4 [IN] 2nd segment end point
     * @param pa [OUT] intersection point on P1P2
     * @param pb [OUT] intersection point on P3P4
     * @param mua [OUT] ratio as Pa = P1 + mua (P2 - P1)
     * @param mub [OUT] ratio as Pb = P3 + mub (P4 - P3)
     * @return Return false if no solution exists, true else
     */
    bool 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);
    /**
     * @brief  Calculate the line segment PaPb that is the shortest route between the segments P1P2 and P3P4.
     * Calculate the line segment PaPb that is the shortest route between two lines P1P2 and P3P4.
     * Calculate also the values of mua and mub where
     *     Pa = P1 + mua (P2 - P1)
     *     Pb = P3 + mub (P4 - P3)
     * source : http://paulbourke.net/geometry/lineline3d
     * @param p1p2 [IN] first segment
     * @param p3p4 [IN] 2nd segment start point
     * @param papb [OUT] intersection segment
     * @param mua [OUT] ratio as Pa = P1 + mua (P2 - P1)
     * @param mub [OUT] ratio as Pb = P3 + mub (P4 - P3)
     * @return Return false if no solution exists, true else
     */
    bool intersection (const Carthesian::Segment3D & p1p2,
                       const Carthesian::Segment3D & p3p4,
                       Carthesian::Segment3D & papb,
                       double & mua, double & mub);
    /**
     * @brief compute the angle in radians between 2 vectors
     * @param v1 first vector
     * @param v2 second vector
     * @return the oriented angle in radians
     */
    double angle (const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2);
    /**
     * @brief compute the scalar product between 2 vectors
     * @param v1 first vector
     * @param v2 second vector
     * @return the scalar product
     */
    double dotproduct (const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2);
    /**
     * @brief compute the dot product between a matrix and a vector
     * @param m [IN] a matrix
     * @param v [IN] a vector
     * @param vout [OUT] resulting vector
     */
    void dotproduct (const Carthesian::Matrix3D & m, const Carthesian::Vector3D & v, Carthesian::Vector3D &vout);
    /**
     * @brief compute the dot product between a matrix and a vector
     * @param m a matrix
     * @param v a vector
     * @return the resulting vector
     */
    Carthesian::Vector3D dotproduct (const Carthesian::Matrix3D & m, const Carthesian::Vector3D & v);
    /**
     * @brief compute the cross product between 2 vectors
     * @param v1 [IN] first vector
     * @param v2 [IN] second vector
     * @param vout [OUT] the cross product vector
     */
    void crossproduct (const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2, Carthesian::Vector3D & vout);
    /**
     * @brief compute the cross product between 2 vectors
     * @param v1 first vector
     * @param v2 second vector
     * @return the cross product vector
     */
    Carthesian::Vector3D crossproduct (const Carthesian::Vector3D & v1, const Carthesian::Vector3D & v2);
    /**
     * @brief construct the rotation matrix of an angle @a theta in radians using the vector @a axis as rotation axis
     * Uses the Rodrigues' Rotation Formula set in http://mathworld.wolfram.com/RodriguesRotationFormula.html
     * @param axis [IN] vector used as rotation axis
     * @param theta [IN] rotation angle in radians
     * @param matrix [OUT] rotation matrix
     */
    void rotation_matrix (const Carthesian::Vector3D & axis, const double theta, Carthesian::Matrix3D &matrix);
    /**
     * @brief construct the vector issued of the rotation of @a v by an angle @a theta in radians using the vector @a axis as rotation axis
     * Uses the Rodrigues' Rotation Formula set in http://mathworld.wolfram.com/RodriguesRotationFormula.html
     * @param v [IN] vector to rotate
     * @param axis [IN] vector used as rotation axis
     * @param theta [IN] rotation angle in radians
     * @param vout [OUT] vector issued of the rotation
     */
    void rotate (const Carthesian::Vector3D & v, const Carthesian::Vector3D & axis, const double theta, Carthesian::Vector3D & vout);
    /**
     * @brief construct the vector issued of the rotation of @a v by an angle @a theta in radians using the vector @a axis as rotation axis
     * Uses the Rodrigues' Rotation Formula set in http://mathworld.wolfram.com/RodriguesRotationFormula.html
     * @param v1 vector to rotate
     * @param axis vector used as rotation axis
     * @param theta rotation angle in radians
     * @return the vector issued of the rotation
     */
    Carthesian::Vector3D rotate (const Carthesian::Vector3D & v, const Carthesian::Vector3D & axis, const double theta);
    /**
     * @class Point3D represents a 3D carthesian point
     */
    class Point3D {
    public:
        double x;
        double y;
        double z;
        /**
         * @brief constructor
         * @param x x coordinate
         * @param y y coordinate
         * @param z z coordinate
         */
        Point3D (double x = 0.0, double y = 0.0, double z = 0.0);
        /**
         * @brief copy constructor
         * @param p point to copy
         */
        Point3D (const Carthesian::Point3D &p);
        /**
         * @brief affectation constructor
         * @param p point to copy
         */
        Point3D& operator= (const Carthesian::Point3D &p);
        /**
         * @brief translation operator
         * @param v translation vector
         * @return the point resulting of the translation of self by the vector v
         */
        const Point3D operator + (const Carthesian::Vector3D & v) const;
        /**
         * @brief content printer
         * @param os output flux
         * @param pixel current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Point3D& p) {
            os << "Carthesian::Point3D (" << p.x << ", " << p.y << ", " << p.z << ")";
            return os;
        };
        /**
         * @brief general setter
         * @param x x coordinate
         * @param y y coordinate
         * @param z z coordinate
         */
        void set (double x = 0.0, double y = 0.0, double z = 0.0);
        /**
         * @brief convert to spherical coordinates
         * @return P in spherical coordinates
         */
        Spherical::Point3D to_spherical (void) const;
        /**
         * @brief convert to Geocentric coordinates
         * @return P in Geocentric coordinates
         */
        Geocentric::Point3D to_geocentric (void) const;
        /**
         * @brief convert to geodetic coordinates (earth as an ellipsoid)
         * @return P in geodetic coordinates
         */
        Geodetic::Point3D to_geodetic (void) const;
        /**
         * @brief translate the point by the given vector
         * @param v translation vector
         */
        void translate (const Carthesian::Vector3D &v);
    };

    /**
     * @class Segment3D represents a 3D segment
     */
    class Segment3D {
    public:
        /** origin of the segment */
        Point3D p1;
        /** end of segment */
        Point3D p2;
        /**
         * @brief default constructor
         * @param p1 point of origin
         * @param p2 end point
         */
//         Segment3D ();
        /**
         * @brief constructor
         * @param p1 point of origin
         * @param p2 end point
         */
        Segment3D (const Carthesian::Point3D &p1 = Carthesian::Point3D(), const Carthesian::Point3D &p2 = Carthesian::Point3D());
        /**
         * @brief constructor
         * @param p1 point of origin
         * @param p2 end point
         */
//         Segment3D (const Geocentric::Point3D &p1, const Geocentric::Point3D &p2);
        /**
         * @brief constructor
         * @param p1 point of origin
         * @param p2 end point
         */
        Segment3D (const Geodetic::Point3D &p1, const Geodetic::Point3D &p2);
        /**
         * @brief copy constructor
         * @param s object to copy
         */
        Segment3D (const Segment3D &s);
        /**
         * @brief affectation constructor
         * @param s object to copy
         */
        Segment3D& operator= (const Segment3D &s);
        /**
         * @brief geneal setter
         * @param p1 point of origin
         * @param p2 end point
         */
        void set (const Carthesian::Point3D &p1, const Carthesian::Point3D &p2);
        /**
         * @brief content printer
         * @param os output flux
         * @param pixel current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Segment3D& p) {
            os << "Carthesian::Segment3D {" << p.get_p1() << " -> " << p.get_p2() << "}";
//             os << "Geocentric::Segment3D {" << p.get_p1().to_geocentric() << " -> " << p.get_p2().to_geocentric() << "}";
            return os;
        };
        /**
         * @brief accessor to origin of the segment
         * @return point origin of the segment
         */
        Point3D get_p1() const {
            return p1;
        }
        /**
         * @brief accessor to end of the segment
         * @return point end of the segment
         */
        Point3D get_p2() const {
            return p2;
        }
        /**
         * @brief return the length of the segment
         * @return the length of the segment
         */
        double length () const {
            double dx = p2.x - p1.x;
            double dy = p2.y - p1.y;
            double dz = p2.z - p1.z;
            return sqrt ((dx * dx) + (dy * dy) + (dz * dz));
        }

        /**
         * @brief construct the vector corresponding to the segment
         * @return the vector having the same orientation and the same length than the segment
         */
        Carthesian::Vector3D to_vector () const;
        /**
         * @brief construct the unit vector corresponding to the segment
         * @return the vector having the same orientation and a length of 1
         */
        Carthesian::Vector3D to_unit_vector () const;
    };

    /**
     * @class Vector3D represents a 3D vector
     */
    class Vector3D {
    public:
        double x;
        double y;
        double z;
        /**
         * @brief constructor
         * @param x x component
         * @param y y component
         * @param z z component
         */
        Vector3D (double x = 0.0, double y = 0.0, double z = 0.0);
        /**
         * @brief constructor from extremities
         * @param p1 point of origin
         * @param p2 end point
         */
        Vector3D (const Carthesian::Point3D &p1, const Carthesian::Point3D &p2);
        /**
         * @brief copy constructor
         * @param v object to copy
         */
        Vector3D (const Vector3D &v);
        /**
         * @brief affectation constructor
         * @param v object to copy
         */
        Vector3D& operator = (const Vector3D &v);
        /**
         * @brief dilation operator
         * @param val dilatation coefficient
         */
        const Vector3D operator * (const double & val);
        /**
         * @brief reduction operator
         * @param val redution coefficient
         */
        const Vector3D operator / (const double & val);
        /**
         * @brief geneal setter
         * @param x x component
         * @param y y component
         * @param z z component
         */
        void set (double x = 0.0, double y = 0.0, double z = 0.0);
        /**
         * @brief content printer
         * @param os output flux
         * @param pixel current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Vector3D& p) {
            os << "Carthesian::Vector3D [" << p.x << ", " << p.y << ", " << p.z << "]";
            return os;
        };
        /**
         * @brief return the length of the segment
         * @return the length of the segment
         */
        double norm ();
    };
    /**
     * @class Vector3D represents a 3D vector
     */
    class Matrix3D {
    public:
        double xx;
        double xy;
        double xz;
        double yx;
        double yy;
        double yz;
        double zx;
        double zy;
        double zz;
        /**
         * @brief constructor
         * @param xx xx element
         * @param xy xy element
         * @param xz xz element
         * @param yx yx element
         * @param yy yy element
         * @param yz yz element
         * @param zx zx element
         * @param zy zy element
         * @param zz zz element
         */
        Matrix3D (double xx = 0.0, double xy = 0.0, double xz = 0.0,
                  double yx = 0.0, double yy = 0.0, double yz = 0.0,
                  double zx = 0.0, double zy = 0.0, double zz = 0.0);
        /**
         * @brief copy constructor
         * @param v object to copy
         */
        Matrix3D (const Matrix3D &v);
        /**
         * @brief affectation constructor
         * @param v object to copy
         */
        Matrix3D& operator= (const Matrix3D &v);
        /**
         * @brief geneal setter
         * @param xx xx element
         * @param xy xy element
         * @param xz xz element
         * @param yx yx element
         * @param yy yy element
         * @param yz yz element
         * @param zx zx element
         * @param zy zy element
         * @param zz zz element
         */
        void set (double xx = 0.0, double xy = 0.0, double xz = 0.0,
                  double yx = 0.0, double yy = 0.0, double yz = 0.0,
                  double zx = 0.0, double zy = 0.0, double zz = 0.0);
        /**
         * @brief content printer
         * @param os output flux
         * @param m current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Matrix3D& m) {
            os << "Carthesian::Matrix3D |" << m.xx << ", " << m.xy << ", " << m.xz << std::endl;
            os << "                     |" << m.yx << ", " << m.yy << ", " << m.yz << std::endl;
            os << "                     |" << m.zx << ", " << m.zy << ", " << m.zz << std::endl;
            return os;
        };
    };
} // end namespace Carthesian

/**
 * @namespace Spherical Spherical geometry kernel
 */
namespace Spherical {
    /**
     * @class Point3D represents a 3D point in spherical geometry
     */
    class Point3D {
    public:
        /** radius from center */
        double r;
        /** zenith angle in radians (colatitude) */
        double theta;
        /** azimuth angle in radians */
        double phi;
        /**
         * @brief constructor
         * @param r the radius from center
         * @param theta the zenith angle in radians (ie colatitude)
         * @param phi the azimuth angle in radians
         */
        Point3D (double r = 0.0, double theta = 0.0, double phi = 0.0);
        /**
         * @brief copy constructor
         * @param p point to copy
         */
        Point3D (const Spherical::Point3D &p);
        /**
         * @brief affectation constructor
         * @param p point to copy
         */
        Point3D& operator= (const Spherical::Point3D &p);
        /**
         * @brief content printer
         * @param os output flux
         * @param pixel current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Point3D& p)
        {
            os << "Spherical::Point3D (" << p.r << ", " << p.theta << ", " << p.phi << ")";
            return os;
        }
        /**
         * @brief general setter
         * @param r the radius from center
         * @param theta the zenith angle in radians (ie colatitude)
         * @param phi the azimuth angle in radians
         */
        void set (double r = 0.0, double theta = 0.0, double phi = 0.0);
        /**
         * @brief convert to carthesian coordinates
         * @return P in carthesian coordinates
         */
        Carthesian::Point3D to_carthesian (void) const;
        /**
         * @brief convert to Geocentric coordinates
         * @return P in Geocentric coordinates
         */
        Geocentric::Point3D to_geocentric (void) const;
    };
} // end namespace Spherical

/**
 * @namespace Geocentric (lat, lon, alt) system geometry kernel. Here the coordinates named as "Geocentric" are the (latitude in degrees, longitude in degrees, and altitude above sea level in meters)
 */
namespace Geocentric {
    /**
     * @brief compute the angle in radians between 2 points P1 and P2 on a sphere, represented by their (latitude, longitude) coordinates
     * Source : http://www.faqs.org/faqs/geography/infosystems-faq/ ; Q5.1
     * Uses the Haversine Formula (from R.W. Sinnott, "Virtues of the Haversine", Sky and Telescope, vol. 68, no. 2, 1984, p. 159):
     *
     * @param lat1 latitude of the 1st point, in degrees. In the range [ -90, 90 ]
     * @param lon1 longitude of the 1st point, in degrees. In the range ] -180, 180 ]
     * @param lat2 latitude of the 2nd point, in degrees. In the range [ -90, 90 ]
     * @param lon2 longitude of the 2nd point, in degrees. In the range ] -180, 180 ]
     * @return the angle between P1 and P2 in radians
     */
    const double get_orthodormic_angle ( double lat1, double lon1,
                                         double lat2, double lon2);
    /**
     * @brief return the square plane distance between 2 points using their latitude and longitude difference
     * @param dlat latitude difference, in degrees. In the range [ -90, 90 ]
     * @param dlon longitude difference, in degrees. In the range ] -180, 180 ]
     * @return the distance between the 2 points using the square plane approximation
     */
    const double get_square_plane_dist ( const double dlat, const double dlon );
    /**
     * @brief return the square plane distance between 2 points using their latitudes and longitudes
     * @param lat1 latitude of the first point, in degrees. In the range [ -90, 90 ]
     * @param lon1 longitude of the first point, in degrees. In the range ] -180, 180 ]
     * @param lat2 latitude of the 2nd point, in degrees. In the range [ -90, 90 ]
     * @param lon2 longitude of the 2nd point, in degrees. In the range ] -180, 180 ]
     * @return the distance between the 2 points using the square plane approximation
     */
    const double get_square_plane_dist ( const double lat1, const double lon1,
                                         const double lat2, const double lon2 );
    /**
     * @brief compute the Haversine distance in km between 2 points P1 and P2 on the earth, represented by their (latitude, longitude) coordinates.
     * Here, the earth is considered as an ideal sphere, using the authalic radius ("equal aera"), from http://en.wikipedia.org/wiki/Earth_radius
     * Source : http://www.faqs.org/faqs/geography/infosystems-faq/ ; Q5.1
     * Uses the Haversine Formula (from R.W. Sinnott, "Virtues of the Haversine", Sky and Telescope, vol. 68, no. 2, 1984, p. 159):
     * @param lat1 latitude of the 1st point, in degrees. In the range [ -90, 90 ]
     * @param lon1 longitude of the 1st point, in degrees. In the range ] -180, 180 ]
     * @param lat2 latitude of the 2nd point, in degrees. In the range [ -90, 90 ]
     * @param lon2 longitude of the 2nd point, in degrees. In the range ] -180, 180 ]
     * @return the Haversine distance between P1 and P2 in m
     */
    const double get_haversine_dist (double lat1, double lon1, double lat2, double lon2);
    /**
     * @class Point3D represents a 3D point in Geocentric coordinates. Spherical earth model is assumed
     */
    class Point3D {
    public:
        /** latitude in degrees from north pole */
        double lat;
        /** longitude in degrees from greenwich meridian */
        double lon;
        /** altitude above mean-sea level in meters */
        double alt;
        /**
         * @brief constructor
         * @param lat latitude in degrees from north pole
         * @param lon longitude in degrees from greenwich meridian
         * @param alt altitude above mean-sea level in meters
         */
        Point3D (double lat = 0.0, double lon = 0.0, double alt = 0.0);
        /**
         * @brief copy constructor
         * @param p point to copy
         */
        Point3D (const Geocentric::Point3D &p);
        /**
         * @brief affectation constructor
         * @param p point to copy
         */
        Point3D& operator= (const Geocentric::Point3D &p);
        /**
         * @brief content printer
         * @param os output flux
         * @param pixel current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Point3D& p) {
            os << "Geocentric::Point3D (" << p.lat << ", " << p.lon << ", " << p.alt << ")";
            return os;
        };
        /**
         * @brief general setter
         * @param lat latitude in degrees from north pole
         * @param lon longitude in degrees from greenwich meridian
         * @param alt altitude above mean-sea level in meters
         */
        void set (double lat = 0.0, double lon = 0.0, double alt = 0.0);
        /**
         * @brief convert to spherical coordinates
         * @return P in spherical coordinates
         */
        Spherical::Point3D to_spherical (void) const;
        /**
         * @brief convert to carthesian coordinates. Spherical earth model is assumed
         * @return P in carthesian coordinates
         */
        Carthesian::Point3D to_carthesian () const;
    };
} // end namespace Geocentric

/**
 * @namespace Geodetic (lat, lon, alt) system geometry kernel, where earth is considered as an ellipsoid, as specified in the WGS84 system
 */
namespace Geodetic {
    /**
     * @class Point3D represents a 3D point in geodetic coordinates.
     */
    class Point3D {
    public:
        /** latitude in degrees from north pole */
        double lat;
        /** longitude in degrees from greenwich meridian */
        double lon;
        /** altitude above mean-sea level of ellipsoid in meters */
        double alt;
        /**
         * @brief constructor
         * @param lat latitude in degrees from north pole
         * @param lon longitude in degrees from greenwich meridian
         * @param alt altitude above mean-sea of ellipsoid level in meters
         */
        Point3D (double lat = 0.0, double lon = 0.0, double alt = 0.0);
        /**
         * @brief copy constructor
         * @param p point to copy
         */
        Point3D (const Geodetic::Point3D &p);
        /**
         * @brief affectation constructor
         * @param p point to copy
         */
        Point3D& operator= (const Geodetic::Point3D &p);
        /**
         * @brief content printer
         * @param os output flux
         * @param pixel current oject alias
         * @return the content description added to the output flux
         */
        friend std::ostream &operator<< (std::ostream &os, const Geodetic::Point3D& p) {
            os << "Geodetic::Point3D (" << p.lat << ", " << p.lon << ", " << p.alt << ")";
            return os;
        };
        /**
         * @brief general setter
         * @param lat latitude in degrees from north pole
         * @param lon longitude in degrees from greenwich meridian
         * @param alt altitude above mean-sea level in meters
         */
        void set (double lat = 0.0, double lon = 0.0, double alt = 0.0);
        /**
         * @brief convert to spherical coordinates
         * @return P in spherical coordinates
         */
        Spherical::Point3D to_spherical (void) const;
        /**
         * @brief convert to carthesian coordinates. WGS84 ellipsoidal earth model is assumed
         * @return P in carthesian coordinates
         */
        Carthesian::Point3D to_carthesian () const;
    };
} // end namespace Geocentric


#endif