Added a function to check if the goal state is reached