rustsim-pathfinding 0.0.1

//! Integration tests for the full pathfinding pipeline.

use rustsim_pathfinding::astar::{astar, astar_grid2d, astar_grid2d_opts, GridAStarOpts};
use rustsim_pathfinding::continuous_astar::{
    ContinuousAStar, ContinuousAStar3D, ContinuousAStarOpts,
};
use rustsim_pathfinding::metrics::{DirectDistance, Manhattan, MaxDistance, PenaltyMap};
use rustsim_pathfinding::route::RoutePlanner;
use rustsim_pathfinding::walkability::{
    nearby_walkable, random_walkable, random_walkable_continuous,
};

// ---- Existing tests (kept) ----

#[test]
fn astar_simple_grid_path() {
    let result = astar_grid2d((0, 0), (3, 3), 5, 5, &|_x, _y| true, true);
    let result = result.expect("should find a path");
    assert_eq!(*result.path.first().unwrap(), (0, 0));
    assert_eq!(*result.path.last().unwrap(), (3, 3));
    assert!(result.cost > 0.0);
}

#[test]
fn astar_blocked_path() {
    let result = astar_grid2d((0, 0), (4, 0), 5, 1, &|x, _y| x != 2, false);
    assert!(result.is_none(), "wall should block the path");
}

#[test]
fn astar_manhattan_path() {
    let result = astar_grid2d((0, 0), (2, 2), 5, 5, &|_x, _y| true, false);
    let result = result.expect("should find a path");
    assert_eq!(*result.path.first().unwrap(), (0, 0));
    assert_eq!(*result.path.last().unwrap(), (2, 2));
    assert!((result.cost - 4.0).abs() < 1e-10);
}

#[test]
fn astar_generic_graph() {
    let result = astar(
        "A",
        "D",
        |_a, _b| 1.0,
        |node: &&str| -> Vec<(&str, f64)> {
            match *node {
                "A" => vec![("B", 1.0), ("C", 4.0)],
                "B" => vec![("C", 1.0), ("D", 5.0)],
                "C" => vec![("D", 1.0)],
                _ => vec![],
            }
        },
    );
    let result = result.expect("should find a path");
    assert_eq!(result.path, vec!["A", "B", "C", "D"]);
    assert!((result.cost - 3.0).abs() < 1e-10);
}

// ---- GridAStarOpts tests ----

#[test]
fn grid_opts_periodic_wraps_around() {
    let mut opts = GridAStarOpts::new(10, 10);
    opts.periodic = true;
    opts.diagonal = false;

    // (0,0) to (9,0): on a periodic grid, should be 1 step (wrap), not 9 steps
    let result = astar_grid2d_opts((0, 0), (9, 0), &opts);
    let result = result.expect("periodic path should exist");
    assert!((result.cost - 1.0).abs() < 1e-10);
}

#[test]
fn grid_opts_admissibility() {
    let mut opts = GridAStarOpts::new(50, 50);
    opts.admissibility = 0.0;
    let optimal = astar_grid2d_opts((0, 0), (49, 49), &opts).unwrap();

    let mut opts_fast = GridAStarOpts::new(50, 50);
    opts_fast.admissibility = 2.0;
    let suboptimal = astar_grid2d_opts((0, 0), (49, 49), &opts_fast).unwrap();

    // Suboptimal path cost should be >= optimal, but not more than (1+admissibility) times
    assert!(suboptimal.cost >= optimal.cost - 1e-10);
}

#[test]
fn grid_opts_penalty_map() {
    // Create a 10x10 grid with a high-penalty column at x=5
    let mut penalties = vec![0i32; 100];
    for y in 0..10 {
        penalties[y * 10 + 5] = 1000;
    }
    let metric = PenaltyMap::new(penalties, 10, 10, DirectDistance::new()).unwrap();

    let mut opts = GridAStarOpts::new(10, 10);
    opts.cost_metric = Some(&metric);

    let result = astar_grid2d_opts((0, 0), (9, 0), &opts).unwrap();

    // The path should avoid x=5 due to high penalty
    for &(x, _y) in &result.path {
        if x == 5 {
            // If it goes through x=5, the cost should reflect the penalty
            assert!(
                result.cost > 500.0,
                "path through penalty column should be expensive"
            );
            return;
        }
    }
    // If path avoids x=5, it should still reach the goal
    assert_eq!(*result.path.last().unwrap(), (9, 0));
}

#[test]
fn grid_opts_manhattan_metric() {
    let metric = Manhattan;
    let mut opts = GridAStarOpts::new(10, 10);
    opts.diagonal = false;
    opts.cost_metric = Some(&metric);

    let result = astar_grid2d_opts((0, 0), (5, 5), &opts).unwrap();
    assert!((result.cost - 10.0).abs() < 1e-10); // Manhattan distance = 5+5 = 10
}

#[test]
fn grid_opts_max_distance_metric() {
    let metric = MaxDistance;
    let mut opts = GridAStarOpts::new(10, 10);
    opts.cost_metric = Some(&metric);

    let result = astar_grid2d_opts((0, 0), (5, 5), &opts);
    assert!(result.is_some());
}

// ---- Continuous-space A* tests ----

#[test]
fn continuous_2d_navigate_around_wall() {
    // 20x20 grid over 100x100 space. Wall at x=10 (grid column 2) with gap at y=50 (grid row 10)
    let mut walkmap = vec![true; 400]; // 20x20
    for y in 0..20 {
        if y != 10 {
            walkmap[y * 20 + 10] = false;
        }
    }

    let pf = ContinuousAStar::new(
        100.0,
        100.0,
        &walkmap,
        20,
        20,
        ContinuousAStarOpts::default(),
    )
    .unwrap();
    let result = pf.find_path((5.0, 5.0), (90.0, 90.0));
    assert!(result.is_some(), "should find path through gap");
    let path = result.unwrap();
    assert!(
        path.waypoints.len() >= 3,
        "path should have multiple waypoints"
    );
    assert_eq!(path.waypoints.last(), Some(&(90.0, 90.0)));
}

#[test]
fn continuous_2d_with_custom_metric() {
    let walkmap = vec![true; 100];
    let pf = ContinuousAStar::new(10.0, 10.0, &walkmap, 10, 10, ContinuousAStarOpts::default())
        .unwrap()
        .with_metric(Manhattan);
    let result = pf.find_path((0.5, 0.5), (9.5, 9.5));
    assert!(result.is_some());
}

#[test]
fn continuous_3d_navigate() {
    let walkmap = vec![true; 8000]; // 20x20x20
    let pf = ContinuousAStar3D::new(100.0, 100.0, 100.0, &walkmap, 20, 20, 20).unwrap();
    let result = pf.find_path((5.0, 5.0, 5.0), (95.0, 95.0, 95.0));
    assert!(result.is_some());
    let path = result.unwrap();
    assert_eq!(path.waypoints.last(), Some(&(95.0, 95.0, 95.0)));
}

// ---- Route planner integration tests ----

#[test]
fn route_planner_end_to_end_2d() {
    // Find a path, then walk along it
    let walkmap = vec![true; 100];
    let pf =
        ContinuousAStar::new(10.0, 10.0, &walkmap, 10, 10, ContinuousAStarOpts::default()).unwrap();
    let path = pf.find_path((0.5, 0.5), (9.5, 9.5)).unwrap();

    let mut planner: RoutePlanner<(f64, f64)> = RoutePlanner::new();
    planner.plan_from_path(1, path.waypoints);

    // Walk until finished
    let mut pos = (0.5, 0.5);
    let mut steps = 0;
    loop {
        let result = planner.move_along_route_2d(1, pos, 2.0, 1.0, false, 10.0, 10.0);
        pos = result.position;
        steps += 1;
        if result.finished || steps > 100 {
            break;
        }
    }

    // Should have reached the goal
    assert!((pos.0 - 9.5).abs() < 1e-10);
    assert!((pos.1 - 9.5).abs() < 1e-10);
    assert!(steps < 100, "should finish in reasonable steps");
}

#[test]
fn route_planner_grid_end_to_end() {
    // Use grid A* to plan, then step through grid waypoints
    let result = astar_grid2d((0, 0), (4, 4), 5, 5, &|_, _| true, true).unwrap();

    let mut planner = RoutePlanner::new();
    // Skip the start position in the path (agent is already there)
    planner.set_route(1, result.path[1..].to_vec());

    let mut pos = (0, 0);
    while let Some(wp) = planner.next_waypoint(1) {
        pos = wp;
    }
    assert_eq!(pos, (4, 4));
}

// ---- Walkability integration tests ----

#[test]
fn walkability_with_pathfinding() {
    use rand::rngs::StdRng;
    use rand::SeedableRng;

    let mut walkmap = vec![true; 100];
    // Block some cells
    walkmap[5 * 10 + 5] = false;
    walkmap[5 * 10 + 6] = false;

    // Verify walkable neighbors exclude blocked cells
    let neighbors = nearby_walkable((5, 5), 1, &walkmap, 10, 10, false);
    assert!(!neighbors.contains(&(5, 5))); // self excluded
    assert!(!neighbors.contains(&(6, 5))); // blocked

    // Random walkable should never return blocked cells
    let mut rng = StdRng::seed_from_u64(42);
    for _ in 0..100 {
        if let Some((x, y)) = random_walkable(&walkmap, 10, 10, &mut rng) {
            assert!(walkmap[y * 10 + x], "random_walkable returned blocked cell");
        }
    }

    // Random walkable continuous positions should be in bounds
    for _ in 0..100 {
        if let Some((x, y)) = random_walkable_continuous(&walkmap, 10, 10, 100.0, 100.0, &mut rng) {
            assert!((0.0..100.0).contains(&x));
            assert!((0.0..100.0).contains(&y));
        }
    }
}