/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2003 Robert Osfield 
 *
 * This library is open source and may be redistributed and/or modified under  
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or 
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 * 
 * This library 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 
 * OpenSceneGraph Public License for more details.
*/

#ifndef OSGTERRAIN_COORDINATESYSTEM
#define OSGTERRAIN_COORDINATESYSTEM 1

#include <osg/Object>
#include <osg/Vec2>
#include <osg/Vec3>
#include <osg/Matrixd>

#include <osgTerrain/Export>

namespace osgTerrain
{

const double WGS_84_RADIUS_EQUATOR = 6378137.0;
const double WGS_84_RADIUS_POLAR = 6356752.3142;

class EllipsodeTransform
{
    public:

        EllipsodeTransform(double radiusEquator = WGS_84_RADIUS_EQUATOR,
                           double radiusPolar = WGS_84_RADIUS_POLAR):
            _radiusEquator(radiusEquator),
            _radiusPolar(radiusPolar) { computeCoefficients(); }

        EllipsodeTransform(const EllipsodeTransform& et):
            _radiusEquator(et._radiusEquator),
            _radiusPolar(et._radiusPolar) { computeCoefficients(); }

        void setRadiusEquator(double radius) { _radiusEquator = radius; computeCoefficients(); }
        double getRadiusEquator() const { return _radiusEquator; }

        void setRadiusPolar(double radius) { _radiusPolar = radius; computeCoefficients(); }
        double getRadiusPolar() const { return _radiusPolar; }

        inline void convertLatLongHeightToXYZ(double latitude, double longitude, double height,
                                              double& X, double& Y, double& Z) const
        {
            // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif
            double sin_latitude = sin(latitude);
            double cos_latitude = cos(latitude);
            double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
            X = (N+height)*cos_latitude*cos(longitude);
            Y = (N+height)*cos_latitude*sin(longitude);
            Z = (N*(1-_eccentricitySquared)+height)*sin(latitude);
        }

        inline void convertXYZToLatLongHeight(double X, double Y, double Z,
                                              double& latitude, double& longitude, double& height) const
        {
            // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
            double p = sqrt(X*X + Y*Y);
            double theta = atan(Z*_radiusEquator/ (p*_radiusPolar));
            double eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar)/
                                  (_radiusPolar*_radiusPolar);

            double sin_theta = sin(theta);
            double cos_theta = cos(theta);

            latitude = atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) /
                             (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) );
            longitude = atan2(Y,X);

            double sin_latitude = sin(latitude);
            double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);

            height = p/cos(latitude) - N;
        }

        inline void computeLocalToWorldTransform(double latitude, double longitude, double height, osg::Matrixd& localToWorld) const
        {
            double X, Y, Z;
            convertLatLongHeightToXYZ(latitude,longitude,height,X,Y,Z);
            
            localToWorld.makeTranslate(X,Y,Z);
            
            // normalize X,Y,Z
            double inverse_length = 1.0/sqrt(X*X + Y*Y + Z*Z);
            X *= inverse_length;
            Y *= inverse_length;
            Z *= inverse_length;
            
            double length_XY = sqrt(X*X + Y*Y);
            double inverse_length_XY = 1.0/length_XY;
            
            // Vx = |(-Y,X,0)|
            localToWorld(0,0) = -Y*inverse_length_XY;
            localToWorld(0,1) = X*inverse_length_XY;
            localToWorld(0,2) = 0.0;
            
            // Vy = /(-Z*X/(sqrt(X*X+Y*Y), -Z*Y/(sqrt(X*X+Y*Y),sqrt(X*X+Y*Y))| 
            double Vy_x = -Z*X*inverse_length_XY;
            double Vy_y = -Z*Y*inverse_length_XY;
            double Vy_z = length_XY;
            inverse_length = 1.0/sqrt(Vy_x*Vy_x + Vy_y*Vy_y + Vy_z*Vy_z);            
            localToWorld(1,0) = Vy_x*inverse_length;
            localToWorld(1,1) = Vy_y*inverse_length;
            localToWorld(1,2) = Vy_z*inverse_length;

            // Vz = (X,Y,Z)
            localToWorld(2,0) = X;
            localToWorld(2,1) = Y;
            localToWorld(2,2) = Z;
        }

        osg::Vec3 computeGavitationVector(double X, double Y, double Z) const
        {
            osg::Vec3 normal(-X,-Y,-Z);
            normal.normalize();
            return normal;
        }
        
        
    protected:
    
        void computeCoefficients()
        {
            double flattening = (_radiusEquator-_radiusPolar)/_radiusEquator;
            _eccentricitySquared = 2*flattening - flattening*flattening;
        }

        double _radiusEquator;
        double _radiusPolar;
        double _eccentricitySquared;

};

/** CoordinateSystem encapsulate the coordinate system that associated with objects in a scene.*/
class CoordinateSystem : public osg::Object
{
    public:

        CoordinateSystem();

        CoordinateSystem(const std::string& WTK);

        /** Copy constructor using CopyOp to manage deep vs shallow copy.*/
        CoordinateSystem(const CoordinateSystem&,const osg::CopyOp& copyop=osg::CopyOp::SHALLOW_COPY);
        
        META_Object(osgTerrain,CoordinateSystem);
        
        inline bool operator == (const CoordinateSystem& cs) const
        {
            if (this == &cs) return true;
            if (_WKT != cs._WKT) return false;
            return true;
        }
        
        inline bool operator != (const CoordinateSystem& cs) const
        {
            return !(*this==cs);
        }
                
        /** Set the CoordinateSystem reference string, should be stored in OpenGIS Well Know Text form.*/
        void setWKT(const std::string& WKT) { _WKT = WKT; }
        
        /** Get the CoordinateSystem reference string.*/
        const std::string& getWKT() const { return _WKT; }
        
        
        /** CoordinateTransformation is a helper class for transforming between two different CoodinateSystems.
          * To use, simply constructor a CoordinateSystem::CoordinateTransformation convertor(sourceCS,destinateCS)
          * and then convert indiviual points via v_destination = convert(v_source), or the
          * CoordinateTransformation.convert(ptr,num) method when handling arrays of Vec2/Vec3's.*/
        class CoordinateTransformation : public osg::Referenced
        {
            public:
            
                static CoordinateTransformation* createCoordinateTransformation(const CoordinateSystem& source, const CoordinateSystem& destination);
                
                static void setCoordinateTransformationPrototpe(CoordinateTransformation* ct);
                
                virtual osg::Vec2 operator () (const osg::Vec2& source) const = 0;
                virtual osg::Vec3 operator () (const osg::Vec3& source) const = 0;
                
                virtual bool transform(unsigned int numPoints, osg::Vec2* vec2ptr) const = 0;
                virtual bool transform(unsigned int numPoints, osg::Vec3* vec3ptr) const = 0;
                
            protected:
            
                CoordinateTransformation() {}
                virtual ~CoordinateTransformation() {}
                
                virtual CoordinateTransformation* cloneCoordinateTransformation(const CoordinateSystem& source, const CoordinateSystem& destination) const = 0;
                
        };
        
    protected:

        virtual ~CoordinateSystem() {}
                
        std::string _WKT;

};

}
#endif