Added comments and fixed final goal state policy