Route finding somewhat done, refactored somewhat