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

#Teil von Aufgabe 7
def f_fwd(x,u):
    """ Modellgleichung Traktor (nicht-linear)
    """

    # ODE
    xdot = ...

    return np.squeeze(xdot)

def f_bwd(x,u):
    """ Modellgleichung Traktor (nicht-linear)
    """
    

    # ODE
    xdot = ...

    return np.squeeze(xdot)


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


#Aufgabe 2

#Parameter


#Systemmatrizen
A = ...
B = ...
C = ...
D = ...

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


#Aufgabe 3

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

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


#Aufgabe 4

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

#Systemmatrizen
A = ...
B = ...

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

#Pole berechnen mit "control.pole(...)"
pole_bwd = ...
print("Die Polstellen des Rückwärts-Systems sind:",pole_bwd)


#Aufgabe 5

# Simulation der linearen Systeme mit control.forced_response(...)
num_steps = ...
T = ...
U = ...
[T1, Y_fwd, X_fwd] = ...
[T1, Y_bwd, X_bwd] = ...

# Nichtlineare Simulation mit toolbox_sr1.nlsim(...)
[X_fwd_nl, Y_fwd_nl, T_nl] = ...
[X_bwd_nl, Y_fwd_nl, T_nl] = ...