#!/usr/bin/perl -w
# NAME: test-bearing.pl
# AIM: Just some test on getting a bearing from 2 lat,lon positions
use strict;
use warnings;
use Math::Trig;


my $startlat = 43.6822;
my $startlon = -70.450769;

my $endlat = 43.682211;
my $endlon = -70.45070;

my $DiffLat = 0.000011;
my $distLat = 1.22; # m
my $DiffLon = 0.000069;
my $distLon = 7.68; # m

sub prt($) { print shift; }

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

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;  # km
}

sub distPythag($$$$) {
    my ($x1, $y1, $x2, $y2) = @_;
	my $d = sqrt((($x2 - $x1) ** 2) + (($y2 - $y1) ** 2));
    return $d * 110.54; # km
}


sub showBearing($$$$) {
    my ($lat1,$lon1,$lat2,$lon2) = @_;
    my ($alat,$alon,$dlat,$dlon);
    my ($angl1,$angl2,$angl3);

    my $distH = 1000 * distHaversine($lat1,$lon1,$lat2,$lon2);
    my $distP = 1000 * distPythag($lat1,$lon1,$lat2,$lon2);
    $alat = sprintf("%.6f",$distH);
    $alon = sprintf("%.6f",$distP);

    prt("distH $alat ($distH), distP $alon ($distP)\n");

    my $diflat = $lat1 - $lat2;
    my $diflon = $lon1 - $lon2;
    my $dlat2m = 1.22 / ($diflat * 1000);
    my $dlon2m = 7.68 / ($diflon * 1000);

    $alat = sprintf("%.6f",$diflat);
    $alon = sprintf("%.6f",$diflon);
    $dlat = $diflat * $Latitude1deg * 1000;
    $dlon = $diflon * $Latitude1deg * 1000;
    prt("dif lat $alat ($diflat) dlat $dlat  dlat2m $dlat2m\n");
    prt("dif lon $alon ($diflon) dlon $dlon  dlon2m $dlon2m\n");
    $angl1 = atan( $dlon / $dlat );
    $angl2 = atan( $dlat / $dlon );
    $angl3 = atan( 7.68 / 1.22 );
    ### prt("angle $angl1 $angl2 $angl3\n");
    $angl1 = atan( ($d2r * $dlon) / ($d2r * $dlat) );
    $angl2 = atan( ($d2r * $dlat) / ($d2r * $dlon) );
    $angl3 = atan( ($d2r * 7.68) / ($d2r * 1.22) );
    ### prt("angle $angl1 $angl2 $angl3\n");
    $angl1 = $r2d * atan( ($d2r * $dlon) / ($d2r * $dlat) );
    ### $angl2 = $r2d * atan( ($d2r * $dlat) / ($d2r * $dlon) );
    $angl3 = $r2d * atan( ($d2r * 7.68) / ($d2r * 1.22) );
    ### prt("angle $angl1 $angl2 $angl3\n");
    prt("angle $angl1 $angl3\n");

}

my $D2M = 111000;   # m
sub porkypigBearing($$$$) {
    my ($lat1,$lon1,$lat2,$lon2) = @_;
    my $diflat = ($lat1 - $lat2) * $D2M;
    my $diflon = ($lon1 - $lon2) * $D2M;
    my $b = $r2d * atan( ($d2r * $diflon) / ($d2r * $diflat) );
    return int($b);
}

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;
}


#showBearing($startlat,$startlon,$endlat,$endlon);
my $b1 = getBearing($startlat,$startlon,$endlat,$endlon);
prt("From $startlat,$startlon to $endlat,$endlon is $b1 degrees\n");
my $b2 = porkypigBearing($startlat,$startlon,$endlat,$endlon);
prt("From $startlat,$startlon to $endlat,$endlon is $b2 degrees\n");

# eof