from lib.config import Distance, Angle, Unit, AngleVelocity, Mass, Torque, Current


def iterate_chain(name, segments, func):
    """ Executes func(name, segment_config) based on 'segments' and 'name',
        than recursively calls itself on
        segment_config['next_segment'] if it exists. """

    func(name, segments[name])
    if 'next_segment' in segments[name]:
        iterate_chain(segments[name]['next_segment'], segments, func)


def convert_ros(val):
    """ converts data into ros compatible units """
    if isinstance(val, Distance):
        return val.as_m()
    elif isinstance(val, Angle):
        return val.as_radians()
    elif isinstance(val, AngleVelocity):
        return val.as_radians_s()
    elif isinstance(val, Mass):
        return val.as_kg()
    elif isinstance(val, Torque):
        return val.as_n_m()
    elif isinstance(val, Current):
        return val.as_a()
    elif isinstance(val, Unit):
        raise Exception("Don't now how to convert unit for ros", val)
    elif isinstance(val, (list, tuple)):
        res = list()
        for item in val:
            res.append(convert_ros(item))
        return res
    elif isinstance(val, dict):
        res = dict()
        for key, item in val.items():
            res[key] = convert_ros(item )
        return res
    else:
        return val


def convert_scad(val):
    """ converts data into scad format, especially Units """
    if isinstance(val, Distance):
        return val.as_mm()
    elif isinstance(val, Angle):
        return val.as_degrees()
    elif isinstance(val, Unit):
        return float(val)
    elif isinstance(val, (list, tuple)):
        res = list()
        for item in val:
            res.append(convert_scad(item))
        return res
    else:
        return val
