import numpy as np
import matplotlib.pyplot as plt
import control
import toolbox_sr1

#Aufgabe 1 Matrizen für linearisierte Zustandsraummodell

#Teil von Aufgabe 7
def f_fwd(x,u):
    """ Modellgleichung Traktor (nicht-linear)
    fwd - ..................
    """
    
    # Parameter
    V  = .............. # [m/s]
    L1 = 6 # [m]
    L2 = 4 # [m]

    # ODE
    xdot = np.array([
        [...........],[V/L2*np.cos(............ - np.pi/2)]
    ])

    return np.squeeze(..........)
    #np.squeeze
    ############################################################

def f_bwd(x,u):
    """ Modellgleichung Traktor (nicht-linear)
    bwd - ......................
    """
    
    # Parameter
    V  = ................. # [m/s]
    L1 = 6 # [m]
    L2 = 4 # [m]

    # ODE
    xdot = np.array([
        [............],
        [............................]
    ])

    return ..................


def g(x,u):
    """ Ausgangsgleichung Traktor (nicht-linear)
    """
    return x[1]


#Aufgabe 2 Zustandsraummodell
#Vorwärtsfahren

#Parameter
V = ....................
L1 = 6.0 # [m]
L2 = 4.0 # [m]


#Systemmatrizen
#########################################
A = np.matrix([
    [...........,.........],
    [...........,.........]
    ])
B = np.matrix([
    [.....................],
    [0]
    ])
C = np.matrix([
    [0,1]
    ])
D = np.matrix([[0]])

#Zustandsraummodell erstellen mit "ss"
ss_fwd = control......(......)


#Aufgabe 3 Stablität

#Pole berechnen für
pole_fwd = control.pole(ss_fwd)
print("Die Polstellen des Vorwärts-Systems sind:",pole_fwd)

#Eigenvektoren berechnen 
U,V = np.linalg.eig(A)
print("Die Eigenwerte des Vorwärts-Systems sind:",U,"\n"
      "Die Eigenvektoren des Vorwärts-Systems sind:","\n",V)

#Das System ist .........................?

#Aufgabe 4

#Rückwärts fahren
V = -5 # [m/s]

#Systemmatrizen
A = np.matrix([
    [0,0],
    [V/L2, -V/L2]
    ])

B = np.matrix([
    [V/L1],
    [0]
    ])

#Zustandsraummodell erstellen mit "ss"
...... = control.ss(..........)

#Pole berechnen
pole_bwd = control.pole(........)
print("..................................:",pole_bwd)


#Aufgabe 5

#Simulation der linearen Systeme
num_steps = int(10.0/0.01) #solution of int? ..................
T = np.linspace(0, 10.0, num_steps)
U = 0.3*np.ones((num_steps,))  
[.............] = control. ...............(ss_fwd, T = T, U = U, X0 = [0,0], return_x = True)
[.............] = control. ...............(ss_bwd, T = T, U = U, X0 = [0,0], return_x = True)


# Aufgabe 6

#Vorwärts fahren
plt.figure()
plt.plot(T1, X_fwd[0,:], color='blue') #beta
plt.plot(T1, X_fwd[1,:], color='red') #gamma
plt.title("Vorwärts fahren")
plt.legend(['beta','gamma'])
plt.grid(True)
plt.show()

#Rückwärts fahren
plt.figure()
plt.plot(T1, X_bwd[0,:], color='blue') #beta
plt.plot(T1, X_bwd[1,:], color='red') #gamma
plt.title("Rückwärts fahren")
plt.legend(['beta','gamma'])
plt.grid(True)
plt.show()

# Aufgabe 7
x0 = ... # Anfangsbedingung
dt = ... # Zeitschritt
[X_fwd_nl, Y_fwd_nl, T_nl] = toolbox_sr1.nlsim(f_fwd, x0 ,U, dt,g)
[........................] = toolbox_sr1.nlsim(.........................)

#Aufgabe 8

#Vorwärts fahren
plt.figure()
plt.plot(T1, X_fwd[0,:], color='blue') #beta
plt.plot(T1, X_fwd[1,:], color='red') #gamma
plt.plot(T_nl, X_fwd_nl[:,0],color ='blue',linestyle='--')
plt.plot(T_nl, X_fwd_nl[:,1],color='red',linestyle='--')
plt.title("Vorwärts fahren")
plt.legend(['beta','gamma','beta_nonl','gamma_nonl'])
plt.grid(True)
plt.show()

#Rückwärts fahren
plt.figure()
plt.plot(T1, X_bwd[0,:], color='blue') #beta
plt.plot(T1, X_bwd[1,:], color='red') #gamma
plt.plot(T_nl, X_bwd_nl[:,0],color ='blue',linestyle='--')
plt.plot(T_nl, X_bwd_nl[:,1],color ='red',linestyle='--')
plt.title("Rückwärts fahren")
plt.legend(['beta','gamma','beta_nonl','gamma_nonl'])
#plt.ylim([-4,4])
plt.grid(True)
plt.show()