Moved some code over to a new Route class to make it easier to implement waypoints, cleaned up some other bits of code