Implement first version of path finding