Fixed planner bug and added regression tests