azalea/pathfinder/
mod.rs

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