Added Tobias work to it, now calculates route correct