Added some improvements that should make the planner work at a better rate