import csv
import os
import numpy as np
from matplotlib import pyplot as plt

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
fig, ax = plt.subplots(1,2, sharey=True)
for root, dir, files in os.walk(os.path.join(os.getcwd(), 'DataCSV\\name\\')):
	for file in files:
		if file.endswith(".csv"):
			path = os.path.join(root, file)
			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
			x = [header[i][5:] for i in range(18) ]
			y = [sum_distance_joints[i] / counter for i in range(18)]
			if "FirstPerson" in file:
				ax[0].scatter(x,y, label="1st")
				ax[0].set_label("1st")
			else:
				ax[1].scatter(x,y, label="3rd")
plt.show()

path = os.path.join(os.getcwd(), 'DataCSV\\name\\FirstPersonPerspective_OneArm_Forward_HapticFeedback_Slow.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)