Simple A* implementation in Rust