%Uppgift 2, Filip Johansson och Anton Hagelberg
%Eulers metod - Framåt Uppgift 2A/B
clc; clear all; close all;
k1 = 5300;
k2 = 136000;
vVec = [0;0;0;0];
n = 10000;
L = 1;
v = 65/3.6;
T = (L/v)*30;
h = T/n;

for i = 1:n
    t = (i-1)*h;
    vVec(:,i+1) = vVec(:,i) + h*quartercar(t,vVec(:,i),k1,k2);
end

tid = 0:h:T;
figure(1)
plot(tid,vVec(1,:),tid,vVec(2,:))
title('Euler som z1 och z2')
xlabel('tid[s]')
ylabel('förskjutning [m]')
legend('z1','z2');


%% ODE45-metoden
clc; clear all; close all;
k1 = 5300;
k2 = 136000;
t0 = 0;
v = 65/3.6;
L = 1;
T = (L/v)*30;

options = odeset('RelTol',1e-6,'refine', 5);
vVec = [0;0;0;0]';

[tVec, yVec] = ode45(@(t,vVec) quartercar(t,vVec,k1,k2),[t0 T],vVec,options);

figure(2)
plot(tVec(:,1),yVec(:,1),tVec(:,1),yVec(:,2));

delt = zeros();
n = length(tVec);

for i = 1:n-1
    delt = [delt, abs(tVec(i) - tVec(i+1))];
end
figure(3)
plot(1:length(delt),delt);

%%
%%Jämförelse mellan olika delta T
clc; clear all; close all;
k1 = 5300;
k2 = 136000;
vVec = [0;0;0;0];
L = 1;
v = 65/3.6;
t0 = 0;
T = (L/v)*30;
for j = 1:2
    h = [5e-3, 5e-4];
    n = T/h(j);

    for i = 1:n
        t = (i-1)*h(j);
        vVec(:,i+1) = vVec(:,i) + h(j)*quartercar(t,vVec(:,i),k1,k2);
    end
    figure(4)
    tid = 0:h(j):T;
    plot(tid,vVec(1,:),tid,vVec(2,:)); hold on;

end


options = odeset('RelTol',1e-6,'refine', 5);
vVec = [0;0;0;0]';

[tVec, yVec] = ode45(@(t,vVec) quartercar(t,vVec,k1,k2),[t0 T],vVec,options);
figure(4)
plot(tVec(:,1),yVec(:,1),tVec(:,1),yVec(:,2));
legend('ode45','5e-3','5e-4');