import json

import numpy as np

# rotation matrix from anybody to LeapMotion

ROTATION_KS = np.array([[0, 0, 1],

                        [0, 1, 0],

                        [-1, 0, 0]])

ROTATION_FINGER = np.array([[0, -1, 0],

                            [-1, 0, 0],

                            [0, 0, -1]])

ROTATION_THUMB = np.array([[0.9497,    0.1712,   -0.2624],

                           [-0.0351,   -0.7742,   -0.6320],

                           [-0.3113,    0.6094,   -0.7292]])

ROTATION_HAND = np.array([[-1, -0.2, -0.0],

                          [0.2, -0.5, 0.9],

                          [-0.2, 0.88, 0.5]])

ROTATION_ELBOW = np.array([[-1, 0, 0],

                          [0, -0.3, 1],

                          [0, 1, 0.3]])

class AnybodyFirstFrame:

    def __init__(self, update_values=False):

        self.joint_values = None

        if not update_values:

            self.load('config/anybody_joint_values_waagerecht.json')

    def load(self, filename):

        with open(filename) as o:

            self.joint_values = json.load(o)

    def get_basis(self, joint_name):

        anybody_basis = self.joint_values[joint_name]['Axes0']

        # print("joint: {}, basis: \n{}".format(joint_name, np.matmul(ROTATION_BASIS,

        # np.matmul(ROTATION_KS, np.array(anybody_basis)))))

        if joint_name in ('RightElbow'):

            return np.matmul(ROTATION_ELBOW, np.transpose(np.array(anybody_basis)))

        if joint_name in ('RightHand'):

            return np.matmul(ROTATION_HAND, np.transpose(np.array(anybody_basis)))

        if 'RightHandThumb' in joint_name:

            return np.matmul(ROTATION_THUMB, np.transpose(np.array(anybody_basis)))

        return np.matmul(ROTATION_FINGER, np.transpose(np.array(anybody_basis)))

    def get_position(self, joint_name):

        if '_Nub' in joint_name:

            anybody_position = self.joint_values[joint_name]['r']

        else:

            anybody_position = self.joint_values[joint_name]['r0']

        return np.matmul(ROTATION_KS, np.array(anybody_position) * 1000)