close all
clear all
clc

%% Aufgabe 1a

dt = 0.01;
N  = 10/dt;
x0 = [0;0;0;0];
u  = 0.3*ones(1,N);

[X, Y, T] = nlsim(@tractor_trailer_ode_v_pos, x0, u, dt, @tractor_trailer_out);

%% Aufgabe 1b

% Plotten der Ausgaenge
figure;
plot(T,Y)
legend('xT','yT','xA','yA')
grid on

% Plotten der Zustaende
figure;
plot(T,X)
legend('xT','yT','\beta','\gamma')
grid on

% Plotten der Positionen
figure;
plot(Y(1,:),Y(2,:))
hold on
plot(Y(3,:),Y(4,:))
axis equal
legend('Traktor','Anhaenger')
grid on

%% Aufgabe 1c

% animation(X,u,dt)

%% Aufgabe 1d

[Xbwd, Ybwd, Tbwd] = nlsim(@tractor_trailer_ode_v_neg, x0, u, dt, @tractor_trailer_out);
% animation(Xbwd,u,dt)

%% Aufgabe 2a

V = 5;
L1 = 6;
L2 = 4;

x_ss = [0;0];
u_ss = [0];

A = [0 0; V/L2*(cos(x_ss(1)-x_ss(2))) -V/L2*(cos(x_ss(1)-x_ss(2)))];
B = [V/L1/cos(u_ss)^2; 0];
C = [0 1];
D = 0;

%% Aufgabe 2b

ss_fwd = ss(A,B,C,D);

%% Aufgabe 2c

eig(A)

%% Aufgabe 2d

V = -5;
A = [0 0; V/L2*(cos(x_ss(1)-x_ss(2))) -V/L2*(cos(x_ss(1)-x_ss(2)))];
B = [V/L1/cos(u_ss)^2; 0];
C = [0 1];
D = 0;

ss_bwd = ss(A,B,C,D);
eig(A)

%% Aufgabe 2e

[y,t_fwd,x_fwd] = lsim(ss_fwd, u, T(1:end-1), x_ss);
[y,t_bwd,x_bwd] = lsim(ss_bwd, u, T(1:end-1), x_ss);

%% Aufgabe 2f

figure;
plot(t_fwd,x_fwd(:,1),'-r');
hold on
plot(T,X(3,:),'--r')
plot(t_fwd,x_fwd(:,2),'-b');
plot(T,X(4,:),'--b')
legend({'$\beta$','$\beta_{\mathrm{nl}}$', '$\gamma$','$\gamma_{\mathrm{nl}}$'}, 'Interpreter', 'latex', ...
    'Location', 'southeast');
grid on

figure;
plot(t_bwd,x_bwd(:,1),'-r');
hold on
plot(Tbwd,Xbwd(3,:),'--r')
plot(t_bwd,x_bwd(:,2),'-b');
plot(Tbwd,Xbwd(4,:),'--b')
legend({'$\beta$','$\beta_{\mathrm{nl}}$', '$\gamma$','$\gamma_{\mathrm{nl}}$'}, 'Interpreter', 'latex', ...
    'Location', 'southeast');
ylim([-4 4])
grid on

%% Aufgabe 2g

animation([zeros(N,2) x_bwd]',u,dt)