#!/usr/bin/perl -w
# NAME: get-bearing.pl
# AIM: Just some tests getting the bearing between two lat,lon points
use strict;
use warnings;
use File::Basename;  # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] )
use Math::Trig;
use Math::Trig qw(great_circle_distance great_circle_direction deg2rad rad2deg);
use Cwd;
my $os = $^O;
my $perl_dir = '/home/geoff/bin';
my $PATH_SEP = '/';
my $temp_dir = '/tmp';
if ($os =~ /win/i) {
    $perl_dir = 'C:\GTools\perl';
    $temp_dir = $perl_dir;
    $PATH_SEP = "\\";
}
unshift(@INC, $perl_dir);
require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl' Check paths in \@INC...\n";
require 'fg_wsg84.pl' or die "Unable to load fg_wsg84.pl ...\n";
# log file stuff
our ($LF);
my $pgmname = $0;
if ($pgmname =~ /(\\|\/)/) {
    my @tmpsp = split(/(\\|\/)/,$pgmname);
    $pgmname = $tmpsp[-1];
}
my $outfile = $temp_dir.$PATH_SEP."temp.$pgmname.txt";
open_log($outfile);

# user variables
my $VERS = "0.0.2 2014-01-13";
my $load_log = 0;
my $in_file = '';
my $verbosity = 0;
my $out_file = '';
my $bad_ll = 400;
my $usr_lat1 = $bad_ll;
my $usr_lon1 = $bad_ll;
my $usr_lat2 = $bad_ll;
my $usr_lon2 = $bad_ll;

my $SG_METER_TO_NM = 0.0005399568034557235;
my $pi = atan2(1,1) * 4;
my $pi2 = $pi + $pi;
my $d2r = $pi / 180;
my $r2d = 180 / $pi;
my $R = 6371; # earth's mean radius in km
my $Rm = 6370137;

my $Latitude1deg = 110.54;  # km
my $Longitude1deg = 111.320; # *cos(latitude) km
# my $D2M = 111000;   # m

# ### DEBUG ###
my $debug_on = 0;
my $def_file = 'def_file';
my $startlat = 43.6822;
my $startlon = -70.450769;
my $endlat = 43.682211;
my $endlon = -70.45070;

### program variables
my @warnings = ();
my $cwd = cwd();

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

sub show_warnings($) {
    my ($val) = @_;
    if (@warnings) {
        prt( "\nGot ".scalar @warnings." WARNINGS...\n" );
        foreach my $itm (@warnings) {
           prt("$itm\n");
        }
        prt("\n");
    } else {
        prt( "\nNo warnings issued.\n\n" ) if (VERB9());
    }
}

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 prtw($) {
   my ($tx) = shift;
   $tx =~ s/\n$//;
   prt("$tx\n");
   push(@warnings,$tx);
}

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

sub process_in_file($) {
    my ($inf) = @_;
    if (! open INF, "<$inf") {
        pgm_exit(1,"ERROR: Unable to open file [$inf]\n"); 
    }
    my @lines = <INF>;
    close INF;
    my $lncnt = scalar @lines;
    prt("Processing $lncnt lines, from [$inf]...\n");
    my ($line,$inc,$lnn);
    $lnn = 0;
    foreach $line (@lines) {
        chomp $line;
        $lnn++;
        if ($line =~ /\s*#\s*include\s+(.+)$/) {
            $inc = $1;
            prt("$lnn: $inc\n");
        }
    }
}

sub distHaversine($$$$) {
    my ($lat1, $lon1, $lat2, $lon2) = @_;
    my $dLat = $d2r * ($lat2-$lat1);
    my $dLon = $d2r * ($lon2-$lon1);
    $lat1 = $d2r * $lat1;
    $lat2 = $d2r * $lat2;
    my $a = sin($dLat/2) * sin($dLat/2) +
        cos($lat1) * cos($lat2) * sin($dLon/2) * sin($dLon/2);
    my $c = 2 * atan2(sqrt($a), sqrt(1-$a));
    my $d = $R * $c;
    return $d;  # m
}

sub getBearing($$$$) {
    my ($Lat1,$Lon1,$Lat2,$Lon2) = @_;
    #// convert to radians
    my $startLat = $Lat1 * $d2r;
    my $startLon = $Lon1 * $d2r;
    my $endLat   = $Lat2 * $d2r;
    my $endLon   = $Lon2 * $d2r;
    my $dLon = $endLon - $startLon;
    my $dPhi = log(tan($endLat/2.0+$pi/4.0)/tan($startLat/2.0+$pi/4.0));
    if (abs($dLon) > $pi) {
        if ($dLon > 0.0) {
            $dLon = -($pi2 - $dLon);
        } else {
            $dLon =  ($pi2 + $dLon);
        }
    }
    my $d = atan2($dLon, $dPhi) * $r2d;
    return ($d + 360.0) % 360.0;
}

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

my $D2M = 111000;   # m
sub porkypigBearing($$$$) {
    my ($lat1,$lon1,$lat2,$lon2) = @_;
    my $diflat = ($lat1 - $lat2) * $D2M;
    my $diflon = ($lon1 - $lon2) * $D2M * cos( ($lat1 + $lat2) / 2 );
    #my $b = rad2deg(atan( deg2rad($diflon) / deg2rad($diflat) ));
    my $b = $r2d * atan( ($d2r * $diflon) / ($d2r * $diflat) );

    # return int($b);
    return $b;
}

sub porkypigBearing2($$$$) {
    my ($lat1,$lon1,$lat2,$lon2) = @_;
    my $diflat = ($lat2 - $lat1);
    my $diflon = ($lon2 - $lon1);
    my $b = atan( $diflon / $diflat );
    if (abs($diflon) > $pi) {
        if ($b > 0.0) {
            $b = -($pi2 - $b);
        } else {
            $b =  ($pi2 + $b);
        }
    }
    return int($b);
}


sub process_inputs() {
    my ($az1, $az2, $s, $nm);
    fg_geo_inverse_wgs_84( $usr_lat1, $usr_lon1, $usr_lat2, $usr_lon2, \$az1, \$az2, \$s );
    prt("Inputs start [$usr_lat1,$usr_lon1] to [$usr_lat2,$usr_lon2]\n");
    $nm = $s * $SG_METER_TO_NM;
    prt("SG Distance $s meters, $nm nm\n"); 
    prt("SG Bearing $az1, reciprocal $az2\n");
    if (VERB1()) {
        my ($s1,$s2,$s3,$s4);
        fg_geo_inverse_wgs_84( $usr_lat1, $usr_lon1, $usr_lat1, $usr_lon2, \$az1, \$az2, \$s1 );
        fg_geo_inverse_wgs_84( $usr_lat1, $usr_lon2, $usr_lat2, $usr_lon2, \$az1, \$az2, \$s2 );
        fg_geo_inverse_wgs_84( $usr_lat2, $usr_lon2, $usr_lat2, $usr_lon1, \$az1, \$az2, \$s3 );
        fg_geo_inverse_wgs_84( $usr_lat2, $usr_lon1, $usr_lat1, $usr_lon1, \$az1, \$az2, \$s4 );
        prt("SG Rectangle $s1 x $s2 x $s3 x $s4\n");
    }
    my @Pos1 = NESW( $usr_lat1, $usr_lon1 );
    my @Pos2 = NESW( $usr_lat2, $usr_lon2 );
    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
    $az1 = rad2deg(great_circle_direction(@Pos1, @Pos2)); # track
    $az2 = rad2deg(great_circle_direction(@Pos2, @Pos1)); # track
    $s = $d_km * 1000;
    $nm = $s * $SG_METER_TO_NM;
    prt("GC Distance $s meters, $nm nm\n"); 
    prt("GC Bearing $az1, reciprocal $az2\n");
    $s = distHaversine($usr_lat1, $usr_lon1, $usr_lat2, $usr_lon2) * 1000;
    $nm = $s * $SG_METER_TO_NM;
    $az1 = getBearing($usr_lat1, $usr_lon1, $usr_lat2, $usr_lon2);
    $az2 = ($az1 + 180) % 360;
    prt("HS Distance $s meters, $nm nm\n"); 
    prt("GB Bearing $az1, reciprocal $az2\n");

    $az1 = porkypigBearing($usr_lat1, $usr_lon1, $usr_lat2, $usr_lon2);
    $az2 = porkypigBearing2($usr_lat1, $usr_lon1, $usr_lat2, $usr_lon2);
    prt("PP Bearing $az1, PP2 $az2\n");

}

#########################################
### MAIN ###
parse_args(@ARGV);
process_inputs();
pgm_exit(0,"");
########################################

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

sub parse_args {
    my (@av) = @_;
    my ($arg,$sarg,$cnt,@arr,$len);
    my $verb = VERB2();
    $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 =~ /^v/) {
                if ($sarg =~ /^v.*(\d+)$/) {
                    $verbosity = $1;
                } else {
                    while ($sarg =~ /^v/) {
                        $verbosity++;
                        $sarg = substr($sarg,1);
                    }
                }
                $verb = VERB2();
                prt("Verbosity = $verbosity\n") if ($verb);
            } elsif ($sarg =~ /^l/) {
                if ($sarg =~ /^ll/) {
                    $load_log = 2;
                } else {
                    $load_log = 1;
                }
                prt("Set to load log at end. ($load_log)\n") if ($verb);
            } elsif ($sarg =~ /^o/) {
                need_arg(@av);
                shift @av;
                $sarg = $av[0];
                $out_file = $sarg;
                prt("Set out file to [$out_file].\n") if ($verb);
            } else {
                pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n");
            }
        } else {
            if ($cnt == 0) {
                @arr = split(";",$arg);
                $len = scalar @arr;
                if ($len == 1) {
                    $usr_lat1 = $arg;
                } elsif ($len == 2) {
                    $usr_lat1 = $arr[0];
                    $usr_lon1 = $arr[1];
                    $cnt++;
                } else {
                    pgm_exit(1,"Invalid lat,lon pair [$arg]\n");
                }

            } elsif ($cnt == 1) {
                $usr_lon1 = $arg;
            } elsif ($cnt == 2) {
                @arr = split(";",$arg);
                $len = scalar @arr;
                if ($len == 1) {
                    $usr_lat2 = $arg;
                } elsif ($len == 2) {
                    $usr_lat2 = $arr[0];
                    $usr_lon2 = $arr[1];
                    $cnt++;
                } else {
                    pgm_exit(1,"Invalid lat,lon pair [$arg]\n");
                }
            } elsif ($cnt == 3) {
                $usr_lon2 = $arg;
            } else {
                pgm_exit(1,"Already have start [$usr_lat1,$usr_lon1] to [$usr_lat2,$usr_lon2] What is this [$arg]!\n");
            }
            $cnt++;
        }
        shift @av;
    }

    if ($debug_on) {
        prtw("WARNING: DEBUG is ON!\n");
        my $set = 0;
        if ($usr_lat1 == $bad_ll) {
            $usr_lat1 = $startlat;
            $set |= 1;
        }
        if ($usr_lon1 == $bad_ll) {
            $usr_lon1 = $startlon;
            $set |= 2;
        }
        if ($usr_lat2 == $bad_ll) {
            $usr_lat2 = $endlat;
            $set |= 4;
        }
        if ($usr_lon2 == $bad_ll) {
            $usr_lon2 = $endlon;
            $set |= 8;
        }
        if ($set) {
            prt("Set DEFAULT input start [$usr_lat1,$usr_lon1] to [$usr_lat2,$usr_lon2]\n");
        }
    }
    if ($usr_lat1 == $bad_ll) {
        pgm_exit(1,"Did not get a start lat,lon and an end lat,lon!\n");
    }
    if ($usr_lon1 == $bad_ll) {
        pgm_exit(1,"Did not get a start lat,lon and an end lat,lon!\n");
    }
    if ($usr_lat2 == $bad_ll) {
        pgm_exit(1,"Did not get a start lat,lon and an end lat,lon!\n");
    }
    if ($usr_lon2 == $bad_ll) {
        pgm_exit(1,"Did not get a start lat,lon and an end lat,lon!\n");
    }

    if (!in_world_range($usr_lat1,$usr_lon1) ||
        !in_world_range($usr_lat2,$usr_lon2)) {
        pgm_exit(1,"Inputs start [$usr_lat1,$usr_lon1] to [$usr_lat2,$usr_lon2] NOT in world range\n");
    }
}

sub give_help {
    prt("$pgmname: version $VERS\n");
    prt("Usage: $pgmname [options] lat,lon lat,lon\n");
    prt("Options:\n");
    prt(" --help  (-h or -?) = This help, and exit 0.\n");
    prt(" --verb[n]     (-v) = Bump [or set] verbosity. def=$verbosity\n");
    prt(" --load        (-l) = Load LOG at end. ($outfile)\n");
    prt(" --out <file>  (-o) = Write output to this file.\n");
    prt(" Given a start lat,lon and an end lat,lon compute distance and bearing...\n");
}

# eof - get-bearing.pl