Updated path finding algorithm