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, 3, sharey=True)

name = 'Pieter'
for root, dir, files in os.walk(os.path.join(os.getcwd(), f'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)
            elif "ThirdPersonPerspective_" in file:
                ax[1].scatter(x, y)
            elif "MultipleViews" in file:
                ax[2].scatter(x, y)
            # Reset values
            sum_distance_joints = np.zeros(18)
            counter = 0


fig.autofmt_xdate(rotation=45)
ax[0].set_title('First Person')
ax[1].set_title('Third Person')
ax[2].set_title('Third Person Multiple View')


# plt.legend(files).set_draggable(True)
plt.show()


# ----------------------------------------


sum_distance_joints = np.zeros(18)
counter = 0

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