Getting closer, only have to generalize calculate_route now