Added path planning V1