#!/perl -w
# NAME: fg_telnet.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.
#
# 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 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 =~ /\w{1}:\\.*/) {
    my @tmpsp = split(/\\/,$pgmname);
    $pgmname = $tmpsp[-1];
}
my $outfile = $perl_sdir."\\temp.$pgmname.txt";
open_log($outfile);

my $dbg_on = 1;   # force DEBUG on

my $def_fg_root = "C:\\FG\\28\\data";
my $def_fg_rt = 'C:\FG\28\bin';
#my $def_fg_root = "C:\\FG\\27\\data";
#my $def_fg_rt = 'C:\FG\27\bin';
my $def_fg_binary = 'flightgear';

my $cwd = cwd();
my $HOME = $ENV{HOME} || ".";
my $FG_HOME = $ENV{FG_HOME} || $HOME . "/.fgfs";
my $FG_ROOT = $ENV{FG_ROOT} || $def_fg_root;
my $BASEDIR = "$FG_ROOT/Local/signs";

my $FGFS = $def_fg_binary;
my $HOST = "localhost";
my $PORT = 5500;
my $TIMEOUT = 120;  # 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 $CLR_ERR = "41m\033[33;1";
my $CLR_WARN = "47m\033[31;1";
my $CLR_INFO = "32";
my $CLR_BULK = "";
my $CLR_DEBUG = "36;1";
#             $ERR      $WARN      $INFO      $BULK      $DEBUG
my @COLOR = ( $CLR_ERR, $CLR_WARN, $CLR_INFO, $CLR_BULK, $CLR_DEBUG );

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

my $ERR = 0;
my $WARN = 1;
my $INFO = 2;
my $BULK = 3;
my $DEBUG = 4;
my $VERBOSITY = $INFO;

my $help = <<EOF;
Usage: $pgmname
   -h or -?          This brief help.
   -v[vvv...]        Add to verbosity level.
   -q                Quiet (verbosity=0).
   -rt=path          Runtime folder. (Def=$def_fg_rt).
                     NOTE: Will change to this directory to run FG!
   -binary=name      FG binary EXE. (Def=$def_fg_binary).
   -root=path        Set FG root. (Def=$def_fg_root).
   --add-fg-params   Ends internal commands, and this, plus any following, will be passed to FG.
EOF

my $HTZ = 5;
my $DAT_FILE = 'C:\FG\27\bin\records\temp-tn.txt';
my $g_parking_on = 0;

# command arguments to FG
my @fgfsargs = ( "--fg-root=$FG_ROOT",
#    "--aircraft=c182rg",
    "--aircraft=pa24-250",
    "--fdm=yasim",
#    "--atlas=socket,out,5,localhost,5500,udp",
    "--generic=file,out,$HTZ,$DAT_FILE,playback",
    "--fg-scenery=C:/FG/Scenery-1.0.1",
    "--airport=YGIL",
    "--timeofday=noon",
    "--disable-random-objects",
    "--disable-ai-models",
    "--fog-disable",
    "--disable-real-weather-fetch"
    );

my @fgfsargs2 = ( "--fg-root=$FG_ROOT",
    "--aircraft=ufo",
    "--fdm=ufo",
    "--prop:/sim/rendering/fps-display=true",
    "--timeofday=noon",
    "--disable-random-objects",
    "--disable-ai-models",
    "--fog-disable",
    "--disable-real-weather-fetch",
    "--altitude=1000",
    "--lon=-122.33276046",
    "--lat=37.60364931",
    "--heading=297"
    );

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


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 set_verbosity {
    my (@av) = @_;
    while (@av) {
        my $arg = $av[0];
        if ($arg =~ /^-(v+)$/) {
            $VERBOSITY += length($1);
        } elsif ($arg =~ /^-q$/) {
            $VERBOSITY = 0;
        }
        shift @av;
    }
    mylog( $DEBUG, "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 $cnt = 0;
    while (@av) {
        my $arg = $av[0];
        my $len = length($arg);
        $cnt++;
        mylog( $DEBUG, "$cnt: $arg($len)\n" );
        last if (($len > 2)&&(substr($arg,0,2) eq '--'));   # assume this is a FG argument
        if (($arg =~ /^-h$/)||
            ($arg =~ /^-\?$/)) {
            print $help;
            if ($VERBOSITY > $WARN) {
                print "Arguments to FG are :\n";
                print join("\n", @fgfsargs);
            }
            print "\n";
            exit(0);
        } elsif ($arg =~ /^-(v+)$/) {
            # done - $VERBOSITY += length($1);
        } elsif ($arg =~ /^-q$/) {
            # done $VERBOSITY = 0;
        } elsif ($arg =~ /^-rt=(.+)$/) {
            $def_fg_rt = $1;
            mylog( $DEBUG, "Runtime folder to [$def_fg_rt].\n" );
        } elsif ($arg =~ /^-rt$/) {
            chk_arg(@av);
            shift @av;
            $arg = $av[0];
            $def_fg_rt = $arg;
            mylog( $DEBUG, "Runtime folder to [$def_fg_rt].\n" );
        } elsif ($arg =~ /^-binary=(.+)$/) {
            $def_fg_binary = $1;
            mylog( $DEBUG, "FG binary EXE to [$def_fg_binary].\n" );
        } elsif ($arg =~ /^-binary$/) {
            chk_arg(@av);
            shift @av;
            $arg = $av[0];
            $def_fg_binary = $arg;
            mylog( $DEBUG, "FG binary EXE to [$def_fg_binary].\n" );
        } elsif ($arg =~ /^-root=(.+)$/) {
            $def_fg_root = $1;
            mylog( $DEBUG, "FG root to [$def_fg_root].\n" );
        } elsif ($arg =~ /^-root$/) {
            chk_arg(@av);
            shift @av;
            $arg = $av[0];
            $def_fg_root = $arg;
            mylog( $DEBUG, "FG root to [$def_fg_root].\n" );
        } else {
            mylog( $ERR, "Unknown argument! [$arg] ... -? for help ... aborting ...\n" );
            exit(-1);
        }
        shift @av;
    }
    if (@av) {
        mylog( $DEBUG, "Adding ".join(" ",@av)." to exec commands\n" );

        push(@fgfsargs, @av);
    }

    ##print "aborting ...\n";
    ##exit(0);
}

sub main() {

    $VERBOSITY = $DEBUG if ($dbg_on);

    parse_args(@ARGV);

    if (! chdir($def_fg_rt) ) {
        fatal( "ERROR: Unable to change to $def_fg_rt ...\n" );
    }

    my $dir = cwd();
    mylog( $BULK, "Current work directory = $dir\n" );

	if (my $pid = fork) {
		main_loop();    # main processing loop
        mylog( $DEBUG, "Returned from main_loop();\n" );
	} else {
		defined $pid or fatal("cannot fork: $!");
		#exec("$FGFS --telnet=$PORT --config=$BASEDIR/signs.xml @fgfsargs");
		exec("$FGFS --telnet=$PORT @fgfsargs") or
            mylog( $WARN, "Exec FAILED ...\n" );
	}

    mylog( $DEBUG, "Closing output LOG ...\n" );
    close_log($outfile,0);
    mylog( $DEBUG, "Exit, returning zero ...\n" );
    exit(0);

} main;

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($) {
    my $secs = @_; # = $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 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)
	$FGFS_IO = fgfs_connect($HOST, $PORT, $TIMEOUT) || die " can't open socket\n";

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

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

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

    fgfs_get_parking(\$g_parking_on);

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

    mylog( $INFO, "Initial: Lat=$oldlat, Lon=$oldlon, xyz=($x,$y,$z)\n" );

    $px = $x;
    $py = $y;
    $pz = $z;
    $dtot = 0;
    $lastilat = 0;
    $lastilon = 0;
    # ### FOREVER ###
	for ($i = 0;; $i++) {
        # to exit, just EXIT FG should work
        my_sleep($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;
                $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
            #mylog( $BULK, "$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);
            mylog( $INFO, "$msg\n" );
            $oldlat = $lat;
            $oldlon = $lon;
            $last_lat = $lat;
            $last_lon = $lon;
            $px = $x;
            $py = $y;
            $pz = $z;
        }
        if ( got_keyboard(\$char) ) {
            $val = ord($char);
            $pmsg = sprintf( "%02X", $val );
            mylog( $WARN, "Got keyboard input hex[$pmsg]...\n" );
            last if ($val == 27); # ESC key to EXIT
            if ($char eq 'B') {
                if ($g_parking_on) {
                    $val = 0;
                    mylog($WARN,"Set Parking OFF\n");
                    $g_parking_on = 0;
                } else {
                    $val = 1;
                    mylog($WARN,"Set Parking ON\n");
                    $g_parking_on = 1;
                }
                set_parking_break($val);
            }
        }
	}

    mylog( $DEBUG, "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);
    mylog( $DEBUG, "Closing telnet IO ...\n" );
	close $FGFS_IO;
	undef $FGFS_IO;
    ReadMode('normal'); # not sure this is required, or what it does exactly
}

sub fgfs_connect() {
	my $host = shift;
	my $port = shift;
	my $timeout = (shift || 120);
	my $socket;
	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);
			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 (mylog($WARN, "$1") and return 0);
	return 1;
}

END {
	if (defined $FGFS_IO) {
        mylog( $WARN, "$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;
}

sub fatal {
	mylog($ERR, "$pgmname: @_");
	exit -1;
}

sub mylog {
    my ($v) = shift;
    my $txt = "@_";
	return if $v > $VERBOSITY;
	$v = 4 if $v > 4;
    my $msg = '';
	$msg .= "\033[$COLOR[$v]m" if $USECOLOR;
	$msg .= "$txt";
	$msg .= "\033[m" if $USECOLOR;
    if (is_ulog_open()) {
		print $LF $txt;
	}
	print $msg;
    #prt( "$msg" );
}

# eof - fg_telnet.pl
