#!/perl -w
# NAME: ll2xyz.pl
# AIM: Convert latitude, logitude to x,y,z co-ordinates
use strict;
use warnings;
use Time::HiRes qw(gettimeofday tv_interval);       # provide more accurate timings
use constant PI    => 4 * atan2(1, 1);
require 'logfile.pl' or die "Unable to load logfile.pl ...\n";
require 'fg_wsg84.pl' or die "Unable to load fg_wsg84.pl ...\n";
# log file stuff
my ($LF);
my $pgmname = $0;
if ($pgmname =~ /\w{1}:\\.*/) {
    my @tmpsp = split(/\\/,$pgmname);
    $pgmname = $tmpsp[-1];
}
my $outfile = "temp.$pgmname.txt";
open_log($outfile);
prt( "$0 ... Hello, World ...\n" );

my $do_dbg_sets = 0;
my $do_dbg_timing = 1;

### 2516 VHHH HONG KONG INTL (22.3083428148148,113.917764876543) tile=e110n20
my $hklat = 22.3083428148148;
my $hklon = 113.917764876543;
### NDME  49.20933100,  007.39577800,  1201, 10910,    25,        0.0,  ZND, ZWEIBRUCKEN DME (1)
my $zlat = 49.20933100;
my $zlon = 7.39577800;

#/** Meters to Nautical Miles.  1 nm = 6076.11549 feet */
my $METER_TO_NM = 0.0005399568034557235;

my $PI = 3.1415926535897932384626433832795029;
my $D2R = $PI / 180;
my $R2D = 180 / $PI;
my $ERAD = 6378138.12;

#// These are hard numbers from the WGS84 standard.  DON'T MODIFY
#// unless you want to change the datum.
my $HP_EQURAD = 6378137.0;
my $HP_FLATTENING = 298.257223563;
#// High-precision versions of the above produced with an arbitrary
#// precision calculator (the compiler might lose a few bits in the FPU
#// operations).  These are specified to 81 bits of mantissa, which is
#// higher than any FPU known to me:
my $HP_SQUASH = 0.9966471893352525192801545;
my $HP_STRETCH = 1.0033640898209764189003079;
my $HP_POLRAD = 6356752.3142451794975639668;

my $HP_MIN = 2.22507e-308;

#my $D2_FACTOR = 10000000;
my $D2_FACTOR = $ERAD;
#my $D2_FACTOR = $HP_POLRAD;

my $lat = 0.0;
my $lon = 0.0;
my $nlat = 0;
my $nlon = 0;

my $lastlon = 0.0;
my $lastlat = 0.0;

my ($i);

prt( "Using D2_FACTOR of $D2_FACTOR ...\n" );
prt( "Perl PI=".PI.", vs PI=$PI. Diff=".abs(PI - $PI)."\n" );

if ($do_dbg_sets) {
    for ($i = 0; $i < 10; $i++) {
        $nlat = $i * 10;
        $nlon = $lon;
        do_lat_lon( $nlat, $nlon );
    }
    for ($i = 0; $i < 10; $i++) {
        $nlat = $lat;
        $nlon = $i * 10;
        do_lat_lon( $nlat, $nlon );
    }
    for ($i = 0; $i < 10; $i++) {
        $nlat = $i * 10;
        $nlon = $i * 10;
        do_lat_lon( $nlat, $nlon );
    }
}

$lastlat = $hklat;
$lastlon = $hklon;
do_lat_lon( $zlat, $zlon );
$lastlat = 0.0;
$lastlon = 0.0;
do_lat_lon( $lastlat, 1.0 );
do_lat_lon( 1.0, $lastlon );
do_lat_lon( $lastlat, 2.0 );
do_lat_lon( $lastlat, 3.0 );
do_lat_lon( $lastlat, 89.9999 );
do_lat_lon( $lastlat, 179.3 );


if ($do_dbg_timing) {
    do_time_tests();
}


close_log($outfile,1);
exit(0);

sub do_time_tests {
    my ($d, @bgn, @end, $td1, $td2, $j, $ret, $az1, $az2, $s);
    $lat = 0;
    $lon = 0;
    $lastlat = 1;
    $lastlon = 1;
    @bgn = gettimeofday();
    for ($j = 0; $j < 1000; $j++) {
        for ($i = 0; $i < 10; $i++) {
            $nlat = $i * 10;
            $nlon = $lon;
            $d = fg_lat_lon_distance_m( $nlat, $nlon, $lastlat, $lastlon );
        }
        for ($i = 0; $i < 10; $i++) {
            $nlat = $lat;
            $nlon = $i * 10;
            $d = fg_lat_lon_distance_m( $nlat, $nlon, $lastlat, $lastlon );
        }
        for ($i = 0; $i < 10; $i++) {
            $nlat = $i * 10;
            $nlon = $i * 10;
            $d = fg_lat_lon_distance_m( $nlat, $nlon, $lastlat, $lastlon );
        }
    }
    @end = gettimeofday();
    $td1 = tv_interval( \@bgn, \@end );
    prt( "Took $td1 seconds ...\n");
    @bgn = gettimeofday();
    for ($j = 0; $j < 1000; $j++) {
        for ($i = 0; $i < 10; $i++) {
            $nlat = $i * 10;
            $nlon = $lon;
            $ret = fg_geo_inverse_wgs_84( $nlat, $nlon, $lastlat, $lastlon, \$az1, \$az2, \$s );
        }
        for ($i = 0; $i < 10; $i++) {
            $nlat = $lat;
            $nlon = $i * 10;
            $ret = fg_geo_inverse_wgs_84( $nlat, $nlon, $lastlat, $lastlon, \$az1, \$az2, \$s );
        }
        for ($i = 0; $i < 10; $i++) {
            $nlat = $i * 10;
            $nlon = $i * 10;
            $ret = fg_geo_inverse_wgs_84( $nlat, $nlon, $lastlat, $lastlon, \$az1, \$az2, \$s );
        }
    }
    @end = gettimeofday();
    $td2 = tv_interval( \@bgn, \@end );
    prt( "Took $td2 seconds ... factor of ".($td2 / $td1)."!\n");

}


sub do_lat_lon {
    my ($lat, $lon) = @_;
    my ($x, $y, $z) = fg_ll2xyz($lon, $lat);
    my ($clon, $clat) = fg_xyz2ll($x, $y, $z);
    my ($lx, $ly, $lz) = fg_ll2xyz($lastlon, $lastlat);
    ###my $dist = sqrt ( coord_dist_sq( $lx, $ly, $lz, $x, $y, $z ) ) * $D2_FACTOR;
    my $dist = fg_coord_distance_m( $lx, $ly, $lz, $x, $y, $z );
    my $dist2 = fg_lat_lon_distance_m( $lat, $lon, $lastlat, $lastlon );
    my $nm = $dist * $METER_TO_NM;
    my $nm2 = $dist2 * $METER_TO_NM;
    prt( "Lon $lon, Lat $lat, is $x, $y, $z ($clon,$clat)\n" );
    prt( " DIST1    = $dist m ($nm nm) to ($lastlat,$lastlon)\n" );
    prt( " DIST2    = $dist2 m ($nm2 nm) to ($lastlat,$lastlon)\n" );
    my ($az1, $az2, $s);
    $az1 = -1;
    $az2 = -1;
    $s = -1;
    my $ret = fg_geo_inverse_wgs_84( $lat, $lon, $lastlat, $lastlon, \$az1, \$az2, \$s );
    if ($ret == 0) {
        my $nm2 = $s * $METER_TO_NM;
        my $diff = (abs($nm - $nm2) / $nm2) * 100.0;
        my $taz = $az1 + $az2 - 360;
        prt( " Distance = $s m ($nm2 nm), on azimuth $az1 ($taz) ret=$ret diff=".int($diff + 0.5)." pct\n" );
        # sub fg_geo_direct_wgs_84 {
        # my ( $lat1, $lon1, $az1, $s, $lat2, $lon2, $az2 ) = @_;
        $ret = fg_geo_direct_wgs_84( $lat, $lon, $az1, $s, \$clat, \$clon, \$taz );
        prt( sprintf("  Verify1 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n",
            $lat, $lon, $az1, $s, $clat, $clon, $lastlat,$lastlon ) );
        $ret = fg_geo_direct_wgs_84( $lastlat, $lastlon, $az2, $s, \$clat, \$clon, \$taz );
        prt( sprintf("  Verify2 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n",
            $lastlat, $lastlon, $az2, $s, $clat, $clon, $lat,$lon ) );
    } else {
        prt( "fg_geo_inverse_wgs_84 FAILED to find solution!!!\n" );
    }

    if ($do_dbg_sets) {
        $lastlat = $lat;
        $lastlon = $lon;
    }
}

sub do_lat_lon_ok {
    my ($lat, $lon) = @_;
    my ($x, $y, $z) = ll2xyz_ok($lon, $lat);
    my ($clon, $clat) = xyz2ll_ok($x, $y, $z);
    my ($lx, $ly, $lz) = ll2xyz_ok($lastlon, $lastlat);
    ###my $dist = sqrt ( coord_dist_sq( $lx, $ly, $lz, $x, $y, $z ) ) * $D2_FACTOR;
    my $dist = coord_distance_m_ok( $lx, $ly, $lz, $x, $y, $z );
    my $dist2 = lat_lon_distance_m_ok( $lat, $lon, $lastlat, $lastlon );
    my $nm = $dist * $METER_TO_NM;
    my $nm2 = $dist2 * $METER_TO_NM;
    prt( "Lon $lon, Lat $lat, is $x, $y, $z ($clon,$clat)\n" );
    prt( " DIST1    = $dist m ($nm nm) to ($lastlat,$lastlon)\n" );
    prt( " DIST2    = $dist2 m ($nm2 nm) to ($lastlat,$lastlon)\n" );
    my ($az1, $az2, $s);
    $az1 = -1;
    $az2 = -1;
    $s = -1;
    my $ret = fg_geo_inverse_wgs_84( $lat, $lon, $lastlat, $lastlon, \$az1, \$az2, \$s );
    if ($ret == 0) {
        my $nm2 = $s * $METER_TO_NM;
        my $diff = (abs($nm - $nm2) / $nm2) * 100.0;
        my $taz = $az1 + $az2 - 360;
        prt( " Distance = $s m ($nm2 nm), on azimuth $az1 ($taz) ret=$ret diff=".int($diff + 0.5)." pct\n" );
        # sub fg_geo_direct_wgs_84 {
        # my ( $lat1, $lon1, $az1, $s, $lat2, $lon2, $az2 ) = @_;
        $ret = fg_geo_direct_wgs_84( $lat, $lon, $az1, $s, \$clat, \$clon, \$taz );
        prt( sprintf("  Verify1 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n",
            $lat, $lon, $az1, $s, $clat, $clon, $lastlat,$lastlon ) );
        $ret = fg_geo_direct_wgs_84( $lastlat, $lastlon, $az2, $s, \$clat, \$clon, \$taz );
        prt( sprintf("  Verify2 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n",
            $lastlat, $lastlon, $az2, $s, $clat, $clon, $lat,$lon ) );
    } else {
        prt( "fg_geo_inverse_wgs_84 FAILED to find solution!!!\n" );
    }

    if ($do_dbg_sets) {
        $lastlat = $lat;
        $lastlon = $lon;
    }
}

###########################################
# *** These were just for development. ***
# The final services have been placed in
# the fg_wsg84.pl module.
###########################################
sub ll2xyz_ok($$) {
	my $lon = (shift) * $D2R;
	my $lat = (shift) * $D2R;
	my $cosphi = cos $lat;
	my $di = $cosphi * cos $lon;
	my $dj = $cosphi * sin $lon;
	my $dk = sin $lat;
	return ($di, $dj, $dk);
}

sub xyz2ll_ok($$$) {
	my ($di, $dj, $dk) = @_;
	my $aux = $di * $di + $dj * $dj;
	my $lat = atan2($dk, sqrt $aux) * $R2D;
	my $lon = atan2($dj, $di) * $R2D;
	return ($lon, $lat);
}

sub coord_dist_sq_ok {
	my ($xa, $ya, $za, $xb, $yb, $zb) = @_;
	my $x = $xb - $xa;
	my $y = $yb - $ya;
	my $z = $zb - $za;
	return $x * $x + $y * $y + $z * $z;
}

sub coord_distance_m_ok {
	my ($xa, $ya, $za, $xb, $yb, $zb) = @_;
    return (sqrt( coord_dist_sq_ok( $xa, $ya, $za, $xb, $yb, $zb ) ) * $D2_FACTOR);
}

sub lat_lon_distance_m_ok {
    my ($lat1, $lon1, $lat2, $lon2) = @_;
    my ($xa, $ya, $za) = ll2xyz_ok($lon1, $lat1);
    my ($xb, $yb, $zb) = ll2xyz_ok($lon2, $lat2);
    return coord_distance_m_ok( $xa, $ya, $za, $xb, $yb, $zb );
}

# eof - ll2xyz.pl
