#!/usr/bin/perl -w
# NAME: distance02.pl
# AIM: Use perl trig function for distance London to Tokyo...
# from : http://perldoc.perl.org/Math/Trig.html
# previous disptance.pl, with no use of SIMGEAR
# 17/12/2013 - Give precise info, unless verbosity raised
# 08/09/2011 - Minor adjustment - when -v2 output SG_Head in FULL - see possible error in az2!!! 
#              And -i <NUM> option, to output a list of points inbetween
# 06/08/2011 - Add -s speed in knots, default = 100 kt, and FIX inadvertent lat,lon reversal ;=))
# 26/02/2011 - Improved Distance: display
# 18/12/2010 - Fix heading from reverse track to heading (true)...
# 05/12/2010 - 01/12/2010 - Allow two comma separated pairs of input
# 20/11/2010 geoff mclane http://geoffair.net/mperl
use strict;
use warnings;
use Math::Trig qw(great_circle_distance great_circle_direction deg2rad rad2deg);
my $perl_dir = 'C:/GTools/perl'; 
unshift(@INC, $perl_dir);
require 'fg_wsg84.pl' or die "Unable to load fg_wsg84.pl ...\n";

my $VERS = "0.0.3 2011-09-08"; # added --inter <NUM>, to output a SET of points between
# my $VERS = "0.0.2 2011-08-06"; # updated version
# my $VERS = "0.0.1 2010-12-01" # premier version

# log file stuff
# my ($LF);
my $pgmname = $0;
if ($pgmname =~ /(\\|\/)/) {
    my @tmpsp = split(/(\\|\/)/,$pgmname);
    $pgmname = $tmpsp[-1];
}
# my $outfile = $perl_dir."\\temp.$pgmname.txt";
# open_log($outfile);
 
# 1 knots = 1.85200 kph
my $K2KPH = 1.85200;
my $Km2NMiles = 1 / $K2KPH; # Nautical Miles.
#my $Km2NMiles = 0.53995680346; # Nautical Miles.
my $MAD_LL = -200;

# London and Tokyo - not used
#my $lonlon = -0.5;
#my $lonlat = 51.3;
#my $toklon = 139.8;
#my $toklat = 35.7;

my $g_lat1 = $MAD_LL;
my $g_lon1 = $MAD_LL;
my $g_lat2 = $MAD_LL;
my $g_lon2 = $MAD_LL;

my $g_ias = 100; # Knots - c182
#my $g_ias = 80; # Knots - c152-c172
my $g_speed = $g_ias * $K2KPH; # Knots to Kilometers/Hour
# my $CP_EPSILON = 0.00001;
my $CP_EPSILON = 0.0000001; # EQUALS SG_EPSILON 20101121
my $g_interval = $MAD_LL;

my $verbosity = 0;

sub VERB1() { return ($verbosity >= 1); }
sub VERB2() { return ($verbosity >= 2); }
sub VERB5() { return ($verbosity >= 5); }
sub VERB9() { return ($verbosity >= 9); }

sub prt($) {
    printf shift;
}

sub pgm_exit($$) {
    my ($val,$msg) = @_;
    if (length($msg)) {
        $msg .= "\n" if (!($msg =~ /\n$/));
        prt($msg);
    }
    #show_warnings($val);
    #close_log($outfile,$load_log);
    exit($val);
}

sub in_world($$) {
    my ($lat,$lon) = @_;
    if (($lat < -90)||
        ($lat >  90)||
        ($lon < -180)||
        ($lon >  180)) {
        return 0;   # FAILED
    }
    return 1;
}

sub in_world_show($$) {
    my ($lat,$lon) = @_;
    if (($lat < -90)||
        ($lat >  90)||
        ($lon < -180)||
        ($lon >  180)) {
        prt("Testing lat=$lat, lon=$lon ");
        prt("lat LT -90 ") if ($lat < -90);
        prt("lat GT  90 ") if ($lat >  90);
        prt("lon LT -180 ") if ($lon < -180);
        prt("lon GT  180 ") if ($lon >  180);
        prt("\n");
        return 0;   # FAILED
    }
    return 1;
}


sub set_decimal1_stg($) {
    my $r = shift;
    ${$r} =  int((${$r} + 0.05) * 10) / 10;
    ${$r} = "0.0" if (${$r} == 0);
    ${$r} .= ".0" if !(${$r} =~ /\./);
}

sub set_decimal2_stg($) {
    my $r = shift;
    ${$r} =  int((${$r} + 0.005) * 100) / 100;
    ${$r} = "0.00" if (${$r} == 0);
    ${$r} .= ".00" if !(${$r} =~ /\./);
}

sub set_decimal_stg($) {
    my $r = shift;
    if (${$r} < 10) {
        set_decimal2_stg($r);
    } else {
        set_decimal1_stg($r);
    }
}


 # Notice the 90 - latitude: phi zero is at the North Pole
sub NESW { deg2rad($_[0]), deg2rad(90 - $_[1]) }

sub show_sg_distance_vs_est($$$$$$) {
    my ($lat1,$lat2,$lon1,$lon2,$estdist,$esthdg) = @_;
    my ($sg_az1,$sg_az2,$sg_dist,$res,$sg_km,$sg_ikm,$dsg_az1,$dsg_az2);
    my ($eddiff,$edpc,$ahdg,$ahdiff,$hdgpc,$adhdg,$distmsg,$hdgmsg);
    $res = fg_geo_inverse_wgs_84 ($lat1,$lon1,$lat2,$lon2,\$dsg_az1,\$dsg_az2,\$sg_dist);
    $sg_km = $sg_dist / 1000;
    $sg_ikm = int($sg_km + 0.5);
    $sg_az1 = int(($dsg_az1 * 10) + 0.05) / 10;
    $sg_az2 = int(($dsg_az2 * 10) + 0.05) / 10;
    $eddiff = abs($sg_km - $estdist);
    $edpc = ($eddiff / $sg_km) * 100;
    $edpc = int(($edpc * 10) + 0.05) / 10;
    if ($eddiff < $CP_EPSILON) {
        $edpc = "SAME";
    } elsif ($edpc < 1) {
        $edpc = 'lt 1';
    }
    $edpc = " $edpc" while (length($edpc) < 5);

    $adhdg = abs($sg_az1 - $esthdg);
    $ahdiff = ($adhdg / $sg_az1);
    $hdgpc = ($ahdiff / 360);
    $hdgpc = int(($hdgpc * 10) + 0.05) / 10;
    if ($ahdiff < $CP_EPSILON) {
        $hdgpc = "SAME";
    } elsif ($hdgpc < 1) {
        $hdgpc = 'lt 1';
    }
    $hdgpc = " $hdgpc" while (length($hdgpc) < 5);
    print "SG_Dist : $sg_ikm kilometers ($sg_km) ($edpc \% diff)\n";
    if (VERB2()) {
        print "SG_Head : $dsg_az1 degs (inverse $dsg_az2) ($hdgpc \% diff)\n";
    } else {
        print "SG_Head : $sg_az1 degs (inverse $sg_az2) ($hdgpc \% diff)\n";
    }
}

my $flag_show_sg_stg = 0;
my $flag_show_sg_rev = 0;

# get_sg_distance_vs_est( $lat1,$lon1,$lat2,$lon2,$d_km,$t_degs, \$cmpdist, \$cmphdg );
sub get_sg_distance_vs_est($$$$$$$$) {
    my ($lat1,$lon1,$lat2,$lon2,$estdist,$esthdg,$rdist,$rhdg) = @_;
    my ($sg_az1,$sg_az2,$sg_dist,$res,$sg_km,$sg_ikm);
    my ($eddiff,$edpc,$ahdg,$ahdiff,$hdgpc,$adhdg,$distmsg,$hdgmsg);
    my $flag = 0;
    #$res = fg_geo_inverse_wgs_84 ($lat1,$lon1,$lat2,$lon2,\$sg_az1,\$sg_az2,\$sg_dist);
    $res = fg_geo_inverse_wgs_84 ($lat1,$lon1,$lat2,$lon2,\$sg_az2,\$sg_az1,\$sg_dist);
    $sg_km = $sg_dist / 1000;
    $sg_ikm = int($sg_km + 0.5);
    $sg_az1 = int(($sg_az1 * 10) + 0.05) / 10;
    $sg_az2 = int(($sg_az2 * 10) + 0.05) / 10;
    $eddiff = abs($sg_km - $estdist);
    $edpc = ($eddiff / $sg_km) * 100;
    $edpc = int(($edpc * 10) + 0.05) / 10;
    if ($eddiff < $CP_EPSILON) {
        $edpc = "SAME";
        $flag |= 1;
    } elsif ($edpc < 1) {
        $edpc = 'lt 1';
        $flag |= 1;
    }
    $edpc = " $edpc" while (length($edpc) < 5);

    # could be subtraction 359 from 1 - max difference = 360 degrees
    $adhdg = abs($sg_az1 - $esthdg);
    $ahdiff = $adhdg; # ($adhdg / $sg_az1);
    $ahdiff -= 180 if ($ahdiff >= 180);
    $hdgpc = ($ahdiff / 180); # ($ahdiff / 360);
    $hdgpc = int(($hdgpc * 10) + 0.05) / 10;
    # prt("Comparing HEADINGS SG $sg_az1 EST $esthdg... diff [$adhdg] [$ahdiff] [$hdgpc]\n");
    if ($ahdiff < $CP_EPSILON) {
        $hdgpc = "SAME";
        $flag |= 2;
    } elsif ($hdgpc < 0.001) {
        $hdgpc = 'lt 1';
        $flag |= 2;
    } else {
        $hdgpc = int($hdgpc * 100);
    }
    $hdgpc = " $hdgpc" while (length($hdgpc) < 5);
    #${$rdist} = " SG_Dist : $sg_ikm kilometers ($sg_km) ($edpc \% diff)";
    #${$rhdg} = " SG_Head : $sg_az1 degs (inverse $sg_az2) ($hdgpc \% diff)";
    if ( !$flag_show_sg_stg && ($flag == 3) ) {
        ${$rdist} = " (SG ok)";
        ${$rhdg}  = " (SG ok)";
    } else {
        ${$rdist} = " SG_Dist : $sg_ikm km ($edpc \% diff)";
        if ($flag_show_sg_rev) {
            $sg_az2 = int($sg_az2 + 0.5);
            ${$rhdg}  = " SG_Head : $sg_az1/$sg_az2 degs ($hdgpc \% diff)";
        } else {
            ${$rhdg}  = " SG_Head : $sg_az1 degs ($hdgpc \% diff)";
        }
    }
    return $flag;
}

sub get_latlon_stg($) {
    my ($ll) = shift;
    my $stg = "";
    my $len = length($ll);
    my ($i,$ch);
    for ($i = 0; $i < $len; $i++) {
        $ch = substr($ll,$i,1);
        last if ($ch eq '.');
        $stg .= $ch;
    }
    $stg = " ".$stg while (length($stg) < 4);
    $ch = '.' if ($ch ne '.');
    $stg .= $ch;
    $i++;
    for (; $i < $len; $i++) {
        $ch = substr($ll,$i,1);
        $stg .= $ch;
    }
    $stg .= " " while (length($stg) < 19);
    return $stg;
}
sub get_heading_stg($) {
    my $hdg = shift;
    my $stg = sprintf("%3.1f",$hdg);
    return $stg;
}


sub show_intervals($$$$) {
    my ($lat1,$lon1,$lat2,$lon2) = @_;
    my ($dsg_az1,$dsg_az2,$sg_dist);
    my ($i,$i2,$nlat,$nlon,$naz,$scnt,$nlat2,$nlon2,$naz2,$raz,$max);
    my $res = fg_geo_inverse_wgs_84 ($lat1,$lon1,$lat2,$lon2,\$dsg_az1,\$dsg_az2,\$sg_dist);
    if (VERB5()) {
        prt("List of $g_interval intervals from $lat1,$lon1,\n");
        prt("on azimuth $dsg_az1, $sg_dist meters, to $lat2,$lon2\n");
    } else {
        #$naz2 = get_heading_stg($dsg_az1);
        #prt("List $g_interval intervals, hdg $naz2\n");
        $scnt = sprintf("%3d:",0);
        $nlat2 = get_latlon_stg($lat1);
        $nlon2 = get_latlon_stg($lon1);
        $naz2  = get_heading_stg($dsg_az1);
        $scnt = " " x length($scnt);
        prt("$scnt $nlat2 $nlon2 in $g_interval intervals\n");

    }
    $max = $g_interval; # - 1;
    my $dist = $sg_dist;
    $dist = $sg_dist / $max if ($max > 0);
    $scnt = sprintf("%3d:",0);
    if (VERB9()) {
        $nlat2 = get_latlon_stg($lat1);
        $nlon2 = get_latlon_stg($lon1);
        $naz2  = get_heading_stg($dsg_az1);
        $scnt = " " x length($scnt);
        prt("$scnt $nlat2 $nlon2 $naz2\n");
    }
    for ($i = 0; $i < $max; $i++) {
        $i2 = $i + 1;
        fg_geo_direct_wgs_84( $lat1, $lon1, $dsg_az1, $dist * $i2, \$nlat, \$nlon, \$naz );
        $scnt = sprintf("%3d:",$i2);
        #if (VERB1()) {
            $nlat2 = get_latlon_stg($nlat);
            $nlon2 = get_latlon_stg($nlon);
            $raz = $naz + 180;
            $raz -= 360 if ($raz > 360);
            $naz2  = get_heading_stg($raz);
            prt("$scnt $nlat2 $nlon2 $naz2\n");
        #} else {
        #    prt("$scnt $nlat $nlon $naz\n");
        #}
    }
    #if (VERB9()) {
        $scnt = sprintf("%3d:",0);
        $nlat2 = get_latlon_stg($lat2);
        $nlon2 = get_latlon_stg($lon2);
        $naz2  = get_heading_stg($dsg_az1);
        $scnt = " " x length($scnt);
        prt("$scnt $nlat2 $nlon2 $naz2\n");
    #}
}


sub show_distance($$$$) {
    my ($lon1,$lat1,$lon2,$lat2) = @_;
    my ($cmpdist,$cmphdg);
    my @Pos1 = NESW( $lon1, $lat1);
    my @Pos2 = NESW( $lon2, $lat2);
    my $d_km = great_circle_distance(@Pos1, @Pos2, 6378); # About 9600 km Lon-Tok.
    my $t_degs = rad2deg(great_circle_distance(@Pos1, @Pos2)); # degrees to cover
    my $hdg = rad2deg(great_circle_direction(@Pos2, @Pos1)); # track
    my $rhdg = rad2deg(great_circle_direction(@Pos1, @Pos2)); # track
    my $d_nmiles = $d_km * $Km2NMiles;

    # derived
    my $chdg = int($hdg + 0.05); # only WHOLE degrees
    $chdg = "0$chdg" while (length($chdg) < 3);
    my $crhdg = int($rhdg + 0.5);
    $crhdg = "0$crhdg" while (length($crhdg) < 3);

    my $hrs = $d_km / $g_speed;
    my $eta = get_hhmmss($hrs);
    #my $ikm = int($d_km + 0.5);
    my $ikm = $d_km;
    set_decimal_stg(\$ikm);
    #my $inm = int(($d_nmiles + 0.05) * 10) / 10;
    my $inm = $d_nmiles;
    set_decimal_stg(\$inm);
    #$degs = int($degs + 0.5);
    #$degs = int($degs + 0.5);
    $t_degs = sprintf("%0.6f",$t_degs);
    get_sg_distance_vs_est( $lat1,$lon1,$lat2,$lon2,$d_km,$hdg, \$cmpdist, \$cmphdg );
    if (VERB2()) {
        print "From (lon,lat): $lon1,$lat1 to $lon2,$lat2 is about -\n";
        print "Distance: $ikm kilometers ($d_km) $inm Nm $cmpdist\n";
        print "Heading : $chdg/$crhdg, for $t_degs degs $cmphdg\n";
        print "ETA     : $eta, at $g_ias Knots\n";
    } else {
        print "Dist: $ikm Km, $inm Nm, hdg $crhdg, $eta, at $g_ias\n";
    }
    # print "km $km / spd $g_speed = $hours\n";
    if (VERB5()) {
        show_sg_distance_vs_est($lat1,$lat2,$lon1,$lon2,$d_km,$t_degs);
    }
    # =================================================
    if ($g_interval != $MAD_LL) {
        show_intervals($lat1,$lon1,$lat2,$lon2);
    }
}

sub get_hhmmss($) {
    my ($hours) = @_;
    my $hrs = int($hours);
    my $mins = ($hours * 60) - ($hrs * 60);
    my $min = int($mins);
    my $secs = ($mins * 60) - ($min * 60);
    my $days = 0;
    if ($hrs >= 24) {
        $days++;
        $hrs -= 24;
    }
    $min = ($min < 10) ? "0$min" : $min;
    $secs = int( $secs + 0.5 );
    $secs = ($secs < 10) ? "0$secs" : $secs;
    $hrs = ($hrs < 10) ? "0$hrs" : $hrs;
    my $stg = '';
    $stg .= "Days $days, " if ($days);
    $stg .= "$hrs:$min:$secs";
    return $stg;
}

# ==========================
# ### MAIN ###

parse_args(@ARGV);
show_distance( $g_lon1, $g_lat1, $g_lon2, $g_lat2 );
exit(0);

# ==========================

sub give_help {
    prt("$pgmname: version $VERS\n");
    prt("Usage: $pgmname [options] lat1 lon1 lat2 lon2\n");
    prt("Options:\n");
    prt(" --help (-h or -?) = This help, and exit 0.\n");
    prt(" --inter Num  (-i) = Show intervals between points.\n");
    prt(" --rev        (-r) = Reverse the calculation.\n");
    prt(" --speed      (-s) = Set the speed, in knots.\n");
    prt(" -v[N]             = Bump or set verbosity. def=$verbosity.\n");
    prt("The lat/lon can be input as comma separated pairs.\n");
    prt("The calculation is first done using Math::Trig qw(great_circle_distance ...), and\n");
    prt(" then repeated using a perl rendition of simgear fg_geo_inverse_wgs_84(), and\n");
    prt(" the results are compared. Bumping verbosity will display the SG values.\n");

}

sub need_arg {
    my ($arg,@av) = @_;
    pgm_exit(1,"ERROR: [$arg] must have following argument!\n") if (!@av);
}

sub parse_args {
    my (@av) = @_;
    my ($arg,$sarg,$cnt,@arr1,@arr2);
    my $argcnt = scalar @av;
    my $rev = 0;
    my $xchg = 0;
    $cnt = 0;
    while (@av) {
        $arg = $av[0];
        if ( ($arg =~ /^-/) && !($arg =~ /^-\d+/) ) {
            $sarg = substr($arg,1);
            $sarg = substr($sarg,1) while ($sarg =~ /^-/);
            if (($sarg =~ /^h/i)||($sarg eq '?')) {
                give_help();
                pgm_exit(0,"Help exit(0)");
            } elsif ($sarg =~ /^r/) {
                $rev = 1;
            } elsif ($sarg =~ /^i/) {
                need_arg(@av);
                shift @av;
                $sarg = $av[0];
                if (($sarg =~ /^\d+$/)&&($sarg > 0)&&(int($sarg) == $sarg)) {
                    $g_interval = $sarg; # interval
                    prt("List $g_interval betweeen.\n") if (VERB5());
                } else {
                    pgm_exit(1,"ERROR: Argument [$arg], must be followed by integer number of intervals! Got [$sarg]\n");
                }
            } elsif ($sarg =~ /^s/) {
                need_arg(@av);
                shift @av;
                $sarg = $av[0];
                if ($sarg =~ /^\d+$/) {
                    $g_ias = $sarg; # Knots
                    $g_speed = $g_ias * $K2KPH; # Knots to Kilometers/Hour
                    prt("Set speed to $g_ias Knots.\n") if (VERB5());
                } else {
                    pgm_exit(1,"ERROR: Argument [$arg], must be followed by Number of Knots! Got [$sarg]\n");
                }
            } elsif ($sarg =~ /^v/) {
                if ($sarg =~ /^v(\d+)$/) {
                    $verbosity = $1;
                } else {
                    while ($sarg =~ /^v/) {
                        $verbosity++;
                        $sarg = substr($sarg,1);
                    }
                }
                prt("Set verbosity to [$verbosity].\n");
            } elsif ($sarg =~ /^x/) {
                $xchg = 1;
            } else {
                pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n");
            }
        } else {
            if ($cnt == 0) {
                if ($arg =~ /,/) {
                    @arr1 = split(',',$arg);
                    $g_lat1 = $arr1[0];
                    $g_lon1 = $arr1[1];
                    $cnt++;
                } else {
                    $g_lat1 = $arg; # set LAT1
                    prt("Set lat 1 $g_lat1\n") if (VERB5());
                }
            } elsif ($cnt == 1) {
                $g_lon1 = $arg;     # set LON1
                prt("Set lon 1 $g_lon1\n") if (VERB5());
            } elsif ($cnt == 2) {
                if ($arg =~ /,/) {
                    @arr2 = split(',',$arg);
                    $g_lat2 = $arr2[0];
                    $g_lon2 = $arr2[1];
                    $cnt++;
                } else {
                    $g_lat2 = $arg; # set LAT2
                    prt("Set lat 2 $g_lat2\n") if (VERB5());
                }
            } elsif ($cnt == 3) {
                $g_lon2 = $arg;     # set LON2
                prt("Set lon 2 $g_lon1\n") if (VERB5());
            } else {
                pgm_exit(1,"ERROR: Invalid argument [$arg]! Only max 4 bare args?\n");
            }
            $cnt++;
        }
        shift @av;
    }
    if ($xchg) {
        prt("Echanging lat and lon...\n") if (VERB5());
        my $tmp = $g_lat1;
        $g_lat1 = $g_lon1;
        $g_lon1 = $tmp;
        $tmp = $g_lat2;
        $g_lat2 = $g_lon2;
        $g_lon2 = $tmp;
    }
    if (in_world($g_lat1,$g_lon1) &&
        in_world($g_lat2,$g_lon2) ) {
        if ($rev) {
            my $tmp = $g_lat1;
            $g_lat1 = $g_lat2;
            $g_lat2 = $tmp;
            $tmp = $g_lon1;
            $g_lon1 = $g_lon2;
            $g_lon2 = $tmp;
        }
        prt("Input (lat,lon) $g_lat1,$g_lon1 to $g_lat2,$g_lon2\n") if (VERB5());
    } else {
        in_world_show($g_lat1,$g_lon1);
        in_world_show($g_lat2,$g_lon2);
        prt("Input ONE $g_lat1,$g_lon1 ");
        prt( (in_world($g_lat1,$g_lon1) ? "ok" : "NIW!") );
        prt(" TWO $g_lat2,$g_lon2 ");
        prt( (in_world($g_lat2,$g_lon2) ? "ok" : "NIW!") );
        prt("\n");
        pgm_exit(1,"ERROR: Invalid input! Need 4 bare args lat1 lon1 lat2 lon2 -?\n");
    }
}

# eof - distance02.pl
