Added more functionality to the routeing:

More point in the route but this funtion is commented...