azalea/pathfinder/
mod.rs

1//! A pathfinding plugin to make bots able to traverse the world.
2//!
3//! Much of this code is based on [Baritone](https://github.com/cabaletta/baritone).
4
5pub mod astar;
6pub mod costs;
7pub mod custom_state;
8pub mod debug;
9pub mod goals;
10mod goto_event;
11pub mod mining;
12pub mod moves;
13pub mod rel_block_pos;
14pub mod simulation;
15#[cfg(test)]
16mod tests;
17pub mod world;
18
19use std::{
20    cmp,
21    collections::VecDeque,
22    ops::RangeInclusive,
23    sync::{
24        Arc,
25        atomic::{self, AtomicUsize},
26    },
27    thread,
28    time::{Duration, Instant},
29};
30
31use astar::{Edge, PathfinderTimeout};
32use azalea_client::{
33    StartSprintEvent, StartWalkEvent,
34    inventory::{Inventory, InventorySet, SetSelectedHotbarSlotEvent},
35    local_player::InstanceHolder,
36    mining::{Mining, MiningSet, StartMiningBlockEvent},
37    movement::MoveEventsSet,
38};
39use azalea_core::{position::BlockPos, tick::GameTick};
40use azalea_entity::{LocalEntity, Physics, Position, metadata::Player};
41use azalea_physics::PhysicsSet;
42use azalea_world::{InstanceContainer, InstanceName};
43use bevy_app::{PreUpdate, Update};
44use bevy_ecs::prelude::*;
45use bevy_tasks::{AsyncComputeTaskPool, Task};
46use custom_state::{CustomPathfinderState, CustomPathfinderStateRef};
47use futures_lite::future;
48use goals::BlockPosGoal;
49pub use goto_event::{GotoEvent, PathfinderOpts};
50use parking_lot::RwLock;
51use rel_block_pos::RelBlockPos;
52use tokio::sync::broadcast::error::RecvError;
53use tracing::{debug, error, info, trace, warn};
54
55use self::{
56    debug::debug_render_path_with_particles,
57    goals::Goal,
58    mining::MiningCache,
59    moves::{ExecuteCtx, IsReachedCtx, SuccessorsFn},
60};
61use crate::{
62    BotClientExt, WalkDirection,
63    app::{App, Plugin},
64    bot::{JumpEvent, LookAtEvent},
65    ecs::{
66        component::Component,
67        entity::Entity,
68        event::{EventReader, EventWriter},
69        query::{With, Without},
70        system::{Commands, Query, Res},
71    },
72    pathfinder::{astar::a_star, moves::PathfinderCtx, world::CachedWorld},
73};
74
75#[derive(Clone, Default)]
76pub struct PathfinderPlugin;
77impl Plugin for PathfinderPlugin {
78    fn build(&self, app: &mut App) {
79        app.add_event::<GotoEvent>()
80            .add_event::<PathFoundEvent>()
81            .add_event::<StopPathfindingEvent>()
82            .add_systems(
83                // putting systems in the GameTick schedule makes them run every Minecraft tick
84                // (every 50 milliseconds).
85                GameTick,
86                (
87                    timeout_movement,
88                    check_for_path_obstruction,
89                    check_node_reached,
90                    tick_execute_path,
91                    debug_render_path_with_particles,
92                    recalculate_near_end_of_path,
93                    recalculate_if_has_goal_but_no_path,
94                )
95                    .chain()
96                    .after(PhysicsSet)
97                    .after(azalea_client::movement::send_position)
98                    .after(MiningSet),
99            )
100            .add_systems(PreUpdate, add_default_pathfinder)
101            .add_systems(
102                Update,
103                (
104                    goto_listener,
105                    handle_tasks,
106                    stop_pathfinding_on_instance_change,
107                    path_found_listener,
108                    handle_stop_pathfinding_event,
109                )
110                    .chain()
111                    .before(MoveEventsSet)
112                    .before(InventorySet),
113            );
114    }
115}
116
117/// A component that makes this client able to pathfind.
118#[derive(Component, Default, Clone)]
119#[non_exhaustive]
120pub struct Pathfinder {
121    pub goal: Option<Arc<dyn Goal>>,
122    pub opts: Option<PathfinderOpts>,
123    pub is_calculating: bool,
124    pub goto_id: Arc<AtomicUsize>,
125}
126
127/// A component that's present on clients that are actively following a
128/// pathfinder path.
129#[derive(Component, Clone)]
130pub struct ExecutingPath {
131    pub path: VecDeque<astar::Edge<BlockPos, moves::MoveData>>,
132    pub queued_path: Option<VecDeque<astar::Edge<BlockPos, moves::MoveData>>>,
133    pub last_reached_node: BlockPos,
134    pub last_node_reached_at: Instant,
135    pub is_path_partial: bool,
136}
137
138#[derive(Event, Clone, Debug)]
139#[non_exhaustive]
140pub struct PathFoundEvent {
141    pub entity: Entity,
142    pub start: BlockPos,
143    pub path: Option<VecDeque<astar::Edge<BlockPos, moves::MoveData>>>,
144    pub is_partial: bool,
145    pub successors_fn: SuccessorsFn,
146    pub allow_mining: bool,
147}
148
149#[allow(clippy::type_complexity)]
150pub fn add_default_pathfinder(
151    mut commands: Commands,
152    mut query: Query<Entity, (Without<Pathfinder>, With<LocalEntity>, With<Player>)>,
153) {
154    for entity in &mut query {
155        commands.entity(entity).insert(Pathfinder::default());
156    }
157}
158
159pub trait PathfinderClientExt {
160    /// Pathfind to the given goal and wait until either the target is reached
161    /// or the pathfinding is canceled.
162    ///
163    /// You can use [`Self::start_goto`] instead if you don't want to wait.
164    ///
165    /// ```
166    /// # use azalea::prelude::*;
167    /// # use azalea::{BlockPos, pathfinder::goals::BlockPosGoal};
168    /// # async fn example(bot: &Client) {
169    /// bot.goto(BlockPosGoal(BlockPos::new(0, 70, 0))).await;
170    /// # }
171    /// ```
172    fn goto(&self, goal: impl Goal + 'static) -> impl Future<Output = ()>;
173    /// Same as [`Self::goto`], but allows you to set custom options for
174    /// pathfinding, including disabling mining and setting custom moves.
175    ///
176    /// ```
177    /// # use azalea::prelude::*;
178    /// # use azalea::{BlockPos, pathfinder::{goals::BlockPosGoal, PathfinderOpts}};
179    /// # async fn example(bot: &Client) {
180    /// bot.goto_with_opts(
181    ///     BlockPosGoal(BlockPos::new(0, 70, 0)),
182    ///     PathfinderOpts::new().allow_mining(false),
183    /// )
184    /// .await;
185    /// # }
186    /// ```
187    fn goto_with_opts(
188        &self,
189        goal: impl Goal + 'static,
190        opts: PathfinderOpts,
191    ) -> impl Future<Output = ()>;
192    /// Start pathfinding to a given goal.
193    ///
194    /// ```
195    /// # use azalea::prelude::*;
196    /// # use azalea::{BlockPos, pathfinder::goals::BlockPosGoal};
197    /// # fn example(bot: &Client) {
198    /// bot.start_goto(BlockPosGoal(BlockPos::new(0, 70, 0)));
199    /// # }
200    /// ```
201    fn start_goto(&self, goal: impl Goal + 'static);
202    /// Same as [`Self::start_goto`], but allows you to set custom
203    /// options for pathfinding, including disabling mining and setting custom
204    /// moves.
205    ///
206    /// Also see [`Self::goto_with_opts`].
207    fn start_goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts);
208    /// Stop calculating a path, and stop moving once the current movement is
209    /// finished.
210    ///
211    /// This behavior exists to prevent the bot from taking damage if
212    /// `stop_pathfinding` was called while executing a parkour jump, but if
213    /// it's undesirable then you may want to consider using
214    /// [`Self::force_stop_pathfinding`] instead.
215    fn stop_pathfinding(&self);
216    /// Stop calculating a path and stop executing the current movement
217    /// immediately.
218    fn force_stop_pathfinding(&self);
219    /// Waits forever until the bot no longer has a pathfinder goal.
220    fn wait_until_goto_target_reached(&self) -> impl Future<Output = ()>;
221    /// Returns true if the pathfinder has no active goal and isn't calculating
222    /// a path.
223    fn is_goto_target_reached(&self) -> bool;
224}
225
226impl PathfinderClientExt for azalea_client::Client {
227    async fn goto(&self, goal: impl Goal + 'static) {
228        self.goto_with_opts(goal, PathfinderOpts::new()).await;
229    }
230    async fn goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts) {
231        self.start_goto_with_opts(goal, opts);
232        self.wait_until_goto_target_reached().await;
233    }
234    fn start_goto(&self, goal: impl Goal + 'static) {
235        self.start_goto_with_opts(goal, PathfinderOpts::new());
236    }
237    fn start_goto_with_opts(&self, goal: impl Goal + 'static, opts: PathfinderOpts) {
238        self.ecs
239            .lock()
240            .send_event(GotoEvent::new(self.entity, goal, opts));
241    }
242    fn stop_pathfinding(&self) {
243        self.ecs.lock().send_event(StopPathfindingEvent {
244            entity: self.entity,
245            force: false,
246        });
247    }
248    fn force_stop_pathfinding(&self) {
249        self.ecs.lock().send_event(StopPathfindingEvent {
250            entity: self.entity,
251            force: true,
252        });
253    }
254    async fn wait_until_goto_target_reached(&self) {
255        // we do this to make sure the event got handled before we start checking
256        // is_goto_target_reached
257        self.wait_updates(1).await;
258
259        let mut tick_broadcaster = self.get_tick_broadcaster();
260        while !self.is_goto_target_reached() {
261            // check every tick
262            match tick_broadcaster.recv().await {
263                Ok(_) => (),
264                Err(RecvError::Closed) => return,
265                Err(err) => warn!("{err}"),
266            };
267        }
268    }
269    fn is_goto_target_reached(&self) -> bool {
270        self.map_get_component::<Pathfinder, _>(|p| p.goal.is_none() && !p.is_calculating)
271            .unwrap_or(true)
272    }
273}
274
275#[derive(Component)]
276pub struct ComputePath(Task<Option<PathFoundEvent>>);
277
278#[allow(clippy::type_complexity)]
279pub fn goto_listener(
280    mut commands: Commands,
281    mut events: EventReader<GotoEvent>,
282    mut query: Query<(
283        &mut Pathfinder,
284        Option<&ExecutingPath>,
285        &Position,
286        &InstanceName,
287        &Inventory,
288        Option<&CustomPathfinderState>,
289    )>,
290    instance_container: Res<InstanceContainer>,
291) {
292    let thread_pool = AsyncComputeTaskPool::get();
293
294    for event in events.read() {
295        let Ok((mut pathfinder, executing_path, position, instance_name, inventory, custom_state)) =
296            query.get_mut(event.entity)
297        else {
298            warn!("got goto event for an entity that can't pathfind");
299            continue;
300        };
301
302        if event.goal.success(BlockPos::from(position)) {
303            // we're already at the goal, nothing to do
304            pathfinder.goal = None;
305            pathfinder.opts = None;
306            pathfinder.is_calculating = false;
307            debug!("already at goal, not pathfinding");
308            continue;
309        }
310
311        // we store the goal so it can be recalculated later if necessary
312        pathfinder.goal = Some(event.goal.clone());
313        pathfinder.opts = Some(event.opts.clone());
314        pathfinder.is_calculating = true;
315
316        let start = if let Some(executing_path) = executing_path
317            && let Some(final_node) = executing_path.path.back()
318        {
319            // if we're currently pathfinding and got a goto event, start a little ahead
320            executing_path
321                .path
322                .get(50)
323                .unwrap_or(final_node)
324                .movement
325                .target
326        } else {
327            BlockPos::from(position)
328        };
329
330        if start == BlockPos::from(position) {
331            info!("got goto {:?}, starting from {start:?}", event.goal);
332        } else {
333            info!(
334                "got goto {:?}, starting from {start:?} (currently at {:?})",
335                event.goal,
336                BlockPos::from(position)
337            );
338        }
339
340        let world_lock = instance_container
341            .get(instance_name)
342            .expect("Entity tried to pathfind but the entity isn't in a valid world");
343
344        let goal = event.goal.clone();
345        let entity = event.entity;
346
347        let goto_id_atomic = pathfinder.goto_id.clone();
348
349        let allow_mining = event.opts.allow_mining;
350        let mining_cache = MiningCache::new(if allow_mining {
351            Some(inventory.inventory_menu.clone())
352        } else {
353            None
354        });
355
356        let custom_state = custom_state.cloned().unwrap_or_default();
357        let opts = event.opts.clone();
358        let task = thread_pool.spawn(async move {
359            calculate_path(CalculatePathCtx {
360                entity,
361                start,
362                goal,
363                world_lock,
364                goto_id_atomic,
365                mining_cache,
366                custom_state,
367                opts,
368            })
369        });
370
371        commands.entity(event.entity).insert(ComputePath(task));
372    }
373}
374
375pub struct CalculatePathCtx {
376    pub entity: Entity,
377    pub start: BlockPos,
378    pub goal: Arc<dyn Goal>,
379    pub world_lock: Arc<RwLock<azalea_world::Instance>>,
380    pub goto_id_atomic: Arc<AtomicUsize>,
381    pub mining_cache: MiningCache,
382    pub custom_state: CustomPathfinderState,
383
384    pub opts: PathfinderOpts,
385}
386
387/// Calculate the [`PathFoundEvent`] for the given pathfinder options.
388///
389/// You usually want to just use [`PathfinderClientExt::goto`] or send a
390/// [`GotoEvent`] instead of calling this directly.
391///
392/// You are expected to immediately send the `PathFoundEvent` you received after
393/// calling this function. `None` will be returned if the pathfinding was
394/// interrupted by another path calculation.
395pub fn calculate_path(ctx: CalculatePathCtx) -> Option<PathFoundEvent> {
396    debug!("start: {:?}", ctx.start);
397
398    let goto_id = ctx.goto_id_atomic.fetch_add(1, atomic::Ordering::SeqCst) + 1;
399
400    let origin = ctx.start;
401    let cached_world = CachedWorld::new(ctx.world_lock, origin);
402    let successors = |pos: RelBlockPos| {
403        call_successors_fn(
404            &cached_world,
405            &ctx.mining_cache,
406            &ctx.custom_state.0.read(),
407            ctx.opts.successors_fn,
408            pos,
409        )
410    };
411
412    let start_time = Instant::now();
413
414    let astar::Path {
415        movements,
416        is_partial,
417    } = a_star(
418        RelBlockPos::get_origin(origin),
419        |n| ctx.goal.heuristic(n.apply(origin)),
420        successors,
421        |n| ctx.goal.success(n.apply(origin)),
422        ctx.opts.min_timeout,
423        ctx.opts.max_timeout,
424    );
425    let end_time = Instant::now();
426    debug!("partial: {is_partial:?}");
427    let duration = end_time - start_time;
428    if is_partial {
429        if movements.is_empty() {
430            info!("Pathfinder took {duration:?} (empty path)");
431        } else {
432            info!("Pathfinder took {duration:?} (incomplete path)");
433        }
434        // wait a bit so it's not a busy loop
435        thread::sleep(Duration::from_millis(100));
436    } else {
437        info!("Pathfinder took {duration:?}");
438    }
439
440    debug!("Path:");
441    for movement in &movements {
442        debug!("  {}", movement.target.apply(origin));
443    }
444
445    let path = movements.into_iter().collect::<VecDeque<_>>();
446
447    let goto_id_now = ctx.goto_id_atomic.load(atomic::Ordering::SeqCst);
448    if goto_id != goto_id_now {
449        // we must've done another goto while calculating this path, so throw it away
450        warn!("finished calculating a path, but it's outdated");
451        return None;
452    }
453
454    if path.is_empty() && is_partial {
455        debug!("this path is empty, we might be stuck :(");
456    }
457
458    let mut mapped_path = VecDeque::with_capacity(path.len());
459    let mut current_position = RelBlockPos::get_origin(origin);
460    for movement in path {
461        let mut found_edge = None;
462        for edge in successors(current_position) {
463            if edge.movement.target == movement.target {
464                found_edge = Some(edge);
465                break;
466            }
467        }
468
469        let found_edge = found_edge.expect(
470            "path should always still be possible because we're using the same world cache",
471        );
472        current_position = found_edge.movement.target;
473
474        // we don't just clone the found_edge because we're using BlockPos instead of
475        // RelBlockPos as the target type
476        mapped_path.push_back(Edge {
477            movement: astar::Movement {
478                target: movement.target.apply(origin),
479                data: movement.data,
480            },
481            cost: found_edge.cost,
482        });
483    }
484
485    Some(PathFoundEvent {
486        entity: ctx.entity,
487        start: ctx.start,
488        path: Some(mapped_path),
489        is_partial,
490        successors_fn: ctx.opts.successors_fn,
491        allow_mining: ctx.opts.allow_mining,
492    })
493}
494
495// poll the tasks and send the PathFoundEvent if they're done
496pub fn handle_tasks(
497    mut commands: Commands,
498    mut transform_tasks: Query<(Entity, &mut ComputePath)>,
499    mut path_found_events: EventWriter<PathFoundEvent>,
500) {
501    for (entity, mut task) in &mut transform_tasks {
502        if let Some(optional_path_found_event) = future::block_on(future::poll_once(&mut task.0)) {
503            if let Some(path_found_event) = optional_path_found_event {
504                path_found_events.write(path_found_event);
505            }
506
507            // Task is complete, so remove task component from entity
508            commands.entity(entity).remove::<ComputePath>();
509        }
510    }
511}
512
513// set the path for the target entity when we get the PathFoundEvent
514#[allow(clippy::type_complexity)]
515pub fn path_found_listener(
516    mut events: EventReader<PathFoundEvent>,
517    mut query: Query<(
518        &mut Pathfinder,
519        Option<&mut ExecutingPath>,
520        &InstanceName,
521        &Inventory,
522        Option<&CustomPathfinderState>,
523    )>,
524    instance_container: Res<InstanceContainer>,
525    mut commands: Commands,
526) {
527    for event in events.read() {
528        let (mut pathfinder, executing_path, instance_name, inventory, custom_state) = query
529            .get_mut(event.entity)
530            .expect("Path found for an entity that doesn't have a pathfinder");
531        if let Some(path) = &event.path {
532            if let Some(mut executing_path) = executing_path {
533                let mut new_path = VecDeque::new();
534
535                // combine the old and new paths if the first node of the new path is a
536                // successor of the last node of the old path
537                if let Some(last_node_of_current_path) = executing_path.path.back() {
538                    let world_lock = instance_container
539                        .get(instance_name)
540                        .expect("Entity tried to pathfind but the entity isn't in a valid world");
541                    let origin = event.start;
542                    let successors_fn: moves::SuccessorsFn = event.successors_fn;
543                    let cached_world = CachedWorld::new(world_lock, origin);
544                    let mining_cache = MiningCache::new(if event.allow_mining {
545                        Some(inventory.inventory_menu.clone())
546                    } else {
547                        None
548                    });
549                    let custom_state = custom_state.cloned().unwrap_or_default();
550                    let custom_state_ref = custom_state.0.read();
551                    let successors = |pos: RelBlockPos| {
552                        call_successors_fn(
553                            &cached_world,
554                            &mining_cache,
555                            &custom_state_ref,
556                            successors_fn,
557                            pos,
558                        )
559                    };
560
561                    if let Some(first_node_of_new_path) = path.front() {
562                        let last_target_of_current_path = RelBlockPos::from_origin(
563                            origin,
564                            last_node_of_current_path.movement.target,
565                        );
566                        let first_target_of_new_path = RelBlockPos::from_origin(
567                            origin,
568                            first_node_of_new_path.movement.target,
569                        );
570
571                        if successors(last_target_of_current_path)
572                            .iter()
573                            .any(|edge| edge.movement.target == first_target_of_new_path)
574                        {
575                            debug!("combining old and new paths");
576                            debug!(
577                                "old path: {:?}",
578                                executing_path.path.iter().collect::<Vec<_>>()
579                            );
580                            debug!("new path: {:?}", path.iter().take(10).collect::<Vec<_>>());
581                            new_path.extend(executing_path.path.iter().cloned());
582                        }
583                    } else {
584                        new_path.extend(executing_path.path.iter().cloned());
585                    }
586                }
587
588                new_path.extend(path.to_owned());
589
590                debug!(
591                    "set queued path to {:?}",
592                    new_path.iter().take(10).collect::<Vec<_>>()
593                );
594                executing_path.queued_path = Some(new_path);
595                executing_path.is_path_partial = event.is_partial;
596            } else if path.is_empty() {
597                debug!("calculated path is empty, so didn't add ExecutingPath");
598                if !pathfinder.opts.as_ref().is_some_and(|o| o.retry_on_no_path) {
599                    debug!("retry_on_no_path is set to false, removing goal");
600                    pathfinder.goal = None;
601                }
602            } else {
603                commands.entity(event.entity).insert(ExecutingPath {
604                    path: path.to_owned(),
605                    queued_path: None,
606                    last_reached_node: event.start,
607                    last_node_reached_at: Instant::now(),
608                    is_path_partial: event.is_partial,
609                });
610                debug!("set path to {:?}", path.iter().take(10).collect::<Vec<_>>());
611                debug!("partial: {}", event.is_partial);
612            }
613        } else {
614            error!("No path found");
615            if let Some(mut executing_path) = executing_path {
616                // set the queued path so we don't stop in the middle of a move
617                executing_path.queued_path = Some(VecDeque::new());
618            } else {
619                // wasn't executing a path, don't need to do anything
620            }
621        }
622        pathfinder.is_calculating = false;
623    }
624}
625
626#[allow(clippy::type_complexity)]
627pub fn timeout_movement(
628    mut query: Query<(
629        Entity,
630        &mut Pathfinder,
631        &mut ExecutingPath,
632        &Position,
633        Option<&Mining>,
634        &InstanceName,
635        &Inventory,
636        Option<&CustomPathfinderState>,
637    )>,
638    instance_container: Res<InstanceContainer>,
639) {
640    for (
641        entity,
642        mut pathfinder,
643        mut executing_path,
644        position,
645        mining,
646        instance_name,
647        inventory,
648        custom_state,
649    ) in &mut query
650    {
651        // don't timeout if we're mining
652        if let Some(mining) = mining {
653            // also make sure we're close enough to the block that's being mined
654            if mining.pos.distance_squared_to(position.into()) < 6_i32.pow(2) {
655                // also reset the last_node_reached_at so we don't timeout after we finish
656                // mining
657                executing_path.last_node_reached_at = Instant::now();
658                continue;
659            }
660        }
661
662        if executing_path.last_node_reached_at.elapsed() > Duration::from_secs(2)
663            && !pathfinder.is_calculating
664            && !executing_path.path.is_empty()
665        {
666            warn!("pathfinder timeout, trying to patch path");
667            executing_path.queued_path = None;
668            executing_path.last_reached_node = BlockPos::from(position);
669
670            let world_lock = instance_container
671                .get(instance_name)
672                .expect("Entity tried to pathfind but the entity isn't in a valid world");
673            let Some(opts) = pathfinder.opts.clone() else {
674                warn!(
675                    "pathfinder was going to patch path because of timeout, but pathfinder.opts was None"
676                );
677                return;
678            };
679
680            let custom_state = custom_state.cloned().unwrap_or_default();
681
682            // try to fix the path without recalculating everything.
683            // (though, it'll still get fully recalculated by `recalculate_near_end_of_path`
684            // if the new path is too short)
685            patch_path(
686                0..=cmp::min(20, executing_path.path.len() - 1),
687                &mut executing_path,
688                &mut pathfinder,
689                inventory,
690                entity,
691                world_lock,
692                custom_state,
693                opts,
694            );
695            // reset last_node_reached_at so we don't immediately try to patch again
696            executing_path.last_node_reached_at = Instant::now();
697        }
698    }
699}
700
701pub fn check_node_reached(
702    mut query: Query<(
703        Entity,
704        &mut Pathfinder,
705        &mut ExecutingPath,
706        &Position,
707        &Physics,
708    )>,
709    mut walk_events: EventWriter<StartWalkEvent>,
710    mut commands: Commands,
711) {
712    for (entity, mut pathfinder, mut executing_path, position, physics) in &mut query {
713        'skip: loop {
714            // we check if the goal was reached *before* actually executing the movement so
715            // we don't unnecessarily execute a movement when it wasn't necessary
716
717            // see if we already reached any future nodes and can skip ahead
718            for (i, edge) in executing_path
719                .path
720                .clone()
721                .into_iter()
722                .enumerate()
723                .take(20)
724                .rev()
725            {
726                let movement = edge.movement;
727                let is_reached_ctx = IsReachedCtx {
728                    target: movement.target,
729                    start: executing_path.last_reached_node,
730                    position: **position,
731                    physics,
732                };
733                let extra_strict_if_last = if i == executing_path.path.len() - 1 {
734                    let x_difference_from_center = position.x - (movement.target.x as f64 + 0.5);
735                    let z_difference_from_center = position.z - (movement.target.z as f64 + 0.5);
736                    // this is to make sure we don't fall off immediately after finishing the path
737                    physics.on_ground()
738                    // 0.5 to handle non-full blocks
739                    && BlockPos::from(position.up(0.5)) == movement.target
740                    // adding the delta like this isn't a perfect solution but it helps to make
741                    // sure we don't keep going if our delta is high
742                    && (x_difference_from_center + physics.velocity.x).abs() < 0.2
743                    && (z_difference_from_center + physics.velocity.z).abs() < 0.2
744                } else {
745                    true
746                };
747                if (movement.data.is_reached)(is_reached_ctx) && extra_strict_if_last {
748                    executing_path.path = executing_path.path.split_off(i + 1);
749                    executing_path.last_reached_node = movement.target;
750                    executing_path.last_node_reached_at = Instant::now();
751                    trace!("reached node {}", movement.target);
752
753                    if let Some(new_path) = executing_path.queued_path.take() {
754                        debug!(
755                            "swapped path to {:?}",
756                            new_path.iter().take(10).collect::<Vec<_>>()
757                        );
758                        executing_path.path = new_path;
759
760                        if executing_path.path.is_empty() {
761                            info!("the path we just swapped to was empty, so reached end of path");
762                            walk_events.write(StartWalkEvent {
763                                entity,
764                                direction: WalkDirection::None,
765                            });
766                            commands.entity(entity).remove::<ExecutingPath>();
767                            break;
768                        }
769
770                        // run the function again since we just swapped
771                        continue 'skip;
772                    }
773
774                    if executing_path.path.is_empty() {
775                        debug!("pathfinder path is now empty");
776                        walk_events.write(StartWalkEvent {
777                            entity,
778                            direction: WalkDirection::None,
779                        });
780                        commands.entity(entity).remove::<ExecutingPath>();
781                        if let Some(goal) = pathfinder.goal.clone()
782                            && goal.success(movement.target)
783                        {
784                            info!("goal was reached!");
785                            pathfinder.goal = None;
786                            pathfinder.opts = None;
787                        }
788                    }
789
790                    break;
791                }
792            }
793            break;
794        }
795    }
796}
797
798#[allow(clippy::type_complexity)]
799pub fn check_for_path_obstruction(
800    mut query: Query<(
801        Entity,
802        &mut Pathfinder,
803        &mut ExecutingPath,
804        &InstanceName,
805        &Inventory,
806        Option<&CustomPathfinderState>,
807    )>,
808    instance_container: Res<InstanceContainer>,
809) {
810    for (entity, mut pathfinder, mut executing_path, instance_name, inventory, custom_state) in
811        &mut query
812    {
813        let Some(opts) = pathfinder.opts.clone() else {
814            continue;
815        };
816
817        let world_lock = instance_container
818            .get(instance_name)
819            .expect("Entity tried to pathfind but the entity isn't in a valid world");
820
821        // obstruction check (the path we're executing isn't possible anymore)
822        let origin = executing_path.last_reached_node;
823        let cached_world = CachedWorld::new(world_lock, origin);
824        let mining_cache = MiningCache::new(if opts.allow_mining {
825            Some(inventory.inventory_menu.clone())
826        } else {
827            None
828        });
829        let custom_state = custom_state.cloned().unwrap_or_default();
830        let custom_state_ref = custom_state.0.read();
831        let successors = |pos: RelBlockPos| {
832            call_successors_fn(
833                &cached_world,
834                &mining_cache,
835                &custom_state_ref,
836                opts.successors_fn,
837                pos,
838            )
839        };
840
841        let Some(obstructed_index) = check_path_obstructed(
842            origin,
843            RelBlockPos::from_origin(origin, executing_path.last_reached_node),
844            &executing_path.path,
845            successors,
846        ) else {
847            continue;
848        };
849
850        drop(custom_state_ref);
851
852        warn!(
853            "path obstructed at index {obstructed_index} (starting at {:?})",
854            executing_path.last_reached_node,
855        );
856        debug!("obstructed path: {:?}", executing_path.path);
857        // if it's near the end, don't bother recalculating a patch, just truncate and
858        // mark it as partial
859        if obstructed_index + 5 > executing_path.path.len() {
860            debug!(
861                "obstruction is near the end of the path, truncating and marking path as partial"
862            );
863            executing_path.path.truncate(obstructed_index);
864            executing_path.is_path_partial = true;
865            continue;
866        }
867
868        let Some(opts) = pathfinder.opts.clone() else {
869            error!("got PatchExecutingPathEvent but the bot has no pathfinder opts");
870            continue;
871        };
872
873        let world_lock = instance_container
874            .get(instance_name)
875            .expect("Entity tried to pathfind but the entity isn't in a valid world");
876
877        // patch up to 20 nodes
878        let patch_end_index = cmp::min(obstructed_index + 20, executing_path.path.len() - 1);
879
880        patch_path(
881            obstructed_index..=patch_end_index,
882            &mut executing_path,
883            &mut pathfinder,
884            inventory,
885            entity,
886            world_lock,
887            custom_state.clone(),
888            opts,
889        );
890    }
891}
892
893/// update the given [`ExecutingPath`] to recalculate the path of the nodes in
894/// the given index range.
895///
896/// You should avoid making the range too large, since the timeout for the A*
897/// calculation is very low. About 20 nodes is a good amount.
898#[allow(clippy::too_many_arguments)]
899fn patch_path(
900    patch_nodes: RangeInclusive<usize>,
901    executing_path: &mut ExecutingPath,
902    pathfinder: &mut Pathfinder,
903    inventory: &Inventory,
904    entity: Entity,
905    world_lock: Arc<RwLock<azalea_world::Instance>>,
906    custom_state: CustomPathfinderState,
907    opts: PathfinderOpts,
908) {
909    let patch_start = if *patch_nodes.start() == 0 {
910        executing_path.last_reached_node
911    } else {
912        executing_path.path[*patch_nodes.start() - 1]
913            .movement
914            .target
915    };
916
917    let patch_end = executing_path.path[*patch_nodes.end()].movement.target;
918
919    // this doesn't override the main goal, it's just the goal for this A*
920    // calculation
921    let goal = Arc::new(BlockPosGoal(patch_end));
922
923    let goto_id_atomic = pathfinder.goto_id.clone();
924    let allow_mining = opts.allow_mining;
925
926    let mining_cache = MiningCache::new(if allow_mining {
927        Some(inventory.inventory_menu.clone())
928    } else {
929        None
930    });
931
932    // the timeout is small enough that this doesn't need to be async
933    let path_found_event = calculate_path(CalculatePathCtx {
934        entity,
935        start: patch_start,
936        goal,
937        world_lock,
938        goto_id_atomic,
939        mining_cache,
940        custom_state,
941        opts: PathfinderOpts {
942            min_timeout: PathfinderTimeout::Nodes(10_000),
943            max_timeout: PathfinderTimeout::Nodes(10_000),
944            ..opts
945        },
946    });
947
948    // this is necessary in case we interrupted another ongoing path calculation
949    pathfinder.is_calculating = false;
950
951    debug!("obstruction patch: {path_found_event:?}");
952
953    let mut new_path = VecDeque::new();
954    if *patch_nodes.start() > 0 {
955        new_path.extend(
956            executing_path
957                .path
958                .iter()
959                .take(*patch_nodes.start())
960                .cloned(),
961        );
962    }
963
964    let mut is_patch_complete = false;
965    if let Some(path_found_event) = path_found_event {
966        if let Some(found_path_patch) = path_found_event.path
967            && !found_path_patch.is_empty()
968        {
969            new_path.extend(found_path_patch);
970
971            if !path_found_event.is_partial {
972                new_path.extend(executing_path.path.iter().skip(*patch_nodes.end()).cloned());
973                is_patch_complete = true;
974                debug!("the patch is not partial :)");
975            } else {
976                debug!("the patch is partial, throwing away rest of path :(");
977            }
978        }
979    } else {
980        // no path found, rip
981    }
982
983    executing_path.path = new_path;
984    if !is_patch_complete {
985        executing_path.is_path_partial = true;
986    }
987}
988
989pub fn recalculate_near_end_of_path(
990    mut query: Query<(Entity, &mut Pathfinder, &mut ExecutingPath)>,
991    mut walk_events: EventWriter<StartWalkEvent>,
992    mut goto_events: EventWriter<GotoEvent>,
993    mut commands: Commands,
994) {
995    for (entity, mut pathfinder, mut executing_path) in &mut query {
996        let Some(mut opts) = pathfinder.opts.clone() else {
997            continue;
998        };
999
1000        // start recalculating if the path ends soon
1001        if (executing_path.path.len() == 50 || executing_path.path.len() < 5)
1002            && !pathfinder.is_calculating
1003            && executing_path.is_path_partial
1004        {
1005            match pathfinder.goal.as_ref().cloned() {
1006                Some(goal) => {
1007                    debug!("Recalculating path because it's empty or ends soon");
1008                    debug!(
1009                        "recalculate_near_end_of_path executing_path.is_path_partial: {}",
1010                        executing_path.is_path_partial
1011                    );
1012
1013                    opts.min_timeout = if executing_path.path.len() == 50 {
1014                        // we have quite some time until the node is reached, soooo we might as
1015                        // well burn some cpu cycles to get a good path
1016                        PathfinderTimeout::Time(Duration::from_secs(5))
1017                    } else {
1018                        PathfinderTimeout::Time(Duration::from_secs(1))
1019                    };
1020
1021                    goto_events.write(GotoEvent { entity, goal, opts });
1022                    pathfinder.is_calculating = true;
1023
1024                    if executing_path.path.is_empty() {
1025                        if let Some(new_path) = executing_path.queued_path.take() {
1026                            executing_path.path = new_path;
1027                            if executing_path.path.is_empty() {
1028                                info!(
1029                                    "the path we just swapped to was empty, so reached end of path"
1030                                );
1031                                walk_events.write(StartWalkEvent {
1032                                    entity,
1033                                    direction: WalkDirection::None,
1034                                });
1035                                commands.entity(entity).remove::<ExecutingPath>();
1036                                break;
1037                            }
1038                        } else {
1039                            walk_events.write(StartWalkEvent {
1040                                entity,
1041                                direction: WalkDirection::None,
1042                            });
1043                            commands.entity(entity).remove::<ExecutingPath>();
1044                        }
1045                    }
1046                }
1047                _ => {
1048                    if executing_path.path.is_empty() {
1049                        // idk when this can happen but stop moving just in case
1050                        walk_events.write(StartWalkEvent {
1051                            entity,
1052                            direction: WalkDirection::None,
1053                        });
1054                    }
1055                }
1056            }
1057        }
1058    }
1059}
1060
1061#[allow(clippy::type_complexity)]
1062pub fn tick_execute_path(
1063    mut query: Query<(
1064        Entity,
1065        &mut ExecutingPath,
1066        &Position,
1067        &Physics,
1068        Option<&Mining>,
1069        &InstanceHolder,
1070        &Inventory,
1071    )>,
1072    mut look_at_events: EventWriter<LookAtEvent>,
1073    mut sprint_events: EventWriter<StartSprintEvent>,
1074    mut walk_events: EventWriter<StartWalkEvent>,
1075    mut jump_events: EventWriter<JumpEvent>,
1076    mut start_mining_events: EventWriter<StartMiningBlockEvent>,
1077    mut set_selected_hotbar_slot_events: EventWriter<SetSelectedHotbarSlotEvent>,
1078) {
1079    for (entity, executing_path, position, physics, mining, instance_holder, inventory_component) in
1080        &mut query
1081    {
1082        if let Some(edge) = executing_path.path.front() {
1083            let ctx = ExecuteCtx {
1084                entity,
1085                target: edge.movement.target,
1086                position: **position,
1087                start: executing_path.last_reached_node,
1088                physics,
1089                is_currently_mining: mining.is_some(),
1090                instance: instance_holder.instance.clone(),
1091                menu: inventory_component.inventory_menu.clone(),
1092
1093                look_at_events: &mut look_at_events,
1094                sprint_events: &mut sprint_events,
1095                walk_events: &mut walk_events,
1096                jump_events: &mut jump_events,
1097                start_mining_events: &mut start_mining_events,
1098                set_selected_hotbar_slot_events: &mut set_selected_hotbar_slot_events,
1099            };
1100            trace!(
1101                "executing move, position: {}, last_reached_node: {}",
1102                **position, executing_path.last_reached_node
1103            );
1104            (edge.movement.data.execute)(ctx);
1105        }
1106    }
1107}
1108
1109pub fn recalculate_if_has_goal_but_no_path(
1110    mut query: Query<(Entity, &mut Pathfinder), Without<ExecutingPath>>,
1111    mut goto_events: EventWriter<GotoEvent>,
1112) {
1113    for (entity, mut pathfinder) in &mut query {
1114        if pathfinder.goal.is_some()
1115            && !pathfinder.is_calculating
1116            && let Some(goal) = pathfinder.goal.as_ref().cloned()
1117            && let Some(opts) = pathfinder.opts.clone()
1118        {
1119            debug!("Recalculating path because it has a goal but no ExecutingPath");
1120            goto_events.write(GotoEvent { entity, goal, opts });
1121            pathfinder.is_calculating = true;
1122        }
1123    }
1124}
1125
1126#[derive(Event)]
1127pub struct StopPathfindingEvent {
1128    pub entity: Entity,
1129    /// If false, then let the current movement finish before stopping. If true,
1130    /// then stop moving immediately. This might cause the bot to fall if it was
1131    /// in the middle of parkouring.
1132    pub force: bool,
1133}
1134
1135pub fn handle_stop_pathfinding_event(
1136    mut events: EventReader<StopPathfindingEvent>,
1137    mut query: Query<(&mut Pathfinder, &mut ExecutingPath)>,
1138    mut walk_events: EventWriter<StartWalkEvent>,
1139    mut commands: Commands,
1140) {
1141    for event in events.read() {
1142        // stop computing any path that's being computed
1143        commands.entity(event.entity).remove::<ComputePath>();
1144
1145        let Ok((mut pathfinder, mut executing_path)) = query.get_mut(event.entity) else {
1146            continue;
1147        };
1148        pathfinder.goal = None;
1149        if event.force {
1150            executing_path.path.clear();
1151            executing_path.queued_path = None;
1152        } else {
1153            // switch to an empty path as soon as it can
1154            executing_path.queued_path = Some(VecDeque::new());
1155            // make sure it doesn't recalculate
1156            executing_path.is_path_partial = false;
1157        }
1158
1159        if executing_path.path.is_empty() {
1160            walk_events.write(StartWalkEvent {
1161                entity: event.entity,
1162                direction: WalkDirection::None,
1163            });
1164            commands.entity(event.entity).remove::<ExecutingPath>();
1165        }
1166    }
1167}
1168
1169pub fn stop_pathfinding_on_instance_change(
1170    mut query: Query<(Entity, &mut ExecutingPath), Changed<InstanceName>>,
1171    mut stop_pathfinding_events: EventWriter<StopPathfindingEvent>,
1172) {
1173    for (entity, mut executing_path) in &mut query {
1174        if !executing_path.path.is_empty() {
1175            debug!("instance changed, clearing path");
1176            executing_path.path.clear();
1177            stop_pathfinding_events.write(StopPathfindingEvent {
1178                entity,
1179                force: true,
1180            });
1181        }
1182    }
1183}
1184
1185/// Checks whether the path has been obstructed, and returns Some(index) if it
1186/// has been. The index is of the first obstructed node.
1187pub fn check_path_obstructed<SuccessorsFn>(
1188    origin: BlockPos,
1189    mut current_position: RelBlockPos,
1190    path: &VecDeque<astar::Edge<BlockPos, moves::MoveData>>,
1191    successors_fn: SuccessorsFn,
1192) -> Option<usize>
1193where
1194    SuccessorsFn: Fn(RelBlockPos) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>>,
1195{
1196    for (i, edge) in path.iter().enumerate() {
1197        let movement_target = RelBlockPos::from_origin(origin, edge.movement.target);
1198
1199        let mut found_edge = None;
1200        for candidate_edge in successors_fn(current_position) {
1201            if candidate_edge.movement.target == movement_target {
1202                found_edge = Some(candidate_edge);
1203                break;
1204            }
1205        }
1206
1207        current_position = movement_target;
1208        // if found_edge is None or the cost increased, then return the index
1209        if found_edge
1210            .map(|found_edge| found_edge.cost > edge.cost)
1211            .unwrap_or(true)
1212        {
1213            // if the node that we're currently executing was obstructed then it's often too
1214            // late to change the path, so it's usually better to just ignore this case :/
1215            if i == 0 {
1216                warn!("path obstructed at index 0, ignoring");
1217                continue;
1218            }
1219
1220            return Some(i);
1221        }
1222    }
1223
1224    None
1225}
1226
1227pub fn call_successors_fn(
1228    cached_world: &CachedWorld,
1229    mining_cache: &MiningCache,
1230    custom_state: &CustomPathfinderStateRef,
1231    successors_fn: SuccessorsFn,
1232    pos: RelBlockPos,
1233) -> Vec<astar::Edge<RelBlockPos, moves::MoveData>> {
1234    let mut edges = Vec::with_capacity(16);
1235    let mut ctx = PathfinderCtx {
1236        edges: &mut edges,
1237        world: cached_world,
1238        mining_cache,
1239        custom_state,
1240    };
1241    successors_fn(&mut ctx, pos);
1242    edges
1243}