#!/perl

#  nmea.perl
# Convert generic NMEA output into something more spreadsheet-friendly.
# Understands some Garmin-specific sentences.
#
# Note: we depend on the $GPRMC sentence appearing once in every
# "data burst" from the GPS, preferably as the first sentence.
# If this is not true for a particular GPS, the code that calls
# output() must be moved to a more appropriate place.
#
# Dave Martindale, May 2000
# Geoff McLane, July 2005
# The original (beatifully FULL) output MUST be kept, but usually
# we are interested in only a FEW of the values ...
# Assumtions
# WGS84 datum 
# The range of the visible screen-coordinates is  x = 0 (left)
# to x = 1350 (right) and y = 0 (bottom) to y = 1000 (top):
# x,y: top left  0,1000    top right 1350,1000
#  bottom left   0,0    bottom right 1350,0
$maxxwid = 1350;
$maxyht = 1000;
#uses $SGD_PI = 3.14159265358979323846; #/* From M_PI under Linux/X86 */

$dbg2 = 1;
$dbg3 = 0;

$outhtml = 'tempgps1.htm';

%sentlist = (); # list, and count, of sentences found ...
@ll_list = (); # list of lat,lon
$ll_grp = 3;

$maxtrack = 12;
$dbgon = 1; # 0 for runtime
$actdist = 0;
$actazim = 0;

if ($dbgon == 1) {
$infile = 'gps01.txt';
$outfile = 'tempgps4.csv';
} else {
$infile = shift or die "No input file given ...\n";
$outfile = shift or die "No output file given ...\n";
}

###$infile = "<-";
###$outfile = ">-";
###open (INFILE, $infile) or die "couldn't open input file $infile\n";
###open (OUTFILE, $outfile) or die "couldn't open output file $outfile\n";
open (INFILE, "<$infile") or die "couldn't open input file $infile\n";
open (OUTFILE, ">$outfile") or die "couldn't open output file $outfile\n";

print "Running ...gps02.pl ...\n";

print OUTFILE "GPRMC,,,,,,,,";
print OUTFILE "GPGGA,,,,,,,,,,";
print OUTFILE "GPGSA,,,,,,,,,,,,,,,,,";
print OUTFILE "GPGSV,", "," x (4 * $maxtrack);
print OUTFILE "GPVTG,,,,";
print OUTFILE "GPGLL,,,,";
print OUTFILE "PGRME,,,";
print OUTFILE "PGRMZ,,";
print OUTFILE "PGRMM";
print OUTFILE "\n";

print OUTFILE "Time,Valid,Lat,Long,SOG,CMG,Date,MVar,";		# GPRMC
print OUTFILE "Time,Lat,Long,Qual,Sats,HDOP,Alt,GeoH,DGPSAge,DGPSID,";	# GPGGA
print OUTFILE "Auto,Dim,PRN,,,,,,,,,,,,PDOP,HDOP,VDOP,";	# GPGSA
print OUTFILE "View,";						# GPGSV
for ($sat = 1; $sat <= $maxtrack; $sat++) {
	print OUTFILE "PRN ", $sat, ",Elev,Azim,S/N,";
}
print OUTFILE "CRS (T),CRS (M),SOG (Kt),SOG (km/h),";		# GPVTG
print OUTFILE "Lat,Long,Time,Valid,";				# GPGLL
print OUTFILE "HPE,VPE,SPE,";					# PGRME
print OUTFILE "Alt,Dim,";					# PGRMZ
print OUTFILE "Datum";						# PGRMM
print OUTFILE "\n";

$outgpsline = 0;
$invcnt = 0; # invalid sentences
$currhr = 25; # invalid hour
$currmin = 60; # invalid minute
$currsec = 60; # invalid second
$currposlaty = 0;
$currposlonx = 0;
$sentcnt = 0;
$minlat = 90;
$maxlat = -90;
$maxlon = -180;
$minlon = 180;

sub do_file_output {
	print OUTFILE $time_rmc, ',', $ok_rmc, ',', $lat_rmc, ',',
		$long_rmc, ',', $speed, ',', $cmg, ',', $date, ',',
		$mvar, ',';
	print OUTFILE $time_gga, ',', $lat_gga, ',', $long_gga, ',',
		$fixqual, ',', $nsat, ',', $hdop_gga, ',', $alt_gga, ',',
		$gheight, ',', $DGPS_age, ',', $DGPS_ID, ',';
	print OUTFILE $fixsel, ',', $fixdeg, ',',
		join(',', @satsused), ',', $pdop, ',', $hdop, ',',
		$vdop, ',';
	print OUTFILE $nsiv, ',';
	for ($sat = 0; $sat < $maxtrack; $sat++) {
		if ($sat < $nsiv) {
			print OUTFILE $prn[$sat], ',', $elev[$sat], ',',
				$azim[$sat], ',', $signal[$sat], ',';
		} else {
			print OUTFILE ",,,,";
		}
	}
	print OUTFILE $crs_t, ',', $crs_m, ',', $sog_kt, ',', $sog_km, ',';
	print OUTFILE $lat_gll, ',', $long_gll, ',', $time_gll, ',',
		$ok_gll, ',';
	print OUTFILE $hpe, ',', $vpe, ',', $epe, ',';
	print OUTFILE $alt_rmz, ',', $alt_type, ',';
	print OUTFILE $datum;
	print OUTFILE "\n";
	$outgpsline++;
}

sub output {
	do_file_output();
	if ($ok_rmc eq 'A') {
		# valid record
#	print OUTFILE $time_rmc, ',', $ok_rmc, ',', $lat_rmc, ',',
#		$long_rmc, ',', $speed, ',', $cmg, ',', $date, ',',
#		$mvar, ',';
		# $time_rmc = '13:02:10' HH:MM:SS format
		my (@hms) = split ':', $time_rmc; # split TIME
		# $date = 26/07/05 # split DATE
		my (@dte) = split '/', $date; # split date
		$currhr = $hms[0];
		$currmin = $hms[1];
		$currsec = $hms[2];
		$currday = $dte[0];
		$currmth = $dte[1];
		$curryr = $dte[2];
		if( ( ($currhr >=  0) && ($currhr  < 24) ) &&
			( ($currmin >= 0) && ($currmin < 60) ) &&
			( ($currsec >= 0) && ($currsec < 60) ) ) {
			$currtimed = "$currhr:$currmin:$currsec";
			$currdated = "$curryr/$currmth/$currday";
			$dispdt = "$currdated $currtimed";
			# got a VALID time
			my (@ll) = ($lat_rmc, $long_rmc, $alt_gga);
			push(@ll_list, @ll);
			if (($currposlaty != $lat_rmc ) ||
				($currposlonx != $long_rmc) ) {
				# position CHANGED, but by HOW MUCH?
				$mets = -1;
				if ($sentcnt) {
					$mets = dist_m ( 10, $lat_rmc, $long_rmc, $currposlaty, $currposlonx );
				}
				#print $currhr, ":", $currmin, ":", $currsec,
				#print $dispdt,
				print $currtimed,
					" at lat ", $lat_rmc, " long ", $long_rmc,
					" $mets meters ($actdist)\n";
				$currposlaty = $lat_rmc;
				$currposlonx = $long_rmc;
				if ($currposlaty < $minlat) { $minlat = $currposlaty }
				if ($currposlaty > $maxlat) { $maxlat = $currposlaty }
				if ($currposlonx < $minlon) { $minlon = $currposlonx }
				if ($currposlonx > $maxlon) { $maxlon = $currposlonx }
				$sentcnt++;
			} else {
				# repeating SAME POSITION
				print "SAME POSITION: $currhr", ":", $currmin, ":", $currsec, "\n";
			}
		} else {
			print "OUT-OF-TIME: ", $currhr, ":", $currmin, ":", $currsec, "\n";
		}
	} else {
		$invcnt++;
	}
}

# @_ = (3352.3955,S) == -33 52.3955 == -33.8732583333333
sub latitude {
	my ($deg, $min) = unpack "a2a*", $_[0];
	my $lat = $deg + $min / 60;
	$lat = - $lat if $_[1] =~ /[Ss]/;
	return $lat;
}

# @_ = (15113.3678,E) == +151 13.6678 == 151.222796666667
sub longitude {
	my ($deg, $min) = unpack "a3a*", $_[0];
	my $long = $deg + $min / 60;
	$long = - $long if $_[1] =~ /[Ww]/;
	return $long;
}

sub add_2_list {
	my ($tx) = @_;
	if (exists $sentlist{$tx} ) { # list, and count, of sentences found ...
		$sentlist{$tx}++; # bump the count
	} else {
		$sentlist{$tx} = 1;
		if ($dbgon > 1) {
			print "$tx\n";
		}
	}
}

$first = 1;
$linecnt = 0;
while ($line = <INFILE>) {
	$linecnt++; # count each line ....
	chomp($line);
	@field = split /[,*]/, $line; # split on comma, and * (checksum)
	$csent = $field[0]; # get this sentence 
	if (length($csent) == 6) { # NMEA length, at least
		if (substr($csent,0,1) eq '$') {
			add_2_list ( $csent ); # keep list of sentence type found
		} else {
			print "What is this? [$csent] of ", length($csent), " bytes\n";
		}
	}

	SWITCH: {

		# recommended minimum specific GPS/Transit data
		if ($field[0] eq '$GPRMC') {

			# We don't know exactly what sentences to expect
			# from an arbitrary GPS, but we assume that RMC
			# will always be included.  Each time we see this
			# sentence, print out the accumulated information
			# from the previous burst.
			output() unless ($first) ;
			$first = 0;

			# Now process the new RMC record - example
### $GPRMC,130210.030,A,3352.3955,S,15113.3678,E,0.257256,125.59,260605,,*10                                                                        
			$time_rmc = join ':', unpack "a2" x 3, $field[1];
			$ok_rmc = $field[2];
			$lat_rmc = latitude(@field[3..4]);
			$long_rmc = longitude(@field[5..6]);
			$speed = $field[7];
			$cmg = $field[8];
			$date = join '/', unpack "a2" x 3, $field[9];
			$mvar = $field[10] . $field[11];
			# field[12] is checksum
			last SWITCH;
		}
	
		# GPS fix data
		if ($field[0] eq '$GPGGA') {
			$time_gga = join ':', unpack "a2" x 3, $field[1];
			$lat_gga = latitude(@field[2..3]);
			$long_gga = longitude(@field[4..5]);
			$fixqual = $field[6];
			$nsat = $field[7];
			$hdop_gga = $field[8];
			$alt_gga = $field[9];
			# $field[10] is altitude units (always M)
			$gheight = $field[11];
			# $field[12] is geoid height units (always M)
			$DGPS_age = $field[13];
			$DGPS_ID = $field[14];
			# field[15] is checksum;
			last SWITCH;
		}

		# GPS DOP and active satellites
		if ($field[0] eq '$GPGSA') {
			$fixsel = $field[1];	# A for auto selection, M for manual
			$fixdeg = $field[2];	# 2 or 3 for 2D or 3D fix
			@satsused = @field[3..14];
			$pdop = $field[15];
			$hdop = $field[16];
			$vdop = $field[17];
			# field[18] is checksum;
			last SWITCH;
		}

		# satellites in view
		if ($field[0] eq '$GPGSV') {
			# $field[1] is total number of sentences for full data
			# $field[2] is current sentence number
			$nsiv = $field[3];
			#
			# Unpack next 16 fields as PRN, elevation, azimuth, and
			# signal quality for next 4 satellites
			#
			$sat = ($field[2] - 1) * 4;
			$lim = $sat + 4;
			if ($nsiv < $lim) {
				$lim = $nsiv;
			}
			$f = 4;
			while ($sat < $lim) {
				$prn[$sat] = $field[$f++];
				$elev[$sat] = $field[$f++];
				$azim[$sat] = $field[$f++];
				$sig = $field[$f++];
				$sig = '' if $sig == 0;
				$signal[$sat] = $sig;
				$sat++;
			}
			# field[20] is checksum;
			last SWITCH;
		}

		# track/ground speed
		if ($field[0] eq '$GPVTG') {
			$crs_t = $field[1];
			# $field[2] is reference (always T);
			$crs_m = $field[3];
			# $field[4] is reference (always M);
			$sog_kt = $field[5];
			# $field[6] is units (always N);
			$sog_km = $field[7];
			# $field[8] is units (always K);
			# $field[9] is checksum;
			last SWITCH;
		}

		# position error info
		if ($field[0] eq '$PGRME') {
			$hpe = $field[1];
			# $field[2] is units (always M)
			$vpe = $field[3];
			# $field[4] is units (always M)
			$epe = $field[5];
			# $field[6] is units (always M)
			# field[7] is checksum;
			last SWITCH;
		}

		# latitude/longitude
		if ($field[0] eq '$GPGLL') {
			$lat_gll = latitude(@field[1..2]);
			$long_gll = longitude(@field[3..4]);
			$time_gll = join ':', unpack "a2" x 3, $field[5];
			$ok_gll = $field[6];
			# field[7] is checksum;
			last SWITCH;
		}

		# altitude
		if ($field[0] eq '$PGRMZ') {
			$alt_rmz = $field[1];
			# $field[2] is units (always F)
			$alt_type = $field[3]; # 2: user alt, 3: 3D fix
			# field[4] is checksum;
			last SWITCH;
		}

		# datum
		if ($field[0] eq '$PGRMM') {
			$datum = $field[1];
			# field[2] is checksum;
			last SWITCH;
		}
	}
}

close INFILE;
close OUTFILE;
print "min lat $minlat, max lat $maxlat, diff = ", $maxlat - $minlat, " degs.\n";
print "min lon $minlon, max lon $maxlon, diff = ", $maxlon - $minlon, " degs.\n";
print "Processed $linecnt lines, in file $infile ...\n";
print "Wrote $outgpsline GPS lines to $outfile ...\n";
print "Got ", scalar @ll_list, " in list ...\n";

foreach $key (keys %sentlist) {
	print $key, " ", $sentlist{$key}, "\n";
}

do_html_file();

if ($dbgon > 1) {
 system($outfile) ### = 'tempgps1.csv';
}
system($outhtml);

#// from
#// *********************************************************************
#// sg_geodesy.cxx -- routines to convert between geodetic and geocentric 
#//                   coordinate systems.
#//
#// Copied and adapted directly from LaRCsim/ls_geodesy.c
#//
#// See below for the complete original LaRCsim comments.
#//
#// $Id: sg_geodesy.cxx,v 1.2 2002/12/31 14:47:36 david Exp $
#//
#// Direct and inverse distance functions 
#//
#// Proceedings of the 7th International Symposium on Geodetic
#// Computations, 1985
#//
#// "The Nested Coefficient Method for Accurate Solutions of Direct and
#// Inverse Geodetic Problems With Any Length"
#//
#// Zhang Xue-Lian
#// pp 747-763
#//
#// modified for FlightGear to use WGS84 only -- Norman Vine
#
###$GEOD_INV_PI = $SGD_PI; # = 3.14159265358979323846; #/* From M_PI under Linux/X86 */

#// s == distance
#// az = azimuth
#// for WGS_84 a = 6378137.000, rf = 298.257223563;
#static double M0( double e2 ) {
sub M0 {
	my ($e2) = @_;
    #//double e4 = e2*e2;
    #return GEOD_INV_PI*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + 
	#					  e2*(5.0/256.0) )))/2.0;
	return (3.14159265358979323846*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + 
						  e2*(5.0/256.0) )))/2.0);
}

#// given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2
#// and az2.  Lat, lon, and azimuth are in degrees.  distance in meters
#int geo_direct_wgs_84( double alt, double lat1, double lon1, double az1, 
#			double s, double *lat2, double *lon2,  double *az2 )
$ret_lat2 = 0;
$ret_lon2 = 0;
$ret_az2 = 0;
sub geo_direct_wgs_84 {
	#($alt, $lat1, $lon1, $az1, $s, $lat2, $lon2, $az2) = @_;
	($alt, $lat1, $lon1, $az1, $s) = @_;

	$a = 6378137.000;
	$rf = 298.257223563;
	$RADDEG = (GEOD_INV_PI)/180.0;
	$testv = 1.0E-10;
    $f = ( $rf > 0.0 ? 1.0/$rf : 0.0 );
    $b = $a*(1.0-$f);
    $e2 = $f*(2.0-$f);
    $phi1 = $lat1*$RADDEG;
	$lam1 = $lon1*$RADDEG;
    $sinphi1 = sin($phi1);
	$cosphi1 = cos($phi1);
    $azm1 = $az1*$RADDEG;
    $sinaz1 = sin($azm1);
	$cosaz1 = cos($azm1);
	
	###if( fabs($s) < 0.01 ) {	###// distance < centimeter => congruency
	if( abs($s) < 0.01 ) {	###// distance < centimeter => congruency
	##if( $s < 0.01 ) {	###// distance < centimeter => congruency
		$ret_lat2 = $lat1;
		$ret_lon2 = $lon1;
		$ret_az2 = 180.0 + $az1;
		if( $ret_az2 > 360.0 ) { $ret_az2 -= 360.0 };
		return 0;
    } elsif( $cosphi1 ) {	#// non-polar origin
		#// u1 is reduced latitude
		$tanu1 = sqrt(1.0-$e2)*$sinphi1/$cosphi1;
		$sig1 = atan2($tanu1,$cosaz1);
		$cosu1 = 1.0/sqrt( 1.0 + $tanu1*$tanu1 );
		$sinu1 = $tanu1*$cosu1;
		$sinaz =  $cosu1*$sinaz1;
		$cos2saz = 1.0-$sinaz*$sinaz;
		$us = $cos2saz*$e2/(1.0-$e2);

		#// Terms
		$ta = 1.0+$us*(4096.0+$us*(-768.0+$us*(320.0-175.0*$us)))/16384.0;
		$tb = $us*(256.0+$us*(-128.0+$us*(74.0-47.0*$us)))/1024.0;
		$tc = 0;

		#// FIRST ESTIMATE OF SIGMA (SIG)
		$first = $s/($b*$ta);  ###// !!
		$sig = $first;
		$c2sigm = 0;
		$sinsig = 0;
		$cossig = 0;
		$temp = 0;
		$denom = 0;
		$rnumer = 0;
		$dlams = 0;
		$dlam = 0;
		do {
			$c2sigm = cos(2.0*$sig1+$sig);
			$sinsig = sin($sig);
			$cossig = cos($sig);
			$temp = $sig;
			$sig = $first + 
			 $tb*$sinsig*($c2sigm+$tb*($cossig*(-1.0+2.0*$c2sigm*$c2sigm) - 
						  $tb*$c2sigm*(-3.0+4.0*$sinsig*$sinsig)
						  *(-3.0+4.0*$c2sigm*$c2sigm)/6.0)
				   /4.0);
		} while( abs($sig-$temp) > $testv);

		#// LATITUDE OF POINT 2
		#// DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER)
		$temp = $sinu1*$sinsig-$cosu1*$cossig*$cosaz1;
		$denom = (1.0-$f)*sqrt($sinaz*$sinaz+$temp*$temp);

		#// NUMERATOR
		$rnumer = $sinu1*$cossig+$cosu1*$sinsig*$cosaz1;
		$ret_lat2 = atan2($rnumer,$denom)/$RADDEG;

		#// DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS )
		$rnumer = $sinsig*$sinaz1;
		$denom = $cosu1*$cossig-$sinu1*$sinsig*$cosaz1;
		$dlams = atan2($rnumer,$denom);

		#// TERM C
		$tc = $f*$cos2saz*(4.0+$f*(4.0-3.0*$cos2saz))/16.0;

		#// DIFFERENCE IN LONGITUDE
		$dlam = $dlams-(1.0-$tc)*$f*$sinaz*($sig+$tc*$sinsig*
						   ($c2sigm+
						$tc*$cossig*(-1.0+2.0*
							   $c2sigm*$c2sigm)));
		$ret_lon2 = ($lam1+$dlam)/$RADDEG;

		if ($ret_lon2 > 180.0  ) { $ret_lon2 -= 360.0; }
		if ($ret_lon2 < -180.0 ) { $ret_lon2 += 360.0; }

		#// AZIMUTH - FROM NORTH
		$ret_az2 = atan2(-$sinaz,$temp)/$RADDEG;
		if ( abs($ret_az2) < $testv ) { $ret_az2 = 0.0; }
		if( $ret_az2 < 0.0) { $ret_az2 += 360.0; }
		return 0;
    } else {	#	// phi1 == 90 degrees, polar origin
	    $dM = $a*M0($e2) - $s;
		$paz = ( $phi1 < 0.0 ? 180.0 : 0.0 );
		###return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM,lat2,lon2,az2 );
		return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM );
    } 
}

#// These are hard numbers from the WGS84 standard.  DON'T MODIFY
#// unless you want to change the datum.
$EQURAD = 6378137;
$iFLATTENING = 298.257223563;
###$GEOD_INV_PI = 3.14159265358979323846; #/* From M_PI under Linux/X86 */

#// given alt, lat1, lon1, lat2, lon2, calculate starting and ending
#// az1, az2 and distance (s).  Lat, lon, and azimuth are in degrees.
#// distance in meters
#int geo_inverse_wgs_84( double alt, double lat1,
#                        double lon1, double lat2,
#			double lon2, double *az1, double *az2,
#                        double *s )
$r_az1 = 0;
$r_az2 = 0;
$r_s = 0;
sub geo_inverse_wgs_84 {
	( $alt, $lat1, $lon1, $lat2, $lon2 ) = @_;

	$a = 6378137; # was $EQURAD
	$rf = 298.257223563; # was $iFLATTENING
	$iter=0;
###    $RADDEG = ($GEOD_INV_PI)/180.0;
    $RADDEG = (3.14159265358979323846)/180.0;
	$testv = 1.0E-10;
    $f = ( $rf > 0.0 ? 1.0/$rf : 0.0 );
	$b = $a*(1.0-$f);
	#// double e2 = f*(2.0-f); // unused in this routine
	$phi1 = $lat1*$RADDEG;
	$lam1 = $lon1*$RADDEG;
	$sinphi1 = sin($phi1);
	$cosphi1 = cos($phi1);
	$phi2 = $lat2*$RADDEG;
	$lam2 = $lon2*$RADDEG;
    $sinphi2 = sin($phi2);
	$cosphi2 = cos($phi2);
	if ($dbg3) {
		print "geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat2, $lon2 )\n";
		print "Div by square of b=$b derived from a=$a ...\n";
		print "RADDEG=$RADDEG is equal GEOD_INV_PI=",$RADDEG * 180.0, " div by 180 ...\n";
		print "lam1=$lam1 and lam2=$lam2 ...\n";
		print 'very small value =', $testv, "\n";
	}
    if( (abs($lat1-$lat2) < $testv && 
	 ( abs($lon1-$lon2) < $testv) || abs($lat1-90.0) < $testv ) )
    {	
		#// TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
		$r_az1 = 0.0;
		$r_az2 = 0.0;
		$r_s = 0.0;
		print "geo_inverse_wgs_84 (IDENTICAL STATION)\n" if $dbg3;
		return 0;
    } elsif(  abs($cosphi1) < $testv ) {
		#// initial point is polar
		#$k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
		print "doing geo_inverse_wgs_84 ( $alt, $lat2, $lon2, $lat1, $lon1 )\n" if $dbg3;
		$k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1 );
		#$k = $k; #// avoid compiler error since return result is unused
		$b = $r_az1;
		$r_az1 = $r_az2;
		$r_az2 = $b;
		return 0;
    } elsif( abs($cosphi2) < $testv ) {
		#// terminal point is polar
        $rr_lon1 = $lon1 + 180.0;
		#$k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1, 
		#		    az1, az2, s );
		print "doing geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat1, $rr_lon1 )\n" if $dbg3;
		$k = geo_inverse_wgs_84( $alt, $lat1, $lon1, $lat1, $rr_lon1 );
		#k = k; // avoid compiler error since return result is unused
		$r_s /= 2.0;
		$r_az2 = $r_az1 + 180.0;
		if( $r_az2 > 360.0 ) { $r_az2 -= 360.0; }
		return 0;
    } elsif( (abs( abs($lon1-$lon2) - 180 ) < $testv) && 
	       (abs($lat1+$lat2) < $testv) ) 
    {
		print "Geodesic passes through the pole (antipodal) ...\n" if $dbg3;
		$s1 = 0;
		$s2 = 0;
		#geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
		#geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
		print "doing geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat1, $lon2 )\n" if $dbg3;
		geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2 );
		$s1 = $r_s;
		print "doing geo_inverse_wgs_84 ( $alt, $lat2, $lon2, $lat1, $lon2 )\n" if $dbg3;
		geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2 );
		$s2 = $r_s;
		$r_az2 = $r_az1;
		$r_s = $s1 + $s2;
		return 0;
    } else {
		print "antipodal and polar points don't get here ...\n" if $dbg3;
		$dlam = $lam2 - $lam1;
		$dlams = $dlam;
		$sdlams = 0;
		$cdlams = 0;
		$sig = 0;
		$sinsig = 0;
		$cossig = 0;
		$sinaz = 0;
	    $cos2saz = 0;
		$c2sigm = 0;
		$tc = 0;
		$temp = 0;
		$us = 0;
		$rnumer = 0;
		$denom = 0;
		$ta = 0;
		$tb = 0;
		$cosu1 = 0;
		$sinu1 = 0;
		$sinu2 = 0;
		$cosu2 = 0;

		#// Reduced latitudes
		$temp = (1.0-$f)*$sinphi1/$cosphi1;
		$cosu1 = 1.0/sqrt(1.0+$temp*$temp);
		$sinu1 = $temp*$cosu1;
		$temp = (1.0-$f)*$sinphi2/$cosphi2;
		$cosu2 = 1.0/sqrt(1.0+$temp*$temp);
		$sinu2 = $temp*$cosu2;
    
	do {
	    $sdlams = sin($dlams);
		$cdlams = cos($dlams);
		if ($dbg3) {
			print "part1 = sq $cosu2 times sq $sdlams\n";
			$part1 = $cosu2*$cosu2*$sdlams*$sdlams; # plus
			$part2 = ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams); # times
			$part3 = ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams);
			print "got part1=$part1 + ( part2=$part2 * part3=$part3 )\n";
		}

		$sinsig = sqrt($cosu2*$cosu2*$sdlams*$sdlams+
				  ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams)*
				  ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams));
		$cossig = $sinu1*$sinu2+$cosu1*$cosu2*$cdlams;
		print "Got sinsig=$sinsig cosu2=$cosu2 sdlams=$sdlams\n" if $dbg3;
	    $sig = atan2($sinsig,$cossig);
	    $sinaz = $cosu1*$cosu2*$sdlams/$sinsig;
	    $cos2saz = 1.0-$sinaz*$sinaz;
	    $c2sigm = ($sinu1 == 0.0 || $sinu2 == 0.0 ? $cossig : 
		      $cossig-2.0*$sinu1*$sinu2/$cos2saz);
	    $tc = $f*$cos2saz*(4.0+$f*(4.0-3.0*$cos2saz))/16.0;
	    $temp = $dlams;
	    $dlams = $dlam+(1.0-$tc)*$f*$sinaz*
			($sig+$tc*$sinsig*
			($c2sigm+$tc*$cossig*(-1.0+2.0*$c2sigm*$c2sigm)));
		###if (abs($dlams) > $GEOD_INV_PI && $iter++ > 50) {
		if (( abs($dlams) > 3.14159265358979323846 ) && ($iter++ > 50)) {
			return $iter;
	    }
	} while ( abs($temp-$dlams) > $testv);
	print "Div by square of b=$b ...\n" if $dbg3;
	$us = $cos2saz*($a*$a-$b*$b)/($b*$b); ## // !!
	#// BACK AZIMUTH FROM NORTH
	$rnumer = -($cosu1*$sdlams);
	$denom = $sinu1*$cosu2-$cosu1*$sinu2*$cdlams;
	$r_az2 = atan2($rnumer,$denom)/$RADDEG;
	if( abs($r_az2) < $testv ) {$r_az2 = 0.0;}
	if($r_az2 < 0.0) { $r_az2 += 360.0; }

	#// FORWARD AZIMUTH FROM NORTH
	$rnumer = $cosu2*$sdlams;
	$denom = $cosu1*$sinu2-$sinu1*$cosu2*$cdlams;
	$r_az1 = atan2($rnumer,$denom)/$RADDEG;
	if( abs($r_az1) < $testv ) {$r_az1 = 0.0; }
	if( $r_az1 < 0.0) { $r_az1 += 360.0; }

	#// Terms a & b
	$ta = 1.0+$us*(4096.0+$us*(-768.0+$us*(320.0-175.0*$us)))/16384.0;
	$tb = $us*(256.0+$us*(-128.0+$us*(74.0-47.0*$us)))/1024.0;

	#// GEODETIC DISTANCE
	$r_s = $b * $ta * ( $sig - $tb * $sinsig * 
		($c2sigm + $tb * ( $cossig * ( -1.0 + 2.0 * $c2sigm * $c2sigm ) - $tb *
		$c2sigm * ( -3.0 + 4.0 * $sinsig * $sinsig ) * 
		( -3.0 + 4.0 * $c2sigm * $c2sigm ) / 6.0 ) / 4.0 ) );
	return 0;
    }
}

sub dist_m {
	( $alt, $lat1, $lon1, $lat2, $lon2 ) = @_;
	geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat2, $lon2 );
	print "Distance = $r_s metres ...\n" if $dbg3;
	$az = (int(($r_az1 + 0.05) * 10) /10);
	$s = (int(($r_s + 0.05) * 10) /10);
	$actdist = $s;
	$actazim = $az;
	###return ($r_s . '@' . $r_az1);
	return ($s . '@' . $az);
}


sub html_head {
	my ($fh, $hdr) = @_;
	print $fh <<"EOF";
<html>
<head>
<title>$hdr</title>
</head>
<body>
<h1 align="center">$hdr</h1>
EOF

}

sub new_row {
	($tx) = @_;
	return "<tr>$tx</tr>\n";
}
sub in_col {
	($tx) = @_;
	return "<td>$tx</td>";
}
sub in_colb {
	($tx) = @_;
	return in_col("<b>$tx</b>");
}

$totlat = 0;
$totlon = 0;
$totalt = 0;
$avlat = 0;
$avlon = 0;
$avalt = 0;
$clat = 0;
$clon = 0;
$calt = 0;
$llat = 0;
$llon = 0;
$lalt = 0;
$actdist = 0;
$actazim = 0;
$OF = 0;
sub do_html_file {
	## OUPUT HTML
open ($OF, ">$outhtml") or die "warning: couldn't open output file [$outhtml]!\n";
html_head ( \*$OF, $outhtml );
print $OF "<center>\n";
print $OF "<table border=\"1\">\n";
for ($i = 0; $i < $outgpsline; $i++) {
	$i2 = $i * $ll_grp;
	$clat = $ll_list[$i2];
	$clon = $ll_list[$i2+1];
	$calt = $ll_list[$i2+2];
	$totlat += $clat;
	$totlon += $clon;
	$totalt += $calt;
	#print OF new_row( in_col($clat) . in_col($clon) . in_col($calt) );
	#print "lat $ll_list[$i2], lon $ll_list[$i2+1]";
	print "lat $clat, lon $clon, alt $calt";
	$mets = 0;
	if ($i) {
		$mets = dist_m ( 10, $clat, $clon, $llat, $llon );
		###print " at $mets m.";
		print " at $actdist m.";
	}
	print "\n";
	###print OF new_row( in_col($clat) . in_col($clon) . in_col($calt) . in_col($mets) );
	print $OF new_row( in_col($clat) . in_col($clon) . in_col($calt) . in_col($actdist) .
		in_col($actazim) );
	$avlat = (($avlat * $i) + $clat) / ($i + 1);
	$avlon = (($avlon * $i) + $clon) / ($i + 1);
	$avalt = (($avalt * $i) + $calt) / ($i + 1);
	$llat = $clat;
	$llon = $clon;
	$lalt = $calt;
}

	print $OF new_row( in_colb($totlat / $i) . in_colb($totlon / $i) . in_colb($totalt / $i) . in_colb($actdist) .
		in_colb($actazim) );
	print $OF new_row( in_colb($avlat) . in_colb($avlon) . in_colb($avalt) . in_colb($actdist) .
		in_colb($actazim) );

print $OF "</table>\n";
print $OF "</center>\n";
print $OF "</body>\n";
print $OF "</html>\n";
close $OF;

} ## done HTML file


#eof
