Rough version of path finding complete!