From 25322aab0e75cf60eca70fd7175b7efcfbb9fbd7 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 19 Dec 2023 18:13:08 -0600
Subject: [PATCH] fixed digging

---
 server/src/blocks.rs | 29 +++++++++++++++++++++++++++--
 server/src/main.rs   |  9 +++++----
 server/src/paths.rs  |  6 ++++--
 server/src/turtle.rs | 13 ++++++++++++-
 4 files changed, 48 insertions(+), 9 deletions(-)

diff --git a/server/src/blocks.rs b/server/src/blocks.rs
index afba9f5..f4c942d 100644
--- a/server/src/blocks.rs
+++ b/server/src/blocks.rs
@@ -5,7 +5,7 @@ use rstar::{self, PointDistance, RTree, RTreeObject, AABB};
 use serde::{Deserialize, Serialize};
 use tokio::sync::{RwLock, RwLockReadGuard, OwnedRwLockReadGuard};
 
-use crate::turtle::TurtleCommand;
+use crate::{turtle::TurtleCommand, paths};
 
 pub type WorldReadLock = OwnedRwLockReadGuard<RTree<Block>>;
 
@@ -34,12 +34,17 @@ impl World {
         self.state.read().await.locate_at_point(&block.into()).is_some_and(|b| b.name != "minecraft:air")
     }
 
+    /// Returns true if a "garbage" block exists at the given point which you are free to destroy
+    pub async fn garbage(&self, block: Vec3) -> bool {
+        self.state.read().await.locate_at_point(&block.into()).is_some_and(|b| paths::difficulty(&b.name).is_some())
+    }
+
     pub async fn lock(self) -> WorldReadLock {
         self.state.read_owned().await
     }
 }
 
-#[derive(Serialize, Deserialize, Clone)]
+#[derive(Serialize, Deserialize, Clone, Debug)]
 pub struct Block {
     pub name: String,
     pub pos: Vec3,
@@ -99,6 +104,26 @@ impl Position {
         }
     }
 
+    /// Command to dig 
+    /// Assumes that "to" can be dug from your position
+    pub fn dig(&self, to: Vec3) -> Option<TurtleCommand> {
+
+        // manhattan distance of 1 required to dig
+        if (self.pos-to).abs().sum()!=1 {
+            return None;
+        }
+
+        // not covered: pointing away from to
+
+        Some(match self.pos.y - to.y {
+            0 => TurtleCommand::Dig,
+            1 => TurtleCommand::DigDown,
+            -1 => TurtleCommand::DigUp,
+            _ => None?
+        })
+    }
+
+
 }
 
 #[derive(Serialize, Deserialize, Clone, Hash, PartialEq, Eq, Copy, Debug)]
diff --git a/server/src/main.rs b/server/src/main.rs
index cf107bf..255c640 100644
--- a/server/src/main.rs
+++ b/server/src/main.rs
@@ -9,7 +9,7 @@ use axum::{
     routing::{get, post},
     Json, Router,
 };
-use blocks::{World, Position};
+use blocks::{World, Position, Vec3};
 use mine::TurtleMineJob;
 use rstar::{self, AABB, RTree};
 
@@ -100,6 +100,7 @@ async fn main() -> Result<(), Error> {
         .route("/turtle/:id/update", post(command))
         .route("/turtle/client.lua", get(client))
         .route("/turtle/:id/setGoal", post(set_goal))
+        .route("/turtle/:id/dig", post(dig))
         .route("/turtle/:id/cancelTask", post(cancel))
         .route("/turtle/:id/info", get(turtle_info))
         //.route("/turtle/:id/placeUp", get(place_up))
@@ -165,13 +166,13 @@ async fn place_up(
 async fn dig(
     Path(id): Path<u32>,
     State(state): State<SharedControl>,
-    Json(req): Json<Position>,
+    Json(req): Json<Vec3>,
 ) -> &'static str {
     let turtle = state.read().await.get_turtle(id).await.unwrap();
     tokio::spawn(
         async move {
-            turtle.goto_adjacent(req.pos).await;
-            turtle.execute(TurtleCommand::Dig).await
+            let pos = turtle.goto_adjacent(req).await.unwrap();
+            turtle.execute(pos.dig(req).unwrap()).await
         }
     );
 
diff --git a/server/src/paths.rs b/server/src/paths.rs
index ed09db1..19f1375 100644
--- a/server/src/paths.rs
+++ b/server/src/paths.rs
@@ -8,7 +8,9 @@ use pathfinding::prelude::astar;
 pub async fn route_facing(from: Position, to: Vec3, world: &World) -> Option<Vec<Position>> {
     let facing = |p: &Position| {
         let ahead = p.dir.unit() + p.pos;
-        to == ahead
+        let above = Vec3::y() + p.pos;
+        let below = -Vec3::y() + p.pos;
+        to == ahead || to == below || to == above
     };
     route_to(from, to, facing, world).await
 }
@@ -87,7 +89,7 @@ const GARBAGE: [&str; 6] = [
 const UNKNOWN: Option<u32> = Some(2);
 
 // time to go somewhere
-fn difficulty(name: &str) -> Option<u32> {
+pub fn difficulty(name: &str) -> Option<u32> {
     if name == "minecraft:air" {
         return Some(1);
     };
diff --git a/server/src/turtle.rs b/server/src/turtle.rs
index 7228c8d..23a8b54 100644
--- a/server/src/turtle.rs
+++ b/server/src/turtle.rs
@@ -7,6 +7,7 @@ use crate::blocks::Vec3;
 use crate::blocks::World;
 use crate::blocks::nearest;
 use crate::mine::TurtleMineJob;
+use crate::paths;
 use crate::paths::route_facing;
 
 use anyhow::Ok;
@@ -217,10 +218,19 @@ impl TurtleCommander {
                 // valid routes will explicitly tell you to break ground
 
                 if world.occupied(next_position.pos).await {
-                    break 'route;
+                    if world.garbage(next_position.pos).await {
+                        self.execute(dbg!(recent.dig(next_position.pos))?).await;
+                    } else {
+                        break 'route;
+                    }
                 }
 
                 let state = self.execute(command).await;
+
+                if let TurtleCommandResponse::Failure =  state.ret {
+                    break 'route;
+                }
+
                 recent = state.pos;
             }
         }
@@ -292,6 +302,7 @@ pub(crate) async fn process_turtle_update(
                 _ => {}
             }
             turtle.queued_movement = cmd.unit(turtle.position.dir);
+            println!("Turtle {}: {cmd:?}", turtle.name.to_str());
             return Ok(cmd);
         }
     }