import json

posMinMax = [
    (-95, 10), 
    (-2, 161),
    (-37, 80),
    (15, 106),
    (-90, 90),
    (-91, 1),
    (-20, 40),
    (-2, 60),
    (8, 90),
    (-2, 92),
    (-2, 180),
    (-2, 90),
    (-2, 180),
    (-2, 90),
    (-2, 180),
    (-2, 270),
]


def strToNumList(strList):
    return [float(i) for i in strList.split()]


def rescale(num, scale):
    minN, maxN = scale
    return (num - minN) / (maxN - minN)


def processPos(config):
    return [rescale(config[i], posMinMax[i]) for i in range(len(posMinMax))]


def processTouch(config):
    res = [
        config[i] != 0 or
        config[i+1] != 0 or
        config[i+2] != 0 or
        config[i+3] != 0 or
        config[i+4] != 0 or
        config[i+5] != 0 or
        config[i+6] != 0 or
        config[i+7] != 0 or
        config[i+8] != 0 or
        config[i+9] != 0 or
        config[i+10] != 0 or
        config[i+11] != 0
        for i in range(0, len(config), 12)]
    return [1 if i else 0 for i in res]


def processHands(config):
    return config[0:60] + config[96:144]


def processForearm(config):
    x = config[0:30*12]         # -2
    x = x[0:27*12] + x[28*12:]  # -1
    x = x[0:22*12] + x[24*12:]  # -2
    x = x[0:18*12] + x[21*12:]  # -3
    x = x[0:16*12] + x[17*12:]  # -1
    return x


def posproces(config):
    rightHandTouch = config["right-hand"] + config["right-forearm"]
    leftHandTouch = config["left-hand"] + config["left-forearm"]

    return [
        {
            "proprio": config["left-pos"] + config["right-pos"],
            "touch": leftHandTouch + rightHandTouch,
            "rightHandPro": config["right-pos"],
            "leftHandPro": config["left-pos"],
            "rightTouch": rightHandTouch,
            "leftTouch": leftHandTouch
        },
        # simetrical
        {
            "proprio": config["right-pos"] + config["left-pos"],
            "touch": rightHandTouch + leftHandTouch,
            "rightHandPro": config["left-pos"],
            "leftHandPro": config["right-pos"],
            "rightTouch": leftHandTouch,
            "leftTouch": rightHandTouch
        }
    ]


def process(config):
    res = {}
    for idx in ["left-pos", "right-pos", "left-hand", "right-hand", "left-forearm", "right-forearm"]:
        res[idx] = strToNumList(config[idx])

    for idx in ["left-pos", "right-pos"]:
        res[idx] = processPos(res[idx])

    for idx in ["left-hand", "right-hand"]:
        res[idx] = processHands(res[idx])

    for idx in ["left-forearm", "right-forearm"]:
        res[idx] = processForearm(res[idx])

    for idx in ["left-hand", "right-hand", "left-forearm", "right-forearm"]:
        res[idx] = processTouch(res[idx])

    return posproces(res)


# main
if __name__ == "__main__":
    """
    Process data from iCub
    file ../data.rawdata to file ../data.data
    """
    with open('../data.rawdata') as f:
        data = json.load(f)
        res = [conf for sublist in data for conf in (process(sublist))]

    with open('../data.data', "w") as out:
        out.write(str(json.dumps(res, indent=4)))
    print("data size", len(res))
