/******************************************************************************
 *{@C
 *      Copyright:      2011-2016 Paul Obermeier (obermeier@tcl3d.org)
 *
 *                      See the file "Tcl3D_License.txt" for information on
 *                      usage and redistribution of this file, and for a
 *                      DISCLAIMER OF ALL WARRANTIES.
 *
 *      Module:         Tcl3D -> tcl3dOgl
 *      Filename:       tcl3dGeoMath.c
 *
 *      Author:         Paul Obermeier
 *
 *      Description:    C functions for handling geographic coordinate
 *                      conversions.
 *                      Currently supported are position and orientation
 *                      conversions from geocentric (ECEF) to geodetic (LLA)
 *                      and vice versa.
 *                      These conversions are typically needed when running
 *                      DIS/HLA simulations.
 *
 *                      Latitude and longitude as well as orientations are
 *                      specified in radians. Altitude and ECEF variables
 *                      are specified in meters.
 *
 *                      Note: The algorithmns in this file are ported from the
 *                            C++ implementation of the KDIS project.
 *
 *                      This module is intended to be wrapped with Swig for the
 *                      Tcl3D package.
 *
 *****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>

#include "tcl3dGeoMath.h"

static void 
GetEllipsoidAxis (RefEllipsoid refEll, double *MajorAxis, double *MinorAxis)
{
    switch( refEll )
    {
        case Airy:
            *MajorAxis = 6377563.396;
            *MinorAxis = 6356256.909;
            /* 1/F 299.324965 */
            break;

        case Airy_Modified:
            *MajorAxis = 6377340.189;
            *MinorAxis = 6356034.448;
            /* 1/F 299.324965 */
            break;

        case Australian_National:
            *MajorAxis = 6378160.000;
            *MinorAxis = 6356774.719;
            /* 1/F 298.250000 */
            break;

        case Bessel_1841:
            *MajorAxis = 6377397.155;
            *MinorAxis = 6356078.963;
            /* 1/F 299.152813 */
            break;

        case Bessel_1841_Namibia:
            *MajorAxis = 6377483.865;
            *MinorAxis = 6356078.963;
            /* 1/F 299.152813 */
            break;

        case Clarke_1866:
            *MajorAxis = 6378206.400;
            *MinorAxis = 6356583.800;
            /* 1/F 294.978698 */
            break;

        case Clarke_1880:
            *MajorAxis = 6378249.145;
            *MinorAxis = 6356514.870;
            /* 1/F 293.465000 */
            break;

        case Everest_Sabah_Sarawak:
            *MajorAxis = 6377298.556;
            *MinorAxis = 6356097.550;
            /* 1/F 300.801700 */
            break;

        case Everest_1830:
            *MajorAxis = 6377276.345;
            *MinorAxis = 6356075.413;
            /* 1/F 300.801700 */
            break;

        case Everest_1948:
            *MajorAxis = 6377304.063;
            *MinorAxis = 6356103.039;
            /* 1/F 300.801700 */
            break;

        case Everest_1956:
            *MajorAxis = 6377301.243;
            *MinorAxis = 6356100.228;
            /* 1/F 300.801700 */
            break;

        case Everest_1969:
            *MajorAxis = 6377295.664;
            *MinorAxis = 6356094.668;
            /* 1/F 300.801700 */
            break;

        case Fischer_1960:
            *MajorAxis = 6378166.000;
            *MinorAxis = 6356784.284;
            /* 1/F 298.300000 */
            break;

        case Fischer_1960_Modified:
            *MajorAxis = 6378155.000;
            *MinorAxis = 6356773.320;
            /* 1/F 298.300000 */
            break;

        case Fischer_1968:
            *MajorAxis = 6378150.000;
            *MinorAxis = 6356768.337;
            /* 1/F 298.300000 */
            break;

        case GRS_1980:
            *MajorAxis = 6378137.000;
            *MinorAxis = 6356752.314;
            /* 1/F 298.257222 */
            break;

        case Helmert_1906:
            *MajorAxis = 6378200.000;
            *MinorAxis = 6356818.170;
            /* 1/F 298.300000 */
            break;

        case Hough:
            *MajorAxis = 6378270.000;
            *MinorAxis = 6356794.343;
            /* 1/F 297.000000 */
            break;

        case International_1924:
            *MajorAxis = 6378388.000;
            *MinorAxis = 6356911.946;
            /* 1/F 297.000000 */
            break;

        case Karsovsky_1940:
            *MajorAxis = 6378245.000;
            *MinorAxis = 6356863.019;
            /* 1/F 298.300000 */
            break;

        case SGS_1985:
            *MajorAxis = 6378136.000;
            *MinorAxis = 6356751.302;
            /* 1/F 298.257000 */
            break;

        case South_American_1969:
            *MajorAxis = 6378160.000;
            *MinorAxis = 6356774.719;
            /* 1/F 298.250000 */
            break;

        case Sphere_6371km:
            *MajorAxis = 6371000;
            *MinorAxis = 6371000;
            break;

        case WGS_1960:
            *MajorAxis = 6378165.000;
            *MinorAxis = 6356783.287;
            /* 1/F 298.300000 */
            break;

        case WGS_1966:
            *MajorAxis = 6378145.000;
            *MinorAxis = 6356759.769;
            /* 1/F 298.250000 */
            break;

        case WGS_1972:
            *MajorAxis = 6378135.000;
            *MinorAxis = 6356750.520;
            /* 1/F 298.260000 */
            break;

        case WGS_1984:
            *MajorAxis = 6378137.000;
            *MinorAxis = 6356752.314245;
            /* 1/F 298.257224 */
            break;
    }
}

void tcl3dGeodetic2Geocentric (double lat, double lon, double alt,
                               RefEllipsoid refEll,
                               double *x, double *y, double *z)
{
    double MajorAxis, MinorAxis;
    double Esq, V;

    GetEllipsoidAxis( refEll, &MajorAxis, &MinorAxis );

    Esq = ( pow( MajorAxis, 2 ) - pow( MinorAxis, 2 ) ) / pow( MajorAxis, 2 );
    V = MajorAxis / sqrt( 1 - ( Esq * pow( sin( lat ), 2 ) ) );

    *x = ( V + alt ) * cos( lat ) * cos( lon );
    *y = ( V + alt ) * cos( lat ) * sin( lon );
    *z = ( ( 1 - Esq ) * V + alt ) * sin( lat );
}

void tcl3dGeocentric2Geodetic (double x, double y, double z, 
                               RefEllipsoid refEll,
                               double *lat, double *lon, double *alt)
{
    double a, b, t;
    double e2, ed2, a2, b2, z2,e4, r2, r;
    double E2, F, G, C, S, P, Q, r0;
    double U, V;

    GetEllipsoidAxis (refEll, &a, &b);

    e2  = 1.0 - ( b * b ) / ( a * a ) ;   /* 1st eccentricity squared */
    ed2 = ( a * a ) / ( b * b ) - 1.0 ;   /* 2nd eccentricity squared */
    a2  = a * a ;
    b2  = b * b ;
    z2  = z * z ;
    e4  = e2 * e2 ;
    r2  = x * x + y * y ;
    r   = sqrt( r2 );

    E2 = a2 - b2 ;
    F = 54.0 * b2 * z2 ;
    G = r2 + ( 1.0 - e2 ) * z2 - e2 * E2 ;
    C = e4 * F * r2 / ( G * G * G );
    S = pow( 1.0 + C + sqrt( C * C + 2.0 * C ) , 1.0 / 3.0 );

    t = S + 1.0 / S + 1.0 ;
    P = F / ( 3.0 * t * t * G * G );
    Q = sqrt( 1.0 + 2.0 * e4 * P );
    r0 = -( P * e2 * r ) / ( 1.0 + Q ) + sqrt( 0.5 * a2 * ( 1.0 + 1.0 / Q ) - ( P * ( 1 - e2 ) * z2 ) / ( Q * ( 1.0 + Q ) ) - 0.5 * P * r2 );

    t = r - e2 * r0;
    U = sqrt( t * t + z2 );
    V = sqrt( t * t + ( 1.0 - e2 ) * z2 );

    t = b2 / ( a * V );

    *alt = U * ( 1.0 - t );
    *lat = atan2( z + ed2 * t * z , r );
    *lon = atan2( y, x );
}

static void 
RotateAboutAxis (double d[3], const double s[3],
                 const double n[3], double t)
{
    double st = sin (t);
    double ct = cos (t);

    d[0] = (1.0-ct)*(n[0]*n[0]*s[0] +
                     n[0]*n[1]*s[1] +
                     n[0]*n[2]*s[2]) +
           ct*s[0] + st*(n[1]*s[2]-n[2]*s[1]);
    d[1] = (1.0-ct)*(n[0]*n[1]*s[0] +
                     n[1]*n[1]*s[1] +
                     n[1]*n[2]*s[2]) +
           ct*s[1] + st*(n[2]*s[0]-n[0]*s[2]);
    d[2] = (1.0-ct)*(n[0]*n[2]*s[0] +
                     n[1]*n[2]*s[1] +
                     n[2]*n[2]*s[2]) +
           ct*s[2] + st*(n[0]*s[1]-n[1]*s[0]);
}

static void Cross (double d[3], const double a[3], const double b[3])
{
    d[0] = a[1]*b[2] - b[1]*a[2];
    d[1] = b[0]*a[2] - a[0]*b[2];
    d[2] = a[0]*b[1] - b[0]*a[1];
}

static double Dot (const double a[3], const double b[3])
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

void tcl3dLocal2GeoOrient (double head, double pitch, double roll, 
                           double lat, double lon, 
                           double *psi, double *theta, double *phi)
{
    /* local NED */
    double D0[3] = { 1.0, 0.0, 0.0 };
    double E0[3] = { 0.0, 1.0, 0.0 };
    double N0[3] = { 0.0, 0.0, 1.0 };
    double me[3];
    double N[3];
    double E[3];
    double D[3];

    double  N1[3] , E1[3] , D1[3] ;
    double  N2[3] , E2[3] , D2[3] ;
    double  N3[3] , E3[3] , D3[3] ;

    double  x0[3] = { 1.0 , 0.0 , 0.0 };   /* == D0 */
    double  y0[3] = { 0.0 , 1.0 , 0.0 };   /* == E0 */
    double  z0[3] = { 0.0 , 0.0 , 1.0 };   /* == Z0 */
    double  y2[3];
    double  z2[3];

    /* 'E' */
    RotateAboutAxis( E , E0 , N0 , lon );
    me[0] = -E[0] ;
    me[1] = -E[1] ;
    me[2] = -E[2] ;
    /* 'N' */
    RotateAboutAxis( N , N0 , me , lat );
    /* 'D' */
    Cross( D , N , E );

    /* Orientation */
    /* rotate about D by heading */
    RotateAboutAxis( N1 , N  , D , head );
    RotateAboutAxis( E1 , E  , D , head );
    memcpy( D1 , D  , sizeof( double[3] ) );
    /* rotate about E1 vector by pitch */
    RotateAboutAxis( N2 , N1 , E1 , pitch );
    memcpy( E2 , E1 , sizeof( double[3] ) );
    RotateAboutAxis( D2 , D1 , E1 , pitch );
    /* rotate about N2 by roll */
    memcpy( N3 , N2 , sizeof( double[3] ) );
    RotateAboutAxis( E3 , E2 , N2 , roll );
    RotateAboutAxis( D3 , D2 , N2 , roll );

    /* calculate angles from vectors */
    *psi = atan2(  Dot( N3 , y0 ) , Dot( N3 , x0 ) );
    *theta = atan2( -Dot( N3 , z0 ) , sqrt( pow(Dot( N3 , x0 ), 2) + pow(Dot( N3 , y0 ), 2) ) );
    RotateAboutAxis( y2 , y0 , z0 , *psi );
    RotateAboutAxis( z2 , z0 , y2 , *theta );
    *phi = atan2(  Dot( E3 , z2 ) , Dot( E3 , y2 ) );
}

void tcl3dGeo2LocalOrient (double psi, double theta, double phi,
                           double lat, double lon,
                           double *head, double *pitch, double *roll )
{
    /* local NED vectors in ECEF coordinate frame */
    double  N[3];
    double  E[3];
    double  D[3];

    /* Calculate NED from lat and lon */
    /* local NED */
    double D0[3] = { 1.0, 0.0, 0.0 };
    double E0[3] = { 0.0, 1.0, 0.0 };
    double N0[3] = { 0.0, 0.0, 1.0 };
    double me[3];

    double  X[3] = { 1.0, 0.0, 0.0 };
    double  Y[3] = { 0.0, 1.0, 0.0 };
    double  Z[3] = { 0.0, 0.0, 1.0 };
    double  X1[3], Y1[3], Z1[3];
    double  X2[3], Y2[3], Z2[3];
    double  X3[3], Y3[3], Z3[3];

    double x0[3], y0[3], z0[3];
    double y2[3];
    double z2[3];

    /* 'E' */
    RotateAboutAxis( E , E0 , N0 , lon );
    me[0] = -E[0] ;
    me[1] = -E[1] ;
    me[2] = -E[2] ;
    /* 'N' */
    RotateAboutAxis( N , N0 , me , lat );
    /* 'D' */
    Cross( D , N , E );

    /*
     *  Orientation:
     *  input : (x0,y0,z0)=(N,E,D) and (psi,theta,phi Euler angles)
     *  output: (x3,y3,z3)=body vectors in local frame
     */
    /* rotate about Z by psi */
    RotateAboutAxis( X1 , X  , Z , psi );
    RotateAboutAxis( Y1 , Y  , Z , psi );
    memcpy(          Z1 , Z  , sizeof( double[3] ) );
    /* rotate about Y1 vector by theta */
    RotateAboutAxis( X2 , X1 , Y1 , theta );
    memcpy(          Y2 , Y1 , sizeof( double[3] ) );
    RotateAboutAxis( Z2 , Z1 , Y1 , theta );
    /* rotate about X2 by phi */
    memcpy(          X3 , X2 , sizeof( double[3] ) );
    RotateAboutAxis( Y3 , Y2 , X2 , phi );
    RotateAboutAxis( Z3 , Z2 , X2 , phi );
    /* calculate angles from vectors */
    memcpy( x0 , N , sizeof( double[3] ) );
    memcpy( y0 , E , sizeof( double[3] ) );
    memcpy( z0 , D , sizeof( double[3] ) );
    *head = atan2(  Dot( X3 , y0 ) , Dot( X3 , x0 ) );
    *pitch = atan2( -Dot( X3 , z0 ),
                    sqrt( pow(Dot( X3 , x0 ), 2) + pow(Dot( X3 , y0 ), 2) ) );
    RotateAboutAxis( y2 , y0 , z0 , *head );
    RotateAboutAxis( z2 , z0 , y2 , *pitch );
    *roll = atan2(  Dot( Y3 , z2 ) , Dot( Y3 , y2 ) );
}
