From 8546659ae0674ef6b50645f7f5cc03b8272423f9 Mon Sep 17 00:00:00 2001
From: Niklas Kapelle <niklas@kapelle.org>
Date: Sun, 11 Aug 2024 20:17:37 +0200
Subject: [PATCH] first implementation of turtle plans

---
 src/lib/turtle/planner/Action.hx   |  15 +++
 src/lib/turtle/planner/Executer.hx |  41 +++++++++
 src/lib/turtle/planner/Plan.hx     | 142 +++++++++++++++++++++++++++++
 3 files changed, 198 insertions(+)
 create mode 100644 src/lib/turtle/planner/Action.hx
 create mode 100644 src/lib/turtle/planner/Executer.hx
 create mode 100644 src/lib/turtle/planner/Plan.hx

diff --git a/src/lib/turtle/planner/Action.hx b/src/lib/turtle/planner/Action.hx
new file mode 100644
index 0000000..3dc379c
--- /dev/null
+++ b/src/lib/turtle/planner/Action.hx
@@ -0,0 +1,15 @@
+package lib.turtle.planner;
+
+import kernel.turtle.Types.InteractDirections;
+
+enum Action {
+	Forward;
+	Back;
+	Up;
+	Down;
+	TurnLeft;
+	TurnRight;
+	Clear(dir:InteractDirections); // Digs in the direction. Does not fail if nothing to dig. Trys again if still blocked.
+	FullTunnel; // Digs front, move forward, dig down. Like a tunnel for a player.
+	HalfTunnel; // Just clears s 1x2 tunnel infront of it. Like FullTunnel but  without moving.
+}
diff --git a/src/lib/turtle/planner/Executer.hx b/src/lib/turtle/planner/Executer.hx
new file mode 100644
index 0000000..bec2ef1
--- /dev/null
+++ b/src/lib/turtle/planner/Executer.hx
@@ -0,0 +1,41 @@
+package lib.turtle.planner;
+
+import kernel.turtle.Turtle;
+
+using tink.CoreApi;
+
+class Executer {
+	public function new() {}
+
+	public function execute(action:Action) {
+		var r = translate(action);
+		switch r {
+			case Failure(failure):
+				throw new Error(failure); // I think that this a valid place to use exeptions.
+			default:
+		}
+	}
+
+	private function translate(action:Action):Outcome<Noise, String> {
+		switch (action) {
+			case Forward:
+				return Turtle.forward();
+			case Back:
+				return Turtle.back();
+			case Up:
+				return Turtle.up();
+			case Down:
+				return Turtle.down();
+			case TurnLeft:
+				return Turtle.turnLeft();
+			case TurnRight:
+				return Turtle.turnRight();
+			case Clear(dir):
+				return Turtle.digEmpty(dir);
+			case FullTunnel:
+				return Helper.combine([Turtle.digEmpty.bind(Front), Turtle.forward, Turtle.digEmpty.bind(Down)]);
+			case HalfTunnel:
+				return Helper.combine([Turtle.digEmpty.bind(Front), Turtle.down, Turtle.digEmpty.bind(Front), Turtle.up]);
+		}
+	}
+}
diff --git a/src/lib/turtle/planner/Plan.hx b/src/lib/turtle/planner/Plan.hx
new file mode 100644
index 0000000..1f7d7dc
--- /dev/null
+++ b/src/lib/turtle/planner/Plan.hx
@@ -0,0 +1,142 @@
+package lib.turtle.planner;
+
+import kernel.turtle.TurtleMutex;
+import kernel.ps.ProcessHandle;
+
+using tink.CoreApi;
+
+/**
+	A plan repesents a pice of a squence of turtle commands.
+	It acts like node in a linked list.
+	It also has no interal state and can be reused.
+	Once a plan has been setup it (should be) is immutable.
+**/
+class Plan {
+	private var prev:Null<Plan>;
+	private var delegate:(Executer) -> Void;
+
+	public static function newPlan():Plan {
+		return new Plan();
+	}
+
+	private function new() {}
+
+	/**
+		Do a sequence of actions
+	**/
+	public function act(actions:Array<Action>):Plan {
+		#if debug
+		if (this.delegate != null) {
+			throw new Error("Can't reuse plans");
+		}
+		#end
+		this.delegate = (ex) -> {
+			trace('Act: $actions');
+			for (a in actions) {
+				ex.execute(a);
+			}
+		};
+
+		var next = new Plan();
+		next.prev = this;
+		return next;
+	}
+
+	/**
+		Do a sequence of actions multiple times.
+	**/
+	public function repeat(actions:Array<Action>, times:Int):Plan {
+		#if debug
+		if (this.delegate != null) {
+			throw new Error("Can't reuse plans");
+		}
+		#end
+		this.delegate = (ex) -> {
+			trace('Rep: $actions');
+			for (i in 0...times) {
+				for (a in actions) {
+					ex.execute(a);
+				}
+			}
+		};
+
+		var next = new Plan();
+		next.prev = this;
+		return next;
+	}
+
+	/**
+		Append a plan to the current chain.
+	**/
+	public function subplan(plan:Plan):Plan {
+		#if debug
+		if (this.delegate != null) {
+			throw new Error("Can't reuse plans");
+		}
+		#end
+		this.delegate = (ex) -> {
+			trace('Subplan');
+			plan.execute(ex);
+		};
+
+		var next = new Plan();
+		next.prev = this;
+		return next;
+	}
+
+	/**
+		Execute a plan mutiple times. 
+	**/
+	public function repateSubplan(plan:Plan, times:Int):Plan {
+		#if debug
+		if (this.delegate != null) {
+			throw new Error("Can't reuse plans");
+		}
+		#end
+		this.delegate = (ex) -> {
+			trace('Subplan repeat');
+			for (_ in 0...times) {
+				plan.execute(ex);
+			}
+		};
+
+		var next = new Plan();
+		next.prev = this;
+		return next;
+	}
+
+	private function execute(ex:Executer) {
+		if (this.prev == null) {
+			this.delegate(ex);
+			return;
+		}
+
+		this.prev.execute(ex);
+
+		if (this.delegate != null) { // The last plan in a line does not have a delegate
+			this.delegate(ex);
+		}
+
+		return;
+	}
+
+	public function begin(handle:ProcessHandle) {
+		trace("begin plan");
+		if (!handle.claimTurtleMutex()) {
+			handle.writeLine("Failed to claim turtle mutex");
+			handle.close();
+			return;
+		}
+
+		var ex = new Executer();
+
+		TurtleMutex.runInTThread(() -> {
+			try {
+				this.execute(ex);
+			} catch (e) {
+				handle.writeLine(e.message);
+			}
+			handle.close(true);
+		});
+	}
+}