Implement goto so node

Encompasses determining a route and a distance to travel

7 files changed, 101 insertions, 6 deletions

diff --git a/application/main.cpp b/application/main.cpp
index df8828a..2fc9f99 100644
--- a/application/main.cpp
+++ b/application/main.cpp
@@ -1,11 +1,16 @@
 #include <SDL2/SDL.h>
+#include <array>
 #include <chrono>
 #include <collection.hpp>
 #include <game/activities/go.h>
 #include <game/activities/idle.h>
 #include <game/activity.h>
 #include <game/gamestate.h>
+#include <game/network/link.h>
 #include <game/network/rail.h>
+#include <game/objective.h>
+#include <game/objectives/goto.h>
+#include <game/orders.h>
 #include <game/terrain.h>
 #include <game/vehicles/railVehicle.h>
 #include <game/vehicles/railVehicleClass.h>
@@ -109,6 +114,9 @@ public:
 			for (int N = 0; N < 6; N++) {
 				train->create<RailVehicle>(b47);
 			}
+			train->orders.removeAll();
+			train->orders.create<GoTo>(&train->orders, l3->ends[1], l3->length, rl->findNodeAt({-1100, 15, -450}));
+			train->currentActivity = train->orders.current()->createActivity();
 		}
 
 		Shader shader;
diff --git a/game/activities/go.cpp b/game/activities/go.cpp
new file mode 100644
index 0000000..767e59c
--- /dev/null
+++ b/game/activities/go.cpp
@@ -0,0 +1,3 @@
+#include "go.h"
+
+Go::Go(float d) : dist {d} { }
diff --git a/game/activities/go.h b/game/activities/go.h
index 858b804..54af31b 100644
--- a/game/activities/go.h
+++ b/game/activities/go.h
@@ -1,10 +1,16 @@
 #ifndef GO_H
 #define GO_H
 
+#include "../activity.h"
 #include "../activityOf.h"
+#include <optional>
 
 class Go : public Activity::Of<Go> {
 public:
+	Go() = default;
+	explicit Go(float dist);
+
+	std::optional<float> dist;
 };
 
 #endif
diff --git a/game/objectives/goto.cpp b/game/objectives/goto.cpp
new file mode 100644
index 0000000..c089bc3
--- /dev/null
+++ b/game/objectives/goto.cpp
@@ -0,0 +1,43 @@
+#include "goto.h"
+#include <algorithm>
+#include <game/activities/go.h>
+#include <game/activity.h>
+#include <game/network/link.h>
+#include <game/network/routeWalker.h>
+#include <game/objective.h>
+#include <memory>
+#include <numeric>
+#include <vector>
+
+GoTo::GoTo(Orders * o, const Link::End & cp, float d, const NodePtr & dest) :
+	Objective(o), links(RouteWalker().findRouteTo(cp, dest)), startDist {d}
+{
+}
+
+ActivityPtr
+GoTo::createActivity() const
+{
+	return std::make_unique<Go>(std::accumulate(links.begin(), links.end(), 0,
+										[](auto p, const auto & l) {
+											return p += l.first.lock()->length;
+										})
+			+ startDist
+
+	);
+}
+
+inline bool
+operator==(const Link::Next & l, const Link::Next & n)
+{
+	return l.second == n.second && l.first.lock() == n.first.lock();
+}
+
+Link::Next
+GoTo::navigate(Link::Nexts::const_iterator begin, Link::Nexts::const_iterator end) const
+{
+	const auto nextStep = std::find_first_of(links.begin(), links.end(), begin, end);
+	if (nextStep == links.end()) {
+		return *begin;
+	}
+	return *nextStep;
+}
diff --git a/game/objectives/goto.h b/game/objectives/goto.h
new file mode 100644
index 0000000..01e47ae
--- /dev/null
+++ b/game/objectives/goto.h
@@ -0,0 +1,22 @@
+#ifndef GOTO_H
+#define GOTO_H
+
+#include <game/activity.h>
+#include <game/network/link.h>
+#include <game/objective.h>
+
+class Orders;
+
+class GoTo : public Objective {
+public:
+	GoTo(Orders * os, const Link::End &, float, const NodePtr & dest);
+
+	[[nodiscard]] ActivityPtr createActivity() const override;
+	[[nodiscard]] Link::Next navigate(Link::Nexts::const_iterator, Link::Nexts::const_iterator) const override;
+
+private:
+	Link::Nexts links;
+	float startDist;
+};
+
+#endif
diff --git a/game/vehicles/train.cpp b/game/vehicles/train.cpp
index 874db46..14753c0 100644
--- a/game/vehicles/train.cpp
+++ b/game/vehicles/train.cpp
@@ -6,6 +6,7 @@
 #include "location.hpp"
 #include <algorithm>
 #include <functional>
+#include <optional>
 #include <utility>
 
 void
@@ -33,11 +34,24 @@ Train::tick(TickDuration dur)
 }
 
 void
-Train::doActivity(Go *, TickDuration dur)
+Train::doActivity(Go * go, TickDuration dur)
 {
 	const auto maxSpeed = objects.front()->rvClass->maxSpeed;
-	if (speed != maxSpeed) {
-		speed += ((maxSpeed - speed) * dur.count());
+	if (go->dist) {
+		*go->dist -= speed * dur.count();
+		if (*go->dist < (speed * speed) / 60.F) {
+			speed -= std::min(speed, 30.F * dur.count());
+		}
+		else {
+			if (speed != maxSpeed) {
+				speed += ((maxSpeed - speed) * dur.count());
+			}
+		}
+	}
+	else {
+		if (speed != maxSpeed) {
+			speed += ((maxSpeed - speed) * dur.count());
+		}
 	}
 }
 
diff --git a/game/vehicles/vehicle.cpp b/game/vehicles/vehicle.cpp
index cd2331d..8730d6d 100644
--- a/game/vehicles/vehicle.cpp
+++ b/game/vehicles/vehicle.cpp
@@ -1,8 +1,8 @@
 #include "vehicle.h"
+#include "game/activities/idle.h"
 #include "game/activity.h"
 #include "game/network/link.h"
 #include "game/objective.h"
-#include "game/objectives/freeroam.h"
 #include "game/orders.h"
 #include "game/vehicles/linkHistory.h"
 #include <algorithm>
@@ -18,8 +18,7 @@
 Vehicle::Vehicle(const LinkPtr & l, float ld) : linkDist {ld}
 {
 	linkHist.add(l, 0);
-	orders.create<FreeRoam>(&orders);
-	currentActivity = orders.current()->createActivity();
+	currentActivity = std::make_unique<Idle>();
 }
 
 void