Change AI and refactor NavMesh stuff