Enabled path segment planning. Still buggy. Need to fix.