#include <GFC.h>
#pragma hdrstop

#include <stdio.h>

/*
** Author: Samuel R. Blackburn
** Internet: sblackbu@erols.com
**
** You can use it any way you like as long as you don't try to sell it.
**
** Any attempt to sell GFC in source code form must have the permission
** of the original author. You can produce commercial executables with
** GFC but you can't sell GFC.
**
** Copyright, 1998, Samuel R. Blackburn
**
** $Workfile: CEarth.cpp $
** $Revision: 11 $
** $Modtime: 9/01/98 9:56p $
*/

CEarth::CEarth( int ellipsoid_identifier )
{
   m_Initialize();
   SetEllipsoid( ellipsoid_identifier );
}

CEarth::~CEarth()
{
   m_Initialize();
}

void CEarth::AddLineOfSightDistanceAndDirectionToCoordinate( const CPolarCoordinate& point_1, double distance, double direction, CPolarCoordinate& point_2, double height_above_surface_of_point_2 )
{
   // The method used here is to convert the straight (line-of-sight) distance to
   // a surface distance and then find out the position using the surface distance.
   // This is a translation of the MMDIST routine found in the FORWRD3D program at
   // ftp://ftp.ngs.noaa.gov/pub/pcsoft/for_inv.3d/source/forwrd3d.for

   double c                                           = 0.0;
   double cosine_of_latitude_squared                  = 0.0;
   double cosine_of_direction_squared                 = 0.0;
   double cosine_of_point_1_latitude                  = 0.0;
   double difference_in_height                        = 0.0;
   double direction_in_radians                        = 0.0;
   double distance_adjusted_for_differences_in_height = 0.0;
   double height_above_surface_of_point_1             = 0.0;
   double n                                           = 0.0;
   double point_1_latitude_in_radians                 = 0.0;
   double polar_eccentricity_squared                  = 0.0;
   double polar_flattening                            = 0.0;
   double r                                           = 0.0;
   double sine_of_point_1_latitude                    = 0.0;
   double surface_distance                            = 0.0;
   double term_1                                      = 0.0;
   double term_2                                      = 0.0;
   double term_3                                      = 0.0;
   double two_r                                       = 0.0;
   double x                                           = 0.0;

   // Many thanks to Peter Dana (pdana@mail.utexas.edu) for educating me
   // on the finer points of Geodesy, one of which was how to compute
   // "second eccentricity squared"

   polar_eccentricity_squared = ( ( m_EquatorialRadiusInMeters * m_EquatorialRadiusInMeters ) - ( m_PolarRadiusInMeters * m_PolarRadiusInMeters ) ) / ( m_PolarRadiusInMeters * m_PolarRadiusInMeters );

   //printf( "polar_eccentricity_squared is %.23lf\n", polar_eccentricity_squared );

   point_1_latitude_in_radians = CMath::ConvertDegreesToRadians( point_1.GetUpDownAngleInDegrees() );
   direction_in_radians        = CMath::ConvertDegreesToRadians( direction );

   cosine_of_point_1_latitude = CMath::Cosine( point_1_latitude_in_radians );
   cosine_of_latitude_squared = cosine_of_point_1_latitude * cosine_of_point_1_latitude;

   cosine_of_direction_squared = CMath::Cosine( direction_in_radians ) * CMath::Cosine( direction_in_radians );

   c = ( m_EquatorialRadiusInMeters * m_EquatorialRadiusInMeters ) / m_PolarRadiusInMeters;

   n = c / CMath::SquareRoot( 1.0 + ( polar_eccentricity_squared * cosine_of_latitude_squared ) );

   r = n / ( 1.0 + ( polar_eccentricity_squared * cosine_of_latitude_squared * cosine_of_direction_squared ) );

   height_above_surface_of_point_1 = point_1.GetDistanceFromSurfaceInMeters();

   difference_in_height = height_above_surface_of_point_2 - height_above_surface_of_point_1;

   term_1 = ( distance * distance ) - ( difference_in_height * difference_in_height );
   term_2 = 1.0 + ( height_above_surface_of_point_1 / r );
   term_3 = 1.0 + ( height_above_surface_of_point_2 / r );
   distance_adjusted_for_differences_in_height = CMath::SquareRoot( term_1 / ( term_2 * term_3 ) );

   // printf( "distance_adjusted_for_differences_in_height is %.11lf\n", distance_adjusted_for_differences_in_height );

   two_r = 2.0 * r;

   surface_distance = two_r * CMath::ArcSine( distance_adjusted_for_differences_in_height / two_r );

   // printf( "surface_distance is %.11lf\n", surface_distance );

   AddSurfaceDistanceAndDirectionToCoordinate( point_1, surface_distance, direction, point_2 );
}

void CEarth::AddSurfaceDistanceAndDirectionToCoordinate( const CEarthCoordinate& point_1, double distance, double direction, CPolarCoordinate& point_2 )
{
   CPolarCoordinate polar_point_1;

   Convert( point_1, polar_point_1 );

   AddSurfaceDistanceAndDirectionToCoordinate( polar_point_1, distance, direction, point_2 );
}

void CEarth::AddSurfaceDistanceAndDirectionToCoordinate( const CEarthCoordinate& point_1, double distance, double direction, CEarthCoordinate& point_2 )
{
   CPolarCoordinate polar_point_1;
   CPolarCoordinate polar_point_2;

   Convert( point_1, polar_point_1 );

   AddSurfaceDistanceAndDirectionToCoordinate( polar_point_1, distance, direction, polar_point_2 );

   Convert( polar_point_2, point_2 );
}

void CEarth::AddSurfaceDistanceAndDirectionToCoordinate( const CPolarCoordinate& point_1, double distance, double direction, CEarthCoordinate& point_2 )
{
   CPolarCoordinate polar_coordinate;

   AddSurfaceDistanceAndDirectionToCoordinate( point_1, distance, direction, polar_coordinate );

   Convert( polar_coordinate, point_2 );
}

void CEarth::AddSurfaceDistanceAndDirectionToCoordinate( const CPolarCoordinate& point_1, double distance, double direction, CPolarCoordinate& point_2 )
{
   // This is a translation of the Fortran routine DIRCT1 found in the
   // FORWRD3D program at:
   // ftp://ftp.ngs.noaa.gov/pub/pcsoft/for_inv.3d/source/forwrd3d.for

   double c                                          = 0.0;
   double c2a                                        = 0.0;
   double cosine_of_direction                        = 0.0;
   double cosine_of_y                                = 0.0;
   double cu                                         = 0.0;
   double cz                                         = 0.0;
   double d                                          = 0.0;
   double e                                          = 0.0;
   double direction_in_radians                       = 0.0;
   double eps                                        = 0.0;
   double heading_from_point_2_to_point_1_in_radians = 0.0;
   double point_1_latitude_in_radians                = 0.0;
   double point_1_longitude_in_radians               = 0.0;
   double point_2_latitude_in_radians                = 0.0;
   double point_2_longitude_in_radians               = 0.0;
   double r                                          = 0.0;
   double sa                                         = 0.0;
   double sine_of_direction                          = 0.0;
   double sine_of_y                                  = 0.0;
   double su                                         = 0.0;
   double tangent_u                                  = 0.0;
   double term_1                                     = 0.0;
   double term_2                                     = 0.0;
   double term_3                                     = 0.0;
   double x                                          = 0.0;
   double y                                          = 0.0;

   direction_in_radians = CMath::ConvertDegreesToRadians( direction );

   eps = 0.000000000000005;

   r = 1.0 - m_Flattening;

   point_1_latitude_in_radians  = CMath::ConvertDegreesToRadians( point_1.GetUpDownAngleInDegrees()    );
   point_1_longitude_in_radians = CMath::ConvertDegreesToRadians( point_1.GetLeftRightAngleInDegrees() );

   tangent_u = ( r * CMath::Sine( point_1_latitude_in_radians ) ) / CMath::Cosine( point_1_latitude_in_radians );

   sine_of_direction = CMath::Sine( direction_in_radians );

   cosine_of_direction = CMath::Cosine( direction_in_radians );

   heading_from_point_2_to_point_1_in_radians = 0.0;

   if ( cosine_of_direction != 0.0 )
   {
      heading_from_point_2_to_point_1_in_radians = CMath::ArcTangentOfYOverX( tangent_u, cosine_of_direction ) * 2.0;
   }

   cu = 1.0 / CMath::SquareRoot( ( tangent_u * tangent_u ) + 1.0 );
   su = tangent_u * cu;
   sa = cu * sine_of_direction;
   c2a = ( (-sa) * sa ) + 1.0;
   x = CMath::SquareRoot( ( ( ( 1.0 / r / r ) - 1.0 ) * c2a ) + 1.0 ) + 1.0;
   x = ( x - 2.0 ) / x;
   c = 1.0 - x;
   c = ( ( ( x * x ) / 4.0 ) + 1.0 ) / c;
   d = ( ( 0.375 * ( x * x ) ) -1.0 ) * x;

   tangent_u = distance / r / m_EquatorialRadiusInMeters / c;

   y = tangent_u;

   bool exit_loop = false;

   while( exit_loop != true )
   {
      sine_of_y = CMath::Sine( y );
      cosine_of_y = CMath::Cosine( y );
      cz = CMath::Cosine( heading_from_point_2_to_point_1_in_radians + y );
      e = ( cz * cz * 2.0 ) - 1.0;
      c = y;
      x = e * cosine_of_y;
      y = ( e + e ) - 1.0;

      term_1 = ( sine_of_y * sine_of_y * 4.0 ) - 3.0;
      term_2 = ( ( term_1 * y * cz * d ) / 6.0 ) + x;
      term_3 = ( ( term_2 * d ) / 4.0 ) - cz;
      y = ( term_3 * sine_of_y * d ) + tangent_u;

      if ( CMath::AbsoluteValue( y - c ) > eps )
      {
         exit_loop = false;
      }
      else
      {
         exit_loop = true;
      }
   }

   heading_from_point_2_to_point_1_in_radians = ( cu * cosine_of_y * cosine_of_direction ) - ( su * sine_of_y );

   c = r * CMath::SquareRoot( ( sa * sa ) + ( heading_from_point_2_to_point_1_in_radians * heading_from_point_2_to_point_1_in_radians ) );
   d = ( su * cosine_of_y ) + ( cu * sine_of_y * cosine_of_direction );

   point_2_latitude_in_radians = CMath::ArcTangentOfYOverX( d, c );

   c = ( cu * cosine_of_y ) - ( su * sine_of_y * cosine_of_direction );
   x = CMath::ArcTangentOfYOverX( sine_of_y * sine_of_direction, c );
   c = ( ( ( ( ( -3.0 * c2a ) + 4.0 ) * m_Flattening ) + 4.0 ) * c2a * m_Flattening ) / 16.0;
   d = ( ( ( ( e * cosine_of_y * c ) + cz ) * sine_of_y * c ) + y ) * sa;

   point_2_longitude_in_radians = ( point_1_longitude_in_radians + x ) - ( ( 1.0 - c ) * d * m_Flattening );

   heading_from_point_2_to_point_1_in_radians = CMath::ArcTangentOfYOverX( sa, heading_from_point_2_to_point_1_in_radians ) + CMath::Pi();

   point_2.SetUpDownAngleInDegrees(    CMath::ConvertRadiansToDegrees( point_2_latitude_in_radians ) );
   point_2.SetLeftRightAngleInDegrees( CMath::ConvertRadiansToDegrees( point_2_longitude_in_radians ) );
}

void CEarth::Convert( const CEarthCoordinate& cartesian_coordinate, CPolarCoordinate& polar_coordinate ) const
{
   // convert from cartesian to polar

   double equatorial_radius_times_eccentricity_squared = 0.0;

   equatorial_radius_times_eccentricity_squared = m_EquatorialRadiusInMeters * m_EccentricitySquared;

   double p = 0.0;

   p = CMath::SquareRoot( ( cartesian_coordinate.GetXCoordinateInMeters() * cartesian_coordinate.GetXCoordinateInMeters() ) + 
                          ( cartesian_coordinate.GetYCoordinateInMeters() * cartesian_coordinate.GetYCoordinateInMeters() ) );

   double temp_latitude = 0.0;
   double z_coordinate  = cartesian_coordinate.GetZCoordinateInMeters(); // for convienance
   double one_minus_eccentricity_squared = 1.0 - m_EccentricitySquared;

   temp_latitude = z_coordinate / p / one_minus_eccentricity_squared;

   double old_value  = 0.0;
   double temp_value = 0.0;
   double part_a     = 0.0;
   double part_b     = 0.0;
   double part_c     = 0.0;

   unsigned long loop_index              = 0;
   unsigned long maximum_number_of_tries = 1024;

   bool convergence_was_acheived = false;

   while( convergence_was_acheived != true && loop_index < maximum_number_of_tries )
   {
      old_value = temp_latitude;

      part_a = one_minus_eccentricity_squared * temp_latitude * temp_latitude;
      part_b = equatorial_radius_times_eccentricity_squared / CMath::SquareRoot( 1.0 + part_a );
      part_c = p - part_b;

      temp_latitude = z_coordinate / part_c;

      loop_index++;

      if ( CMath::AbsoluteValue( temp_latitude - old_value ) > 0.000000000000000000001 )
      {
         // Oh well, try again...
      }
      else
      {
         // YIPEE!! We've reached convergence!
         convergence_was_acheived = true;
      }
   }

   if ( convergence_was_acheived == true )
   {
      double latitude_angle_in_radians = 0.0;

      // Save the UpDown angle in degrees
      
      latitude_angle_in_radians = CMath::ArcTangent( temp_latitude );

      polar_coordinate.SetUpDownAngleInDegrees( CMath::ConvertRadiansToDegrees( latitude_angle_in_radians ) ); // Latitude

      double sine_of_latitude_in_radians   = 0.0;
      double cosine_of_latitude_in_radians = 0.0;

      sine_of_latitude_in_radians   = CMath::Sine(   latitude_angle_in_radians );
      cosine_of_latitude_in_radians = CMath::Cosine( latitude_angle_in_radians );

      double longitude_in_radians = 0.0;

      longitude_in_radians = CMath::ArcTangentOfYOverX( cartesian_coordinate.GetYCoordinateInMeters(), cartesian_coordinate.GetXCoordinateInMeters() );

      polar_coordinate.SetLeftRightAngleInDegrees( CMath::ConvertRadiansToDegrees( longitude_in_radians ) ); // Longitude

      double w = 0.0;

      w = CMath::SquareRoot( 1.0 - ( m_EccentricitySquared * sine_of_latitude_in_radians * sine_of_latitude_in_radians ) );

      double distance_from_center_to_surface_of_the_ellipsoid = 0.0;

      distance_from_center_to_surface_of_the_ellipsoid = m_EquatorialRadiusInMeters / w;

      double distance_from_surface = 0.0;

      if ( CMath::AbsoluteValue( latitude_angle_in_radians ) < 0.7854 )
      {
         //printf( "fabs( %14.10lf ) < 0.7854\n", latitude_angle_in_radians );
         distance_from_surface = ( p / cosine_of_latitude_in_radians ) - distance_from_center_to_surface_of_the_ellipsoid;
      }
      else
      {
         //printf( "fabs( %14.10lf ) >= 0.7854\n", latitude_angle_in_radians );
         distance_from_surface = ( z_coordinate / sine_of_latitude_in_radians ) - distance_from_center_to_surface_of_the_ellipsoid + ( m_EccentricitySquared * distance_from_center_to_surface_of_the_ellipsoid );
      }

      // printf( "CEarth::Convert() : First method produced a length of %14.10lf\n", distance_from_surface );

      polar_coordinate.SetDistanceFromSurfaceInMeters( distance_from_surface );
   }
   else
   {
      // Oh well, we gave it a shot..
      polar_coordinate.Set( 0.0, 0.0, 0.0 );
   }
}

void CEarth::Convert( const CPolarCoordinate& polar_coordinate, CEarthCoordinate& cartesian_coordinate ) const
{
   // convert from polar to cartesian

   double up_down_radians    = 0.0; // latitude
   double left_right_radians = 0.0; // longitude angle

   up_down_radians    = CMath::ConvertDegreesToRadians( polar_coordinate.GetUpDownAngleInDegrees()    );
   left_right_radians = CMath::ConvertDegreesToRadians( polar_coordinate.GetLeftRightAngleInDegrees() );

   double sine_of_up_down_radians      = 0.0;
   double cosine_of_left_right_radians = 0.0; // cosine_of_longitude
   double cosine_of_up_down_radians    = 0.0; // cosine_of_latitude

   sine_of_up_down_radians      = CMath::Sine(   up_down_radians    );
   cosine_of_left_right_radians = CMath::Cosine( left_right_radians );
   cosine_of_up_down_radians    = CMath::Cosine( up_down_radians    );

   // Now we need to calculate the distance from the center of the ellipsoid to the surface of the ellipsoid
   double w = 0.0;

   w = CMath::SquareRoot( 1.0 - ( m_EccentricitySquared * sine_of_up_down_radians * sine_of_up_down_radians ) );

   double distance_from_center_to_surface_of_the_ellipsoid = 0.0;

   distance_from_center_to_surface_of_the_ellipsoid = m_EquatorialRadiusInMeters / w;

   // printf( "en = %.25lf\n", distance_from_center_to_surface_of_the_ellipsoid );

   double coordinate = 0.0;

   coordinate = ( distance_from_center_to_surface_of_the_ellipsoid + polar_coordinate.GetDistanceFromSurfaceInMeters() ) * cosine_of_up_down_radians * cosine_of_left_right_radians;
   cartesian_coordinate.SetXCoordinateInMeters( coordinate );