Added a basic path finding algorithm