diff --git a/lib/App/Log.pm b/lib/App/Log.pm
index 9615ef11e1e5a751c6c3836a3ad035ce8b61464e..11fdfc209c61e595155a78cefc16396935f865af 100644
--- a/lib/App/Log.pm
+++ b/lib/App/Log.pm
@@ -5,6 +5,14 @@ use warnings;
 
 use Time::HiRes qw(gettimeofday);
 
+our $DEBUG_MODE = 1;
+
+sub d
+{
+    if (!$DEBUG_MODE) { return; }
+    print 'DEBUG ||| ' . $_[0] . "\n";
+}
+
 sub i
 {
     print 'INFO | ' . $_[0] . "\n";
diff --git a/lib/App/MasterUtils.pm b/lib/App/MasterUtils.pm
index 58df7c75fd5c4c73dedb19b87e99b0da84f88fa7..48c19173c7628bee45bdb7839c44f8a079242bea 100644
--- a/lib/App/MasterUtils.pm
+++ b/lib/App/MasterUtils.pm
@@ -10,7 +10,7 @@ BEGIN
 
 use App::Log;
 
-use List::Util qw(first);
+use List::Util qw(all first);
 use Data::Printer;
 
 sub sta_from_id
@@ -56,6 +56,7 @@ sub get_eki_position
     my $rosen = get_rosen_frame $master, $rosen_id;
     my @ekis = @{ $rosen->{'eki'} };
 
+    my @pos;
     return first { $ekis[$_] eq $id } 0..$#ekis;
 }
 
diff --git a/lib/App/Navi.pm b/lib/App/Navi.pm
new file mode 100644
index 0000000000000000000000000000000000000000..5f16ca849aa5c1eab2ec100f0ec47695b34ddd0b
--- /dev/null
+++ b/lib/App/Navi.pm
@@ -0,0 +1,201 @@
+package Navi;
+
+use strict;
+use warnings;
+
+BEGIN
+{
+    unshift @INC, '.';
+}
+
+use App::NaviUtils;
+use App::MasterUtils;
+use App::Log;
+use App::Ressya;
+
+use List::Util qw(min);
+use Data::Printer;
+
+our $MAX_LEGS = 5;
+
+sub Raptor_simple
+{
+    my $master = $_[0];
+    my $from_id = $_[1];
+    my $to_id = $_[2];
+    my $dep_time = $_[3];
+
+    my $from = MasterUtils::sta_from_id $master, $from_id;
+    my $to = MasterUtils::sta_from_id $master, $to_id;
+
+    print "Calculating fastest routes from $from->{'ekimei'} to $to->{'ekimei'} leaving at $dep_time\n";
+
+    # From now on we are just blindly following the algorithm from the paper...
+    # Use ~0 as a substitute to infty, not that travel times would exceed this anyway
+    my %taus;
+    my %taustar;
+    my @marked;
+
+    sub earliest_trip
+    {
+	my $k = $_[0];
+	my $r = $_[1];
+	my $r_frame = MasterUtils::get_rosen_frame $master, $r;
+	my $pi = $_[2];
+	my $pos = MasterUtils::get_eki_position $master, $r, $pi;
+	# 0 is down, 1 is up
+	my $way = $_[3];
+	$pos = $way
+	    ? $r_frame->{'eki_num'} - $pos - 1
+	    : $pos;
+	Log::d "Finding the earliest trip on $r starting from $pi @ $taus{$pi}[$k - 1], direction is " . ($way ? 'up' : 'down');
+
+	my $services = $way
+	    ? $r_frame->{'dia'}{'nobori'} : $r_frame->{'dia'}{'kudari'};
+	my $final;
+	foreach my $unten (@$services)
+	{
+	    my $dt = Ressya::dep_time $unten, $pos;
+	    if (!defined $dt) { next; }
+	    if ($dt >= $taus{$pi}[$k - 1] && (!defined $final || $dt <= (Ressya::dep_time $final, $pos)))
+	    {
+		$final = $unten;
+	    }
+	}
+
+	defined $final
+	    ? Log::d 'Found earliest possible departure @ ' . (Ressya::dep_time $final, $pos)
+	    : Log::d 'No earliest possible departure found';
+	return $final;
+    }
+    
+    foreach my $key (keys %{ $master->{'eki_index'} })
+    {
+	my @a = (~0) x $MAX_LEGS;
+	$taus{$key} = \@a;
+	$taustar{$key} = ~0;
+    }
+    
+    $taus{$from_id}[0] = $dep_time;
+    push @marked, { eki => $from_id, direction => 0 };
+    push @marked, { eki => $from_id, direction => 1 };
+
+    for (my $k = 1; $k < $MAX_LEGS; $k++)
+    {
+	Log::d "Round $k";
+	# Accumulate routes serving marked stops from previous round
+	my @Q;
+	foreach my $ref (@marked)
+	{
+	    my $p = $ref->{'eki'};
+	    my $d = $ref->{'direction'};
+	    my $p_display = MasterUtils::get_ekimei $master, $p;
+	    my @rosens = MasterUtils::get_routes $master, $p;
+	    foreach my $r (@rosens)
+	    {
+		Log::d "$p_display ($p) served by $r";
+		my $substituted = 0;
+		foreach my $triplet (@Q)
+		{
+		    if ($triplet->[0] eq $r)
+		    {
+			my $dprime = $triplet->[1];
+			my $pprime = $triplet->[2];
+			my $dist =
+			    (MasterUtils::get_eki_position $master, $r, $pprime)
+			    - (MasterUtils::get_eki_position $master, $r, $p);
+			if (abs $dist == 1 && $d == $dprime)
+			{
+			    # Log::d "Substituting $pprime for $p on $r";
+			    # $substituted = 1;
+			}
+		    }
+		}
+		if (!$substituted)
+		{
+		    push @Q, [$r, $d, $p];
+		}
+	    }
+	    Log::d "Unmarking $p_display ($p)";
+	}
+	@marked = ();
+
+	# Traverse each route
+ 	foreach my $triplet (@Q)
+	{
+	    my $r = $triplet->[0];
+	    my $d = $triplet->[1];
+	    my $p = $triplet->[2];
+	    my $trip;
+	    my @path = undef;
+	    
+	    # We do two runs starting from $p, one towards each end of the route.
+	    my @r_ekis = @{ (MasterUtils::get_rosen_frame $master, $r)->{'eki'} };
+	    my $ekinum = $#r_ekis;
+	    my $p_index = MasterUtils::get_eki_position $master, $r, $p;
+
+	    # New idea: find the fastest service on a single route from $p to $index
+	    for (my $dist = -$#r_ekis;
+		 $dist <= $#r_ekis; # TODO optimization
+		 $dist++)
+	    {
+		my $index = $p_index - $dist;
+		if ($index < 0 || $index > $#r_ekis) { next; }
+		if ($index < $p_index && $d == 0 ||
+		    $index > $p_index && $d == 1) { next; }
+		my $index_rel = $d
+		    ? $ekinum - $index
+		    : $index;
+		my $pi = $r_ekis[$index];
+
+		$trip = NaviUtils::direct_trip $master, $r, $p, $pi, $taus{$p}[$k-1];
+
+		if (defined $trip
+		    && defined (Ressya::arr_time $trip, $index_rel)
+		    && (Ressya::arr_time $trip, $index_rel) < (min $taustar{$pi}, $taustar{$to_id}))
+		{
+		    my $trip_arrival = Ressya::arr_time $trip, $index_rel;
+		    Log::d "Updating ETA($k) for $pi >> $trip_arrival";
+		    $taus{$pi}->[$k] = $trip_arrival;
+		    $taustar{$pi} = $trip_arrival;
+		    
+		    # Check if the target station is not already marked
+		    my $mark = 1;
+		    for (@marked)
+		    {
+			if ($_->{'eki'} eq $pi) { $mark = 0; }
+		    }
+		    if ($mark)
+		    {
+			push @marked, { eki => $pi, direction => 0 };
+			push @marked, { eki => $pi, direction => 1 };
+			Log::d "Marking station $pi";
+		    }
+		}
+		else
+		{
+		    Log::d "Found earliest departure is later than the current earliest departure, or is later than the current ETA @ $to_id ($taustar{$to_id})";
+		}
+		
+		
+		# if (!defined $trip ||
+		# 	(!defined Ressya::dep_time $trip, $index_rel ||
+		# 	$taus{$pi}[$k-1] <= Ressya::dep_time $trip, $index_rel))
+		# {
+		# 	# $trip = earliest_trip $k, $r, $pi, $dir;
+		# }
+	    }
+	    
+	}
+	
+	if ($#Q == 0)
+	{
+	    # TODO return a route
+	    return;
+	}
+    }
+    p %taus;
+    p %taustar;
+}
+
+1;
diff --git a/lib/App/NaviUtils.pm b/lib/App/NaviUtils.pm
new file mode 100644
index 0000000000000000000000000000000000000000..bfe0e4ad07cc4cc2361443442442ba36cfe8861c
--- /dev/null
+++ b/lib/App/NaviUtils.pm
@@ -0,0 +1,89 @@
+package NaviUtils;
+
+use strict;
+use warnings;
+
+BEGIN
+{
+    unshift @INC, '.';
+}
+
+use App::MasterUtils;
+use App::Log;
+use App::Ressya;
+
+use List::Util qw(any);
+use Data::Printer;
+
+sub direct_trip
+{
+    # TODO support for next day
+    my ($master, $r, $from_id, $to_id, $dep) = @_;
+    Log::d "Searching for the fastest direct trip from $from_id to $to_id @ $dep";
+    if ($from_id eq $to_id)
+    {
+	Log::d "This is the same station";
+	return;
+    }
+
+    # Skip if $from and $to are not on the same route
+    my $found = 0;
+    my $frame = $master->{'rosen'}->{$r};
+    if ((any { $_ eq $from_id } @{ $frame->{'eki'} }) &&
+	(any { $_ eq $to_id } @{ $frame->{'eki'} }))
+    {
+	$found = 1;
+    }
+    # TODO this could be problematic for through services
+    if (!$found)
+    {
+	Log::d "$from_id and $to_id are not on the same route, skipping";
+	return;
+    }
+
+    # Determine direction
+    # 0 down, 1 up
+    my $from_pos = MasterUtils::get_eki_position $master, $r, $from_id;
+    my $to_pos = MasterUtils::get_eki_position $master, $r, $to_id;
+    my $dir = $to_pos - $from_pos < 0 ? 1 : 0;
+    Log::d 'Direction is ' . ($dir ? 'up' : 'down');
+    my $dia = $dir
+	? $frame->{'dia'}->{'nobori'} : $frame->{'dia'}->{'kudari'};
+
+    my $from_corr = $dir
+	? $frame->{'eki_num'} - $from_pos - 1
+	: $from_pos;
+    my $to_corr = $dir
+	? $frame->{'eki_num'} - $to_pos - 1
+	: $to_pos;
+
+    # Only keep departures after $dep that stop at $from_id
+    
+    my $final;
+    foreach my $u (@$dia)
+    {
+	my $udep = Ressya::dep_time $u, $from_corr;
+	if (!defined $udep || $udep < $dep) { next; }
+	my $uarr = Ressya::arr_time $u, $to_corr;
+	if (!defined $uarr) { next; }
+	# TODO Times > 23:59 are currently not supported.
+	if ($uarr < $udep) { next; }
+	if (!defined $final ||
+	    $uarr < Ressya::arr_time $final, $to_corr)
+	{
+	    $final = $u;
+	}
+    }
+
+    if (!defined $final)
+    {
+	Log::d "No earliest departure was found.";
+	return;
+    }
+    
+    my $display_dep = Ressya::dep_time $final, $from_corr;
+    Log::d "Found earliest departure from $from_id @ $display_dep";
+    return $final;
+}
+
+1;
diff --git a/lib/App/Ressya.pm b/lib/App/Ressya.pm
index 0b8d25abfa7cef5291ab3c891daf70a8a0d1aa9e..74fcfa9dab1f386ce2606c240983f2f76c055294 100644
--- a/lib/App/Ressya.pm
+++ b/lib/App/Ressya.pm
@@ -3,32 +3,61 @@ package Ressya;
 use strict;
 use warnings;
 
-use Data::Dumper;
+BEGIN
+{
+    unshift @INC, '.';
+}
+
+use App::Log;
+
+use Data::Printer;
 
 sub kukan
 {
-    my @jikoku = @{ $_[0]{'jikoku'} };
+    my $ressya = $_[0];
+    if (!defined $ressya)
+    {
+	Log::e 'Specified service was undefined.';
+    }
+    my $jikoku = $ressya->{'jikoku'};
     my $start = 0;
-    my $end;
+    my $end = scalar @{ $jikoku };
     my %res;
 
-    while (!defined $jikoku[0])
+    while ($jikoku->[$start]->{'mode'} == 0)
     {
-	shift @jikoku;
 	$start++;
     }
 
-    $end = $start;
-    while (defined $jikoku[0])
-    {
-	shift @jikoku;
-	$end++;
-    }
-
     $res{'start'} = $start;
     $res{'end'} = $end;
-    
+
     return \%res;
 }
 
+sub dep_time
+{
+    my $ressya = $_[0];
+    my $index = $_[1];
+
+    my $k = kukan $ressya;
+    if ($index >= $k->{'end'} || $index < $k->{'start'}) { return; }
+    my $ts = $ressya->{'jikoku'}->[$index];
+    if ($ts->{'mode'} != 1 || !defined $ts->{'hatsu'}) { return; }
+    return $ts->{'hatsu'};
+}
+
+sub arr_time
+{
+    my $ressya = $_[0];
+    my $index = $_[1];
+
+    my $k = kukan $ressya;
+    if ($index >= $k->{'end'} || $index < $k->{'start'}) { return; }
+    my $ts = $ressya->{'jikoku'}->[$index];
+    if ($ts->{'mode'} != 1) { return; }
+    # If not provided, arr_time is by default equal to dep_time
+    return defined $ts->{'chaku'} ? $ts->{'chaku'} : $ts->{'hatsu'};
+}
+
 1;
diff --git a/src/oud2_parser.pl b/src/oud2_parser.pl
index 47eb083adb5572c30af2f994256a4c6c92d3fe73..908146cd810eb3678b209f659e7c5b79dafd9b53 100644
--- a/src/oud2_parser.pl
+++ b/src/oud2_parser.pl
@@ -35,6 +35,7 @@ use App::Frame;
 use App::FrameUtils;
 use App::MasterUtils;
 use App::Dia;
+use App::Navi;
 
 # .oud2 grammar
 my $oud_rules = << 'endrule'
@@ -327,4 +328,5 @@ foreach my $frame (values %{ $master{'rosen'} })
 }
 
 # MasterUtils::get_routes \%master, 'konpoku_9';
-FrameUtils::hassya_hyou \%master, 'konpoku_9';
+# FrameUtils::hassya_hyou \%master, 'konpoku_9';
+Navi::Raptor_simple \%master, 'konpoku_3', 'sibetu_6', 1111;