Function astar 

pub fn astar(
    graph: &Graph,
    start: usize,
    goal: usize,
    heuristic: &dyn Fn(usize) -> f64,
) -> Option<(f64, Vec<Option<usize>>)>

A* shortest path from start to goal with a heuristic h(node) -> f64.

Returns the distance and predecessor array, or None if no path exists. Use path_reconstruct on the returned prev to get the path.