#!/usr/bin/perl -w
# NAME: telnet2fg.pl
# AIM: Attempt a telent (socket) connection to FG, and report the
# aircraft position, if running...
use strict;
use warnings;
use File::Basename;  # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] )
use IO::Socket;
use Cwd;
use Time::HiRes qw( usleep gettimeofday tv_interval );
my $perl_dir = 'C:\GTools\perl';
unshift(@INC, $perl_dir);
require 'logfile.pl' or die "Unable to load logfile.pl ...\n";
# 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);

# user variables
my $load_log = 0;
my $get_playback_nodes = 0;
my $in_file = '';
my $HOST = "localhost";
my $PORT = 5500;
my $TIMEOUT = 5; # 120;  # second to wait for a connect.
my $FGFS_IO;

my $debug_on = 0;
my $def_file = 'def_file';

### program variables
my @warnings = ();
my $cwd = cwd();
my $os = $^O;
my $Tbgn = [gettimeofday];
my ($orglat, $orglon);

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

sub pgm_exit($$) {
    my ($val,$msg) = @_;
    if (length($msg)) {
        $msg .= "\n" if (!($msg =~ /\n$/));
        prt($msg);
    }
    show_warnings($val);
    prt("$pgmname: ran for ".tv_interval( $Tbgn, [gettimeofday] )." seconds...\n");
    close_log($outfile,$load_log);
    exit($val);
}


sub prtw($) {
   my ($tx) = shift;
   $tx =~ s/\n$//;
   prt("$tx\n");
   push(@warnings,$tx);
}

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 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 get_KAP140_nodes() {
    my %locks = (
        'alt-hold' => 'bool',
        'apr-hold' => 'bool',
        'gs-hold' => 'bool',
        'hdg-hold' => 'bool',
        'nav-hold' => 'bool',
        'pitch-arm' => 'int',
        'pitch-axis' => 'bool',
        'pitch-mode' => 'int',
        'rev-hold' => 'bool',
        'roll-arm' => 'int',
        'roll-axis' => 'bool',
        'roll-mode' => 'int'
    );
    my %settings = (
        'auto-pitch-trim' => 'bool',
        'baro-setting-hpa' => 'double',
        'baro-setting-inhg' => 'double',
        'target-alt-ft' => 'double',
        'target-alt-pressure' => 'double',
        'target-intercept-angle' => 'double',
        'target-pressure-rate' => 'double',
        'target-turn-rate' => 'double'
    );

    my %nodes = (
        '/autopilot/KAP140/locks' => \%locks,
        '/autopilot/KAP140/settings' => \%settings
        );
    return \%nodes;
}

######################################################
##### socket stuff ######

sub fgfs_connect($$$) {
	my ($host,$port,$timeout) = @_;
	my $socket;
	STDOUT->autoflush(1);
	print "fg 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);
			sleep 1;
			return $socket;
		}
        if ($timeout) {
    		print ".";
	    	sleep(1);
        }
	}
	print ".. failed.\n";
	return 0;
}


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

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("WARNING: $1 \n")) and return 0;
	return 1;
}


sub fgfs_close($) {
    my ($re) = @_;
	if (defined $FGFS_IO) {
        ##mylog( $WARN, "$pgmname: Ending ...\n\n" );
		fgfs_send("run exit") if ($re);
		close $FGFS_IO;
        undef $FGFS_IO;
	}
}

# addtional commands
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 read_playback_nodes() {
    my $ra = get_playback_nodes();
    my ($node,$val,$cnt,$tb,$te,$elap,$pn);
    $cnt = scalar @{$ra};
    prt("Getting $cnt playback nodes...\n");
    $cnt = 0;
    my %hash = ();
    $tb = [gettimeofday];
    foreach $node (@{$ra}) {
        fgfs_get($node,\$val);
        $cnt++;
        #prt("$cnt: $node = $val\n");
        $hash{$node} = $val;
    }
    $te = [gettimeofday];
    $elap = tv_interval( $tb, $te );
    $pn = $elap / $cnt;
    prt("$cnt nodes, took $elap secs, av $pn per node fetched...\n");
    return \%hash;
}

sub read_autopilot_nodes() {
    my $rh = get_KAP140_nodes();
    my ($root,$rh2,$key,$typ,$val,$path);
    foreach $root (keys %{$rh}) {
        $rh2 = ${$rh}{$root};
        foreach $key (sort keys %{$rh2}) {
            $typ = ${$rh2}{$key};
            $path = $root."/".$key;
            fgfs_get($path,\$val);
            prt("$path = $val ($typ)\n");
        }
    }
}


sub attempt_connection() {
    prt("Attempting connection to [$HOST], on [$PORT], for [$TIMEOUT] seconds...\n");
    my ($rh,$ra,$node,$val);
    if ($FGFS_IO = fgfs_connect($HOST, $PORT, $TIMEOUT)) {
        fgfs_send("data");  # switch exchange to data mode
    	if ( fgfs_get_coord(\$orglon, \$orglat) ) {
            prt("FG report lat $orglat, lon $orglon\n");
            if ($get_playback_nodes) {
                $rh = read_playback_nodes();
                $ra = get_playback_nodes();
                foreach $node (@{$ra}) {
                    $val = ${$rh}{$node};
                    prt("$node = $val\n");
                }
            }
            read_autopilot_nodes();
        } else {
            prt("Failed to get lat,lon from connection...\n");
        }
        fgfs_close(0);        
    } else {
        prt("ERROR: Can NOT open socket a socket to [$HOST], on [$PORT]\n");
    }
}

#########################################
### MAIN ###
parse_args(@ARGV);
# prt( "$pgmname: in [$cwd]: Hello, World...\n" );
# process_in_file($in_file);
attempt_connection();
pgm_exit(0,"");
########################################
sub give_help {
    prt("$pgmname: version 0.0.1 2010-09-11\n");
    prt("Usage: $pgmname [options] in-file\n");
    prt("Options:\n");
    prt(" --help (-h or -?) = This help, and exit 0.\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);
    while (@av) {
        $arg = $av[0];
        if ($arg =~ /^-/) {
            $sarg = substr($arg,1);
            $sarg = substr($sarg,1) while ($sarg =~ /^-/);
            if (($sarg =~ /^h/i)||($sarg eq '?')) {
                give_help();
                pgm_exit(0,"Help exit(0)");
            } else {
                pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n");
            }
        } else {
            $in_file = $arg;
            prt("Set input to [$in_file]\n");
        }
        shift @av;
    }

#    if ((length($in_file) ==  0) && $debug_on) {
#        $in_file = $def_file;
#    }
#    if (length($in_file) ==  0) {
#        pgm_exit(1,"ERROR: No input files found in command!\n");
#    }
#    if (! -f $in_file) {
#        pgm_exit(1,"ERROR: Unable to find in file [$in_file]! Check name, location...\n");
#    }
}

# eof - template.pl
