Initial version of path finding stuff