#!/usr/bin/perl -w
# NAME: pbcsv2xg.pl
# AIM: Take a protocol playback csv, and extract lat,lon,alt to an xg file
# Uses items per 'D:\FG\fg-64\install\FlightGear\fgdata\Protocol\playback.xml';
# Run fgfs with "--generic=file,out,20,%TMPFIL%,playback"
# The CSV is 77 fields
# Use showplayback.pl to enumerate the in/out fields, and the property being sampled
# 15/07/2016 - Reduce noise in kml file
# 16/06/2016 - Add BBOX bounding box option
###############################################################################

use strict;
use warnings;
use File::Basename;  # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] )
use Cwd;
my $os = $^O;
my $perl_dir = '/home/geoff/bin';
my $PATH_SEP = '/';
my $temp_dir = '/tmp';
if ($os =~ /win/i) {
    $perl_dir = 'C:\GTools\perl';
    $temp_dir = $perl_dir;
    $PATH_SEP = "\\";
}
unshift(@INC, $perl_dir);
require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl' Check paths in \@INC...\n";
require 'fg_wsg84.pl' or die "Unable to load 'lib_utils.pl' Check paths in \@INC...\n";

# log file stuff
our ($LF);
my $pgmname = $0;
if ($pgmname =~ /(\\|\/)/) {
    my @tmpsp = split(/(\\|\/)/,$pgmname);
    $pgmname = $tmpsp[-1];
}
my $outfile = $temp_dir.$PATH_SEP."temp.$pgmname.txt";
open_log($outfile);

# user variables
my $VERS = "0.0.5 2015-01-09";
my $load_log = 0;
my $in_file = '';
my $verbosity = 0;
my $out_file = '';
my $out_xg = $temp_dir.$PATH_SEP."temptrack.xg";
my $max_oow = 10;
my $out_kml = '';   # if KML output required
my $add_xg_bbox = 0;

my $wpt_htz = 20;
my $wpt_set = 10;   # only add each 10th wpt
my $add_placemarks = 0;
my $open_wpts = 0;

# ### DEBUG ###
my $debug_on = 0;
my $def_file = 'D:\FG\fg-64\EGPR-01.csv';
# my $def_file = 'D:\FG\fg-64\tempplay.csv';

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

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

sub pgm_exit($$) {
    my ($val,$msg) = @_;
    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);
}

# if ($add_xg_bbox) {
my $min_lon = 200;
my $max_lon = -200;
my $min_lat = 200;
my $max_lat = -200;
sub get_bbox_xg() {
    my $xg .= "# bbox $min_lon,$min_lat,$max_lon,$max_lat\n";
    $xg .= "color gray\n";
    $xg .= "$min_lon $min_lat\n";
    $xg .= "$min_lon $max_lat\n";
    $xg .= "$max_lon $max_lat\n";
    $xg .= "$max_lon $min_lat\n";
    $xg .= "$min_lon $min_lat\n";
    $xg .= "NEXT\n";
    return $xg;
}

sub sdd_to_bbox($$) {
    my ($lat,$lon) = @_;
    $min_lon = $lon if ($lon < $min_lon);
    $max_lon = $lon if ($lon > $max_lon);
    $min_lat = $lat if ($lat < $min_lat);
    $max_lat = $lat if ($lat > $max_lat);
}

sub in_world_range($$) {
    my ($lat,$lon) = @_;
    return 0 if ($lat < -90);
    return 0 if ($lat >  90);
    return 0 if ($lon < -180);
    return 0 if ($lon >  180);
    sdd_to_bbox($lat,$lon);
    return 1;
}

#################################################
### Collect all waypoints from the csv, and 
### write a kml file

#### UTILITIES
sub get_kml_head() {
    my $txt = <<EOF;
<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2">
EOF
    return $txt;
}

sub get_kml_tail() {
    my $txt = <<EOF;
</kml>
EOF
    return $txt;
}


my $no_wpt_style = 0;
####################################################################
### Add (heavy) Waypoints style
sub get_fgt_head_simple($) {
    my $name = shift;
    my $txt = <<EOF;
<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2">
<Document>
	<name>FGFS Flight #$name</name>
	<open>1</open>
	<Folder>
		<name>Waypoints</name>
		<open>$open_wpts</open>

EOF
    return $txt;
}



sub get_fgt_head($) {
    my $name = shift;
    return get_fgt_head_simple($name) if ($no_wpt_style);
    my $txt = <<EOF;
<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2">
<Document>
	<name>FGFS Flight #$name</name>
	<open>1</open>
	<Style id="Arrival">
      <IconStyle>
        <Icon>
          <href>http://maps.google.com/mapfiles/kml/paddle/A.png</href>
        </Icon>
      </IconStyle>
    </Style>
	<Style id="Departure">
		<IconStyle>
			<Icon>
				<href>http://maps.google.com/mapfiles/kml/paddle/D.png</href>
			</Icon>
		</IconStyle>
    </Style>
	<Style id="Waypoints">
		<IconStyle>
			<Icon>
				<href>http://mpserver15.flightgear.org/modules/fgtracker/icons/reddot.png</href>
			</Icon>
	</IconStyle>
    </Style>
	<Folder>
		<name>Waypoints</name>
		<open>$open_wpts</open>

EOF
    return $txt;
}

sub get_fgt_tail() {
    my $txt = <<EOF;
	</Folder>
</Document>
</kml>
EOF
    return $txt;
}

sub get_fgt_tail2($$$$$$$) {
    my ($list,$lon1,$lat1,$alt1,$lon2,$lat2,$alt2) = @_;
    my $txt = <<EOF;
	</Folder>
	<Placemark>
		<name>Migration path</name>
		<Style>
			<LineStyle>
			<color>ff0000ff</color>
			<width>2</width>
			</LineStyle>
		</Style>
		<LineString>
			<tessellate>1</tessellate>
			<altitudeMode>absolute</altitudeMode>
			<coordinates>
			$list
			</coordinates>
		</LineString>
	</Placemark>
  	<Placemark> 
		<name>Departure</name> 
		<styleUrl>#Departure</styleUrl>
		<Point>
		  <coordinates>$lon1,$lat1,$alt1</coordinates>
		</Point> 
	</Placemark>
	<Placemark> 
		<name>Arrival</name> 
		<styleUrl>#Arrival</styleUrl>
		<Point>
		  <coordinates>$lon2,$lat2,$alt2</coordinates>
		</Point> 
	</Placemark>
</Document>
</kml>
EOF
    return $txt;
}

#  <description>$desc</description>
#            <Point>
#                <coordinates>$lon,$lat,$alt</coordinates>
#            </Point>
#            <name>$nm</name>
sub get_placemark($$$$$) {
    my ($lon,$lat,$alt,$nm,$desc) = @_;
    if ($no_wpt_style) {
        my $pm = <<EOF;
	    <Placemark>
            <description>$desc</description>
            <Point>
                <coordinates>$lon,$lat,$alt</coordinates>
            </Point>
        </Placemark>
EOF
        return $pm;
    }
    my $pm = <<EOF;
		<Placemark>
            <name>$nm</name>
            <description>$desc</description>
			<styleUrl>#Waypoints</styleUrl>
            <Point>
                <coordinates>$lon,$lat,$alt</coordinates>
            </Point>
        </Placemark>
EOF
    return $pm;
}

# Speed: 105 knots</description>
sub get_description($$$) {
    my ($lon,$lat,$alt) = @_;
    my $desc = <<EOF;

			Coordinate: $lon,$lat
			Altitude: $alt ft
EOF
    return $desc;
}

#####################################################
### The main...

sub process_waypoints($$) {
    my ($rwpts,$name) = @_;
    my $len = scalar @{$rwpts};
    if ($len < 3) {
        prt("Found only $len waypoints...\n");
        return;
    }
    my ($ra,$lat1,$lon1,$lat2,$lon2,$res,$s,$dist,$time,$az1,$az2);
    my ($alt1,$alt2,$altm,$nm,$desc);
    my $diff = 1 / $wpt_htz;
    $len = 0;
    $dist = 0;

    $time = 0;
    foreach $ra (@{$rwpts}) {
        $lon2 = ${$ra}[0];
        $lat2 = ${$ra}[1];
        if ($len > 0) {
            $res = fg_geo_inverse_wgs_84($lat1, $lon1, $lat2, $lon2, \$az1, \$az2, \$s);
            $dist += $s;
            $time += $diff;
        }
        $len++;
        $lon1 = $lon2;
        $lat1 = $lat2;
    }
    my $Nms = meter_2_nm($dist);
    my $ddist = int($Nms)." nm";
    my $dtime = secs_HHMMSS($time);
    prt("Found $len waypoints... dist=$ddist, time=$dtime secs\n");
    my $csv = '';
    my $kml = '';
    my $wpts = '';
    my $list = '';
    my $wrap = 0;   # $wpt_set
    $len = 0;
    foreach $ra (@{$rwpts}) {
        $lon1 = ${$ra}[0];
        $lat1 = ${$ra}[1];
        $alt1 = ${$ra}[2];
        $altm = feet_2_meter($alt1);
        $csv .= "$lon1,$lat1,$alt1\n";

        if ($wrap == 0) {
            $len++;
            $nm = "#$len:";
            $desc = get_description($lon1,$lat1,$alt1);
            chomp $desc;
            $wpts .= get_placemark($lon1,$lat1,$altm,$nm,$desc);
            $list .= "$lon1,$lat1,$altm ";
        }

        if ($wpt_set > 0) {
            $wrap++;
            if ($wrap > $wpt_set) {
                $wrap = 0;
            }
        }

    }
    # get beginning
    $ra = ${$rwpts}[0];
    $lon1 = ${$ra}[0];
    $lat1 = ${$ra}[1];
    $alt1 = feet_2_meter(${$ra}[2]);
    # get ending
    $ra = ${$rwpts}[-1];
    $lon2 = ${$ra}[0];
    $lat2 = ${$ra}[1];
    $alt2 = feet_2_meter(${$ra}[2]);
    if ($wrap) {
        $len++;
        $nm = "#$len:";
        $desc = get_description($lon2,$lat2,$alt2);
        chomp $desc;
        $wpts .= get_placemark($lon2,$lat2,$alt2,$nm,$desc);
        $list .= "$lon2,$lat2,$alt2 ";
    }

    if (length($out_kml)) {
        #$kml = get_kml_head().$kml.get_kml_tail();
        #$kml = get_fgt_head().$kml.get_fgt_tail();
        if ($add_placemarks) {
            if ($no_wpt_style) {
                $kml = get_fgt_head($name).$wpts.get_fgt_tail();
            } else {
                $kml = get_fgt_head($name).$wpts.get_fgt_tail2($list,$lon1,$lat1,$alt1,$lon2,$lat2,$alt2);
            }
        } else {
            $kml = get_fgt_head($name).get_fgt_tail2($list,$lon1,$lat1,$alt1,$lon2,$lat2,$alt2);
        }
        write2file($kml,$out_kml);
        prt("KML output written to '$out_kml'\n");
    } else {
        prt("No -o output file name given...\n");
    }
    
}

################################################
# sub fg_geo_inverse_wgs_84 {
#    my ($lat1, $lon1, $lat2, $lon2, $az1, $az2, $s) = @_;

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 ($i,$line,$inc,$lnn,@arr,$len,$lat,$lon,$alt,$lat2,$lon2,$alt2,$cnt);
    my ($res,$az1,$az2,$dist,$hdg,$idist,$cdist,$dcdist);
    $lnn = 0;
    my $disc = 0;
    my $oow = 0;
    $cnt = 0;
    $cdist = 0;
    my $totdist = 0;
    my $xg = '';
    my $added = 0;
    my @wpts = ();
    ############################################
    for ($i = 0; $i < $lncnt; $i++) {
        $line = $lines[$i];
        chomp $line;
        $lnn = $i + 1;
        @arr = split(",",$line);
        $inc = scalar @arr;
        if ($inc < 51) {
            $disc++;
            next;
        }
        $lat = $arr[48];
        $lon = $arr[49];
        $alt = $arr[50];
        if (!in_world_range($lat,$lon)) {
            prtw("WARNING:$lnn: $lat $lon NOT IN WORLD!\n");
            $oow++;
            if ($oow > $max_oow) {
                pgm_exit(1,"Too many out of world values $oow\n");
            }
            $disc++;
            next;
        }
        if ($alt < -9990) {
            $disc++;
            next;
        }
        if ($cnt == 0) {
            $lat2 = $lat;
            $lon2 = $lon;
            $alt2 = $alt;
        }
        $cnt++;
        push(@wpts, [$lon,$lat,$alt]);
        $res = fg_geo_inverse_wgs_84($lat2,$lon2,$lat,$lon,\$az1,\$az2,\$dist);
        $cdist += $dist;
        next if ($cdist < 1);
        $totdist += $cdist;
        $hdg = int($az1 + 0.5);
        $hdg = '0'.$hdg while (length($hdg) < 3);
        $idist = int($totdist);
        #$dcdist = int(($cdist + 0.05) * 10) / 10;
        $dcdist = sprintf("%.1f", ($cdist + 0.05));
        prt("$lnn: $lat, $lon, $alt $hdg $dcdist $idist\n") if (VERB5());
        $xg .= "$lon $lat ; $alt $hdg $dcdist $idist\n";
        $added++;
        #########################
        $lat2 = $lat;
        $lon2 = $lon;
        $alt2 = $alt;
        $cdist = 0;
    }
    prt("Of $lncnt file lines, had $cnt valid records, discarded $disc, added $added to track...\n");
    #$load_log = 1;
    if (length($out_file)) {
        if ($add_xg_bbox) {
            $line = get_bbox_xg();
            $line .= "color yellow\n";
            #$xg = $line.$xg;
            my ($n,$d,$e) = fileparse($out_file, qr/\.[^.]*/ );
            $inc = $d.$n."-bbox".$e;
            rename_2_old_bak($inc);
            write2file($line,$inc);
            prt("xg BBOX written to $inc\n");
        }
        rename_2_old_bak($out_file);
        write2file($xg,$out_file);
        prt("xg track written to $out_file\n");
    } else {
        if ($add_xg_bbox) {
            $line = get_bbox_xg();
            $line .= "color yellow\n";
            $xg = $line.$xg;
        }
        rename_2_old_bak($out_xg);
        write2file($xg,$out_xg);
        prt("xg track written DEFAULT to $out_xg\n");
        prt("Use -o file.xg to set a specific output.\n");
    }
    my ($name,$dir,$ext) = fileparse($inf, qr/\.[^.]*/ );
    #######################################
    process_waypoints(\@wpts,$name);
    #######################################

}

#########################################
### MAIN ###
parse_args(@ARGV);
process_in_file($in_file);
pgm_exit(0,"");
########################################

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);
    my $verb = VERB2();
    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 =~ /^b/) {
                $add_xg_bbox = 1;
                prt("Set to add BBOX to xg output\n") if ($verb);
            } elsif ($sarg =~ /^v/) {
                if ($sarg =~ /^v.*(\d+)$/) {
                    $verbosity = $1;
                } else {
                    while ($sarg =~ /^v/) {
                        $verbosity++;
                        $sarg = substr($sarg,1);
                    }
                }
                $verb = VERB2();
                prt("Verbosity = $verbosity\n") if ($verb);
            } elsif ($sarg =~ /^l/) {
                if ($sarg =~ /^ll/) {
                    $load_log = 2;
                } else {
                    $load_log = 1;
                }
                prt("Set to load log at end. ($load_log)\n") if ($verb);
            } elsif ($sarg =~ /^o/) {
                need_arg(@av);
                shift @av;
                $sarg = $av[0];
                $out_file = $sarg;
                prt("Set out file to [$out_file].\n") if ($verb);
            } elsif ($sarg =~ /^k/) {
                need_arg(@av);
                shift @av;
                $sarg = $av[0];
                $out_kml = $sarg;
                prt("Set out kml file to [$out_kml].\n") if ($verb);
            } else {
                pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n");
            }
        } else {
            $in_file = $arg;
            prt("Set input to [$in_file]\n") if ($verb);
        }
        shift @av;
    }

    if ($debug_on) {
        prtw("WARNING: DEBUG is ON!\n");
        if (length($in_file) ==  0) {
            $in_file = $def_file;
            prt("Set DEFAULT input to [$in_file]\n");
        }
    }
    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");
    }
}

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(" --bbox        (-b) = Add a bounding box (-bbox) xg output. (def=$add_xg_bbox)\n");
    prt(" --verb[n]     (-v) = Bump [or set] verbosity. def=$verbosity\n");
    prt(" --load        (-l) = Load LOG at end. ($outfile)\n");
    prt(" --out <file>  (-o) = Write XG output to this file.\n");
    prt(" --kml <file>  (-k) = Write KML output to this file.\n");
}

# eof - template.pl