#! /usr/bin/perl
#
#  Copyright (C) 2008 Joerg Reuter <jreuter@yaina.de>
# 
#  This program is free software; you can redistribute it and/or modify
#  it under the terms of the GNU General Public License Version 2 as
#  published by the Free Software Foundation.
# 
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
# 
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
# 

use strict;
use Geo::Proj;
use Geo::Distance;
use Geo::Line;
use Date::Parse;
use XML::Parser;
use Getopt::Long;

my ($Vmax, $Amax, $Tmax, $dmin, $devmax);

my ($dostats, $statinrecs, $statoutrecs, $statinvalid, $stattimeout,
    $statvmax, $statamax, $statpointonline, $statidentical) = (0, 0, 0, 0, 0, 0, 0, 0, 0);

# The output format is:
# epoch, flag, lat, lon, ele
#
# epoch: unix time in "seconds.milliseconds"
# flag:  0: normal trackpoint, 1: first trackpoint of new track segment
# lat:   decimal representation of latitude (E)
# lon:   decimal representation of longituted (N)
# ele:   elevation

# Print record
sub print_record {
	my @record = @_;
	print OUT join(",", @record)."\n";
	$statoutrecs++ if ($dostats);
}

my @record_list = ();

# Print everything (debug)
sub print_records {
	my $record;
	foreach $record (@record_list) {
		print_record(@$record);
	}
}

# We need only the first and the last point...
sub print_line {
	if (@record_list > 0) {
		my $record = @record_list[0];
		print_record(@$record);
	}
	if (@record_list > 1) {
		my $record = @record_list[-1];
		print_record(@$record);
	}
}

my $geo = new Geo::Distance();
my $epsilon = 1e-6;

# Distance of a point to line. Calculation mostly done with polar
# coordinates as we only look at relatively small distances and
# don't need to care about accuracy anyway.

sub distance_point_to_line {
	my ($lonA, $latA, $lonB, $latB, $lonP, $latP) = @_;

	my $dx=$lonA-$lonB;
	my $dy=$latA-$latB;

	if (abs($dx) < $epsilon) {
		return $geo->distance('meter', $lonA, $latP => $lonP,$latP);
	} 
	elsif (abs($dy) < $epsilon) {
		return $geo->distance('meter', $lonP, $latA => $lonP,$latP);
	} else {
		my $latC=($lonP-$lonA)*$dy/$dx+$latA;
		my $lonC=$lonP;

		my $latD=$latP;
		my $lonD=($latP-$latA)*$dx/$dy+$lonA;

		# print "\n($lonC,$latC),($lonD,$latD)\n";
		return $geo->distance('meter', $lonC, $latC => $lonD,$latD)/2;
	}
}

# Eliminate any points on a straight line except the start and end points.

sub combine_to_line {
	my @record = @_;

	# restart flag always starts a new line
	if ($record[1] > 0) {
		print_line();
		@record_list=(\@record);
		return;
	}

	# we need at least three points
	if (@record_list < 2) {
		push @record_list, \@record;
		return;
	}

	# calculate the distance between the first and the current point
	my $r1=$record_list[0];
	my @p1=@$r1;
	my @p2=@record;

	# because we do not really have to include points close to the
	# start point. We know we can do without it.
	my $dist = $geo->distance("meter", $p1[3], $p1[2] => $p2[3], $p2[2]);
	if ($dist < $dmin) {
		$statpointonline++ if ($dostats);
		return;
	}

	# iterate over previous points. Yes, this is highly inefficient indeed.
	for (my $count=1; $count < @record_list; $count++) {
		my $r3=$record_list[$count];
		my @p3=@$r3;

		if (abs(distance_point_to_line($p1[3], $p1[2], $p2[3], $p2[2], $p3[3], $p3[2])) > $devmax) {
			$statpointonline+=@record_list-2 if ($dostats);
			print_line();
			@record_list=(\@record);
			return;
		}
	}

	push @record_list, \@record;
}

my $newpoint=1;
my $lon_prev=0;
my $lat_prev=0;
my $ele_prev=0;
my $speed_prev=0;
my $t_prev=0;
my $resync;

# Process a location record. A trackpoint in GPX, hence the name.

sub process_trkpt {
	my ($t, $lon, $lat, $ele, $sats) = @_;

	$statinrecs++ if ($dostats);

	# need at least 3 satellites
	if ($sats < 3) {
		$statinvalid++ if ($dostats);
		return;
	}

	# first record, don't know yet if it is valid
	if ($newpoint) {
		$resync=1;
		$newpoint=0;
		goto getout;
	}

	# get distance between two records
	my $dist = $geo->distance("meter", $lon_prev, $lat_prev => $lon, $lat);

	# ignore small distances, but keep record
	if ($dist > $dmin) {
		# time difference between two records
		my $dt=$t-$t_prev;
		$dt=$dt+86400 if ($dt < 0);	# new day

		# new start after a certain amount of time w/o updates
		if ($dt > $Tmax) {
			$stattimeout++ if ($dostats);
			$newpoint=1;
			return;
		}

		$dt=1 if ($dt == 0);

		# current speed, cannot exceed $Vmax, otherwise: new start
		my $speed=$dist/$dt;
		if ($speed > $Vmax) {
			$statvmax++ if ($dostats);
			$newpoint=1;
			return;
		}

		# acceleration larger than positively possible?
		if (!$resync && ($speed > $speed_prev)) {
			my $accel=($speed-$speed_prev)/$dt;
			if ($accel > $Amax) {
				$statamax++ if ($dostats);
				$newpoint=1;
				return;
			}
		}

		# print records
		combine_to_line($t_prev, 1, $lat_prev, $lon_prev, $ele_prev) if ($resync);
		combine_to_line($t, 0, $lat, $lon, $ele);

		$speed_prev=$speed;
		$resync=0;
	} else {
		$statidentical++ if ($dostats);
	}

getout:
	$lon_prev=$lon;
	$lat_prev=$lat;
	$t_prev=$t;
	$ele_prev=$ele;
}


# XML parser for GPX files

my ($p_state, $p_state2, $p_lon, $p_lat, $p_time, $p_ele, $p_sat);

sub xml_tag_start {
	my ($e, $tag, %attribs) = @_;

	if ($tag eq 'trkpt') {
		$p_lon=$attribs{'lon'};
		$p_lat=$attribs{'lat'};
		$p_state=$tag;
	} elsif ($p_state eq 'trkpt') {
		if (($tag eq 'ele') || ($tag eq 'time') || ($tag eq 'sat')) {
			$p_state2=$tag;
		} else {
			$p_state2='';
		}
	}
}

sub xml_tag_end {
	my ($e, $tag) = @_;
	if ($tag eq 'trkpt') {
		# print "$p_time $p_lon,$p_lat $p_ele $p_sat\n";
		$p_sat='4' if ($p_sat eq '');

		my $t=str2time($p_time);
		my $msec = $p_time;
		$t=$t.$msec if ($msec =~ s/^.*(\.[[:digit:]]*).*$/\1/);

		process_trkpt($t, $p_lon, $p_lat, $p_ele*1.0, $p_sat*1);

		$p_time='';
		$p_ele='';
		$p_sat='';
		$p_state='';
		$p_state2='';
	}
}

sub xml_cdata {
	my ($e, $data) = @_;
	if ($p_state2 eq 'ele') {
		$p_ele.=$data;
	} elsif ($p_state2 eq 'time') {
		$p_time.=$data;
	} elsif ($p_state2 eq 'sat') {
		$p_sat.=$data;
	}
}

# Convert GPS NMEA location to decimal

sub dmc2dec {
	my $dmc = $_[0];
	my $deg=int($dmc/100);
	my $min=$dmc-$deg*100.0;
	return $deg+$min/60;
}

# Main

my $help=0;
my $inputtype="";
my $infile="-";
my $outfile="";
my $vmax = 200;		# maximum speed (set to 250 for Geko)
my $amax = 11;		# maximum acceleration (set to 4 for Geko)
$Tmax = 10;		# maximum time before filter restart
$dmin = 0.8;		# minimum distance
$devmax = 1.2;		# maximum distance from line

Getopt::Long::Configure ("bundling");
my $result = GetOptions(
	"i|type=s" => \$inputtype,
	"f|infile=s" => \$infile,
	"F|outfile=s" => \$outfile,
	"v|maxspeed=f" => \$vmax,
	"a|maxaccel=f" => \$amax,
	"t|tmax=f" => \$Tmax,
	"d|mindist=f" => \$dmin,
	"e|maxdist=f" => \$devmax,
	"h|help" => \$help,
	"s|stats" => \$dostats);

if ($help) {
	print "usage: gpsfilter [options...]\n";
	print "--type|-i gpx|nmea|sav\n\tFormat of input data\n";
	print "--maxspeed|-v $vmax\n\tMaximum speed between two records [km/h]\n";
	print "--maxaccel|-a $amax\n\tMaximum acceleration [s to reach 100 km/h]\n";
	print "--tmax|-t $Tmax\n\tMaximum time between records before starting new track [s]\n";
	print "--mindist|-d $dmin\n\tMinimum distance between two records [m]\n";
	print "--maxdist|-e $devmax\n\tMaximum distance for a record being on straight line [m]\n";
	print "--stats|-s\n\tPrint stats\n";
	print "--infile|-f filename\n\tInput file name ('-' for stdin)\n";
	print "--outfile|-F filename\n\tOutput file name ('-' for stdout)\n";
	exit(0);
}

$Vmax=$vmax/3.6;
$Amax=100/($amax*3.6);

if ($outfile eq "") {
	my $dot=rindex($infile, ".");
	if ($dot > 1) {
		$outfile=substr($infile, 0, $dot).".csv";
	} else {
		$outfile="-";
	}
} 

$outfile="/dev/stdout" if ($outfile eq "-");
$infile="/dev/stdin" if ($infile eq "-");

if ($inputtype eq "") {
	$inputtype="gpx";
	$inputtype="nmea" if ($infile =~ /.nmea$/i);
	$inputtype="sav" if ($infile =~ /.sav$/i);
}

print STDERR "$infile ($inputtype) => $outfile (csv)\n";

my $line;

if ($inputtype eq "gpx") {
	open(OUT, ">", $outfile) || die "cannot write '$outfile'.";
	my $parser = new XML::Parser(Handlers => {
		Start => \&xml_tag_start, End => \&xml_tag_end, Char => \&xml_cdata});
	$parser->parsefile($infile);
	print_line();
	close(OUT);
} elsif ($inputtype eq "nmea") {
	open(fp, "<", $infile) || die "cannot read '$infile'.";
	open(OUT, ">", $outfile) || die "cannot write '$outfile'.";
	my $date;
	while ($line = <fp>) {
		$date=(split(",",$line))[9] if ($line =~ /^\$GPRMC/); # actual date is only in RMC
		next if ($line !~ /^\$GPGGA/);
		next if (!$date);

		# $GPGGA,213713.000,3722.4014,N,12155.8924,W,1,04,2.0,16.9,M,-25.6,M,,0000*5F
        	my ($id, $thms, $latdmc, $lato, $londmc, $lono, 
	        $valid, $sat, $dilution, $ele, $eleunit, 
        	$geoidalsep, $gisepunit,$age,$ref) = split (",",$line);
		next if ($latdmc == 0 || $londmc== 0);	# need coordinates
		next if ($valid < 1 || $valid > 2);	# need data marked valid by RX

		# decimal values for longitude, latitude and timestamp
		my $lon=dmc2dec($londmc);
		my $lat=dmc2dec($latdmc);
		
		$lon=-$lon if ($lono eq 'W');
		$lat=-$lat if ($lato eq 'S');
		
		my $ts="20".substr($date, 4,2)."-".
			substr($date, 2, 2)."-".
			substr($date, 0, 2)."T".
			substr($thms, 0, 2).":".
			substr($thms, 2, 2).":".
			substr($thms, 4, 2)."Z";
		my $t=str2time($ts);
		my $msec = $thms;
		$t=$t.$msec if ($msec =~ s/^.*(\.[[:digit:]]*).*$/\1/);

		process_trkpt($t*1.0, $lon*1.0, $lat*1.0, $ele*1.0, $sat*1);
	}
	print_line();
	close(OUT);
} elsif ($inputtype eq "sav") {
	open(fp, "<", $infile) || die "cannot open '$infile'..\n";
	open(OUT, ">", $outfile) || die "cannot write '$outfile'.";
	while ($line = <fp>) {
		my ($lat, $lon, $ele, @date)=split(" ", $line);
		my $t=str2time(join(" ", @date));
		process_trkpt($t*1.0, $lon*1.0, $lat*1.0, $ele*1.0, 4);
	}
	print_line();
	close(OUT);
} else {
	die "Unknown file format '$inputtype'";
}

if ($dostats) {
	print STDERR
	"$statinrecs records read\n".
	"$statoutrecs records written\n".
	"$statinvalid invalid records\n".
	"$statidentical identical records\n".
	"$statvmax records with max speed exceeded\n".
	"$statamax records with max acceleration exceeded\n".
	"$statpointonline records on straight line eliminated (approx.)\n";
}