Fixed bug with start and goal states