Add mining to the pathfinder (#122)

* basic pathfinder mining poc

* mining descending and autotool

* pathfinder mining descending

* pathfinder fixes

* allow disabling pathfinder miner and other fixes

* small optimization to avoid chunk vec iter lookup sometimes

* seeded rng in pathfinder bench

* consistently use f32::INFINITY

this brings performance much closer to how it was before

* astar heuristic optimization from baritone

* add downward_move

* fix downward move execute

* avoid liquids and falling blocks when mining

* fix COST_HEURISTIC

* fix to not path through flowing liquids

* only reset pathfinder timeout while mining if the block is close enough

* cache mining costs of block positions

* fix mine_while_at_start and move PathfinderDebugParticles to its own module

* add ReachBlockPosGoal

in other news: azalea's sin/cos functions were broken this whole time and i never noticed

* clippy and add things that i accidentally didn't commit

* improve wording on doc for azalea::pathfinder

4 files changed, 316 insertions, 144 deletions

diff --git a/azalea-world/src/chunk_storage.rs b/azalea-world/src/chunk_storage.rs
index 681f979b..8bc0b32c 100755
--- a/azalea-world/src/chunk_storage.rs
+++ b/azalea-world/src/chunk_storage.rs
@@ -34,7 +34,7 @@ pub struct PartialChunkStorage {
 /// A storage for chunks where they're only stored weakly, so if they're not
 /// actively being used somewhere else they'll be forgotten. This is used for
 /// shared worlds.
-#[derive(Debug)]
+#[derive(Debug, Clone)]
 pub struct ChunkStorage {
     pub height: u32,
     pub min_y: i32,
@@ -514,7 +514,12 @@ impl Default for ChunkStorage {
 /// and the minimum y coordinate of the world.
 #[inline]
 pub fn section_index(y: i32, min_y: i32) -> u32 {
-    assert!(y >= min_y, "y ({y}) must be at least {min_y}");
+    if y < min_y {
+        #[cfg(debug_assertions)]
+        panic!("y ({y}) must be at most {min_y}");
+        #[cfg(not(debug_assertions))]
+        tracing::error!("y ({y}) must be at least {min_y}")
+    };
     let min_section_index = min_y >> 4;
     ((y >> 4) - min_section_index) as u32
 }
diff --git a/azalea-world/src/find_blocks.rs b/azalea-world/src/find_blocks.rs
new file mode 100644
index 00000000..2d2c6d7a
--- /dev/null
+++ b/azalea-world/src/find_blocks.rs
@@ -0,0 +1,306 @@
+use azalea_block::{BlockState, BlockStates};
+use azalea_core::position::{BlockPos, ChunkPos};
+
+use crate::{iterators::ChunkIterator, palette::Palette, ChunkStorage, Instance};
+
+fn palette_maybe_has_block(palette: &Palette, block_states: &BlockStates) -> bool {
+    match &palette {
+        Palette::SingleValue(id) => block_states.contains(&BlockState { id: *id }),
+        Palette::Linear(ids) => ids
+            .iter()
+            .any(|&id| block_states.contains(&BlockState { id })),
+        Palette::Hashmap(ids) => ids
+            .iter()
+            .any(|&id| block_states.contains(&BlockState { id })),
+        Palette::Global => true,
+    }
+}
+
+impl Instance {
+    /// Find the coordinates of a block in the world.
+    ///
+    /// Note that this is sorted by `x+y+z` and not `x^2+y^2+z^2` for
+    /// optimization purposes.
+    ///
+    /// ```
+    /// # fn example(client: &azalea_client::Client) {
+    /// client.world().read().find_block(client.position(), &azalea_registry::Block::Chest.into());
+    /// # }
+    /// ```
+    pub fn find_block(
+        &self,
+        nearest_to: impl Into<BlockPos>,
+        block_states: &BlockStates,
+    ) -> Option<BlockPos> {
+        // iterate over every chunk in a 3d spiral pattern
+        // and then check the palette for the block state
+
+        let nearest_to: BlockPos = nearest_to.into();
+        let start_chunk: ChunkPos = (&nearest_to).into();
+        let mut iter = ChunkIterator::new(start_chunk, 32);
+
+        let mut nearest_found_pos: Option<BlockPos> = None;
+        let mut nearest_found_distance = 0;
+
+        // we do `while` instead of `for` so we can access iter later
+        while let Some(chunk_pos) = iter.next() {
+            let Some(chunk) = self.chunks.get(&chunk_pos) else {
+                // if the chunk isn't loaded then we skip it.
+                // we don't just return since it *could* cause issues if there's a random
+                // unloaded chunk and then more that are loaded.
+                // unlikely but still something to consider, and it's not like this slows it
+                // down much anyways.
+                continue;
+            };
+
+            for (section_index, section) in chunk.read().sections.iter().enumerate() {
+                let maybe_has_block =
+                    palette_maybe_has_block(&section.states.palette, block_states);
+                if !maybe_has_block {
+                    continue;
+                }
+
+                for i in 0..4096 {
+                    let block_state = section.states.get_at_index(i);
+                    let block_state = BlockState { id: block_state };
+
+                    if block_states.contains(&block_state) {
+                        let (section_x, section_y, section_z) = section.states.coords_from_index(i);
+                        let (x, y, z) = (
+                            chunk_pos.x * 16 + (section_x as i32),
+                            self.chunks.min_y + (section_index * 16) as i32 + section_y as i32,
+                            chunk_pos.z * 16 + (section_z as i32),
+                        );
+                        let this_block_pos = BlockPos { x, y, z };
+                        let this_block_distance = (nearest_to - this_block_pos).length_manhattan();
+                        // only update if it's closer
+                        if nearest_found_pos.is_none()
+                            || this_block_distance < nearest_found_distance
+                        {
+                            nearest_found_pos = Some(this_block_pos);
+                            nearest_found_distance = this_block_distance;
+                        }
+                    }
+                }
+            }
+
+            if let Some(nearest_found_pos) = nearest_found_pos {
+                // this is required because find_block searches chunk-by-chunk, which can cause
+                // us to find blocks first that aren't actually the closest
+                let required_chunk_distance = u32::max(
+                    u32::max(
+                        (chunk_pos.x - start_chunk.x).unsigned_abs(),
+                        (chunk_pos.z - start_chunk.z).unsigned_abs(),
+                    ),
+                    (nearest_to.y - nearest_found_pos.y)
+                        .unsigned_abs()
+                        .div_ceil(16),
+                ) + 1;
+                let nearest_chunk_distance = iter.layer;
+
+                // if we found the position and there's no chance there's something closer,
+                // return it
+                if nearest_chunk_distance > required_chunk_distance {
+                    return Some(nearest_found_pos);
+                }
+            }
+        }
+
+        if nearest_found_pos.is_some() {
+            nearest_found_pos
+        } else {
+            None
+        }
+    }
+
+    /// Find all the coordinates of a block in the world.
+    ///
+    /// This returns an iterator that yields the [`BlockPos`]s of blocks that
+    /// are in the given block states. It's sorted by `x+y+z`.
+    pub fn find_blocks<'a>(
+        &'a self,
+        nearest_to: impl Into<BlockPos>,
+        block_states: &'a BlockStates,
+    ) -> FindBlocks<'a> {
+        FindBlocks::new(nearest_to.into(), &self.chunks, block_states)
+    }
+}
+
+pub struct FindBlocks<'a> {
+    nearest_to: BlockPos,
+    start_chunk: ChunkPos,
+    chunk_iterator: ChunkIterator,
+    chunks: &'a ChunkStorage,
+    block_states: &'a BlockStates,
+
+    queued: Vec<BlockPos>,
+}
+
+impl<'a> FindBlocks<'a> {
+    pub fn new(
+        nearest_to: BlockPos,
+        chunks: &'a ChunkStorage,
+        block_states: &'a BlockStates,
+    ) -> Self {
+        let start_chunk: ChunkPos = (&nearest_to).into();
+        Self {
+            nearest_to,
+            start_chunk,
+            chunk_iterator: ChunkIterator::new(start_chunk, 32),
+            chunks,
+            block_states,
+
+            queued: Vec::new(),
+        }
+    }
+}
+
+impl<'a> Iterator for FindBlocks<'a> {
+    type Item = BlockPos;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if let Some(queued) = self.queued.pop() {
+            return Some(queued);
+        }
+
+        let mut found = Vec::new();
+
+        let mut nearest_found_pos: Option<BlockPos> = None;
+        let mut nearest_found_distance = 0;
+
+        while let Some(chunk_pos) = self.chunk_iterator.next() {
+            let Some(chunk) = self.chunks.get(&chunk_pos) else {
+                // if the chunk isn't loaded then we skip it.
+                // we don't just return since it *could* cause issues if there's a random
+                // unloaded chunk and then more that are loaded.
+                // unlikely but still something to consider, and it's not like this slows it
+                // down much anyways.
+                continue;
+            };
+
+            for (section_index, section) in chunk.read().sections.iter().enumerate() {
+                let maybe_has_block =
+                    palette_maybe_has_block(&section.states.palette, self.block_states);
+                if !maybe_has_block {
+                    continue;
+                }
+
+                for i in 0..4096 {
+                    let block_state = section.states.get_at_index(i);
+                    let block_state = BlockState { id: block_state };
+
+                    if self.block_states.contains(&block_state) {
+                        let (section_x, section_y, section_z) = section.states.coords_from_index(i);
+                        let (x, y, z) = (
+                            chunk_pos.x * 16 + (section_x as i32),
+                            self.chunks.min_y + (section_index * 16) as i32 + section_y as i32,
+                            chunk_pos.z * 16 + (section_z as i32),
+                        );
+                        let this_block_pos = BlockPos { x, y, z };
+                        let this_block_distance =
+                            (self.nearest_to - this_block_pos).length_manhattan();
+
+                        found.push((this_block_pos, this_block_distance));
+
+                        if nearest_found_pos.is_none()
+                            || this_block_distance < nearest_found_distance
+                        {
+                            nearest_found_pos = Some(this_block_pos);
+                            nearest_found_distance = this_block_distance;
+                        }
+                    }
+                }
+            }
+
+            if let Some(nearest_found_pos) = nearest_found_pos {
+                // this is required because find_block searches chunk-by-chunk, which can cause
+                // us to find blocks first that aren't actually the closest
+                let required_chunk_distance = u32::max(
+                    u32::max(
+                        (chunk_pos.x - self.start_chunk.x).unsigned_abs(),
+                        (chunk_pos.z - self.start_chunk.z).unsigned_abs(),
+                    ),
+                    (self.nearest_to.y - nearest_found_pos.y)
+                        .unsigned_abs()
+                        .div_ceil(16),
+                ) + 1;
+                let nearest_chunk_distance = self.chunk_iterator.layer;
+
+                // if we found the position and there's no chance there's something closer,
+                // return it
+                if nearest_chunk_distance > required_chunk_distance {
+                    // sort so nearest is at the end
+                    found.sort_unstable_by_key(|(_, distance)| u32::MAX - distance);
+
+                    self.queued = found.into_iter().map(|(pos, _)| pos).collect();
+                    return self.queued.pop();
+                }
+            }
+        }
+
+        None
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use azalea_registry::Block;
+
+    use crate::{Chunk, PartialChunkStorage};
+
+    use super::*;
+
+    #[test]
+    fn find_block() {
+        let mut instance = Instance::default();
+
+        let chunk_storage = &mut instance.chunks;
+        let mut partial_chunk_storage = PartialChunkStorage::default();
+
+        // block at (17, 0, 0) and (0, 18, 0)
+
+        partial_chunk_storage.set(
+            &ChunkPos { x: 0, z: 0 },
+            Some(Chunk::default()),
+            chunk_storage,
+        );
+        partial_chunk_storage.set(
+            &ChunkPos { x: 1, z: 0 },
+            Some(Chunk::default()),
+            chunk_storage,
+        );
+
+        chunk_storage.set_block_state(&BlockPos { x: 17, y: 0, z: 0 }, Block::Stone.into());
+        chunk_storage.set_block_state(&BlockPos { x: 0, y: 18, z: 0 }, Block::Stone.into());
+
+        let pos = instance.find_block(BlockPos { x: 0, y: 0, z: 0 }, &Block::Stone.into());
+        assert_eq!(pos, Some(BlockPos { x: 17, y: 0, z: 0 }));
+    }
+
+    #[test]
+    fn find_block_next_to_chunk_border() {
+        let mut instance = Instance::default();
+
+        let chunk_storage = &mut instance.chunks;
+        let mut partial_chunk_storage = PartialChunkStorage::default();
+
+        // block at (-1, 0, 0) and (15, 0, 0)
+
+        partial_chunk_storage.set(
+            &ChunkPos { x: -1, z: 0 },
+            Some(Chunk::default()),
+            chunk_storage,
+        );
+        partial_chunk_storage.set(
+            &ChunkPos { x: 0, z: 0 },
+            Some(Chunk::default()),
+            chunk_storage,
+        );
+
+        chunk_storage.set_block_state(&BlockPos { x: -1, y: 0, z: 0 }, Block::Stone.into());
+        chunk_storage.set_block_state(&BlockPos { x: 15, y: 0, z: 0 }, Block::Stone.into());
+
+        let pos = instance.find_block(BlockPos { x: 0, y: 0, z: 0 }, &Block::Stone.into());
+        assert_eq!(pos, Some(BlockPos { x: -1, y: 0, z: 0 }));
+    }
+}
diff --git a/azalea-world/src/lib.rs b/azalea-world/src/lib.rs
index 6677b326..8514ce24 100644
--- a/azalea-world/src/lib.rs
+++ b/azalea-world/src/lib.rs
@@ -4,6 +4,7 @@
 mod bit_storage;
 pub mod chunk_storage;
 mod container;
+pub mod find_blocks;
 pub mod heightmap;
 pub mod iterators;
 pub mod palette;
diff --git a/azalea-world/src/world.rs b/azalea-world/src/world.rs
index 7b6854f7..84a5857c 100644
--- a/azalea-world/src/world.rs
+++ b/azalea-world/src/world.rs
@@ -1,5 +1,5 @@
-use crate::{iterators::ChunkIterator, palette::Palette, ChunkStorage, PartialChunkStorage};
-use azalea_block::{BlockState, BlockStates, FluidState};
+use crate::{ChunkStorage, PartialChunkStorage};
+use azalea_block::{BlockState, FluidState};
 use azalea_core::position::{BlockPos, ChunkPos};
 use azalea_core::registry_holder::RegistryHolder;
 use bevy_ecs::{component::Component, entity::Entity};
@@ -104,110 +104,6 @@ impl Instance {
     pub fn set_block_state(&self, pos: &BlockPos, state: BlockState) -> Option<BlockState> {
         self.chunks.set_block_state(pos, state)
     }
-
-    /// Find the coordinates of a block in the world.
-    ///
-    /// Note that this is sorted by `x+y+z` and not `x^2+y^2+z^2` for
-    /// optimization purposes.
-    ///
-    /// ```
-    /// # fn example(client: &azalea_client::Client) {
-    /// client.world().read().find_block(client.position(), &azalea_registry::Block::Chest.into());
-    /// # }
-    /// ```
-    pub fn find_block(
-        &self,
-        nearest_to: impl Into<BlockPos>,
-        block_states: &BlockStates,
-    ) -> Option<BlockPos> {
-        // iterate over every chunk in a 3d spiral pattern
-        // and then check the palette for the block state
-
-        let nearest_to: BlockPos = nearest_to.into();
-        let start_chunk: ChunkPos = (&nearest_to).into();
-        let mut iter = ChunkIterator::new(start_chunk, 32);
-
-        let mut nearest_found_pos: Option<BlockPos> = None;
-        let mut nearest_found_distance = 0;
-
-        // we do `while` instead of `for` so we can access iter later
-        while let Some(chunk_pos) = iter.next() {
-            let Some(chunk) = self.chunks.get(&chunk_pos) else {
-                // if the chunk isn't loaded then we skip it.
-                // we don't just return since it *could* cause issues if there's a random
-                // unloaded chunk and then more that are loaded.
-                // unlikely but still something to consider, and it's not like this slows it
-                // down much anyways.
-                continue;
-            };
-
-            for (section_index, section) in chunk.read().sections.iter().enumerate() {
-                let maybe_has_block = match &section.states.palette {
-                    Palette::SingleValue(id) => block_states.contains(&BlockState { id: *id }),
-                    Palette::Linear(ids) => ids
-                        .iter()
-                        .any(|&id| block_states.contains(&BlockState { id })),
-                    Palette::Hashmap(ids) => ids
-                        .iter()
-                        .any(|&id| block_states.contains(&BlockState { id })),
-                    Palette::Global => true,
-                };
-                if !maybe_has_block {
-                    continue;
-                }
-
-                for i in 0..4096 {
-                    let block_state = section.states.get_at_index(i);
-                    let block_state = BlockState { id: block_state };
-
-                    if block_states.contains(&block_state) {
-                        let (section_x, section_y, section_z) = section.states.coords_from_index(i);
-                        let (x, y, z) = (
-                            chunk_pos.x * 16 + (section_x as i32),
-                            self.chunks.min_y + (section_index * 16) as i32 + section_y as i32,
-                            chunk_pos.z * 16 + (section_z as i32),
-                        );
-                        let this_block_pos = BlockPos { x, y, z };
-                        let this_block_distance = (nearest_to - this_block_pos).length_manhattan();
-                        // only update if it's closer
-                        if nearest_found_pos.is_none()
-                            || this_block_distance < nearest_found_distance
-                        {
-                            nearest_found_pos = Some(this_block_pos);
-                            nearest_found_distance = this_block_distance;
-                        }
-                    }
-                }
-            }
-
-            if let Some(nearest_found_pos) = nearest_found_pos {
-                // this is required because find_block searches chunk-by-chunk, which can cause
-                // us to find blocks first that aren't actually the closest
-                let required_chunk_distance = u32::max(
-                    u32::max(
-                        (chunk_pos.x - start_chunk.x).unsigned_abs(),
-                        (chunk_pos.z - start_chunk.z).unsigned_abs(),
-                    ),
-                    (nearest_to.y - nearest_found_pos.y)
-                        .unsigned_abs()
-                        .div_ceil(16),
-                );
-                let nearest_chunk_distance = iter.layer;
-
-                // if we found the position and there's no chance there's something closer,
-                // return it
-                if nearest_chunk_distance >= required_chunk_distance {
-                    return Some(nearest_found_pos);
-                }
-            }
-        }
-
-        if nearest_found_pos.is_some() {
-            nearest_found_pos
-        } else {
-            None
-        }
-    }
 }
 
 impl Debug for PartialInstance {
@@ -244,39 +140,3 @@ impl From<ChunkStorage> for Instance {
         }
     }
 }
-
-#[cfg(test)]
-mod tests {
-    use azalea_registry::Block;
-
-    use crate::Chunk;
-
-    use super::*;
-
-    #[test]
-    fn find_block() {
-        let mut instance = Instance::default();
-
-        let chunk_storage = &mut instance.chunks;
-        let mut partial_chunk_storage = PartialChunkStorage::default();
-
-        // block at (17, 0, 0) and (0, 18, 0)
-
-        partial_chunk_storage.set(
-            &ChunkPos { x: 0, z: 0 },
-            Some(Chunk::default()),
-            chunk_storage,
-        );
-        partial_chunk_storage.set(
-            &ChunkPos { x: 1, z: 0 },
-            Some(Chunk::default()),
-            chunk_storage,
-        );
-
-        chunk_storage.set_block_state(&BlockPos { x: 17, y: 0, z: 0 }, Block::Stone.into());
-        chunk_storage.set_block_state(&BlockPos { x: 0, y: 18, z: 0 }, Block::Stone.into());
-
-        let pos = instance.find_block(BlockPos { x: 0, y: 0, z: 0 }, &Block::Stone.into());
-        assert_eq!(pos, Some(BlockPos { x: 17, y: 0, z: 0 }));
-    }
-}