//! Hierarchical Task-Based Pathfinding System
//!
//! This module implements a three-tier pathfinding architecture optimized for
//! Dwarf Fortress-like gameplay with large procedural worlds.
//!
//! # Architecture
//!
//! ```text
//! Entity needs path
//!       │
//!       ▼
//! ┌─────────────────┐
//! │ Calculate       │
//! │ Distance        │
//! │ + Chunk Distance│
//! └─────────────────┘
//!       │
//! ┌─────┼─────────────────────┐
//! ▼     ▼                     ▼
//! TIER1 TIER2               TIER3
//! │     │                     │
//! │     │                     │
//! ▼     ▼                     ▼
//! Sync  Provisional + Queue  Chunk-Path + Queue
//! A*    (immediate start)    (segmented execution)
//! ```
//!
//! # Tiers
//!
//! ## Tier 1: Synchronous A* (≤64 tiles or adjacent chunk)
//! - Executes immediately on main thread
//! - Uses thread-local scratchpad for zero allocation
//! - ~50-200µs for short paths
//!
//! ## Tier 2: Provisional + Queue (2-4 chunks)
//! - Provisional path (capped at 64 nodes) for immediate movement
//! - Full path computed via queue, spread across frames
//! - Queue processes 8 paths per frame max
//!
//! ## Tier 3: Hierarchical Chunk-Path (>4 chunks)
//! - Macro A* on chunk coordinates (<225 nodes, <10µs)
//! - Provisional path for immediate movement
//! - Segmented execution via queue with chunk waypoints
//!
//! # Key Data Structures
//!
//! - `AStarScratchpad`: Thread-local reused HashMaps/Heaps (zero allocation)
//! - `ChunkMap::chunk_connectivity`: Graph of adjacent loaded chunks
//! - `PathRequestQueue`: Time-sliced path computation queue
//!
//! # Performance
//!
//! - P95: ~357µs (target: <500µs)
//! - Success rate: 100%
//! - 96% of paths complete in <500µs

use bevy::prelude::*;
use rustc_hash::FxHashMap;
use rustc_hash::FxHashSet;
use std::{cell::RefCell, collections::BinaryHeap, collections::VecDeque, time::Instant};

use crate::constants::{
    ITILE_SIZE, PATHFINDER_HIERARCHICAL_THRESHOLD_CHUNKS, PATHFINDER_MAX_NODES,
    PATHFINDER_PROVISIONAL_NODE_LIMIT, PIXEL_RATIO, TILE_SIZE,
};

use crate::entities::item::inventory::{update_encumbrance, InventoryChangedEvent};
use crate::entities::shared_systems::constants::{
    CONVOY_DOT_THRESHOLD, ES_DIRECTION_THRESHOLD, HEAD_ON_DOT_THRESHOLD, OCCUPANCY_CROWD_THRESHOLD,
    PATHFINDER_DIRTY_LOOKAHEAD, PATHFINDER_VALIDATE_STEPS, PATHFINDER_VALIDATION_COOLDOWN,
    WALK_SPEED_DIVISOR,
};
use crate::entities::shared_systems::occupancy::{rebuild_tile_occupancy, TileOccupancy};

use crate::world::tiles::tile_changed::{PathfindingDirtyChunks, TileChangedEvent};
use crate::world::tiles::TileMap;
use crate::world::{
    chunks::CHUNK_SIZE,
    chunks::{world_to_chunk, ChunkMap, Z_ABOVE, Z_BELOW},
};
use crate::{constants::*, entities::shared_components::Ambulatory};
use bevy::math::ivec3;
use bevy_rand::prelude::*;
use rand::RngExt;

thread_local! {
    static LOCAL_PATH_TIMES: RefCell<Vec<u128>> = const { RefCell::new(Vec::new()) };
    static LOCAL_PATH_LENGTHS: RefCell<Vec<usize>> = const { RefCell::new(Vec::new()) };
    static LOCAL_NODES_EXPANDED: RefCell<Vec<usize>> = const { RefCell::new(Vec::new()) };
    static LOCAL_FAILED_PATHS: RefCell<u64> = const { RefCell::new(0) };
}

/// Single consolidated scratchpad for A* pathfinding.
/// One RefCell borrow instead of multiple nested borrows.
struct AStarScratchpad {
    g_scores: FxHashMap<IVec3, i32>,
    came_from: FxHashMap<IVec3, IVec3>,
    closed_set: FxHashSet<IVec3>,
    open_set: BinaryHeap<PathNode>,
}

impl Default for AStarScratchpad {
    fn default() -> Self {
        Self {
            g_scores: FxHashMap::default(),
            came_from: FxHashMap::default(),
            closed_set: FxHashSet::default(),
            open_set: BinaryHeap::new(),
        }
    }
}

impl AStarScratchpad {
    fn clear_and_reserve(&mut self, capacity: usize) {
        self.g_scores.clear();
        self.came_from.clear();
        self.closed_set.clear();
        self.open_set.clear();

        if self.g_scores.capacity() < capacity {
            self.g_scores.reserve(capacity);
            self.came_from.reserve(capacity);
            self.closed_set.reserve(capacity);
        }
    }
}

thread_local! {
    static SCRATCHPAD: RefCell<AStarScratchpad> = RefCell::new(AStarScratchpad::default());
}

const ALLOWED_MOVES: [IVec3; 24] = [
    IVec3::new(-ITILE_SIZE, 0, 0),
    IVec3::new(ITILE_SIZE, 0, 0),
    IVec3::new(0, -ITILE_SIZE, 0),
    IVec3::new(0, ITILE_SIZE, 0),
    IVec3::new(-ITILE_SIZE, -ITILE_SIZE, 0),
    IVec3::new(-ITILE_SIZE, ITILE_SIZE, 0),
    IVec3::new(ITILE_SIZE, -ITILE_SIZE, 0),
    IVec3::new(ITILE_SIZE, ITILE_SIZE, 0),
    IVec3::new(-ITILE_SIZE, 0, ITILE_SIZE),
    IVec3::new(-ITILE_SIZE, 0, -ITILE_SIZE),
    IVec3::new(ITILE_SIZE, 0, ITILE_SIZE),
    IVec3::new(ITILE_SIZE, 0, -ITILE_SIZE),
    IVec3::new(0, -ITILE_SIZE, ITILE_SIZE),
    IVec3::new(0, -ITILE_SIZE, -ITILE_SIZE),
    IVec3::new(0, ITILE_SIZE, ITILE_SIZE),
    IVec3::new(0, ITILE_SIZE, -ITILE_SIZE),
    IVec3::new(-ITILE_SIZE, -ITILE_SIZE, ITILE_SIZE),
    IVec3::new(-ITILE_SIZE, -ITILE_SIZE, -ITILE_SIZE),
    IVec3::new(-ITILE_SIZE, ITILE_SIZE, ITILE_SIZE),
    IVec3::new(-ITILE_SIZE, ITILE_SIZE, -ITILE_SIZE),
    IVec3::new(ITILE_SIZE, -ITILE_SIZE, ITILE_SIZE),
    IVec3::new(ITILE_SIZE, -ITILE_SIZE, -ITILE_SIZE),
    IVec3::new(ITILE_SIZE, ITILE_SIZE, ITILE_SIZE),
    IVec3::new(ITILE_SIZE, ITILE_SIZE, -ITILE_SIZE),
];

#[derive(Clone, Eq, PartialEq, Debug)]
struct PathNode {
    position: IVec3,
    f_score: i32,
    g_score: i32,
}

impl Ord for PathNode {
    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
        other
            .f_score
            .cmp(&self.f_score)
            .then_with(|| other.g_score.cmp(&self.g_score))
    }
}

impl PartialOrd for PathNode {
    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
        Some(self.cmp(other))
    }
}

#[derive(Resource, Default)]
#[allow(dead_code)]
pub struct PathfindingBenchmark {
    pub path_calc_times_us: Vec<u128>,
    pub path_lengths: Vec<usize>,
    pub nodes_expanded: Vec<usize>,
    pub movement_system_times_us: Vec<u128>,
    pub wander_system_times_us: Vec<u128>,
    pub total_paths_calculated: u64,
    pub total_failed_paths: u64,
    pub report_every_n: u32,
    pub sample_count: u32,
}

impl PathfindingBenchmark {
    pub fn new(report_every_n: u32) -> Self {
        Self {
            report_every_n,
            ..Default::default()
        }
    }
}

#[derive(Resource, Default)]
pub struct PathRequestQueue {
    pub pending: VecDeque<PathRequest>,
}

#[derive(Clone)]
#[allow(dead_code)]
pub struct PathRequest {
    pub entity: Entity,
    pub start: IVec3,
    pub goal: IVec3,
    pub chunk_path: Option<Vec<IVec2>>,
}

const MAX_PATHS_PER_FRAME: usize = 8;

pub struct PathfindingPlugin;

impl Plugin for PathfindingPlugin {
    fn build(&self, app: &mut App) {
        app.add_message::<InventoryChangedEvent>()
            .insert_resource(PathfindingBenchmark::new(100))
            .insert_resource(crate::entities::shared_components::CompletedPaths::default())
            .insert_resource(crate::entities::shared_components::PathRequestCounter::default())
            .insert_resource(PathRequestQueue::default())
            .init_resource::<TileOccupancy>()
            .insert_resource(PathfindingDirtyChunks::default())
            .add_systems(
                FixedUpdate,
                (
                    rebuild_tile_occupancy,
                    update_encumbrance,
                    collect_pathfinding_dirty_chunks,
                    invalidate_paths_on_tile_change,
                    crate::entities::tasks::task_executor_system,
                    prepare_paths,
                    movement,
                )
                    .chain(),
            )
            .add_systems(
                PostUpdate,
                (
                    merge_benchmark_stats,
                    process_completed_paths,
                    process_path_queue,
                    clear_pathfinding_dirty_chunks,
                ),
            )
            .add_systems(Update, bench_report_system);
    }
}

/// Collects chunk positions from TileChangedEvents into PathfindingDirtyChunks.
/// Runs once per frame, O(events). HashSet deduplicates — many tile changes
/// in the same chunk produce one entry.
pub fn collect_pathfinding_dirty_chunks(
    mut events: MessageReader<TileChangedEvent>,
    mut dirty: ResMut<PathfindingDirtyChunks>,
) {
    for event in events.read() {
        let chunk_pos = world_to_chunk(event.pos);
        dirty.chunks.insert(chunk_pos);
    }
}

/// Clears paths for entities whose upcoming steps pass through a changed chunk.
/// Checks only the next 8 steps (not the full path) for performance.
/// O(entities × 8) regardless of how many tiles changed.
/// Only runs when dirty chunks exist, which is rare in normal gameplay.
pub fn invalidate_paths_on_tile_change(
    dirty: Res<PathfindingDirtyChunks>,
    mut query: Query<&mut Ambulatory>,
) {
    if dirty.chunks.is_empty() {
        return;
    }
    for mut ambulatory in query.iter_mut() {
        let Some(ref path) = ambulatory.current_path else {
            continue;
        };
        let check_end = (ambulatory.path_index + PATHFINDER_DIRTY_LOOKAHEAD).min(path.len());
        let affected = path[ambulatory.path_index..check_end]
            .iter()
            .any(|p| dirty.chunks.contains(&world_to_chunk(p.as_ivec3())));
        if affected {
            ambulatory.current_path = None;
            // Keep target — entity will recompute path to same destination
        }
    }
}

/// Clears PathfindingDirtyChunks at the end of the frame.
/// Must run AFTER invalidate_paths_on_tile_change.
pub fn clear_pathfinding_dirty_chunks(mut dirty: ResMut<PathfindingDirtyChunks>) {
    dirty.chunks.clear();
}

pub fn prepare_paths(
    mut commands: Commands,
    mut queue: ResMut<PathRequestQueue>,
    mut query: Query<
        (
            Entity,
            &mut crate::entities::shared_components::Ambulatory,
            &Transform,
        ),
        Without<crate::entities::shared_components::PendingPath>,
    >,
    tilemap: Res<TileMap>,
    chunk_map: Res<ChunkMap>,
) {
    let mut paths_needed = 0;
    let mut paths_computed = 0;

    for (entity, mut ambulatory, transform) in query.iter_mut() {
        if ambulatory.current_path.is_some() || ambulatory.target.is_none() {
            continue;
        }
        paths_needed += 1;

        let Some(target) = ambulatory.target else {
            continue;
        };
        let start = transform.translation.as_ivec3();
        let goal = target.as_ivec3() - ivec3(0, 0, 1);
        let distance = octile_distance_3d(start, goal);

        let start_chunk = world_to_chunk(start);
        let goal_chunk = world_to_chunk(goal);
        let chunk_distance = manhattan_distance_2d(start_chunk, goal_chunk);

        if distance <= PATHFINDER_SHORT_PATH_MAX_TILES || chunk_distance <= 1 {
            let path = calculate_path_benchmarked(&tilemap, start, goal);
            ambulatory.current_path = Some(path);
            ambulatory.path_index = 0;
            paths_computed += 1;
        } else if chunk_distance > PATHFINDER_HIERARCHICAL_THRESHOLD_CHUNKS {
            let chunk_path = calculate_chunk_path(&chunk_map, start_chunk, goal_chunk);
            let provisional_goal = goal;
            let provisional = calculate_provisional_path(
                &tilemap,
                start,
                provisional_goal,
                PATHFINDER_PROVISIONAL_NODE_LIMIT,
            );
            if !provisional.is_empty() {
                ambulatory.current_path = Some(provisional);
                ambulatory.path_index = 0;

                queue.pending.push_back(PathRequest {
                    entity,
                    start,
                    goal,
                    chunk_path: if chunk_path.is_empty() {
                        None
                    } else {
                        Some(chunk_path)
                    },
                });
                commands
                    .entity(entity)
                    .insert(crate::entities::shared_components::PendingPath {
                        start,
                        goal,
                        waypoint_path: Vec::new(),
                        request_id: 0,
                    });
            } else {
                let path = calculate_path_benchmarked(&tilemap, start, goal);
                if path.len() <= 1 {
                    // Path failed — clear target so entity picks a new reachable one
                    ambulatory.target = None;
                    ambulatory.current_path = None;
                } else {
                    ambulatory.current_path = Some(path);
                    ambulatory.path_index = 0;
                }
            }
        } else {
            let provisional = calculate_provisional_path(
                &tilemap,
                start,
                goal,
                PATHFINDER_PROVISIONAL_NODE_LIMIT,
            );
            if !provisional.is_empty() {
                ambulatory.current_path = Some(provisional);
                ambulatory.path_index = 0;

                queue.pending.push_back(PathRequest {
                    entity,
                    start,
                    goal,
                    chunk_path: None,
                });
                commands
                    .entity(entity)
                    .insert(crate::entities::shared_components::PendingPath {
                        start,
                        goal,
                        waypoint_path: Vec::new(),
                        request_id: 0,
                    });
            } else {
                let path = calculate_path_benchmarked(&tilemap, start, goal);
                if path.len() <= 1 {
                    // Path failed — clear target so entity picks a new reachable one
                    ambulatory.target = None;
                    ambulatory.current_path = None;
                } else {
                    ambulatory.current_path = Some(path);
                    ambulatory.path_index = 0;
                }
            }
        }
    }
}

pub fn process_path_queue(
    mut commands: Commands,
    mut queue: ResMut<PathRequestQueue>,
    tilemap: Res<TileMap>,
    chunk_map: Res<ChunkMap>,
    mut query: Query<
        (Entity, &mut Ambulatory, &Transform),
        With<crate::entities::shared_components::PendingPath>,
    >,
) {
    let mut processed = 0;
    while processed < MAX_PATHS_PER_FRAME {
        if let Some(request) = queue.pending.pop_front() {
            processed += 1;

            if let Ok((_, mut ambulatory, transform)) = query.get_mut(request.entity) {
                let actual_start = transform.translation.as_ivec3();

                if let Some(ref chunk_waypoints) = request.chunk_path {
                    let current_chunk = world_to_chunk(actual_start);
                    if let Some(next_chunk) = chunk_waypoints.iter().find(|&&c| c != current_chunk)
                    {
                        match directional_chunk_waypoint(
                            actual_start,
                            *next_chunk,
                            request.goal,
                            &tilemap,
                            &chunk_map,
                        ) {
                            None => {
                                // Chunk unloaded or no standable tiles — abandon this path,
                                // entity will retarget via prepare_paths next frame
                                ambulatory.current_path = None;
                                ambulatory.target = None;
                            }
                            Some(waypoint) => {
                                let segment_path =
                                    calculate_path_benchmarked(&tilemap, actual_start, waypoint);
                                if !segment_path.is_empty() {
                                    ambulatory.current_path = Some(segment_path);
                                    ambulatory.path_index = 0;
                                }
                            }
                        }
                    }
                } else {
                    let full_path =
                        calculate_path_benchmarked(&tilemap, actual_start, request.goal);
                    if !full_path.is_empty() {
                        ambulatory.current_path = Some(full_path);
                        ambulatory.path_index = 0;
                    }
                }
                commands
                    .entity(request.entity)
                    .remove::<crate::entities::shared_components::PendingPath>();
            }
        } else {
            break;
        }
    }
}

pub fn process_completed_paths(
    mut completed: ResMut<crate::entities::shared_components::CompletedPaths>,
    mut query: Query<(
        Entity,
        &mut crate::entities::shared_components::Ambulatory,
        &mut crate::entities::shared_components::PendingPath,
    )>,
    mut commands: Commands,
) {
    for (request_id, path) in completed.paths.drain(..) {
        for (entity, mut ambulatory, pending) in query.iter_mut() {
            if pending.request_id == request_id {
                ambulatory.current_path = Some(path.clone());
                commands
                    .entity(entity)
                    .remove::<crate::entities::shared_components::PendingPath>();
            }
        }
    }
}

pub fn merge_benchmark_stats(mut bench: ResMut<PathfindingBenchmark>) {
    LOCAL_PATH_TIMES.with(|t| {
        let mut times = t.borrow_mut();
        bench.path_calc_times_us.extend(times.iter());
        bench.total_paths_calculated += times.len() as u64;
        times.clear();
    });
    LOCAL_PATH_LENGTHS.with(|l| {
        let mut lengths = l.borrow_mut();
        bench.path_lengths.extend(lengths.iter());
        lengths.clear();
    });
    LOCAL_NODES_EXPANDED.with(|n| {
        let mut nodes = n.borrow_mut();
        bench.nodes_expanded.extend(nodes.iter());
        nodes.clear();
    });
    LOCAL_FAILED_PATHS.with(|f| {
        let mut failed = f.borrow_mut();
        bench.total_failed_paths += *failed;
        *failed = 0;
    });
}

pub fn update_wandering_targets(
    mut query: Query<(&mut Ambulatory, &Transform)>,
    tilemap: Res<TileMap>,
    chunk_map: Res<ChunkMap>,
    mut rng_q: Query<&mut WyRand, With<GlobalRng>>,
) {
    let Ok(mut rng) = rng_q.single_mut() else {
        return;
    };

    for (mut ambulatory, transform) in query.iter_mut() {
        if ambulatory.target.is_none() {
            // Only target interior chunks — exclude boundary chunks that
            // have unloaded neighbours, which strand entities at world edges.
            let current_chunk = world_to_chunk(transform.translation.as_ivec3());

            // Reservoir sampling - pick one interior chunk with zero allocation
            let mut chosen: Option<IVec2> = None;
            let mut count = 0usize;
            for &chunk in chunk_map.loaded_chunks.keys() {
                let dx = (chunk.x - current_chunk.x).abs();
                let dy = (chunk.y - current_chunk.y).abs();
                if dx <= 2 && dy <= 2 {
                    continue;
                }
                if !chunk_map
                    .loaded_chunks
                    .contains_key(&IVec2::new(chunk.x + 1, chunk.y))
                {
                    continue;
                }
                if !chunk_map
                    .loaded_chunks
                    .contains_key(&IVec2::new(chunk.x - 1, chunk.y))
                {
                    continue;
                }
                if !chunk_map
                    .loaded_chunks
                    .contains_key(&IVec2::new(chunk.x, chunk.y + 1))
                {
                    continue;
                }
                if !chunk_map
                    .loaded_chunks
                    .contains_key(&IVec2::new(chunk.x, chunk.y - 1))
                {
                    continue;
                }
                count += 1;
                if rng.random_range(0..count) == 0 {
                    chosen = Some(chunk);
                }
            }

            // Fallback to any chunk if no interior chunks found (small map / startup)
            let target_chunk = chosen.or_else(|| chunk_map.loaded_chunks.keys().next().copied());

            if let Some(chunk_pos) = target_chunk {
                let chunk_x = chunk_pos.x * CHUNK_SIZE;
                let chunk_y = chunk_pos.y * CHUNK_SIZE;
                let target_x = chunk_x + rng.random_range(0..CHUNK_SIZE);
                let target_y = chunk_y + rng.random_range(0..CHUNK_SIZE);

                for z in -3..=4 {
                    let mut target_pos = IVec3::new(target_x, target_y, z) * ITILE_SIZE;
                    if tilemap.floor_tiles.get(&target_pos).is_some() {
                        target_pos.z += ITILE_SIZE;
                        if tilemap.floor_tiles.get(&target_pos).is_some()
                            && is_standable_tile(&tilemap, target_pos)
                        {
                            ambulatory.target = Some(Vec3::new(
                                target_pos.x as f32,
                                target_pos.y as f32,
                                target_pos.z as f32 + 1.0,
                            ));
                            ambulatory.current_path = None;
                            ambulatory.path_index = 0;
                            break;
                        }
                    }
                }
            }
        }
    }
}

pub fn movement(
    mut query: Query<(&mut Ambulatory, &mut Transform)>,
    tilemap: Res<TileMap>,
    occupancy: Res<TileOccupancy>,
) {
    query
        .par_iter_mut()
        .for_each(|(mut ambulatory, mut transform)| {
            let current_pos = transform.translation;
            if !is_standable_tile(&tilemap, current_pos.as_ivec3()) {
                // Entities can spawn above the loaded world range (z > Z_ABOVE*TILE_SIZE).
                // Snap them to the top instead of falling through unloaded z-space one
                // tile per tick.
                if transform.translation.z > Z_ABOVE as f32 * TILE_SIZE {
                    transform.translation.z = Z_ABOVE as f32 * TILE_SIZE;
                } else if transform.translation.z > -(Z_BELOW as f32) * TILE_SIZE {
                    transform.translation.z -= TILE_SIZE;
                }
                ambulatory.current_path = None;
                ambulatory.target = None;
                return;
            }

            if ambulatory.current_path.is_none() {
                return;
            }

            if ambulatory.validation_cooldown > 0 {
                ambulatory.validation_cooldown -= 1;
            } else if let Some(path) = &ambulatory.current_path {
                if !validate_next_steps(
                    &tilemap,
                    path,
                    ambulatory.path_index,
                    PATHFINDER_VALIDATE_STEPS,
                ) {
                    ambulatory.current_path = None;
                    ambulatory.validation_cooldown = PATHFINDER_VALIDATION_COOLDOWN;
                    return;
                }
                ambulatory.validation_cooldown = PATHFINDER_VALIDATION_COOLDOWN;
            }

            if ambulatory.walk_speed > 0. {
                let tile_weight = get_tile_weight(&tilemap, current_pos.as_ivec3());
                let threshold =
                    (ambulatory.walk_speed as i32 * tile_weight as i32 / WALK_SPEED_DIVISOR) as u32;

                if ambulatory.step_recovery <= threshold {
                    ambulatory.step_recovery += 1;
                    return;
                } else {
                    ambulatory.step_recovery = 0;
                }
            }

            if let Some(path) = &ambulatory.current_path {
                if ambulatory.path_index < path.len() {
                    let old_translation = transform.translation;
                    let next_point = path[ambulatory.path_index];

                    let current_count = occupancy.count_at(transform.translation);
                    let next_count = occupancy.count_at(next_point);
                    let current_crowded = current_count > OCCUPANCY_CROWD_THRESHOLD;
                    let occupied = !current_crowded && next_count > current_count;

                    let actual_move: Vec3;
                    let advance_path: bool;

                    let move_dir = next_point - transform.translation;
                    let our_dir = Vec2::new(move_dir.x, move_dir.y).normalize();
                    let their_dir = if occupied {
                        occupancy.direction_at(next_point)
                    } else {
                        Vec2::ZERO
                    };

                    // Case 1: Convoy — same direction, treat as unoccupied
                    let convoy = occupied
                        && their_dir != Vec2::ZERO
                        && their_dir.dot(our_dir) > CONVOY_DOT_THRESHOLD;

                    // Case 2: Head-on (directly opposite directions)
                    let head_on =
                        their_dir != Vec2::ZERO && their_dir.dot(our_dir) < HEAD_ON_DOT_THRESHOLD;

                    let we_are_es =
                        our_dir.x > ES_DIRECTION_THRESHOLD || our_dir.y < -ES_DIRECTION_THRESHOLD;

                    if !occupied || convoy {
                        // Normal movement
                        actual_move = next_point;
                        advance_path = true;
                    } else if occupied && head_on {
                        // Both yield left — existing sidestep chain
                        let forward_2d = our_dir;
                        let left_2d = Vec2::new(-forward_2d.y, forward_2d.x);
                        let right_2d = Vec2::new(forward_2d.y, -forward_2d.x);
                        let cur_z = transform.translation.z;

                        let candidates = [
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(
                                        (left_2d.x + forward_2d.x).signum() * TILE_SIZE,
                                        (left_2d.y + forward_2d.y).signum() * TILE_SIZE,
                                        0.0,
                                    ),
                                cur_z,
                                TILE_SIZE,
                            ),
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(left_2d.x * TILE_SIZE, left_2d.y * TILE_SIZE, 0.0),
                                cur_z,
                                TILE_SIZE,
                            ),
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(
                                        (right_2d.x + forward_2d.x).signum() * TILE_SIZE,
                                        (right_2d.y + forward_2d.y).signum() * TILE_SIZE,
                                        0.0,
                                    ),
                                cur_z,
                                TILE_SIZE,
                            ),
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(
                                        right_2d.x * TILE_SIZE,
                                        right_2d.y * TILE_SIZE,
                                        0.0,
                                    ),
                                cur_z,
                                TILE_SIZE,
                            ),
                        ];

                        let sidestep = candidates.iter().copied().find(|&c| {
                            tilemap.is_standable(c.as_ivec3())
                                && occupancy.count_at(c) <= current_count
                        });

                        if let Some(step) = sidestep {
                            actual_move = step;
                            advance_path = false;
                        } else {
                            actual_move = next_point;
                            advance_path = true;
                            let tile_weight =
                                get_tile_weight(&tilemap, transform.translation.as_ivec3());
                            let excuse_threshold = if ambulatory.walk_speed > 0. {
                                (ambulatory.walk_speed as i32 * tile_weight as i32
                                    / WALK_SPEED_DIVISOR) as u32
                            } else {
                                0
                            };
                            if ambulatory.step_recovery == 0 {
                                ambulatory.step_recovery = excuse_threshold;
                            }
                        }
                    } else if occupied && we_are_es {
                        // We yield — sidestep chain first, then excuse-me
                        let forward_2d = our_dir;
                        let left_2d = Vec2::new(-forward_2d.y, forward_2d.x);
                        let right_2d = Vec2::new(forward_2d.y, -forward_2d.x);
                        let cur_z = transform.translation.z;

                        let candidates = [
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(
                                        (left_2d.x + forward_2d.x).signum() * TILE_SIZE,
                                        (left_2d.y + forward_2d.y).signum() * TILE_SIZE,
                                        0.0,
                                    ),
                                cur_z,
                                TILE_SIZE,
                            ),
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(left_2d.x * TILE_SIZE, left_2d.y * TILE_SIZE, 0.0),
                                cur_z,
                                TILE_SIZE,
                            ),
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(
                                        (right_2d.x + forward_2d.x).signum() * TILE_SIZE,
                                        (right_2d.y + forward_2d.y).signum() * TILE_SIZE,
                                        0.0,
                                    ),
                                cur_z,
                                TILE_SIZE,
                            ),
                            snap_to_grid(
                                transform.translation
                                    + Vec3::new(
                                        right_2d.x * TILE_SIZE,
                                        right_2d.y * TILE_SIZE,
                                        0.0,
                                    ),
                                cur_z,
                                TILE_SIZE,
                            ),
                        ];

                        let sidestep = candidates.iter().copied().find(|&c| {
                            tilemap.is_standable(c.as_ivec3())
                                && occupancy.count_at(c) <= current_count
                        });

                        if let Some(step) = sidestep {
                            actual_move = step;
                            advance_path = false;
                        } else {
                            // excuse-me: push through with delay
                            actual_move = next_point;
                            advance_path = true;
                            let tile_weight =
                                get_tile_weight(&tilemap, transform.translation.as_ivec3());
                            let excuse_threshold = if ambulatory.walk_speed > 0. {
                                (ambulatory.walk_speed as i32 * tile_weight as i32
                                    / WALK_SPEED_DIVISOR) as u32
                            } else {
                                0
                            };
                            if ambulatory.step_recovery == 0 {
                                ambulatory.step_recovery = excuse_threshold;
                            }
                        }
                    } else {
                        // occupied && we_are_wn — right of way, advance with excuse-me delay
                        actual_move = next_point;
                        advance_path = true;
                        let tile_weight =
                            get_tile_weight(&tilemap, transform.translation.as_ivec3());
                        let excuse_threshold = if ambulatory.walk_speed > 0. {
                            (ambulatory.walk_speed as i32 * tile_weight as i32 / WALK_SPEED_DIVISOR)
                                as u32
                        } else {
                            0
                        };
                        if ambulatory.step_recovery == 0 {
                            ambulatory.step_recovery = excuse_threshold;
                        }
                    }

                    let direction = (actual_move - transform.translation).normalize_or_zero();
                    transform.translation = actual_move;
                    if direction.x > 0.0 {
                        transform.scale.x = PIXEL_RATIO;
                    } else if direction.x < 0.0 {
                        transform.scale.x = -PIXEL_RATIO;
                    }

                    if advance_path && transform.translation.distance(next_point) < TILE_SIZE {
                        let ITILE: i16 = ITILE_SIZE as i16;

                        ambulatory.step_history[0] = ambulatory.step_history[2];
                        ambulatory.step_history[1] = ambulatory.step_history[3];
                        ambulatory.step_history[2] = (old_translation.x as i16) / ITILE;
                        ambulatory.step_history[3] = (old_translation.y as i16) / ITILE;

                        let mut dir_sum = Vec2::ZERO;
                        let mut n = 0u32;

                        let h = &ambulatory.step_history;
                        if h[0] != 0 || h[1] != 0 {
                            let d = Vec2::new((h[2] - h[0]) as f32, (h[3] - h[1]) as f32);
                            if d.length_squared() > 0.0 {
                                dir_sum += d.normalize();
                                n += 1;
                            }
                        }

                        let cur_d = Vec2::new(
                            actual_move.x - old_translation.x,
                            actual_move.y - old_translation.y,
                        );
                        if cur_d.length_squared() > 0.0 {
                            dir_sum += cur_d.normalize();
                            n += 1;
                        }

                        if let Some(ref path) = ambulatory.current_path {
                            let start = ambulatory.path_index;
                            for i in start..((start + 2).min(path.len().saturating_sub(1))) {
                                let a = path[i];
                                let b = path[i + 1];
                                let d = Vec2::new(b.x - a.x, b.y - a.y);
                                if d.length_squared() > 0.0 {
                                    dir_sum += d.normalize();
                                    n += 1;
                                }
                            }
                        }

                        if n > 0 {
                            ambulatory.move_direction = (dir_sum / n as f32).normalize_or_zero();
                        }

                        ambulatory.path_index += 1;
                    }
                } else {
                    ambulatory.current_path = None;
                    if let Some(target) = ambulatory.target {
                        if transform.translation.distance(target) < TILE_SIZE * 2.0 {
                            ambulatory.target = None;
                        }
                    }
                }
            }
        });
}

fn validate_next_steps(tilemap: &TileMap, path: &[Vec3], start_index: usize, steps: usize) -> bool {
    let end = (start_index + steps).min(path.len());
    for i in start_index..end {
        let pos = path[i].as_ivec3();
        if !is_standable_tile(tilemap, pos) {
            return false;
        }
    }
    true
}

fn is_standable_tile(tilemap: &TileMap, pos: IVec3) -> bool {
    tilemap.is_standable(pos)
}

#[inline]
fn snap_to_grid(raw: Vec3, preserve_z: f32, tile_size: f32) -> Vec3 {
    Vec3::new(
        (raw.x / tile_size).round() * tile_size,
        (raw.y / tile_size).round() * tile_size,
        preserve_z,
    )
}

/// Sample a random standable tile on an edge of `next_chunk`, picking the one
/// whose world position is closest to the straight-line projection from
/// `current_pos` toward `goal`.  Falls back to chunk centre if no standable
/// edge tile is found.
///
/// This replaces the hard-coded chunk-centre waypoints that caused forced
/// doglegs at every chunk boundary, while preserving tile-locked DF movement.
fn directional_chunk_waypoint(
    current_pos: IVec3,
    next_chunk: IVec2,
    goal: IVec3,
    tilemap: &TileMap,
    chunk_map: &ChunkMap,
) -> Option<IVec3> {
    if !chunk_map.loaded_chunks.contains_key(&next_chunk) {
        return None;
    }

    let cx = next_chunk.x * CHUNK_SIZE;
    let cy = next_chunk.y * CHUNK_SIZE;
    let cur_tile = current_pos / ITILE_SIZE;
    let goal_tile = goal / ITILE_SIZE;
    let dir = goal_tile - cur_tile;
    let straight = goal - current_pos;

    let mut candidates = arrayvec::ArrayVec::<IVec3, 16>::new();

    let mut scan_edge = |fixed_axis: bool, fixed_tile: i32, var_min: i32, var_max: i32| {
        let var_range = var_max - var_min;
        let step = if var_range <= 0 {
            1
        } else {
            (var_range / 7).max(1)
        };
        let mut var = var_min;
        while var <= var_max {
            let (tile_x, tile_y) = if fixed_axis {
                (fixed_tile, var)
            } else {
                (var, fixed_tile)
            };
            let world_pos = IVec3::new(
                tile_x * ITILE_SIZE,
                tile_y * ITILE_SIZE,
                cur_tile.z * ITILE_SIZE,
            );
            if tilemap.is_standable(world_pos) {
                candidates.push(world_pos);
            }
            var += step;
        }
    };

    let near_diagonal = (dir.x.abs() - dir.y.abs()).abs() < CHUNK_SIZE / 2;

    if dir.x.abs() >= dir.y.abs() {
        scan_edge(
            true,
            if dir.x >= 0 { cx } else { cx + CHUNK_SIZE - 1 },
            cy,
            cy + CHUNK_SIZE - 1,
        );
        if near_diagonal {
            scan_edge(
                false,
                if dir.y >= 0 { cy } else { cy + CHUNK_SIZE - 1 },
                cx,
                cx + CHUNK_SIZE - 1,
            );
        }
    } else {
        scan_edge(
            false,
            if dir.y >= 0 { cy } else { cy + CHUNK_SIZE - 1 },
            cx,
            cx + CHUNK_SIZE - 1,
        );
        if near_diagonal {
            scan_edge(
                true,
                if dir.x >= 0 { cx } else { cx + CHUNK_SIZE - 1 },
                cy,
                cy + CHUNK_SIZE - 1,
            );
        }
    }

    if candidates.is_empty() {
        let centre = IVec3::new(
            (cx + CHUNK_SIZE / 2) * ITILE_SIZE,
            (cy + CHUNK_SIZE / 2) * ITILE_SIZE,
            cur_tile.z * ITILE_SIZE,
        );
        if tilemap.is_standable(centre) {
            return Some(centre);
        }
        for lx in 0..CHUNK_SIZE {
            for ly in 0..CHUNK_SIZE {
                let p = IVec3::new(
                    (cx + lx) * ITILE_SIZE,
                    (cy + ly) * ITILE_SIZE,
                    cur_tile.z * ITILE_SIZE,
                );
                if tilemap.is_standable(p) {
                    return Some(p);
                }
            }
        }
        None
    } else {
        // Find the candidate most aligned with the straight-line direction
        let best = candidates
            .iter()
            .enumerate()
            .max_by(|(_, &a), (_, &b)| {
                let da = a - current_pos;
                let db = b - current_pos;
                (da.x * straight.x + da.y * straight.y + da.z * straight.z)
                    .cmp(&(db.x * straight.x + db.y * straight.y + db.z * straight.z))
            })
            .map(|(idx, _)| idx)
            .unwrap_or(0);

        // Spread entities using both their start position AND goal position as entropy.
        // current_pos varies per entity (each is at a different world position).
        // goal varies per entity (each has a different random wander target).
        // Together they produce unique spread values for entities even when
        // heading through the same chunk, without needing to pass entity ID.
        // Small primes spread entity starting positions across waypoint edge tiles.
        // Different values per axis prevent aliasing when positions are on a regular grid.
        let entropy = (cur_tile.x.unsigned_abs() as usize)
            .wrapping_mul(1619)
            .wrapping_add((cur_tile.y.unsigned_abs() as usize).wrapping_mul(31337))
            .wrapping_add((goal_tile.x.unsigned_abs() as usize).wrapping_mul(6271))
            .wrapping_add((goal_tile.y.unsigned_abs() as usize).wrapping_mul(2053));
        let spread = entropy % candidates.len();
        let idx = (best + spread) % candidates.len();
        Some(candidates[idx])
    }
}

/// Get the A* weight for a tile position. Higher = slower to traverse.
/// Returns 100 (default) if tile not found.
#[inline]
fn get_tile_weight(tilemap: &TileMap, pos: IVec3) -> u8 {
    let floor_pos = IVec3::new(pos.x, pos.y, pos.z - ITILE_SIZE);
    tilemap.get_astar_weight(floor_pos)
}

/// Calculate movement cost including tile weight.
/// Base costs: cardinal=10, diagonal=14, vertical~50.
/// Tile weight adds: (weight - 50) / 5 to make higher-weight tiles more costly.
fn calculate_movement_cost(move_dir: IVec3, tile_weight: u8) -> i32 {
    let base_cost = match (
        move_dir.x.abs() / ITILE_SIZE,
        move_dir.y.abs() / ITILE_SIZE,
        move_dir.z.abs() / ITILE_SIZE,
    ) {
        (1, 0, 0) | (0, 1, 0) => 10, // Cardinal
        (1, 1, 0) => 14,             // Diagonal
        (1, 0, 1) | (0, 1, 1) => 42, // Vertical + cardinal
        (1, 1, 1) => 56,             // Vertical + diagonal
        _ => 0,
    };

    if base_cost == 0 {
        return 0;
    }

    // Weight cost: normalize so rock(50) adds 0, grass(100) adds 10, bedrock(150) adds 20
    let weight_cost = (tile_weight as i32).saturating_sub(50) / 5;

    base_cost + weight_cost
}

fn octile_distance_3d(a: IVec3, b: IVec3) -> i32 {
    let dx = (a.x - b.x).abs() / ITILE_SIZE;
    let dy = (a.y - b.y).abs() / ITILE_SIZE;
    let dz = (a.z - b.z).abs() / ITILE_SIZE;
    let (dmax, dmid, dmin) = sorted_desc(dx, dy, dz);
    10 * dmax + 4 * dmid + dmin
}

fn sorted_desc(a: i32, b: i32, c: i32) -> (i32, i32, i32) {
    let mut arr = [a, b, c];
    arr.sort_unstable_by(|x, y| y.cmp(x));
    (arr[0], arr[1], arr[2])
}

fn reconstruct_path(came_from: &FxHashMap<IVec3, IVec3>, mut current: IVec3) -> Vec<Vec3> {
    let mut path = vec![Vec3::new(
        current.x as f32,
        current.y as f32,
        current.z as f32,
    )];
    while let Some(&prev) = came_from.get(&current) {
        path.push(Vec3::new(prev.x as f32, prev.y as f32, prev.z as f32));
        current = prev;
    }
    path.reverse();
    path
}

pub fn calculate_path_benchmarked(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
    let timer = Instant::now();

    if !is_standable_tile(tilemap, start) || !is_standable_tile(tilemap, goal) {
        LOCAL_FAILED_PATHS.with(|f| {
            *f.borrow_mut() += 1;
        });
        return Vec::new();
    }

    let estimated_tiles = octile_distance_3d(start, goal) / ITILE_SIZE;
    let (result, nodes_expanded) =
        calculate_path_with_scratchpad(tilemap, start, goal, estimated_tiles);

    let elapsed = timer.elapsed().as_micros();
    LOCAL_PATH_TIMES.with(|t| {
        t.borrow_mut().push(elapsed);
    });
    LOCAL_PATH_LENGTHS.with(|l| {
        l.borrow_mut().push(result.len());
    });
    LOCAL_NODES_EXPANDED.with(|n| {
        n.borrow_mut().push(nodes_expanded);
    });

    if result.is_empty() {
        vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)]
    } else {
        result
    }
}

fn calculate_path_with_scratchpad(
    tilemap: &TileMap,
    start: IVec3,
    goal: IVec3,
    estimated_tiles: i32,
) -> (Vec<Vec3>, usize) {
    SCRATCHPAD.with(|s| {
        let mut scratch = s.borrow_mut();
        let capacity = ((estimated_tiles as usize).max(64)).min(4096);
        scratch.clear_and_reserve(capacity);

        let h = octile_distance_3d(start, goal);
        scratch.open_set.push(PathNode {
            position: start,
            f_score: h,
            g_score: 0,
        });
        scratch.g_scores.insert(start, 0);

        let mut nodes_expanded: usize = 0;

        while let Some(current_node) = scratch.open_set.pop() {
            let current = current_node.position;
            nodes_expanded += 1;

            if nodes_expanded > PATHFINDER_MAX_NODES {
                return (Vec::new(), nodes_expanded); // force retarget
            }

            if current == goal {
                return (
                    reconstruct_path(&scratch.came_from, current),
                    nodes_expanded,
                );
            }

            scratch.closed_set.insert(current);

            let current_g = *scratch.g_scores.get(&current).unwrap_or(&i32::MAX);

            for &move_dir in &ALLOWED_MOVES {
                let neighbor_pos = current + move_dir;

                if !is_standable_tile(tilemap, neighbor_pos)
                    || scratch.closed_set.contains(&neighbor_pos)
                {
                    continue;
                }

                let tile_weight = get_tile_weight(tilemap, neighbor_pos);
                let movement_cost = calculate_movement_cost(move_dir, tile_weight);
                if movement_cost == 0 {
                    continue;
                }

                let new_g = current_g + movement_cost;

                if new_g < *scratch.g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) {
                    scratch.came_from.insert(neighbor_pos, current);
                    scratch.g_scores.insert(neighbor_pos, new_g);
                    let f = new_g + octile_distance_3d(neighbor_pos, goal);
                    scratch.open_set.push(PathNode {
                        position: neighbor_pos,
                        f_score: f,
                        g_score: new_g,
                    });
                }
            }
        }

        (Vec::new(), nodes_expanded)
    })
}

pub fn calculate_provisional_path(
    tilemap: &TileMap,
    start: IVec3,
    goal: IVec3,
    node_limit: usize,
) -> Vec<Vec3> {
    let timer = Instant::now();

    if !is_standable_tile(tilemap, start) {
        LOCAL_FAILED_PATHS.with(|f| {
            *f.borrow_mut() += 1;
        });
        return vec![Vec3::new(start.x as f32, start.y as f32, start.z as f32)];
    }

    let estimated_tiles = octile_distance_3d(start, goal) / ITILE_SIZE;

    let result = SCRATCHPAD.with(|s| {
        let mut scratch = s.borrow_mut();
        let capacity = ((estimated_tiles as usize).max(64)).min(4096);
        scratch.clear_and_reserve(capacity);

        let initial_h = octile_distance_3d(start, goal);
        scratch.open_set.push(PathNode {
            position: start,
            f_score: initial_h,
            g_score: 0,
        });
        scratch.g_scores.insert(start, 0);

        let mut nodes_expanded: usize = 0;
        let mut best_node = start;
        let mut best_h = initial_h;

        while let Some(current_node) = scratch.open_set.pop() {
            let current = current_node.position;
            nodes_expanded += 1;

            let h = octile_distance_3d(current, goal);
            if h < best_h {
                best_h = h;
                best_node = current;
            }

            if current == goal {
                return (
                    reconstruct_path(&scratch.came_from, current),
                    nodes_expanded,
                );
            }

            if nodes_expanded >= node_limit {
                return (Vec::new(), nodes_expanded);
            }

            scratch.closed_set.insert(current);

            for &move_dir in &ALLOWED_MOVES {
                let neighbor_pos = current + move_dir;

                if !is_standable_tile(tilemap, neighbor_pos)
                    || scratch.closed_set.contains(&neighbor_pos)
                {
                    continue;
                }

                let tile_weight = get_tile_weight(tilemap, neighbor_pos);
                let movement_cost = calculate_movement_cost(move_dir, tile_weight);
                if movement_cost == 0 {
                    continue;
                }

                let new_g = *scratch.g_scores.get(&current).unwrap_or(&i32::MAX) + movement_cost;

                if new_g < *scratch.g_scores.get(&neighbor_pos).unwrap_or(&i32::MAX) {
                    scratch.came_from.insert(neighbor_pos, current);
                    scratch.g_scores.insert(neighbor_pos, new_g);
                    let f = new_g + octile_distance_3d(neighbor_pos, goal);
                    scratch.open_set.push(PathNode {
                        position: neighbor_pos,
                        f_score: f,
                        g_score: new_g,
                    });
                }
            }
        }

        (
            reconstruct_path(&scratch.came_from, best_node),
            nodes_expanded,
        )
    });

    let elapsed = timer.elapsed().as_micros();
    LOCAL_PATH_TIMES.with(|t| {
        t.borrow_mut().push(elapsed);
    });
    LOCAL_PATH_LENGTHS.with(|l| {
        l.borrow_mut().push(result.0.len());
    });
    LOCAL_NODES_EXPANDED.with(|n| {
        n.borrow_mut().push(result.1);
    });

    result.0
}

struct ChunkPathNode {
    position: IVec2,
    f_score: i32,
    g_score: i32,
}

impl Eq for ChunkPathNode {}

impl PartialEq for ChunkPathNode {
    fn eq(&self, other: &Self) -> bool {
        self.position == other.position
    }
}

impl Ord for ChunkPathNode {
    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
        self.f_score
            .cmp(&other.f_score)
            .then_with(|| self.g_score.cmp(&other.g_score))
    }
}

impl PartialOrd for ChunkPathNode {
    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
        Some(self.cmp(other))
    }
}

thread_local! {
    static CHUNK_SCRATCHPAD: RefCell<ChunkAStarScratchpad> = RefCell::new(ChunkAStarScratchpad::default());
}

struct ChunkAStarScratchpad {
    g_scores: FxHashMap<IVec2, i32>,
    came_from: FxHashMap<IVec2, IVec2>,
    closed_set: FxHashSet<IVec2>,
    open_set: BinaryHeap<ChunkPathNode>,
}

impl Default for ChunkAStarScratchpad {
    fn default() -> Self {
        Self {
            g_scores: FxHashMap::default(),
            came_from: FxHashMap::default(),
            closed_set: FxHashSet::default(),
            open_set: BinaryHeap::new(),
        }
    }
}

impl ChunkAStarScratchpad {
    fn clear(&mut self) {
        self.g_scores.clear();
        self.came_from.clear();
        self.closed_set.clear();
        self.open_set.clear();
    }
}

fn manhattan_distance_2d(a: IVec2, b: IVec2) -> i32 {
    (a.x - b.x).abs() + (a.y - b.y).abs()
}

pub fn calculate_chunk_path(
    chunk_map: &ChunkMap,
    start_chunk: IVec2,
    goal_chunk: IVec2,
) -> Vec<IVec2> {
    if start_chunk == goal_chunk {
        return vec![start_chunk];
    }

    if !chunk_map.loaded_chunks.contains_key(&goal_chunk) {
        return Vec::new();
    }

    if !chunk_map.loaded_chunks.contains_key(&start_chunk) {
        return Vec::new();
    }

    CHUNK_SCRATCHPAD.with(|s| {
        let mut scratch = s.borrow_mut();
        scratch.clear();

        let h = manhattan_distance_2d(start_chunk, goal_chunk);
        scratch.open_set.push(ChunkPathNode {
            position: start_chunk,
            f_score: h,
            g_score: 0,
        });
        scratch.g_scores.insert(start_chunk, 0);

        while let Some(current_node) = scratch.open_set.pop() {
            let current = current_node.position;

            if current == goal_chunk {
                let mut path = vec![current];
                let mut curr = current;
                while let Some(&prev) = scratch.came_from.get(&curr) {
                    path.push(prev);
                    curr = prev;
                }
                path.reverse();
                return path;
            }

            scratch.closed_set.insert(current);

            if let Some(neighbors) = chunk_map.chunk_connectivity.get(&current) {
                for &neighbor in neighbors {
                    if scratch.closed_set.contains(&neighbor) {
                        continue;
                    }

                    let new_g = *scratch.g_scores.get(&current).unwrap_or(&i32::MAX) + 1;

                    if new_g < *scratch.g_scores.get(&neighbor).unwrap_or(&i32::MAX) {
                        scratch.came_from.insert(neighbor, current);
                        scratch.g_scores.insert(neighbor, new_g);
                        let f = new_g + manhattan_distance_2d(neighbor, goal_chunk);
                        scratch.open_set.push(ChunkPathNode {
                            position: neighbor,
                            f_score: f,
                            g_score: new_g,
                        });
                    }
                }
            }
        }

        Vec::new()
    })
}

pub fn bench_report_system(
    keys: Res<ButtonInput<KeyCode>>,
    mut bench: ResMut<PathfindingBenchmark>,
) {
    if keys.just_pressed(KeyCode::F8) {
        println!("\n=== PATHFINDING BENCHMARK REPORT ===");
        report_stat("path_calc", &bench.path_calc_times_us);
        report_stat(
            "path_length",
            &bench
                .path_lengths
                .iter()
                .map(|&l| l as u128)
                .collect::<Vec<_>>(),
        );
        report_stat(
            "nodes_expanded",
            &bench
                .nodes_expanded
                .iter()
                .map(|&n| n as u128)
                .collect::<Vec<_>>(),
        );

        if !bench.movement_system_times_us.is_empty() {
            report_stat("movement_system", &bench.movement_system_times_us);
        }
        if !bench.wander_system_times_us.is_empty() {
            report_stat("wander_system", &bench.wander_system_times_us);
        }

        let total = bench.total_paths_calculated;
        let failed = bench.total_failed_paths;
        println!(
            "[BENCH] total_paths={} failed_paths={} success_rate={:.1}%",
            total,
            failed,
            if total > 0 {
                100.0 * (total - failed) as f64 / total as f64
            } else {
                100.0
            }
        );

        if let Err(e) = write_benchmark_csv(&bench, "pathfinding_benchmark_current.csv") {
            eprintln!("Failed to write benchmark CSV: {}", e);
        }
        println!("=====================================\n");
    }
}

fn report_stat(label: &str, times: &[u128]) {
    if times.is_empty() {
        return;
    }
    let sum: u128 = times.iter().sum();
    let avg = sum / times.len() as u128;
    let min = *times.iter().min().unwrap();
    let max = *times.iter().max().unwrap();
    let mut sorted = times.to_vec();
    sorted.sort_unstable();
    let median = sorted[sorted.len() / 2];
    let p95_idx = (sorted.len() as f64 * 0.95) as usize;
    let p95 = sorted[p95_idx.min(sorted.len().saturating_sub(1))];

    println!(
        "[BENCH][{}] n={} avg={}µs median={}µs min={}µs max={}µs p95={}µs",
        label,
        times.len(),
        avg,
        median,
        min,
        max,
        p95
    );
}

fn write_benchmark_csv(bench: &PathfindingBenchmark, filename: &str) -> std::io::Result<()> {
    use std::fs::File;
    use std::io::Write;

    let mut file = File::create(filename)?;
    writeln!(
        file,
        "sample,path_duration_us,path_length,nodes_expanded,success"
    )?;

    let n = bench.path_calc_times_us.len();
    for i in 0..n {
        let duration = bench.path_calc_times_us.get(i).copied().unwrap_or(0);
        let length = bench.path_lengths.get(i).copied().unwrap_or(0);
        let nodes = bench.nodes_expanded.get(i).copied().unwrap_or(0);
        let success = i < n.saturating_sub(bench.total_failed_paths as usize);
        writeln!(file, "{},{},{},{},{}", i, duration, length, nodes, success)?;
    }

    writeln!(file, "# Summary")?;
    if !bench.path_calc_times_us.is_empty() {
        let avg: u128 =
            bench.path_calc_times_us.iter().sum::<u128>() / bench.path_calc_times_us.len() as u128;
        writeln!(file, "# avg_duration_us,{}", avg)?;
    }
    writeln!(file, "# total_paths,{}", bench.total_paths_calculated)?;
    writeln!(file, "# failed_paths,{}", bench.total_failed_paths)?;

    Ok(())
}