import csv
import os
import numpy as np

sum_shoulder_left_error = 0
sum_elbow_left_error = 0
sum_shoulder_right_error = 0
sum_elbow_right_error = 0
sum_knee_left_error = 0
sum_ankle_left_error = 0
sum_knee_right_error = 0
sum_ankle_right_error = 0


def get_vector(row, joint_name):
    """
    Get vector from csv row and joint_name (convert string to float)

    Parameters:
        row: row from csv
        joint_name: name of joint
    Returns:
        vector: vector with type float
    """ 
    vector = row[joint_name].split(';')
    vector = list(map(float, vector))
    return vector


def get_angle(a, b, c):
    """
    Get angle b (imagine there are line ba and bc)

    Parameters:
        a: Vector a
        b: Vector b
        c: Vector c
    Returns:
        angle: Angle b in degrees
    """ 
    p = np.subtract(a, b)
    q = np.subtract(c, b)
    angle = np.arccos(np.dot(p, q) / (np.linalg.norm(p) * np.linalg.norm(q)))
    return np.rad2deg(angle)

path = os.path.join(os.getcwd(), 'Assets\\demo_and_body_positions.csv')
with open(path, newline='') as csvfile:
    reader = csv.DictReader(csvfile)
    for row in reader:
        demo_spine_shoulder = get_vector(row, 'Demo_SpineShoulder')
        demo_shoulder_left = get_vector(row, 'Demo_ShoulderLeft')
        demo_elbow_left = get_vector(row, 'Demo_ElbowLeft')
        demo_wrist_left = get_vector(row, 'Demo_WristLeft')
        demo_shoulder_right = get_vector(row, 'Demo_ShoulderRight')
        demo_elbow_right = get_vector(row, 'Demo_ElbowRight')
        demo_wrist_right = get_vector(row, 'Demo_WristRight')

        demo_hip_left = get_vector(row, 'Demo_HipLeft')
        demo_knee_left = get_vector(row, 'Demo_KneeLeft')
        demo_ankle_left = get_vector(row, 'Demo_AnkleLeft')
        demo_foot_left = get_vector(row, 'Demo_FootLeft')
        demo_hip_right = get_vector(row, 'Demo_HipRight')
        demo_knee_right = get_vector(row, 'Demo_KneeRight')
        demo_ankle_right = get_vector(row, 'Demo_AnkleRight')
        demo_foot_right = get_vector(row, 'Demo_FootRight')
        
        body_spine_shoulder = get_vector(row, 'Body_SpineShoulder')
        body_shoulder_left = get_vector(row, 'Body_ShoulderLeft')
        body_elbow_left = get_vector(row, 'Body_ElbowLeft')
        body_wrist_left = get_vector(row, 'Body_WristLeft')
        body_shoulder_right = get_vector(row, 'Body_ShoulderRight')
        body_elbow_right = get_vector(row, 'Body_ElbowRight')
        body_wrist_right = get_vector(row, 'Body_WristRight')

        body_hip_left = get_vector(row, 'Body_HipLeft')
        body_knee_left = get_vector(row, 'Body_KneeLeft')
        body_ankle_left = get_vector(row, 'Body_AnkleLeft')
        body_foot_left = get_vector(row, 'Body_FootLeft')
        body_hip_right = get_vector(row, 'Body_HipRight')
        body_knee_right = get_vector(row, 'Body_KneeRight')
        body_ankle_right = get_vector(row, 'Body_AnkleRight')
        body_foot_right = get_vector(row, 'Body_FootRight')
        
        # Shoulder left
        demo_angle_shoulder_left = get_angle(demo_spine_shoulder, demo_shoulder_left, demo_elbow_left)
        body_angle_shoulder_left = get_angle(body_spine_shoulder, body_shoulder_left, body_elbow_left)
        shoulder_left_error = abs(demo_angle_shoulder_left - body_angle_shoulder_left)
        sum_shoulder_left_error += shoulder_left_error

        # Elbow left
        demo_angle_elbow_left = get_angle(demo_shoulder_left, demo_elbow_left, demo_wrist_left)
        body_angle_elbow_left = get_angle(body_shoulder_left, body_elbow_left, body_wrist_left)
        elbow_left_error = abs(demo_angle_elbow_left - body_angle_elbow_left)
        sum_elbow_left_error += elbow_left_error

        # Shoulder right
        demo_angle_shoulder_right = get_angle(demo_spine_shoulder, demo_shoulder_right, demo_elbow_right)
        body_angle_shoulder_right = get_angle(body_spine_shoulder, body_shoulder_right, body_elbow_right)
        shoulder_right_error = abs(demo_angle_shoulder_right - body_angle_shoulder_right)
        sum_shoulder_right_error += shoulder_right_error
        
        # Elbow right
        demo_angle_elbow_right = get_angle(demo_shoulder_right, demo_elbow_right, demo_wrist_right)
        body_angle_elbow_right = get_angle(body_shoulder_right, body_elbow_right, body_wrist_right)
        elbow_right_error = abs(demo_angle_elbow_right - body_angle_elbow_right)
        sum_elbow_right_error += elbow_right_error

        # Knee left
        demo_angle_knee_left = get_angle(demo_hip_left, demo_knee_left, demo_ankle_left)
        body_angle_knee_left = get_angle(body_hip_left, body_knee_left, body_ankle_left)
        knee_left_error = abs(demo_angle_knee_left - body_angle_knee_left)
        sum_knee_left_error += knee_left_error

        # Ankle left
        demo_angle_ankle_left = get_angle(demo_knee_left, demo_ankle_left, demo_foot_left)
        body_angle_ankle_left = get_angle(body_knee_left, body_ankle_left, body_foot_left)
        ankle_left_error = abs(demo_angle_ankle_left - body_angle_ankle_left)
        sum_ankle_left_error += ankle_left_error

        # Knee right
        demo_angle_knee_right = get_angle(demo_hip_right, demo_knee_right, demo_ankle_right)
        body_angle_knee_right = get_angle(body_hip_right, body_knee_right, body_ankle_right)
        knee_right_error = abs(demo_angle_knee_right - body_angle_knee_right)
        sum_knee_right_error += knee_right_error

        # Ankle right
        demo_angle_ankle_right = get_angle(demo_knee_right, demo_ankle_right, demo_foot_right)
        body_angle_ankle_right = get_angle(body_knee_right, body_ankle_right, body_foot_right)
        ankle_right_error = abs(demo_angle_ankle_right - body_angle_ankle_right)
        sum_ankle_right_error += ankle_right_error

    size = reader.line_num - 1
    print("shoulder_left_error: ", sum_shoulder_left_error / size)
    print("elbow_left_error: ", sum_elbow_left_error / size)
    print("shoulder_right_error: ", sum_shoulder_right_error / size)
    print("elbow_right_error: ", sum_elbow_right_error / size)

    print("knee_left_error: ", sum_knee_left_error / size)
    print("ankle_left_error: ", sum_ankle_left_error / size)
    print("knee_right_error: ", sum_knee_right_error / size)
    print("ankle_right_error: ", sum_ankle_right_error / size)