%%
clc
clear all 
close all

%% caricamento dati sperimentali di input
TT = readtimetable('Set sperimentali\test_IN_2_mod.txt');
time_set = TT.Var11;
time_set = time2num(time_set,'seconds');
time_set = time_set - time_set(1,1)*ones(length(time_set),1);

x_exp_set = TT.Var4;

y_exp_set = TT.Var2;

psi_exp_set = TT.Var6;
psi_exp_set = deg2rad(psi_exp_set);

% nei file .txt ci sono più righe per gli stessi istanti di tempo, questo
% ciclo elimina i dati ridondanti

counter = 0;

for ii = 2:length(time_set)
if time_set(ii,1)~=time_set(ii-1,1)
counter=counter+1;
end
end

x_set = ones(counter,1);

y_set = ones(counter,1);

psi_set = ones(counter,1);

time = ones(counter,1);

variable=1;

for ii = 2:length(time_set)
if time_set(ii,1)~=time_set(ii-1,1)

    time(variable,1) = time_set(ii,1);
   
    x_set(variable,1) = x_exp_set(ii,1);

    y_set(variable,1) = y_exp_set(ii,1);

    psi_set(variable,1) = psi_exp_set(ii,1); 

    variable = variable + 1;
end
end

time = [time_set(1,1);time]; 

x_set = [x_exp_set(1,1);x_set];

y_set = [y_exp_set(1,1);y_set];

psi_set = [psi_exp_set(1,1);psi_set];

figure
hold on
grid on
plot(time,x_set,time,y_set,time,psi_set,'linewidth',1.5)
title('Coordinate sperimentali di set')
xlabel('tempo [s]')
ylabel('coordinate')

ExpDataSET = struct;
ExpDataSET.t = time';
ExpDataSET.x = x_set';
ExpDataSET.y = y_set';
ExpDataSET.psi = psi_set';

save('ExpDataSET_s','ExpDataSET')
%% caricamento velocità sperimentali di input

TT = readtimetable('Set sperimentali\test_IN_2_mod.txt');
time_set = TT.Var11;
time_set = time2num(time_set,'seconds');
time_set = time_set - time_set(1,1)*ones(length(time_set),1);

Vx = TT.Var8;

Psid = TT.Var10;
Psid = deg2rad(Psid);

counter = 0;

for ii = 2:length(time_set)
if time_set(ii,1)~=time_set(ii-1,1)
counter=counter+1;
end
end

Vx_set = ones(counter,1);

Psid_set = ones(counter,1);

time = ones(counter,1);

variable=1;

for ii = 2:length(time_set)
if time_set(ii,1)~=time_set(ii-1,1)

    time(variable,1) = time_set(ii,1);
   
    Vx_set(variable,1) = Vx(ii,1);

    Psid_set(variable,1) = Psid(ii,1); 

    variable = variable + 1;
end
end

time = [time_set(1,1);time]; 

Vx_set = [Vx(1,1);Vx_set];
Vy_set = zeros(length(Vx_set),1);
Psid_set = [Psid(1,1);Psid_set];


figure
hold on
grid on
plot(time,Vx_set,time,Vy_set,time,Psid_set,'linewidth',1.5)
title('Velocità sperimentali di set')
xlabel('tempo [s]')
ylabel('Velocità')

ExpDataSET = struct;
ExpDataSET.t = time;
ExpDataSET.Vx = Vx_set;
ExpDataSET.Vy = Vy_set;
ExpDataSET.Psid = Psid_set;

save('ExpDataSET_v','ExpDataSET')