#!/usr/bin/perl -w
# NAME: fg_f14b.pl
# AIM: Add auto pilot to f14b - first attempt
# 24/09/2011 geoff mclane http://geoffair.net/mperl
use strict;
use warnings;
use File::Basename;  # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] )
use Cwd;
use Term::ReadKey;
use Time::HiRes qw( usleep gettimeofday tv_interval );
use GeoCoord;
# ==================================
my $perl_dir = 'C:\GTools\perl';
unshift(@INC, $perl_dir);
require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl' Check paths in \@INC...\n";
require 'lib_fgio.pl' or die "Unable to load 'lib_fgio.pl'! Chech paths in \@INC...\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_dir."\\temp.$pgmname.txt";
open_log($outfile);

# user variables
my $VERS = "0.0.1 2011-09-26";
my $load_log = 0;
my $in_file = '';
my $verbosity = 0;
my $debug_on = 0;
my $def_file = 'def_file';

# constants
my $MPS2KNOT = 1.9438444924406; # Knots 

# defaults
my $HOST = "192.168.1.105"; # Dell02 machine
#my $HOST = "localhost";
my $PORT = 5555;
my $TIMEOUT = 2;  # second to wait for a connect.

my $auto_state = "/auto/state";
my $uas_state = "/uas/state";
my $last_state = "";
my $last_time = time();

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

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

sub pgm_exit($$) {
    my ($val,$msg) = @_;
    fgfs_disconnect();
    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 prtt($) {
    my $txt = shift;
    prt(lu_get_hhmmss_UTC(time()).": $txt");
}

sub local_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 key_help() {
    prt("Keyboard HELP\n");
    prt(" ESC = Exit main loop.\n");
    prt(" ?   = This help output.\n");
    prt(" a   = Output A/P Locks.\n");
    prt(" e   = Show engines.\n");
    prt(" g   = Show gear.\n");
    prt(" i   = Get all info.\n");

}

sub show_aplocks() {
    show_ap_locks(fgfs_get_ap_locks());
}

my $last_pos_time = 0;
my $delay_secs = 5 - 1; # each 5 seconds

sub show_position() {
    my $rlp = get_curr_posit();
    if (!defined ${$rlp}{'time'}) {
        $rlp = fgfs_get_position();
        return if (!defined ${$rlp}{'time'});
    }
    my $tm = time();
    if ($tm > $last_pos_time) {
        $last_pos_time = $tm + $delay_secs;
        # get previous values
        my ($llat,$llon);
        $llon  = ${$rlp}{'lon'};
        $llat  = ${$rlp}{'lat'};
        my $tb = ${$rlp}{'gettimeofday'};
        # get NEW values
        my $rp = fgfs_get_position();
        my $tnow = [gettimeofday];
        my $elap_secs = tv_interval( $tb, $tnow );
        my $ctm = lu_get_hhmmss_UTC($tm);
        my ($lon,$lat,$alt,$hdg,$agl,$hb,$mag,$aspd,$gspd,$cpos);
        $lon  = ${$rp}{'lon'};
        $lat  = ${$rp}{'lat'};
        $alt  = ${$rp}{'alt'};
        $hdg  = ${$rp}{'hdg'};
        $agl  = ${$rp}{'agl'};
        $hb   = ${$rp}{'bug'};
        $mag  = ${$rp}{'mag'};
        $aspd = ${$rp}{'aspd'}; # Knots
        $gspd = ${$rp}{'gspd'}; # Knots
        my ($az1,$az2,$dist,$mps,$speed,$secs);
        # calculate distance travelled in meters since last
        fg_geo_inverse_wgs_84 ($llat,$llon,$lat,$lon,\$az1,\$az2,\$dist);
        $mps = 0;
        $speed = 0;
        if ($elap_secs > 0) {
            $mps = ($dist / $elap_secs);
            $speed = $mps * $MPS2KNOT;
        }
        # note values DESTROYED for display
        # =================================
        set_int_stg(\$alt);
        set_lat_stg(\$lat);
        set_lon_stg(\$lon);
        set_int_stg(\$speed);
        set_int_stg(\$aspd);
        set_int_stg(\$gspd);
        $elap_secs = (int(($elap_secs * 100)+0.5) / 100);
        $secs = sprintf("%4.2f",$elap_secs);
        $speed = sprintf("%4d",$speed);
        $cpos = "$lat,$lon,$alt $speed $secs $aspd $gspd";
        prtt("$cpos\n");
        if (${$rp}{'gps-update'} == 1) { # maybe in display, show difference, if ANY...
            my $rg = get_curr_gps();
            if (defined ${$rg}{'time'}) {
                ${$rp}{'gps-update'} = 0;
                my $gctm = lu_get_hhmmss_UTC(${$rg}{'time'});
                my $glon = ${$rg}{'lon'};
                my $glat = ${$rg}{'lat'};
                my $galt = ${$rg}{'alt'};
                my $ghdg = ${$rg}{'hdg'};
                # $agl = ${$rp}{'agl'};
                # $hb  = ${$rp}{'bug'};
                my $gmag = ${$rg}{'mag'};
                #fgfs_get_mag_var(\$mag_variation);

                # display mess
                set_lat_stg(\$glat);
                set_lon_stg(\$glon);
                set_hdg_stg(\$ghdg);
                set_hdg_stg(\$gmag);
                set_int_stg(\$galt);
                prt("$gctm: GPS $glat,$glon,$galt ft, hdg=${ghdg}T/${gmag}M \n");
                #show_environ(fgfs_get_environ());
            }
        }
    }
}

my ($m_curr_lon,$m_curr_lat,$m_curr_alt,$m_curr_hdg,$m_curr_tim);

sub set_init_position($$$$) {
    my ($lon,$lat,$alt,$hdg) = @_;
    $m_curr_lon = $lon;
    $m_curr_lat = $lat;
    $m_curr_alt = $alt;
    $m_curr_hdg = $hdg;
    $m_curr_tim = time();
}

sub check_auto_mode() {
    #my $state = getprop($uas_state);
    my $state = getprop($auto_state);
    if (!defined $state || (length($state)==0)) {
        return;
    }
    my ($lon,$lat,$alt,$hdg);
    if ($state ne $last_state) {
        $last_state = $state;
        if ($state eq 'ready') {
            # clear any locks
            setprop("/autopilot/locks/heading","");
            setprop("/autopilot/locks/altitude","");
            setprop("/autopilot/locks/speed","");
            setprop("/controls/gear/brake-parking",1);
            setprop("/controls/engines/engine[0]/throttle",0.0);
            setprop("/controls/engines/engine[1]/throttle",0.0);
            setprop("/controls/engines/engine[2]/throttle",0.0);
            setprop("/controls/engines/engine[3]/throttle",0.0);
            setprop("/controls/flight/elevator",0.0);
            setprop("/controls/flight/elevator-trim",0.0);
        } elsif ($state eq "init") {
            prt("Initialise autoflight system\n");
            setprop("/controls/gear/brake-parking",1);
            my $rc = fgfs_get_position();
            $lon = ${$rc}{'lon'};
            $lat = ${$rc}{'lat'};
            $alt = ${$rc}{'alt'};
            $hdg = ${$rc}{'hdg'};
            #$hdg = getprop("/orientation/heading-deg");
            set_init_position($lon,$lat,$alt,$hdg);

        } elsif ($state eq "init-settle") {
            my $talt = 
            setprop("/autopilot/settings/target-bank-deg",0);
            setprop("/autopilot/locks/heading","bank-hold");
            setprop("/autopilot/locks/altitude","altitude-hold");
        } elsif ($state eq "pretakoff") {
        } elsif ($state eq "climbout") {
        } elsif ($state eq "circle") {
            my $hxDP=getprop("/uas/flight-altitude-ft");
            my $talt=getprop("/autopilot/settings/target-altitude-ft");
            $lon = getprop("/sim/input/click/longitude-deg");
            $lat = getprop("/sim/input/click/latitude-deg");
            $alt = getprop("/sim/input/click/elevation-ft");
            #var OTy8=geo.aircraft_position();
            #var b9oZ=OTy8.course_to(Iqvt);
            #var RwYr=OTy8.distance_to(Iqvt);
            #var WJDx=b9oZ+90;
            #if(WJDx>360.0){WJDx-=360.0;}
            #var AATK=(weRf*weRf)/(11.23*math.tan(0.01745*XJtG));
            #var yOmS=AATK*FT2M;
            #var jRnP=WJDx;
            #if(RwYr<yOmS){
                #var vjvt=90*(1.0-RwYr/yOmS);
                #jRnP+=vjvt;
                #if(jRnP>360.0){ jRnP-=360.0; }
            #}elsif(RwYr>yOmS){
                #var Cclx=RwYr-yOmS;
                #if(Cclx>yOmS){ Cclx=yOmS; }
                #var vjvt=90*Cclx/yOmS;
                #jRnP-=vjvt;
                #if(jRnP<0.0){ jRnP+=360.0;}
            #}
            my $f8Ep=getprop("/instrumentation/airspeed-indicator/true-speed-kt");
            #var Qxt9=VZeb(jRnP,f8Ep,0);
            my $head=getprop("/autopilot/settings/true-heading-deg"); #,Qxt9.heading);
            my $tspd=getprop("/autopilot/settings/target-speed-kt");    #,weRf);
            #lOnA(weRf,5);
            my $zhU7=getprop("/velocities/airspeed-kt");
            #if(zhU7<=GQOf){
            #    setprop("/controls/flight/flaps",1);
            #    setprop("/controls/flight/flapscommand",1);
            #}else if(zhU7>GQOf+10){
            #    setprop("/controls/flight/flaps",0);
            #    setprop("/controls/flight/flapscommand",0);
            #}
            prt("NEW $state: $hxDP $talt $lon $lat $alt $f8Ep $head $tspd $zhU7\n");

        } elsif ($state eq "gohome") {
        } elsif ($state eq "downwind") {
        } elsif ($state eq "base") {
        } elsif ($state eq "final") {
        } elsif ($state eq "flare") {
        } elsif ($state eq "touchdown") {
        }
    } else {
        my $tm = time();
        if ($tm > ($last_time + 4)) {
            if ($state eq 'circle') {
                #my $hxDP=getprop("/uas/flight-altitude-ft");
                #my $talt=getprop("/autopilot/settings/target-altitude-ft");
                $lon = getprop("/sim/input/click/longitude-deg");
                $lat = getprop("/sim/input/click/latitude-deg");
                $alt = getprop("/sim/input/click/elevation-ft");
                my $f8Ep=getprop("/instrumentation/airspeed-indicator/true-speed-kt");
                #var Qxt9=VZeb(jRnP,f8Ep,0);
                my $head=getprop("/autopilot/settings/true-heading-deg"); #,Qxt9.heading);
                my $tspd=getprop("/autopilot/settings/target-speed-kt");    #,weRf);
                #lOnA(weRf,5);
                my $zhU7=getprop("/velocities/airspeed-kt");
                #prt("$state: $hxDP $talt $lon $lat $alt $f8Ep $head $tspd\n");
                prt("$state: $head $lon $lat $alt $f8Ep $tspd\n");
            }
            $last_time = $tm;
        }
    }
}

# ====================================================
sub main_loop() {
    my $ok = 1;
    my $char = '';
    prtt("Entering MAIN loop...\n");
    my ($val,$pmsg);
    # FOREVER, until ESC = exit
    while ($ok) {
        if ( got_keyboard(\$char) ) {
            $val = ord($char);
            $pmsg = sprintf( "%02X", $val );
            if ($val == 27) {
                prt("ESC key... Exiting...\n");
                $ok = 0;
                last;
            } elsif ($char eq '?') {
                key_help();
            } elsif ($char eq 'a') {
                show_aplocks();
            } elsif ($char eq 'e') {
                show_engines();
            } elsif ($char eq 'g') {
                fgfs_show_gear_wow();
            } elsif ($char eq 'i') {
                get_all_info();
            } else {
                prtt("Unused keyboard $val\n");
            }
        }
        show_position();
        check_auto_mode();
    }
    prtt("Exit MAIN loop...\n");
}
# ====================================================

sub get_all_info() {
    prtt("Get 'sim' information...\n");
    show_sim_info(fgfs_get_sim_info());
    prtt("Get Fuel - comsumables...\n");
    show_consumables(fgfs_get_consumables());
    prtt("Getting current environment...\n");
    show_environ(fgfs_get_environ());
    prtt("Getting current COMS...\n");
    show_comms(fgfs_get_comms());
    prtt("Getting current autopilot settings...\n");
    show_autopilot_settings(fgfs_get_aps());
    #my ($alt);
    #fgfs_get_aps_targ_alt(\$alt);
    #prt("Target altitude = $alt");
    #fgfs_get("/autopilot/settings/target-altitude-ft",\$alt);
    #prt(" $alt\n");
    show_aplocks();
    show_engines();
    fgfs_show_gear_wow();
}

sub init() {
    fgfs_connect($HOST, $PORT, $TIMEOUT) ||
        pgm_exit(1,"ERROR: can't open socket! Is FG running, with TELNET enabled?\n");

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

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

    set_wait_ms_get(-1);

    #setprop($auto_state,"ready"); # this will this be created
    get_all_info();

}

#########################################
### MAIN ###
parse_args(@ARGV);
init();
main_loop();
pgm_exit(0,"");
########################################
sub give_help {
    prt("$pgmname: version $VERS\n");
    prt("Usage: $pgmname [options] in-file\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");
}
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);
    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)");
            } elsif ($sarg =~ /v/) {
                if ($sarg =~ /v.*(\d+)$/) {
                    $verbosity = $1;
                } else {
                    while ($sarg =~ /v/) {
                        $verbosity++;
                        $sarg = substr($sarg,1);
                    }
                }
                prt("Verbosity = $verbosity\n") if (VERB1());
            } else {
                pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n");
            }
        } else {
            $in_file = $arg;
            pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n");
        }
        shift @av;
    }
}

# eof - fg_f14b.pl
