%Uppgift 3, Anton Hagelberg och Filip Johansson, fjäderkonstanter
clear all; clc; close all;
k = [5300,136000]';
kn = 0;
tol = 1e-6;
error = [tol*2,tol*2]';
ck = 0.7;
NR = [k(1),k(2)]';
cs = 1;
n = 1;

while error(:,n) > tol

    F = transfer_functions(k(1),k(2),ck,cs);
    J = Jacobian_transfer_functions(k(1),k(2));

    kn = k - J\F;

    k = kn;
    NR = [NR, k];
    error = [error, abs(NR(:,n+1) - NR(:,n))];

    n = n + 1;
end


%%
%Uppgift 2 med värden från uppgift 3
clc; clear all; close all;
vVec = [0;0;0;0];
k1 = [5300, 312.0365];
k2 = [136000, 189649.006846771];
n = 10000;
L = 1;
v = 65/3.6;
T = (L/v)*30;
h = T/n;
for j = 1:2
    for i = 1:n
        t = (i-1)*h;
        vVec(:,i+1) = vVec(:,i) + h*quartercar(t,vVec(:,i),k1(j),k2(j));
    end
    tid = 0:h:T;
    title('Beräknade värden från U3 i U2')
    plot(tid,vVec(2,:)); hold on;
    xlabel('Tid [s]')
    ylabel('Förskjutning [m]')
    legend('Värden från U3','Värden från referens');
end