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)]


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.plot(t,alpha,'.',color='red',label='alpha')
plt.plot(t,beta,'.',color='green',label='beta')
plt.plot(t,gamma,'.',color='blue',label='gamma')
plt.plot(t,beta_next,'.',color='lightgray',label='theta')
#plt.plot(t,alpha+gamma,'.',color='magenta',label='a+g')
plt.plot(t_sing,np.zeros_like(t_sing),'x',color='black',label='singularity',markersize=12)
plt.xlabel('frame')
plt.ylabel('Euler angles')
#plt.legend(loc='upper right')
plt.show()
