Shrink final selection for testing path finding
Get path finding into workable state
Working version of path finding with crude vertex and end detection.