import numpy as np

import matplotlib.pyplot as plt

from resources.pymo.pymo.parsers import BVHParser

from matplotlib2tikz import save as tikz_save

# BVH-File

bvh = BVHParser()

bvh_data = bvh.parse('../../output/BVH/20190213_validierung_1.bvh')

bvh_number_frames = np.size(bvh.data.values.RightHandIndex2_Xrotation.values)

bvh_frames = np.arange(0, bvh_number_frames)

print(bvh_number_frames, bvh_frames)

# Thumb

bvh_RightHandThumb2 = np.array([bvh_data.values.RightHandThumb2_Xrotation.values,

                                bvh_data.values.RightHandThumb2_Yrotation.values,

                                bvh_data.values.RightHandThumb2_Zrotation.values])

bvh_RightHandThumb3 = np.array([bvh_data.values.RightHandThumb3_Xrotation.values,

                                bvh_data.values.RightHandThumb3_Yrotation.values,

                                bvh_data.values.RightHandThumb3_Zrotation.values])

bvh_RightHandThumb4 = np.array([bvh_data.values.RightHandThumb4_Xrotation.values,

                                bvh_data.values.RightHandThumb4_Yrotation.values,

                                bvh_data.values.RightHandThumb4_Zrotation.values])

bvh_RightHandThumb5 = np.array([bvh_data.values.RightHandThumb5_Xrotation.values,

                                bvh_data.values.RightHandThumb5_Yrotation.values,

                                bvh_data.values.RightHandThumb5_Zrotation.values])

# Index

bvh_RightHandIndex1 = np.array([bvh_data.values.RightHandIndex1_Xrotation.values,

                                bvh_data.values.RightHandIndex1_Yrotation.values,

                                bvh_data.values.RightHandIndex1_Zrotation.values])

bvh_RightHandIndex2 = np.array([bvh_data.values.RightHandIndex2_Xrotation.values,

                                bvh_data.values.RightHandIndex2_Yrotation.values,

                                bvh_data.values.RightHandIndex2_Zrotation.values])

bvh_RightHandIndex3 = np.array([bvh_data.values.RightHandIndex3_Xrotation.values,

                                bvh_data.values.RightHandIndex3_Yrotation.values,

                                bvh_data.values.RightHandIndex3_Zrotation.values])

bvh_RightHandIndex4 = np.array([bvh_data.values.RightHandIndex4_Xrotation.values,

                                bvh_data.values.RightHandIndex4_Yrotation.values,

                                bvh_data.values.RightHandIndex4_Zrotation.values])

bvh_RightHandIndex5 = np.array([bvh_data.values.RightHandIndex5_Xrotation.values,

                                bvh_data.values.RightHandIndex5_Yrotation.values,

                                bvh_data.values.RightHandIndex5_Zrotation.values])

# Middle

bvh_RightHandMiddle1 = np.array([bvh_data.values.RightHandMiddle1_Xrotation.values,

                                 bvh_data.values.RightHandMiddle1_Yrotation.values,

                                 bvh_data.values.RightHandMiddle1_Zrotation.values])

bvh_RightHandMiddle2 = np.array([bvh_data.values.RightHandMiddle2_Xrotation.values,

                                 bvh_data.values.RightHandMiddle2_Yrotation.values,

                                 bvh_data.values.RightHandMiddle2_Zrotation.values])

bvh_RightHandMiddle3 = np.array([bvh_data.values.RightHandMiddle3_Xrotation.values,

                                 bvh_data.values.RightHandMiddle3_Yrotation.values,

                                 bvh_data.values.RightHandMiddle3_Zrotation.values])

bvh_RightHandMiddle4 = np.array([bvh_data.values.RightHandMiddle4_Xrotation.values,

                                 bvh_data.values.RightHandMiddle4_Yrotation.values,

                                 bvh_data.values.RightHandMiddle4_Zrotation.values])

bvh_RightHandMiddle5 = np.array([bvh_data.values.RightHandMiddle5_Xrotation.values,

                                 bvh_data.values.RightHandMiddle5_Yrotation.values,

                                 bvh_data.values.RightHandMiddle5_Zrotation.values])

# Ring

bvh_RightHandRing1 = np.array([bvh_data.values.RightHandRing1_Xrotation.values,

                               bvh_data.values.RightHandRing1_Yrotation.values,

                               bvh_data.values.RightHandRing1_Zrotation.values])

bvh_RightHandRing2 = np.array([bvh_data.values.RightHandRing2_Xrotation.values,

                               bvh_data.values.RightHandRing2_Yrotation.values,

                               bvh_data.values.RightHandRing2_Zrotation.values])

bvh_RightHandRing3 = np.array([bvh_data.values.RightHandRing3_Xrotation.values,

                               bvh_data.values.RightHandRing3_Yrotation.values,

                               bvh_data.values.RightHandRing3_Zrotation.values])

bvh_RightHandRing4 = np.array([bvh_data.values.RightHandRing4_Xrotation.values,

                               bvh_data.values.RightHandRing4_Yrotation.values,

                               bvh_data.values.RightHandRing4_Zrotation.values])

bvh_RightHandRing5 = np.array([bvh_data.values.RightHandRing5_Xrotation.values,

                               bvh_data.values.RightHandRing5_Yrotation.values,

                               bvh_data.values.RightHandRing5_Zrotation.values])

# Pinky

bvh_RightHandPinky1 = np.array([bvh_data.values.RightHandPinky1_Xrotation.values,

                                bvh_data.values.RightHandPinky1_Yrotation.values,

                                bvh_data.values.RightHandPinky1_Zrotation.values])

bvh_RightHandPinky2 = np.array([bvh_data.values.RightHandPinky2_Xrotation.values,

                                bvh_data.values.RightHandPinky2_Yrotation.values,

                                bvh_data.values.RightHandPinky2_Zrotation.values])

bvh_RightHandPinky3 = np.array([bvh_data.values.RightHandPinky3_Xrotation.values,

                                bvh_data.values.RightHandPinky3_Yrotation.values,

                                bvh_data.values.RightHandPinky3_Zrotation.values])

bvh_RightHandPinky4 = np.array([bvh_data.values.RightHandPinky4_Xrotation.values,

                                bvh_data.values.RightHandPinky4_Yrotation.values,

                                bvh_data.values.RightHandPinky4_Zrotation.values])

bvh_RightHandPinky5 = np.array([bvh_data.values.RightHandPinky5_Xrotation.values,

                                bvh_data.values.RightHandPinky5_Yrotation.values,

                                bvh_data.values.RightHandPinky5_Zrotation.values])

# Anybody

interpol_data = np.loadtxt("hand_interpolated_joint_angles_n047_2.txt", delimiter=",", skiprows=1, unpack=True)

any_number_frame = np.size(interpol_data[1])

any_frame = np.arange(0, bvh_number_frames, np.round(bvh_number_frames/any_number_frame))

print(any_number_frame, any_frame)

# Thumb

any_RightHandThumb2 = np.array([np.multiply(interpol_data[1], (180 / np.pi)),   # CMC1Flexion

                                np.multiply(interpol_data[2], (180 / np.pi)),   # CMC1Abduction

                                np.zeros(any_number_frame)])                    # CMC1Deviation = 0

any_RightHandThumb3 = np.array([np.multiply(interpol_data[3], (180 / np.pi)),   # MCP1Flexion

                                np.multiply(interpol_data[4], (180 / np.pi)),   # MCP1Abduction

                                np.zeros(any_number_frame)])                    # MCP1Deviation = 0

any_RightHandThumb4 = np.array([np.multiply(interpol_data[5], (180 / np.pi)),   # DIP1Flexion

                                np.zeros(any_number_frame),                     # DIP1Abduction = 0

                                np.zeros(any_number_frame)])                    # DIP1Deviation = 0

any_RightHandThumb5 = np.array([np.zeros(any_number_frame),                     # = 0

                                np.zeros(any_number_frame),                     # = 0

                                np.zeros(any_number_frame)])                    # = 0

# Index

any_RightHandIndex1 = np.array([np.zeros(any_number_frame),                     # CMC2Flexion = 0

                                np.zeros(any_number_frame),                     # CMC2Abduction = 0

                                np.zeros(any_number_frame)])                    # CMC2Deviation = 0

any_RightHandIndex2 = np.array([np.multiply(interpol_data[6], (180 / np.pi)),   # MCP2Flexion

                                np.multiply(interpol_data[7], (180 / np.pi)),   # MCP2Abduction

                                np.zeros(any_number_frame)])                    # MCP2Deviation = 0

any_RightHandIndex3 = np.array([np.multiply(interpol_data[8], (180 / np.pi)),   # PIP2Flexion

                                np.zeros(any_number_frame),                     # PIP2Abduction = 0

                                np.zeros(any_number_frame)])                    # PIP2Deviation = 0

any_RightHandIndex4 = np.array([np.multiply(interpol_data[9], (180 / np.pi)),   # DIP2Flexion

                                np.zeros(any_number_frame),                     # DIP2Abduction = 0

                                np.zeros(any_number_frame)])                    # DIP2Deviation = 0

any_RightHandIndex5 = np.array([np.zeros(any_number_frame),                     # = 0

                                np.zeros(any_number_frame),                     # = 0

                                np.zeros(any_number_frame)])                    # = 0

# Middle

any_RightHandMiddle1 = np.array([np.zeros(any_number_frame),                     # CMC3Flexion = 0

                                 np.zeros(any_number_frame),                     # CMC3Abduction = 0

                                 np.zeros(any_number_frame)])                    # CMC3Deviation = 0

any_RightHandMiddle2 = np.array([np.multiply(interpol_data[10], (180 / np.pi)),  # MCP3Flexion

                                 np.multiply(interpol_data[11], (180 / np.pi)),  # MCP3Abduction

                                 np.zeros(any_number_frame)])                    # MCP3Deviation = 0

any_RightHandMiddle3 = np.array([np.multiply(interpol_data[12], (180 / np.pi)),  # PIP3Flexion

                                 np.zeros(any_number_frame),                     # PIP3Abduction = 0

                                 np.zeros(any_number_frame)])                    # PIP3Deviation = 0

any_RightHandMiddle4 = np.array([np.multiply(interpol_data[13], (180 / np.pi)),  # DIP3Flexion

                                 np.zeros(any_number_frame),                     # DIP3Abduction = 0

                                 np.zeros(any_number_frame)])                    # DIP3Deviation = 0

any_RightHandMiddle5 = np.array([np.zeros(any_number_frame),                     # = 0

                                 np.zeros(any_number_frame),                     # = 0

                                 np.zeros(any_number_frame)])                    # = 0

# Ring

any_RightHandRing1 = np.array([np.zeros(any_number_frame),                     # CMC4Flexion = 0

                               np.zeros(any_number_frame),                     # CMC4Abduction = 0

                               np.zeros(any_number_frame)])                    # CMC4Deviation = 0

any_RightHandRing2 = np.array([np.multiply(interpol_data[14], (180 / np.pi)),  # MCP4Flexion

                               np.multiply(interpol_data[15], (180 / np.pi)),  # MCP4Abduction

                               np.zeros(any_number_frame)])                    # MCP4Deviation = 0

any_RightHandRing3 = np.array([np.multiply(interpol_data[16], (180 / np.pi)),  # PIP4Flexion

                               np.zeros(any_number_frame),                     # PIP4Abduction = 0

                               np.zeros(any_number_frame)])                    # PIP4Deviation = 0

any_RightHandRing4 = np.array([np.multiply(interpol_data[17], (180 / np.pi)),  # DIP4Flexion

                               np.zeros(any_number_frame),                     # DIP4Abduction = 0

                               np.zeros(any_number_frame)])                    # DIP4Deviation = 0

any_RightHandRing5 = np.array([np.zeros(any_number_frame),                     # = 0

                               np.zeros(any_number_frame),                     # = 0

                               np.zeros(any_number_frame)])                    # = 0

# Pinky

any_RightHandPinky1 = np.array([np.zeros(any_number_frame),                     # CMC5Flexion = 0

                                np.zeros(any_number_frame),                     # CMC5Abduction = 0

                                np.zeros(any_number_frame)])                    # CMC5Deviation = 0

any_RightHandPinky2 = np.array([np.multiply(interpol_data[18], (180 / np.pi)),  # MCP5Flexion

                                np.multiply(interpol_data[19], (180 / np.pi)),  # MCP5Abduction

                                np.zeros(any_number_frame)])                    # MCP5Deviation = 0

any_RightHandPinky3 = np.array([np.multiply(interpol_data[20], (180 / np.pi)),  # PIP5Flexion

                                np.zeros(any_number_frame),                     # PIP5Abduction = 0

                                np.zeros(any_number_frame)])                    # PIP5Deviation = 0

any_RightHandPinky4 = np.array([np.multiply(interpol_data[21], (180 / np.pi)),  # DIP5Flexion

                                np.zeros(any_number_frame),                     # DIP5Abduction = 0

                                np.zeros(any_number_frame)])                    # DIP5Deviation = 0

any_RightHandPinky5 = np.array([np.zeros(any_number_frame),                     # = 0

                                np.zeros(any_number_frame),                     # = 0

                                np.zeros(any_number_frame)])                    # = 0

# Plot

bvh_plot = bvh_RightHandIndex3

any_plot = any_RightHandIndex3

# use LaTeX fonts in the plot

# plt.rc('text', usetex=True)

plt.rc('font', family='serif')

fig, axs = plt.subplots(3, 1)

axs[0].plot(bvh_frames, bvh_plot[0], linestyle='-', linewidth=2)

axs[0].plot(any_frame, -any_plot[0], linestyle=':', linewidth=2, marker='x')

axs[0].set_xlim(0, bvh_number_frames)

axs[0].set_ylim(-90, 90)

axs[0].set_xlabel('frames')

axs[0].set_ylabel('angle in degree')

axs[0].set_title('flexion (x-axis)')

axs[0].legend(['bvh', 'any'], loc='center left', bbox_to_anchor=(1, 0.5))

axs[0].grid(True)

axs[1].plot(bvh_frames, bvh_plot[1], linestyle='-', linewidth=2)

axs[1].plot(any_frame, -any_plot[1], linestyle=':', linewidth=2, marker='x')

axs[1].set_xlim(0, bvh_number_frames)

axs[1].set_ylim(-90, 90)

axs[1].set_xlabel('frames')

axs[1].set_ylabel('angle in degree')

axs[1].set_title('abduction (y-axis)')

axs[1].legend(['bvh', 'any'], loc='center left', bbox_to_anchor=(1, 0.5))

axs[1].grid(True)

axs[2].plot(bvh_frames, bvh_plot[2], linestyle='-', linewidth=2)

axs[2].plot(any_frame, -any_plot[2], linestyle=':', linewidth=2, marker='x')

axs[2].set_xlim(0, bvh_number_frames)

axs[2].set_ylim(-90, 90)

axs[2].set_xlabel('frames')

axs[2].set_ylabel('angle in degree')

axs[2].set_title('deviation (z-axis)')

axs[2].legend(['bvh', 'any'], loc='center left', bbox_to_anchor=(1, 0.5))

axs[2].grid(True)

fig.tight_layout()

plt.savefig("pgf_texsystem.pdf")

plt.savefig("pgf_texsystem.png")

plt.show()