% PlotComparison.m
% INSTRUCTIONS: Before running this script is necessary to copy the data in
% the folder 'plotToCompare'

%% Initialization
clear
clc
close all

addpath('plotToCompare')

DIR = dir('plotToCompare');

%% Uploading files in to the workspace
out = struct;
for n_file=3:length(DIR) % parto da 3 perchè nella cartella ci sono due elementi vuoti
    load(DIR(n_file).name,'out_c');
    name = DIR(n_file).name(1:end-4);
    name = name(6:end);
    out(n_file-2).Name = name;
    out(n_file-2).Values = out_c;
    clear out_c
end

%% --------------------- PLOT: Joint Position ---------------------

figure()
% Set
for i=1:length(out(1).Values.logsout) % i che varia da 1 al numero di variabili acquisite (uguali in tutte le prove)
    check = strcmp(out(1).Values.logsout{1,i}.Name,'qSet_j2');
    if check == 1
        plot(out(1).Values.Time,out(1).Values.logsout{1,i}.Values.Data*180/pi,...
            'LineWidth',1.5) 
        hold on
    end
end
% Feedback
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'qFB_j2');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data*180/pi,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend('qSet',out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend('qSet',out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend('qSet',out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('Joint angular position [°]','FontSize',20)
title('Joint: Shoulder','FontSize',24) 

%% --------------------- PLOT: Motor Position ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'qm_j2');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data*180/pi,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('Motor angular position [°]','FontSize',20)
title('Joint: Shoulder','FontSize',24) 

%% --------------------- PLOT: Velocity ---------------------

figure()
% Ref
for i=1:length(out(1).Values.logsout) % i che varia da 1 al numero di variabili acquisite (uguali in tutte le prove)
    check = strcmp(out(1).Values.logsout{1,i}.Name,'qdmRef_j2');
    if check == 1
        plot(out(1).Values.Time,out(1).Values.logsout{1,i}.Values.Data*180/pi/101,...
            'LineWidth',1.5)
        hold on
    end
end
% Feedback
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'qdmFB_j2');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data*180/pi/101,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend('qdmRef',out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend('qdmRef',out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend('qdmRef',out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('Angular velocity [°/s]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: Current ---------------------

figure()
% Feedback
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'iq_j2');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
% Ref
for i=1:length(out(1).Values.logsout) % i che varia da 1 al numero di variabili acquisite (uguali in tutte le prove)
    check = strcmp(out(1).Values.logsout{1,i}.Name,'iRef_j2');
    if check == 1
        plot(out(1).Values.Time,out(1).Values.logsout{1,i}.Values.Data,...
            'LineWidth',1.5)
        hold on
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'iRef','FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'iRef','FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'iRef','FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('Current (q-axis) [A]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: Electromagnetic Torque ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'Tem_j2');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('Electromagnetic Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: Backlash Torque ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'C_j2');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('Backlash Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: TCP x coordinate ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'TCP_x');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('TCP: x coordinate [m]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: TCP y coordinate ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'TCP_y');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('TCP: y coordinate [m]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: TCP z coordinate ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout) 
        check = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'TCP_z');
        if check == 1
            plot(out(n_file-2).Values.Time,out(n_file-2).Values.logsout{1,i}.Values.Data,...
                    'LineWidth',1.5) 
                hold on
        end
    end
end
grid minor 
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
xlabel('Time [s]','FontSize',20)
ylabel('TCP: z coordinate [m]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------- PLOT: TCP 3D plot ---------------------

figure()
for n_file=3:length(DIR)
    for i=1:length(out(1).Values.logsout)
    check_x = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'TCP_x');
    check_y = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'TCP_y');
    check_z = strcmp(out(n_file-2).Values.logsout{1,i}.Name,'TCP_z');
    if check_x == 1
        index_x = i;
    end
     if check_y == 1
        index_y = i;
    end
     if check_z == 1
        index_z = i;
     end
    end
    plot3(out(n_file-2).Values.logsout{1,index_x}.Values.Data,...
        out(n_file-2).Values.logsout{1,index_y}.Values.Data, ...
        out(n_file-2).Values.logsout{1,index_z}.Values.Data,'LineWidth',1.5)
    hold on
end
grid minor
switch length(out)
    case 2
        legend(out(1).Name,out(2).Name,'FontSize',18)
    case 3
        legend(out(1).Name,out(2).Name,out(3).Name,'FontSize',18)
    case 4
        legend(out(1).Name,out(2).Name,out(3).Name,out(4).Name,'FontSize',18)
    otherwise
        disp('Condizioni dello switch non sufficienti')
end  
axis equal
xlabel('x(t) [m]')
ylabel('y(t) [m]')
zlabel('z(t) [m]')
title('Tool Center Point','FontSize',24)