import numpy as np
import matplotlib.pyplot as plt
import math

data = np.loadtxt('euler_angles.txt')

t = data[:,0]*24
alpha = data[:,1]
beta = data[:,2]
gamma = data[:,3]
beta_next = data[:,4]
sing = data[:,5]
t_sing = t[sing.astype(bool)]


#eps=1e-2
##alpha[alpha < (-np.pi + eps)] += 2 * np.pi
#gamma[gamma > (np.pi - eps)] -= 2 * np.pi
#alpha[alpha > (np.pi - eps)] -= 2 * np.pi


#plt.axhline(y=math.pi/2, color='black', linestyle='--', label='y = pi/2')
#plt.axhline(y=math.pi, color='black', linestyle='-', label='y = pi')
#plt.axhline(y=0, color='black', linestyle='-', label='y = pi')
#plt.axhline(y=-math.pi, color='black', linestyle='-', label='y = -pi')
#plt.axhline(y=-math.pi/2, color='black', linestyle='--', label='y = -pi/2')
#plt.axhline(y=-3*math.pi/2, color='black', linestyle='--', label='y = -pi/2')

plt.axhline(y=math.pi/2, color='black', linestyle='--')
plt.axhline(y=math.pi, color='black', linestyle='-')
plt.axhline(y=0, color='black', linestyle='-')
plt.axhline(y=-math.pi, color='black', linestyle='-')
plt.axhline(y=-math.pi/2, color='black', linestyle='--')
#plt.axhline(y=-3*math.pi/2, color='black', linestyle='--')

skip=1
plt.plot(t[1::skip],alpha[1::skip],'x',color='red',label=r'$\alpha$',markersize=24)
plt.plot(t[1::skip],beta[1::skip],'^',color='green',label=r'$\beta$',markersize=18)
plt.plot(t[1::skip],gamma[1::skip],'.',color='blue',label=r'$\gamma$',markersize=24)
#plt.plot(t,beta_next,'.',color='lightgreen',label='predicted beta',markersize=24)
plt.plot(t,beta_next,'.',color='cyan',label=r'$\gamma+\gamma_o$',markersize=12)
#plt.plot(t,alpha+gamma,'.',color='magenta',label='a+g')
fs = 64
#plt.plot(t_sing,np.zeros_like(t_sing),'x',color='black',label='singularity',markersize=fs)
#plt.xlabel('frame',fontsize=fs)
#plt.ylabel('Euler angles',fontsize=fs)

plt.xticks(fontsize=fs)
plt.yticks(fontsize=fs)

for i in range(len(sing)):
    if sing[i]:
        plt.axvline(x=t[i], color='black', linestyle='--', linewidth=1.5)

plt.legend(fontsize=fs,loc='lower left')
plt.show()
