import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from IPython.html.widgets import interact
from IPython.html import widgets

%matplotlib inline
np.set_printoptions(suppress=True, precision=3) # to avoid 1e-17 expressions in rotation matrix

def Rypr(y, p, r):
    '''
    Rotationsmatrix für y=yaw, p=pitch, r=roll in degrees
    '''
    # from Degree to Radians
    y = y*np.pi/180.0
    p = p*np.pi/180.0
    r = r*np.pi/180.0        
    
    Rr = np.matrix([[1.0, 0.0, 0.0],[0.0, np.cos(r), -np.sin(r)],[0.0, np.sin(r), np.cos(r)]])
    Rp = np.matrix([[np.cos(p), 0.0, np.sin(p)],[0.0, 1.0, 0.0],[-np.sin(p), 0.0, np.cos(p)]])
    Ry = np.matrix([[np.cos(y), -np.sin(y), 0.0],[np.sin(y), np.cos(y), 0.0],[0.0, 0.0, 1.0]])
    
    return Ry*Rp*Rr

gieren = 0.0
nicken = 45.0
wanken = 0.0

R = Rypr(gieren, nicken, wanken)
R

g = np.matrix([[1.0], [0.0], [0.0]])

vB = R*g
vB

@interact
def plotrot3Dpersp(gieren = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=0.0, description=""), \
                   nicken = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=0.0, description=""), \
                   wanken = widgets.FloatSliderWidget(min=-180.0, max=180.0, step=1.0, value=0.0, description="")):
    
    R = Rypr(gieren, nicken, wanken)
    print('%s' % R)
    x, y, z = R*g
    
    hor = (0, 90, 45)
    vert= (90, 0, 45)
    tit = ('Seitenansicht', 'Draufsicht', '3D')
    fig = plt.figure(figsize=(15,4.5))
    for subplotnumber in range(3):
        ax = fig.add_subplot(1,3, subplotnumber, projection='3d')
        
        ax.plot([0, x], [0, y], [0, z], label='$g \cdot R$')
        ax.plot([0, g[0]], [0, g[1]], [0, g[2]], label='$g$')
        
        ax.scatter(0,0,0, color='k')
        
        ax.axis('equal')
        ax.view_init(hor[subplotnumber], vert[subplotnumber])
        plt.xlabel('X')
        plt.ylabel('Y')
        plt.xlim(-1, 1)
        plt.ylim(-1, 1)
        plt.title(tit[subplotnumber] + '\nG: %i, N: %i, W: %i' % (gieren, nicken, wanken))
        ax.set_zlim3d(-1, 1)
        plt.legend(loc='best');
        plt.tight_layout()
        #plt.savefig('Rotation3D.png', dpi=100, bbox_inches='tight', transparent=True)

RT = np.transpose(R)
RT

g2 = RT*vB
g2

def Q2DuD(q):
    '''Calculates the Rotation Angle and Rotation Axis from Quaternion'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    a, b, c, d = q
    
    
    w = 2.0*np.arccos(a) # rotation angle
    s = np.sin(w/2.0)
    n = np.array([b/s, c/s, d/s]) # rotation axis
    
    return w, n

def normQ(q):
    '''Calculates the normalized Quaternion
    a is the real part
    b, c, d are the complex elements'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    a, b, c, d = q
   
    # Betrag
    Z = np.sqrt(a**2+b**2+c**2+d**2)
    
    if Z!=0.0:
        return np.array([a/Z,b/Z,c/Z,d/Z])
    else:
        return np.array([0.0,0.0,0.0,0.0])

def QtoR(q):
    '''Calculates the Rotation Matrix from Quaternion
    a is the real part
    b, c, d are the complex elements'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    q = normQ(q)
    
    a, b, c, d = q
    
    R11 = (a**2+b**2-c**2-d**2)
    R12 = 2.0*(b*c-a*d)
    R13 = 2.0*(b*d+a*c)
    
    R21 = 2.0*(b*c+a*d)
    R22 = a**2-b**2+c**2-d**2
    R23 = 2.0*(c*d-a*b)
    
    R31 = 2.0*(b*d-a*c)
    R32 = 2.0*(c*d+a*b)
    R33 = a**2-b**2-c**2+d**2
    
    return np.matrix([[R11, R12, R13],[R21, R22, R23],[R31, R32, R33]])

def Q2Eul(q):
    '''Calculates the Euler Angles from Quaternion
    a is the real part
    b, c, d are the complex elements'''
    # Source: Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler.
    # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler
    q = normQ(q)
    
    a, b, c, d = q
    
    gieren = np.arctan2(2.0*(b*c+a*d),(a**2+b**2-c**2-d**2)) * 180.0/np.pi
    nicken = np.arcsin(2.0*(a*c-b*d)) * 180.0/np.pi
    wanken = -np.arctan2(2.0*(c*d+a*b),-(a**2-b**2-c**2+d**2)) * 180.0/np.pi

    return np.array([gieren, nicken, wanken])

@interact
def plotrot3Dpersp(a = widgets.FloatSliderWidget(min=-0.99, max=0.99, step=0.01, value=0.0, description=""), \
                   b = widgets.FloatSliderWidget(min=-1.0, max=1.0, step=0.01, value=1.0, description=""), \
                   c = widgets.FloatSliderWidget(min=-1.0, max=1.0, step=0.01, value=0.0, description=""), \
                   d = widgets.FloatSliderWidget(min=-1.0, max=1.0, step=0.01, value=0.0, description="")):

    q = np.array([a, b, c, d])
    
    R = QtoR(q)
    print('Rotationsmatrix aus Quaternion\n%s' % R)
    
    gieren, nicken, wanken = Q2Eul(q)
    
    x, y, z = R*g

    hor = (0, 90, 45)
    vert= (90, 0, 45)
    tit = ('Seitenansicht', 'Draufsicht', '3D')
    fig = plt.figure(figsize=(15,4.5))
    for subplotnumber in range(3):
        ax = fig.add_subplot(1,3, subplotnumber, projection='3d')
        
        ax.plot([0, x], [0, y], [0, z], label='$g \cdot R$')
        ax.plot([0, g[0]], [0, g[1]], [0, g[2]], label='$g$')
        
        # Drehachse einzeichnen
        w, n = Q2DuD(q)
        ax.plot([0, n[0]], [0, n[1]], [0, n[2]], label='$n_R$', color='gray')
        
        ax.scatter(0,0,0, color='k')
        
        ax.axis('equal')
        ax.view_init(hor[subplotnumber], vert[subplotnumber])
        plt.xlabel('X')
        plt.ylabel('Y')
        plt.title(tit[subplotnumber] + '\nG: %i, N: %i, W: %i' % (gieren, nicken, wanken))
        plt.xlim(-1, 1)
        plt.ylim(-1, 1)
        ax.set_zlim3d(-1, 1)
        plt.legend(loc='best');
        plt.tight_layout()