Added path planning V2