
% ax laterale ay verticale, az orizzontale
clear;
clc;
fullScriptPath = matlab.desktop.editor.getActiveFilename();
[folderPath, ~, ~] = fileparts(fullScriptPath);
% FILE1
%nomefile
file1 = 'C2_2025-05-20_16-15-24.txt';
% Define the names of the files you want to read
filename1 = fullfile(folderPath, file1);
% Importa i dati
data_import_1 = importdata(filename1,'\t');
% Visualizza i dati
data1=data_import_1.data(5:end,:);
disp(data_import_1);
% FILE2
%nomefile
file2 = 'DRONE_2025-05-20_16-15-24.txt';
% Define the names of the files you want to read
filename2 = fullfile(folderPath, file2);
% Importa i dati
data_import_2 = importdata(filename2,'\t');
% Visualizza i dati
data2=data_import_2.data(5:6500,:);
disp(data_import_2);
% FILE3
%nomefile
file3 = 'DRONE_2025-05-20_11-25-24.txt';
% Define the names of the files you want to read
filename3 = fullfile(folderPath, file3);
% Importa i dati
data_import_3 = importdata(filename3,'\t');
% Visualizza i dati
data3=data_import_3.data(5:7000,:);
disp(data_import_3);
% % FILE4
% %nomefile
% file4 = 'C2_2025-05-20_11-25-24.txt';
% % Define the names of the files you want to read
% filename4 = fullfile(folderPath, file4);
% % Importa i dati
% data_import_4 = importdata(filename4,'\t');
% % Visualizza i dati
% data4=data_import_4.data(5:7000,:);
% proiezione mercatore
x1=6371*(data1(:,3)-data1(1,3));
y1=6371*log(tan(pi/4+(data1(:,2)-data1(1,2))/2));



x2=6371*(data2(:,3)-data2(1,3));
y2=6371*log(tan(pi/4+(data2(:,2)-data2(1,2))/2));

x3=6371*(data3(:,3)-data3(1,3));
y3=6371*log(tan(pi/4+(data3(:,2)-data3(1,2))/2));

x3(5000:end)=x3(5000:end)+100;
y3(5000:end)=y3(5000:end)-400;

x3(6300:end)=x3(6300:end)+55;
y3(6300:end)=y3(6300:end)-355;

% x4=6371*(data4(:,3)-data4(1,3));
% y4=6371*log(tan(pi/4+(data4(:,2)-data4(1,2))/2));


theta = -1 * pi/180;   % convert degrees to radians

% Build rotation matrix
R = [cos(theta) -sin(theta);
     sin(theta)  cos(theta)];

% Apply rotation to dataset 3
rot3 = R * [x3(:)'; y3(:)'];
x3 = rot3(1,:)';
y3t = rot3(2,:)';

% % Apply rotation to dataset 4
% rot4 = R * [x4(:)'; y4(:)'];
% x4 = rot4(1,:)';
% y4 = rot4(2,:)';

%%
figure()
plot3(x1,y1,data1(:,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2,y2,data2(:,4)*0.3048,'--k', 'LineWidth', 1.5)
hold on
plot3(x3(1:4500), y3(1:4500), data3(1:4500,4)*0.3048, '--r', 'LineWidth', 1.5);
hold on
plot3(x3(5000:6300), y3(5000:6300), data3(5000:6300,4)*0.3048, '--r', 'LineWidth', 1.5);
hold on
plot3(x3(6600:end), y3(6600:end), data3(6600:end,4)*0.3048, '--r', 'LineWidth', 1.5);

% plot3(x4,y4,data4(:,4)*0.3048,'r', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]

figure()
plot(x1,data1(:,4)*0.3048,'k', 'LineWidth', 1.5)
hold on
plot(x2,data2(:,4)*0.3048,'--k', 'LineWidth', 1.5)
hold on
plot(x3(1:4500),data3(1:4500,4)*0.3048,'--r', 'LineWidth', 1.5)
 hold on
plot(x3(5000:6300), data3(5000:6300,4)*0.3048, '--r', 'LineWidth', 1.5);
 hold on
plot(x3(6600:end), data3(6600:end,4)*0.3048, '--r', 'LineWidth', 1.5);
hold off
grid on
xlabel('X');
ylabel('Z');
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
pbaspect([1 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]

figure()
plot(x1,y1,'k', 'LineWidth', 1.5)
hold on
plot(x2,y2,'--k', 'LineWidth', 1.5)
hold on
plot(x3(1:4500),y3(1:4500),'--r', 'LineWidth', 1.5)
hold on
plot(x3(5000:6300),y3(5000:6300),'--r', 'LineWidth', 1.5)
hold on
plot(x3(6600:end),y3(6600:end),'--r', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
pbaspect([1 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
%%
figure()
n=4600;
plot3(x1(1:n),y1(1:n),data1(1:n,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(1:n),y2(1:n),data2(1:n,4)*0.3048,'--k', 'LineWidth', 1.5)
hold on
plot3(x3(1:n),y3(1:n),data3(1:n,4)*0.3048,'--r', 'LineWidth', 1.5)

hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]

%NED
figure()
subplot(3,1,1)
plot(data1(1:n,1)-50,data1(1:n,21),'k', 'LineWidth', 1.5)
hold on
plot(data3(1:n,1)-5,data3(1:n,21),'--r', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
legend('Perturbed','Unperturbed', 'Location', 'northeast');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(1:n,1)-50,data1(1:n,19),'k', 'LineWidth', 1.5)
hold on
plot(data3(1:n,1)-5,data3(1:n,19),'--r', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(1:n,1)-50,data1(1:n,17),'k', 'LineWidth', 1.5)
hold on
plot(data3(1:n,1)-5,data3(1:n,17),'--r', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]



%%
figure()
n=5000;
m=5850;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold on
plot3(x3(5500:6200)+1,y3(5500:6200)-150,data3(5500:6200,4)*0.3048,'--r', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 1300])
%NED
figure()
subplot(3,1,1)
yline(-40,'--k', 'LineWidth', 1.5)
hold on
plot(data1(n:m,1)-13,data1(n:m,21)+10,'k', 'LineWidth', 1.5)
hold on
plot(data3(5500:6300,1),data3(5500:6300,21),'--r', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
legend('comand','Perturbed','Unperturbed', 'Location', 'southeast');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
yline(20,'--k', 'LineWidth', 1.5)
hold on
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
hold on
plot(data3(5500:6300,1)+13,data3(5500:6300,19),'--r', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
yline(20,'--k', 'LineWidth', 1.5)
hold on
plot(data1(n:m,1)-13,data1(n:m,17),'k', 'LineWidth', 1.5)
hold on
plot(data3(5500:6300,1),data3(5500:6300,17),'--r', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]



%%
figure()
n=5700;
m=6496;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold on
plot3(x3(n+400:6200),y3(n+400:6200),data3(n+400:6200,4)*0.3048,'--r', 'LineWidth', 1.5)
hold on
plot3(x3(6500:end),y3(6500:end),data3(6500:end,4)*0.3048,'--r', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 1400])

%NED
figure()
subplot(3,1,1)
yline(-40,'--k', 'LineWidth', 1.5)
hold on
plot(data1(n:m,1)-5,data1(n:m,21),'k', 'LineWidth', 1.5)
hold on
plot(data3(n+400:end,1),data3(n+400:end,21),'--r', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
legend('leader','Perturbed','Unperturbed', 'Location', 'southeast');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
yline(20,'--k', 'LineWidth', 1.5)
hold on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
plot(data1(n:m,1)-5,data1(n:m,19),'k', 'LineWidth', 1.5)
hold on
plot(data3(n+400:end,1),data3(n+400:end,19),'--r', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
yline(20,'--k', 'LineWidth', 1.5)
hold on
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
hold on
yline(20,'--r', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');

grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]


%%
figure()
n=7200;
m=7600;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
legend('leader','Perturbed','Unperturbed', 'Location', 'northeast');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 2000])

%NED
figure()
subplot(3,1,1)
plot(data1(n:m,1),data1(n:m,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]


%%
figure()
n=7600;
m=10100;
plot3(x1(n:m),y1(n:m),data1(n:m,4)*0.3048,'k', 'LineWidth', 1.5);
hold on
plot3(x2(n:m),y2(n:m),data2(n:m,4)*0.3048,'--k', 'LineWidth', 1.5)
hold off
grid on
xlabel('X');
ylabel('Y');
zlabel('Z');
pbaspect([1 1 0.75]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 600, 600]) % [x, y, width, height]
zlim([800 4000])

%NED
figure()
subplot(3,1,1)
plot(data1(n:m,1),data1(n:m,21),'k', 'LineWidth', 1.5)
ylabel('NORD [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,2)
plot(data1(n:m,1),data1(n:m,19),'k', 'LineWidth', 1.5)
ylabel('EAST [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
subplot(3,1,3)
plot(data1(n:m,1),data1(n:m,17),'k', 'LineWidth', 1.5)
ylabel('DOWN [m]');
xlabel('t [s]');
grid on
pbaspect([4 1 1]) % Aspect ratio X:Y:Z
set(gcf, 'Position', [100, 100, 800, 600]) % [x, y, width, height]


