import csv
import os
import numpy as np

sum_distance_joints = np.zeros(18)
counter = 0

def vector_string_to_float(vector):
    """
    Convert vector string to float

    Parameters:
        vector: vector still in string
    Returns:
        vector: vector with type float
    """ 
    vector = vector.split(';')
    vector = list(map(float, vector))
    return vector

path = os.path.join(os.getcwd(), 'Assets\\demo_and_body_positions.csv')
with open(path, newline='') as csvfile:
    reader = csv.reader(csvfile)
    header = next(reader)
    for row in reader:
        for i in range(18):
            demo = vector_string_to_float(row[i])
            body = vector_string_to_float(row[i+18])
            distance = np.linalg.norm(np.subtract(demo, body))
            sum_distance_joints[i] += distance
        counter += 1

for i in range(sum_distance_joints.size):
    print("3d accuracy ", header[i][5:], ": ", sum_distance_joints[i] / counter)

