#!/usr/bin/perl
#-------------------------------------------------------------------------------
#	$Id: routing.pl,v 1.5 2023/01/27 13:57:54 wolf Exp $
#-------------------------------------------------------------------------------
#	https://www.netzwolf.info/code/geo/routing/routing.html
#-------------------------------------------------------------------------------
#	This program is free software.
#	It comes without any warranty, to the extent permitted by applicable law.
#	You can redistribute it and/or modify it under the terms of the
#		"Do What The Fuck You Want To" Public License, Version 2,
#	as published by Sam Hocevar. See http://www.wtfpl.net/ for more details.
#-------------------------------------------------------------------------------

# ./routing.pl {initial-lat} {initial-lon} {goal-lat} {goal-lon} <data.graph

use strict;
use utf8;

# process args

my $initialLat	= 1.0 * shift (@ARGV);
my $initialLon	= 1.0 * shift (@ARGV);
my $goalLat	= 1.0 * shift (@ARGV);
my $goalLon	= 1.0 * shift (@ARGV);

# graph

my %lon;
my %lat;

my %neighbours;		# node -> list of nodes
my %arcLength;		# snode,enode -> distance

#-----------------------------------------------------------------------
#	read graph
#-----------------------------------------------------------------------

while (<>) {

	chomp;
	my @f = split /\t/;

	# node

	if ($f[0] eq 'node') {

		my $node = $f[1];
		my $lon  = $f[2];
		my $lat  = $f[3];

		$lon{$node} = $lon;
		$lat{$node} = $lat;
		next;
	}

	# arc

	if ($f[0] eq 'arc') {

		my $snode = $f[1];
		my $enode = $f[2];
		my $dist  = $f[3] || 0.001;	# must NOT be zero

		# ignode multiple arcs between two nodes

		next if defined $arcLength{$snode, $enode};

		# no neighbours yet -> create list

		unless ($neighbours{$snode}) {
			$neighbours{$snode} = [];
		}

		# store arc

		$arcLength{$snode, $enode} = $dist;
		push (@{$neighbours{$snode}}, $enode);

		next;
	}
}

#-----------------------------------------------------------------------
#	lookup initial node
#-----------------------------------------------------------------------

warn "looking up initial node ...\n" if -t STDERR;

my $initialNode;

{
	my $bestDistance = 1e30;
	my $cos = cos ($initialLat * 3.1415925 / 180.0);

	foreach my $node (keys %lon) {

		# distance in degrees

		my $dLon = ($lon{$node}-$initialLon) * $cos;
		my $dLat = ($lat{$node}-$initialLat);

		# skip square root and scaling

		my $distance = $dLon*$dLon + $dLat*$dLat;

		next unless $distance < $bestDistance;

		# found better node

		$initialNode = $node;
		$bestDistance = $distance;
	}
}

die unless $initialNode;

# initial node found

warn "initial node $initialNode @ lat $lat{$initialNode} lon $lon{$initialNode}.\n" if -t STDERR;

print "start\t$lon{$initialNode}\t$lat{$initialNode}\n";

#-----------------------------------------------------------------------
#	lookup goal node
#-----------------------------------------------------------------------

warn "looking up goal node ...\n" if -t STDERR;

my $goalNode;
my $goalCos = cos ($goalLat * 3.1415925 / 180.0);

{
	my $bestDistance = 1e30;

	foreach my $node (keys %lon) {

		# distance in degrees

		my $dLon = ($lon{$node}-$goalLon) * $goalCos;
		my $dLat = ($lat{$node}-$goalLat);

		# skip square root and scaling

		my $distance = $dLon*$dLon + $dLat*$dLat;

		next unless $distance < $bestDistance;

		# found better node

		$goalNode = $node;
		$bestDistance = $distance;
	}
}

die unless $goalNode;

# goal node found

warn "goal node $goalNode @ lat $lat{$goalNode} lon $lon{$goalNode}.\n" if -t STDERR;

#-----------------------------------------------------------------------
#	estimate distance from a node to the goal node (A*)
#-----------------------------------------------------------------------

sub estimate{

	my ($node) = @_;

	my $dLon = ($lon{$node}-$lon{$goalNode}) * $goalCos;
	my $dLat = ($lat{$node}-$lat{$goalNode});

	return sqrt ($dLon*$dLon + $dLat*$dLat) / 360.0 * 40000000;
}

#-----------------------------------------------------------------------
#	initialize variables
#-----------------------------------------------------------------------

my %pastDistance;	# distance on (preliminary) best route from initialnode to node
my %predecessor;	# predecessor on (preliminary) best route to node

my %futureDistance;	# estimated distance from node to goal node
my %estimatedDistance;	# estimated distance from initial to goal via this node
			# keys are the todo list

$pastDistance     {$initialNode} = 0;
$futureDistance   {$initialNode} = estimate ($initialNode);
$estimatedDistance{$initialNode} = $pastDistance{$initialNode} + $futureDistance{$initialNode};

$pastDistance     {$goalNode} = 1e30;
$futureDistance   {$goalNode} = 1e30;
$estimatedDistance{$goalNode} = $pastDistance{$goalNode} + $futureDistance{$goalNode};

#-----------------------------------------------------------------------
#	routing
#-----------------------------------------------------------------------

warn "searching route ...\n" if -t STDERR;

# as long as nodes available

while (%estimatedDistance) {

	# order by estimated distance and get nearest, terminate if beyond goal

	my $thisNode = (sort {$estimatedDistance{$a} <=> $estimatedDistance{$b}} keys %estimatedDistance)[0];

	last if $estimatedDistance{$thisNode} >= $pastDistance{$goalNode};

	# remove from list

	delete $estimatedDistance{$thisNode};

	# check arcs

	foreach my $thatNode (@{$neighbours{$thisNode}}) {

		# dist from initial node to thatNode via thisNode

		my $newThatPastDist = $pastDistance{$thisNode} + $arcLength{$thisNode, $thatNode};

		if (defined $pastDistance{$thatNode}) {

			# if not first visit and new dist not better than known dist skip node

			 next if $newThatPastDist >= $pastDistance{$thatNode};

		} else {

			# if first visit compute future distance

			$futureDistance{$thatNode} = estimate ($thatNode);
		}

		# update past distance and best via node

		$pastDistance{$thatNode} = $newThatPastDist;
		$predecessor {$thatNode} = $thisNode;

		# update estimated distance (adding that node to todo list (again)

		$estimatedDistance{$thatNode} = $newThatPastDist + $futureDistance{$thatNode};
	}
}

#-----------------------------------------------------------------------
#	route to goal found?
#-----------------------------------------------------------------------

my @route;

if ($predecessor{$goalNode}) {

	# show goal node
	print "goal\t$lon{$goalNode}\t$lat{$goalNode}\n";

	# and gather route from goal to initial

	for (my $node=$goalNode; $node; $node=delete $predecessor{$node}) {
		push (@route, $node);
	}
} else {

	# not found

	print "nogoal\t$lon{$goalNode}\t$lat{$goalNode}\n";
}

#-----------------------------------------------------------------------
#	show best route to other nodes
#-----------------------------------------------------------------------

foreach my $node (keys %predecessor) {

	my $prev = $predecessor{$node};
	print "best\t$lon{$prev}\t$lat{$prev}\t$lon{$node}\t$lat{$node}\n";
}

#-----------------------------------------------------------------------
#	show route
#-----------------------------------------------------------------------

while (@route>=2) {
	print "route\t$lon{$route[0]}\t$lat{$route[0]}\t$lon{$route[1]}\t$lat{$route[1]}\n";
	shift @route;
}

exit 0;

#-------------------------------------------------------------------------------
#	$Id: routing.pl,v 1.5 2023/01/27 13:57:54 wolf Exp $
#-------------------------------------------------------------------------------
