import inspect

import os

import re

import sys

import numpy as np

# import math

from pymo.RotationUtil import vec2eul, rot2eul

from leapmotion_setup_rot import root_name, skeleton, framerate

from pymo.data import MocapData

src_dir = os.path.dirname(inspect.getfile(inspect.currentframe()))

# Windows and Linux

arch_dir = '../../lib/x64' if sys.maxsize > 2 ** 32 else '../../lib/x86'

sys.path.insert(0, os.path.abspath(os.path.join(src_dir, arch_dir)))

import Leap

class Leap2BVH:

    """

    A class to convert LeapMotion frames to PyMO data structure (MocapData) to be parsed to a BVH file

    Calculates translations (offsets) and rotation data for the joints

    """

    def __init__(self, filename=None):

        self._skeleton = {}

        self.bone_context = []

        self._motion_channels = []

        self._motions = []

        self.current_token = 0

        self.framerate = 0.0

        self.root_name = ''

        self.data = MocapData()

        self.do()

    def parse(self):

        self.data.skeleton = self._skeleton

        self.data.channel_names = self._motion_channels

        self.data.values = self._to_DataFrame()

        self.data.root_name = self.root_name

        self.data.framerate = self.framerate

        return self.data

    def do(self):

        self._skeleton = skeleton

        # self._motion_channels = motion_channels

        self.root_name = root_name

        self.framerate = framerate  # TODO: framerate

        # frame_count = 188#TODO:frame_count

        # frame_time = 0.0

        # self._motions = [()] * frame_count

        for key, value in self._skeleton.items():

            value['offsets'] = [0, 0, 0]

            for channel in value['channels']:

                self._motion_channels.append((key, channel))

        # for ii in range(frame_count):

        #     channel_values = []

        #     for key, value in self._skeleton.items():

        #         for channel in value['channels']:

        #             channel_values.append((key, channel, float(1)))

        #     self._motions[ii] = (frame_time, channel_values)

        #     frame_time = frame_time + framerate

    def add_frame(self, frame_id, hand):

        channel_values = []

        for key, value in self._skeleton.items():

            if frame_id == 0:

                # offsets

                if key == 'Leap_Root':

                    x_offset, y_offset, z_offset, _, _, _ = self._get_root_values()

                elif key == 'RightHand':

                    x_offset, y_offset, z_offset, _, _, _ = self._get_wrist_values(hand)

                elif 'End' in key:

                    # Workaround for getting motion data also from finger tip by adding a not used end

                    x_offset = y_offset = z_offset = 0.0

                else:

                    x_offset, y_offset, z_offset, _, _, _ = self._get_finger_values(key, hand)

                value['offsets'] = [x_offset, y_offset, z_offset]

                # print("pitch hand x = {}, angle hand x = {}".format(hand.basis.x_basis.pitch * Leap.RAD_TO_DEG,

                #                                                     hand.basis.x_basis.angle_to(Leap.Vector.x_axis) * Leap.RAD_TO_DEG))

            for channel in value['channels']:

                # motion data with rotations

                if key == 'Leap_Root':

                    x_pos, y_pos, z_pos, x_rot, y_rot, z_rot = self._get_root_values()

                elif key == 'RightHand':

                    x_pos, y_pos, z_pos, x_rot, y_rot, z_rot = self._get_wrist_values(hand)

                else:

                    x_pos, y_pos, z_pos, x_rot, y_rot, z_rot = self._get_finger_values(key, hand)

                if channel == 'Xposition':

                    channel_values.append((key, channel, x_pos))

                if channel == 'Yposition':

                    channel_values.append((key, channel, y_pos))

                if channel == 'Zposition':

                    channel_values.append((key, channel, z_pos))

                if channel == 'Xrotation':

                    channel_values.append((key, channel, x_rot))

                if channel == 'Yrotation':

                    channel_values.append((key, channel, y_rot))

                if channel == 'Zrotation':

                    channel_values.append((key, channel, z_rot))

        self._motions.append((frame_id, channel_values))

    @staticmethod

    def _get_root_values():

        return 0, 0, 0, 0, 0, 0

    def _get_wrist_values(self, hand):

        x_wrist = hand.wrist_position.x

        y_wrist = hand.wrist_position.y

        z_wrist = hand.wrist_position.z

        # rotation matrix from basis vectors

        rotmat = self._basis2rot(hand.basis)

        eul_x, eul_y, eul_z = rot2eul(rotmat)

        return \

            x_wrist, \

            y_wrist, \

            z_wrist, \

            eul_x * Leap.RAD_TO_DEG, \

            eul_y * Leap.RAD_TO_DEG, \

            eul_z * Leap.RAD_TO_DEG

    def _get_finger_values(self, key, hand):

        key, bone_number = self._split_key(key)

        fingerlist = hand.fingers.finger_type(self._get_finger_type(key))

        # vector between wrist and metacarpal proximal (carpals)

        if bone_number == 1 or ('Thumb' in key and bone_number == 2):

            bone = fingerlist[0].bone(self._get_bone_type(bone_number))

            x_wrist, y_wrist, z_wrist, _, _, _ = self._get_wrist_values(hand)

            # print("1: key: {}, bone_number: {}, bone: {}, prev_joint: {}"

            #       .format(key, bone_number, bone, bone.prev_joint))

            # print("p1_rot = [{}; {}; {}];".format(x_wrist, y_wrist, z_wrist))

            # print("p2_rot = [{}; {}; {}];".format(bone.prev_joint.x, bone.prev_joint.y, bone.prev_joint.z))

            # print("p3_rot = [{}; {}; {}];".format(bone.next_joint.x, bone.next_joint.y, bone.next_joint.z))

            # print('bone.next_joint.x = {}, bone.prev_joint.x = {}, x_wrist = {}, vec_prev = {}'.format(bone.next_joint.x, bone.prev_joint.x, x_wrist, vec_prev[0]))

            # print('bone.next_joint.y = {}, bone.prev_joint.y = {}, y_wrist = {}, vec_prev = {}'.format(bone.next_joint.y, bone.prev_joint.y, y_wrist, vec_prev[1]))

            # print('bone.next_joint.z = {}, bone.prev_joint.z = {}, z_wrist = {}, vec_prev = {}'.format(bone.next_joint.z, bone.prev_joint.z, z_wrist, vec_prev[2]))

            return \

                bone.prev_joint.x - x_wrist, \

                bone.prev_joint.y - y_wrist, \

                bone.prev_joint.z - z_wrist, \

                0.0, \

                0.0, \

                0.0

        # vector for bones metacarpal, proximal, intermediate, distal

        bone_prev = fingerlist[0].bone(self._get_bone_type(bone_number - 1))

        #  no rotation for finger tip

        if not bone_number == 5:

            bone_next = fingerlist[0].bone(self._get_bone_type(bone_number))

            # # rotation matrix from basis vectors

            # rotmat_prev = self._basis2rot(bone_prev.basis)

            # rotmat_next = self._basis2rot(bone_next.basis)

            #

            # # rotation matrix between rotmat_prev and rotmat_next by multiplying

            # eul_x, eul_y, eul_z = rot2eul(np.matmul(rotmat_next, np.transpose(rotmat_prev)))

            vec_prev = np.array([bone_prev.next_joint.x - bone_prev.prev_joint.x,

                                 bone_prev.next_joint.y - bone_prev.prev_joint.y,

                                 bone_prev.next_joint.z - bone_prev.prev_joint.z])

            vec_next = np.array([bone_next.next_joint.x - bone_next.prev_joint.x,

                                 bone_next.next_joint.y - bone_next.prev_joint.y,

                                 bone_next.next_joint.z - bone_next.prev_joint.z])

            eul_x, eul_y, eul_z = vec2eul(vec_prev, vec_next)

            return \

                bone_prev.next_joint.x - bone_prev.prev_joint.x, \

                bone_prev.next_joint.y - bone_prev.prev_joint.y, \

                bone_prev.next_joint.z - bone_prev.prev_joint.z, \

                eul_x * Leap.RAD_TO_DEG, \

                eul_y * Leap.RAD_TO_DEG, \

                eul_z * Leap.RAD_TO_DEG

        return \

            bone_prev.next_joint.x - bone_prev.prev_joint.x, \

            bone_prev.next_joint.y - bone_prev.prev_joint.y, \

            bone_prev.next_joint.z - bone_prev.prev_joint.z, \

            0.0, \

            0.0, \

            0.0

    @staticmethod

    def _split_key(key):

        key_split = re.split('(\d)', key)

        key = key_split[0]

        if key_split[-1] == '_Nub':

            return key, 5

        else:

            return key, int(key_split[1])

    @staticmethod

    def _get_finger_type(key):

        if key == 'RightHandThumb':

            return Leap.Finger.TYPE_THUMB

        if key == 'RightHandIndex':

            return Leap.Finger.TYPE_INDEX

        if key == 'RightHandMiddle':

            return Leap.Finger.TYPE_MIDDLE

        if key == 'RightHandRing':

            return Leap.Finger.TYPE_RING

        if key == 'RightHandPinky':

            return Leap.Finger.TYPE_PINKY

        else:

            raise Exception('Key ({}) did not match'.format(key))

    @staticmethod

    def _get_bone_type(bone_number):

        if bone_number == 4:

            return Leap.Bone.TYPE_DISTAL

        if bone_number == 3:

            return Leap.Bone.TYPE_INTERMEDIATE

        if bone_number == 2:

            return Leap.Bone.TYPE_PROXIMAL

        if bone_number == 1:

            return Leap.Bone.TYPE_METACARPAL

        else:

            raise Exception('bone number ({}) did not match'.format(bone_number))

    @staticmethod

    def _basis2rot(basis):

        return np.array([[basis.x_basis.x, basis.y_basis.x, basis.z_basis.x],

                        [basis.x_basis.y, basis.y_basis.y, basis.z_basis.y],

                        [basis.x_basis.z, basis.y_basis.z, basis.z_basis.z]])

    def _to_DataFrame(self):

        """Returns all of the channels parsed from the file as a pandas DataFrame"""

        import pandas as pd

        time_index = pd.to_timedelta([f[0] for f in self._motions], unit='s')

        frames = [f[1] for f in self._motions]

        channels = np.asarray([[channel[2] for channel in frame] for frame in frames])

        column_names = ['%s_%s' % (c[0], c[1]) for c in self._motion_channels]

        return pd.DataFrame(data=channels, index=time_index, columns=column_names)