astar-mumu 0.2.0-rc.4

// FILE: astar/src/astar.rs

use mumu::parser::types::{Value};
use anyhow::Result;
use indexmap::IndexMap;
use std::collections::{HashSet, VecDeque, HashMap};
use std::time::Instant;

/// This function is the bridging logic for `astar:astar`.
/// We expect exactly one keyed-array argument:
///   [ dimension: [width, height],
///     start: [sx, sy],
///     end: [ex, ey],
///     obj: [ [ox,oy], ... ] ]
///
/// Returns a keyed array with the same fields plus:
///   path, success, cost, steps, timeMs, pathLength
pub fn astar_bridge(_args: Vec<Value>) -> Result<Value, String> {
    // Must have exactly 1 argument.
    if _args.len() != 1 {
        return Err(format!(
            "astar:astar => expected exactly 1 keyed-array argument, got {}",
            _args.len()
        ));
    }
    let first_arg = &_args[0];

    // Must be Value::KeyedArray(IndexMap<...>)
    let keyed_map = match first_arg {
        Value::KeyedArray(map) => map,
        _ => {
            return Err("astar:astar => argument must be a keyed array".to_string());
        }
    };

    // dimension: [width, height] => (i32, i32)
    let (width, height) = match keyed_map.get("dimension") {
        Some(Value::IntArray(arr)) if arr.len() == 2 => (arr[0], arr[1]),
        Some(Value::FloatArray(arr)) if arr.len() == 2 => {
            let w = arr[0] as i32;
            let h = arr[1] as i32;
            (w, h)
        }
        _ => return Err("astar:astar => 'dimension' must be [width, height]".to_string()),
    };

    // start: [sx, sy] => (i32, i32)
    let (sx, sy) = match keyed_map.get("start") {
        Some(Value::IntArray(arr)) if arr.len() == 2 => (arr[0], arr[1]),
        Some(Value::FloatArray(arr)) if arr.len() == 2 => {
            let x = arr[0] as i32;
            let y = arr[1] as i32;
            (x, y)
        }
        _ => return Err("astar:astar => 'start' must be [sx, sy]".to_string()),
    };

    // end: [ex, ey] => (i32, i32)
    let (ex, ey) = match keyed_map.get("end") {
        Some(Value::IntArray(arr)) if arr.len() == 2 => (arr[0], arr[1]),
        Some(Value::FloatArray(arr)) if arr.len() == 2 => {
            let x = arr[0] as i32;
            let y = arr[1] as i32;
            (x, y)
        }
        _ => return Err("astar:astar => 'end' must be [ex, ey]".to_string()),
    };

    // obj => obstacles as a 2D array. We'll store them in a HashSet<(i32,i32)>.
    let mut obstacles = HashSet::<(i32, i32)>::new();
    if let Some(val) = keyed_map.get("obj") {
        match val {
            Value::Int2DArray(rows) => {
                for row in rows {
                    if row.len() == 2 {
                        obstacles.insert((row[0], row[1]));
                    }
                }
            }
            Value::Float2DArray(rows) => {
                for row in rows {
                    if row.len() == 2 {
                        let ox = row[0] as i32;
                        let oy = row[1] as i32;
                        obstacles.insert((ox, oy));
                    }
                }
            }
            _ => {
                // If "obj" is present but not recognized as a 2D numeric array,
                // we ignore or treat it as empty. Just ignoring for now.
            }
        }
    }

    // Run a BFS-like search to keep it simple:
    let start_time = Instant::now();
    let (path, expansions, cost) = run_bfs(width, height, sx, sy, ex, ey, &obstacles);
    let time_ms = (Instant::now() - start_time).as_millis() as i32;

    // Build new KeyedArray with the same fields plus path, success, cost, steps, timeMs, pathLength.
    let mut out_map = IndexMap::new();

    // dimension => put it back as IntArray
    out_map.insert(
        "dimension".to_string(),
        Value::IntArray(vec![width, height]),
    );
    // start
    out_map.insert("start".to_string(), Value::IntArray(vec![sx, sy]));
    // end
    out_map.insert("end".to_string(), Value::IntArray(vec![ex, ey]));

    // Rebuild obj => Int2DArray of obstacles
    let mut obs_vec = Vec::new();
    for &(ox, oy) in &obstacles {
        obs_vec.push(vec![ox, oy]);
    }
    // Sort them if you like, not strictly needed
    obs_vec.sort_by_key(|a| (a[0], a[1]));
    out_map.insert("obj".to_string(), Value::Int2DArray(obs_vec));

    // path => Int2DArray
    let mut path2d = Vec::new();
    for &(px, py) in &path {
        path2d.push(vec![px, py]);
    }
    out_map.insert("path".to_string(), Value::Int2DArray(path2d));

    // success => bool
    let success = !path.is_empty() && path.last().unwrap() == &(ex, ey);
    out_map.insert("success".to_string(), Value::Bool(success));

    // cost => float
    out_map.insert("cost".to_string(), Value::Float(cost as f64));

    // steps => expansions => must be Value::Int(i32)
    out_map.insert("steps".to_string(), Value::Int(expansions));

    // timeMs => also i32
    out_map.insert("timeMs".to_string(), Value::Int(time_ms));

    // pathLength => i32
    let path_len = path.len() as i32;
    out_map.insert("pathLength".to_string(), Value::Int(path_len));

    Ok(Value::KeyedArray(out_map))
}

/// A toy BFS, ignoring diagonal. cost = number of steps.
fn run_bfs(
    w: i32,
    h: i32,
    sx: i32,
    sy: i32,
    ex: i32,
    ey: i32,
    obstacles: &HashSet<(i32, i32)>,
) -> (Vec<(i32, i32)>, i32, f32) {
    let start = (sx, sy);
    let mut queue = VecDeque::new();
    let mut visited = HashMap::<(i32, i32), (i32, i32)>::new();
    let mut cost_map = HashMap::<(i32, i32), f32>::new();

    cost_map.insert(start, 0.0);
    queue.push_back(start);

    let directions = [(1,0), (-1,0), (0,1), (0,-1)];
    let mut expansions = 0;

    let mut found = false;

    while let Some(current) = queue.pop_front() {
        expansions += 1;
        if current == (ex, ey) {
            found = true;
            break;
        }
        let current_cost = *cost_map.get(&current).unwrap_or(&0.0);

        for (dx, dy) in &directions {
            let nx = current.0 + dx;
            let ny = current.1 + dy;
            if nx < 0 || ny < 0 || nx >= w || ny >= h {
                continue;
            }
            if obstacles.contains(&(nx, ny)) {
                continue;
            }
            if !cost_map.contains_key(&(nx, ny)) {
                cost_map.insert((nx, ny), current_cost + 1.0);
                visited.insert((nx, ny), current);
                queue.push_back((nx, ny));
            }
        }
    }

    if !found {
        return (vec![], expansions, 0.0);
    }

    // Reconstruct path:
    let mut path_rev = vec![];
    let end_pt = (ex, ey);
    let total_cost = *cost_map.get(&end_pt).unwrap_or(&0.0);

    let mut node = end_pt;
    path_rev.push(node);

    while node != start {
        if let Some(&p) = visited.get(&node) {
            node = p;
            path_rev.push(node);
        } else {
            break;
        }
    }
    path_rev.reverse();
    (path_rev, expansions, total_cost)
}