Refactored lots of stuff to use Collections, and cleaned FindNearestRoad a bit