dorf/src/entities/shared_systems/pathfinding.rs

use bevy::prelude::*;
use rayon::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_MAX_NODES, PATHFINDER_PROVISIONAL_NODE_LIMIT, TILE_SIZE,
};
use crate::world::tiles::TileMap;
use crate::world::{chunks::ChunkMap, chunks::CHUNK_SIZE};
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)]
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<(Entity, IVec3, IVec3)>,
}

const MAX_PATHS_PER_FRAME: usize = 8;

pub struct PathfindingPlugin;

impl Plugin for PathfindingPlugin {
    fn build(&self, app: &mut App) {
        app.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())
            .add_systems(
                FixedUpdate,
                (prepare_paths, update_wandering_targets, movement).chain(),
            )
            .add_systems(
                PostUpdate,
                (
                    merge_benchmark_stats,
                    process_completed_paths,
                    process_path_queue,
                ),
            )
            .add_systems(Update, bench_report_system);
    }
}

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>,
) {
    for (entity, mut ambulatory, transform) in query.iter_mut() {
        if ambulatory.current_path.is_some() || ambulatory.target.is_none() {
            continue;
        }
        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);

        if distance <= PATHFINDER_SHORT_PATH_MAX_TILES {
            let path = calculate_path_benchmarked(&tilemap, start, goal);
            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((entity, start, goal));
                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);
                ambulatory.current_path = Some(path);
                ambulatory.path_index = 0;
            }
        }
    }
}

pub fn process_path_queue(
    mut commands: Commands,
    mut queue: ResMut<PathRequestQueue>,
    tilemap: Res<TileMap>,
    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((entity, _old_start, goal)) = queue.pending.pop_front() {
            processed += 1;

            if let Ok((_, mut ambulatory, transform)) = query.get_mut(entity) {
                let actual_start = transform.translation.as_ivec3();
                let full_path = calculate_path_benchmarked(&tilemap, actual_start, goal);

                if !full_path.is_empty() {
                    ambulatory.current_path = Some(full_path);
                    ambulatory.path_index = 0;
                }
                commands
                    .entity(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, _) in query.iter_mut() {
        if ambulatory.target.is_none()
            || (ambulatory.current_path.is_some()
                && ambulatory.path_index >= ambulatory.current_path.as_ref().unwrap().len())
        {
            let loaded_chunks: Vec<&IVec2> = chunk_map.loaded_chunks.keys().collect();
            if !loaded_chunks.is_empty() {
                let random_index = rng.random_range(0..loaded_chunks.len());
                if let Some(&chunk_pos) = loaded_chunks.get(random_index) {
                    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>) {
    query
        .par_iter_mut()
        .for_each(|(mut ambulatory, mut transform)| {
            let current_pos = transform.translation;
            if !is_standable_tile(&tilemap, current_pos.as_ivec3()) {
                transform.translation.z -= TILE_SIZE;
                ambulatory.current_path = None;
                ambulatory.target = None;
                return;
            }

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

            if ambulatory.walk_speed > 0. {
                if ambulatory.step_recovery <= ambulatory.walk_speed as u32 {
                    ambulatory.step_recovery += 1;
                    return;
                } else {
                    ambulatory.step_recovery = 0;
                }
            }

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

                    if direction.x > 0.0 {
                        transform.scale.x = PIXEL_RATIO;
                    } else if direction.x < 0.0 {
                        transform.scale.x = -PIXEL_RATIO;
                    }

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

fn is_standable_tile(tilemap: &TileMap, pos: IVec3) -> bool {
    let can_stand_in_tile = tilemap
        .floor_tiles
        .get(&pos)
        .map(|t| t.can_stand_in())
        .unwrap_or(false);
    let can_stand_in_fixture = tilemap
        .fixture_tiles
        .get(&pos)
        .map(|t| t.can_stand_in())
        .unwrap_or(false);
    let pos_below = pos - IVec3::new(0, 0, ITILE_SIZE);
    let can_stand_on_tile_below = tilemap
        .floor_tiles
        .get(&pos_below)
        .map(|t| t.can_stand_on())
        .unwrap_or(false);
    let can_stand_on_fixture_below = tilemap
        .fixture_tiles
        .get(&pos_below)
        .map(|t| t.can_stand_on())
        .unwrap_or(false);

    (can_stand_in_tile || can_stand_in_fixture)
        && (can_stand_on_tile_below || can_stand_on_fixture_below)
}

fn calculate_movement_cost(move_dir: IVec3) -> i32 {
    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,
        (1, 1, 0) => 14,
        (1, 0, 1) | (0, 1, 1) => 42,
        (1, 1, 1) => 56,
        _ => 0,
    }
}

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 (
                    reconstruct_path(&scratch.came_from, current),
                    nodes_expanded,
                );
            }

            if current == goal {
                return (
                    reconstruct_path(&scratch.came_from, current),
                    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 movement_cost = calculate_movement_cost(move_dir);
                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,
                    });
                }
            }
        }

        (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 (
                    reconstruct_path(&scratch.came_from, best_node),
                    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 movement_cost = calculate_movement_cost(move_dir);
                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
}

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(())
}