From ebaf5128fbc87970b2ba1f6157e5da035ae379c8 Mon Sep 17 00:00:00 2001
From: mat <git@matdoes.dev>
Date: Sat, 28 Dec 2024 01:48:25 +0000
Subject: [PATCH] better pathfinder debug messages

---
 azalea-physics/src/collision/mod.rs          |  9 ++++-----
 azalea/examples/testbot/commands/movement.rs | 20 +++++++++++++++++++-
 azalea/src/pathfinder/goals.rs               |  4 ++--
 azalea/src/pathfinder/mod.rs                 | 16 ++++++++++++----
 4 files changed, 37 insertions(+), 12 deletions(-)

diff --git a/azalea-physics/src/collision/mod.rs b/azalea-physics/src/collision/mod.rs
index bff9d6f83..39fc43f84 100644
--- a/azalea-physics/src/collision/mod.rs
+++ b/azalea-physics/src/collision/mod.rs
@@ -356,6 +356,10 @@ pub fn fluid_shape(
             return &BLOCK_SHAPE;
         }
     }
+    if fluid.amount > 9 {
+        warn!("Tried to calculate shape for fluid with height > 9: {fluid:?} at {pos}");
+        return &EMPTY_SHAPE;
+    }
 
     // pre-calculate these in a LazyLock so this function can return a
     // reference instead
@@ -375,11 +379,6 @@ pub fn fluid_shape(
         ]
     });
 
-    if fluid.amount > 9 {
-        warn!("Tried to calculate shape for fluid with height > 9: {fluid:?} at {pos}");
-        return &EMPTY_SHAPE;
-    }
-
     &FLUID_SHAPES[fluid.amount as usize]
 }
 fn calculate_shape_for_fluid(amount: u8) -> VoxelShape {
diff --git a/azalea/examples/testbot/commands/movement.rs b/azalea/examples/testbot/commands/movement.rs
index 3c66fefa9..b0adec8b7 100644
--- a/azalea/examples/testbot/commands/movement.rs
+++ b/azalea/examples/testbot/commands/movement.rs
@@ -3,7 +3,7 @@ use std::time::Duration;
 use azalea::{
     brigadier::prelude::*,
     entity::{EyeHeight, Position},
-    pathfinder::goals::{BlockPosGoal, XZGoal},
+    pathfinder::goals::{BlockPosGoal, RadiusGoal, XZGoal},
     prelude::*,
     BlockPos, SprintDirection, WalkDirection,
 };
@@ -42,6 +42,24 @@ pub fn register(commands: &mut CommandDispatcher<Mutex<CommandSource>>) {
                     1
                 }),
             )))
+            .then(literal("radius").then(argument("radius", float()).then(
+                argument("x", integer()).then(argument("y", integer()).then(
+                    argument("z", integer()).executes(|ctx: &Ctx| {
+                        let source = ctx.source.lock();
+                        let radius = get_float(ctx, "radius").unwrap();
+                        let x = get_integer(ctx, "x").unwrap();
+                        let y = get_integer(ctx, "y").unwrap();
+                        let z = get_integer(ctx, "z").unwrap();
+                        println!("goto radius {radius}, position: {x} {y} {z}");
+                        source.reply("ok");
+                        source.bot.goto(RadiusGoal {
+                            pos: BlockPos::new(x, y, z).center(),
+                            radius,
+                        });
+                        1
+                    }),
+                )),
+            )))
             .then(argument("x", integer()).then(argument("y", integer()).then(
                 argument("z", integer()).executes(|ctx: &Ctx| {
                     let source = ctx.source.lock();
diff --git a/azalea/src/pathfinder/goals.rs b/azalea/src/pathfinder/goals.rs
index 531e40368..0fb72446b 100644
--- a/azalea/src/pathfinder/goals.rs
+++ b/azalea/src/pathfinder/goals.rs
@@ -1,6 +1,6 @@
 //! The goals that a pathfinder can try to reach.
 
-use std::f32::consts::SQRT_2;
+use std::{f32::consts::SQRT_2, fmt::Debug};
 
 use azalea_core::position::{BlockPos, Vec3};
 use azalea_world::ChunkStorage;
@@ -9,7 +9,7 @@ use serde::{Deserialize, Serialize};
 
 use super::costs::{COST_HEURISTIC, FALL_N_BLOCKS_COST, JUMP_ONE_BLOCK_COST};
 
-pub trait Goal {
+pub trait Goal: Debug {
     #[must_use]
     fn heuristic(&self, n: BlockPos) -> f32;
     #[must_use]
diff --git a/azalea/src/pathfinder/mod.rs b/azalea/src/pathfinder/mod.rs
index 013755903..95982215a 100644
--- a/azalea/src/pathfinder/mod.rs
+++ b/azalea/src/pathfinder/mod.rs
@@ -235,6 +235,7 @@ pub fn goto_listener(
         let Ok((mut pathfinder, executing_path, position, instance_name, inventory)) =
             query.get_mut(event.entity)
         else {
+            warn!("got goto event for an entity that can't pathfind");
             continue;
         };
 
@@ -243,6 +244,7 @@ pub fn goto_listener(
             pathfinder.goal = None;
             pathfinder.successors_fn = None;
             pathfinder.is_calculating = false;
+            debug!("already at goal, not pathfinding");
             continue;
         }
 
@@ -262,10 +264,16 @@ pub fn goto_listener(
         } else {
             BlockPos::from(position)
         };
-        info!(
-            "got goto, starting from {start:?} (currently at {:?})",
-            BlockPos::from(position)
-        );
+
+        if start == BlockPos::from(position) {
+            info!("got goto {:?}, starting from {start:?}", event.goal);
+        } else {
+            info!(
+                "got goto {:?}, starting from {start:?} (currently at {:?})",
+                event.goal,
+                BlockPos::from(position)
+            );
+        }
 
         let successors_fn: moves::SuccessorsFn = event.successors_fn;