pub fn check_goal( state: &State, goal: (f64, f64), target_index: usize, final_index: usize, ) -> bool