%Lab 5, Zadanie 4 - Całkowanie numeryczne
clear all; close all;

%Dane
pozycje = readmatrix("accel_logs3.csv")
i = 1;
while i < length(pozycje)
    if (pozycje(i,4) == pozycje(i+1,4))
        pozycje(i+1,:) = [];
    else
        i = i+1;
    end
end
aX = pozycje(:,1);
aY = pozycje(:,2);
aZ = pozycje(:,3);
czas = pozycje(:,4);


vX = zeros(5475,1);
vY = zeros(5475,1);
vZ = zeros(5475,1);

for i = 1:5474
    vX(i) = aX(i)/(czas(i+1)-czas(i));
    vY(i) = aY(i)/(czas(i+1)-czas(i));
    vZ(i) = aZ(i)/(czas(i+1)-czas(i));
end
V = zeros(5474,1);
for i = 1:5474
    V(i) = norm([vX(i), vY(i), vZ(i)]);
end

for i = 1:5474
    X(i) = vX(i)/(czas(i+1)-czas(i));
    Y(i) = vY(i)/(czas(i+1)-czas(i));
    Z(i) = vZ(i)/(czas(i+1)-czas(i));
end
poz = zeros(5474,1);
for i = 1:5474
    poz(i) = norm([vX(i), vY(i), vZ(i)]);
end
dystans = 0;
for i = 1:5474
    dystans = dystans + poz(i);
end