#!/usr/bin/perl -w
# NAME: fg_telnet02.pl
# AIM: To RUN FlightGear, and get/send information to it using TELNET
# With much thanks to Franz Melchior for the 'signs' perl script,
# on which this is based.
# 23/06/2016 - Revisit...
# 20/10/2014 - Revisit and add some 'keys'
# 19/04/2011 - Switched to this version 02, which no longer 'runs' the fg
# EXE, and removed the ANSI coloring attempt!
#
# Note, although Term::ReadKey is used to CHECK for any keyboard input,
# and the main_loop() is terminated on an ESC keyboard input, the process
# will NOT exit, due to the nature of fork() and exec() as implemented in WIN32
# The secondary process of fork() will WAIT until exec(FG) exits, and at present it appears
# sending the command "quit" is ignored by FG - found sending 'run exit' worked ;=))
# 10/12/2010 Review - Updated to FG 2.0 (C:\FG\28) - Showed a/c position...
# 13/12/2008 geoff mclane http://geoffair.net/mperl
use strict;
use warnings;
use IO::Socket;
use IO::Select;
use Cwd;
# use Win32::Console::ANSI;   # for WIN32
use Term::ReadKey;
use Time::HiRes qw( usleep gettimeofday tv_interval );
my $perl_sdir = 'C:\GTools\perl';
unshift(@INC, $perl_sdir);
require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl'! Check location and \@INC content.\n";
require 'fg_wsg84.pl' or die "Unable to load fg_wsg84.pl ...\n";
require "Bucket2.pm" or die "Unable to load Bucket2.pm ...\n";
# log file stuff
our ($LF);
my $pgmname = $0;
if ($pgmname =~ /(\\|\/)/) {
    my @tmpsp = split(/(\\|\/)/,$pgmname);
    $pgmname = $tmpsp[-1];
}
my $outfile = $perl_sdir."\\temp.$pgmname.txt";
open_log($outfile);

my $be_quiet = 0;

my $dbg_on = 0;   # force DEBUG VERB5 on
my $cwd = cwd();
my $VERS = "0.0.4 2016-06-23";
###my $VERS = "0.0.3 2014-10-20";

my $HOST = "192.168.1.33"; # Win7-PC
#my $HOST = "192.168.1.105"; # Dell02 machine
#my $HOST = "localhost";
my $PORT = 5557; # 5501;    #5555;
my $TIMEOUT = 2;  # second to wait for a connect.
#my $HOST = "localhost";
#my $PORT = 5555;
#my $TIMEOUT = 5;  # second to wait for a connect.

my $INTERVAL = 1;       # get postion EACH second
my $USECOLOR = 1;
my $MIN_CHANGE = 0.00001;
my $SG_EPSILON = 0.0000001;

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


my $read_handles; # = IO::Select->new(); $read_handles->add($cfg::listen_socket);
my $m_timeout = 50;

my $VERBOSITY = 0;
sub VERB1() { return ($VERBOSITY >= 9); }
sub VERB2() { return ($VERBOSITY >= 9); }
sub VERB5() { return ($VERBOSITY >= 9); }
sub VERB9() { return ($VERBOSITY >= 9); }

my $load_log = 0;
my $test_node_list = 0;

my $HTZ = 5;
my $g_parking_on = 0;

my $t0 = [gettimeofday];
my $done_ll_chg = 0;
my $g_total_dist = 0;
my ($first_lat,$first_lon);
my ($last_lat,$last_lon);
my $g_curr_heading = 0;
my ($g_sg_az1,$g_sg_az2,$g_sg_dist);
my $got_usr_home = 0;
my ($g_home_lat,$g_home_lon);

my $def_home_lat = -33.949273;
my $def_home_lon = 151.181346833333;
my $set_dbg_home = 0;

sub fatal($) {
    my $txt = shift;
	prt($txt);
	exit 1;
}

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

sub get_playback_nodes() {
    my @ppns = qw(
/accelerations/ned/east-accel-fps_sec
/accelerations/ned/north-accel-fps_sec
/accelerations/nlf
/accelerations/pilot/x-accel-fps_sec
/accelerations/pilot/y-accel-fps_sec
/accelerations/pilot/z-accel-fps_sec
/controls/autoflight/altitude-select
/controls/autoflight/autopilot[0]/engage
/controls/autoflight/bank-angle-select
/controls/autoflight/heading-select
/controls/autoflight/speed-select
/controls/autoflight/vertical-speed-select
/controls/electric/APU-generator
/controls/electric/battery-switch
/controls/electric/external-power
/controls/engines/engine[0]/cutoff
/controls/engines/engine[0]/fuel-pump
/controls/engines/engine[0]/ignition
/controls/engines/engine[0]/magnetos
/controls/engines/engine[0]/mixture
/controls/engines/engine[0]/propeller-pitch
/controls/engines/engine[0]/starter
/controls/engines/engine[0]/throttle
/controls/engines/engine[1]/cutoff
/controls/engines/engine[1]/fuel-pump
/controls/engines/engine[1]/ignition
/controls/engines/engine[1]/magnetos
/controls/engines/engine[1]/mixture
/controls/engines/engine[1]/propeller-pitch
/controls/engines/engine[1]/starter
/controls/engines/engine[1]/throttle
/controls/flight/aileron-trim
/controls/flight/aileron[0]
/controls/flight/elevator
/controls/flight/elevator-trim
/controls/flight/flaps
/controls/flight/rudder
/controls/flight/rudder-trim
/controls/flight/slats
/controls/flight/speedbrake
/controls/gear/brake-left
/controls/gear/brake-parking
/controls/gear/brake-right
/controls/gear/gear-down
/controls/gear/steering
/controls/hydraulic/system[0]/electric-pump
/controls/hydraulic/system[0]/engine-pump
/controls/hydraulic/system[1]/electric-pump
/controls/hydraulic/system[1]/engine-pump
/gear/gear/position-norm
/gear/gear[1]/position-norm
/gear/gear[2]/position-norm
/gear/gear[3]/position-norm
/gear/gear[4]/position-norm
/orientation/heading-deg
/orientation/pitch-deg
/orientation/roll-deg
/orientation/side-slip-deg
/position/altitude-ft
/position/latitude-deg
/position/longitude-deg
/surface-positions/elevator-pos-norm[0]
/surface-positions/flap-pos-norm[0]
/surface-positions/left-aileron-pos-norm[0]
/surface-positions/right-aileron-pos-norm[0]
/surface-positions/rudder-pos-norm[0]
/velocities/airspeed-kt
/velocities/glideslope
/velocities/mach
/velocities/speed-down-fps
/velocities/speed-east-fps
/velocities/speed-north-fps
/velocities/uBody-fps
/velocities/vBody-fps
/velocities/vertical-speed-fps
/velocities/wBody-fps
);
    return \@ppns;
}


sub got_keyboard {
    my ($rc) = shift;
    if (defined (my $char = ReadKey(-1)) ) {
		# input was waiting and it was $char
        $$rc = $char;
        return 1;
	}
    return 0;
}

sub my_sleep_secs($) {
    my $secs = shift; # = $INTERVAL
	sleep $secs;    # sampling interval
}

sub secs2minsecs($) {
    my ($secs) = @_;
    my $mins = int($secs / 60);
    $secs = $secs - ($mins * 60);
    $mins = "0$mins" if ($mins < 10);
    $secs = sprintf("%02.2f", $secs);
    # $secs = (int($secs * 100) / 100);
    $secs = "0$secs" if ($secs < 10);
    return "$mins:$secs";
}

sub get_decimal_stg($$$) {
    my ($dec,$il,$dl) = @_;
    my (@arr);
    if ($dec =~ /\./) {
        @arr = split(/\./,$dec);
        if (scalar @arr == 2) {
            $arr[0] = " ".$arr[0] while (length($arr[0]) < $il);
            $dec = $arr[0];
            if ($dl > 0) {
                $dec .= ".";
                $arr[1] = substr($arr[1],0,$dl) if (length($arr[1]) > $dl);
                $dec .= $arr[1];
            }
        }
    } else {
        $dec = " $dec" while (length($dec) < $il);
        if ($dl) {
            $dec .= ".";
            while ($dl--) {
                $dec .= "0";
            }
        }
    }
    return $dec;
}


sub get_heading_stg($) {
    my ($hdg) = @_;
    return get_decimal_stg($hdg,3,1);
}

sub get_sg_dist_stg($) {
    my ($sg_dist) = @_;
    my $sg_km = $sg_dist / 1000;
    my $sg_im = int($sg_dist);
    my $sg_ikm = int($sg_km + 0.5);
    # if (abs($sg_pdist) < $CP_EPSILON)
    my $sg_dist_stg = "";
    if (abs($sg_km) > $SG_EPSILON) { # = 0.0000001; # EQUALS SG_EPSILON 20101121
        if ($sg_ikm && ($sg_km >= 1)) {
            $sg_km = int(($sg_km * 10) + 0.05) / 10;
            $sg_dist_stg .= get_decimal_stg($sg_km,5,1)." km";
        } else {
            #$sg_dist_stg .= "$sg_im m, <1km";
            $sg_dist_stg .= get_decimal_stg($sg_im,7,0)." m.";
        }
    } else {
        #$sg_dist_stg .= "0 m";
        $sg_dist_stg .= get_decimal_stg('0',7,0)." m.";
    }
    return $sg_dist_stg;
}

sub get_dist_stg($$) {
    my ($dlat,$dlon) = @_;
    my ($sg_az1,$sg_az2,$sg_dist);
    my $res = fg_geo_inverse_wgs_84 ($first_lat,$first_lon,$dlat,$dlon,\$g_sg_az1,\$g_sg_az2,\$g_sg_dist);
    $res = fg_geo_inverse_wgs_84 ($last_lat,$last_lon,$dlat,$dlon,\$sg_az1,\$sg_az2,\$sg_dist);
    $g_total_dist += $sg_dist;
    $g_sg_az1 = int(($g_sg_az1 * 10) + 0.05) / 10;
    $g_sg_az2 = $g_sg_az1 + 180;
    $g_sg_az2 -= 360 if ($g_sg_az2 >= 360);
    # $g_sg_az2 = int(($g_sg_az2 * 10) + 0.05) / 10;
    my $sg_km = $sg_dist / 1000;
    my $sg_im = int($sg_dist);
    my $sg_ikm = int($sg_km + 0.5);
    # if (abs($sg_pdist) < $CP_EPSILON)
    my $dist_hdg = " ";
    #my $dist_hdg = " (SGD: ";
    $sg_az1 = int(($sg_az1 * 10) + 0.05) / 10;
    $g_curr_heading = $sg_az1; # only keep to first decimal place
    if (abs($sg_km) > $SG_EPSILON) { # = 0.0000001; # EQUALS SG_EPSILON 20101121
        if ($sg_ikm && ($sg_km >= 1)) {
            $sg_km = int(($sg_km * 10) + 0.05) / 10;
            $dist_hdg .= "$sg_km km";
        } else {
            $dist_hdg .= "$sg_im m, <1km";
        }
    } else {
        $dist_hdg .= "0 m";
    }
    $dist_hdg .= " on $sg_az1";
    # $dist_hdg .= ")";
    return $dist_hdg;
}

sub keyboard_help() {
    prt("Keyboard Help\n");
    prt(" ESC = Exit loop\n");
    prt(" ?   = Repeat of this help.\n");
    prt(" B   = Toggle parking brake ($g_parking_on)\n");
    prt(" a   = Show autopilot status\n");
    prt(" R   = Increase heading bug by 90 degrees\n");
    prt(" L   = Decrease heading bug by 90 degrees\n");
}

sub check_key_sleep($) {
    my $secs = shift;
    my ($char,$val,$pmsg);
    while ($secs > 0) {
        if ( got_keyboard(\$char) ) {
            $val = ord($char);
            $pmsg = sprintf( "%02X", $val );
            return 1 if ($val == 27); # ESC key to EXIT
            if ($char eq '?') {
                keyboard_help();
            } elsif ($char eq 'a') {
                show_autopilot();
            } elsif ($char eq 'B') {
                if ($g_parking_on) {
                    $val = 0;
                    prt("Set Parking OFF\n");
                    $g_parking_on = 0;
                } else {
                    $val = 1;
                    prt("Set Parking ON\n");
                    $g_parking_on = 1;
                }
                set_parking_break($val);
            } elsif ($char eq 'R') {
                fgfs_R_hb();
            } elsif ($char eq 'L') {
                fgfs_L_hb();
            } elsif ($char eq 'r') {
                fgfs_r_hb();
            } elsif ($char eq 'l') {
                fgfs_l_hb();
            } else {
                prt("Got unused keyboard input hex[$pmsg]...\n" );
            }
        }
        usleep(150 * 1000);
        $secs -= 0.2;
    }
}

sub fgfs_get_raw($) {
    return 0 if (!defined $FGFS_IO);
	return 0 if (eof $FGFS_IO);
	my $val = shift;
    my ($res);
    #my $rh = IO::Select->new();
    #$rh->add($FGFS_IO);
    #if ( IO::Select->select($read_handles, undef, undef, $m_timeout ) > 0 ) {
    #if ($rh->can_read(5)) {
        $res = <$FGFS_IO>;
        return 0 if ( ! defined $res );
        $res =~ s/\015?\012$//;
        return 0 if (length($res) == 0);
        $res =~ /^-ERR (.*)/ and (prt("$1") and return 0);
        ${$val} = $res;
        return 1;
    #}
    #return 0;
}

# IO::Select->select($read_handles, undef, undef, $cfg::params{'LISTEN_WAIT'} );
sub get_all_nodes() {
    my ($txt,$cnt,$res,$node,$cmd,$ncnt);
	fgfs_send("ls");
    $txt = '';
    $cnt = 0;
    my @nodes = ();
    while (fgfs_get_raw(\$txt) && (defined $txt) && length($txt)) {
        $cnt++;
        prt("$cnt: $txt\n");
        if ($txt =~ /\/$/) {
            push(@nodes,$txt);
        }
        $txt = '';
        last if ($cnt == 30);
    }
    $ncnt = scalar @nodes;
    prt("Now the $ncnt nodes...\n");
    foreach $node (@nodes) {
        $node =~ s/\/$//;
        $cmd = "cd $node";
        prt("Doing cmd [$cmd]\n");
    	fgfs_send($cmd);
        #if (fgfs_get_raw(\$txt) && (defined $txt) && length($txt)) {
        #    prt("reply: $txt\n");
        #}
    	fgfs_send("ls");
        while (fgfs_get_raw(\$txt) && (defined $txt) && length($txt)) {
            $cnt++;
            prt("$cnt: $txt\n");
            push(@nodes,$txt);
            $txt = '';
            last if ($cnt == 30);
        }
    }
    pgm_exit(1,"Temp exit");
}


sub get_all_nodes_bad() {
    my ($txt,$cnt,$res);
	fgfs_send("ls");
    $txt = '';
    $cnt = 0;
    while ($read_handles->can_read(5)) {
        $res = fgfs_get_raw(\$txt);
    #while ($res && (defined $txt) && length($txt)) {
        if ($res && (defined $txt) && length($txt)) {
            $cnt++;
            prt("$cnt: $txt\n");
            $txt = '';
            #$res = fgfs_get_raw(\$txt);
        } else {
            last;
        }
    }
    pgm_exit(1,"Temp exit");
}

# This is for the beech99-yasim aircraft
sub fgfs_get_ap_ah($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/altitude", $ref) or get_exit(-2); # = true/false
    return 1;
}
sub fgfs_get_ap_gs($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/glide-slope", $ref) or get_exit(-2); # = true/false
    return 1;
}
sub fgfs_get_ap_hh($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/heading", $ref) or get_exit(-2); # = true/false
    return 1;
}
sub fgfs_get_ap_nav($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/nav", $ref) or get_exit(-2); # = true/false
    return 1;
}
sub fgfs_get_ap_pm($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/passive-mode", $ref) or get_exit(-2); # = true/false
    return 1;
}
sub fgfs_get_ap_sh($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/speed", $ref) or get_exit(-2); # = true/false
    return 1;
}
sub fgfs_get_ap_wl($) {
    my $ref = shift;
    fgfs_get("/autopilot/locks/wing-leveler", $ref) or get_exit(-2); # = true/false
    return 1;
}

my $hdg_bug_stg = "/autopilot/settings/heading-bug-deg";
sub fgfs_get_ap_hb($) {
    my $ref = shift;
    fgfs_get($hdg_bug_stg, $ref) or get_exit(-2); # = true/false
    return 1;
}

sub fgfs_R_hb() {
    my ($hb,$msg);
    fgfs_get_ap_hb(\$hb);
    if (defined $hb) {
        if ($hb =~ /^\d+$/) {
            my $nb = $hb + 90;
            $nb -= 360 if ($nb >= 360);
            fgfs_set($hdg_bug_stg, $nb);
            prt("Changed autopilot hdg bug from $hb to $nb\n");
            return;
        } else {
            $msg = "hb not digits '$hb'";
        }
    } else {
        $msg = "fgfs_get_ap_hb failed";
    }
    prt("Failed to get or set autopilot hdg bug! $msg\n");
}
sub fgfs_r_hb() {
    my ($hb,$msg);
    fgfs_get_ap_hb(\$hb);
    if (defined $hb) {
        if ($hb =~ /^\d+$/) {
            my $nb = $hb + 5;
            $nb -= 360 if ($nb >= 360);
            fgfs_set($hdg_bug_stg, $nb);
            prt("Changed autopilot hdg bug from $hb to $nb\n");
            return;
        } else {
            $msg = "hb not digits '$hb'";
        }
    } else {
        $msg = "fgfs_get_ap_hb failed";
    }
    prt("Failed to get or set autopilot hdg bug! $msg\n");
}

sub fgfs_L_hb() {
    my ($hb);
    fgfs_get_ap_hb(\$hb);
    if (defined $hb) {
        if ($hb =~ /^\d+$/) {
            my $nb = $hb - 90;
            $nb += 360 if ($nb < 0);
            fgfs_set($hdg_bug_stg, $nb);
            prt("Changed autopilot hdg bug from $hb to $nb\n");
            return;
        }
    }
    prt("Failed to get or set autopilot hdg bug!\n");
}
sub fgfs_l_hb() {
    my ($hb);
    fgfs_get_ap_hb(\$hb);
    if (defined $hb) {
        if ($hb =~ /^\d+$/) {
            my $nb = $hb - 5;
            $nb += 360 if ($nb < 0);
            fgfs_set($hdg_bug_stg, $nb);
            prt("Changed autopilot hdg bug from $hb to $nb\n");
            return;
        }
    }
    prt("Failed to get or set autopilot hdg bug!\n");
}

my %m_curr_locks = ();
sub get_curr_locks() { return \%m_curr_locks; }


sub fgfs_get_ap_locks() {
    my ($ah,$gs,$hh,$nav,$pm,$sh,$wl,$hb);
    fgfs_get_ap_ah(\$ah);
    fgfs_get_ap_gs(\$gs);
    fgfs_get_ap_hh(\$hh);
    fgfs_get_ap_nav(\$nav);
    fgfs_get_ap_pm(\$pm);
    fgfs_get_ap_sh(\$sh);
    fgfs_get_ap_wl(\$wl);
    fgfs_get_ap_hb(\$hb);
    $ah = 'none' if ((!defined $ah)||(length($ah)==0));
    $gs = 'none' if ((!defined $gs)||(length($gs)==0));
    $hh = 'none' if ((!defined $hh)||(length($hh)==0));
    $nav = 'none' if ((!defined $nav)||(length($nav)==0));
    $pm = 'none' if ((!defined $pm)||(length($pm)==0));
    $sh = 'none' if ((!defined $sh)||(length($sh)==0));
    $wl = 'none' if ((!defined $wl)||(length($wl)==0));
    $hb = 'none' if ((!defined $hb)||(length($hb)==0));
    my $rh = get_curr_locks();
    ${$rh}{'time'} = time();
    ${$rh}{'ah'} = $ah;
    ${$rh}{'gs'} = $gs;
    ${$rh}{'hh'} = $hh;
    ${$rh}{'nav'} = $nav;
    ${$rh}{'pm'} = $pm;
    ${$rh}{'sh'} = $sh;
    ${$rh}{'wl'} = $wl;
    ${$rh}{'hb'} = $hb;
    return $rh;
}

sub show_autopilot() {
    my ($ah,$gs,$hh,$nav,$pm,$sh,$wl,$hb);
    my $rh = fgfs_get_ap_locks();
    $ah = ${$rh}{'ah'};
    $gs = ${$rh}{'gs'};
    $hh = ${$rh}{'hh'};
    $nav = ${$rh}{'nav'};
    $pm = ${$rh}{'pm'};
    $sh = ${$rh}{'sh'};
    $wl = ${$rh}{'wl'};
    $hb = ${$rh}{'hb'};
    prt("autopilot: ");
    prt("ah $ah ") if ($ah ne 'none');
    prt("gs $gs ") if ($gs ne 'none');
    prt("hh $hh ") if ($hh ne 'none');
    prt("nav $nav ") if ($nav ne 'none');
    prt("pm $pm ") if ($pm ne 'none');
    prt("sh $sh ") if ($sh ne 'none');
    prt("wl $wl ") if ($wl ne 'none');
    prt("hb $hb ") if ($hb ne 'none');
    prt("\n");
}

sub main_loop() {
    my ($x, $y, $z);
    my ($px, $py, $pz);
	my ($lon, $lat, $i, $i2);
	my ($oldlon, $oldlat);
    my ($dist, $dtot);
    my ($alt, $agl, $hdg);          # alititude and heading
    my ($pmsg, $amsg);
    my ($char, $val);
    my ($elap, $ms, $sg_dist, $msg);
    my ($ilat,$ilon,$lastilat,$lastilon);
    # If it takes a WHILE for FG to start, use greater than 2 minutes (120 seconds)
    if (VERB5()) {
        prt("[v5] Connecting to $HOST, on $PORT... to $TIMEOUT\n");
    }
	$FGFS_IO = fgfs_connect($HOST, $PORT, $TIMEOUT) ||
        fatal( "\nERROR: Can't open a socket to FG!\n" );

    ReadMode('cbreak'); # not sure this is required, or what it does exactly

    get_all_nodes() if ($test_node_list);

	fgfs_send("data");  # switch exchange to data mode

	fgfs_get_coord(\$oldlon, \$oldlat) or return 0;

    fgfs_get_parking(\$g_parking_on);

    keyboard_help();

    $t0 = [gettimeofday];   # set START TIME
    ($x, $y, $z) = ll2xyz($oldlon, $oldlat);

    prt("Initial: Lat=$oldlat, Lon=$oldlon");
    prt(", xyz=($x,$y,$z)") if (VERB9());
    prt("\n");

    $px = $x;
    $py = $y;
    $pz = $z;
    $dtot = 0;
    $lastilat = 0;
    $lastilon = 0;
    # ### FOREVER ###
    $i = 0;
	while (1) {
        # to exit, just EXIT FG should work
        last if (check_key_sleep($INTERVAL));
        #my_sleep_secs($INTERVAL);
		fgfs_get_coord(\$lon, \$lat) or last;
        $i2 = $i + 1;
        if ((abs($oldlat - $lat) > $MIN_CHANGE)||
            (abs($oldlon - $lon) > $MIN_CHANGE)) {
            fgfs_get_altitude( \$alt );
            fgfs_get_agl( \$agl );
            fgfs_get_heading( \$hdg );

            if (!$done_ll_chg) {
                $done_ll_chg = 1;
                if ($got_usr_home) {
                    $first_lat = $g_home_lat;
                    $first_lon = $g_home_lon;
                } else {
                    $first_lat = $lat;
                    $first_lon = $lon;
                }
                $last_lat  = $lat;
                $last_lon  = $lon;
            }

    		#($x, $y, $z) = ll2xyz($lon, $lat);
            #$dist = sqrt( coord_dist_sq( $px, $py, $pz, $x, $y, $z ) ) * 1000;  # Km??? maybe???
            #$dtot += $dist;
            $elap = tv_interval ( $t0, [gettimeofday]);
            $ms = secs2minsecs($elap);
            $sg_dist = get_dist_stg($lat,$lon); # distance from last lat, lon, accumulated
            #prt("$i2: Lat=$lat, Lon=$lon, xyz=($x,$y,$z) d=$dist, t=$dtot\n" );
            #$dmsg = sprintf( "d=%0.6f, t=%0.6f", $dist, $dtot );
            $ilat = int((abs($lat)+0.0005) * 1000);
            $ilon = int((abs($lon)+0.0005) * 1000);
            if (($ilat == $lastilat)&&($ilon == $lastilon)) {
                $pmsg = "        ,        ";
            } else {
                $pmsg = sprintf( "%3.4f,%4.4f", $lat, $lon );
            }
            $lastilat = $ilat;
            $lastilon = $ilon;
            #$pmsg = sprintf( "lat=%0.8f, lon=%0.8f", $lat, $lon );
            #$amsg = sprintf( "alt=%5d agl=%5d ft", int($alt + 0.5), int($agl + 0.5));
            #$amsg = sprintf( "alt=%5d/%5d ft", int($alt + 0.5), int($agl + 0.5));
            $amsg = sprintf( "%5d/%5d ft", int($alt + 0.5), int($agl + 0.5));
            $msg = "$ms: $pmsg, $amsg";
            # $msg = "$ms: $pmsg, $dmsg, alt=".int($alt + 0.5).", agl=".int($agl + 0.5)." ft";
            # $msg .= " $sg_dist, total ".get_sg_dist_stg($g_total_dist);
            $msg .= " ".get_heading_stg($g_curr_heading);
            $msg .= " ".get_heading_stg($hdg);
            $msg .= ", home ".get_heading_stg($g_sg_az2)." at ".get_sg_dist_stg($g_sg_dist);
            $msg .= ", tot ".get_sg_dist_stg($g_total_dist);
            prt("$msg\n" );
            $oldlat = $lat;
            $oldlon = $lon;
            $last_lat = $lat;
            $last_lon = $lon;
            $px = $x;
            $py = $y;
            $pz = $z;
        }
	}

    ###prt("Sending 'quit' to FG ...\n" );
	#fgfs_send("quit");  # this ONLY closes the interface
	#fgfs_send("\033");  # try an ESC key, did nothing
	###fgfs_send("run exit"); # YAHOO! THAT WORKED!!! PHEW!!!
    ###sleep(5);
    prt("Closing telnet IO ...\n" );
	close $FGFS_IO;
	undef $FGFS_IO;
    ReadMode('normal'); # not sure this is required, or what it does exactly
}

# Blocking => 0
sub fgfs_connect_no_better() {
	my $host = shift;
	my $port = shift;
	my $timeout = (shift || 120);
	my $socket;
    my $nonblocking = 1;
	STDOUT->autoflush(1);
	print "connect $host, $port, timeout $timeout secs ";
	while ($timeout--) {
		if ($socket = IO::Socket::INET->new(
				Proto => 'tcp',
				PeerAddr => $host,
				PeerPort => $port,
                Blocking => 0 )) {
			print ".. done.\n";
			$socket->autoflush(1);
            $socket->blocking(0);
            #ioctl($socket, 0x8004667e, $nonblocking);
            $read_handles = IO::Select->new();
            $read_handles->add($socket);
			sleep 1;
			return $socket;
		}	
		print ".";
		sleep(1);
	}
	return 0;
}

sub fgfs_connect() {
	my $host = shift;
	my $port = shift;
	my $timeout = (shift || 120);
	my $socket;
    my $nonblocking = 1;
	STDOUT->autoflush(1);
	print "connect $host, $port, timeout $timeout secs ";
	while ($timeout--) {
		if ($socket = IO::Socket::INET->new(
				Proto => 'tcp',
				PeerAddr => $host,
				PeerPort => $port)) {
			print ".. done.\n";
			$socket->autoflush(1);
            $socket->blocking(0);
            ioctl($socket, 0x8004667e, \$nonblocking);
			sleep 1;
			return $socket;
		}	
		print ".";
		sleep(1);
	}
	return 0;
}

sub fgfs_send {
	print $FGFS_IO shift, "\015\012";
}

sub fgfs_get_coord($$) {
	my $lon = shift;
	my $lat = shift;
	fgfs_get("/position/longitude-deg", $lon) or exit -2;
	fgfs_get("/position/latitude-deg", $lat) or exit -2;
	return 1;
}

sub fgfs_get_altitude($) {
	my $ref_alt = shift;
	fgfs_get("/position/altitude-ft", $ref_alt) or exit -2;
	return 1;
}

sub fgfs_get_heading($) {
	my $ref_hdg = shift;
	fgfs_get("/orientation/heading-deg", $ref_hdg) or exit -2;
	return 1;
}

sub fgfs_get_agl($) {
	my $ref_alt = shift;
	fgfs_get("/position/altitude-agl-ft", $ref_alt) or exit -2;
	return 1;
}

sub get_parking_node() {
    my $node = "/controls/gear/brake-parking";
    return $node;
}

sub fgfs_get_parking($) {
	my $ref_alt = shift;
    fgfs_get(get_parking_node(),$ref_alt) or exit -2;
	# fgfs_get("/controls/gear/brake-parking", $ref_alt) or exit -2;
	return 1;
}

sub fgfs_set($$) {
    my ($node,$val) = @_;
	fgfs_send("set $node $val");
    return 1;
}

sub set_parking_break($) {
    my $val = shift;
    fgfs_set(get_parking_node(), $val);
}

sub fgfs_get() {
	fgfs_send("get " . shift);
	eof $FGFS_IO and return 0;
	my $val = shift;
	$$val = <$FGFS_IO>;
	$$val =~ s/\015?\012$//;
	$$val =~ /^-ERR (.*)/ and (prt("$1") and return 0);
	return 1;
}

END {
	if (defined $FGFS_IO) {
        prt("$pgmname: Ending ...\n\n" );
		# fgfs_send("run exit");
		close $FGFS_IO;
        undef $FGFS_IO;
	}
    print "\n";
}

sub ll2xyz($$) {
	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($$$) {
	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($$$$$$) {
	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;
}

# ### MAIN ###
# ============================================
sub main() {

    $VERBOSITY = 5 if ($dbg_on);

    parse_args(@ARGV);

    my $dir = cwd();
    prt("Current work directory = $dir\n" );

	main_loop();    # main processing loop

    prt("Returned from main_loop();\n" );

    prt("Closing output LOG [$outfile]...\n" );
    pgm_exit(0,"");

} main;
# ===============================================

sub set_verbosity {
    my (@av) = @_;
    while (@av) {
        my $arg = $av[0];
        if ($arg =~ /^-(v+)$/) {
            $VERBOSITY += length($1);
        } elsif ($arg =~ /^-q$/) {
            $VERBOSITY = 0;
            $be_quiet = 1;
        }
        shift @av;
    }
    prt("Verbosity set to $VERBOSITY\n" );
}

sub chk_arg {
    my ($arg, @av) = @_;
    fatal( "Invalid $arg - needs value ... -? for help ... aborting!\n" ) if !(@av);
}

sub parse_args {
    my (@av) = @_;
    set_verbosity(@av);     # parse only for verbosity
    my ($arg,$len,$sarg);
    my $cnt = 0;
    while (@av) {
        $arg = $av[0];
        $len = length($arg);
        $cnt++;
        prt("$cnt: $arg($len)\n" ) if (VERB9());
        if (($arg =~ /^-h$/)||
            ($arg =~ /^-\?$/)) {
            give_help();
            exit(0);
        } elsif ($arg =~ /^-(v+)$/) {
            # done - $VERBOSITY += length($1);
        } elsif ($arg =~ /^-q$/) {
            # done $VERBOSITY = 0;
        } elsif ($arg =~ /^-/) {
            $sarg = substr($arg,1);
            $sarg = substr($sarg,1) while (substr($sarg,0,1) eq '-');
            if ($sarg =~ /^i/) {
                chk_arg($arg, @av);
                shift @av;
                $sarg = $av[0];
                $HOST = $sarg;
                prt("Set HOST = $HOST...\n") if (VERB1());
            } else {
                prt("Unknown argument! [$arg $sarg] ... -? for help ... aborting ...\n" );
                exit(-1);
            }
        } else {
            prt("Unknown argument! [$arg] ... -? for help ... aborting ...\n" );
            exit(-1);
        }
        shift @av;
    }
    if ($set_dbg_home) {
        if (!$got_usr_home) {
            $got_usr_home = 1;
            $g_home_lat = $def_home_lat;
            $g_home_lon = $def_home_lon;
            prt("Set default home lat,lon $g_home_lat,$g_home_lon\n");
        }
    }
    ##print "aborting ...\n";
    ##exit(0);
}

sub give_help() {
    my $help = <<EOF;
Usage: $pgmname $VERS
   -h or -?          This brief help.
   -v[vvv...]        Add to verbosity level.
   -q                Quiet (verbosity=0).
   -i <ip>           Set HOST IP address. (def=$HOST)
   -p <port>         Set HOST port. (def=$PORT)
EOF
    prt($help);
}

# eof - fg_telnet02.pl