Add replanning code