import math


def ICC_dist(vel_l, vel_r, wheel_dist=1):
    """ 
    calculate distance to ICC
    - given left and right velocity and wheel distance (l)
    """

    # fail if wheel distance is zero
    assert wheel_dist != 0, 'Wheel distance cannot be 0!'

    # avoid division by zero (return infinity)
    if(vel_l == vel_r):
        return math.inf
    else:
        R = (wheel_dist / 2) * (vel_r + vel_l) / (vel_r - vel_l)
        return R