import pyquaternion
from pyquaternion import Quaternion
import numpy as np


def getQfromR(R):
    return pyquaternion.Quaternion(matrix=R)

def quaternion_distance(r1, r2):
    r1 = r1.reshape((3, 3))
    r2 = r2.reshape((3, 3))
    q1 = getQfromR(r1)
    q2 = getQfromR(r2)
    dist =  q1 * q2.inverse
    return dist.degrees

#статистики по всем
dists = {"Alexey": [], "Ratmir": [], "Anvar": [], "Ilmir": [], "Anvar": [], "Arkadiy": [], "Alice": [], "Ilya": []}

#Считывание файла
content = json.load(f)
position = content["RightRing"]
label = position["Imu"]["CalibratedRelativeHmd"]
for i in range(len(list(position["Markup"]["Orientation"]))):
    q1 = Quaternion(label[3], label[0], label[1], label[2])
    label_human = list(position["Markup"]["Orientation"][i].values())[-1]
    key = list(position["Markup"]["Orientation"][i].keys())[-1]

    q2 = Quaternion(label_human[3], label_human[0], label_human[1], label_human[2])
    dist = (quaternion_distance(q1.rotation_matrix, q2.rotation_matrix))
    #сохраняем результат на данном человека для данного кадра
    dists[key].append((dist))


#после того, как считали для всех файлов - сохраняем
for x in dists:
    print(x)
    arr = dists[x]
    np.save(f"../{x}.npy", arr)
    print(np.mean(arr), np.median(arr))


#рисуем гистограмму
data = np.load("Ratmir.npy")
plt.hist(data, bins=30, alpha=0.5)
plt.show()